From de75de9d8342a396edd88ba5f0f9d5ab0d32b99d Mon Sep 17 00:00:00 2001 From: gered Date: Wed, 14 Aug 2013 19:26:42 -0400 Subject: [PATCH] add a bunch of ported math classes --- .../Blarg.GameFramework.csproj | 19 + Blarg.GameFramework/Math/BoundingBox.cs | 164 +++ Blarg.GameFramework/Math/BoundingSphere.cs | 134 ++ .../Math/IntersectionTester.cs | 527 +++++++ Blarg.GameFramework/Math/LineSegment.cs | 77 + Blarg.GameFramework/Math/MathConstants.cs | 28 + Blarg.GameFramework/Math/MathHelpers.cs | 315 ++++ Blarg.GameFramework/Math/Matrix3x3.cs | 668 +++++++++ Blarg.GameFramework/Math/Matrix4x4.cs | 1272 +++++++++++++++++ Blarg.GameFramework/Math/Plane.cs | 155 ++ Blarg.GameFramework/Math/Point2.cs | 213 +++ Blarg.GameFramework/Math/Point3.cs | 224 +++ Blarg.GameFramework/Math/Quaternion.cs | 541 +++++++ Blarg.GameFramework/Math/Ray.cs | 80 ++ Blarg.GameFramework/Math/RectF.cs | 113 ++ .../Math/SweptEllipsoidCollisionPacket.cs | 21 + Blarg.GameFramework/Math/Transformation.cs | 64 + Blarg.GameFramework/Math/Vector2.cs | 337 +++++ Blarg.GameFramework/Math/Vector3.cs | 391 +++++ Blarg.GameFramework/Math/Vector4.cs | 351 +++++ 20 files changed, 5694 insertions(+) create mode 100644 Blarg.GameFramework/Math/BoundingBox.cs create mode 100644 Blarg.GameFramework/Math/BoundingSphere.cs create mode 100644 Blarg.GameFramework/Math/IntersectionTester.cs create mode 100644 Blarg.GameFramework/Math/LineSegment.cs create mode 100644 Blarg.GameFramework/Math/MathConstants.cs create mode 100644 Blarg.GameFramework/Math/MathHelpers.cs create mode 100644 Blarg.GameFramework/Math/Matrix3x3.cs create mode 100644 Blarg.GameFramework/Math/Matrix4x4.cs create mode 100644 Blarg.GameFramework/Math/Plane.cs create mode 100644 Blarg.GameFramework/Math/Point2.cs create mode 100644 Blarg.GameFramework/Math/Point3.cs create mode 100644 Blarg.GameFramework/Math/Quaternion.cs create mode 100644 Blarg.GameFramework/Math/Ray.cs create mode 100644 Blarg.GameFramework/Math/RectF.cs create mode 100644 Blarg.GameFramework/Math/SweptEllipsoidCollisionPacket.cs create mode 100644 Blarg.GameFramework/Math/Transformation.cs create mode 100644 Blarg.GameFramework/Math/Vector2.cs create mode 100644 Blarg.GameFramework/Math/Vector3.cs create mode 100644 Blarg.GameFramework/Math/Vector4.cs 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); + } + } +}