diff --git a/Blarg.GameFramework/Blarg.GameFramework.csproj b/Blarg.GameFramework/Blarg.GameFramework.csproj
index f89f667..35b8d4a 100644
--- a/Blarg.GameFramework/Blarg.GameFramework.csproj
+++ b/Blarg.GameFramework/Blarg.GameFramework.csproj
@@ -56,6 +56,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/Blarg.GameFramework/Math/BoundingBox.cs b/Blarg.GameFramework/Math/BoundingBox.cs
new file mode 100644
index 0000000..7a1afab
--- /dev/null
+++ b/Blarg.GameFramework/Math/BoundingBox.cs
@@ -0,0 +1,164 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct BoundingBox : IEquatable
+ {
+ public Vector3 Min;
+ public Vector3 Max;
+
+ public float Width
+ {
+ get { return Math.Abs(Max.X - Min.X); }
+ }
+
+ public float Height
+ {
+ get { return Math.Abs(Max.Y - Min.Y); }
+ }
+
+ public float Depth
+ {
+ get { return Math.Abs(Max.Z - Min.Z); }
+ }
+
+ public BoundingBox(Vector3 min, Vector3 max)
+ : this(ref min, ref max)
+ {
+ }
+
+ public BoundingBox(ref Vector3 min, ref Vector3 max)
+ {
+ Min = min;
+ Max = max;
+ }
+
+ public BoundingBox(float minX, float minY, float minZ, float maxX, float maxY, float maxZ)
+ {
+ Min.X = minX;
+ Min.Y = minY;
+ Min.Z = minZ;
+ Max.X = maxX;
+ Max.Y = maxY;
+ Max.Z = maxZ;
+ }
+
+ public BoundingBox(Vector3 center, float halfWidth)
+ : this(ref center, halfWidth)
+ {
+ }
+
+ public BoundingBox(ref Vector3 center, float halfWidth)
+ {
+ Min.X = center.X - halfWidth;
+ Min.Y = center.Y - halfWidth;
+ Min.Z = center.Z - halfWidth;
+ Max.X = center.X + halfWidth;
+ Max.Y = center.Y + halfWidth;
+ Max.Z = center.Z + halfWidth;
+ }
+
+ public BoundingBox(Vector3[] vertices)
+ {
+ float minX = 0.0f;
+ float minY = 0.0f;
+ float minZ = 0.0f;
+ float maxX = 0.0f;
+ float maxY = 0.0f;
+ float maxZ = 0.0f;
+
+ for (int i = 0; i < vertices.Length; ++i)
+ {
+ minX = Math.Min(vertices[i].X, minX);
+ minY = Math.Min(vertices[i].Y, minY);
+ minZ = Math.Min(vertices[i].Z, minZ);
+ maxX = Math.Max(vertices[i].X, maxX);
+ maxY = Math.Max(vertices[i].Y, maxY);
+ maxZ = Math.Max(vertices[i].Z, maxZ);
+ }
+
+ Min.X = minX;
+ Min.Y = minY;
+ Min.Z = minZ;
+ Max.X = maxX;
+ Max.Y = maxY;
+ Max.Z = maxZ;
+ }
+
+ public static float GetSquaredDistanceFromPointToBox(Vector3 point, BoundingBox box)
+ {
+ return GetSquaredDistanceFromPointToBox(ref point, ref box);
+ }
+
+ public static float GetSquaredDistanceFromPointToBox(ref Vector3 point, ref BoundingBox box)
+ {
+ float distanceSq = 0.0f;
+ float v;
+
+ v = point.X;
+ if (v < box.Min.X)
+ distanceSq += (box.Min.X - v) * (box.Min.X - v);
+ if (v > box.Max.X)
+ distanceSq += (v - box.Max.X) * (v - box.Max.X);
+
+ v = point.Y;
+ if (v < box.Min.Y)
+ distanceSq += (box.Min.Y - v) * (box.Min.Y - v);
+ if (v > box.Max.Y)
+ distanceSq += (v - box.Max.Y) * (v - box.Max.Y);
+
+ v = point.Z;
+ if (v < box.Min.Z)
+ distanceSq += (box.Min.Z - v) * (box.Min.Z - v);
+ if (v > box.Max.Z)
+ distanceSq += (v - box.Max.Z) * (v - box.Max.Z);
+
+ return distanceSq;
+ }
+
+ public static float GetDistanceFromPointToBox(Vector3 point, BoundingBox box)
+ {
+ return GetDistanceFromPointToBox(ref point, ref box);
+ }
+
+ public static float GetDistanceFromPointToBox(ref Vector3 point, ref BoundingBox box)
+ {
+ return (float)Math.Sqrt(GetSquaredDistanceFromPointToBox(ref point, ref box));
+ }
+
+ public static bool operator ==(BoundingBox left, BoundingBox right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(BoundingBox left, BoundingBox right)
+ {
+ return !left.Equals(right);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is BoundingBox)
+ return this.Equals((BoundingBox)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(BoundingBox other)
+ {
+ return (Min == other.Min && Max == other.Max);
+ }
+
+ public override int GetHashCode()
+ {
+ return Min.GetHashCode() ^ Max.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{{0}, {1}}}", Min, Max);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/BoundingSphere.cs b/Blarg.GameFramework/Math/BoundingSphere.cs
new file mode 100644
index 0000000..f495845
--- /dev/null
+++ b/Blarg.GameFramework/Math/BoundingSphere.cs
@@ -0,0 +1,134 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct BoundingSphere : IEquatable
+ {
+ public Vector3 Center;
+ public float Radius;
+
+ public BoundingSphere(Vector3 center, float radius)
+ : this(ref center, radius)
+ {
+ }
+
+ public BoundingSphere(ref Vector3 center, float radius)
+ {
+ Center = center;
+ Radius = radius;
+ }
+
+ public BoundingSphere(float centerX, float centerY, float centerZ, float radius)
+ {
+ Center.X = centerX;
+ Center.Y = centerY;
+ Center.Z = centerZ;
+ Radius = radius;
+ }
+
+ public BoundingSphere(Vector3[] vertices)
+ {
+ int min;
+ int max;
+
+ int minX = 0;
+ int minY = 0;
+ int minZ = 0;
+ int maxX = 0;
+ int maxY = 0;
+ int maxZ = 0;
+
+ // find min & max points for x, y and z
+ for (int i = 0; i < vertices.Length; ++i)
+ {
+ if (vertices[i].X < vertices[minX].X)
+ minX = i;
+ if (vertices[i].X > vertices[maxX].X)
+ maxX = i;
+ if (vertices[i].Y < vertices[minY].Y)
+ minY = i;
+ if (vertices[i].Y > vertices[maxY].Y)
+ maxY = i;
+ if (vertices[i].Z < vertices[minZ].Z)
+ minZ = i;
+ if (vertices[i].Z > vertices[maxZ].Z)
+ maxZ = i;
+ }
+
+ // distances between these extremes for x, y and z
+ float distanceSqX = Vector3.Dot(vertices[maxX] - vertices[minX], vertices[maxX] - vertices[minX]);
+ float distanceSqY = Vector3.Dot(vertices[maxY] - vertices[minY], vertices[maxY] - vertices[minY]);
+ float distanceSqZ = Vector3.Dot(vertices[maxZ] - vertices[minZ], vertices[maxZ] - vertices[minZ]);
+
+ // get the pair of points representing the most distance points from each other
+ min = minX;
+ max = maxX;
+ if (distanceSqY > distanceSqX && distanceSqY > distanceSqZ)
+ {
+ min = minY;
+ max = maxY;
+ }
+ if (distanceSqZ > distanceSqX && distanceSqZ > distanceSqY)
+ {
+ min = minZ;
+ max = maxZ;
+ }
+
+ // we now have enough info to set the initial sphere properties
+ Center = (vertices[min] + vertices[max]) / 2.0f;
+ Radius = (float)Math.Sqrt(Vector3.Dot(vertices[max] - Center, vertices[max] - Center));
+
+ // now expand the sphere to make sure it encompasses all the points (if it doesn't already)
+ Vector3 d;
+ for (int i = 0; i < vertices.Length; ++i)
+ {
+ d = vertices[i] - Center;
+ float distanceSq = Vector3.Dot(d, d);
+ if (distanceSq > (Radius * Radius))
+ {
+ float distance = (float)Math.Sqrt(distanceSq);
+ float newRadius = (Radius + distance) * 0.5f;
+ float k = (newRadius - Radius) / distance;
+ Radius = newRadius;
+ d = d * k;
+ Center = Center + d;
+ }
+ }
+ }
+
+ public static bool operator ==(BoundingSphere left, BoundingSphere right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(BoundingSphere left, BoundingSphere right)
+ {
+ return !left.Equals(right);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is BoundingSphere)
+ return this.Equals((BoundingSphere)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(BoundingSphere other)
+ {
+ return (Center == other.Center && Radius == other.Radius);
+ }
+
+ public override int GetHashCode()
+ {
+ return Center.GetHashCode() ^ Radius.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{Center:{0} Radius:{1}}}", Center, Radius);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/IntersectionTester.cs b/Blarg.GameFramework/Math/IntersectionTester.cs
new file mode 100644
index 0000000..188df3e
--- /dev/null
+++ b/Blarg.GameFramework/Math/IntersectionTester.cs
@@ -0,0 +1,527 @@
+using System;
+
+namespace Blarg.GameFramework
+{
+ public static class IntersectionTester
+ {
+ public static bool Test(ref BoundingBox box, ref Vector3 point)
+ {
+ if ((point.X >= box.Min.X && point.X <= box.Max.X) &&
+ (point.Y >= box.Min.Y && point.Y <= box.Max.Y) &&
+ (point.Z >= box.Min.Z && point.Z <= box.Max.Z))
+ return true;
+ else
+ return false;
+ }
+
+ public static bool Test(ref BoundingSphere sphere, ref Vector3 point)
+ {
+ if ((float)Math.Abs(Vector3.Distance(ref point, ref sphere.Center)) < sphere.Radius)
+ return true;
+ else
+ return false;
+ }
+
+ public static bool Test(ref BoundingBox box, Vector3[] vertices, ref Vector3 outFirstIntersection)
+ {
+ for (int i = 0; i < vertices.Length; ++i)
+ {
+ if ((vertices[i].X >= box.Min.X && vertices[i].X <= box.Max.X) &&
+ (vertices[i].Y >= box.Min.Y && vertices[i].Y <= box.Max.Y) &&
+ (vertices[i].Z >= box.Min.Z && vertices[i].Z <= box.Max.Z))
+ {
+ outFirstIntersection = vertices[i];
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+ public static bool Test(ref BoundingSphere sphere, Vector3[] vertices, ref Vector3 outFirstIntersection)
+ {
+ for (int i = 0; i < vertices.Length; ++i)
+ {
+ if ((float)Math.Abs(Vector3.Distance(ref vertices[i], ref sphere.Center)) < sphere.Radius)
+ {
+ outFirstIntersection = vertices[i];
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+ public static bool Test(ref BoundingBox a, ref BoundingBox b)
+ {
+ if (a.Max.X < b.Min.X || a.Min.X > b.Max.X)
+ return false;
+ if (a.Max.Y < b.Min.Y || a.Min.Y > b.Max.Y)
+ return false;
+ if (a.Max.Z < b.Min.Z || a.Min.Z > b.Max.Z)
+ return false;
+
+ return true;
+ }
+
+ public static bool Test(ref BoundingSphere a, ref BoundingSphere b)
+ {
+ Vector3 temp = a.Center - b.Center;
+ float distanceSquared = Vector3.Dot(temp, temp);
+
+ float radiusSum = a.Radius + b.Radius;
+ if (distanceSquared <= radiusSum * radiusSum)
+ return true;
+ else
+ return false;
+ }
+
+ public static bool Test(ref BoundingSphere sphere, ref Plane plane)
+ {
+ float distance = Vector3.Dot(sphere.Center, plane.Normal) - plane.D;
+ if ((float)Math.Abs(distance) <= sphere.Radius)
+ return true;
+ else
+ return false;
+ }
+
+ public static bool Test(ref BoundingBox box, ref Plane plane)
+ {
+ Vector3 temp1 = (box.Max + box.Min) / 2.0f;
+ Vector3 temp2 = box.Max - temp1;
+
+ float radius = (temp2.X * (float)Math.Abs(plane.Normal.X)) + (temp2.Y * (float)Math.Abs(plane.Normal.Y)) + (temp2.Z * (float)Math.Abs(plane.Normal.Z));
+
+ float distance = Vector3.Dot(plane.Normal, temp1) - plane.D;
+
+ if ((float)Math.Abs(distance) <= radius)
+ return true;
+ else
+ return true;
+ }
+
+ public static bool Test(ref Ray ray, ref Plane plane, ref Vector3 outIntersection)
+ {
+ float denominator = Vector3.Dot(ray.Direction, plane.Normal);
+ if (denominator == 0.0f)
+ return false;
+
+ float t = ((-plane.D - Vector3.Dot(ray.Position, plane.Normal)) / denominator);
+ if (t < 0.0f)
+ return false;
+
+ Vector3 temp1 = ray.GetPositionAt(t);
+ outIntersection.X = temp1.X;
+ outIntersection.Y = temp1.Y;
+ outIntersection.Z = temp1.Z;
+
+ return true;
+ }
+
+ public static bool Test(ref Ray ray, ref BoundingSphere sphere, ref Vector3 outFirstIntersection)
+ {
+ Vector3 temp1 = ray.Position - sphere.Center;
+
+ float b = Vector3.Dot(temp1, ray.Direction);
+ float c = Vector3.Dot(temp1, temp1) - (sphere.Radius * sphere.Radius);
+
+ if (c > 0.0f && b > 0.0f)
+ return false;
+
+ float discriminant = b * b - c;
+ if (discriminant < 0.0f)
+ return false;
+
+ float t = -b - (float)Math.Sqrt(discriminant);
+ if (t < 0.0f)
+ t = 0.0f;
+
+ Vector3 temp2 = ray.GetPositionAt(t);
+ outFirstIntersection.X = temp2.X;
+ outFirstIntersection.Y = temp2.Y;
+ outFirstIntersection.Z = temp2.Z;
+
+ return true;
+ }
+
+ public static bool Test(ref Ray ray, ref BoundingBox box, ref Vector3 outFirstIntersection)
+ {
+ float tmin = 0.0f;
+ float tmax = float.MaxValue;
+
+ if ((float)Math.Abs(ray.Direction.X) < float.Epsilon)
+ {
+ if (ray.Position.X < box.Min.X || ray.Position.X > box.Max.X)
+ return false;
+ }
+ else
+ {
+ float invD = 1.0f / ray.Direction.X;
+ float t1 = (box.Min.X - ray.Position.X) * invD;
+ float t2 = (box.Max.X - ray.Position.X) * invD;
+
+ if (t1 > t2)
+ {
+ float tswap = t1;
+ t1 = t2;
+ t2 = tswap;
+ }
+
+ tmin = Math.Max(tmin, t1);
+ tmax = Math.Min(tmax, t2);
+
+ if (tmin > tmax)
+ return false;
+ }
+
+ if ((float)Math.Abs(ray.Direction.Y) < float.Epsilon)
+ {
+ if (ray.Position.Y < box.Min.Y || ray.Position.Y > box.Max.Y)
+ return false;
+ }
+ else
+ {
+ float invD = 1.0f / ray.Direction.Y;
+ float t1 = (box.Min.Y - ray.Position.Y) * invD;
+ float t2 = (box.Max.Y - ray.Position.Y) * invD;
+
+ if (t1 > t2)
+ {
+ float tswap = t1;
+ t1 = t2;
+ t2 = tswap;
+ }
+
+ tmin = Math.Max(tmin, t1);
+ tmax = Math.Min(tmax, t2);
+
+ if (tmin > tmax)
+ return false;
+ }
+
+ if ((float)Math.Abs(ray.Direction.Z) < float.Epsilon)
+ {
+ if (ray.Position.Z < box.Min.Z || ray.Position.Z > box.Max.Z)
+ return false;
+ }
+ else
+ {
+ float invD = 1.0f / ray.Direction.Z;
+ float t1 = (box.Min.Z - ray.Position.Z) * invD;
+ float t2 = (box.Max.Z - ray.Position.Z) * invD;
+
+ if (t1 > t2)
+ {
+ float tswap = t1;
+ t1 = t2;
+ t2 = tswap;
+ }
+
+ tmin = Math.Max(tmin, t1);
+ tmax = Math.Min(tmax, t2);
+
+ if (tmin > tmax)
+ return false;
+ }
+
+ Vector3 temp1 = ray.GetPositionAt(tmin);
+ outFirstIntersection.X = temp1.X;
+ outFirstIntersection.Y = temp1.Y;
+ outFirstIntersection.Z = temp1.Z;
+
+ return true;
+ }
+
+ public static bool Test(ref BoundingBox box, ref BoundingSphere sphere)
+ {
+ float distanceSq = BoundingBox.GetSquaredDistanceFromPointToBox(ref sphere.Center, ref box);
+
+ if (distanceSq <= (sphere.Radius * sphere.Radius))
+ return true;
+ else
+ return false;
+ }
+
+ public static bool Test(ref Ray ray, ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 outIntersection)
+ {
+ float r, num1, num2;
+
+ Vector3 temp1 = Vector3.Cross(b - a, c - a);
+ if (temp1.X == 0.0f && temp1.Y == 0.0f && temp1.Z == 0.0f)
+ return false;
+
+ Vector3 temp2 = ray.Position - a;
+ num1 = -Vector3.Dot(temp1, temp2);
+ num2 = Vector3.Dot(temp1, ray.Direction);
+ if ((float)Math.Abs(num2) < float.Epsilon)
+ {
+ if (num1 == 0.0f)
+ {
+ outIntersection = ray.Position;
+ return true;
+ }
+ else
+ return false;
+ }
+
+ r = num1 / num2;
+ if (r < 0.0f)
+ return false;
+
+ Vector3 temp3 = ray.GetPositionAt(r);
+ if (Test(ref temp3, ref a, ref b, ref c))
+ {
+ outIntersection = temp3;
+ return true;
+ }
+ else
+ return false;
+ }
+
+ public static bool Test(ref Vector3 point, ref Vector3 a, ref Vector3 b, ref Vector3 c)
+ {
+ Vector3 v0 = c - a;
+ Vector3 v1 = b - a;
+ Vector3 v2 = point - a;
+
+ float dot00 = (v0.X * v0.X) + (v0.Y * v0.Y) + (v0.Z * v0.Z);
+ float dot01 = (v0.X * v1.X) + (v0.Y * v1.Y) + (v0.Z * v1.Z);
+ float dot02 = (v0.X * v2.X) + (v0.Y * v2.Y) + (v0.Z * v2.Z);
+ float dot11 = (v1.X * v1.X) + (v1.Y * v1.Y) + (v1.Z * v1.Z);
+ float dot12 = (v1.X * v2.X) + (v1.Y * v2.Y) + (v1.Z * v2.Z);
+
+ float denom = dot00 * dot11 - dot01 * dot01;
+ if (denom == 0)
+ return false;
+
+ float u = (dot11 * dot02 - dot01 * dot12) / denom;
+ float v = (dot00 * dot12 - dot01 * dot02) / denom;
+
+ if (u >= 0 && v >= 0 && u + v <= 1)
+ return true;
+ else
+ return false;
+ }
+
+ public static bool Test(ref SweptEllipsoidCollisionPacket packet, ref Vector3 v1, ref Vector3 v2, ref Vector3 v3)
+ {
+ bool foundCollision = false;
+
+ Vector3 p1;
+ Vector3 p2;
+ Vector3 p3;
+ Vector3.Divide(ref v1, ref packet.EllipsoidRadius, out p1);
+ Vector3.Divide(ref v2, ref packet.EllipsoidRadius, out p2);
+ Vector3.Divide(ref v3, ref packet.EllipsoidRadius, out p3);
+
+ var trianglePlane = new Plane(ref p1, ref p2, ref p3);
+
+ // Is the triangle front-facing to the entity's velocity?
+ if (Plane.IsFrontFacingTo(ref trianglePlane, ref packet.esNormalizedVelocity))
+ {
+ float t0;
+ float t1;
+ bool embeddedInPlane = false;
+ float distToTrianglePlane = Plane.DistanceBetween(ref trianglePlane, ref packet.esPosition);
+ float normalDotVelocity = Vector3.Dot(trianglePlane.Normal, packet.esVelocity);
+
+ // Is the sphere travelling parallel to the plane?
+ if (normalDotVelocity == 0.0f)
+ {
+ if ((float)Math.Abs(distToTrianglePlane) >= 1.0f)
+ {
+ // Sphere is not embedded in the plane, no collision possible
+ return false;
+ }
+ else
+ {
+ // Sphere is embedded in the plane, it intersects throughout the whole time period
+ embeddedInPlane = true;
+ t0 = 0.0f;
+ t1 = 1.0f;
+ }
+ }
+ else
+ {
+ // Not travelling parallel to the plane
+ t0 = (-1.0f - distToTrianglePlane) / normalDotVelocity;
+ t1 = (1.0f - distToTrianglePlane) / normalDotVelocity;
+
+ // Swap so t0 < t1
+ if (t0 > t1)
+ {
+ float temp = t1;
+ t1 = t0;
+ t0 = temp;
+ }
+
+ // Check that at least one result is within range
+ if (t0 > 1.0f || t1 < 0.0f)
+ {
+ // Both values outside the range [0,1], no collision possible
+ return false;
+ }
+
+ t0 = MathHelpers.Clamp(t0, 0.0f, 1.0f);
+ t1 = MathHelpers.Clamp(t1, 0.0f, 1.0f);
+ }
+
+ // At this point, we have two time values (t0, t1) between which the
+ // swept sphere intersects with the triangle plane
+ Vector3 collisionPoint = new Vector3();
+ float t = 1.0f;
+
+ // First, check for a collision inside the triangle. This will happen
+ // at time t0 if at all as this is when the sphere rests on the front
+ // side of the triangle plane.
+ if (!embeddedInPlane)
+ {
+ Vector3 planeIntersectionPoint = (packet.esPosition - trianglePlane.Normal) + packet.esVelocity * t0;
+
+ if (Test(ref planeIntersectionPoint, ref p1, ref p2, ref p3))
+ {
+ foundCollision = true;
+ t = t0;
+ collisionPoint = planeIntersectionPoint;
+ }
+ }
+
+ // If we haven't found a collision at this point, we need to check the
+ // points and edges of the triangle
+ if (foundCollision == false)
+ {
+ Vector3 velocity = packet.esVelocity;
+ Vector3 basePoint = packet.esPosition;
+ float velocitySquaredLength = velocity.LengthSquared;
+ float a, b, c;
+ float newT = 0.0f;
+
+ // For each vertex or edge, we have a quadratic equation to be solved
+ // Check against the points first
+
+ a = velocitySquaredLength;
+
+ // P1
+ b = 2.0f * Vector3.Dot(velocity, basePoint - p1);
+ c = (p1 - basePoint).LengthSquared - 1.0f;
+ if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
+ {
+ t = newT;
+ foundCollision = true;
+ collisionPoint = p1;
+ }
+
+ // P2
+ b = 2.0f * Vector3.Dot(velocity, basePoint - p2);
+ c = (p2 - basePoint).LengthSquared - 1.0f;
+ if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
+ {
+ t = newT;
+ foundCollision = true;
+ collisionPoint = p2;
+ }
+
+ // P3
+ b = 2.0f * Vector3.Dot(velocity, basePoint - p3);
+ c = (p3 - basePoint).LengthSquared - 1.0f;
+ if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
+ {
+ t = newT;
+ foundCollision = true;
+ collisionPoint = p3;
+ }
+
+ // Now check against the edges
+
+ // P1 -> P2
+ Vector3 edge = p2 - p1;
+ Vector3 baseToVertex = p1 - basePoint;
+ float edgeSquaredLength = edge.LengthSquared;
+ float edgeDotVelocity = Vector3.Dot(edge, velocity);
+ float edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);
+
+ a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
+ b = edgeSquaredLength * (2.0f * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
+ c = edgeSquaredLength * (1.0f - baseToVertex.LengthSquared) + edgeDotBaseToVertex * edgeDotBaseToVertex;
+
+ if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
+ {
+ // Check if intersection is within line segment
+ float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
+ if (f >= 0.0f && f <= 1.0f)
+ {
+ // Intersection took place within the segment
+ t = newT;
+ foundCollision = true;
+ collisionPoint = p1 + edge * f;
+ }
+ }
+
+ // P2 -> P3
+ edge = p3 - p2;
+ baseToVertex = p2 - basePoint;
+ edgeSquaredLength = edge.LengthSquared;
+ edgeDotVelocity = Vector3.Dot(edge, velocity);
+ edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);
+
+ a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
+ b = edgeSquaredLength * (2.0f * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
+ c = edgeSquaredLength * (1.0f - baseToVertex.LengthSquared) + edgeDotBaseToVertex * edgeDotBaseToVertex;
+
+ if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
+ {
+ // Check if intersection is within line segment
+ float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
+ if (f >= 0.0f && f <= 1.0f)
+ {
+ // Intersection took place within the segment
+ t = newT;
+ foundCollision = true;
+ collisionPoint = p2 + edge * f;
+ }
+ }
+
+ // P3 -> P1
+ edge = p1 - p3;
+ baseToVertex = p3 - basePoint;
+ edgeSquaredLength = edge.LengthSquared;
+ edgeDotVelocity = Vector3.Dot(edge, velocity);
+ edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);
+
+ a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
+ b = edgeSquaredLength * (2.0f * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
+ c = edgeSquaredLength * (1.0f - baseToVertex.LengthSquared) + edgeDotBaseToVertex * edgeDotBaseToVertex;
+
+ if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
+ {
+ // Check if intersection is within line segment
+ float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
+ if (f >= 0.0f && f <= 1.0f)
+ {
+ // Intersection took place within the segment
+ t = newT;
+ foundCollision = true;
+ collisionPoint = p3 + edge * f;
+ }
+ }
+ }
+
+ // Set result of test
+ if (foundCollision == true)
+ {
+ float distanceToCollision = t * packet.esVelocity.Length;
+
+ // Does this triangle qualify for the closest collision?
+ if (packet.FoundCollision == false || distanceToCollision < packet.NearestDistance)
+ {
+ packet.NearestDistance = distanceToCollision;
+ packet.esIntersectionPoint = collisionPoint;
+ packet.FoundCollision = true;
+ }
+ }
+ }
+
+ return foundCollision;
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/LineSegment.cs b/Blarg.GameFramework/Math/LineSegment.cs
new file mode 100644
index 0000000..b1ebd02
--- /dev/null
+++ b/Blarg.GameFramework/Math/LineSegment.cs
@@ -0,0 +1,77 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct LineSegment : IEquatable
+ {
+ public Vector3 A;
+ public Vector3 B;
+
+ public LineSegment(Vector3 a, Vector3 b)
+ : this(ref a, ref b)
+ {
+ }
+
+ public LineSegment(ref Vector3 a, ref Vector3 b)
+ {
+ A = a;
+ B = b;
+ }
+
+ public LineSegment(float ax, float ay, float az, float bx, float by, float bz)
+ {
+ A.X = ax;
+ A.Y = ay;
+ A.Z = az;
+ B.X = bx;
+ B.Y = by;
+ B.Z = bz;
+ }
+
+ public LineSegment(Ray ray, float length)
+ : this(ref ray, length)
+ {
+ }
+
+ public LineSegment(ref Ray ray, float length)
+ {
+ A = ray.Position;
+ B = ray.GetPositionAt(length);
+ }
+
+ public static bool operator ==(LineSegment left, LineSegment right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(LineSegment left, LineSegment right)
+ {
+ return !left.Equals(right);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is LineSegment)
+ return this.Equals((LineSegment)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(LineSegment other)
+ {
+ return (A == other.A && B == other.B);
+ }
+
+ public override int GetHashCode()
+ {
+ return A.GetHashCode() ^ B.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{{0}-{1}}}", A, B);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/MathConstants.cs b/Blarg.GameFramework/Math/MathConstants.cs
new file mode 100644
index 0000000..e820130
--- /dev/null
+++ b/Blarg.GameFramework/Math/MathConstants.cs
@@ -0,0 +1,28 @@
+using System;
+
+namespace Blarg.GameFramework
+{
+ public static class MathConstants
+ {
+ public const float Pi = 3.14159274f; // 180 degrees
+ public const float HalfPi = Pi / 2.0f; // 90 degrees
+ public const float QuarterPi = Pi / 4.0f; // 45 degrees
+ public const float TwoPi = Pi * 2.0f; // 360 degrees
+
+ public const float PiOver180 = Pi / 180.0f;
+
+ public const float Radians0 = 0.0f;
+ public const float Radians45 = Pi / 4.0f;
+ public const float Radians90 = Pi / 2.0f;
+ public const float Radians135 = (3.0f * Pi) / 4.0f;
+ public const float Radians180 = Pi;
+ public const float Radians225 = (5.0f * Pi) / 4.0f;
+ public const float Radians270 = (3.0f * Pi) / 2.0f;
+ public const float Radians315 = (7.0f * Pi) / 4.0f;
+ public const float Radians360 = Pi * 2.0f;
+
+ public const float Radians60 = (60.0f * MathConstants.PiOver180);
+
+ public const float Tolerance = 0.000001f;
+ }
+}
diff --git a/Blarg.GameFramework/Math/MathHelpers.cs b/Blarg.GameFramework/Math/MathHelpers.cs
new file mode 100644
index 0000000..c35020a
--- /dev/null
+++ b/Blarg.GameFramework/Math/MathHelpers.cs
@@ -0,0 +1,315 @@
+using System;
+
+namespace Blarg.GameFramework
+{
+ public static class MathHelpers
+ {
+ ///
+ /// Converts coordinates from spherical to cartesian.
+ ///
+ /// distance from the origin "O" to the point "P" (the "r" value)
+ /// the angle (in radians) between the zenith direction and the line segment OP
+ /// the signed angle (in radians) measured from the azimuth reference direction to the orthogonal projection of the line segment OP on the reference plane
+ /// the output cartesian coordinates
+ public static void GetCartesianCoordsFromSpherical(float radius, float angleTheta, float angleRho, out Vector3 cartesianCoords)
+ {
+ cartesianCoords.X = radius * (float)Math.Sin(angleTheta) * (float)Math.Sin(angleRho);
+ cartesianCoords.Y = radius * (float)Math.Cos(angleTheta);
+ cartesianCoords.Z = radius * (float)Math.Sin(angleTheta) * (float)Math.Cos(angleRho);
+ }
+
+ ///
+ /// Converts an angle around the Y axis to a direction vector that
+ /// lies flat on the XZ plane. Note that the angle is offset so that 0 degrees
+ /// points directly down the +Y axis on a 2D cartesian grid, instead of the +X
+ /// axis as one might expect.
+ ///
+ /// direction vector on the XZ plane
+ /// the Y axis angle (in radians)
+ public static Vector3 GetDirectionFromYAxisOrientation(float angle)
+ {
+ Vector3 facing;
+ facing.Y = 0.0f;
+
+ // TODO: perhaps the whole "90 degree offset" thing we're doing below
+ // is too scenario-specific. maybe have an overload of this function
+ // which accepts an offset angle parameter?
+ //
+ // GetPointOnCircle() returns an angle based on a 2D cartesian grid where
+ // 0 degrees points in the +X direction. We want it to point in the
+ // +Y direction (2D cartesian Y = our 3D Z), so we offset our angle by 90
+ // degrees before calling it to get the intended result
+ float adjustedAngle = RolloverClamp(angle - MathConstants.Radians90, MathConstants.Radians0, MathConstants.Radians360);
+ GetPointOnCircle(1.0f, adjustedAngle, out facing.X, out facing.Z);
+
+ return facing;
+ }
+
+ ///
+ /// Converts euler angles to an equivalent direction vector. Note that this just
+ /// uses one of many other ways to do this (the others using any other rotation
+ /// order).
+ ///
+ /// direction vector
+ /// the yaw rotation angle (in radians)
+ /// the pitch rotation angle (in radians)
+ public static Vector3 GetDirectionFromAngles(float yaw, float pitch)
+ {
+ Vector3 result;
+ result.X = (float)Math.Cos(yaw) * (float)Math.Cos(pitch);
+ result.Y = (float)Math.Sin(yaw) * (float)Math.Cos(pitch);
+ result.Z = (float)Math.Sin(pitch);
+
+ return result;
+ }
+
+ ///
+ /// Returns the angle between two 2D points. Note that the returned angle is
+ /// offset so that 0 degrees points directly down the +Y axis on a 2D cartesian
+ /// grid, instead of the +X axis as one might expect.
+ ///
+ /// the angle (in radians) between the two points
+ /// X coordinate of the first point
+ /// Y coordinate of the first point
+ /// X coordinate of the second point
+ /// Y coordinate of the second point
+ public static float GetAngleBetweenPoints(float x1, float y1, float x2, float y2)
+ {
+ float angle = (float)Math.Atan2(y2 - y1, x2 - x1);
+
+ // we offset the angle by 90 degrees to ensure 0 degrees points in the
+ // +Y direction on a 2D cartesian grid instead of +X. This corresponds with
+ // the rest of our direction coordinate system.
+ return angle - MathConstants.Radians90;
+ }
+
+ ///
+ /// Solves a quadratic equation and returns the lowest root.
+ ///
+ /// true if the quadratic could be solved, false if not
+ /// the value of the a variable in the quadratic equation
+ /// the value of the b variable in the quadratic equation
+ /// the value of the c variable in the quadratic equation
+ /// the maximum root value we will accept as an answer. anything that is higher will not be accepted as a solution
+ /// the variable to hold the lowest root value if one is found
+ public static bool GetLowestQuadraticRoot(float a, float b, float c, float maxR, out float root)
+ {
+ root = 0.0f;
+ float determinant = (b * b) - (4.0f * a * c);
+
+ // If the determinant is negative, there is no solution (can't square root a negative)
+ if (determinant < 0.0f)
+ return false;
+
+ float sqrtDeterminant = (float)Math.Sqrt(determinant);
+ float root1 = (-b - sqrtDeterminant) / (2 * a);
+ float root2 = (-b + sqrtDeterminant) / (2 * a);
+ // Sort so root1 <= root2
+ if (root1 > root2)
+ {
+ float temp = root2;
+ root2 = root1;
+ root1 = temp;
+ }
+
+ // Get the lowest root
+ if (root1 > 0 && root1 < maxR)
+ {
+ root = root1;
+ return true;
+ }
+
+ if (root2 > 0 && root2 < maxR)
+ {
+ root = root2;
+ return true;
+ }
+
+ // No valid solutions found
+ return false;
+ }
+
+ ///
+ /// Returns the 2D point on a circle's circumference given a radius and angle.
+ ///
+ /// the radius of the circle
+ /// the angle around the circle (in radians)
+ /// the variable to hold the X point on the circle's circumference
+ /// the variable to hold the Y point on the circle's circumference
+ public static void GetPointOnCircle(float radius, float angle, out float x, out float y)
+ {
+ x = radius * (float)Math.Cos(angle);
+ y = radius * (float)Math.Sin(angle);
+ }
+
+ ///
+ /// Returns the 2D point on a circle's circumference given a radius and angle.
+ ///
+ /// the radius of the circle
+ /// the angle around the circle (in radians)
+ /// the vector to hold the point on the circle's circumference
+ public static void GetPointOnCircle(float radius, float angle, out Vector2 point)
+ {
+ point.X = radius * (float)Math.Cos(angle);
+ point.Y = radius * (float)Math.Sin(angle);
+ }
+
+ ///
+ /// Clamps a value to a given range.
+ ///
+ /// the clamped value
+ /// the value to be clamped
+ /// the low end of the range to clamp to
+ /// the high end of the range to clamp to
+ public static float Clamp(float value, float low, float high)
+ {
+ if (value < low)
+ return low;
+ if (value > high)
+ return high;
+
+ return value;
+ }
+
+ ///
+ /// Clamps a value to a given range.
+ ///
+ /// the clamped value
+ /// the value to be clamped
+ /// the low end of the range to clamp to
+ /// the high end of the range to clamp to
+ public static int Clamp(int value, int low, int high)
+ {
+ if (value < low)
+ return low;
+ if (value > high)
+ return high;
+
+ return value;
+ }
+
+ ///
+ /// Converts degrees to radians.
+ ///
+ /// equivalent value in radians
+ /// degree value to convert
+ public static float DegreesToRadians(float degrees)
+ {
+ return degrees * MathConstants.PiOver180;
+ }
+
+ ///
+ /// Checks two floats for equality using a defined "tolerance" to account
+ /// for floating point rounding errors, etc.
+ ///
+ /// true if equal, false if not
+ /// first value to check
+ /// second value to check
+ /// tolerance value to use
+ public static bool IsCloseEnough(float a, float b, float tolerance = MathConstants.Tolerance)
+ {
+ return Math.Abs((a - b) / ((b == 0.0f) ? 1.0f : b)) < tolerance;
+ }
+
+ ///
+ /// Determines if a given number is a power of two.
+ ///
+ /// true if a power of two, false if not
+ /// number to check
+ public static bool IsPowerOf2(int n)
+ {
+ return (n != 0) && ((n & (n - 1)) == 0);
+ }
+
+ ///
+ /// Linearly interpolates between two values.
+ ///
+ /// first value (low end of range)
+ /// second value (high end of range)
+ /// the amount to interpolate between the two values
+ public static float Lerp(float a, float b, float t)
+ {
+ return a + (b - a) * t;
+ }
+
+ ///
+ /// Given a linearly interpolated value and the original range (high and
+ /// low) of the linear interpolation, this will determine what the original
+ /// 0.0 to 1.0 value (weight) was used to perform the interpolation.
+ ///
+ /// the interpolation value (weight, in the range 0.0 to 1.0) used in the original interpolation
+ /// first value (low end of range)
+ /// second value (high end of range)
+ /// the result of the original interpolation
+ public static float InverseLerp(float a, float b, float lerpValue)
+ {
+ return (lerpValue - a) / (b - a);
+ }
+
+ ///
+ /// Converts radians to degrees.
+ ///
+ /// equivalent value in degrees
+ /// radian value to convert
+ public static float RadiansToDegrees(float radians)
+ {
+ return radians * (1.0f / MathConstants.PiOver180);
+ }
+
+ ///
+ /// Clamps a value to a given range, but if the value is outside the range
+ /// instead of returning the low/high end of the range, this will continue
+ /// counting after moving to the opposite end of the range to arrive at a
+ /// final value.
+ ///
+ /// the clamped value
+ /// the value to be clamped
+ /// the low end of the range to clamp to
+ /// the high end of the range to clamp to
+ public static float RolloverClamp(float value, float low, float high)
+ {
+ float temp = value;
+ // TODO: this is really shitty... make it better
+ do
+ {
+ float range = Math.Abs(high - low);
+ if (temp < low)
+ temp = temp + range;
+ if (value > high)
+ temp = temp - range;
+ }
+ while (temp < low || temp > high); // loop through as many times as necessary to put the value within the low/high range
+
+ return temp;
+ }
+
+ ///
+ /// Re-scales a given value from an old min/max range to a new and
+ /// different min/max range such that the value is approximately
+ /// at the same distance between both min and max values.
+ ///
+ /// re-scaled value which will fall between newMin and newMax
+ /// the value to be rescaled which is currently between originalMin and originalMax
+ /// original min value (low end of range)
+ /// original max value (high end of range)
+ /// new min value (low end of range)
+ /// new max value (high end of range)
+ public static float ScaleRange(float value, float originalMin, float originalMax, float newMin, float newMax)
+ {
+ return (value / ((originalMax - originalMin) / (newMax - newMin))) + newMin;
+ }
+
+ ///
+ /// Interpolates between two values using a cubic equation.
+ ///
+ /// the interpolated value
+ /// low end of range to interpolate between
+ /// high end of range to interpolate between
+ /// amount to interpolate by (the weight)
+ public static float SmoothStep(float low, float high, float t)
+ {
+ float n = Clamp(t, 0.0f, 1.0f);
+ return Lerp(low, high, (n * n) * (3.0f - (2.0f * n)));
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Matrix3x3.cs b/Blarg.GameFramework/Math/Matrix3x3.cs
new file mode 100644
index 0000000..6c6589a
--- /dev/null
+++ b/Blarg.GameFramework/Math/Matrix3x3.cs
@@ -0,0 +1,668 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Matrix3x3 : IEquatable
+ {
+ public float M11;
+ public float M21;
+ public float M31;
+ public float M12;
+ public float M22;
+ public float M32;
+ public float M13;
+ public float M23;
+ public float M33;
+
+ private const int _11 = 0;
+ private const int _12 = 3;
+ private const int _13 = 6;
+ private const int _21 = 1;
+ private const int _22 = 4;
+ private const int _23 = 7;
+ private const int _31 = 2;
+ private const int _32 = 5;
+ private const int _33 = 8;
+
+ public float Determinant
+ {
+ get
+ {
+ return
+ M11 * M22 * M33 +
+ M12 * M23 * M31 +
+ M13 * M21 * M32 -
+ M11 * M23 * M32 -
+ M12 * M21 * M33 -
+ M13 * M22 * M31;
+ }
+ }
+
+ public Vector3 Forward
+ {
+ get { return new Vector3(M13, M23, M33); }
+ }
+
+ public Vector3 Backward
+ {
+ get { return new Vector3(-M13, -M23, -M33); }
+ }
+
+ public Vector3 Left
+ {
+ get { return new Vector3(M11, M21, M31); }
+ }
+
+ public Vector3 Right
+ {
+ get { return new Vector3(-M11, -M21, -M31); }
+ }
+
+ public Vector3 Up
+ {
+ get { return new Vector3(M12, M22, M32); }
+ }
+
+ public Vector3 Down
+ {
+ get { return new Vector3(-M12, -M22, -M32); }
+ }
+
+ public Matrix3x3(
+ float m11, float m12, float m13,
+ float m21, float m22, float m23,
+ float m31, float m32, float m33
+ )
+ {
+ M11 = m11;
+ M12 = m12;
+ M13 = m13;
+ M21 = m21;
+ M22 = m22;
+ M23 = m23;
+ M31 = m31;
+ M32 = m32;
+ M33 = m33;
+ }
+
+ public Matrix3x3(float[] m)
+ {
+ M11 = m[_11];
+ M12 = m[_12];
+ M13 = m[_13];
+ M21 = m[_21];
+ M22 = m[_22];
+ M23 = m[_23];
+ M31 = m[_31];
+ M32 = m[_32];
+ M33 = m[_33];
+ }
+
+ public void Set(
+ float m11, float m12, float m13,
+ float m21, float m22, float m23,
+ float m31, float m32, float m33
+ )
+ {
+ M11 = m11;
+ M12 = m12;
+ M13 = m13;
+ M21 = m21;
+ M22 = m22;
+ M23 = m23;
+ M31 = m31;
+ M32 = m32;
+ M33 = m33;
+ }
+
+ public void Set(float[] m)
+ {
+ M11 = m[_11];
+ M12 = m[_12];
+ M13 = m[_13];
+ M21 = m[_21];
+ M22 = m[_22];
+ M23 = m[_23];
+ M31 = m[_31];
+ M32 = m[_32];
+ M33 = m[_33];
+ }
+
+ public static readonly Matrix3x3 Identity = new Matrix3x3(
+ 1.0f, 0.0f, 0.0f,
+ 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f
+ );
+
+ public Quaternion ToQuaternion()
+ {
+ Quaternion result;
+ ToQuaternion(out result);
+ return result;
+ }
+
+ public void ToQuaternion(out Quaternion result)
+ {
+ Quaternion temp;
+
+ float n = M11 + M22 + M33;
+ if (n > 0.0f)
+ {
+ float a = (float)Math.Sqrt(n + 1.0f);
+ temp.W = a / 2.0f;
+ a = 0.5f / a;
+ temp.X = (M32 - M23) * a;
+ temp.Y = (M13 - M31) * a;
+ temp.Z = (M21 - M12) * a;
+ }
+ else if ((M11 >= M22) && (M11 >= M33))
+ {
+ float a = (float)Math.Sqrt(1.0f + M11 - M22 - M33);
+ float b = 0.5f / a;
+
+ temp.X = 0.5f * a;
+ temp.Y = (M21 + M12) * b;
+ temp.Z = (M31 + M13) * b;
+ temp.W = (M32 - M23) * b;
+ }
+ else if (M22 > M33)
+ {
+ float a = (float)Math.Sqrt(1.0f + M22 - M11 - M33);
+ float b = 0.5f / a;
+
+ temp.X = (M12 + M21) * b;
+ temp.Y = 0.5f * a;
+ temp.Z = (M23 + M32) * b;
+ temp.W = (M13 - M31) * b;
+ }
+ else
+ {
+ float a = (float)Math.Sqrt(1.0f + M33 - M11 - M22);
+ float b = 0.5f / a;
+
+ temp.X = (M13 + M31) * b;
+ temp.Y = (M23 + M32) * b;
+ temp.Z = 0.5f * a;
+ temp.W = (M21 - M12) * b;
+ }
+
+ Quaternion.Normalize(ref temp, out result);
+ }
+
+ public Matrix4x4 ToMatrix4x4()
+ {
+ Matrix4x4 result;
+ ToMatrix4x4(out result);
+ return result;
+ }
+
+ public void ToMatrix4x4(out Matrix4x4 result)
+ {
+ result.M11 = M11;
+ result.M12 = M12;
+ result.M13 = M13;
+ result.M14 = 0.0f;
+
+ result.M21 = M21;
+ result.M22 = M22;
+ result.M23 = M23;
+ result.M24 = 0.0f;
+
+ result.M31 = M31;
+ result.M32 = M32;
+ result.M33 = M33;
+ result.M34 = 0.0f;
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+ }
+
+ public void ToArray(float[] result)
+ {
+ if (result == null || result.Length < 9)
+ throw new ArgumentException();
+
+ result[0] = M11;
+ result[3] = M12;
+ result[6] = M13;
+ result[1] = M21;
+ result[4] = M22;
+ result[7] = M23;
+ result[2] = M31;
+ result[5] = M32;
+ result[8] = M33;
+ }
+
+ public float[] ToArray()
+ {
+ var result = new float[9];
+ ToArray(result);
+ return result;
+ }
+
+ public static Matrix3x3 CreateFromEulerAngles(float x, float y, float z)
+ {
+ Matrix3x3 result;
+ CreateFromEulerAngles(x, y, z, out result);
+ return result;
+ }
+
+ public static void CreateFromEulerAngles(float x, float y, float z, out Matrix3x3 result)
+ {
+ Matrix3x3 rotateZ;
+ Matrix3x3 rotateY;
+ Matrix3x3 rotateX;
+ Matrix3x3.CreateRotationZ(z, out rotateZ);
+ Matrix3x3.CreateRotationY(y, out rotateY);
+ Matrix3x3.CreateRotationX(x, out rotateX);
+
+ result = rotateZ * rotateY * rotateX;
+ }
+
+ public static Matrix3x3 CreateRotation(float angle, Vector3 axis)
+ {
+ Matrix3x3 result;
+ CreateRotation(angle, ref axis, out result);
+ return result;
+ }
+
+ public static void CreateRotation(float angle, ref Vector3 axis, out Matrix3x3 result)
+ {
+ float s = (float)Math.Sin(angle);
+ float c = (float)Math.Cos(angle);
+
+ result.M11 = (axis.X * axis.X) * (1.0f - c) + c;
+ result.M12 = (axis.X * axis.Y) * (1.0f - c) - (axis.Z * s);
+ result.M13 = (axis.X * axis.Z) * (1.0f - c) + (axis.Y * s);
+
+ result.M21 = (axis.Y * axis.X) * (1.0f - c) + (axis.Z * s);
+ result.M22 = (axis.Y * axis.Y) * (1.0f - c) + c;
+ result.M23 = (axis.Y * axis.Z) * (1.0f - c) - (axis.X * s);
+
+ result.M31 = (axis.Z * axis.X) * (1.0f - c) - (axis.Y * s);
+ result.M32 = (axis.Z * axis.Y) * (1.0f - c) + (axis.X * s);
+ result.M33 = (axis.Z * axis.Z) * (1.0f - c) + c;
+ }
+
+ public static Matrix3x3 CreateRotationX(float angle)
+ {
+ Matrix3x3 result;
+ CreateRotationX(angle, out result);
+ return result;
+ }
+
+ public static void CreateRotationX(float angle, out Matrix3x3 result)
+ {
+ float s = (float)Math.Sin(angle);
+ float c = (float)Math.Cos(angle);
+
+ result.M11 = 1.0f;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+
+ result.M21 = 0.0f;
+ result.M22 = c;
+ result.M23 = -s;
+
+ result.M31 = 0.0f;
+ result.M32 = s;
+ result.M33 = c;
+ }
+
+ public static Matrix3x3 CreateRotationY(float angle)
+ {
+ Matrix3x3 result;
+ CreateRotationY(angle, out result);
+ return result;
+ }
+
+ public static void CreateRotationY(float angle, out Matrix3x3 result)
+ {
+ float s = (float)Math.Sin(angle);
+ float c = (float)Math.Cos(angle);
+
+ result.M11 = c;
+ result.M12 = 0.0f;
+ result.M13 = s;
+
+ result.M21 = 0.0f;
+ result.M22 = 1.0f;
+ result.M23 = 0.0f;
+
+ result.M31 = -s;
+ result.M32 = 0.0f;
+ result.M33 = c;
+ }
+
+ public static Matrix3x3 CreateRotationZ(float angle)
+ {
+ Matrix3x3 result;
+ CreateRotationZ(angle, out result);
+ return result;
+ }
+
+ public static void CreateRotationZ(float angle, out Matrix3x3 result)
+ {
+ float s = (float)Math.Sin(angle);
+ float c = (float)Math.Cos(angle);
+
+ result.M11 = c;
+ result.M12 = -s;
+ result.M13 = 0.0f;
+
+ result.M21 = s;
+ result.M22 = c;
+ result.M23 = 0.0f;
+
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = 1.0f;
+ }
+
+ public static Matrix3x3 Inverse(Matrix3x3 m)
+ {
+ Matrix3x3 result;
+ Inverse(ref m, out result);
+ return result;
+ }
+
+ public static void Inverse(ref Matrix3x3 m, out Matrix3x3 result)
+ {
+ float d = m.Determinant;
+ if (MathHelpers.IsCloseEnough(d, 0.0f))
+ result = Matrix3x3.Identity;
+ else
+ {
+ Matrix3x3 temp;
+
+ d = 1.0f / d;
+
+ temp.M11 = d * (m.M22 * m.M33 - m.M32 * m.M23);
+ temp.M21 = d * (m.M31 * m.M23 - m.M21 * m.M33);
+ temp.M31 = d * (m.M21 * m.M32 - m.M31 * m.M22);
+
+ temp.M12 = d * (m.M32 * m.M13 - m.M12 * m.M33);
+ temp.M22 = d * (m.M11 * m.M33 - m.M31 * m.M13);
+ temp.M32 = d * (m.M31 * m.M12 - m.M11 * m.M32);
+
+ temp.M13 = d * (m.M12 * m.M23 - m.M22 * m.M13);
+ temp.M23 = d * (m.M21 * m.M13 - m.M11 * m.M23);
+ temp.M33 = d * (m.M11 * m.M22 - m.M21 * m.M12);
+
+ result = temp;
+ }
+ }
+
+ public static Matrix3x3 Transpose(Matrix3x3 m)
+ {
+ Matrix3x3 result;
+ Transpose(ref m, out result);
+ return result;
+ }
+
+ public static void Transpose(ref Matrix3x3 m, out Matrix3x3 result)
+ {
+ Matrix3x3 temp;
+
+ temp.M11 = m.M11;
+ temp.M12 = m.M21;
+ temp.M13 = m.M31;
+
+ temp.M21 = m.M12;
+ temp.M22 = m.M22;
+ temp.M23 = m.M32;
+
+ temp.M31 = m.M13;
+ temp.M32 = m.M23;
+ temp.M33 = m.M33;
+
+ result = temp;
+ }
+
+ public static Vector3 Transform(Matrix3x3 m, Vector3 v)
+ {
+ Vector3 result;
+ Transform(ref m, ref v, out result);
+ return result;
+ }
+
+ public static void Transform(ref Matrix3x3 m, ref Vector3 v, out Vector3 result)
+ {
+ Vector3 temp;
+
+ temp.X = v.X * m.M11 + v.Y * m.M12 + v.Z * m.M13;
+ temp.Y = v.X * m.M21 + v.Y * m.M22 + v.Z * m.M23;
+ temp.Z = v.X * m.M31 + v.Y * m.M32 + v.Z * m.M33;
+
+ result = temp;
+ }
+
+ public static Matrix3x3 Add(Matrix3x3 left, Matrix3x3 right)
+ {
+ Matrix3x3 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Add(ref Matrix3x3 left, ref Matrix3x3 right, out Matrix3x3 result)
+ {
+ result.M11 = left.M11 + right.M11;
+ result.M12 = left.M12 + right.M12;
+ result.M13 = left.M13 + right.M13;
+ result.M21 = left.M21 + right.M21;
+ result.M22 = left.M22 + right.M22;
+ result.M23 = left.M23 + right.M23;
+ result.M31 = left.M31 + right.M31;
+ result.M32 = left.M32 + right.M32;
+ result.M33 = left.M33 + right.M33;
+ }
+
+ public static Matrix3x3 Subtract(Matrix3x3 left, Matrix3x3 right)
+ {
+ Matrix3x3 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Subtract(ref Matrix3x3 left, ref Matrix3x3 right, out Matrix3x3 result)
+ {
+ result.M11 = left.M11 - right.M11;
+ result.M12 = left.M12 - right.M12;
+ result.M13 = left.M13 - right.M13;
+ result.M21 = left.M21 - right.M21;
+ result.M22 = left.M22 - right.M22;
+ result.M23 = left.M23 - right.M23;
+ result.M31 = left.M31 - right.M31;
+ result.M32 = left.M32 - right.M32;
+ result.M33 = left.M33 - right.M33;
+ }
+
+ public static Matrix3x3 Multiply(Matrix3x3 left, Matrix3x3 right)
+ {
+ Matrix3x3 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Matrix3x3 left, ref Matrix3x3 right, out Matrix3x3 result)
+ {
+ Matrix3x3 m;
+
+ m.M11 = left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31;
+ m.M12 = left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32;
+ m.M13 = left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33;
+
+ m.M21 = left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31;
+ m.M22 = left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32;
+ m.M23 = left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33;
+
+ m.M31 = left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31;
+ m.M32 = left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32;
+ m.M33 = left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33;
+
+ result = m;
+ }
+
+ public static Matrix3x3 Multiply(Matrix3x3 left, float right)
+ {
+ Matrix3x3 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Matrix3x3 left, float right, out Matrix3x3 result)
+ {
+ result.M11 = left.M11 * right;
+ result.M12 = left.M12 * right;
+ result.M13 = left.M13 * right;
+ result.M21 = left.M21 * right;
+ result.M22 = left.M22 * right;
+ result.M23 = left.M23 * right;
+ result.M31 = left.M31 * right;
+ result.M32 = left.M32 * right;
+ result.M33 = left.M33 * right;
+ }
+
+ public static Matrix3x3 Divide(Matrix3x3 left, Matrix3x3 right)
+ {
+ Matrix3x3 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Matrix3x3 left, ref Matrix3x3 right, out Matrix3x3 result)
+ {
+ result.M11 = left.M11 / right.M11;
+ result.M12 = left.M12 / right.M12;
+ result.M13 = left.M13 / right.M13;
+ result.M21 = left.M21 / right.M21;
+ result.M22 = left.M22 / right.M22;
+ result.M23 = left.M23 / right.M23;
+ result.M31 = left.M31 / right.M31;
+ result.M32 = left.M32 / right.M32;
+ result.M33 = left.M33 / right.M33;
+ }
+
+ public static Matrix3x3 Divide(Matrix3x3 left, float right)
+ {
+ Matrix3x3 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Matrix3x3 left, float right, out Matrix3x3 result)
+ {
+ result.M11 = left.M11 / right;
+ result.M12 = left.M12 / right;
+ result.M13 = left.M13 / right;
+ result.M21 = left.M21 / right;
+ result.M22 = left.M22 / right;
+ result.M23 = left.M23 / right;
+ result.M31 = left.M31 / right;
+ result.M32 = left.M32 / right;
+ result.M33 = left.M33 / right;
+ }
+
+ public static Matrix3x3 operator +(Matrix3x3 left, Matrix3x3 right)
+ {
+ Matrix3x3 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Matrix3x3 operator -(Matrix3x3 left, Matrix3x3 right)
+ {
+ Matrix3x3 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Matrix3x3 operator *(Matrix3x3 left, Matrix3x3 right)
+ {
+ Matrix3x3 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Matrix3x3 operator *(Matrix3x3 left, float right)
+ {
+ Matrix3x3 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static Matrix3x3 operator /(Matrix3x3 left, Matrix3x3 right)
+ {
+ Matrix3x3 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Matrix3x3 operator /(Matrix3x3 left, float right)
+ {
+ Matrix3x3 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static bool operator ==(Matrix3x3 left, Matrix3x3 right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Matrix3x3 left, Matrix3x3 right)
+ {
+ return !left.Equals(right);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Matrix3x3)
+ return this.Equals((Matrix3x3)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Matrix3x3 other)
+ {
+ return (
+ M11 == other.M11 &&
+ M12 == other.M12 &&
+ M13 == other.M13 &&
+ M21 == other.M21 &&
+ M22 == other.M22 &&
+ M23 == other.M23 &&
+ M31 == other.M31 &&
+ M32 == other.M32 &&
+ M33 == other.M33
+ );
+ }
+
+ public override int GetHashCode()
+ {
+ return (
+ M11.GetHashCode() ^
+ M12.GetHashCode() ^
+ M13.GetHashCode() ^
+ M21.GetHashCode() ^
+ M22.GetHashCode() ^
+ M23.GetHashCode() ^
+ M31.GetHashCode() ^
+ M32.GetHashCode() ^
+ M33.GetHashCode()
+ );
+ }
+
+ public override string ToString()
+ {
+ return String.Format(
+ "{{{0}, {1}, {2}\n {3}, {4}, {5}\n {6}, {7}, {8}}}",
+ M11, M12, M13, M21, M22, M23, M31, M32, M33
+ );
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Matrix4x4.cs b/Blarg.GameFramework/Math/Matrix4x4.cs
new file mode 100644
index 0000000..4a0e738
--- /dev/null
+++ b/Blarg.GameFramework/Math/Matrix4x4.cs
@@ -0,0 +1,1272 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Matrix4x4 : IEquatable
+ {
+ public float M11;
+ public float M21;
+ public float M31;
+ public float M41;
+ public float M12;
+ public float M22;
+ public float M32;
+ public float M42;
+ public float M13;
+ public float M23;
+ public float M33;
+ public float M43;
+ public float M14;
+ public float M24;
+ public float M34;
+ public float M44;
+
+ private const int _11 = 0;
+ private const int _12 = 4;
+ private const int _13 = 8;
+ private const int _14 = 12;
+ private const int _21 = 1;
+ private const int _22 = 5;
+ private const int _23 = 9;
+ private const int _24 = 13;
+ private const int _31 = 2;
+ private const int _32 = 6;
+ private const int _33 = 10;
+ private const int _34 = 14;
+ private const int _41 = 3;
+ private const int _42 = 7;
+ private const int _43 = 11;
+ private const int _44 = 15;
+
+ public float Determinant
+ {
+ get
+ {
+ return
+ (M11 * M22 - M21 * M12) *
+ (M33 * M44 - M43 * M34) -
+ (M11 * M32 - M31 * M12) *
+ (M23 * M44 - M43 * M24) +
+ (M11 * M42 - M41 * M12) *
+ (M23 * M34 - M33 * M24) +
+ (M21 * M32 - M31 * M22) *
+ (M13 * M44 - M43 * M14) -
+ (M21 * M42 - M41 * M22) *
+ (M13 * M34 - M33 * M14) +
+ (M31 * M42 - M41 * M32) *
+ (M13 * M24 - M23 * M14);
+ }
+ }
+
+ public Vector3 Forward
+ {
+ get { return new Vector3(M13, M23, M33); }
+ }
+
+ public Vector3 Backward
+ {
+ get { return new Vector3(-M13, -M23, -M33); }
+ }
+
+ public Vector3 Left
+ {
+ get { return new Vector3(M11, M21, M31); }
+ }
+
+ public Vector3 Right
+ {
+ get { return new Vector3(-M11, -M21, -M31); }
+ }
+
+ public Vector3 Up
+ {
+ get { return new Vector3(M12, M22, M32); }
+ }
+
+ public Vector3 Down
+ {
+ get { return new Vector3(-M12, -M22, -M32); }
+ }
+
+ public Vector3 Translation
+ {
+ get { return new Vector3(M14, M24, M34); }
+ }
+
+ public Matrix4x4(
+ float m11, float m12, float m13, float m14,
+ float m21, float m22, float m23, float m24,
+ float m31, float m32, float m33, float m34,
+ float m41, float m42, float m43, float m44
+ )
+ {
+ M11 = m11;
+ M12 = m12;
+ M13 = m13;
+ M14 = m14;
+ M21 = m21;
+ M22 = m22;
+ M23 = m23;
+ M24 = m24;
+ M31 = m31;
+ M32 = m32;
+ M33 = m33;
+ M34 = m34;
+ M41 = m41;
+ M42 = m42;
+ M43 = m43;
+ M44 = m44;
+ }
+
+ public Matrix4x4(float[] m)
+ {
+ M11 = m[_11];
+ M12 = m[_12];
+ M13 = m[_13];
+ M14 = m[_14];
+ M21 = m[_21];
+ M22 = m[_22];
+ M23 = m[_23];
+ M24 = m[_24];
+ M31 = m[_31];
+ M32 = m[_32];
+ M33 = m[_33];
+ M34 = m[_34];
+ M41 = m[_41];
+ M42 = m[_42];
+ M43 = m[_43];
+ M44 = m[_44];
+ }
+
+ public void Set(
+ float m11, float m12, float m13, float m14,
+ float m21, float m22, float m23, float m24,
+ float m31, float m32, float m33, float m34,
+ float m41, float m42, float m43, float m44
+ )
+ {
+ M11 = m11;
+ M12 = m12;
+ M13 = m13;
+ M14 = m14;
+ M21 = m21;
+ M22 = m22;
+ M23 = m23;
+ M24 = m24;
+ M31 = m31;
+ M32 = m32;
+ M33 = m33;
+ M34 = m34;
+ M41 = m41;
+ M42 = m42;
+ M43 = m43;
+ M44 = m44;
+ }
+
+ public void Set(float[] m)
+ {
+ M11 = m[_11];
+ M12 = m[_12];
+ M13 = m[_13];
+ M14 = m[_14];
+ M21 = m[_21];
+ M22 = m[_22];
+ M23 = m[_23];
+ M24 = m[_24];
+ M31 = m[_31];
+ M32 = m[_32];
+ M33 = m[_33];
+ M34 = m[_34];
+ M41 = m[_41];
+ M42 = m[_42];
+ M43 = m[_43];
+ M44 = m[_44];
+ }
+
+ public static readonly Matrix4x4 Identity = new Matrix4x4(
+ 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f
+ );
+
+ public Quaternion ToQuaternion()
+ {
+ Quaternion result;
+ ToQuaternion(out result);
+ return result;
+ }
+
+ public void ToQuaternion(out Quaternion result)
+ {
+ Quaternion temp;
+
+ float n = M11 + M22 + M33;
+ if (n > 0.0f)
+ {
+ float a = (float)Math.Sqrt(n + 1.0f);
+ temp.W = a / 2.0f;
+ a = 0.5f / a;
+ temp.X = (M32 - M23) * a;
+ temp.Y = (M13 - M31) * a;
+ temp.Z = (M21 - M12) * a;
+ }
+ else if ((M11 >= M22) && (M11 >= M33))
+ {
+ float a = (float)Math.Sqrt(1.0f + M11 - M22 - M33);
+ float b = 0.5f / a;
+
+ temp.X = 0.5f * a;
+ temp.Y = (M21 + M12) * b;
+ temp.Z = (M31 + M13) * b;
+ temp.W = (M32 - M23) * b;
+ }
+ else if (M22 > M33)
+ {
+ float a = (float)Math.Sqrt(1.0f + M22 - M11 - M33);
+ float b = 0.5f / a;
+
+ temp.X = (M12 + M21) * b;
+ temp.Y = 0.5f * a;
+ temp.Z = (M23 + M32) * b;
+ temp.W = (M13 - M31) * b;
+ }
+ else
+ {
+ float a = (float)Math.Sqrt(1.0f + M33 - M11 - M22);
+ float b = 0.5f / a;
+
+ temp.X = (M13 + M31) * b;
+ temp.Y = (M23 + M32) * b;
+ temp.Z = 0.5f * a;
+ temp.W = (M21 - M12) * b;
+ }
+
+ Quaternion.Normalize(ref temp, out result);
+ }
+
+ public Matrix3x3 ToMatrix3x3()
+ {
+ Matrix3x3 result;
+ ToMatrix3x3(out result);
+ return result;
+ }
+
+ public void ToMatrix3x3(out Matrix3x3 result)
+ {
+ result.M11 = M11;
+ result.M12 = M12;
+ result.M13 = M13;
+
+ result.M21 = M21;
+ result.M22 = M22;
+ result.M23 = M23;
+
+ result.M31 = M31;
+ result.M32 = M32;
+ result.M33 = M33;
+ }
+
+ public void ToArray(float[] result)
+ {
+ if (result == null || result.Length < 16)
+ throw new ArgumentException();
+
+ result[_11] = M11;
+ result[_12] = M12;
+ result[_13] = M13;
+ result[_14] = M14;
+ result[_21] = M21;
+ result[_22] = M22;
+ result[_23] = M23;
+ result[_24] = M24;
+ result[_31] = M31;
+ result[_32] = M32;
+ result[_33] = M33;
+ result[_34] = M34;
+ result[_41] = M41;
+ result[_42] = M42;
+ result[_43] = M43;
+ result[_44] = M44;
+ }
+
+ public float[] ToArray()
+ {
+ var result = new float[16];
+ ToArray(result);
+ return result;
+ }
+
+ public static Matrix4x4 CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 cameraUp, Vector3 cameraForward)
+ {
+ Matrix4x4 result;
+ CreateBillboard(ref objectPosition, ref cameraPosition, ref cameraUp, ref cameraForward, out result);
+ return result;
+ }
+
+ public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, ref Vector3 cameraUp, ref Vector3 cameraForward, out Matrix4x4 result)
+ {
+ Vector3 forward = objectPosition - cameraPosition;
+ float forwardLengthSquared = forward.LengthSquared;
+ if (forwardLengthSquared < 0.0001f)
+ forward = -cameraForward;
+ else
+ forward = forward * (1.0f / ((float)Math.Sqrt(forwardLengthSquared)));
+
+ Vector3 left = Vector3.Normalize(Vector3.Cross(cameraUp, forward));
+ Vector3 up = Vector3.Cross(forward, left);
+
+ result.M11 = left.X;
+ result.M21 = left.Y;
+ result.M31 = left.Z;
+ result.M41 = 0.0f;
+
+ result.M12 = up.X;
+ result.M22 = up.Y;
+ result.M32 = up.Z;
+ result.M42 = 0.0f;
+
+ result.M13 = forward.X;
+ result.M23 = forward.Y;
+ result.M33 = forward.Z;
+ result.M43 = 0.0f;
+
+ result.M14 = objectPosition.X;
+ result.M24 = objectPosition.Y;
+ result.M34 = objectPosition.Z;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateCylindricalBillboard(Vector3 objectPosition, Vector3 cameraPosition, Vector3 cameraForward, Vector3 axis)
+ {
+ Matrix4x4 result;
+ CreateCylindricalBillboard(ref objectPosition, ref cameraPosition, ref cameraForward, ref axis, out result);
+ return result;
+ }
+
+ public static void CreateCylindricalBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, ref Vector3 cameraForward, ref Vector3 axis, out Matrix4x4 result)
+ {
+ Vector3 temp = objectPosition - cameraPosition;
+ float lengthSquared = temp.LengthSquared;
+ if (lengthSquared < 0.0001f)
+ temp = -cameraForward;
+ else
+ temp = temp * (1.0f / ((float)Math.Sqrt(lengthSquared)));
+
+ Vector3 up = axis;
+ Vector3 forward;
+ Vector3 left;
+
+ float dot = Vector3.Dot(axis, temp);
+ if (Math.Abs(dot) > 0.9982547f)
+ {
+ dot = Vector3.Dot(axis, Vector3.Forward);
+ if (Math.Abs(dot) > 0.9982547f)
+ forward = Vector3.Right;
+ else
+ forward = Vector3.Forward;
+
+ left = Vector3.Normalize(Vector3.Cross(axis, forward));
+ forward = Vector3.Normalize(Vector3.Cross(left, axis));
+ }
+ else
+ {
+ left = Vector3.Normalize(Vector3.Cross(axis, temp));
+ forward = Vector3.Normalize(Vector3.Cross(left, up));
+ }
+
+ result.M11 = left.X;
+ result.M21 = left.Y;
+ result.M31 = left.Z;
+ result.M41 = 0.0f;
+
+ result.M12 = up.X;
+ result.M22 = up.Y;
+ result.M32 = up.Z;
+ result.M42 = 0.0f;
+
+ result.M13 = forward.X;
+ result.M23 = forward.Y;
+ result.M33 = forward.Z;
+ result.M43 = 0.0f;
+
+ result.M14 = objectPosition.X;
+ result.M24 = objectPosition.Y;
+ result.M34 = objectPosition.Z;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateScreenAlignedBillboard(Vector3 objectPosition, Vector3 cameraUp, Vector3 cameraForward)
+ {
+ Matrix4x4 result;
+ CreateScreenAlignedBillboard(ref objectPosition, ref cameraUp, ref cameraForward, out result);
+ return result;
+ }
+
+ public static void CreateScreenAlignedBillboard(ref Vector3 objectPosition, ref Vector3 cameraUp, ref Vector3 cameraForward, out Matrix4x4 result)
+ {
+ Vector3 left = Vector3.Normalize(Vector3.Cross(cameraUp, cameraForward));
+ Vector3 up = Vector3.Cross(cameraForward, left);
+
+ result.M11 = left.X;
+ result.M21 = left.Y;
+ result.M31 = left.Z;
+ result.M41 = 0.0f;
+
+ result.M12 = up.X;
+ result.M22 = up.Y;
+ result.M32 = up.Z;
+ result.M42 = 0.0f;
+
+ result.M13 = cameraForward.X;
+ result.M23 = cameraForward.Y;
+ result.M33 = cameraForward.Z;
+ result.M43 = 0.0f;
+
+ result.M14 = objectPosition.X;
+ result.M24 = objectPosition.Y;
+ result.M34 = objectPosition.Z;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateScreenAndAxisAlignedBillboard(Vector3 objectPosition, Vector3 cameraForward, Vector3 axis)
+ {
+ Matrix4x4 result;
+ CreateScreenAndAxisAlignedBillboard(ref objectPosition, ref cameraForward, ref axis, out result);
+ return result;
+ }
+
+ public static void CreateScreenAndAxisAlignedBillboard(ref Vector3 objectPosition, ref Vector3 cameraForward, ref Vector3 axis, out Matrix4x4 result)
+ {
+ Vector3 up = axis;
+ Vector3 forward;
+ Vector3 left;
+
+ float dot = Vector3.Dot(axis, cameraForward);
+ if (Math.Abs(dot) > 0.9982547f)
+ {
+ dot = Vector3.Dot(axis, Vector3.Forward);
+ if (Math.Abs(dot) > 0.9982547f)
+ forward = Vector3.Right;
+ else
+ forward = Vector3.Forward;
+
+ left = Vector3.Normalize(Vector3.Cross(axis, forward));
+ forward = Vector3.Normalize(Vector3.Cross(left, axis));
+ }
+ else
+ {
+ left = Vector3.Normalize(Vector3.Cross(axis, cameraForward));
+ forward = Vector3.Normalize(Vector3.Cross(left, up));
+ }
+
+ result.M11 = left.X;
+ result.M21 = left.Y;
+ result.M31 = left.Z;
+ result.M41 = 0.0f;
+
+ result.M12 = up.X;
+ result.M22 = up.Y;
+ result.M32 = up.Z;
+ result.M42 = 0.0f;
+
+ result.M13 = forward.X;
+ result.M23 = forward.Y;
+ result.M33 = forward.Z;
+ result.M43 = 0.0f;
+
+ result.M14 = objectPosition.X;
+ result.M24 = objectPosition.Y;
+ result.M34 = objectPosition.Z;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateFromEulerAngles(float x, float y, float z)
+ {
+ Matrix4x4 result;
+ CreateFromEulerAngles(x, y, z, out result);
+ return result;
+ }
+
+ public static void CreateFromEulerAngles(float x, float y, float z, out Matrix4x4 result)
+ {
+ Matrix4x4 rotateZ;
+ Matrix4x4 rotateY;
+ Matrix4x4 rotateX;
+ Matrix4x4.CreateRotationZ(z, out rotateZ);
+ Matrix4x4.CreateRotationY(y, out rotateY);
+ Matrix4x4.CreateRotationX(x, out rotateX);
+
+ result = rotateZ * rotateY * rotateX;
+ }
+
+ public static Matrix4x4 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUp)
+ {
+ Matrix4x4 result;
+ CreateLookAt(ref cameraPosition, ref cameraTarget, ref cameraUp, out result);
+ return result;
+ }
+
+ public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUp, out Matrix4x4 result)
+ {
+ // build basic lookat matrix without translation component included
+ Vector3 forward = Vector3.Normalize(cameraTarget - cameraPosition);
+ Vector3 left = Vector3.Normalize(Vector3.Cross(forward, cameraUp));
+ Vector3 up = Vector3.Cross(left, forward);
+
+ result.M11 = left.X;
+ result.M21 = up.X;
+ result.M31 = -forward.X;
+ result.M41 = 0.0f;
+
+ result.M12 = left.Y;
+ result.M22 = up.Y;
+ result.M32 = -forward.Y;
+ result.M42 = 0.0f;
+
+ result.M13 = left.Z;
+ result.M23 = up.Z;
+ result.M33 = -forward.Z;
+ result.M43 = 0.0f;
+
+ result.M14 = 0.0f;
+ result.M24 = 0.0f;
+ result.M34 = 0.0f;
+ result.M44 = 1.0f;
+
+ // multiply the translation into the lookat matrix
+ // this matrix multiplication is simplified so that we're only multiplying components that can actually affect the result
+ // out = Matrix4x4::CreateTranslation(-cameraPosition.x, -cameraPosition.y, -cameraPosition.z) * out;
+ result.M14 = result.M11 * -cameraPosition.X + result.M12 * -cameraPosition.Y + result.M13 * -cameraPosition.Z + result.M14;
+ result.M24 = result.M21 * -cameraPosition.X + result.M22 * -cameraPosition.Y + result.M23 * -cameraPosition.Z + result.M24;
+ result.M34 = result.M31 * -cameraPosition.X + result.M32 * -cameraPosition.Y + result.M33 * -cameraPosition.Z + result.M34;
+ result.M44 = result.M41 * -cameraPosition.X + result.M42 * -cameraPosition.Y + result.M43 * -cameraPosition.Z + result.M44;
+ }
+
+ public static Matrix4x4 CreateOrthographic(float left, float right, float bottom, float top, float near, float far)
+ {
+ Matrix4x4 result;
+ CreateOrthographic(left, right, bottom, top, near, far, out result);
+ return result;
+ }
+
+ public static void CreateOrthographic(float left, float right, float bottom, float top, float near, float far, out Matrix4x4 result)
+ {
+ result.M11 = 2.0f / (right - left);
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = -((right + left) / (right - left));
+
+ result.M21 = 0.0f;
+ result.M22 = 2.0f / (top - bottom);
+ result.M23 = 0.0f;
+ result.M24 = -((top + bottom) / (top - bottom));
+
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = -2.0f / (far - near);
+ result.M34 = -((far + near) / (far - near));
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreatePerspective(float left, float right, float bottom, float top, float near, float far)
+ {
+ Matrix4x4 result;
+ CreatePerspective(left, right, bottom, top, near, far, out result);
+ return result;
+ }
+
+ public static void CreatePerspective(float left, float right, float bottom, float top, float near, float far, out Matrix4x4 result)
+ {
+ result.M11 = (2.0f * near) / (right - left);
+ result.M12 = 0.0f;
+ result.M13 = (right + left) / (right - left);
+ result.M14 = 0.0f;
+
+ result.M21 = 0.0f;
+ result.M22 = (2.0f * near) / (top - bottom);
+ result.M23 = (top + bottom) / (top - bottom);
+ result.M24 = 0.0f;
+
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = -((far + near)) / (far - near);
+ result.M34 = -((2.0f * far * near)) / (far - near);
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = -1.0f;
+ result.M44 = 0.0f;
+ }
+
+ public static Matrix4x4 CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float near, float far)
+ {
+ Matrix4x4 result;
+ CreatePerspectiveFieldOfView(fieldOfView, aspectRatio, near, far, out result);
+ return result;
+ }
+
+ public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float near, float far, out Matrix4x4 result)
+ {
+ float f = 1.0f / (float)Math.Tan(fieldOfView / 2.0f);
+
+ result.M11 = f / aspectRatio;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+
+ result.M21 = 0.0f;
+ result.M22 = f;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = (far + near) / (near - far);
+ result.M34 = (2.0f * far * near) / (near - far);
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = -1.0f;
+ result.M44 = 0.0f;
+ }
+
+ public static Matrix4x4 CreateRotation(float angle, Vector3 axis)
+ {
+ Matrix4x4 result;
+ CreateRotation(angle, ref axis, out result);
+ return result;
+ }
+
+ public static void CreateRotation(float angle, ref Vector3 axis, out Matrix4x4 result)
+ {
+ float s = (float)Math.Sin(angle);
+ float c = (float)Math.Cos(angle);
+
+ result.M11 = (axis.X * axis.X) * (1.0f - c) + c;
+ result.M12 = (axis.X * axis.Y) * (1.0f - c) - (axis.Z * s);
+ result.M13 = (axis.X * axis.Z) * (1.0f - c) + (axis.Y * s);
+ result.M14 = 0.0f;
+
+ result.M21 = (axis.Y * axis.X) * (1.0f - c) + (axis.Z * s);
+ result.M22 = (axis.Y * axis.Y) * (1.0f - c) + c;
+ result.M23 = (axis.Y * axis.Z) * (1.0f - c) - (axis.X * s);
+ result.M24 = 0.0f;
+
+ result.M31 = (axis.Z * axis.X) * (1.0f - c) - (axis.Y * s);
+ result.M32 = (axis.Z * axis.Y) * (1.0f - c) + (axis.X * s);
+ result.M33 = (axis.Z * axis.Z) * (1.0f - c) + c;
+ result.M34 = 0.0f;
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateRotationX(float angle)
+ {
+ Matrix4x4 result;
+ CreateRotationX(angle, out result);
+ return result;
+ }
+
+ public static void CreateRotationX(float angle, out Matrix4x4 result)
+ {
+ float s = (float)Math.Sin(angle);
+ float c = (float)Math.Cos(angle);
+
+ result.M11 = 1.0f;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+
+ result.M21 = 0.0f;
+ result.M22 = c;
+ result.M23 = -s;
+ result.M24 = 0.0f;
+
+ result.M31 = 0.0f;
+ result.M32 = s;
+ result.M33 = c;
+ result.M34 = 0.0f;
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateRotationY(float angle)
+ {
+ Matrix4x4 result;
+ CreateRotationY(angle, out result);
+ return result;
+ }
+
+ public static void CreateRotationY(float angle, out Matrix4x4 result)
+ {
+ float s = (float)Math.Sin(angle);
+ float c = (float)Math.Cos(angle);
+
+ result.M11 = c;
+ result.M12 = 0.0f;
+ result.M13 = s;
+ result.M14 = 0.0f;
+
+ result.M21 = 0.0f;
+ result.M22 = 1.0f;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+
+ result.M31 = -s;
+ result.M32 = 0.0f;
+ result.M33 = c;
+ result.M34 = 0.0f;
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateRotationZ(float angle)
+ {
+ Matrix4x4 result;
+ CreateRotationZ(angle, out result);
+ return result;
+ }
+
+ public static void CreateRotationZ(float angle, out Matrix4x4 result)
+ {
+ float s = (float)Math.Sin(angle);
+ float c = (float)Math.Cos(angle);
+
+ result.M11 = c;
+ result.M12 = -s;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+
+ result.M21 = s;
+ result.M22 = c;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = 1.0f;
+ result.M34 = 0.0f;
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateScale(float x, float y, float z)
+ {
+ Matrix4x4 result;
+ CreateScale(x, y, z, out result);
+ return result;
+ }
+
+ public static void CreateScale(float x, float y, float z, out Matrix4x4 result)
+ {
+ result.M11 = x;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = 0.0f;
+
+ result.M21 = 0.0f;
+ result.M22 = y;
+ result.M23 = 0.0f;
+ result.M24 = 0.0f;
+
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = z;
+ result.M34 = 0.0f;
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateTranslation(float x, float y, float z)
+ {
+ Matrix4x4 result;
+ CreateTranslation(x, y, z, out result);
+ return result;
+ }
+
+ public static void CreateTranslation(float x, float y, float z, out Matrix4x4 result)
+ {
+ result.M11 = 1.0f;
+ result.M12 = 0.0f;
+ result.M13 = 0.0f;
+ result.M14 = x;
+
+ result.M21 = 0.0f;
+ result.M22 = 1.0f;
+ result.M23 = 0.0f;
+ result.M24 = y;
+
+ result.M31 = 0.0f;
+ result.M32 = 0.0f;
+ result.M33 = 1.0f;
+ result.M34 = z;
+
+ result.M41 = 0.0f;
+ result.M42 = 0.0f;
+ result.M43 = 0.0f;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 CreateWorld(Vector3 position, Vector3 forward, Vector3 up)
+ {
+ Matrix4x4 result;
+ CreateWorld(ref position, ref forward, ref up, out result);
+ return result;
+ }
+
+ public static void CreateWorld(ref Vector3 position, ref Vector3 forward, ref Vector3 up, out Matrix4x4 result)
+ {
+ Vector3 f = Vector3.Normalize(-forward);
+ Vector3 l = Vector3.Normalize(Vector3.Cross(up, f));
+ Vector3 u = Vector3.Cross(f, l);
+
+ result.M11 = l.X;
+ result.M21 = l.Y;
+ result.M31 = l.Z;
+ result.M41 = 0.0f;
+
+ result.M12 = u.X;
+ result.M22 = u.Y;
+ result.M32 = u.Z;
+ result.M42 = 0.0f;
+
+ result.M13 = f.X;
+ result.M23 = f.Y;
+ result.M33 = f.Z;
+ result.M43 = 0.0f;
+
+ result.M14 = position.X;
+ result.M24 = position.Y;
+ result.M34 = position.Z;
+ result.M44 = 1.0f;
+ }
+
+ public static Matrix4x4 Inverse(Matrix4x4 m)
+ {
+ Matrix4x4 result;
+ Inverse(ref m, out result);
+ return result;
+ }
+
+ public static void Inverse(ref Matrix4x4 m, out Matrix4x4 result)
+ {
+ float d = m.Determinant;
+ if (MathHelpers.IsCloseEnough(d, 0.0f))
+ result = Matrix4x4.Identity;
+ else
+ {
+ d = 1.0f / d;
+
+ Matrix4x4 temp;
+
+ temp.M11 = d * (m.M22 * (m.M33 * m.M44 - m.M43 * m.M34) + m.M32 * (m.M43 * m.M24 - m.M23 * m.M44) + m.M42 * (m.M23 * m.M34 - m.M33 * m.M24));
+ temp.M21 = d * (m.M23 * (m.M31 * m.M44 - m.M41 * m.M34) + m.M33 * (m.M41 * m.M24 - m.M21 * m.M44) + m.M43 * (m.M21 * m.M34 - m.M31 * m.M24));
+ temp.M31 = d * (m.M24 * (m.M31 * m.M42 - m.M41 * m.M32) + m.M34 * (m.M41 * m.M22 - m.M21 * m.M42) + m.M44 * (m.M21 * m.M32 - m.M31 * m.M22));
+ temp.M41 = d * (m.M21 * (m.M42 * m.M33 - m.M32 * m.M43) + m.M31 * (m.M22 * m.M43 - m.M42 * m.M23) + m.M41 * (m.M32 * m.M23 - m.M22 * m.M33));
+
+ temp.M12 = d * (m.M32 * (m.M13 * m.M44 - m.M43 * m.M14) + m.M42 * (m.M33 * m.M14 - m.M13 * m.M34) + m.M12 * (m.M43 * m.M34 - m.M33 * m.M44));
+ temp.M22 = d * (m.M33 * (m.M11 * m.M44 - m.M41 * m.M14) + m.M43 * (m.M31 * m.M14 - m.M11 * m.M34) + m.M13 * (m.M41 * m.M34 - m.M31 * m.M44));
+ temp.M32 = d * (m.M34 * (m.M11 * m.M42 - m.M41 * m.M12) + m.M44 * (m.M31 * m.M12 - m.M11 * m.M32) + m.M14 * (m.M41 * m.M32 - m.M31 * m.M42));
+ temp.M42 = d * (m.M31 * (m.M42 * m.M13 - m.M12 * m.M43) + m.M41 * (m.M12 * m.M33 - m.M32 * m.M13) + m.M11 * (m.M32 * m.M43 - m.M42 * m.M33));
+
+ temp.M13 = d * (m.M42 * (m.M13 * m.M24 - m.M23 * m.M14) + m.M12 * (m.M23 * m.M44 - m.M43 * m.M24) + m.M22 * (m.M43 * m.M14 - m.M13 * m.M44));
+ temp.M23 = d * (m.M43 * (m.M11 * m.M24 - m.M21 * m.M14) + m.M13 * (m.M21 * m.M44 - m.M41 * m.M24) + m.M23 * (m.M41 * m.M14 - m.M11 * m.M44));
+ temp.M33 = d * (m.M44 * (m.M11 * m.M22 - m.M21 * m.M12) + m.M14 * (m.M21 * m.M42 - m.M41 * m.M22) + m.M24 * (m.M41 * m.M12 - m.M11 * m.M42));
+ temp.M43 = d * (m.M41 * (m.M22 * m.M13 - m.M12 * m.M23) + m.M11 * (m.M42 * m.M23 - m.M22 * m.M43) + m.M21 * (m.M12 * m.M43 - m.M42 * m.M13));
+
+ temp.M14 = d * (m.M12 * (m.M33 * m.M24 - m.M23 * m.M34) + m.M22 * (m.M13 * m.M34 - m.M33 * m.M14) + m.M32 * (m.M23 * m.M14 - m.M13 * m.M24));
+ temp.M24 = d * (m.M13 * (m.M31 * m.M24 - m.M21 * m.M34) + m.M23 * (m.M11 * m.M34 - m.M31 * m.M14) + m.M33 * (m.M21 * m.M14 - m.M11 * m.M24));
+ temp.M34 = d * (m.M14 * (m.M31 * m.M22 - m.M21 * m.M32) + m.M24 * (m.M11 * m.M32 - m.M31 * m.M12) + m.M34 * (m.M21 * m.M12 - m.M11 * m.M22));
+ temp.M44 = d * (m.M11 * (m.M22 * m.M33 - m.M32 * m.M23) + m.M21 * (m.M32 * m.M13 - m.M12 * m.M33) + m.M31 * (m.M12 * m.M23 - m.M22 * m.M13));
+
+ result = temp;
+ }
+ }
+
+ public static Matrix4x4 Transpose(Matrix4x4 m)
+ {
+ Matrix4x4 result;
+ Transpose(ref m, out result);
+ return result;
+ }
+
+ public static void Transpose(ref Matrix4x4 m, out Matrix4x4 result)
+ {
+ Matrix4x4 temp;
+
+ temp.M11 = m.M11;
+ temp.M12 = m.M21;
+ temp.M13 = m.M31;
+ temp.M14 = m.M41;
+
+ temp.M21 = m.M12;
+ temp.M22 = m.M22;
+ temp.M23 = m.M32;
+ temp.M24 = m.M42;
+
+ temp.M31 = m.M13;
+ temp.M32 = m.M23;
+ temp.M33 = m.M33;
+ temp.M34 = m.M43;
+
+ temp.M41 = m.M14;
+ temp.M42 = m.M24;
+ temp.M43 = m.M34;
+ temp.M44 = m.M44;
+
+ result = temp;
+ }
+
+ public static Vector3 Transform(Matrix4x4 m, Vector3 v)
+ {
+ Vector3 result;
+ Transform(ref m, ref v, out result);
+ return result;
+ }
+
+ public static void Transform(ref Matrix4x4 m, ref Vector3 v, out Vector3 result)
+ {
+ Vector3 temp;
+ temp.X = v.X * m.M11 + v.Y * m.M12 + v.Z * m.M13 + m.M14;
+ temp.Y = v.X * m.M21 + v.Y * m.M22 + v.Z * m.M23 + m.M24;
+ temp.Z = v.X * m.M31 + v.Y * m.M32 + v.Z * m.M33 + m.M34;
+ result = temp;
+ }
+
+ public static Vector4 Transform(Matrix4x4 m, Vector4 v)
+ {
+ Vector4 result;
+ Transform(ref m, ref v, out result);
+ return result;
+ }
+
+ public static void Transform(ref Matrix4x4 m, ref Vector4 v, out Vector4 result)
+ {
+ Vector4 temp;
+ temp.X = v.X * m.M11 + v.Y * m.M12 + v.Z * m.M13 + v.W * m.M14;
+ temp.Y = v.X * m.M21 + v.Y * m.M22 + v.Z * m.M23 + v.W * m.M24;
+ temp.Z = v.X * m.M31 + v.Y * m.M32 + v.Z * m.M33 + v.W * m.M34;
+ temp.W = v.X * m.M41 + v.Y * m.M42 + v.Z * m.M43 + v.W * m.M44;
+ result = temp;
+ }
+
+ public static Vector3 TransformNormal(Matrix4x4 m, Vector3 v)
+ {
+ Vector3 result;
+ TransformNormal(ref m, ref v, out result);
+ return result;
+ }
+
+ public static void TransformNormal(ref Matrix4x4 m, ref Vector3 v, out Vector3 result)
+ {
+ Vector3 temp;
+ temp.X = v.X * m.M11 + v.Y * m.M12 + v.Z * m.M13;
+ temp.Y = v.X * m.M21 + v.Y * m.M22 + v.Z * m.M23;
+ temp.Z = v.X * m.M31 + v.Y * m.M32 + v.Z * m.M33;
+ result = temp;
+ }
+
+ public static Matrix4x4 Add(Matrix4x4 left, Matrix4x4 right)
+ {
+ Matrix4x4 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Add(ref Matrix4x4 left, ref Matrix4x4 right, out Matrix4x4 result)
+ {
+ result.M11 = left.M11 + right.M11;
+ result.M12 = left.M12 + right.M12;
+ result.M13 = left.M13 + right.M13;
+ result.M14 = left.M14 + right.M14;
+ result.M21 = left.M21 + right.M21;
+ result.M22 = left.M22 + right.M22;
+ result.M23 = left.M23 + right.M23;
+ result.M24 = left.M24 + right.M24;
+ result.M31 = left.M31 + right.M31;
+ result.M32 = left.M32 + right.M32;
+ result.M33 = left.M33 + right.M33;
+ result.M34 = left.M34 + right.M34;
+ result.M41 = left.M41 + right.M41;
+ result.M42 = left.M42 + right.M42;
+ result.M43 = left.M43 + right.M43;
+ result.M44 = left.M44 + right.M44;
+ }
+
+ public static Matrix4x4 Subtract(Matrix4x4 left, Matrix4x4 right)
+ {
+ Matrix4x4 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Subtract(ref Matrix4x4 left, ref Matrix4x4 right, out Matrix4x4 result)
+ {
+ result.M11 = left.M11 - right.M11;
+ result.M12 = left.M12 - right.M12;
+ result.M13 = left.M13 - right.M13;
+ result.M14 = left.M14 - right.M14;
+ result.M21 = left.M21 - right.M21;
+ result.M22 = left.M22 - right.M22;
+ result.M23 = left.M23 - right.M23;
+ result.M24 = left.M24 - right.M24;
+ result.M31 = left.M31 - right.M31;
+ result.M32 = left.M32 - right.M32;
+ result.M33 = left.M33 - right.M33;
+ result.M34 = left.M34 - right.M34;
+ result.M41 = left.M41 - right.M41;
+ result.M42 = left.M42 - right.M42;
+ result.M43 = left.M43 - right.M43;
+ result.M44 = left.M44 - right.M44;
+ }
+
+ public static Matrix4x4 Multiply(Matrix4x4 left, Matrix4x4 right)
+ {
+ Matrix4x4 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Matrix4x4 left, ref Matrix4x4 right, out Matrix4x4 result)
+ {
+ Matrix4x4 m;
+
+ m.M11 = left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31 + left.M14 * right.M41;
+ m.M12 = left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32 + left.M14 * right.M42;
+ m.M13 = left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33 + left.M14 * right.M43;
+ m.M14 = left.M11 * right.M14 + left.M12 * right.M24 + left.M13 * right.M34 + left.M14 * right.M44;
+
+ m.M21 = left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31 + left.M24 * right.M41;
+ m.M22 = left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32 + left.M24 * right.M42;
+ m.M23 = left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33 + left.M24 * right.M43;
+ m.M24 = left.M21 * right.M14 + left.M22 * right.M24 + left.M23 * right.M34 + left.M24 * right.M44;
+
+ m.M31 = left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31 + left.M34 * right.M41;
+ m.M32 = left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32 + left.M34 * right.M42;
+ m.M33 = left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33 + left.M34 * right.M43;
+ m.M34 = left.M31 * right.M14 + left.M32 * right.M24 + left.M33 * right.M34 + left.M34 * right.M44;
+
+ m.M41 = left.M41 * right.M11 + left.M42 * right.M21 + left.M43 * right.M31 + left.M44 * right.M41;
+ m.M42 = left.M41 * right.M12 + left.M42 * right.M22 + left.M43 * right.M32 + left.M44 * right.M42;
+ m.M43 = left.M41 * right.M13 + left.M42 * right.M23 + left.M43 * right.M33 + left.M44 * right.M43;
+ m.M44 = left.M41 * right.M14 + left.M42 * right.M24 + left.M43 * right.M34 + left.M44 * right.M44;
+
+ result = m;
+ }
+
+ public static Matrix4x4 Multiply(Matrix4x4 left, float right)
+ {
+ Matrix4x4 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Matrix4x4 left, float right, out Matrix4x4 result)
+ {
+ result.M11 = left.M11 * right;
+ result.M12 = left.M12 * right;
+ result.M13 = left.M13 * right;
+ result.M14 = left.M14 * right;
+ result.M21 = left.M21 * right;
+ result.M22 = left.M22 * right;
+ result.M23 = left.M23 * right;
+ result.M24 = left.M24 * right;
+ result.M31 = left.M31 * right;
+ result.M32 = left.M32 * right;
+ result.M33 = left.M33 * right;
+ result.M34 = left.M34 * right;
+ result.M41 = left.M41 * right;
+ result.M42 = left.M42 * right;
+ result.M43 = left.M43 * right;
+ result.M44 = left.M44 * right;
+ }
+
+ public static Matrix4x4 Divide(Matrix4x4 left, Matrix4x4 right)
+ {
+ Matrix4x4 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Matrix4x4 left, ref Matrix4x4 right, out Matrix4x4 result)
+ {
+ result.M11 = left.M11 / right.M11;
+ result.M12 = left.M12 / right.M12;
+ result.M13 = left.M13 / right.M13;
+ result.M14 = left.M14 / right.M14;
+ result.M21 = left.M21 / right.M21;
+ result.M22 = left.M22 / right.M22;
+ result.M23 = left.M23 / right.M23;
+ result.M24 = left.M24 / right.M24;
+ result.M31 = left.M31 / right.M31;
+ result.M32 = left.M32 / right.M32;
+ result.M33 = left.M33 / right.M33;
+ result.M34 = left.M34 / right.M34;
+ result.M41 = left.M41 / right.M41;
+ result.M42 = left.M42 / right.M42;
+ result.M43 = left.M43 / right.M43;
+ result.M44 = left.M44 / right.M44;
+ }
+
+ public static Matrix4x4 Divide(Matrix4x4 left, float right)
+ {
+ Matrix4x4 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Matrix4x4 left, float right, out Matrix4x4 result)
+ {
+ result.M11 = left.M11 / right;
+ result.M12 = left.M12 / right;
+ result.M13 = left.M13 / right;
+ result.M14 = left.M14 / right;
+ result.M21 = left.M21 / right;
+ result.M22 = left.M22 / right;
+ result.M23 = left.M23 / right;
+ result.M24 = left.M24 / right;
+ result.M31 = left.M31 / right;
+ result.M32 = left.M32 / right;
+ result.M33 = left.M33 / right;
+ result.M34 = left.M34 / right;
+ result.M41 = left.M41 / right;
+ result.M42 = left.M42 / right;
+ result.M43 = left.M43 / right;
+ result.M44 = left.M44 / right;
+ }
+
+ public static Matrix4x4 operator +(Matrix4x4 left, Matrix4x4 right)
+ {
+ Matrix4x4 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Matrix4x4 operator -(Matrix4x4 left, Matrix4x4 right)
+ {
+ Matrix4x4 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Matrix4x4 operator *(Matrix4x4 left, Matrix4x4 right)
+ {
+ Matrix4x4 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Matrix4x4 operator *(Matrix4x4 left, float right)
+ {
+ Matrix4x4 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static Matrix4x4 operator /(Matrix4x4 left, Matrix4x4 right)
+ {
+ Matrix4x4 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Matrix4x4 operator /(Matrix4x4 left, float right)
+ {
+ Matrix4x4 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static bool operator ==(Matrix4x4 left, Matrix4x4 right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Matrix4x4 left, Matrix4x4 right)
+ {
+ return !left.Equals(right);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Matrix4x4)
+ return this.Equals((Matrix4x4)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Matrix4x4 other)
+ {
+ return (
+ M11 == other.M11 &&
+ M12 == other.M12 &&
+ M13 == other.M13 &&
+ M14 == other.M14 &&
+ M21 == other.M21 &&
+ M22 == other.M22 &&
+ M23 == other.M23 &&
+ M24 == other.M24 &&
+ M31 == other.M31 &&
+ M32 == other.M32 &&
+ M33 == other.M33 &&
+ M34 == other.M34 &&
+ M41 == other.M41 &&
+ M42 == other.M42 &&
+ M43 == other.M43 &&
+ M44 == other.M44
+ );
+ }
+
+ public override int GetHashCode()
+ {
+ return (
+ M11.GetHashCode() ^
+ M12.GetHashCode() ^
+ M13.GetHashCode() ^
+ M14.GetHashCode() ^
+ M21.GetHashCode() ^
+ M22.GetHashCode() ^
+ M23.GetHashCode() ^
+ M24.GetHashCode() ^
+ M31.GetHashCode() ^
+ M32.GetHashCode() ^
+ M33.GetHashCode() ^
+ M34.GetHashCode() ^
+ M41.GetHashCode() ^
+ M42.GetHashCode() ^
+ M43.GetHashCode() ^
+ M44.GetHashCode()
+ );
+ }
+
+ public override string ToString()
+ {
+ return String.Format(
+ "{{{0}, {1}, {2}, {3}\n {4}, {5}, {6}, {7}\n {8}, {9}, {10}, {11}\n {12}, {13}, {14}, {15}}}",
+ M11, M12, M13, M14, M21, M22, M23, M24, M31, M32, M33, M34, M41, M42, M43, M44
+ );
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Plane.cs b/Blarg.GameFramework/Math/Plane.cs
new file mode 100644
index 0000000..d0ac9f9
--- /dev/null
+++ b/Blarg.GameFramework/Math/Plane.cs
@@ -0,0 +1,155 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ public enum PlanePointClassify
+ {
+ InFront = 0,
+ Behind = 1,
+ OnPlane = 2
+ }
+
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Plane : IEquatable
+ {
+ public Vector3 Normal;
+ public float D;
+
+ public Plane(float a, float b, float c, float d)
+ {
+ Normal.X = a;
+ Normal.Y = b;
+ Normal.Z = c;
+ D = d;
+ }
+
+ public Plane(Vector3 origin, Vector3 normal)
+ : this(ref origin, ref normal)
+ {
+ }
+
+ public Plane(ref Vector3 origin, ref Vector3 normal)
+ {
+ Normal = normal;
+ D = -Vector3.Dot(ref origin, ref normal);
+ }
+
+ public Plane(Vector3 a, Vector3 b, Vector3 c)
+ : this(ref a, ref b, ref c)
+ {
+ }
+
+ public Plane(ref Vector3 a, ref Vector3 b, ref Vector3 c)
+ {
+ Vector3 e3 = b - a;
+ Vector3 e1 = c - b;
+ Vector3 crossed = Vector3.Cross(e3, e1);
+ float scaleFactor = 1.0f / crossed.Length;
+
+ Normal = crossed * scaleFactor;
+
+ D = -Vector3.Dot(a, Normal);
+ }
+
+ public static PlanePointClassify ClassifyPoint(Plane plane, Vector3 point)
+ {
+ return ClassifyPoint(ref plane, ref point);
+ }
+
+ public static PlanePointClassify ClassifyPoint(ref Plane plane, float x, float y, float z)
+ {
+ Vector3 point = new Vector3(x, y, z);
+ return ClassifyPoint(ref plane, ref point);
+ }
+
+ public static PlanePointClassify ClassifyPoint(ref Plane plane, ref Vector3 point)
+ {
+ float planeDot = Vector3.Dot(ref plane.Normal, ref point) + plane.D;
+
+ if (planeDot < 0.0f)
+ return PlanePointClassify.Behind;
+ else if (planeDot > 0.0f)
+ return PlanePointClassify.InFront;
+ else
+ return PlanePointClassify.OnPlane;
+ }
+
+ public static float DistanceBetween(Plane plane, Vector3 point)
+ {
+ return DistanceBetween(ref plane, ref point);
+ }
+
+ public static float DistanceBetween(ref Plane plane, ref Vector3 point)
+ {
+ return Vector3.Dot(ref point, ref plane.Normal) + plane.D;
+ }
+
+ public static bool IsFrontFacingTo(Plane plane, Vector3 direction)
+ {
+ return IsFrontFacingTo(ref plane, ref direction);
+ }
+
+ public static bool IsFrontFacingTo(ref Plane plane, ref Vector3 direction)
+ {
+ if (Vector3.Dot(plane.Normal, direction) <= 0.0f)
+ return true;
+ else
+ return false;
+ }
+
+ public static Plane Normalize(Plane plane)
+ {
+ Plane result;
+ Normalize(ref plane, out result);
+ return result;
+ }
+
+ public static void Normalize(ref Plane plane, out Plane result)
+ {
+ float length = (float)Math.Sqrt(
+ (plane.Normal.X * plane.Normal.X) +
+ (plane.Normal.Y * plane.Normal.Y) +
+ (plane.Normal.Z * plane.Normal.Z)
+ );
+
+ result.Normal.X = plane.Normal.X / length;
+ result.Normal.Y = plane.Normal.Y / length;
+ result.Normal.Z = plane.Normal.Z / length;
+ result.D = plane.D / length;
+ }
+
+ public static bool operator ==(Plane left, Plane right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Plane left, Plane right)
+ {
+ return !left.Equals(right);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Plane)
+ return this.Equals((Plane)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Plane other)
+ {
+ return (Normal == other.Normal && D == other.D);
+ }
+
+ public override int GetHashCode()
+ {
+ return Normal.GetHashCode() ^ D.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{Normal:{0} D:{1}}}", Normal, D);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Point2.cs b/Blarg.GameFramework/Math/Point2.cs
new file mode 100644
index 0000000..e6b2ee5
--- /dev/null
+++ b/Blarg.GameFramework/Math/Point2.cs
@@ -0,0 +1,213 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Point2 : IEquatable
+ {
+ public int X;
+ public int Y;
+
+ public static readonly Point2 Zero = new Point2(0, 0);
+
+ public Point2(int x, int y)
+ {
+ X = x;
+ Y = y;
+ }
+
+ public Point2(Point2 other)
+ : this(ref other)
+ {
+ }
+
+ public Point2(ref Point2 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ }
+
+ public void Set(int x, int y)
+ {
+ X = x;
+ Y = y;
+ }
+
+ public void Set(Point2 other)
+ {
+ Set(ref other);
+ }
+
+ public void Set(ref Point2 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ }
+
+ public static Point2 Add(Point2 left, Point2 right)
+ {
+ Point2 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Add(ref Point2 left, ref Point2 right, out Point2 result)
+ {
+ result.X = left.X + right.X;
+ result.Y = left.Y + right.Y;
+ }
+
+ public static Point2 Subtract(Point2 left, Point2 right)
+ {
+ Point2 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Subtract(ref Point2 left, ref Point2 right, out Point2 result)
+ {
+ result.X = left.X - right.X;
+ result.Y = left.Y - right.Y;
+ }
+
+ public static Point2 Multiply(Point2 left, Point2 right)
+ {
+ Point2 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Point2 left, ref Point2 right, out Point2 result)
+ {
+ result.X = left.X * right.X;
+ result.Y = left.Y * right.Y;
+ }
+
+ public static Point2 Multiply(Point2 left, float right)
+ {
+ Point2 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Point2 left, float right, out Point2 result)
+ {
+ result.X = (int)((float)left.X * right);
+ result.Y = (int)((float)left.Y * right);
+ }
+
+ public static Point2 Divide(Point2 left, Point2 right)
+ {
+ Point2 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Point2 left, ref Point2 right, out Point2 result)
+ {
+ result.X = left.X / right.X;
+ result.Y = left.Y / right.Y;
+ }
+
+ public static Point2 Divide(Point2 left, float right)
+ {
+ Point2 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Point2 left, float right, out Point2 result)
+ {
+ result.X = (int)((float)left.X / right);
+ result.Y = (int)((float)left.Y / right);
+ }
+
+ public static Point2 operator +(Point2 left, Point2 right)
+ {
+ Point2 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Point2 operator -(Point2 left, Point2 right)
+ {
+ Point2 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Point2 operator *(Point2 left, Point2 right)
+ {
+ Point2 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Point2 operator *(Point2 left, float right)
+ {
+ Point2 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static Point2 operator *(float left, Point2 right)
+ {
+ Point2 result;
+ Multiply(ref right, left, out result);
+ return result;
+ }
+
+ public static Point2 operator /(Point2 left, Point2 right)
+ {
+ Point2 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Point2 operator /(Point2 left, float right)
+ {
+ Point2 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static bool operator ==(Point2 left, Point2 right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Point2 left, Point2 right)
+ {
+ return !left.Equals(right);
+ }
+
+ public static Point2 operator -(Point2 left)
+ {
+ return new Point2(-left.X, -left.Y);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Point2)
+ return this.Equals((Point2)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Point2 other)
+ {
+ return (X == other.X && Y == other.Y);
+ }
+
+ public override int GetHashCode()
+ {
+ return X.GetHashCode() ^ Y.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{X:{0} Y:{1}}}", X, Y);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Point3.cs b/Blarg.GameFramework/Math/Point3.cs
new file mode 100644
index 0000000..35da87b
--- /dev/null
+++ b/Blarg.GameFramework/Math/Point3.cs
@@ -0,0 +1,224 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Point3 : IEquatable
+ {
+ public int X;
+ public int Y;
+ public int Z;
+
+ public static readonly Point3 Zero = new Point3(0, 0, 0);
+
+ public Point3(int x, int y, int z)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ }
+
+ public Point3(Point3 other)
+ : this(ref other)
+ {
+ }
+
+ public Point3(ref Point3 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ Z = other.Z;
+ }
+
+ public void Set(int x, int y, int z)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ }
+
+ public void Set(Point3 other)
+ {
+ Set(ref other);
+ }
+
+ public void Set(ref Point3 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ Z = other.Z;
+ }
+
+ public static Point3 Add(Point3 left, Point3 right)
+ {
+ Point3 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Add(ref Point3 left, ref Point3 right, out Point3 result)
+ {
+ result.X = left.X + right.X;
+ result.Y = left.Y + right.Y;
+ result.Z = left.Z + right.Z;
+ }
+
+ public static Point3 Subtract(Point3 left, Point3 right)
+ {
+ Point3 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Subtract(ref Point3 left, ref Point3 right, out Point3 result)
+ {
+ result.X = left.X - right.X;
+ result.Y = left.Y - right.Y;
+ result.Z = left.Z - right.Z;
+ }
+
+ public static Point3 Multiply(Point3 left, Point3 right)
+ {
+ Point3 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Point3 left, ref Point3 right, out Point3 result)
+ {
+ result.X = left.X * right.X;
+ result.Y = left.Y * right.Y;
+ result.Z = left.Z * right.Z;
+ }
+
+ public static Point3 Multiply(Point3 left, float right)
+ {
+ Point3 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Point3 left, float right, out Point3 result)
+ {
+ result.X = (int)((float)left.X * right);
+ result.Y = (int)((float)left.Y * right);
+ result.Z = (int)((float)left.Z * right);
+ }
+
+ public static Point3 Divide(Point3 left, Point3 right)
+ {
+ Point3 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Point3 left, ref Point3 right, out Point3 result)
+ {
+ result.X = left.X / right.X;
+ result.Y = left.Y / right.Y;
+ result.Z = left.Z / right.Z;
+ }
+
+ public static Point3 Divide(Point3 left, float right)
+ {
+ Point3 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Point3 left, float right, out Point3 result)
+ {
+ result.X = (int)((float)left.X / right);
+ result.Y = (int)((float)left.Y / right);
+ result.Z = (int)((float)left.Z / right);
+ }
+
+ public static Point3 operator +(Point3 left, Point3 right)
+ {
+ Point3 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Point3 operator -(Point3 left, Point3 right)
+ {
+ Point3 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Point3 operator *(Point3 left, Point3 right)
+ {
+ Point3 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Point3 operator *(Point3 left, float right)
+ {
+ Point3 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static Point3 operator *(float left, Point3 right)
+ {
+ Point3 result;
+ Multiply(ref right, left, out result);
+ return result;
+ }
+
+ public static Point3 operator /(Point3 left, Point3 right)
+ {
+ Point3 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Point3 operator /(Point3 left, float right)
+ {
+ Point3 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static bool operator ==(Point3 left, Point3 right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Point3 left, Point3 right)
+ {
+ return !left.Equals(right);
+ }
+
+ public static Point3 operator -(Point3 left)
+ {
+ return new Point3(-left.X, -left.Y, -left.Z);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Point3)
+ return this.Equals((Point3)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Point3 other)
+ {
+ return (X == other.X && Y == other.Y && Z == other.Z);
+ }
+
+ public override int GetHashCode()
+ {
+ return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{X:{0} Y:{1} Z:{2}}}", X, Y, Z);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Quaternion.cs b/Blarg.GameFramework/Math/Quaternion.cs
new file mode 100644
index 0000000..437dc5d
--- /dev/null
+++ b/Blarg.GameFramework/Math/Quaternion.cs
@@ -0,0 +1,541 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Quaternion : IEquatable
+ {
+ public float X;
+ public float Y;
+ public float Z;
+ public float W;
+
+ public float Length
+ {
+ get
+ {
+ return (float)Math.Sqrt(
+ (W * W) +
+ (X * X) +
+ (Y * Y) +
+ (Z * Z)
+ );
+ }
+ }
+
+ public float LengthSquared
+ {
+ get
+ {
+ return
+ (W * W) +
+ (X * X) +
+ (Y * Y) +
+ (Z * Z);
+ }
+ }
+
+ public Vector3 Vector
+ {
+ get { return new Vector3(X, Y, Z); }
+ }
+
+ public Quaternion(float x, float y, float z, float w)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ W = w;
+ }
+
+ public Quaternion(Quaternion q)
+ : this(ref q)
+ {
+ }
+
+ public Quaternion(ref Quaternion q)
+ {
+ X = q.X;
+ Y = q.Y;
+ Z = q.Z;
+ W = q.W;
+ }
+
+ public void Set(float x, float y, float z, float w)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ W = w;
+ }
+
+ public void Set(Quaternion q)
+ {
+ Set(ref q);
+ }
+
+ public void Set(ref Quaternion q)
+ {
+ X = q.X;
+ Y = q.Y;
+ Z = q.Z;
+ W = q.W;
+ }
+
+ public static readonly Quaternion Identity = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
+
+ public Matrix4x4 ToMatrix4x4()
+ {
+ Matrix4x4 result;
+ ToMatrix4x4(out result);
+ return result;
+ }
+
+ public void ToMatrix4x4(out Matrix4x4 result)
+ {
+ result.M11 = 1.0f - (2.0f * ((Y * Y) + (Z * Z)));
+ result.M21 = 2.0f * ((X * Y) + (Z * W));
+ result.M31 = 2.0f * ((Z * X) - (Y * W));
+ result.M41 = 0.0f;
+
+ result.M12 = 2.0f * ((X * Y) - (Z * W));
+ result.M22 = 1.0f - (2.0f * ((Z * Z) + (X * X)));
+ result.M32 = 2.0f * ((Y * Z) + (X * W));
+ result.M42 = 0.0f;
+
+ result.M13 = 2.0f * ((Z * X) + (Y * W));
+ result.M23 = 2.0f * ((Y * Z) - (X * W));
+ result.M33 = 1.0f - (2.0f * ((Y * Y) + (X * X)));
+ result.M43 = 0.0f;
+
+ result.M14 = 0.0f;
+ result.M24 = 0.0f;
+ result.M34 = 0.0f;
+ result.M44 = 1.0f;
+ }
+
+ public Matrix3x3 ToMatrix3x3()
+ {
+ Matrix3x3 result;
+ ToMatrix3x3(out result);
+ return result;
+ }
+
+ public void ToMatrix3x3(out Matrix3x3 result)
+ {
+ result.M11 = 1.0f - (2.0f * ((Y * Y) + (Z * Z)));
+ result.M21 = 2.0f * ((X * Y) + (Z * W));
+ result.M31 = 2.0f * ((Z * X) - (Y * W));
+
+ result.M12 = 2.0f * ((X * Y) - (Z * W));
+ result.M22 = 1.0f - (2.0f * ((Z * Z) + (X * X)));
+ result.M32 = 2.0f * ((Y * Z) + (X * W));
+
+ result.M13 = 2.0f * ((Z * X) + (Y * W));
+ result.M23 = 2.0f * ((Y * Z) - (X * W));
+ result.M33 = 1.0f - (2.0f * ((Y * Y) + (X * X)));
+ }
+
+ public static Quaternion Conjugate(Quaternion q)
+ {
+ Quaternion result;
+ Conjugate(ref q, out result);
+ return result;
+ }
+
+ public static void Conjugate(ref Quaternion q, out Quaternion result)
+ {
+ result.X = -q.X;
+ result.Y = -q.Y;
+ result.Z = -q.Z;
+ result.W = q.W;
+ }
+
+ public static Quaternion CreateFromAxisAngle(float angle, Vector3 axis)
+ {
+ Quaternion result;
+ CreateFromAxisAngle(angle, ref axis, out result);
+ return result;
+ }
+
+ public static void CreateFromAxisAngle(float angle, ref Vector3 axis, out Quaternion result)
+ {
+ float c = (float)Math.Cos(angle / 2.0f);
+ float s = (float)Math.Sin(angle / 2.0f);
+
+ result.W = c;
+ result.X = axis.X * s;
+ result.Y = axis.Y * s;
+ result.Z = axis.Z * s;
+
+ result = Normalize(result);
+ }
+
+ public static Quaternion CreateFromEulerAngles(float x, float y, float z)
+ {
+ Quaternion result;
+ CreateFromEulerAngles(x, y, z, out result);
+ return result;
+ }
+
+ public static void CreateFromEulerAngles(float x, float y, float z, out Quaternion result)
+ {
+ var qx = new Quaternion((float)Math.Sin(x / 2.0f), 0.0f, 0.0f, (float)Math.Cos(x / 2.0f));
+ var qy = new Quaternion(0.0f, (float)Math.Sin(y / 2.0f), 0.0f, (float)Math.Cos(y / 2.0f));
+ var qz = new Quaternion(0.0f, 0.0f, (float)Math.Sin(z / 2.0f), (float)Math.Cos(z / 2.0f));
+
+ result = Normalize(qz * qy * qx);
+ }
+
+ public static Quaternion Cross(Quaternion a, Quaternion b)
+ {
+ Quaternion result;
+ Cross(ref a, ref b, out result);
+ return result;
+ }
+
+ public static void Cross(ref Quaternion a, ref Quaternion b, out Quaternion result)
+ {
+ result = a * b;
+ }
+
+ public static float Dot(Quaternion a, Quaternion b)
+ {
+ return Dot(ref a, ref b);
+ }
+
+ public static float Dot(ref Quaternion a, ref Quaternion b)
+ {
+ return (
+ (a.W * b.W) +
+ (a.X * b.X) +
+ (a.Y * b.Y) +
+ (a.Z * b.Z)
+ );
+ }
+
+ public static void ExtractAxisAngle(Quaternion q, out float angle, out Vector3 axis)
+ {
+ ExtractAxisAngle(ref q, out angle, out axis);
+ }
+
+ public static void ExtractAxisAngle(ref Quaternion q, out float angle, out Vector3 axis)
+ {
+ angle = 2.0f * (float)Math.Acos(q.W);
+ float n = (float)Math.Sqrt(1.0f - (q.W * q.W));
+ if (n > 0.0001f)
+ axis = q.Vector / n;
+ else
+ axis = Vector3.XAxis;
+ }
+
+ public static Quaternion Inverse(Quaternion q)
+ {
+ Quaternion result;
+ Inverse(ref q, out result);
+ return result;
+ }
+
+ public static void Inverse(ref Quaternion q, out Quaternion result)
+ {
+ float inverseSquaredLength = 1.0f / q.LengthSquared;
+ result.X = -q.X * inverseSquaredLength;
+ result.Y = -q.Y * inverseSquaredLength;
+ result.Z = -q.Z * inverseSquaredLength;
+ result.W = q.W * inverseSquaredLength;
+ }
+
+ public static Quaternion Lerp(Quaternion a, Quaternion b, float interpolation)
+ {
+ Quaternion result;
+ Lerp(ref a, ref b, interpolation, out result);
+ return result;
+ }
+
+ public static void Lerp(ref Quaternion a, ref Quaternion b, float interpolation, out Quaternion result)
+ {
+ result = (a * (1.0f - interpolation)) + (b * interpolation);
+ }
+
+ public static Quaternion Normalize(Quaternion q)
+ {
+ Quaternion result;
+ Normalize(ref q, out result);
+ return result;
+ }
+
+ public static void Normalize(ref Quaternion q, out Quaternion result)
+ {
+ float inverseLength = 1.0f / q.Length;
+ result.X = q.X * inverseLength;
+ result.Y = q.Y * inverseLength;
+ result.Z = q.Z * inverseLength;
+ result.W = q.W * inverseLength;
+ }
+
+ public static Quaternion Slerp(Quaternion a, Quaternion b, float interpolation)
+ {
+ Quaternion result;
+ Slerp(ref a, ref b, interpolation, out result);
+ return result;
+ }
+
+ public static void Slerp(ref Quaternion a, ref Quaternion b, float interpolation, out Quaternion result)
+ {
+ if (a.LengthSquared == 0.0f)
+ {
+ if (b.LengthSquared == 0.0f)
+ {
+ result = Identity;
+ return;
+ }
+ else
+ {
+ result = b;
+ return;
+ }
+ }
+ else if (b.LengthSquared == 0.0f)
+ {
+ result = a;
+ return;
+ }
+
+ Quaternion q1 = a;
+ Quaternion q2 = b;
+
+ float cosHalfAngle = q1.W * q2.W + Vector3.Dot(q1.Vector, q2.Vector);
+
+ if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
+ {
+ result = q1;
+ return;
+ }
+ else if (cosHalfAngle < 0.0f)
+ {
+ q2.X = -q2.X;
+ q2.Y = -q2.Y;
+ q2.Z = -q2.Z;
+ q2.W = -q2.W;
+ cosHalfAngle = -cosHalfAngle;
+ }
+
+ float blendA;
+ float blendB;
+ if (cosHalfAngle < 0.99f)
+ {
+ float halfAngle = (float)Math.Acos(cosHalfAngle);
+ float sinHalfAngle = (float)Math.Sin(halfAngle);
+ float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
+ blendA = (float)Math.Sin(halfAngle * (1.0f - interpolation)) * oneOverSinHalfAngle;
+ blendB = (float)Math.Sin(halfAngle * interpolation) * oneOverSinHalfAngle;
+ }
+ else
+ {
+ blendA = 1.0f - interpolation;
+ blendB = interpolation;
+ }
+
+ var v = q1.Vector * blendA + q2.Vector * blendB;
+ float w = q1.W * blendA + q2.W * blendB;
+ var temp = new Quaternion(v.X, v.Y, v.Z, w);
+ if (temp.LengthSquared > 0.0f)
+ result = Normalize(temp);
+ else
+ result = Identity;
+ }
+
+ public static Vector3 Transform(Quaternion q, Vector3 v)
+ {
+ Vector3 result;
+ Transform(ref q, ref v, out result);
+ return result;
+ }
+
+ public static void Transform(ref Quaternion q, ref Vector3 v, out Vector3 result)
+ {
+
+ Vector3 temp;
+ temp.X = ((v.X * ((1.0f - (q.Y * (q.Y + q.Y))) - (q.Z * (q.Z + q.Z)))) + (v.Y * ((q.X * (q.Y + q.Y)) - (q.W * (q.Z + q.Z))))) + (v.Z * ((q.X * (q.Z + q.Z)) + (q.W * (q.Y + q.Y))));
+ temp.Y = ((v.X * ((q.X * (q.Y + q.Y)) + (q.W * (q.Z + q.Z)))) + (v.Y * ((1.0f - (q.X * (q.X + q.X))) - (q.Z * (q.Z + q.Z))))) + (v.Z * ((q.Y * (q.Z + q.Z)) - (q.W * (q.X + q.X))));
+ temp.Z = ((v.X * ((q.X * (q.Z + q.Z)) - (q.W * (q.Y + q.Y)))) + (v.Y * ((q.Y * (q.Z + q.Z)) + (q.W * (q.X + q.X))))) + (v.Z * ((1.0f - (q.X * (q.X + q.X))) - (q.Y * (q.Y + q.Y))));
+ result = temp;
+ }
+
+ public static Quaternion Add(Quaternion left, Quaternion right)
+ {
+ Quaternion result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Add(ref Quaternion left, ref Quaternion right, out Quaternion result)
+ {
+ result.X = left.X + right.X;
+ result.Y = left.Y + right.Y;
+ result.Z = left.Z + right.Z;
+ result.W = left.W + right.W;
+ }
+
+ public static Quaternion Subtract(Quaternion left, Quaternion right)
+ {
+ Quaternion result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Subtract(ref Quaternion left, ref Quaternion right, out Quaternion result)
+ {
+ result.X = left.X - right.X;
+ result.Y = left.Y - right.Y;
+ result.Z = left.Z - right.Z;
+ result.W = left.W - right.W;
+ }
+
+ public static Quaternion Multiply(Quaternion left, Quaternion right)
+ {
+ Quaternion result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Quaternion left, ref Quaternion right, out Quaternion result)
+ {
+ float X = (left.W * right.X) + (left.X * right.W) + (left.Y * right.Z) - (left.Z * right.Y);
+ float Y = (left.W * right.Y) + (left.Y * right.W) + (left.Z * right.X) - (left.X * right.Z);
+ float Z = (left.W * right.Z) + (left.Z * right.W) + (left.X * right.Y) - (left.Y * right.X);
+ float W = (left.W * right.W) - (left.X * right.X) - (left.Y * right.Y) - (left.Z * right.Z);
+
+ result.X = X;
+ result.Y = Y;
+ result.Z = Z;
+ result.W = W;
+ }
+
+ public static Quaternion Multiply(Quaternion left, float right)
+ {
+ Quaternion result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Quaternion left, float right, out Quaternion result)
+ {
+ result.X = left.X * right;
+ result.Y = left.Y * right;
+ result.Z = left.Z * right;
+ result.W = left.W * right;
+ }
+
+ public static Quaternion Divide(Quaternion left, Quaternion right)
+ {
+ Quaternion result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Quaternion left, ref Quaternion right, out Quaternion result)
+ {
+ result.X = left.X / right.X;
+ result.Y = left.Y / right.Y;
+ result.Z = left.Z / right.Z;
+ result.W = left.W / right.W;
+ }
+
+ public static Quaternion Divide(Quaternion left, float right)
+ {
+ Quaternion result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Quaternion left, float right, out Quaternion result)
+ {
+ result.X = left.X / right;
+ result.Y = left.Y / right;
+ result.Z = left.Z / right;
+ result.W = left.W / right;
+ }
+
+ public static Quaternion operator +(Quaternion left, Quaternion right)
+ {
+ Quaternion result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Quaternion operator -(Quaternion left, Quaternion right)
+ {
+ Quaternion result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Quaternion operator *(Quaternion left, Quaternion right)
+ {
+ Quaternion result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Quaternion operator *(Quaternion left, float right)
+ {
+ Quaternion result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static Quaternion operator /(Quaternion left, Quaternion right)
+ {
+ Quaternion result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Quaternion operator /(Quaternion left, float right)
+ {
+ Quaternion result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static bool operator ==(Quaternion left, Quaternion right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Quaternion left, Quaternion right)
+ {
+ return !left.Equals(right);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Quaternion)
+ return this.Equals((Quaternion)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Quaternion other)
+ {
+ return (
+ X == other.X &&
+ Y == other.Y &&
+ Z == other.Z &&
+ W == other.W
+ );
+ }
+
+ public override int GetHashCode()
+ {
+ return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{X:{0} Y:{1} Z:{2} W:{3}}}", X, Y, Z, W);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Ray.cs b/Blarg.GameFramework/Math/Ray.cs
new file mode 100644
index 0000000..0e4243f
--- /dev/null
+++ b/Blarg.GameFramework/Math/Ray.cs
@@ -0,0 +1,80 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Ray : IEquatable
+ {
+ public Vector3 Position;
+ public Vector3 Direction;
+
+ public Ray(Vector3 position, Vector3 direction)
+ : this(ref position, ref direction)
+ {
+ }
+
+ public Ray(ref Vector3 position, ref Vector3 direction)
+ {
+ Position = position;
+ Direction = direction;
+ }
+
+ public Ray(float positionX, float positionY, float positionZ, float directionX, float directionY, float directionZ)
+ {
+ Position.X = positionX;
+ Position.Y = positionY;
+ Position.Z = positionZ;
+ Direction.X = directionX;
+ Direction.Y = directionY;
+ Direction.Z = directionZ;
+ }
+
+ public Vector3 GetPositionAt(float distance)
+ {
+ Vector3 result;
+ GetPositionAt(distance, out result);
+ return result;
+ }
+
+ public void GetPositionAt(float distance, out Vector3 result)
+ {
+ result.X = Direction.X * distance + Position.X;
+ result.Y = Direction.Y * distance + Position.Y;
+ result.Z = Direction.Z * distance + Position.Z;
+ }
+
+ public static bool operator ==(Ray left, Ray right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Ray left, Ray right)
+ {
+ return !left.Equals(right);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Ray)
+ return this.Equals((Ray)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Ray other)
+ {
+ return (Position == other.Position && Direction == other.Direction);
+ }
+
+ public override int GetHashCode()
+ {
+ return Position.GetHashCode() ^ Direction.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{Origin:{0} Direction:{1}}}", Position, Direction);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/RectF.cs b/Blarg.GameFramework/Math/RectF.cs
new file mode 100644
index 0000000..859f452
--- /dev/null
+++ b/Blarg.GameFramework/Math/RectF.cs
@@ -0,0 +1,113 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct RectF : IEquatable
+ {
+ public float Left;
+ public float Top;
+ public float Right;
+ public float Bottom;
+
+ public float Width
+ {
+ get { return Math.Abs(Right - Left); }
+ }
+
+ public float Height
+ {
+ get { return Math.Abs(Bottom - Top); }
+ }
+
+ public RectF(float left, float top, float right, float bottom)
+ {
+ Left = left;
+ Top = top;
+ Right = right;
+ Bottom = bottom;
+ }
+
+ public RectF(ref RectF r)
+ {
+ Left = r.Left;
+ Top = r.Top;
+ Right = r.Right;
+ Bottom = r.Bottom;
+ }
+
+ public RectF(RectF r)
+ {
+ Left = r.Left;
+ Top = r.Top;
+ Right = r.Right;
+ Bottom = r.Bottom;
+ }
+
+ public void Set(float left, float top, float right, float bottom)
+ {
+ Left = left;
+ Top = top;
+ Right = right;
+ Bottom = bottom;
+ }
+
+ public void Set(ref RectF r)
+ {
+ Left = r.Left;
+ Top = r.Top;
+ Right = r.Right;
+ Bottom = r.Bottom;
+ }
+
+ public void Set(RectF r)
+ {
+ Left = r.Left;
+ Top = r.Top;
+ Right = r.Right;
+ Bottom = r.Bottom;
+ }
+
+ public bool Contains(float x, float y)
+ {
+ if (x >= Left && y >= Top && x <= Right && y <= Bottom)
+ return true;
+ else
+ return false;
+ }
+
+ public static bool operator ==(RectF left, RectF right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(RectF left, RectF right)
+ {
+ return !left.Equals(right);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is RectF)
+ return this.Equals((RectF)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(RectF other)
+ {
+ return (Left == other.Left && Top == other.Top && Right == other.Right && Bottom == other.Bottom);
+ }
+
+ public override int GetHashCode()
+ {
+ return Left.GetHashCode() ^ Top.GetHashCode() ^ Right.GetHashCode() ^ Bottom.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{{0}-{1},{2}-{3}}}", Left, Top, Right, Bottom);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/SweptEllipsoidCollisionPacket.cs b/Blarg.GameFramework/Math/SweptEllipsoidCollisionPacket.cs
new file mode 100644
index 0000000..27d9a9e
--- /dev/null
+++ b/Blarg.GameFramework/Math/SweptEllipsoidCollisionPacket.cs
@@ -0,0 +1,21 @@
+using System;
+
+namespace Blarg.GameFramework
+{
+ public struct SweptEllipsoidCollisionPacket
+ {
+ // defines the x/y/z radius of the entity being checked
+ public Vector3 EllipsoidRadius;
+
+ public bool FoundCollision;
+ public float NearestDistance;
+
+ // the below fields are all in "ellipsoid space"
+
+ public Vector3 esVelocity; // velocity of the entity
+ public Vector3 esNormalizedVelocity;
+ public Vector3 esPosition; // current position of the entity
+
+ public Vector3 esIntersectionPoint; // if an intersection is found ...
+ }
+}
diff --git a/Blarg.GameFramework/Math/Transformation.cs b/Blarg.GameFramework/Math/Transformation.cs
new file mode 100644
index 0000000..62656cf
--- /dev/null
+++ b/Blarg.GameFramework/Math/Transformation.cs
@@ -0,0 +1,64 @@
+using System;
+
+namespace Blarg.GameFramework
+{
+ public class Transformation
+ {
+ public Matrix4x4 Transform = Matrix4x4.Identity;
+
+ public void Rotate(float radians, float axisX, float axisY, float axisZ)
+ {
+ var axis = new Vector3(axisX, axisY, axisZ);
+ Matrix4x4 rotation;
+ Matrix4x4.CreateRotation(radians, ref axis, out rotation);
+ Transform *= rotation;
+ }
+
+ public void Rotate(float radians, ref Vector3 axis)
+ {
+ Matrix4x4 rotation;
+ Matrix4x4.CreateRotation(radians, ref axis, out rotation);
+ Transform *= rotation;
+ }
+
+ public void Scale(float scaleFactor)
+ {
+ Matrix4x4 scale;
+ Matrix4x4.CreateScale(scaleFactor, scaleFactor, scaleFactor, out scale);
+ Transform *= scale;
+ }
+
+ public void Scale(float x, float y, float z)
+ {
+ Matrix4x4 scale;
+ Matrix4x4.CreateScale(x, y, z, out scale);
+ Transform *= scale;
+ }
+
+ public void Scale(ref Vector3 v)
+ {
+ Matrix4x4 scale;
+ Matrix4x4.CreateScale(v.X, v.Y, v.Z, out scale);
+ Transform *= scale;
+ }
+
+ public void Translate(float x, float y, float z)
+ {
+ Matrix4x4 translation;
+ Matrix4x4.CreateTranslation(x, y, z, out translation);
+ Transform *= translation;
+ }
+
+ public void Translate(ref Vector3 v)
+ {
+ Matrix4x4 translation;
+ Matrix4x4.CreateTranslation(v.X, v.Y, v.Z, out translation);
+ Transform *= translation;
+ }
+
+ public void Reset()
+ {
+ Transform = Matrix4x4.Identity;
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Vector2.cs b/Blarg.GameFramework/Math/Vector2.cs
new file mode 100644
index 0000000..694a822
--- /dev/null
+++ b/Blarg.GameFramework/Math/Vector2.cs
@@ -0,0 +1,337 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Vector2 : IEquatable
+ {
+ public float X;
+ public float Y;
+
+ public static readonly Vector2 Zero = new Vector2(0.0f, 0.0f);
+ public static readonly Vector2 XAxis = new Vector2(1.0f, 0.0f);
+ public static readonly Vector2 YAxis = new Vector2(0.0f, 1.0f);
+
+ public float Length
+ {
+ get
+ {
+ return (float)Math.Sqrt(
+ (X * X) +
+ (Y * Y)
+ );
+
+ }
+ }
+
+ public float LengthSquared
+ {
+ get
+ {
+ return
+ (X * X) +
+ (Y * Y);
+ }
+ }
+
+ public Vector2(float x, float y)
+ {
+ X = x;
+ Y = y;
+ }
+
+ public Vector2(Vector2 other)
+ : this(ref other)
+ {
+ }
+
+ public Vector2(ref Vector2 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ }
+
+ public Vector2(Point2 point)
+ : this(ref point)
+ {
+ }
+
+ public Vector2(ref Point2 point)
+ {
+ X = (float)point.X;
+ Y = (float)point.Y;
+ }
+
+ public void Set(float x, float y)
+ {
+ X = x;
+ Y = y;
+ }
+
+ public void Set(Vector2 other)
+ {
+ Set(ref other);
+ }
+
+ public void Set(ref Vector2 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ }
+
+ public void Set(Point2 point)
+ {
+ Set(ref point);
+ }
+
+ public void Set(ref Point2 point)
+ {
+ X = (float)point.X;
+ Y = (float)point.Y;
+ }
+
+ public static float Distance(Vector2 a, Vector2 b)
+ {
+ return Distance(ref a, ref b);
+ }
+
+ public static float Distance(ref Vector2 a, ref Vector2 b)
+ {
+ return (float)Math.Sqrt(
+ ((b.X - a.X) * (b.X - a.X)) +
+ ((b.Y - a.Y) * (b.Y - a.Y))
+ );
+ }
+
+ public static float DistanceSquared(Vector2 a, Vector2 b)
+ {
+ return DistanceSquared(ref a, ref b);
+ }
+
+ public static float DistanceSquared(ref Vector2 a, ref Vector2 b)
+ {
+ return
+ ((b.X - a.X) * (b.X - a.X)) +
+ ((b.Y - a.Y) * (b.Y - a.Y));
+ }
+
+ public static float Dot(Vector2 a, Vector2 b)
+ {
+ return Dot(ref a, ref b);
+ }
+
+ public static float Dot(ref Vector2 a, ref Vector2 b)
+ {
+ return
+ (a.X * b.X) +
+ (a.Y * b.Y);
+ }
+
+ public static Vector2 Lerp(Vector2 a, Vector2 b, float interpolation)
+ {
+ Vector2 result;
+ Lerp(ref a, ref b, interpolation, out result);
+ return result;
+ }
+
+ public static void Lerp(ref Vector2 a, ref Vector2 b, float interpolation, out Vector2 result)
+ {
+ result.X = a.X + (b.X - a.X) * interpolation;
+ result.Y = a.Y + (b.Y - a.Y) * interpolation;
+ }
+
+ public static Vector2 Normalize(Vector2 v)
+ {
+ Vector2 result;
+ Normalize(ref v, out result);
+ return result;
+ }
+
+ public static void Normalize(ref Vector2 v, out Vector2 result)
+ {
+ float inverseLength = 1.0f / v.Length;
+ result.X = v.X * inverseLength;
+ result.Y = v.Y * inverseLength;
+ }
+
+ public static Vector2 SetLength(Vector2 v, float length)
+ {
+ Vector2 result;
+ SetLength(ref v, length, out result);
+ return result;
+ }
+
+ public static void SetLength(ref Vector2 v, float length, out Vector2 result)
+ {
+ float temp = length / v.Length;
+ result.X = v.X * temp;
+ result.Y = v.Y * temp;
+ }
+
+ public static Vector2 Add(Vector2 left, Vector2 right)
+ {
+ Vector2 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Add(ref Vector2 left, ref Vector2 right, out Vector2 result)
+ {
+ result.X = left.X + right.X;
+ result.Y = left.Y + right.Y;
+ }
+
+ public static Vector2 Subtract(Vector2 left, Vector2 right)
+ {
+ Vector2 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Subtract(ref Vector2 left, ref Vector2 right, out Vector2 result)
+ {
+ result.X = left.X - right.X;
+ result.Y = left.Y - right.Y;
+ }
+
+ public static Vector2 Multiply(Vector2 left, Vector2 right)
+ {
+ Vector2 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Vector2 left, ref Vector2 right, out Vector2 result)
+ {
+ result.X = left.X * right.X;
+ result.Y = left.Y * right.Y;
+ }
+
+ public static Vector2 Multiply(Vector2 left, float right)
+ {
+ Vector2 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Vector2 left, float right, out Vector2 result)
+ {
+ result.X = left.X * right;
+ result.Y = left.Y * right;
+ }
+
+ public static Vector2 Divide(Vector2 left, Vector2 right)
+ {
+ Vector2 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Vector2 left, ref Vector2 right, out Vector2 result)
+ {
+ result.X = left.X / right.X;
+ result.Y = left.Y / right.Y;
+ }
+
+ public static Vector2 Divide(Vector2 left, float right)
+ {
+ Vector2 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Vector2 left, float right, out Vector2 result)
+ {
+ result.X = left.X / right;
+ result.Y = left.Y / right;
+ }
+
+ public static Vector2 operator +(Vector2 left, Vector2 right)
+ {
+ Vector2 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector2 operator -(Vector2 left, Vector2 right)
+ {
+ Vector2 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector2 operator *(Vector2 left, Vector2 right)
+ {
+ Vector2 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector2 operator *(Vector2 left, float right)
+ {
+ Vector2 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static Vector2 operator *(float left, Vector2 right)
+ {
+ Vector2 result;
+ Multiply(ref right, left, out result);
+ return result;
+ }
+
+ public static Vector2 operator /(Vector2 left, Vector2 right)
+ {
+ Vector2 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector2 operator /(Vector2 left, float right)
+ {
+ Vector2 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static bool operator ==(Vector2 left, Vector2 right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Vector2 left, Vector2 right)
+ {
+ return !left.Equals(right);
+ }
+
+ public static Vector2 operator -(Vector2 left)
+ {
+ return new Vector2(-left.X, -left.Y);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Vector2)
+ return this.Equals((Vector2)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Vector2 other)
+ {
+ return (X == other.X && Y == other.Y);
+ }
+
+ public override int GetHashCode()
+ {
+ return X.GetHashCode() ^ Y.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{X:{0} Y:{1}}}", X, Y);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Vector3.cs b/Blarg.GameFramework/Math/Vector3.cs
new file mode 100644
index 0000000..54aa593
--- /dev/null
+++ b/Blarg.GameFramework/Math/Vector3.cs
@@ -0,0 +1,391 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Vector3 : IEquatable
+ {
+ public float X;
+ public float Y;
+ public float Z;
+
+ public static readonly Vector3 Zero = new Vector3(0.0f, 0.0f, 0.0f);
+ public static readonly Vector3 XAxis = new Vector3(1.0f, 0.0f, 0.0f);
+ public static readonly Vector3 YAxis = new Vector3(0.0f, 1.0f, 0.0f);
+ public static readonly Vector3 ZAxis = new Vector3(0.0f, 0.0f, 1.0f);
+ public static readonly Vector3 Up = new Vector3(0.0f, 1.0f, 0.0f);
+ public static readonly Vector3 Down = new Vector3(0.0f, -1.0f, 0.0f);
+ public static readonly Vector3 Forward = new Vector3(0.0f, 0.0f, -1.0f);
+ public static readonly Vector3 Backward = new Vector3(0.0f, 0.0f, 1.0f);
+ public static readonly Vector3 Left = new Vector3(-1.0f, 0.0f, 0.0f);
+ public static readonly Vector3 Right = new Vector3(1.0f, 0.0f, 0.0f);
+
+ public float Length
+ {
+ get
+ {
+ return (float)Math.Sqrt(
+ (X * X) +
+ (Y * Y) +
+ (Z * Z)
+ );
+
+ }
+ }
+
+ public float LengthSquared
+ {
+ get
+ {
+ return
+ (X * X) +
+ (Y * Y) +
+ (Z * Z);
+ }
+ }
+
+ public Vector3(float x, float y, float z)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ }
+
+ public Vector3(Vector3 other)
+ : this(ref other)
+ {
+ }
+
+ public Vector3(ref Vector3 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ Z = other.Z;
+ }
+
+ public Vector3(Point3 point)
+ : this(ref point)
+ {
+ }
+
+ public Vector3(ref Point3 point)
+ {
+ X = (float)point.X;
+ Y = (float)point.Y;
+ Z = (float)point.Z;
+ }
+
+ public void Set(float x, float y, float z)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ }
+
+ public void Set(Vector3 other)
+ {
+ Set(ref other);
+ }
+
+ public void Set(ref Vector3 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ Z = other.Z;
+ }
+
+ public void Set(Point3 point)
+ {
+ Set(ref point);
+ }
+
+ public void Set(ref Point3 point)
+ {
+ X = (float)point.X;
+ Y = (float)point.Y;
+ Z = (float)point.Z;
+ }
+
+ public static Vector3 Cross(Vector3 a, Vector3 b)
+ {
+ Vector3 result;
+ Cross(ref a, ref b, out result);
+ return result;
+ }
+
+ public static void Cross(ref Vector3 a, ref Vector3 b, out Vector3 result)
+ {
+ result.X = (a.Y * b.Z) - (a.Z * b.Y);
+ result.Y = (a.Z * b.X) - (a.X * b.Z);
+ result.Z = (a.X * b.Y) - (a.Y * b.X);
+ }
+
+ public static float Distance(Vector3 a, Vector3 b)
+ {
+ return Distance(ref a, ref b);
+ }
+
+ public static float Distance(ref Vector3 a, ref Vector3 b)
+ {
+ return (float)Math.Sqrt(
+ ((b.X - a.X) * (b.X - a.X)) +
+ ((b.Y - a.Y) * (b.Y - a.Y)) +
+ ((b.Z - a.Z) * (b.Z - a.Z))
+ );
+ }
+
+ public static float DistanceSquared(Vector3 a, Vector3 b)
+ {
+ return DistanceSquared(ref a, ref b);
+ }
+
+ public static float DistanceSquared(ref Vector3 a, ref Vector3 b)
+ {
+ return
+ ((b.X - a.X) * (b.X - a.X)) +
+ ((b.Y - a.Y) * (b.Y - a.Y)) +
+ ((b.Z - a.Z) * (b.Z - a.Z));
+ }
+
+ public static float Dot(Vector3 a, Vector3 b)
+ {
+ return Dot(ref a, ref b);
+ }
+
+ public static float Dot(ref Vector3 a, ref Vector3 b)
+ {
+ return
+ (a.X * b.X) +
+ (a.Y * b.Y) +
+ (a.Z * b.Z);
+ }
+
+ public static Vector3 Lerp(Vector3 a, Vector3 b, float interpolation)
+ {
+ Vector3 result;
+ Lerp(ref a, ref b, interpolation, out result);
+ return result;
+ }
+
+ public static void Lerp(ref Vector3 a, ref Vector3 b, float interpolation, out Vector3 result)
+ {
+ result.X = a.X + (b.X - a.X) * interpolation;
+ result.Y = a.Y + (b.Y - a.Y) * interpolation;
+ result.Z = a.Z + (b.Z - a.Z) * interpolation;
+ }
+
+ public static Vector3 Normalize(Vector3 v)
+ {
+ Vector3 result;
+ Normalize(ref v, out result);
+ return result;
+ }
+
+ public static void Normalize(ref Vector3 v, out Vector3 result)
+ {
+ float inverseLength = 1.0f / v.Length;
+ result.X = v.X * inverseLength;
+ result.Y = v.Y * inverseLength;
+ result.Z = v.Z * inverseLength;
+ }
+
+ public static Vector3 SetLength(Vector3 v, float length)
+ {
+ Vector3 result;
+ SetLength(ref v, length, out result);
+ return result;
+ }
+
+ public static void SetLength(ref Vector3 v, float length, out Vector3 result)
+ {
+ float temp = length / v.Length;
+ result.X = v.X * temp;
+ result.Y = v.Y * temp;
+ result.Z = v.Z * temp;
+ }
+
+ public static Vector3 SurfaceNormal(Vector3 a, Vector3 b, Vector3 c)
+ {
+ Vector3 result;
+ SurfaceNormal(ref a, ref b, ref c, out result);
+ return result;
+ }
+
+ public static void SurfaceNormal(ref Vector3 a, ref Vector3 b, ref Vector3 c, out Vector3 result)
+ {
+ result = Normalize(Cross((b - a), (c - a)));
+ }
+
+ public static Vector3 Add(Vector3 left, Vector3 right)
+ {
+ Vector3 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Add(ref Vector3 left, ref Vector3 right, out Vector3 result)
+ {
+ result.X = left.X + right.X;
+ result.Y = left.Y + right.Y;
+ result.Z = left.Z + right.Z;
+ }
+
+ public static Vector3 Subtract(Vector3 left, Vector3 right)
+ {
+ Vector3 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Subtract(ref Vector3 left, ref Vector3 right, out Vector3 result)
+ {
+ result.X = left.X - right.X;
+ result.Y = left.Y - right.Y;
+ result.Z = left.Z - right.Z;
+ }
+
+ public static Vector3 Multiply(Vector3 left, Vector3 right)
+ {
+ Vector3 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Vector3 left, ref Vector3 right, out Vector3 result)
+ {
+ result.X = left.X * right.X;
+ result.Y = left.Y * right.Y;
+ result.Z = left.Z * right.Z;
+ }
+
+ public static Vector3 Multiply(Vector3 left, float right)
+ {
+ Vector3 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Vector3 left, float right, out Vector3 result)
+ {
+ result.X = left.X * right;
+ result.Y = left.Y * right;
+ result.Z = left.Z * right;
+ }
+
+ public static Vector3 Divide(Vector3 left, Vector3 right)
+ {
+ Vector3 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Vector3 left, ref Vector3 right, out Vector3 result)
+ {
+ result.X = left.X / right.X;
+ result.Y = left.Y / right.Y;
+ result.Z = left.Z / right.Z;
+ }
+
+ public static Vector3 Divide(Vector3 left, float right)
+ {
+ Vector3 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Vector3 left, float right, out Vector3 result)
+ {
+ result.X = left.X / right;
+ result.Y = left.Y / right;
+ result.Z = left.Z / right;
+ }
+
+ public static Vector3 operator +(Vector3 left, Vector3 right)
+ {
+ Vector3 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector3 operator -(Vector3 left, Vector3 right)
+ {
+ Vector3 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector3 operator *(Vector3 left, Vector3 right)
+ {
+ Vector3 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector3 operator *(Vector3 left, float right)
+ {
+ Vector3 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static Vector3 operator *(float left, Vector3 right)
+ {
+ Vector3 result;
+ Multiply(ref right, left, out result);
+ return result;
+ }
+
+ public static Vector3 operator /(Vector3 left, Vector3 right)
+ {
+ Vector3 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector3 operator /(Vector3 left, float right)
+ {
+ Vector3 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static bool operator ==(Vector3 left, Vector3 right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Vector3 left, Vector3 right)
+ {
+ return !left.Equals(right);
+ }
+
+ public static Vector3 operator -(Vector3 left)
+ {
+ return new Vector3(-left.X, -left.Y, -left.Z);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Vector3)
+ return this.Equals((Vector3)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Vector3 other)
+ {
+ return (X == other.X && Y == other.Y && Z == other.Z);
+ }
+
+ public override int GetHashCode()
+ {
+ return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{X:{0} Y:{1} Z:{2}}}", X, Y, Z);
+ }
+ }
+}
diff --git a/Blarg.GameFramework/Math/Vector4.cs b/Blarg.GameFramework/Math/Vector4.cs
new file mode 100644
index 0000000..9bfb708
--- /dev/null
+++ b/Blarg.GameFramework/Math/Vector4.cs
@@ -0,0 +1,351 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace Blarg.GameFramework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Vector4 : IEquatable
+ {
+ public float X;
+ public float Y;
+ public float Z;
+ public float W;
+
+ public static readonly Vector4 Zero = new Vector4(0.0f, 0.0f, 0.0f, 0.0f);
+
+ public float Length
+ {
+ get
+ {
+ return (float)Math.Sqrt(
+ (X * X) +
+ (Y * Y) +
+ (Z * Z) +
+ (W * W)
+ );
+
+ }
+ }
+
+ public float LengthSquared
+ {
+ get
+ {
+ return
+ (X * X) +
+ (Y * Y) +
+ (Z * Z) +
+ (W * W);
+ }
+ }
+
+ public Vector4(float x, float y, float z, float w)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ W = w;
+ }
+
+ public Vector4(Vector4 other)
+ : this(ref other)
+ {
+ }
+
+ public Vector4(ref Vector4 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ Z = other.Z;
+ W = other.W;
+ }
+
+ public void Set(float x, float y, float z, float w)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ W = w;
+ }
+
+ public void Set(Vector4 other)
+ {
+ Set(ref other);
+ }
+
+ public void Set(ref Vector4 other)
+ {
+ X = other.X;
+ Y = other.Y;
+ Z = other.Z;
+ W = other.W;
+ }
+
+ public static float Distance(Vector4 a, Vector4 b)
+ {
+ return Distance(ref a, ref b);
+ }
+
+ public static float Distance(ref Vector4 a, ref Vector4 b)
+ {
+ return (float)Math.Sqrt(
+ ((b.X - a.X) * (b.X - a.X)) +
+ ((b.Y - a.Y) * (b.Y - a.Y)) +
+ ((b.Z - a.Z) * (b.Z - a.Z)) +
+ ((b.W - a.W) * (b.W - a.W))
+ );
+ }
+
+ public static float DistanceSquared(Vector4 a, Vector4 b)
+ {
+ return DistanceSquared(ref a, ref b);
+ }
+
+ public static float DistanceSquared(ref Vector4 a, ref Vector4 b)
+ {
+ return
+ ((b.X - a.X) * (b.X - a.X)) +
+ ((b.Y - a.Y) * (b.Y - a.Y)) +
+ ((b.Z - a.Z) * (b.Z - a.Z)) +
+ ((b.W - a.W) * (b.W - a.W));
+ }
+
+ public static float Dot(Vector4 a, Vector4 b)
+ {
+ return Dot(ref a, ref b);
+ }
+
+ public static float Dot(ref Vector4 a, ref Vector4 b)
+ {
+ return
+ (a.X * b.X) +
+ (a.Y * b.Y) +
+ (a.Z * b.Z) +
+ (a.W * b.W);
+ }
+
+ public static Vector4 Lerp(Vector4 a, Vector4 b, float interpolation)
+ {
+ Vector4 result;
+ Lerp(ref a, ref b, interpolation, out result);
+ return result;
+ }
+
+ public static void Lerp(ref Vector4 a, ref Vector4 b, float interpolation, out Vector4 result)
+ {
+ result.X = a.X + (b.X - a.X) * interpolation;
+ result.Y = a.Y + (b.Y - a.Y) * interpolation;
+ result.Z = a.Z + (b.Z - a.Z) * interpolation;
+ result.W = a.W + (b.W - a.W) * interpolation;
+ }
+
+ public static Vector4 Normalize(Vector4 v)
+ {
+ Vector4 result;
+ Normalize(ref v, out result);
+ return result;
+ }
+
+ public static void Normalize(ref Vector4 v, out Vector4 result)
+ {
+ float inverseLength = 1.0f / v.Length;
+ result.X = v.X * inverseLength;
+ result.Y = v.Y * inverseLength;
+ result.Z = v.Z * inverseLength;
+ result.W = v.W * inverseLength;
+ }
+
+ public static Vector4 SetLength(Vector4 v, float length)
+ {
+ Vector4 result;
+ SetLength(ref v, length, out result);
+ return result;
+ }
+
+ public static void SetLength(ref Vector4 v, float length, out Vector4 result)
+ {
+ float temp = length / v.Length;
+ result.X = v.X * temp;
+ result.Y = v.Y * temp;
+ result.Z = v.Z * temp;
+ result.W = v.W * temp;
+ }
+
+ public static Vector4 Add(Vector4 left, Vector4 right)
+ {
+ Vector4 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Add(ref Vector4 left, ref Vector4 right, out Vector4 result)
+ {
+ result.X = left.X + right.X;
+ result.Y = left.Y + right.Y;
+ result.Z = left.Z + right.Z;
+ result.W = left.W + right.W;
+ }
+
+ public static Vector4 Subtract(Vector4 left, Vector4 right)
+ {
+ Vector4 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Subtract(ref Vector4 left, ref Vector4 right, out Vector4 result)
+ {
+ result.X = left.X - right.X;
+ result.Y = left.Y - right.Y;
+ result.Z = left.Z - right.Z;
+ result.W = left.W - right.W;
+ }
+
+ public static Vector4 Multiply(Vector4 left, Vector4 right)
+ {
+ Vector4 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Vector4 left, ref Vector4 right, out Vector4 result)
+ {
+ result.X = left.X * right.X;
+ result.Y = left.Y * right.Y;
+ result.Z = left.Z * right.Z;
+ result.W = left.W * right.W;
+ }
+
+ public static Vector4 Multiply(Vector4 left, float right)
+ {
+ Vector4 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static void Multiply(ref Vector4 left, float right, out Vector4 result)
+ {
+ result.X = left.X * right;
+ result.Y = left.Y * right;
+ result.Z = left.Z * right;
+ result.W = left.W * right;
+ }
+
+ public static Vector4 Divide(Vector4 left, Vector4 right)
+ {
+ Vector4 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Vector4 left, ref Vector4 right, out Vector4 result)
+ {
+ result.X = left.X / right.X;
+ result.Y = left.Y / right.Y;
+ result.Z = left.Z / right.Z;
+ result.W = left.W / right.W;
+ }
+
+ public static Vector4 Divide(Vector4 left, float right)
+ {
+ Vector4 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static void Divide(ref Vector4 left, float right, out Vector4 result)
+ {
+ result.X = left.X / right;
+ result.Y = left.Y / right;
+ result.Z = left.Z / right;
+ result.W = left.W / right;
+ }
+
+ public static Vector4 operator +(Vector4 left, Vector4 right)
+ {
+ Vector4 result;
+ Add(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector4 operator -(Vector4 left, Vector4 right)
+ {
+ Vector4 result;
+ Subtract(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector4 operator *(Vector4 left, Vector4 right)
+ {
+ Vector4 result;
+ Multiply(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector4 operator *(Vector4 left, float right)
+ {
+ Vector4 result;
+ Multiply(ref left, right, out result);
+ return result;
+ }
+
+ public static Vector4 operator *(float left, Vector4 right)
+ {
+ Vector4 result;
+ Multiply(ref right, left, out result);
+ return result;
+ }
+
+ public static Vector4 operator /(Vector4 left, Vector4 right)
+ {
+ Vector4 result;
+ Divide(ref left, ref right, out result);
+ return result;
+ }
+
+ public static Vector4 operator /(Vector4 left, float right)
+ {
+ Vector4 result;
+ Divide(ref left, right, out result);
+ return result;
+ }
+
+ public static bool operator ==(Vector4 left, Vector4 right)
+ {
+ return left.Equals(right);
+ }
+
+ public static bool operator !=(Vector4 left, Vector4 right)
+ {
+ return !left.Equals(right);
+ }
+
+ public static Vector4 operator -(Vector4 left)
+ {
+ return new Vector4(-left.X, -left.Y, -left.Z, -left.W);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (obj is Vector4)
+ return this.Equals((Vector4)obj);
+ else
+ return false;
+ }
+
+ public bool Equals(Vector4 other)
+ {
+ return (X == other.X && Y == other.Y && Z == other.Z && W == other.W);
+ }
+
+ public override int GetHashCode()
+ {
+ return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode();
+ }
+
+ public override string ToString()
+ {
+ return String.Format("{{X:{0} Y:{1} Z:{2} W:{3}}}", X, Y, Z, W);
+ }
+ }
+}