parent
							
								
									5e2368bc7d
								
							
						
					
					
						commit
						0d93207ae9
					
				|  | @ -1,6 +1,6 @@ | |||
| using System; | ||||
| using System.Runtime.InteropServices; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Audio | ||||
| { | ||||
|  |  | |||
|  | @ -1,5 +1,5 @@ | |||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Audio | ||||
| { | ||||
|  |  | |||
|  | @ -1,6 +1,6 @@ | |||
| using System; | ||||
| using System.Runtime.InteropServices; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Audio | ||||
| { | ||||
|  | @ -53,7 +53,7 @@ namespace MoonWorks.Audio | |||
| 			get => _pitch; | ||||
| 			set | ||||
| 			{ | ||||
| 				_pitch = MathHelper.Clamp(value, -1f, 1f); | ||||
| 				_pitch = Math.MathHelper.Clamp(value, -1f, 1f); | ||||
| 				UpdatePitch(); | ||||
| 			} | ||||
| 		} | ||||
|  |  | |||
|  | @ -0,0 +1,181 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Axis-aligned bounding box. | ||||
| 	/// </summary> | ||||
| 	public struct AABB2D : System.IEquatable<AABB2D> | ||||
| 	{ | ||||
| 		/// <summary> | ||||
| 		/// The top-left position of the AABB. | ||||
| 		/// </summary> | ||||
| 		/// <value></value> | ||||
| 		public Vector2 Min { get; private set; } | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// The bottom-right position of the AABB. | ||||
| 		/// </summary> | ||||
| 		/// <value></value> | ||||
| 		public Vector2 Max { get; private set; } | ||||
| 
 | ||||
| 		public Fix64 Width { get { return Max.X - Min.X; } } | ||||
| 		public Fix64 Height { get { return Max.Y - Min.Y; } } | ||||
| 
 | ||||
| 		public Fix64 Right { get { return Max.X; } } | ||||
| 		public Fix64 Left { get { return Min.X; } } | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// The top of the AABB. Assumes a downward-aligned Y axis, so this value will be smaller than Bottom. | ||||
| 		/// </summary> | ||||
| 		/// <value></value> | ||||
| 		public Fix64 Top { get { return Min.Y; } } | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// The bottom of the AABB. Assumes a downward-aligned Y axis, so this value will be larger than Top. | ||||
| 		/// </summary> | ||||
| 		/// <value></value> | ||||
| 		public Fix64 Bottom { get { return Max.Y; } } | ||||
| 
 | ||||
| 		public AABB2D(Fix64 minX, Fix64 minY, Fix64 maxX, Fix64 maxY) | ||||
| 		{ | ||||
| 			Min = new Vector2(minX, minY); | ||||
| 			Max = new Vector2(maxX, maxY); | ||||
| 		} | ||||
| 
 | ||||
| 		public AABB2D(int minX, int minY, int maxX, int maxY) | ||||
| 		{ | ||||
| 			Min = new Vector2(minX, minY); | ||||
| 			Max = new Vector2(maxX, maxY); | ||||
| 		} | ||||
| 
 | ||||
| 		public AABB2D(Vector2 min, Vector2 max) | ||||
| 		{ | ||||
| 			Min = min; | ||||
| 			Max = max; | ||||
| 		} | ||||
| 
 | ||||
| 		private static Matrix3x2 AbsoluteMatrix(Matrix3x2 matrix) | ||||
| 		{ | ||||
| 			return new Matrix3x2 | ||||
| 			( | ||||
| 				Fix64.Abs(matrix.M11), Fix64.Abs(matrix.M12), | ||||
| 				Fix64.Abs(matrix.M21), Fix64.Abs(matrix.M22), | ||||
| 				Fix64.Abs(matrix.M31), Fix64.Abs(matrix.M32) | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Efficiently transforms the AABB by a Transform2D. | ||||
| 		/// </summary> | ||||
| 		/// <param name="aabb"></param> | ||||
| 		/// <param name="transform"></param> | ||||
| 		/// <returns></returns> | ||||
| 		public static AABB2D Transformed(AABB2D aabb, Transform2D transform) | ||||
| 		{ | ||||
| 			var two = new Fix64(2); | ||||
| 			var center = (aabb.Min + aabb.Max) / two; | ||||
|             var extent = (aabb.Max - aabb.Min) / two; | ||||
| 
 | ||||
|             var newCenter = Vector2.Transform(center, transform.TransformMatrix); | ||||
|             var newExtent = Vector2.TransformNormal(extent, AbsoluteMatrix(transform.TransformMatrix)); | ||||
| 
 | ||||
|             return new AABB2D(newCenter - newExtent, newCenter + newExtent); | ||||
| 		} | ||||
| 
 | ||||
| 		public AABB2D Compose(AABB2D aabb) | ||||
| 		{ | ||||
| 			Fix64 left = Left; | ||||
| 			Fix64 top = Top; | ||||
| 			Fix64 right = Right; | ||||
| 			Fix64 bottom = Bottom; | ||||
| 
 | ||||
| 			if (aabb.Left < left) | ||||
| 			{ | ||||
| 				left = aabb.Left; | ||||
| 			} | ||||
| 			if (aabb.Right > right) | ||||
| 			{ | ||||
| 				right = aabb.Right; | ||||
| 			} | ||||
| 			if (aabb.Top < top) | ||||
| 			{ | ||||
| 				top = aabb.Top; | ||||
| 			} | ||||
| 			if (aabb.Bottom > bottom) | ||||
| 			{ | ||||
| 				bottom = aabb.Bottom; | ||||
| 			} | ||||
| 
 | ||||
| 			return new AABB2D(left, top, right, bottom); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates an AABB for an arbitrary collection of positions. | ||||
| 		/// This is less efficient than defining a custom AABB method for most shapes, so avoid using this if possible. | ||||
| 		/// </summary> | ||||
| 		/// <param name="vertices"></param> | ||||
| 		/// <returns></returns> | ||||
| 		public static AABB2D FromVertices(IEnumerable<Vector2> vertices) | ||||
| 		{ | ||||
| 			var minX = Fix64.MaxValue; | ||||
| 			var minY = Fix64.MaxValue; | ||||
| 			var maxX = Fix64.MinValue; | ||||
| 			var maxY = Fix64.MinValue; | ||||
| 
 | ||||
| 			foreach (var vertex in vertices) | ||||
| 			{ | ||||
| 				if (vertex.X < minX) | ||||
| 				{ | ||||
| 					minX = vertex.X; | ||||
| 				} | ||||
| 				if (vertex.Y < minY) | ||||
| 				{ | ||||
| 					minY = vertex.Y; | ||||
| 				} | ||||
| 				if (vertex.X > maxX) | ||||
| 				{ | ||||
| 					maxX = vertex.X; | ||||
| 				} | ||||
| 				if (vertex.Y > maxY) | ||||
| 				{ | ||||
| 					maxY = vertex.Y; | ||||
| 				} | ||||
| 			} | ||||
| 
 | ||||
| 			return new AABB2D(minX, minY, maxX, maxY); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool TestOverlap(AABB2D a, AABB2D b) | ||||
| 		{ | ||||
| 			return a.Left < b.Right && a.Right > b.Left && a.Top < b.Bottom && a.Bottom > b.Top; | ||||
| 		} | ||||
| 
 | ||||
| 		public override bool Equals(object obj) | ||||
| 		{ | ||||
| 			return obj is AABB2D aabb && Equals(aabb); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(AABB2D other) | ||||
| 		{ | ||||
| 			return Min == other.Min && | ||||
| 				   Max == other.Max; | ||||
| 		} | ||||
| 
 | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return System.HashCode.Combine(Min, Max); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator ==(AABB2D left, AABB2D right) | ||||
| 		{ | ||||
| 			return left.Equals(right); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator !=(AABB2D left, AABB2D right) | ||||
| 		{ | ||||
| 			return !(left == right); | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
|  | @ -0,0 +1,12 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	public interface ICollidable | ||||
| 	{ | ||||
| 		IEnumerable<IShape2D> Shapes { get; } | ||||
| 		AABB2D AABB { get; } | ||||
| 		AABB2D TransformedAABB(Transform2D transform); | ||||
| 	} | ||||
| } | ||||
|  | @ -1,6 +1,6 @@ | |||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	public interface IShape2D : ICollidable, System.IEquatable<IShape2D> | ||||
| 	{ | ||||
|  | @ -1,6 +1,6 @@ | |||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A Minkowski difference between two shapes. | ||||
|  | @ -0,0 +1,333 @@ | |||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	public static class NarrowPhase | ||||
| 	{ | ||||
| 		private struct Edge | ||||
| 		{ | ||||
| 			public Fix64 Distance; | ||||
| 			public Vector2 Normal; | ||||
| 			public int Index; | ||||
| 		} | ||||
| 
 | ||||
|         public static bool TestCollision(ICollidable collidableA, Transform2D transformA, ICollidable collidableB, Transform2D transformB) | ||||
|         { | ||||
|             foreach (var shapeA in collidableA.Shapes) | ||||
|             { | ||||
|                 foreach (var shapeB in collidableB.Shapes) | ||||
|                 { | ||||
| 					if (TestCollision(shapeA, transformA, shapeB, transformB)) | ||||
|                     { | ||||
| 						return true; | ||||
| 					} | ||||
| 				} | ||||
|             } | ||||
| 
 | ||||
| 			return false; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) | ||||
| 		{ | ||||
| 			// If we can use a fast path check, let's do that! | ||||
| 			if (shapeA is Rectangle rectangleA && shapeB is Rectangle rectangleB && transformA.IsAxisAligned && transformB.IsAxisAligned) | ||||
| 			{ | ||||
| 				return TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB); | ||||
| 			} | ||||
| 			else if (shapeA is Point && shapeB is Rectangle && transformB.IsAxisAligned) | ||||
| 			{ | ||||
| 				return TestPointRectangleOverlap((Point) shapeA, transformA, (Rectangle) shapeB, transformB); | ||||
| 			} | ||||
| 			else if (shapeA is Rectangle && shapeB is Point && transformA.IsAxisAligned) | ||||
| 			{ | ||||
| 				return TestPointRectangleOverlap((Point) shapeB, transformB, (Rectangle) shapeA, transformA); | ||||
| 			} | ||||
| 			else if (shapeA is Rectangle && shapeB is Circle && transformA.IsAxisAligned && transformB.IsUniformScale) | ||||
| 			{ | ||||
| 				return TestCircleRectangleOverlap((Circle) shapeB, transformB, (Rectangle) shapeA, transformA); | ||||
| 			} | ||||
| 			else if (shapeA is Circle && shapeB is Rectangle && transformA.IsUniformScale && transformB.IsAxisAligned) | ||||
| 			{ | ||||
| 				return TestCircleRectangleOverlap((Circle) shapeA, transformA, (Rectangle) shapeB, transformB); | ||||
| 			} | ||||
| 			else if (shapeA is Circle && shapeB is Point && transformA.IsUniformScale) | ||||
| 			{ | ||||
| 				return TestCirclePointOverlap((Circle) shapeA, transformA, (Point) shapeB, transformB); | ||||
| 			} | ||||
| 			else if (shapeA is Point && shapeB is Circle && transformB.IsUniformScale) | ||||
| 			{ | ||||
| 				return TestCirclePointOverlap((Circle) shapeB, transformB, (Point) shapeA, transformA); | ||||
| 			} | ||||
| 			else if (shapeA is Circle circleA && shapeB is Circle circleB && transformA.IsUniformScale && transformB.IsUniformScale) | ||||
| 			{ | ||||
| 				return TestCircleOverlap(circleA, transformA, circleB, transformB); | ||||
| 			} | ||||
| 
 | ||||
| 			// Sad, we can't do a fast path optimization. Time for a simplex reduction. | ||||
| 			return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool TestRectangleOverlap(Rectangle rectangleA, Transform2D transformA, Rectangle rectangleB, Transform2D transformB) | ||||
| 		{ | ||||
| 			var firstAABB = rectangleA.TransformedAABB(transformA); | ||||
| 			var secondAABB = rectangleB.TransformedAABB(transformB); | ||||
| 
 | ||||
| 			return firstAABB.Left < secondAABB.Right && firstAABB.Right > secondAABB.Left && firstAABB.Top < secondAABB.Bottom && firstAABB.Bottom > secondAABB.Top; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool TestPointRectangleOverlap(Point point, Transform2D pointTransform, Rectangle rectangle, Transform2D rectangleTransform) | ||||
| 		{ | ||||
| 			var transformedPoint = pointTransform.Position; | ||||
| 			var AABB = rectangle.TransformedAABB(rectangleTransform); | ||||
| 
 | ||||
| 			return transformedPoint.X > AABB.Left && transformedPoint.X < AABB.Right && transformedPoint.Y < AABB.Bottom && transformedPoint.Y > AABB.Top; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool TestCirclePointOverlap(Circle circle, Transform2D circleTransform, Point point, Transform2D pointTransform) | ||||
| 		{ | ||||
| 			var circleCenter = circleTransform.Position; | ||||
| 			var circleRadius = circle.Radius * circleTransform.Scale.X; | ||||
| 
 | ||||
| 			var distanceX = circleCenter.X - pointTransform.Position.X; | ||||
| 			var distanceY = circleCenter.Y - pointTransform.Position.Y; | ||||
| 
 | ||||
| 			return (distanceX * distanceX) + (distanceY * distanceY) < (circleRadius * circleRadius); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// NOTE: The rectangle must be axis aligned, and the scaling of the circle must be uniform. | ||||
| 		/// </summary> | ||||
| 		public static bool TestCircleRectangleOverlap(Circle circle, Transform2D circleTransform, Rectangle rectangle, Transform2D rectangleTransform) | ||||
| 		{ | ||||
| 			var circleCenter = circleTransform.Position; | ||||
| 			var circleRadius = circle.Radius * circleTransform.Scale.X; | ||||
| 			var AABB = rectangle.TransformedAABB(rectangleTransform); | ||||
| 
 | ||||
| 			var closestX = Fix64.Clamp(circleCenter.X, AABB.Left, AABB.Right); | ||||
| 			var closestY = Fix64.Clamp(circleCenter.Y, AABB.Top, AABB.Bottom); | ||||
| 
 | ||||
| 			var distanceX = circleCenter.X - closestX; | ||||
| 			var distanceY = circleCenter.Y - closestY; | ||||
| 
 | ||||
| 			var distanceSquared = (distanceX * distanceX) + (distanceY * distanceY); | ||||
| 			return distanceSquared < (circleRadius * circleRadius); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool TestCircleOverlap(Circle circleA, Transform2D transformA, Circle circleB, Transform2D transformB) | ||||
| 		{ | ||||
| 			var radiusA = circleA.Radius * transformA.Scale.X; | ||||
| 			var radiusB = circleB.Radius * transformB.Scale.Y; | ||||
| 
 | ||||
| 			var centerA = transformA.Position; | ||||
| 			var centerB = transformB.Position; | ||||
| 
 | ||||
| 			var distanceSquared = (centerA - centerB).LengthSquared(); | ||||
| 			var radiusSumSquared = (radiusA + radiusB) * (radiusA + radiusB); | ||||
| 
 | ||||
| 			return distanceSquared < radiusSumSquared; | ||||
| 		} | ||||
| 
 | ||||
| 		public static (bool, Simplex2D) FindCollisionSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) | ||||
| 		{ | ||||
| 			var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB); | ||||
| 			var c = minkowskiDifference.Support(Vector2.UnitX); | ||||
| 			var b = minkowskiDifference.Support(-Vector2.UnitX); | ||||
| 			return Check(minkowskiDifference, c, b); | ||||
| 		} | ||||
| 
 | ||||
|         public unsafe static Vector2 Intersect(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Simplex2D simplex) | ||||
|         { | ||||
|             if (shapeA == null) { throw new System.ArgumentNullException(nameof(shapeA)); } | ||||
|             if (shapeB == null) { throw new System.ArgumentNullException(nameof(shapeB)); } | ||||
|             if (!simplex.TwoSimplex) { throw new System.ArgumentException("Simplex must be a 2-Simplex.", nameof(simplex)); } | ||||
| 
 | ||||
| 			var epsilon = Fix64.FromFraction(1, 10000); | ||||
| 
 | ||||
| 			var a = simplex.A; | ||||
|             var b = simplex.B.Value; | ||||
|             var c = simplex.C.Value; | ||||
| 
 | ||||
|             Vector2 intersection = default; | ||||
| 
 | ||||
|             for (var i = 0; i < 32; i++) | ||||
|             { | ||||
|                 var edge = FindClosestEdge(simplex); | ||||
|                 var support = CalculateSupport(shapeA, Transform2DA, shapeB, Transform2DB, edge.Normal); | ||||
|                 var distance = Vector2.Dot(support, edge.Normal); | ||||
| 
 | ||||
|                 intersection = edge.Normal; | ||||
|                 intersection *= distance; | ||||
| 
 | ||||
|                 if (Fix64.Abs(distance - edge.Distance) <= epsilon) | ||||
|                 { | ||||
|                     return intersection; | ||||
|                 } | ||||
|                 else | ||||
|                 { | ||||
| 					simplex.Insert(support, edge.Index); | ||||
|                 } | ||||
|             } | ||||
| 
 | ||||
|             return intersection; // close enough | ||||
|         } | ||||
| 
 | ||||
|         private static unsafe Edge FindClosestEdge(Simplex2D simplex) | ||||
|         { | ||||
| 			var closestDistance = Fix64.MaxValue; | ||||
|             var closestNormal = Vector2.Zero; | ||||
|             var closestIndex = 0; | ||||
| 
 | ||||
| 			for (var i = 0; i < 4; i += 1) | ||||
| 			{ | ||||
| 				var j = (i + 1 == 3) ? 0 : i + 1; | ||||
| 
 | ||||
| 				var a = simplex[i]; | ||||
| 				var b = simplex[j]; | ||||
| 
 | ||||
| 				var e = b - a; | ||||
| 
 | ||||
| 				var oa = a; | ||||
| 
 | ||||
| 				var n = Vector2.Normalize(TripleProduct(e, oa, e)); | ||||
| 
 | ||||
| 				var d = Vector2.Dot(n, a); | ||||
| 
 | ||||
| 				if (d < closestDistance) | ||||
| 				{ | ||||
| 					closestDistance = d; | ||||
| 					closestNormal = n; | ||||
| 					closestIndex = j; | ||||
| 				} | ||||
| 			} | ||||
| 
 | ||||
|             return new Edge | ||||
| 			{ | ||||
| 				Distance = closestDistance, | ||||
| 				Normal = closestNormal, | ||||
| 				Index = closestIndex | ||||
| 			}; | ||||
|         } | ||||
| 
 | ||||
|         private static Vector2 CalculateSupport(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Vector2 direction) | ||||
|         { | ||||
|             return shapeA.Support(direction, Transform2DA) - shapeB.Support(-direction, Transform2DB); | ||||
|         } | ||||
| 
 | ||||
| 		private static (bool, Simplex2D) Check(MinkowskiDifference minkowskiDifference, Vector2 c, Vector2 b) | ||||
|         { | ||||
|             var cb = c - b; | ||||
|             var c0 = -c; | ||||
|             var d = Direction(cb, c0); | ||||
|             return DoSimplex(minkowskiDifference, new Simplex2D(b, c), d); | ||||
|         } | ||||
| 
 | ||||
|         private static (bool, Simplex2D) DoSimplex(MinkowskiDifference minkowskiDifference, Simplex2D simplex, Vector2 direction) | ||||
|         { | ||||
|             var a = minkowskiDifference.Support(direction); | ||||
|             var notPastOrigin = Vector2.Dot(a, direction) < Fix64.Zero; | ||||
|             var (intersects, newSimplex, newDirection) = EnclosesOrigin(a, simplex); | ||||
| 
 | ||||
|             if (notPastOrigin) | ||||
|             { | ||||
|                 return (false, default(Simplex2D)); | ||||
|             } | ||||
|             else if (intersects) | ||||
|             { | ||||
|                 return (true, new Simplex2D(simplex.A, simplex.B.Value, a)); | ||||
|             } | ||||
|             else | ||||
|             { | ||||
|                 return DoSimplex(minkowskiDifference, newSimplex, newDirection); | ||||
|             } | ||||
|         } | ||||
| 
 | ||||
|         private static (bool, Simplex2D, Vector2) EnclosesOrigin(Vector2 a, Simplex2D simplex) | ||||
|         { | ||||
|             if (simplex.ZeroSimplex) | ||||
|             { | ||||
|                 return HandleZeroSimplex(a, simplex.A); | ||||
|             } | ||||
|             else if (simplex.OneSimplex) | ||||
|             { | ||||
|                 return HandleOneSimplex(a, simplex.A, simplex.B.Value); | ||||
|             } | ||||
|             else | ||||
|             { | ||||
|                 return (false, simplex, Vector2.Zero); | ||||
|             } | ||||
|         } | ||||
| 
 | ||||
|         private static (bool, Simplex2D, Vector2) HandleZeroSimplex(Vector2 a, Vector2 b) | ||||
|         { | ||||
|             var ab = b - a; | ||||
|             var a0 = -a; | ||||
|             var (newSimplex, newDirection) = SameDirection(ab, a0) ? (new Simplex2D(a, b), Perpendicular(ab, a0)) : (new Simplex2D(a), a0); | ||||
|             return (false, newSimplex, newDirection); | ||||
|         } | ||||
| 
 | ||||
|         private static (bool, Simplex2D, Vector2) HandleOneSimplex(Vector2 a, Vector2 b, Vector2 c) | ||||
|         { | ||||
|             var a0 = -a; | ||||
|             var ab = b - a; | ||||
|             var ac = c - a; | ||||
|             var abp = Perpendicular(ab, -ac); | ||||
|             var acp = Perpendicular(ac, -ab); | ||||
| 
 | ||||
|             if (SameDirection(abp, a0)) | ||||
|             { | ||||
|                 if (SameDirection(ab, a0)) | ||||
|                 { | ||||
|                     return (false, new Simplex2D(a, b), abp); | ||||
|                 } | ||||
|                 else | ||||
|                 { | ||||
|                     return (false, new Simplex2D(a), a0); | ||||
|                 } | ||||
|             } | ||||
|             else if (SameDirection(acp, a0)) | ||||
|             { | ||||
|                 if (SameDirection(ac, a0)) | ||||
|                 { | ||||
|                     return (false, new Simplex2D(a, c), acp); | ||||
|                 } | ||||
|                 else | ||||
|                 { | ||||
|                     return (false, new Simplex2D(a), a0); | ||||
|                 } | ||||
|             } | ||||
|             else | ||||
|             { | ||||
|                 return (true, new Simplex2D(b, c), a0); | ||||
|             } | ||||
|         } | ||||
| 
 | ||||
|         private static Vector2 TripleProduct(Vector2 a, Vector2 b, Vector2 c) | ||||
|         { | ||||
|             var A = new Vector3(a.X, a.Y, Fix64.Zero); | ||||
|             var B = new Vector3(b.X, b.Y, Fix64.Zero); | ||||
|             var C = new Vector3(c.X, c.Y, Fix64.Zero); | ||||
| 
 | ||||
|             var first = Vector3.Cross(A, B); | ||||
|             var second = Vector3.Cross(first, C); | ||||
| 
 | ||||
|             return new Vector2(second.X, second.Y); | ||||
|         } | ||||
| 
 | ||||
|         private static Vector2 Direction(Vector2 a, Vector2 b) | ||||
|         { | ||||
|             var d = TripleProduct(a, b, a); | ||||
|             var collinear = d == Vector2.Zero; | ||||
|             return collinear ? new Vector2(a.Y, -a.X) : d; | ||||
|         } | ||||
| 
 | ||||
|         private static bool SameDirection(Vector2 a, Vector2 b) | ||||
|         { | ||||
|             return Vector2.Dot(a, b) > Fix64.Zero; | ||||
|         } | ||||
| 
 | ||||
|         private static Vector2 Perpendicular(Vector2 a, Vector2 b) | ||||
|         { | ||||
|             return TripleProduct(a, b, a); | ||||
|         } | ||||
| 	} | ||||
| } | ||||
|  | @ -0,0 +1,73 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A Circle is a shape defined by a radius. | ||||
| 	/// </summary> | ||||
| 	public struct Circle : IShape2D, System.IEquatable<Circle> | ||||
| 	{ | ||||
| 		public Fix64 Radius { get; } | ||||
| 		public AABB2D AABB { get; } | ||||
| 		public IEnumerable<IShape2D> Shapes | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				yield return this; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		public Circle(Fix64 radius) | ||||
| 		{ | ||||
| 			Radius = radius; | ||||
| 			AABB = new AABB2D(-Radius, -Radius, Radius, Radius); | ||||
| 		} | ||||
| 
 | ||||
| 		public Circle(int radius) | ||||
| 		{ | ||||
| 			Radius = (Fix64) radius; | ||||
| 			AABB = new AABB2D(-Radius, -Radius, Radius, Radius); | ||||
| 		} | ||||
| 
 | ||||
| 		public Vector2 Support(Vector2 direction, Transform2D transform) | ||||
| 		{ | ||||
| 			return Vector2.Transform(Vector2.Normalize(direction) * Radius, transform.TransformMatrix); | ||||
| 		} | ||||
| 
 | ||||
| 		public AABB2D TransformedAABB(Transform2D transform2D) | ||||
| 		{ | ||||
| 			return AABB2D.Transformed(AABB, transform2D); | ||||
| 		} | ||||
| 
 | ||||
| 		public override bool Equals(object obj) | ||||
| 		{ | ||||
| 			return obj is IShape2D other && Equals(other); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(IShape2D other) | ||||
| 		{ | ||||
| 			return other is Circle circle && Equals(circle); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(Circle other) | ||||
| 		{ | ||||
| 			return Radius == other.Radius; | ||||
| 		} | ||||
| 
 | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return System.HashCode.Combine(Radius); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator ==(Circle a, Circle b) | ||||
| 		{ | ||||
| 			return a.Equals(b); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator !=(Circle a, Circle b) | ||||
| 		{ | ||||
| 			return !(a == b); | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
|  | @ -0,0 +1,83 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
|     /// A line is a shape defined by exactly two points in space. | ||||
|     /// </summary> | ||||
|     public struct Line : IShape2D, System.IEquatable<Line> | ||||
|     { | ||||
|         public Vector2 Start { get; } | ||||
|         public Vector2 End { get; } | ||||
| 
 | ||||
|         public AABB2D AABB { get; } | ||||
| 
 | ||||
| 		public IEnumerable<IShape2D> Shapes | ||||
|         { | ||||
|             get | ||||
|             { | ||||
| 				yield return this; | ||||
| 			} | ||||
|         } | ||||
| 
 | ||||
| 		public Line(Vector2 start, Vector2 end) | ||||
|         { | ||||
|             Start = start; | ||||
|             End = end; | ||||
| 
 | ||||
|             AABB = new AABB2D( | ||||
| 				Fix64.Min(Start.X, End.X), | ||||
| 				Fix64.Min(Start.Y, End.Y), | ||||
| 				Fix64.Max(Start.X, End.X), | ||||
| 				Fix64.Max(Start.Y, End.Y) | ||||
| 			); | ||||
|         } | ||||
| 
 | ||||
|         public Vector2 Support(Vector2 direction, Transform2D transform) | ||||
|         { | ||||
|             var transformedStart = Vector2.Transform(Start, transform.TransformMatrix); | ||||
|             var transformedEnd = Vector2.Transform(End, transform.TransformMatrix); | ||||
|             return Vector2.Dot(transformedStart, direction) > Vector2.Dot(transformedEnd, direction) ? | ||||
|                 transformedStart : | ||||
|                 transformedEnd; | ||||
|         } | ||||
| 
 | ||||
|         public AABB2D TransformedAABB(Transform2D transform) | ||||
|         { | ||||
|             return AABB2D.Transformed(AABB, transform); | ||||
|         } | ||||
| 
 | ||||
|         public override bool Equals(object obj) | ||||
|         { | ||||
|             return obj is IShape2D other && Equals(other); | ||||
|         } | ||||
| 
 | ||||
|         public bool Equals(IShape2D other) | ||||
|         { | ||||
|             return other is Line otherLine && Equals(otherLine); | ||||
|         } | ||||
| 
 | ||||
|         public bool Equals(Line other) | ||||
|         { | ||||
|             return | ||||
| 				(Start == other.Start && End == other.End) || | ||||
| 				(End == other.Start && Start == other.End); | ||||
|         } | ||||
| 
 | ||||
|         public override int GetHashCode() | ||||
|         { | ||||
|             return System.HashCode.Combine(Start, End); | ||||
|         } | ||||
| 
 | ||||
|         public static bool operator ==(Line a, Line b) | ||||
|         { | ||||
|             return a.Equals(b); | ||||
|         } | ||||
| 
 | ||||
|         public static bool operator !=(Line a, Line b) | ||||
|         { | ||||
|             return !(a == b); | ||||
|         } | ||||
|     } | ||||
| } | ||||
|  | @ -1,7 +1,7 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A Point is "that which has no part". | ||||
|  | @ -0,0 +1,130 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A rectangle is a shape defined by a width and height. The origin is the center of the rectangle. | ||||
| 	/// </summary> | ||||
| 	public struct Rectangle : IShape2D, System.IEquatable<Rectangle> | ||||
| 	{ | ||||
| 		public AABB2D AABB { get; } | ||||
| 		public Fix64 Width { get; } | ||||
| 		public Fix64 Height { get; } | ||||
| 
 | ||||
| 		public Fix64 Right { get; } | ||||
| 		public Fix64 Left { get; } | ||||
| 		public Fix64 Top { get; } | ||||
| 		public Fix64 Bottom { get; } | ||||
| 		public Vector2 TopLeft { get; } | ||||
| 		public Vector2 BottomRight { get; } | ||||
| 
 | ||||
| 		public Vector2 Min { get; } | ||||
| 		public Vector2 Max { get; } | ||||
| 
 | ||||
| 		public IEnumerable<IShape2D> Shapes | ||||
|         { | ||||
|             get | ||||
|             { | ||||
| 				yield return this; | ||||
| 			} | ||||
|         } | ||||
| 
 | ||||
| 		public Rectangle(Fix64 left, Fix64 top, Fix64 width, Fix64 height) | ||||
| 		{ | ||||
| 			Width = width; | ||||
| 			Height = height; | ||||
| 			Left = left; | ||||
| 			Right = left + width; | ||||
| 			Top = top; | ||||
| 			Bottom = top + height; | ||||
| 			AABB = new AABB2D(left, top, Right, Bottom); | ||||
| 			TopLeft = new Vector2(Left, Top); | ||||
| 			BottomRight = new Vector2(Right, Bottom); | ||||
| 			Min = AABB.Min; | ||||
| 			Max = AABB.Max; | ||||
| 		} | ||||
| 
 | ||||
| 		public Rectangle(int left, int top, int width, int height) | ||||
| 		{ | ||||
| 			Width = (Fix64) width; | ||||
| 			Height = (Fix64) height; | ||||
| 			Left = (Fix64) left; | ||||
| 			Right = (Fix64) (left + width); | ||||
| 			Top = (Fix64) top; | ||||
| 			Bottom = (Fix64) (top + height); | ||||
| 			AABB = new AABB2D(Left, Top, Right, Bottom); | ||||
| 			TopLeft = new Vector2(Left, Top); | ||||
| 			BottomRight = new Vector2(Right, Bottom); | ||||
| 			Min = AABB.Min; | ||||
| 			Max = AABB.Max; | ||||
| 		} | ||||
| 
 | ||||
| 		private Vector2 Support(Vector2 direction) | ||||
| 		{ | ||||
| 			if (direction.X >= Fix64.Zero && direction.Y >= Fix64.Zero) | ||||
| 			{ | ||||
| 				return Max; | ||||
| 			} | ||||
| 			else if (direction.X >= Fix64.Zero && direction.Y < Fix64.Zero) | ||||
| 			{ | ||||
| 				return new Vector2(Max.X, Min.Y); | ||||
| 			} | ||||
| 			else if (direction.X < Fix64.Zero && direction.Y >= Fix64.Zero) | ||||
| 			{ | ||||
| 				return new Vector2(Min.X, Max.Y); | ||||
| 			} | ||||
| 			else if (direction.X < Fix64.Zero && direction.Y < Fix64.Zero) | ||||
| 			{ | ||||
| 				return new Vector2(Min.X, Min.Y); | ||||
| 			} | ||||
| 			else | ||||
| 			{ | ||||
| 				throw new System.ArgumentException("Support vector direction cannot be zero."); | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		public Vector2 Support(Vector2 direction, Transform2D transform) | ||||
| 		{ | ||||
| 			Matrix3x2 inverseTransform; | ||||
| 			Matrix3x2.Invert(transform.TransformMatrix, out inverseTransform); | ||||
| 			var inverseDirection = Vector2.TransformNormal(direction, inverseTransform); | ||||
| 			return Vector2.Transform(Support(inverseDirection), transform.TransformMatrix); | ||||
| 		} | ||||
| 
 | ||||
| 		public AABB2D TransformedAABB(Transform2D transform) | ||||
| 		{ | ||||
| 			return AABB2D.Transformed(AABB, transform); | ||||
| 		} | ||||
| 
 | ||||
| 		public override bool Equals(object obj) | ||||
| 		{ | ||||
| 			return obj is IShape2D other && Equals(other); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(IShape2D other) | ||||
| 		{ | ||||
| 			return (other is Rectangle rectangle && Equals(rectangle)); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(Rectangle other) | ||||
| 		{ | ||||
| 			return Min == other.Min && Max == other.Max; | ||||
| 		} | ||||
| 
 | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return System.HashCode.Combine(Min, Max); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator ==(Rectangle a, Rectangle b) | ||||
| 		{ | ||||
| 			return a.Equals(b); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator !=(Rectangle a, Rectangle b) | ||||
| 		{ | ||||
| 			return !(a == b); | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
|  | @ -0,0 +1,136 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A simplex is a shape with up to n - 2 vertices in the nth dimension. | ||||
| 	/// </summary> | ||||
| 	public struct Simplex2D : System.IEquatable<Simplex2D> | ||||
| 	{ | ||||
| 		private Vector2 a; | ||||
| 		private Vector2? b; | ||||
| 		private Vector2? c; | ||||
| 
 | ||||
| 		public Vector2 A => a; | ||||
| 		public Vector2? B => b; | ||||
| 		public Vector2? C => c; | ||||
| 
 | ||||
| 		public bool ZeroSimplex { get { return !b.HasValue && !c.HasValue; } } | ||||
| 		public bool OneSimplex { get { return b.HasValue && !c.HasValue; } } | ||||
| 		public bool TwoSimplex { get { return b.HasValue && c.HasValue; } } | ||||
| 
 | ||||
| 		public int Count => TwoSimplex ? 3 : (OneSimplex ? 2 : 1); | ||||
| 
 | ||||
| 		public Simplex2D(Vector2 a) | ||||
| 		{ | ||||
| 			this.a = a; | ||||
| 			b = null; | ||||
| 			c = null; | ||||
| 		} | ||||
| 
 | ||||
| 		public Simplex2D(Vector2 a, Vector2 b) | ||||
| 		{ | ||||
| 			this.a = a; | ||||
| 			this.b = b; | ||||
| 			c = null; | ||||
| 		} | ||||
| 
 | ||||
| 		public Simplex2D(Vector2 a, Vector2 b, Vector2 c) | ||||
| 		{ | ||||
| 			this.a = a; | ||||
| 			this.b = b; | ||||
| 			this.c = c; | ||||
| 		} | ||||
| 
 | ||||
| 		public Vector2 this[int index] | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				if (index == 0) { return a; } | ||||
| 				if (index == 1) { return b.Value; } | ||||
| 				if (index == 2) { return c.Value; } | ||||
| 				throw new System.IndexOutOfRangeException(); | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		public IEnumerable<Vector2> Vertices | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				yield return (Vector2) a; | ||||
| 				if (b.HasValue) { yield return (Vector2) b; } | ||||
| 				if (c.HasValue) { yield return (Vector2) c; } | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		public Vector2 Support(Vector2 direction, Transform2D transform) | ||||
| 		{ | ||||
| 			var maxDotProduct = Fix64.MinValue; | ||||
| 			var maxVertex = a; | ||||
| 			foreach (var vertex in Vertices) | ||||
| 			{ | ||||
| 				var transformed = Vector2.Transform(vertex, transform.TransformMatrix); | ||||
| 				var dot = Vector2.Dot(transformed, direction); | ||||
| 				if (dot > maxDotProduct) | ||||
| 				{ | ||||
| 					maxVertex = transformed; | ||||
| 					maxDotProduct = dot; | ||||
| 				} | ||||
| 			} | ||||
| 			return maxVertex; | ||||
| 		} | ||||
| 
 | ||||
| 		public void Insert(Vector2 point, int index) | ||||
| 		{ | ||||
| 			if (index == 0) | ||||
| 			{ | ||||
| 				c = b; | ||||
| 				b = a; | ||||
| 				a = point; | ||||
| 			} | ||||
| 			else if (index == 1) | ||||
| 			{ | ||||
| 				c = b; | ||||
| 				b = point; | ||||
| 			} | ||||
| 			else | ||||
| 			{ | ||||
| 				c = point; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		public override bool Equals(object obj) | ||||
| 		{ | ||||
| 			return obj is Simplex2D other && Equals(other); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(Simplex2D other) | ||||
| 		{ | ||||
| 			if (Count != other.Count) { return false; } | ||||
| 
 | ||||
| 			return | ||||
| 				(A == other.A && B == other.B && C == other.C) || | ||||
| 				(A == other.A && B == other.C && C == other.B) || | ||||
| 				(A == other.B && B == other.A && C == other.C) || | ||||
| 				(A == other.B && B == other.C && C == other.A) || | ||||
| 				(A == other.C && B == other.A && C == other.B) || | ||||
| 				(A == other.C && B == other.B && C == other.A); | ||||
| 		} | ||||
| 
 | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return System.HashCode.Combine(Vertices); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator ==(Simplex2D a, Simplex2D b) | ||||
| 		{ | ||||
| 			return a.Equals(b); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator !=(Simplex2D a, Simplex2D b) | ||||
| 		{ | ||||
| 			return !(a == b); | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
|  | @ -0,0 +1,252 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math.Fixed; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Used to quickly check if two shapes are potentially overlapping. | ||||
| 	/// </summary> | ||||
| 	/// <typeparam name="T">The type that will be used to uniquely identify shape-transform pairs.</typeparam> | ||||
| 	public class SpatialHash2D<T> where T : System.IEquatable<T> | ||||
| 	{ | ||||
| 		private readonly Fix64 cellSize; | ||||
| 
 | ||||
| 		private readonly Dictionary<long, HashSet<T>> hashDictionary = new Dictionary<long, HashSet<T>>(); | ||||
| 		private readonly Dictionary<T, (ICollidable, Transform2D, uint)> IDLookup = new Dictionary<T, (ICollidable, Transform2D, uint)>(); | ||||
| 
 | ||||
| 		public int MinX { get; private set; } = 0; | ||||
| 		public int MaxX { get; private set; } = 0; | ||||
| 		public int MinY { get; private set; } = 0; | ||||
| 		public int MaxY { get; private set; } = 0; | ||||
| 
 | ||||
| 		private Queue<HashSet<T>> hashSetPool = new Queue<HashSet<T>>(); | ||||
| 
 | ||||
| 		public SpatialHash2D(int cellSize) | ||||
| 		{ | ||||
| 			this.cellSize = new Fix64(cellSize); | ||||
| 		} | ||||
| 
 | ||||
| 		private (int, int) Hash(Vector2 position) | ||||
| 		{ | ||||
| 			return ((int) (position.X / cellSize), (int) (position.Y / cellSize)); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Inserts an element into the SpatialHash. | ||||
| 		/// </summary> | ||||
| 		/// <param name="id">A unique ID for the shape-transform pair.</param> | ||||
| 		/// <param name="shape"></param> | ||||
| 		/// <param name="transform2D"></param> | ||||
| 		/// <param name="collisionGroups">A bitmask value specifying the groups this object belongs to.</param> | ||||
| 		public void Insert(T id, ICollidable shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) | ||||
| 		{ | ||||
| 			var box = shape.TransformedAABB(transform2D); | ||||
| 			var minHash = Hash(box.Min); | ||||
| 			var maxHash = Hash(box.Max); | ||||
| 
 | ||||
| 			foreach (var key in Keys(minHash.Item1, minHash.Item2, maxHash.Item1, maxHash.Item2)) | ||||
| 			{ | ||||
| 				if (!hashDictionary.ContainsKey(key)) | ||||
| 				{ | ||||
| 					hashDictionary.Add(key, new HashSet<T>()); | ||||
| 				} | ||||
| 
 | ||||
| 				hashDictionary[key].Add(id); | ||||
| 				IDLookup[id] = (shape, transform2D, collisionGroups); | ||||
| 			} | ||||
| 
 | ||||
| 			MinX = System.Math.Min(MinX, minHash.Item1); | ||||
| 			MinY = System.Math.Min(MinY, minHash.Item2); | ||||
| 			MaxX = System.Math.Max(MaxX, maxHash.Item1); | ||||
| 			MaxY = System.Math.Max(MaxY, maxHash.Item2); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Retrieves all the potential collisions of a shape-transform pair. Excludes any shape-transforms with the given ID. | ||||
| 		/// </summary> | ||||
| 		public IEnumerable<(T, ICollidable, Transform2D, uint)> Retrieve(T id, ICollidable shape, Transform2D transform2D, uint collisionMask = uint.MaxValue) | ||||
| 		{ | ||||
| 			var returned = AcquireHashSet(); | ||||
| 
 | ||||
| 			var box = shape.TransformedAABB(transform2D); | ||||
| 			var (minX, minY) = Hash(box.Min); | ||||
| 			var (maxX, maxY) = Hash(box.Max); | ||||
| 
 | ||||
| 			if (minX < MinX) { minX = MinX; } | ||||
| 			if (maxX > MaxX) { maxX = MaxX; } | ||||
| 			if (minY < MinY) { minY = MinY; } | ||||
| 			if (maxY > MaxY) { maxY = MaxY; } | ||||
| 
 | ||||
| 			foreach (var key in Keys(minX, minY, maxX, maxY)) | ||||
| 			{ | ||||
| 				if (hashDictionary.ContainsKey(key)) | ||||
| 				{ | ||||
| 					foreach (var t in hashDictionary[key]) | ||||
| 					{ | ||||
| 						if (!returned.Contains(t)) | ||||
| 						{ | ||||
| 							var (otherShape, otherTransform, collisionGroups) = IDLookup[t]; | ||||
| 							if (!id.Equals(t) && ((collisionGroups & collisionMask) > 0) && AABB2D.TestOverlap(box, otherShape.TransformedAABB(otherTransform))) | ||||
| 							{ | ||||
| 								returned.Add(t); | ||||
| 								yield return (t, otherShape, otherTransform, collisionGroups); | ||||
| 							} | ||||
| 						} | ||||
| 					} | ||||
| 				} | ||||
| 			} | ||||
| 
 | ||||
| 			FreeHashSet(returned); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Retrieves all the potential collisions of a shape-transform pair. | ||||
| 		/// </summary> | ||||
| 		public IEnumerable<(T, ICollidable, Transform2D, uint)> Retrieve(ICollidable shape, Transform2D transform2D, uint collisionMask = uint.MaxValue) | ||||
| 		{ | ||||
| 			var returned = AcquireHashSet(); | ||||
| 
 | ||||
| 			var box = shape.TransformedAABB(transform2D); | ||||
| 			var (minX, minY) = Hash(box.Min); | ||||
| 			var (maxX, maxY) = Hash(box.Max); | ||||
| 
 | ||||
| 			if (minX < MinX) { minX = MinX; } | ||||
| 			if (maxX > MaxX) { maxX = MaxX; } | ||||
| 			if (minY < MinY) { minY = MinY; } | ||||
| 			if (maxY > MaxY) { maxY = MaxY; } | ||||
| 
 | ||||
| 			foreach (var key in Keys(minX, minY, maxX, maxY)) | ||||
| 			{ | ||||
| 				if (hashDictionary.ContainsKey(key)) | ||||
| 				{ | ||||
| 					foreach (var t in hashDictionary[key]) | ||||
| 					{ | ||||
| 						if (!returned.Contains(t)) | ||||
| 						{ | ||||
| 							var (otherShape, otherTransform, collisionGroups) = IDLookup[t]; | ||||
| 							if (((collisionGroups & collisionMask) > 0) && AABB2D.TestOverlap(box, otherShape.TransformedAABB(otherTransform))) | ||||
| 							{ | ||||
| 								returned.Add(t); | ||||
| 								yield return (t, otherShape, otherTransform, collisionGroups); | ||||
| 							} | ||||
| 						} | ||||
| 					} | ||||
| 				} | ||||
| 			} | ||||
| 
 | ||||
| 			FreeHashSet(returned); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Retrieves objects based on a pre-transformed AABB. | ||||
| 		/// </summary> | ||||
| 		/// <param name="aabb">A transformed AABB.</param> | ||||
| 		/// <returns></returns> | ||||
| 		public IEnumerable<(T, ICollidable, Transform2D, uint)> Retrieve(AABB2D aabb, uint collisionMask = uint.MaxValue) | ||||
| 		{ | ||||
| 			var returned = AcquireHashSet(); | ||||
| 
 | ||||
| 			var (minX, minY) = Hash(aabb.Min); | ||||
| 			var (maxX, maxY) = Hash(aabb.Max); | ||||
| 
 | ||||
| 			if (minX < MinX) { minX = MinX; } | ||||
| 			if (maxX > MaxX) { maxX = MaxX; } | ||||
| 			if (minY < MinY) { minY = MinY; } | ||||
| 			if (maxY > MaxY) { maxY = MaxY; } | ||||
| 
 | ||||
| 			foreach (var key in Keys(minX, minY, maxX, maxY)) | ||||
| 			{ | ||||
| 				if (hashDictionary.ContainsKey(key)) | ||||
| 				{ | ||||
| 					foreach (var t in hashDictionary[key]) | ||||
| 					{ | ||||
| 						if (!returned.Contains(t)) | ||||
| 						{ | ||||
| 							var (otherShape, otherTransform, collisionGroups) = IDLookup[t]; | ||||
| 							if (((collisionGroups & collisionMask) > 0) && AABB2D.TestOverlap(aabb, otherShape.TransformedAABB(otherTransform))) | ||||
| 							{ | ||||
| 								yield return (t, otherShape, otherTransform, collisionGroups); | ||||
| 							} | ||||
| 						} | ||||
| 					} | ||||
| 				} | ||||
| 			} | ||||
| 
 | ||||
| 			FreeHashSet(returned); | ||||
| 		} | ||||
| 
 | ||||
| 		public void Update(T id, ICollidable shape, Transform2D transform2D, uint collisionGroups = uint.MaxValue) | ||||
| 		{ | ||||
| 			Remove(id); | ||||
| 			Insert(id, shape, transform2D, collisionGroups); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Removes a specific ID from the SpatialHash. | ||||
| 		/// </summary> | ||||
| 		public void Remove(T id) | ||||
| 		{ | ||||
| 			var (shape, transform, collisionGroups) = IDLookup[id]; | ||||
| 
 | ||||
| 			var box = shape.TransformedAABB(transform); | ||||
| 			var minHash = Hash(box.Min); | ||||
| 			var maxHash = Hash(box.Max); | ||||
| 
 | ||||
| 			foreach (var key in Keys(minHash.Item1, minHash.Item2, maxHash.Item1, maxHash.Item2)) | ||||
| 			{ | ||||
| 				if (hashDictionary.ContainsKey(key)) | ||||
| 				{ | ||||
| 					hashDictionary[key].Remove(id); | ||||
| 				} | ||||
| 			} | ||||
| 
 | ||||
| 			IDLookup.Remove(id); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Removes everything that has been inserted into the SpatialHash. | ||||
| 		/// </summary> | ||||
| 		public void Clear() | ||||
| 		{ | ||||
| 			foreach (var hash in hashDictionary.Values) | ||||
| 			{ | ||||
| 				hash.Clear(); | ||||
| 			} | ||||
| 
 | ||||
| 			IDLookup.Clear(); | ||||
| 		} | ||||
| 
 | ||||
| 		private static long MakeLong(int left, int right) | ||||
| 		{ | ||||
| 			return ((long) left << 32) | ((uint) right); | ||||
| 		} | ||||
| 
 | ||||
| 		private IEnumerable<long> Keys(int minX, int minY, int maxX, int maxY) | ||||
| 		{ | ||||
| 			for (var i = minX; i <= maxX; i++) | ||||
| 			{ | ||||
| 				for (var j = minY; j <= maxY; j++) | ||||
| 				{ | ||||
| 					yield return MakeLong(i, j); | ||||
| 				} | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		private HashSet<T> AcquireHashSet() | ||||
| 		{ | ||||
| 			if (hashSetPool.Count == 0) | ||||
| 			{ | ||||
| 				hashSetPool.Enqueue(new HashSet<T>()); | ||||
| 			} | ||||
| 
 | ||||
| 			var hashSet = hashSetPool.Dequeue(); | ||||
| 			hashSet.Clear(); | ||||
| 			return hashSet; | ||||
| 		} | ||||
| 
 | ||||
| 		private void FreeHashSet(HashSet<T> hashSet) | ||||
| 		{ | ||||
| 			hashSetPool.Enqueue(hashSet); | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
|  | @ -1,7 +1,7 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Axis-aligned bounding box. | ||||
|  | @ -0,0 +1,12 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	public interface ICollidable | ||||
| 	{ | ||||
| 		IEnumerable<IShape2D> Shapes { get; } | ||||
| 		AABB2D AABB { get; } | ||||
| 		AABB2D TransformedAABB(Transform2D transform); | ||||
| 	} | ||||
| } | ||||
|  | @ -0,0 +1,15 @@ | |||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	public interface IShape2D : ICollidable, System.IEquatable<IShape2D> | ||||
| 	{ | ||||
| 		/// <summary> | ||||
| 		/// A Minkowski support function. Gives the farthest point on the edge of a shape along the given direction. | ||||
| 		/// </summary> | ||||
| 		/// <param name="direction">A normalized Vector2.</param> | ||||
| 		/// <param name="transform">A Transform for transforming the shape vertices.</param> | ||||
| 		/// <returns>The farthest point on the edge of the shape along the given direction.</returns> | ||||
| 		Vector2 Support(Vector2 direction, Transform2D transform); | ||||
| 	} | ||||
| } | ||||
|  | @ -0,0 +1,57 @@ | |||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A Minkowski difference between two shapes. | ||||
| 	/// </summary> | ||||
| 	public struct MinkowskiDifference : System.IEquatable<MinkowskiDifference> | ||||
| 	{ | ||||
| 		private IShape2D ShapeA { get; } | ||||
| 		private Transform2D TransformA { get; } | ||||
| 		private IShape2D ShapeB { get; } | ||||
| 		private Transform2D TransformB { get; } | ||||
| 
 | ||||
| 		public MinkowskiDifference(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) | ||||
| 		{ | ||||
| 			ShapeA = shapeA; | ||||
| 			TransformA = transformA; | ||||
| 			ShapeB = shapeB; | ||||
| 			TransformB = transformB; | ||||
| 		} | ||||
| 
 | ||||
| 		public Vector2 Support(Vector2 direction) | ||||
| 		{ | ||||
| 			return ShapeA.Support(direction, TransformA) - ShapeB.Support(-direction, TransformB); | ||||
| 		} | ||||
| 
 | ||||
| 		public override bool Equals(object other) | ||||
| 		{ | ||||
| 			return other is MinkowskiDifference minkowskiDifference && Equals(minkowskiDifference); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(MinkowskiDifference other) | ||||
| 		{ | ||||
| 			return | ||||
| 				ShapeA == other.ShapeA && | ||||
| 				TransformA == other.TransformA && | ||||
| 				ShapeB == other.ShapeB && | ||||
| 				TransformB == other.TransformB; | ||||
| 		} | ||||
| 
 | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return System.HashCode.Combine(ShapeA, TransformA, ShapeB, TransformB); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator ==(MinkowskiDifference a, MinkowskiDifference b) | ||||
| 		{ | ||||
| 			return a.Equals(b); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator !=(MinkowskiDifference a, MinkowskiDifference b) | ||||
| 		{ | ||||
| 			return !(a == b); | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
|  | @ -1,6 +1,6 @@ | |||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	public static class NarrowPhase | ||||
| 	{ | ||||
|  | @ -1,14 +1,14 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A Circle is a shape defined by a radius. | ||||
| 	/// </summary> | ||||
| 	public struct Circle : IShape2D, System.IEquatable<Circle> | ||||
| 	{ | ||||
| 		public int Radius { get; } | ||||
| 		public float Radius { get; } | ||||
| 		public AABB2D AABB { get; } | ||||
| 		public IEnumerable<IShape2D> Shapes | ||||
| 		{ | ||||
|  | @ -18,7 +18,7 @@ namespace MoonWorks.Collision | |||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		public Circle(int radius) | ||||
| 		public Circle(float radius) | ||||
| 		{ | ||||
| 			Radius = radius; | ||||
| 			AABB = new AABB2D(-Radius, -Radius, Radius, Radius); | ||||
|  | @ -1,7 +1,7 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	/// <summary> | ||||
|     /// A line is a shape defined by exactly two points in space. | ||||
|  | @ -0,0 +1,61 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A Point is "that which has no part". | ||||
| 	/// All points by themselves are identical. | ||||
| 	/// </summary> | ||||
| 	public struct Point : IShape2D, System.IEquatable<Point> | ||||
| 	{ | ||||
| 		public AABB2D AABB { get; } | ||||
| 		public IEnumerable<IShape2D> Shapes | ||||
|         { | ||||
|             get | ||||
|             { | ||||
| 				yield return this; | ||||
| 			} | ||||
|         } | ||||
| 
 | ||||
| 		public AABB2D TransformedAABB(Transform2D transform) | ||||
| 		{ | ||||
| 			return AABB2D.Transformed(AABB, transform); | ||||
| 		} | ||||
| 
 | ||||
| 		public Vector2 Support(Vector2 direction, Transform2D transform) | ||||
| 		{ | ||||
| 			return Vector2.Transform(Vector2.Zero, transform.TransformMatrix); | ||||
| 		} | ||||
| 
 | ||||
| 		public override bool Equals(object obj) | ||||
| 		{ | ||||
| 			return obj is IShape2D other && Equals(other); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(IShape2D other) | ||||
| 		{ | ||||
| 			return other is Point otherPoint && Equals(otherPoint); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(Point other) | ||||
| 		{ | ||||
| 			return true; | ||||
| 		} | ||||
| 
 | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return 0; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator ==(Point a, Point b) | ||||
| 		{ | ||||
| 			return true; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator !=(Point a, Point b) | ||||
| 		{ | ||||
| 			return false; | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
|  | @ -1,7 +1,7 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A rectangle is a shape defined by a width and height. The origin is the center of the rectangle. | ||||
|  | @ -9,13 +9,13 @@ namespace MoonWorks.Collision | |||
| 	public struct Rectangle : IShape2D, System.IEquatable<Rectangle> | ||||
| 	{ | ||||
| 		public AABB2D AABB { get; } | ||||
| 		public int Width { get; } | ||||
| 		public int Height { get; } | ||||
| 		public float Width { get; } | ||||
| 		public float Height { get; } | ||||
| 
 | ||||
| 		public int Right { get; } | ||||
| 		public int Left { get; } | ||||
| 		public int Top { get; } | ||||
| 		public int Bottom { get; } | ||||
| 		public float Right { get; } | ||||
| 		public float Left { get; } | ||||
| 		public float Top { get; } | ||||
| 		public float Bottom { get; } | ||||
| 		public Vector2 TopLeft { get; } | ||||
| 		public Vector2 BottomRight { get; } | ||||
| 
 | ||||
|  | @ -30,7 +30,7 @@ namespace MoonWorks.Collision | |||
| 			} | ||||
|         } | ||||
| 
 | ||||
| 		public Rectangle(int left, int top, int width, int height) | ||||
| 		public Rectangle(float left, float top, float width, float height) | ||||
| 		{ | ||||
| 			Width = width; | ||||
| 			Height = height; | ||||
|  | @ -1,7 +1,7 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A simplex is a shape with up to n - 2 vertices in the nth dimension. | ||||
|  | @ -1,7 +1,7 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| namespace MoonWorks.Collision.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Used to quickly check if two shapes are potentially overlapping. | ||||
|  | @ -1,12 +0,0 @@ | |||
| using System.Collections.Generic; | ||||
| using MoonWorks.Math; | ||||
| 
 | ||||
| namespace MoonWorks.Collision | ||||
| { | ||||
|     public interface ICollidable | ||||
|     { | ||||
| 		IEnumerable<IShape2D> Shapes { get; } | ||||
|         AABB2D AABB { get; } | ||||
|         AABB2D TransformedAABB(Transform2D transform); | ||||
| 	} | ||||
| } | ||||
|  | @ -19,6 +19,7 @@ using System; | |||
| using System.Diagnostics; | ||||
| using System.Text; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -1,5 +1,5 @@ | |||
| using System.Runtime.InteropServices; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Graphics.Font | ||||
| { | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -16,7 +16,7 @@ | |||
| 
 | ||||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -16,7 +16,7 @@ | |||
| 
 | ||||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -16,7 +16,7 @@ | |||
| 
 | ||||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -14,7 +14,7 @@ | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
| { | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -17,6 +17,7 @@ | |||
| #region Using Statements | ||||
| using System; | ||||
| using MoonWorks.Math; | ||||
| using MoonWorks.Math.Float; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Graphics | ||||
|  |  | |||
|  | @ -22,15 +22,15 @@ | |||
| 			{ | ||||
| 				return Graphics.VertexElementFormat.Float; | ||||
| 			} | ||||
| 			else if (type == typeof(Math.Vector2)) | ||||
| 			else if (type == typeof(Math.Float.Vector2)) | ||||
| 			{ | ||||
| 				return Graphics.VertexElementFormat.Vector2; | ||||
| 			} | ||||
| 			else if (type == typeof(Math.Vector3)) | ||||
| 			else if (type == typeof(Math.Float.Vector3)) | ||||
| 			{ | ||||
| 				return Graphics.VertexElementFormat.Vector3; | ||||
| 			} | ||||
| 			else if (type == typeof(Math.Vector4)) | ||||
| 			else if (type == typeof(Math.Float.Vector4)) | ||||
| 			{ | ||||
| 				return Graphics.VertexElementFormat.Vector4; | ||||
| 			} | ||||
|  |  | |||
|  | @ -0,0 +1,804 @@ | |||
| // This source is heavily borrowed from https://github.com/asik/FixedMath.Net | ||||
| 
 | ||||
| using System; | ||||
| using System.Runtime.CompilerServices; | ||||
| 
 | ||||
| namespace MoonWorks.Math.Fixed | ||||
| { | ||||
| 	public struct Fix64 : IEquatable<Fix64>, IComparable<Fix64> | ||||
| 	{ | ||||
| 		private readonly long RawValue; | ||||
| 
 | ||||
| 		const long MAX_VALUE = long.MaxValue; | ||||
| 		const long MIN_VALUE = long.MinValue; | ||||
| 		const int FRACTIONAL_PLACES = 32; | ||||
| 		const int NUM_BITS = 64; | ||||
| 		const long ONE = 1L << FRACTIONAL_PLACES; | ||||
| 		const long PI_TIMES_2 = 0x6487ED511; | ||||
| 		const long PI = 0x3243F6A88; | ||||
| 		const long PI_OVER_2 = 0x1921FB544; | ||||
| 
 | ||||
| 		public static readonly Fix64 MaxValue = new Fix64(MAX_VALUE); | ||||
| 		public static readonly Fix64 MinValue = new Fix64(MIN_VALUE); | ||||
| 		public static readonly Fix64 One = new Fix64(ONE); | ||||
| 		public static readonly Fix64 Zero = new Fix64(0); | ||||
| 
 | ||||
| 		public static readonly Fix64 Pi = new Fix64(PI); | ||||
| 		public static readonly Fix64 PiOver2 = new Fix64(PI_OVER_2); | ||||
| 		public static readonly Fix64 PiTimes2 = new Fix64(PI_TIMES_2); | ||||
| 
 | ||||
| 		const int LUT_SIZE = (int)(PI_OVER_2 >> 15); | ||||
| 		static readonly Fix64 LutInterval = (Fix64)(LUT_SIZE - 1) / PiOver2; | ||||
| 
 | ||||
| 		public bool IsFractional => (RawValue & 0x00000000FFFFFFFF) != 0; | ||||
| 		public bool IsIntegral => (RawValue & 0x00000000FFFFFFFF) == 0; | ||||
| 
 | ||||
| 		private Fix64(long value) | ||||
| 		{ | ||||
| 			RawValue = value; | ||||
| 		} | ||||
| 
 | ||||
| 		public Fix64(int value) | ||||
| 		{ | ||||
| 			RawValue = value * ONE; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Create a fractional Fix64 number of the value (numerator / denominator). | ||||
| 		/// </summary> | ||||
| 		public static Fix64 FromFraction(int numerator, int denominator) | ||||
| 		{ | ||||
| 			return new Fix64(numerator) / new Fix64(denominator); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns an int indicating the sign of a Fix64 number. | ||||
| 		/// </summary> | ||||
| 		/// <returns>1 if the value is positive, 0 if it is 0, and -1 if it is negative.</returns> | ||||
| 		public static int Sign(Fix64 value) | ||||
| 		{ | ||||
| 			return | ||||
| 				value.RawValue < 0 ? -1 : | ||||
| 				value.RawValue > 0 ? 1 : | ||||
| 				0; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the absolute value of a Fix64 number. | ||||
| 		/// </summary> | ||||
| 		public static Fix64 Abs(Fix64 value) | ||||
| 		{ | ||||
| 			if (value.RawValue == MIN_VALUE) | ||||
| 			{ | ||||
| 				return MaxValue; | ||||
| 			} | ||||
| 
 | ||||
| 			return FastAbs(value); | ||||
| 		} | ||||
| 
 | ||||
| 		[MethodImpl(MethodImplOptions.AggressiveInlining)] | ||||
| 		private static Fix64 FastAbs(Fix64 value) | ||||
| 		{ | ||||
| 			// branchless implementation, see http://www.strchr.com/optimized_abs_function | ||||
| 			var mask = value.RawValue >> 63; | ||||
| 			return new Fix64((value.RawValue + mask) ^ mask); | ||||
| 		} | ||||
| 
 | ||||
|         /// <summary> | ||||
|         /// Returns the largest integral value less than or equal to the specified number. | ||||
|         /// </summary> | ||||
| 		public static Fix64 Floor(Fix64 value) | ||||
| 		{ | ||||
| 			// Zero out the fractional part. | ||||
| 			return new Fix64((long)((ulong)value.RawValue & 0xFFFFFFFF00000000)); | ||||
| 		} | ||||
| 
 | ||||
|         /// <summary> | ||||
|         /// Returns the smallest integral value that is greater than or equal to the specified number. | ||||
|         /// </summary> | ||||
| 		public static Fix64 Ceiling(Fix64 value) | ||||
| 		{ | ||||
| 			return value.IsFractional ? Floor(value) + One : value; | ||||
| 		} | ||||
| 
 | ||||
|         /// <summary> | ||||
|         /// Rounds to the nearest integral value. | ||||
|         /// If the value is halfway between an even and an uneven value, returns the even value. | ||||
|         /// </summary> | ||||
| 		public static Fix64 Round(Fix64 value) | ||||
| 		{ | ||||
| 			var fractionalPart = value.RawValue & 0x00000000FFFFFFFF; | ||||
| 			var integralPart = Floor(value); | ||||
| 			if (fractionalPart < 0x80000000) | ||||
| 			{ | ||||
| 				return integralPart; | ||||
| 			} | ||||
| 			if (fractionalPart > 0x80000000) | ||||
| 			{ | ||||
| 				return integralPart + One; | ||||
| 			} | ||||
| 			// if number is halfway between two values, round to the nearest even number | ||||
| 			// this is the method used by System.Math.Round(). | ||||
| 			return (integralPart.RawValue & ONE) == 0 | ||||
| 					   ? integralPart | ||||
| 					   : integralPart + One; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a remainder value as defined by the IEEE remainder method. | ||||
| 		/// </summary> | ||||
| 		/// <returns></returns> | ||||
| 		public static Fix64 IEEERemainder(Fix64 dividend, Fix64 divisor) | ||||
| 		{ | ||||
| 			//Formula taken from https://docs.microsoft.com/en-us/dotnet/api/system.math.ieeeremainder?view=net-6.0 | ||||
| 			return dividend - (divisor * Round(dividend / divisor)); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the minimum of two given Fix64 values. | ||||
| 		/// </summary> | ||||
| 		public static Fix64 Min(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return (x < y) ? x : y; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the maximum of two given Fix64 values. | ||||
| 		/// </summary> | ||||
| 		public static Fix64 Max(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return (x > y) ? x : y; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a value that is neither greater than nor less than a given min and max value. | ||||
| 		/// </summary> | ||||
| 		public static Fix64 Clamp(Fix64 value, Fix64 min, Fix64 max) | ||||
| 		{ | ||||
| 			return Fix64.Min(Fix64.Max(value, min), max); | ||||
| 		} | ||||
| 
 | ||||
| 		// Trigonometry functions | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the square root of the given Fix64 value. | ||||
| 		/// </summary> | ||||
| 		/// <exception cref="ArgumentOutOfRangeException">Throws if x is less than zero.</exception> | ||||
| 		public static Fix64 Sqrt(Fix64 x) | ||||
| 		{ | ||||
| 			var xl = x.RawValue; | ||||
| 			if (xl < 0) | ||||
| 			{ | ||||
| 				// We cannot represent infinities like Single and Double, and Sqrt is | ||||
| 				// mathematically undefined for x < 0. So we just throw an exception. | ||||
| 				throw new ArgumentOutOfRangeException("Negative value passed to Sqrt", "x"); | ||||
| 			} | ||||
| 
 | ||||
| 			var num = (ulong)xl; | ||||
| 			var result = 0UL; | ||||
| 
 | ||||
| 			// second-to-top bit | ||||
| 			var bit = 1UL << (NUM_BITS - 2); | ||||
| 
 | ||||
| 			while (bit > num) | ||||
| 			{ | ||||
| 				bit >>= 2; | ||||
| 			} | ||||
| 
 | ||||
| 			// The main part is executed twice, in order to avoid | ||||
| 			// using 128 bit values in computations. | ||||
| 			for (var i = 0; i < 2; ++i) | ||||
| 			{ | ||||
| 				// First we get the top 48 bits of the answer. | ||||
| 				while (bit != 0) | ||||
| 				{ | ||||
| 					if (num >= result + bit) | ||||
| 					{ | ||||
| 						num -= result + bit; | ||||
| 						result = (result >> 1) + bit; | ||||
| 					} | ||||
| 					else | ||||
| 					{ | ||||
| 						result = result >> 1; | ||||
| 					} | ||||
| 					bit >>= 2; | ||||
| 				} | ||||
| 
 | ||||
| 				if (i == 0) | ||||
| 				{ | ||||
| 					// Then process it again to get the lowest 16 bits. | ||||
| 					if (num > (1UL << (NUM_BITS / 2)) - 1) | ||||
| 					{ | ||||
| 						// The remainder 'num' is too large to be shifted left | ||||
| 						// by 32, so we have to add 1 to result manually and | ||||
| 						// adjust 'num' accordingly. | ||||
| 						// num = a - (result + 0.5)^2 | ||||
| 						//       = num + result^2 - (result + 0.5)^2 | ||||
| 						//       = num - result - 0.5 | ||||
| 						num -= result; | ||||
| 						num = (num << (NUM_BITS / 2)) - 0x80000000UL; | ||||
| 						result = (result << (NUM_BITS / 2)) + 0x80000000UL; | ||||
| 					} | ||||
| 					else | ||||
| 					{ | ||||
| 						num <<= (NUM_BITS / 2); | ||||
| 						result <<= (NUM_BITS / 2); | ||||
| 					} | ||||
| 
 | ||||
| 					bit = 1UL << (NUM_BITS / 2 - 2); | ||||
| 				} | ||||
| 			} | ||||
| 			// Finally, if next bit would have been 1, round the result upwards. | ||||
| 			if (num > result) | ||||
| 			{ | ||||
| 				++result; | ||||
| 			} | ||||
| 			return new Fix64((long)result); | ||||
| 		} | ||||
| 
 | ||||
| 		private static long ClampSinValue(long angle, out bool flipHorizontal, out bool flipVertical) | ||||
| 		{ | ||||
| 			var largePI = 7244019458077122842; | ||||
| 			// Obtained from ((Fix64)1686629713.065252369824872831112M).m_rawValue | ||||
| 			// This is (2^29)*PI, where 29 is the largest N such that (2^N)*PI < MaxValue. | ||||
| 			// The idea is that this number contains way more precision than PI_TIMES_2, | ||||
| 			// and (((x % (2^29*PI)) % (2^28*PI)) % ... (2^1*PI) = x % (2 * PI) | ||||
| 			// In practice this gives us an error of about 1,25e-9 in the worst case scenario (Sin(MaxValue)) | ||||
| 			// Whereas simply doing x % PI_TIMES_2 is the 2e-3 range. | ||||
| 
 | ||||
| 			var clamped2Pi = angle; | ||||
| 			for (int i = 0; i < 29; ++i) | ||||
| 			{ | ||||
| 				clamped2Pi %= (largePI >> i); | ||||
| 			} | ||||
| 			if (angle < 0) | ||||
| 			{ | ||||
| 				clamped2Pi += PI_TIMES_2; | ||||
| 			} | ||||
| 
 | ||||
| 			// The LUT contains values for 0 - PiOver2; every other value must be obtained by | ||||
| 			// vertical or horizontal mirroring | ||||
| 			flipVertical = clamped2Pi >= PI; | ||||
| 			// obtain (angle % PI) from (angle % 2PI) - much faster than doing another modulo | ||||
| 			var clampedPi = clamped2Pi; | ||||
| 			while (clampedPi >= PI) | ||||
| 			{ | ||||
| 				clampedPi -= PI; | ||||
| 			} | ||||
| 			flipHorizontal = clampedPi >= PI_OVER_2; | ||||
| 			// obtain (angle % PI_OVER_2) from (angle % PI) - much faster than doing another modulo | ||||
| 			var clampedPiOver2 = clampedPi; | ||||
| 			if (clampedPiOver2 >= PI_OVER_2) | ||||
| 			{ | ||||
| 				clampedPiOver2 -= PI_OVER_2; | ||||
| 			} | ||||
| 			return clampedPiOver2; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the sine of the specified angle. | ||||
| 		/// </summary> | ||||
| 		public static Fix64 Sin(Fix64 x) | ||||
| 		{ | ||||
| 			var clampedL = ClampSinValue(x.RawValue, out var flipHorizontal, out var flipVertical); | ||||
| 			var clamped = new Fix64(clampedL); | ||||
| 
 | ||||
| 			// Find the two closest values in the LUT and perform linear interpolation | ||||
| 			// This is what kills the performance of this function on x86 - x64 is fine though | ||||
| 			var rawIndex = FastMul(clamped, LutInterval); | ||||
| 			var roundedIndex = Round(rawIndex); | ||||
| 			var indexError = FastSub(rawIndex, roundedIndex); | ||||
| 
 | ||||
| 			var nearestValue = new Fix64(Fix64Lut.Sin[flipHorizontal ? | ||||
| 				Fix64Lut.Sin.Length - 1 - (int)roundedIndex : | ||||
| 				(int)roundedIndex]); | ||||
| 			var secondNearestValue = new Fix64(Fix64Lut.Sin[flipHorizontal ? | ||||
| 				Fix64Lut.Sin.Length - 1 - (int)roundedIndex - Sign(indexError) : | ||||
| 				(int)roundedIndex + Sign(indexError)]); | ||||
| 
 | ||||
| 			var delta = FastMul(indexError, FastAbs(FastSub(nearestValue, secondNearestValue))).RawValue; | ||||
| 			var interpolatedValue = nearestValue.RawValue + (flipHorizontal ? -delta : delta); | ||||
| 			var finalValue = flipVertical ? -interpolatedValue : interpolatedValue; | ||||
| 			return new Fix64(finalValue); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the cosine of the specified angle. | ||||
| 		/// </summary> | ||||
| 		public static Fix64 Cos(Fix64 x) | ||||
| 		{ | ||||
| 			var xl = x.RawValue; | ||||
| 			var rawAngle = xl + (xl > 0 ? -PI - PI_OVER_2 : PI_OVER_2); | ||||
| 			return Sin(new Fix64(rawAngle)); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the tangent of the specified angle. | ||||
| 		/// </summary> | ||||
| 		public static Fix64 Tan(Fix64 x) | ||||
| 		{ | ||||
| 			var clampedPi = x.RawValue % PI; | ||||
| 			var flip = false; | ||||
| 			if (clampedPi < 0) | ||||
| 			{ | ||||
| 				clampedPi = -clampedPi; | ||||
| 				flip = true; | ||||
| 			} | ||||
| 			if (clampedPi > PI_OVER_2) | ||||
| 			{ | ||||
| 				flip = !flip; | ||||
| 				clampedPi = PI_OVER_2 - (clampedPi - PI_OVER_2); | ||||
| 			} | ||||
| 
 | ||||
| 			var clamped = new Fix64(clampedPi); | ||||
| 
 | ||||
| 			// Find the two closest values in the LUT and perform linear interpolation | ||||
| 			var rawIndex = FastMul(clamped, LutInterval); | ||||
| 			var roundedIndex = Round(rawIndex); | ||||
| 			var indexError = FastSub(rawIndex, roundedIndex); | ||||
| 
 | ||||
| 			var nearestValue = new Fix64(Fix64Lut.Tan[(int)roundedIndex]); | ||||
| 			var secondNearestValue = new Fix64(Fix64Lut.Tan[(int)roundedIndex + Sign(indexError)]); | ||||
| 
 | ||||
| 			var delta = FastMul(indexError, FastAbs(FastSub(nearestValue, secondNearestValue))).RawValue; | ||||
| 			var interpolatedValue = nearestValue.RawValue + delta; | ||||
| 			var finalValue = flip ? -interpolatedValue : interpolatedValue; | ||||
| 			return new Fix64(finalValue); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the angle whose tangent is the specified number. | ||||
| 		/// </summary> | ||||
| 		public static Fix64 Atan(Fix64 z) | ||||
| 		{ | ||||
| 			if (z.RawValue == 0) return Zero; | ||||
| 
 | ||||
| 			// Force positive values for argument | ||||
| 			// Atan(-z) = -Atan(z). | ||||
| 			var neg = z.RawValue < 0; | ||||
| 			if (neg) | ||||
| 			{ | ||||
| 				z = -z; | ||||
| 			} | ||||
| 
 | ||||
| 			Fix64 result; | ||||
| 			var two = (Fix64)2; | ||||
| 			var three = (Fix64)3; | ||||
| 
 | ||||
| 			bool invert = z > One; | ||||
| 			if (invert) z = One / z; | ||||
| 
 | ||||
| 			result = One; | ||||
| 			var term = One; | ||||
| 
 | ||||
| 			var zSq = z * z; | ||||
| 			var zSq2 = zSq * two; | ||||
| 			var zSqPlusOne = zSq + One; | ||||
| 			var zSq12 = zSqPlusOne * two; | ||||
| 			var dividend = zSq2; | ||||
| 			var divisor = zSqPlusOne * three; | ||||
| 
 | ||||
| 			for (var i = 2; i < 30; ++i) | ||||
| 			{ | ||||
| 				term *= dividend / divisor; | ||||
| 				result += term; | ||||
| 
 | ||||
| 				dividend += zSq2; | ||||
| 				divisor += zSq12; | ||||
| 
 | ||||
| 				if (term.RawValue == 0) break; | ||||
| 			} | ||||
| 
 | ||||
| 			result = result * z / zSqPlusOne; | ||||
| 
 | ||||
| 			if (invert) | ||||
| 			{ | ||||
| 				result = PiOver2 - result; | ||||
| 			} | ||||
| 
 | ||||
| 			if (neg) | ||||
| 			{ | ||||
| 				result = -result; | ||||
| 			} | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the angle whose tangent is the quotient of two specified numbers. | ||||
| 		/// </summary> | ||||
| 		public static Fix64 Atan2(Fix64 y, Fix64 x) | ||||
| 		{ | ||||
| 			var yl = y.RawValue; | ||||
| 			var xl = x.RawValue; | ||||
| 			if (xl == 0) | ||||
| 			{ | ||||
| 				if (yl > 0) | ||||
| 				{ | ||||
| 					return PiOver2; | ||||
| 				} | ||||
| 				if (yl == 0) | ||||
| 				{ | ||||
| 					return Zero; | ||||
| 				} | ||||
| 				return -PiOver2; | ||||
| 			} | ||||
| 			Fix64 atan; | ||||
| 			var z = y / x; | ||||
| 
 | ||||
| 			// Deal with overflow | ||||
| 			if (One + (Fix64)0.28M * z * z == MaxValue) | ||||
| 			{ | ||||
| 				return y < Zero ? -PiOver2 : PiOver2; | ||||
| 			} | ||||
| 
 | ||||
| 			if (Abs(z) < One) | ||||
| 			{ | ||||
| 				atan = z / (One + (Fix64)0.28M * z * z); | ||||
| 				if (xl < 0) | ||||
| 				{ | ||||
| 					if (yl < 0) | ||||
| 					{ | ||||
| 						return atan - Pi; | ||||
| 					} | ||||
| 					return atan + Pi; | ||||
| 				} | ||||
| 			} | ||||
| 			else | ||||
| 			{ | ||||
| 				atan = PiOver2 - z / (z * z + (Fix64)0.28M); | ||||
| 				if (yl < 0) | ||||
| 				{ | ||||
| 					return atan - Pi; | ||||
| 				} | ||||
| 			} | ||||
| 			return atan; | ||||
| 		} | ||||
| 
 | ||||
| 		// Operators | ||||
| 
 | ||||
| 		public static Fix64 operator +(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			var xl = x.RawValue; | ||||
| 			var yl = y.RawValue; | ||||
| 			var sum = xl + yl; | ||||
| 			// if signs of operands are equal and signs of sum and x are different | ||||
| 			if (((~(xl ^ yl) & (xl ^ sum)) & MIN_VALUE) != 0) | ||||
| 			{ | ||||
| 				sum = xl > 0 ? MAX_VALUE : MIN_VALUE; | ||||
| 			} | ||||
| 			return new Fix64(sum); | ||||
| 		} | ||||
| 
 | ||||
| 		public static Fix64 operator -(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			var xl = x.RawValue; | ||||
| 			var yl = y.RawValue; | ||||
| 			var diff = xl - yl; | ||||
| 			// if signs of operands are different and signs of sum and x are different | ||||
| 			if ((((xl ^ yl) & (xl ^ diff)) & MIN_VALUE) != 0) | ||||
| 			{ | ||||
| 				diff = xl < 0 ? MIN_VALUE : MAX_VALUE; | ||||
| 			} | ||||
| 			return new Fix64(diff); | ||||
| 		} | ||||
| 
 | ||||
| 		[MethodImpl(MethodImplOptions.AggressiveInlining)] | ||||
| 		private static Fix64 FastSub(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return new Fix64(x.RawValue - y.RawValue); | ||||
| 		} | ||||
| 
 | ||||
| 		private static long AddOverflowHelper(long x, long y, ref bool overflow) | ||||
| 		{ | ||||
| 			var sum = x + y; | ||||
| 			// x + y overflows if sign(x) ^ sign(y) != sign(sum) | ||||
| 			overflow |= ((x ^ y ^ sum) & MIN_VALUE) != 0; | ||||
| 			return sum; | ||||
| 		} | ||||
| 
 | ||||
| 		public static Fix64 operator *(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			var xl = x.RawValue; | ||||
| 			var yl = y.RawValue; | ||||
| 
 | ||||
| 			var xlo = (ulong)(xl & 0x00000000FFFFFFFF); | ||||
| 			var xhi = xl >> FRACTIONAL_PLACES; | ||||
| 			var ylo = (ulong)(yl & 0x00000000FFFFFFFF); | ||||
| 			var yhi = yl >> FRACTIONAL_PLACES; | ||||
| 
 | ||||
| 			var lolo = xlo * ylo; | ||||
| 			var lohi = (long)xlo * yhi; | ||||
| 			var hilo = xhi * (long)ylo; | ||||
| 			var hihi = xhi * yhi; | ||||
| 
 | ||||
| 			var loResult = lolo >> FRACTIONAL_PLACES; | ||||
| 			var midResult1 = lohi; | ||||
| 			var midResult2 = hilo; | ||||
| 			var hiResult = hihi << FRACTIONAL_PLACES; | ||||
| 
 | ||||
| 			bool overflow = false; | ||||
| 			var sum = AddOverflowHelper((long)loResult, midResult1, ref overflow); | ||||
| 			sum = AddOverflowHelper(sum, midResult2, ref overflow); | ||||
| 			sum = AddOverflowHelper(sum, hiResult, ref overflow); | ||||
| 
 | ||||
| 			bool opSignsEqual = ((xl ^ yl) & MIN_VALUE) == 0; | ||||
| 
 | ||||
| 			// if signs of operands are equal and sign of result is negative, | ||||
| 			// then multiplication overflowed positively | ||||
| 			// the reverse is also true | ||||
| 			if (opSignsEqual) | ||||
| 			{ | ||||
| 				if (sum < 0 || (overflow && xl > 0)) | ||||
| 				{ | ||||
| 					return MaxValue; | ||||
| 				} | ||||
| 			} | ||||
| 			else | ||||
| 			{ | ||||
| 				if (sum > 0) | ||||
| 				{ | ||||
| 					return MinValue; | ||||
| 				} | ||||
| 			} | ||||
| 
 | ||||
| 			// if the top 32 bits of hihi (unused in the result) are neither all 0s or 1s, | ||||
| 			// then this means the result overflowed. | ||||
| 			var topCarry = hihi >> FRACTIONAL_PLACES; | ||||
| 			if (topCarry != 0 && topCarry != -1 /*&& xl != -17 && yl != -17*/) | ||||
| 			{ | ||||
| 				return opSignsEqual ? MaxValue : MinValue; | ||||
| 			} | ||||
| 
 | ||||
| 			// If signs differ, both operands' magnitudes are greater than 1, | ||||
| 			// and the result is greater than the negative operand, then there was negative overflow. | ||||
| 			if (!opSignsEqual) | ||||
| 			{ | ||||
| 				long posOp, negOp; | ||||
| 				if (xl > yl) | ||||
| 				{ | ||||
| 					posOp = xl; | ||||
| 					negOp = yl; | ||||
| 				} | ||||
| 				else | ||||
| 				{ | ||||
| 					posOp = yl; | ||||
| 					negOp = xl; | ||||
| 				} | ||||
| 				if (sum > negOp && negOp < -ONE && posOp > ONE) | ||||
| 				{ | ||||
| 					return MinValue; | ||||
| 				} | ||||
| 			} | ||||
| 
 | ||||
| 			return new Fix64(sum); | ||||
| 		} | ||||
| 
 | ||||
| 		private static Fix64 FastMul(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			var xl = x.RawValue; | ||||
| 			var yl = y.RawValue; | ||||
| 
 | ||||
| 			var xlo = (ulong)(xl & 0x00000000FFFFFFFF); | ||||
| 			var xhi = xl >> FRACTIONAL_PLACES; | ||||
| 			var ylo = (ulong)(yl & 0x00000000FFFFFFFF); | ||||
| 			var yhi = yl >> FRACTIONAL_PLACES; | ||||
| 
 | ||||
| 			var lolo = xlo * ylo; | ||||
| 			var lohi = (long)xlo * yhi; | ||||
| 			var hilo = xhi * (long)ylo; | ||||
| 			var hihi = xhi * yhi; | ||||
| 
 | ||||
| 			var loResult = lolo >> FRACTIONAL_PLACES; | ||||
| 			var midResult1 = lohi; | ||||
| 			var midResult2 = hilo; | ||||
| 			var hiResult = hihi << FRACTIONAL_PLACES; | ||||
| 
 | ||||
| 			var sum = (long)loResult + midResult1 + midResult2 + hiResult; | ||||
| 			return new Fix64(sum); | ||||
| 		} | ||||
| 
 | ||||
| 		[MethodImpl(MethodImplOptions.AggressiveInlining)] | ||||
| 		private static int CountLeadingZeroes(ulong x) | ||||
| 		{ | ||||
| 			int result = 0; | ||||
| 			while ((x & 0xF000000000000000) == 0) { result += 4; x <<= 4; } | ||||
| 			while ((x & 0x8000000000000000) == 0) { result += 1; x <<= 1; } | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		public static Fix64 operator /(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			var xl = x.RawValue; | ||||
| 			var yl = y.RawValue; | ||||
| 
 | ||||
| 			if (yl == 0) | ||||
| 			{ | ||||
| 				throw new DivideByZeroException(); | ||||
| 			} | ||||
| 
 | ||||
| 			var remainder = (ulong)(xl >= 0 ? xl : -xl); | ||||
| 			var divider = (ulong)(yl >= 0 ? yl : -yl); | ||||
| 			var quotient = 0UL; | ||||
| 			var bitPos = NUM_BITS / 2 + 1; | ||||
| 
 | ||||
| 
 | ||||
| 			// If the divider is divisible by 2^n, take advantage of it. | ||||
| 			while ((divider & 0xF) == 0 && bitPos >= 4) | ||||
| 			{ | ||||
| 				divider >>= 4; | ||||
| 				bitPos -= 4; | ||||
| 			} | ||||
| 
 | ||||
| 			while (remainder != 0 && bitPos >= 0) | ||||
| 			{ | ||||
| 				int shift = CountLeadingZeroes(remainder); | ||||
| 				if (shift > bitPos) | ||||
| 				{ | ||||
| 					shift = bitPos; | ||||
| 				} | ||||
| 				remainder <<= shift; | ||||
| 				bitPos -= shift; | ||||
| 
 | ||||
| 				var div = remainder / divider; | ||||
| 				remainder = remainder % divider; | ||||
| 				quotient += div << bitPos; | ||||
| 
 | ||||
| 				// Detect overflow | ||||
| 				if ((div & ~(0xFFFFFFFFFFFFFFFF >> bitPos)) != 0) | ||||
| 				{ | ||||
| 					return ((xl ^ yl) & MIN_VALUE) == 0 ? MaxValue : MinValue; | ||||
| 				} | ||||
| 
 | ||||
| 				remainder <<= 1; | ||||
| 				--bitPos; | ||||
| 			} | ||||
| 
 | ||||
| 			// rounding | ||||
| 			++quotient; | ||||
| 			var result = (long)(quotient >> 1); | ||||
| 			if (((xl ^ yl) & MIN_VALUE) != 0) | ||||
| 			{ | ||||
| 				result = -result; | ||||
| 			} | ||||
| 
 | ||||
| 			return new Fix64(result); | ||||
| 		} | ||||
| 
 | ||||
| 		public static Fix64 operator %(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return new Fix64( | ||||
| 				x.RawValue == MIN_VALUE & y.RawValue == -1 ? | ||||
| 				0 : | ||||
| 				x.RawValue % y.RawValue); | ||||
| 		} | ||||
| 
 | ||||
| 		public static Fix64 operator -(Fix64 x) | ||||
| 		{ | ||||
| 			return x.RawValue == MIN_VALUE ? MaxValue : new Fix64(-x.RawValue); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator ==(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return x.RawValue == y.RawValue; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator !=(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return x.RawValue != y.RawValue; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator >(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return x.RawValue > y.RawValue; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator <(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return x.RawValue < y.RawValue; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator >(Fix64 x, int y) | ||||
| 		{ | ||||
| 			return x > ((Fix64) y); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator <(Fix64 x, int y) | ||||
| 		{ | ||||
| 			return x < ((Fix64) y); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator >=(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return x.RawValue >= y.RawValue; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator <=(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			return x.RawValue <= y.RawValue; | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator >=(Fix64 x, int y) | ||||
| 		{ | ||||
| 			return x >= ((Fix64) y); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator <=(Fix64 x, int y) | ||||
| 		{ | ||||
| 			return x <= ((Fix64) y); | ||||
| 		} | ||||
| 
 | ||||
| 		// Casting | ||||
| 
 | ||||
| 		public static explicit operator Fix64(long value) | ||||
| 		{ | ||||
| 			return new Fix64(value * ONE); | ||||
| 		} | ||||
| 
 | ||||
| 		public static explicit operator long(Fix64 value) | ||||
| 		{ | ||||
| 			return value.RawValue >> FRACTIONAL_PLACES; | ||||
| 		} | ||||
| 
 | ||||
| 		public static explicit operator Fix64(float value) | ||||
| 		{ | ||||
| 			return new Fix64((long)(value * ONE)); | ||||
| 		} | ||||
| 
 | ||||
| 		public static explicit operator float(Fix64 value) | ||||
| 		{ | ||||
| 			return (float)value.RawValue / ONE; | ||||
| 		} | ||||
| 
 | ||||
| 		public static explicit operator Fix64(double value) | ||||
| 		{ | ||||
| 			return new Fix64((long)(value * ONE)); | ||||
| 		} | ||||
| 
 | ||||
| 		public static explicit operator double(Fix64 value) | ||||
| 		{ | ||||
| 			return (double)value.RawValue / ONE; | ||||
| 		} | ||||
| 
 | ||||
| 		public static explicit operator Fix64(decimal value) | ||||
| 		{ | ||||
| 			return new Fix64((long)(value * ONE)); | ||||
| 		} | ||||
| 
 | ||||
| 		public static explicit operator decimal(Fix64 value) | ||||
| 		{ | ||||
| 			return (decimal)value.RawValue / ONE; | ||||
| 		} | ||||
| 
 | ||||
| 		public int CompareTo(Fix64 other) | ||||
| 		{ | ||||
| 			return RawValue.CompareTo(other.RawValue); | ||||
| 		} | ||||
| 
 | ||||
| 		public override bool Equals(object obj) | ||||
| 		{ | ||||
| 			return obj is Fix64 fix && RawValue == fix.RawValue; | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(Fix64 other) | ||||
| 		{ | ||||
| 			return RawValue == other.RawValue; | ||||
| 		} | ||||
| 
 | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return RawValue.GetHashCode(); | ||||
| 		} | ||||
| 
 | ||||
| 		// FIXME: can we avoid this cast? | ||||
| 		public override string ToString() | ||||
| 		{ | ||||
| 			// Up to 10 decimal places | ||||
| 			return ((decimal)this).ToString("0.##########"); | ||||
| 		} | ||||
| 
 | ||||
| 		public string ToString(System.Globalization.CultureInfo ci) | ||||
| 		{ | ||||
| 			return ((decimal) this).ToString("0.##########", ci); | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
										
											
												File diff suppressed because it is too large
												Load Diff
											
										
									
								
							
										
											
												File diff suppressed because it is too large
												Load Diff
											
										
									
								
							|  | @ -0,0 +1,846 @@ | |||
| /* MoonWorks - Game Development Framework | ||||
|  * Copyright 2022 Evan Hemsley | ||||
|  */ | ||||
| 
 | ||||
| /* Derived from code by Microsoft. | ||||
|  * Released under the MIT license. | ||||
|  * See microsoft.LICENSE for details. | ||||
|  */ | ||||
| 
 | ||||
| using System; | ||||
| using System.Globalization; | ||||
| 
 | ||||
| namespace MoonWorks.Math.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A structure encapsulating a 3x2 fixed point matrix. | ||||
| 	/// </summary> | ||||
| 	public struct Matrix3x2 : IEquatable<Matrix3x2> | ||||
| 	{ | ||||
| 		#region Public Fields | ||||
| 		/// <summary> | ||||
| 		/// The first element of the first row | ||||
| 		/// </summary> | ||||
| 		public Fix64 M11; | ||||
| 		/// <summary> | ||||
| 		/// The second element of the first row | ||||
| 		/// </summary> | ||||
| 		public Fix64 M12; | ||||
| 		/// <summary> | ||||
| 		/// The first element of the second row | ||||
| 		/// </summary> | ||||
| 		public Fix64 M21; | ||||
| 		/// <summary> | ||||
| 		/// The second element of the second row | ||||
| 		/// </summary> | ||||
| 		public Fix64 M22; | ||||
| 		/// <summary> | ||||
| 		/// The first element of the third row | ||||
| 		/// </summary> | ||||
| 		public Fix64 M31; | ||||
| 		/// <summary> | ||||
| 		/// The second element of the third row | ||||
| 		/// </summary> | ||||
| 		public Fix64 M32; | ||||
| 		#endregion Public Fields | ||||
| 
 | ||||
| 		private static readonly Matrix3x2 _identity = new Matrix3x2 | ||||
| 		( | ||||
| 			1, 0, | ||||
| 			0, 1, | ||||
| 			0, 0 | ||||
| 		); | ||||
| 
 | ||||
| 		private static readonly Fix64 RotationEpsilon = Fix64.FromFraction(1, 1000) * (Fix64.Pi / new Fix64(180)); | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the multiplicative identity matrix. | ||||
| 		/// </summary> | ||||
| 		public static Matrix3x2 Identity | ||||
| 		{ | ||||
| 			get { return _identity; } | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns whether the matrix is the identity matrix. | ||||
| 		/// </summary> | ||||
| 		public bool IsIdentity | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				return  M11 == Fix64.One && M22 == Fix64.One && // Check diagonal element first for early out. | ||||
|                         M12 == Fix64.Zero && | ||||
| 					    M21 == Fix64.Zero && | ||||
| 					    M31 == Fix64.Zero && M32 == Fix64.Zero; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Gets or sets the translation component of this matrix. | ||||
| 		/// </summary> | ||||
| 		public Vector2 Translation | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				return new Vector2(M31, M32); | ||||
| 			} | ||||
| 
 | ||||
| 			set | ||||
| 			{ | ||||
| 				M31 = value.X; | ||||
| 				M32 = value.Y; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Constructs a FixMatrix3x2 from the given components. | ||||
| 		/// </summary> | ||||
| 		public Matrix3x2(Fix64 m11, Fix64 m12, | ||||
| 						 Fix64 m21, Fix64 m22, | ||||
| 						 Fix64 m31, Fix64 m32) | ||||
| 		{ | ||||
| 			M11 = m11; | ||||
| 			M12 = m12; | ||||
| 			M21 = m21; | ||||
| 			M22 = m22; | ||||
| 			M31 = m31; | ||||
| 			M32 = m32; | ||||
| 		} | ||||
| 
 | ||||
|         public Matrix3x2(int m11, int m12, int m21, int m22, int m31, int m32) | ||||
|         { | ||||
|             M11 = new Fix64(m11); | ||||
| 			M12 = new Fix64(m12); | ||||
| 			M21 = new Fix64(m21); | ||||
| 			M22 = new Fix64(m22); | ||||
| 			M31 = new Fix64(m31); | ||||
| 			M32 = new Fix64(m32); | ||||
|         } | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a translation matrix from the given vector. | ||||
| 		/// </summary> | ||||
| 		/// <param name="position">The translation position.</param> | ||||
| 		/// <returns>A translation matrix.</returns> | ||||
| 		public static Matrix3x2 CreateTranslation(Vector2 position) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			result.M11 = Fix64.One; | ||||
| 			result.M12 = Fix64.Zero; | ||||
| 			result.M21 = Fix64.Zero; | ||||
| 			result.M22 = Fix64.One; | ||||
| 
 | ||||
| 			result.M31 = position.X; | ||||
| 			result.M32 = position.Y; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a translation matrix from the given X and Y components. | ||||
| 		/// </summary> | ||||
| 		/// <param name="xPosition">The X position.</param> | ||||
| 		/// <param name="yPosition">The Y position.</param> | ||||
| 		/// <returns>A translation matrix.</returns> | ||||
| 		public static Matrix3x2 CreateTranslation(Fix64 xPosition, Fix64 yPosition) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			result.M11 = Fix64.One; | ||||
| 			result.M12 = Fix64.Zero; | ||||
| 			result.M21 = Fix64.Zero; | ||||
| 			result.M22 = Fix64.One; | ||||
| 
 | ||||
| 			result.M31 = xPosition; | ||||
| 			result.M32 = yPosition; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a scale matrix from the given X and Y components. | ||||
| 		/// </summary> | ||||
| 		/// <param name="xScale">Value to scale by on the X-axis.</param> | ||||
| 		/// <param name="yScale">Value to scale by on the Y-axis.</param> | ||||
| 		/// <returns>A scaling matrix.</returns> | ||||
| 		public static Matrix3x2 CreateScale(Fix64 xScale, Fix64 yScale) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			result.M11 = xScale; | ||||
| 			result.M12 = Fix64.Zero; | ||||
| 			result.M21 = Fix64.Zero; | ||||
| 			result.M22 = yScale; | ||||
| 			result.M31 = Fix64.Zero; | ||||
| 			result.M32 = Fix64.Zero; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a scale matrix that is offset by a given center point. | ||||
| 		/// </summary> | ||||
| 		/// <param name="xScale">Value to scale by on the X-axis.</param> | ||||
| 		/// <param name="yScale">Value to scale by on the Y-axis.</param> | ||||
| 		/// <param name="centerPoint">The center point.</param> | ||||
| 		/// <returns>A scaling matrix.</returns> | ||||
| 		public static Matrix3x2 CreateScale(Fix64 xScale, Fix64 yScale, Vector2 centerPoint) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			Fix64 tx = centerPoint.X * (Fix64.One - xScale); | ||||
| 			Fix64 ty = centerPoint.Y * (Fix64.One - yScale); | ||||
| 
 | ||||
| 			result.M11 = xScale; | ||||
| 			result.M12 = Fix64.Zero; | ||||
| 			result.M21 = Fix64.Zero; | ||||
| 			result.M22 = yScale; | ||||
| 			result.M31 = tx; | ||||
| 			result.M32 = ty; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a scale matrix from the given vector scale. | ||||
| 		/// </summary> | ||||
| 		/// <param name="scales">The scale to use.</param> | ||||
| 		/// <returns>A scaling matrix.</returns> | ||||
| 		public static Matrix3x2 CreateScale(Vector2 scales) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			result.M11 = scales.X; | ||||
| 			result.M12 = Fix64.Zero; | ||||
| 			result.M21 = Fix64.Zero; | ||||
| 			result.M22 = scales.Y; | ||||
| 			result.M31 = Fix64.Zero; | ||||
| 			result.M32 = Fix64.Zero; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a scale matrix from the given vector scale with an offset from the given center point. | ||||
| 		/// </summary> | ||||
| 		/// <param name="scales">The scale to use.</param> | ||||
| 		/// <param name="centerPoint">The center offset.</param> | ||||
| 		/// <returns>A scaling matrix.</returns> | ||||
| 		public static Matrix3x2 CreateScale(Vector2 scales, Vector2 centerPoint) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			Fix64 tx = centerPoint.X * (Fix64.One - scales.X); | ||||
| 			Fix64 ty = centerPoint.Y * (Fix64.One - scales.Y); | ||||
| 
 | ||||
| 			result.M11 = scales.X; | ||||
| 			result.M12 = Fix64.Zero; | ||||
| 			result.M21 = Fix64.Zero; | ||||
| 			result.M22 = scales.Y; | ||||
| 			result.M31 = tx; | ||||
| 			result.M32 = ty; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a scale matrix that scales uniformly with the given scale. | ||||
| 		/// </summary> | ||||
| 		/// <param name="scale">The uniform scale to use.</param> | ||||
| 		/// <returns>A scaling matrix.</returns> | ||||
| 		public static Matrix3x2 CreateScale(Fix64 scale) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			result.M11 = scale; | ||||
| 			result.M12 = Fix64.Zero; | ||||
| 			result.M21 = Fix64.Zero; | ||||
| 			result.M22 = scale; | ||||
| 			result.M31 = Fix64.Zero; | ||||
| 			result.M32 = Fix64.Zero; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a scale matrix that scales uniformly with the given scale with an offset from the given center. | ||||
| 		/// </summary> | ||||
| 		/// <param name="scale">The uniform scale to use.</param> | ||||
| 		/// <param name="centerPoint">The center offset.</param> | ||||
| 		/// <returns>A scaling matrix.</returns> | ||||
| 		public static Matrix3x2 CreateScale(Fix64 scale, Vector2 centerPoint) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			Fix64 tx = centerPoint.X * (Fix64.One - scale); | ||||
| 			Fix64 ty = centerPoint.Y * (Fix64.One - scale); | ||||
| 
 | ||||
| 			result.M11 = scale; | ||||
| 			result.M12 = Fix64.Zero; | ||||
| 			result.M21 = Fix64.Zero; | ||||
| 			result.M22 = scale; | ||||
| 			result.M31 = tx; | ||||
| 			result.M32 = ty; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a skew matrix from the given angles in radians. | ||||
| 		/// </summary> | ||||
| 		/// <param name="radiansX">The X angle, in radians.</param> | ||||
| 		/// <param name="radiansY">The Y angle, in radians.</param> | ||||
| 		/// <returns>A skew matrix.</returns> | ||||
| 		public static Matrix3x2 CreateSkew(Fix64 radiansX, Fix64 radiansY) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			Fix64 xTan = (Fix64) Fix64.Tan(radiansX); | ||||
| 			Fix64 yTan = (Fix64) Fix64.Tan(radiansY); | ||||
| 
 | ||||
| 			result.M11 = Fix64.One; | ||||
| 			result.M12 = yTan; | ||||
| 			result.M21 = xTan; | ||||
| 			result.M22 = Fix64.One; | ||||
| 			result.M31 = Fix64.Zero; | ||||
| 			result.M32 = Fix64.Zero; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a skew matrix from the given angles in radians and a center point. | ||||
| 		/// </summary> | ||||
| 		/// <param name="radiansX">The X angle, in radians.</param> | ||||
| 		/// <param name="radiansY">The Y angle, in radians.</param> | ||||
| 		/// <param name="centerPoint">The center point.</param> | ||||
| 		/// <returns>A skew matrix.</returns> | ||||
| 		public static Matrix3x2 CreateSkew(Fix64 radiansX, Fix64 radiansY, Vector2 centerPoint) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			Fix64 xTan = (Fix64) Fix64.Tan(radiansX); | ||||
| 			Fix64 yTan = (Fix64) Fix64.Tan(radiansY); | ||||
| 
 | ||||
| 			Fix64 tx = -centerPoint.Y * xTan; | ||||
| 			Fix64 ty = -centerPoint.X * yTan; | ||||
| 
 | ||||
| 			result.M11 = Fix64.One; | ||||
| 			result.M12 = yTan; | ||||
| 			result.M21 = xTan; | ||||
| 			result.M22 = Fix64.One; | ||||
| 			result.M31 = tx; | ||||
| 			result.M32 = ty; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a rotation matrix using the given rotation in radians. | ||||
| 		/// </summary> | ||||
| 		/// <param name="radians">The amount of rotation, in radians.</param> | ||||
| 		/// <returns>A rotation matrix.</returns> | ||||
| 		public static Matrix3x2 CreateRotation(Fix64 radians) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			radians = Fix64.IEEERemainder(radians, Fix64.PiTimes2); | ||||
| 
 | ||||
| 			Fix64 c, s; | ||||
| 
 | ||||
| 			if (radians > -RotationEpsilon && radians < RotationEpsilon) | ||||
| 			{ | ||||
| 				// Exact case for zero rotation. | ||||
| 				c = Fix64.One; | ||||
| 				s = Fix64.Zero; | ||||
| 			} | ||||
| 			else if (radians > Fix64.PiOver2 - RotationEpsilon && radians < Fix64.PiOver2 + RotationEpsilon) | ||||
| 			{ | ||||
| 				// Exact case for 90 degree rotation. | ||||
| 				c = Fix64.Zero; | ||||
| 				s = Fix64.One; | ||||
| 			} | ||||
| 			else if (radians < -Fix64.Pi + RotationEpsilon || radians > Fix64.Pi - RotationEpsilon) | ||||
| 			{ | ||||
| 				// Exact case for 180 degree rotation. | ||||
| 				c = -Fix64.One; | ||||
| 				s = Fix64.Zero; | ||||
| 			} | ||||
| 			else if (radians > -Fix64.PiOver2 - RotationEpsilon && radians < -Fix64.PiOver2 + RotationEpsilon) | ||||
| 			{ | ||||
| 				// Exact case for 270 degree rotation. | ||||
| 				c = Fix64.Zero; | ||||
| 				s = -Fix64.One; | ||||
| 			} | ||||
| 			else | ||||
| 			{ | ||||
| 				// Arbitrary rotation. | ||||
| 				c = Fix64.Cos(radians); | ||||
| 				s = Fix64.Sin(radians); | ||||
| 			} | ||||
| 
 | ||||
| 			// [  c  s ] | ||||
| 			// [ -s  c ] | ||||
| 			// [  0  0 ] | ||||
| 			result.M11 = c; | ||||
| 			result.M12 = s; | ||||
| 			result.M21 = -s; | ||||
| 			result.M22 = c; | ||||
| 			result.M31 = Fix64.Zero; | ||||
| 			result.M32 = Fix64.Zero; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a rotation matrix using the given rotation in radians and a center point. | ||||
| 		/// </summary> | ||||
| 		/// <param name="radians">The amount of rotation, in radians.</param> | ||||
| 		/// <param name="centerPoint">The center point.</param> | ||||
| 		/// <returns>A rotation matrix.</returns> | ||||
| 		public static Matrix3x2 CreateRotation(Fix64 radians, Vector2 centerPoint) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			radians = Fix64.IEEERemainder(radians, Fix64.PiTimes2); | ||||
| 
 | ||||
| 			Fix64 c, s; | ||||
| 
 | ||||
| 			if (radians > -RotationEpsilon && radians < RotationEpsilon) | ||||
| 			{ | ||||
| 				// Exact case for zero rotation. | ||||
| 				c = Fix64.One; | ||||
| 				s = Fix64.Zero; | ||||
| 			} | ||||
| 			else if (radians > Fix64.PiOver2 - RotationEpsilon && radians < Fix64.PiOver2 + RotationEpsilon) | ||||
| 			{ | ||||
| 				// Exact case for 90 degree rotation. | ||||
| 				c = Fix64.Zero; | ||||
| 				s = Fix64.One; | ||||
| 			} | ||||
| 			else if (radians < -Fix64.Pi + RotationEpsilon || radians > Fix64.Pi - RotationEpsilon) | ||||
| 			{ | ||||
| 				// Exact case for 180 degree rotation. | ||||
| 				c = -Fix64.One; | ||||
| 				s = Fix64.Zero; | ||||
| 			} | ||||
| 			else if (radians > -Fix64.PiOver2 - RotationEpsilon && radians < -Fix64.PiOver2 + RotationEpsilon) | ||||
| 			{ | ||||
| 				// Exact case for 270 degree rotation. | ||||
| 				c = Fix64.Zero; | ||||
| 				s = -Fix64.One; | ||||
| 			} | ||||
| 			else | ||||
| 			{ | ||||
| 				// Arbitrary rotation. | ||||
| 				c = (Fix64) Fix64.Cos(radians); | ||||
| 				s = (Fix64) Fix64.Sin(radians); | ||||
| 			} | ||||
| 
 | ||||
| 			Fix64 x = centerPoint.X * (Fix64.One - c) + centerPoint.Y * s; | ||||
| 			Fix64 y = centerPoint.Y * (Fix64.One - c) - centerPoint.X * s; | ||||
| 
 | ||||
| 			// [  c  s ] | ||||
| 			// [ -s  c ] | ||||
| 			// [  x  y ] | ||||
| 			result.M11 = c; | ||||
| 			result.M12 = s; | ||||
| 			result.M21 = -s; | ||||
| 			result.M22 = c; | ||||
| 			result.M31 = x; | ||||
| 			result.M32 = y; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Calculates the determinant for this matrix. | ||||
| 		/// The determinant is calculated by expanding the matrix with a third column whose values are (0,0,1). | ||||
| 		/// </summary> | ||||
| 		/// <returns>The determinant.</returns> | ||||
| 		public Fix64 GetDeterminant() | ||||
| 		{ | ||||
| 			// There isn't actually any such thing as a determinant for a non-square matrix, | ||||
| 			// but this 3x2 type is really just an optimization of a 3x3 where we happen to | ||||
| 			// know the rightmost column is always (0, 0, 1). So we expand to 3x3 format: | ||||
| 			// | ||||
| 			//  [ M11, M12, 0 ] | ||||
| 			//  [ M21, M22, 0 ] | ||||
| 			//  [ M31, M32, 1 ] | ||||
| 			// | ||||
| 			// Sum the diagonal products: | ||||
| 			//  (M11 * M22 * 1) + (M12 * 0 * M31) + (0 * M21 * M32) | ||||
| 			// | ||||
| 			// Subtract the opposite diagonal products: | ||||
| 			//  (M31 * M22 * 0) + (M32 * 0 * M11) + (1 * M21 * M12) | ||||
| 			// | ||||
| 			// Collapse out the constants and oh look, this is just a 2x2 determinant! | ||||
| 
 | ||||
| 			return (M11 * M22) - (M21 * M12); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Attempts to invert the given matrix. If the operation succeeds, the inverted matrix is stored in the result parameter. | ||||
| 		/// </summary> | ||||
| 		/// <param name="matrix">The source matrix.</param> | ||||
| 		/// <param name="result">The output matrix.</param> | ||||
| 		/// <returns>True if the operation succeeded, False otherwise.</returns> | ||||
| 		public static bool Invert(Matrix3x2 matrix, out Matrix3x2 result) | ||||
| 		{ | ||||
| 			Fix64 det = (matrix.M11 * matrix.M22) - (matrix.M21 * matrix.M12); | ||||
| 
 | ||||
| 			if (Fix64.Abs(det) == Fix64.Zero) | ||||
| 			{ | ||||
| 				result = new Matrix3x2(Fix64.Zero, Fix64.Zero, Fix64.Zero, Fix64.Zero, Fix64.Zero, Fix64.Zero); | ||||
| 				return false; | ||||
| 			} | ||||
| 
 | ||||
| 			Fix64 invDet = Fix64.One / det; | ||||
| 
 | ||||
| 			result.M11 = matrix.M22 * invDet; | ||||
| 			result.M12 = -matrix.M12 * invDet; | ||||
| 			result.M21 = -matrix.M21 * invDet; | ||||
| 			result.M22 = matrix.M11 * invDet; | ||||
| 			result.M31 = (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22) * invDet; | ||||
| 			result.M32 = (matrix.M31 * matrix.M12 - matrix.M11 * matrix.M32) * invDet; | ||||
| 
 | ||||
| 			return true; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Linearly interpolates from matrix1 to matrix2, based on the third parameter. | ||||
| 		/// </summary> | ||||
| 		/// <param name="matrix1">The first source matrix.</param> | ||||
| 		/// <param name="matrix2">The second source matrix.</param> | ||||
| 		/// <param name="amount">The relative weighting of matrix2.</param> | ||||
| 		/// <returns>The interpolated matrix.</returns> | ||||
| 		public static Matrix3x2 Lerp(Matrix3x2 matrix1, Matrix3x2 matrix2, Fix64 amount) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			// First row | ||||
| 			result.M11 = matrix1.M11 + (matrix2.M11 - matrix1.M11) * amount; | ||||
| 			result.M12 = matrix1.M12 + (matrix2.M12 - matrix1.M12) * amount; | ||||
| 
 | ||||
| 			// Second row | ||||
| 			result.M21 = matrix1.M21 + (matrix2.M21 - matrix1.M21) * amount; | ||||
| 			result.M22 = matrix1.M22 + (matrix2.M22 - matrix1.M22) * amount; | ||||
| 
 | ||||
| 			// Third row | ||||
| 			result.M31 = matrix1.M31 + (matrix2.M31 - matrix1.M31) * amount; | ||||
| 			result.M32 = matrix1.M32 + (matrix2.M32 - matrix1.M32) * amount; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Negates the given matrix by multiplying all values by -1. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">The source matrix.</param> | ||||
| 		/// <returns>The negated matrix.</returns> | ||||
| 		public static Matrix3x2 Negate(Matrix3x2 value) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			result.M11 = -value.M11; | ||||
| 			result.M12 = -value.M12; | ||||
| 			result.M21 = -value.M21; | ||||
| 			result.M22 = -value.M22; | ||||
| 			result.M31 = -value.M31; | ||||
| 			result.M32 = -value.M32; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Adds each matrix element in value1 with its corresponding element in value2. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first source matrix.</param> | ||||
| 		/// <param name="value2">The second source matrix.</param> | ||||
| 		/// <returns>The matrix containing the summed values.</returns> | ||||
| 		public static Matrix3x2 Add(Matrix3x2 value1, Matrix3x2 value2) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			result.M11 = value1.M11 + value2.M11; | ||||
| 			result.M12 = value1.M12 + value2.M12; | ||||
| 			result.M21 = value1.M21 + value2.M21; | ||||
| 			result.M22 = value1.M22 + value2.M22; | ||||
| 			result.M31 = value1.M31 + value2.M31; | ||||
| 			result.M32 = value1.M32 + value2.M32; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Subtracts each matrix element in value2 from its corresponding element in value1. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first source matrix.</param> | ||||
| 		/// <param name="value2">The second source matrix.</param> | ||||
| 		/// <returns>The matrix containing the resulting values.</returns> | ||||
| 		public static Matrix3x2 Subtract(Matrix3x2 value1, Matrix3x2 value2) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			result.M11 = value1.M11 - value2.M11; | ||||
| 			result.M12 = value1.M12 - value2.M12; | ||||
| 			result.M21 = value1.M21 - value2.M21; | ||||
| 			result.M22 = value1.M22 - value2.M22; | ||||
| 			result.M31 = value1.M31 - value2.M31; | ||||
| 			result.M32 = value1.M32 - value2.M32; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Multiplies two matrices together and returns the resulting matrix. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first source matrix.</param> | ||||
| 		/// <param name="value2">The second source matrix.</param> | ||||
| 		/// <returns>The product matrix.</returns> | ||||
| 		public static Matrix3x2 Multiply(Matrix3x2 value1, Matrix3x2 value2) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			// First row | ||||
| 			result.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21; | ||||
| 			result.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22; | ||||
| 
 | ||||
| 			// Second row | ||||
| 			result.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21; | ||||
| 			result.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22; | ||||
| 
 | ||||
| 			// Third row | ||||
| 			result.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value2.M31; | ||||
| 			result.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value2.M32; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		public Matrix4x4 ToMatrix4x4() | ||||
| 		{ | ||||
| 			return new Matrix4x4( | ||||
| 				M11, M12, Fix64.Zero, Fix64.Zero, | ||||
| 				M21, M22, Fix64.Zero, Fix64.Zero, | ||||
| 				Fix64.Zero, Fix64.Zero, Fix64.One, Fix64.Zero, | ||||
| 				M31, M32, Fix64.Zero, Fix64.One | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Scales all elements in a matrix by the given scalar factor. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The source matrix.</param> | ||||
| 		/// <param name="value2">The scaling value to use.</param> | ||||
| 		/// <returns>The resulting matrix.</returns> | ||||
| 		public static Matrix3x2 Multiply(Matrix3x2 value1, Fix64 value2) | ||||
| 		{ | ||||
| 			Matrix3x2 result; | ||||
| 
 | ||||
| 			result.M11 = value1.M11 * value2; | ||||
| 			result.M12 = value1.M12 * value2; | ||||
| 			result.M21 = value1.M21 * value2; | ||||
| 			result.M22 = value1.M22 * value2; | ||||
| 			result.M31 = value1.M31 * value2; | ||||
| 			result.M32 = value1.M32 * value2; | ||||
| 
 | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Negates the given matrix by multiplying all values by -1. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">The source matrix.</param> | ||||
| 		/// <returns>The negated matrix.</returns> | ||||
| 		public static Matrix3x2 operator -(Matrix3x2 value) | ||||
| 		{ | ||||
| 			Matrix3x2 m; | ||||
| 
 | ||||
| 			m.M11 = -value.M11; | ||||
| 			m.M12 = -value.M12; | ||||
| 			m.M21 = -value.M21; | ||||
| 			m.M22 = -value.M22; | ||||
| 			m.M31 = -value.M31; | ||||
| 			m.M32 = -value.M32; | ||||
| 
 | ||||
| 			return m; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Adds each matrix element in value1 with its corresponding element in value2. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first source matrix.</param> | ||||
| 		/// <param name="value2">The second source matrix.</param> | ||||
| 		/// <returns>The matrix containing the summed values.</returns> | ||||
| 		public static Matrix3x2 operator +(Matrix3x2 value1, Matrix3x2 value2) | ||||
| 		{ | ||||
| 			Matrix3x2 m; | ||||
| 
 | ||||
| 			m.M11 = value1.M11 + value2.M11; | ||||
| 			m.M12 = value1.M12 + value2.M12; | ||||
| 			m.M21 = value1.M21 + value2.M21; | ||||
| 			m.M22 = value1.M22 + value2.M22; | ||||
| 			m.M31 = value1.M31 + value2.M31; | ||||
| 			m.M32 = value1.M32 + value2.M32; | ||||
| 
 | ||||
| 			return m; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Subtracts each matrix element in value2 from its corresponding element in value1. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first source matrix.</param> | ||||
| 		/// <param name="value2">The second source matrix.</param> | ||||
| 		/// <returns>The matrix containing the resulting values.</returns> | ||||
| 		public static Matrix3x2 operator -(Matrix3x2 value1, Matrix3x2 value2) | ||||
| 		{ | ||||
| 			Matrix3x2 m; | ||||
| 
 | ||||
| 			m.M11 = value1.M11 - value2.M11; | ||||
| 			m.M12 = value1.M12 - value2.M12; | ||||
| 			m.M21 = value1.M21 - value2.M21; | ||||
| 			m.M22 = value1.M22 - value2.M22; | ||||
| 			m.M31 = value1.M31 - value2.M31; | ||||
| 			m.M32 = value1.M32 - value2.M32; | ||||
| 
 | ||||
| 			return m; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Multiplies two matrices together and returns the resulting matrix. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first source matrix.</param> | ||||
| 		/// <param name="value2">The second source matrix.</param> | ||||
| 		/// <returns>The product matrix.</returns> | ||||
| 		public static Matrix3x2 operator *(Matrix3x2 value1, Matrix3x2 value2) | ||||
| 		{ | ||||
| 			Matrix3x2 m; | ||||
| 
 | ||||
| 			// First row | ||||
| 			m.M11 = value1.M11 * value2.M11 + value1.M12 * value2.M21; | ||||
| 			m.M12 = value1.M11 * value2.M12 + value1.M12 * value2.M22; | ||||
| 
 | ||||
| 			// Second row | ||||
| 			m.M21 = value1.M21 * value2.M11 + value1.M22 * value2.M21; | ||||
| 			m.M22 = value1.M21 * value2.M12 + value1.M22 * value2.M22; | ||||
| 
 | ||||
| 			// Third row | ||||
| 			m.M31 = value1.M31 * value2.M11 + value1.M32 * value2.M21 + value2.M31; | ||||
| 			m.M32 = value1.M31 * value2.M12 + value1.M32 * value2.M22 + value2.M32; | ||||
| 
 | ||||
| 			return m; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Scales all elements in a matrix by the given scalar factor. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The source matrix.</param> | ||||
| 		/// <param name="value2">The scaling value to use.</param> | ||||
| 		/// <returns>The resulting matrix.</returns> | ||||
| 		public static Matrix3x2 operator *(Matrix3x2 value1, Fix64 value2) | ||||
| 		{ | ||||
| 			Matrix3x2 m; | ||||
| 
 | ||||
| 			m.M11 = value1.M11 * value2; | ||||
| 			m.M12 = value1.M12 * value2; | ||||
| 			m.M21 = value1.M21 * value2; | ||||
| 			m.M22 = value1.M22 * value2; | ||||
| 			m.M31 = value1.M31 * value2; | ||||
| 			m.M32 = value1.M32 * value2; | ||||
| 
 | ||||
| 			return m; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a boolean indicating whether the given matrices are equal. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first source matrix.</param> | ||||
| 		/// <param name="value2">The second source matrix.</param> | ||||
| 		/// <returns>True if the matrices are equal; False otherwise.</returns> | ||||
| 		public static bool operator ==(Matrix3x2 value1, Matrix3x2 value2) | ||||
| 		{ | ||||
| 			return (value1.M11 == value2.M11 && value1.M22 == value2.M22 && // Check diagonal element first for early out. | ||||
| 												value1.M12 == value2.M12 && | ||||
| 					value1.M21 == value2.M21 && | ||||
| 					value1.M31 == value2.M31 && value1.M32 == value2.M32); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a boolean indicating whether the given matrices are not equal. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first source matrix.</param> | ||||
| 		/// <param name="value2">The second source matrix.</param> | ||||
| 		/// <returns>True if the matrices are not equal; False if they are equal.</returns> | ||||
| 		public static bool operator !=(Matrix3x2 value1, Matrix3x2 value2) | ||||
| 		{ | ||||
| 			return (value1.M11 != value2.M11 || value1.M12 != value2.M12 || | ||||
| 					value1.M21 != value2.M21 || value1.M22 != value2.M22 || | ||||
| 					value1.M31 != value2.M31 || value1.M32 != value2.M32); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Casts to floating point Matrix3x2. | ||||
| 		/// </summary> | ||||
| 		public static explicit operator Math.Float.Matrix3x2(Matrix3x2 matrix) | ||||
| 		{ | ||||
| 			return new Math.Float.Matrix3x2( | ||||
| 				(float) matrix.M11, (float) matrix.M12, | ||||
| 				(float) matrix.M21, (float) matrix.M22, | ||||
| 				(float) matrix.M31, (float) matrix.M32 | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a boolean indicating whether the matrix is equal to the other given matrix. | ||||
| 		/// </summary> | ||||
| 		/// <param name="other">The other matrix to test equality against.</param> | ||||
| 		/// <returns>True if this matrix is equal to other; False otherwise.</returns> | ||||
| 		public bool Equals(Matrix3x2 other) | ||||
| 		{ | ||||
| 			return (M11 == other.M11 && M22 == other.M22 && // Check diagonal element first for early out. | ||||
| 										M12 == other.M12 && | ||||
| 					M21 == other.M21 && | ||||
| 					M31 == other.M31 && M32 == other.M32); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a boolean indicating whether the given Object is equal to this matrix instance. | ||||
| 		/// </summary> | ||||
| 		/// <param name="obj">The Object to compare against.</param> | ||||
| 		/// <returns>True if the Object is equal to this matrix; False otherwise.</returns> | ||||
| 		public override bool Equals(object obj) | ||||
| 		{ | ||||
| 			if (obj is Matrix3x2) | ||||
| 			{ | ||||
| 				return Equals((Matrix3x2) obj); | ||||
| 			} | ||||
| 
 | ||||
| 			return false; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a String representing this matrix instance. | ||||
| 		/// </summary> | ||||
| 		/// <returns>The string representation.</returns> | ||||
| 		public override string ToString() | ||||
| 		{ | ||||
| 			CultureInfo ci = CultureInfo.CurrentCulture; | ||||
| 			return String.Format(ci, "{{ {{M11:{0} M12:{1}}} {{M21:{2} M22:{3}}} {{M31:{4} M32:{5}}} }}", | ||||
| 								 M11.ToString(ci), M12.ToString(ci), | ||||
| 								 M21.ToString(ci), M22.ToString(ci), | ||||
| 								 M31.ToString(ci), M32.ToString(ci)); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the hash code for this instance. | ||||
| 		/// </summary> | ||||
| 		/// <returns>The hash code.</returns> | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return M11.GetHashCode() + M12.GetHashCode() + | ||||
| 				   M21.GetHashCode() + M22.GetHashCode() + | ||||
| 				   M31.GetHashCode() + M32.GetHashCode(); | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
										
											
												File diff suppressed because it is too large
												Load Diff
											
										
									
								
							|  | @ -0,0 +1,889 @@ | |||
| #region License | ||||
| 
 | ||||
| /* MoonWorks - Game Development Framework | ||||
|  * Copyright 2022 Evan Hemsley | ||||
|  */ | ||||
| 
 | ||||
| /* Derived from code by Ethan Lee (Copyright 2009-2021). | ||||
|  * Released under the Microsoft Public License. | ||||
|  * See fna.LICENSE for details. | ||||
| 
 | ||||
|  * Derived from code by the Mono.Xna Team (Copyright 2006). | ||||
|  * Released under the MIT License. See monoxna.LICENSE for details. | ||||
|  */ | ||||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| #region Using Statements | ||||
| using System; | ||||
| using System.Diagnostics; | ||||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// An efficient mathematical representation for three dimensional fixed point rotations. | ||||
| 	/// </summary> | ||||
| 	[Serializable] | ||||
| 	[DebuggerDisplay("{DebugDisplayString,nq}")] | ||||
| 	public struct Quaternion : IEquatable<Quaternion> | ||||
| 	{ | ||||
| 		#region Public Static Properties | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a quaternion representing no rotation. | ||||
| 		/// </summary> | ||||
| 		public static Quaternion Identity | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				return identity; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Internal Properties | ||||
| 
 | ||||
| 		internal string DebugDisplayString | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				if (this == Quaternion.Identity) | ||||
| 				{ | ||||
| 					return "Identity"; | ||||
| 				} | ||||
| 
 | ||||
| 				return string.Concat( | ||||
| 					X.ToString(), " ", | ||||
| 					Y.ToString(), " ", | ||||
| 					Z.ToString(), " ", | ||||
| 					W.ToString() | ||||
| 				); | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Fields | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// The x coordinate of this <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		public Fix64 X; | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// The y coordinate of this <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		public Fix64 Y; | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// The z coordinate of this <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		public Fix64 Z; | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// The rotation component of this <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		public Fix64 W; | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Private Static Variables | ||||
| 
 | ||||
| 		private static readonly Quaternion identity = new Quaternion(0, 0, 0, 1); | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Constructors | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Constructs a quaternion with X, Y, Z and W from four values. | ||||
| 		/// </summary> | ||||
| 		/// <param name="x">The x coordinate in 3d-space.</param> | ||||
| 		/// <param name="y">The y coordinate in 3d-space.</param> | ||||
| 		/// <param name="z">The z coordinate in 3d-space.</param> | ||||
| 		/// <param name="w">The rotation component.</param> | ||||
| 		public Quaternion(int x, int y, int z, int w) | ||||
| 		{ | ||||
| 			X = new Fix64(x); | ||||
| 			Y = new Fix64(y); | ||||
| 			Z = new Fix64(z); | ||||
| 			W = new Fix64(w); | ||||
| 		} | ||||
| 
 | ||||
|         public Quaternion(Fix64 x, Fix64 y, Fix64 z, Fix64 w) | ||||
|         { | ||||
| 			X = x; | ||||
| 			Y = y; | ||||
| 			Z = z; | ||||
| 			W = w; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Constructs a quaternion with X, Y, Z from <see cref="Vector3"/> and rotation component from a scalar. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">The x, y, z coordinates in 3d-space.</param> | ||||
| 		/// <param name="w">The rotation component.</param> | ||||
| 		public Quaternion(Vector3 vectorPart, Fix64 scalarPart) | ||||
| 		{ | ||||
| 			X = vectorPart.X; | ||||
| 			Y = vectorPart.Y; | ||||
| 			Z = vectorPart.Z; | ||||
| 			W = scalarPart; | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Methods | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Transforms this quaternion into its conjugated version. | ||||
| 		/// </summary> | ||||
| 		public void Conjugate() | ||||
| 		{ | ||||
| 			X = -X; | ||||
| 			Y = -Y; | ||||
| 			Z = -Z; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Compares whether current instance is equal to specified <see cref="Object"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="obj">The <see cref="Object"/> to compare.</param> | ||||
| 		/// <returns><c>true</c> if the instances are equal; <c>false</c> otherwise.</returns> | ||||
| 		public override bool Equals(object obj) | ||||
| 		{ | ||||
| 			return (obj is Quaternion) && Equals((Quaternion) obj); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Compares whether current instance is equal to specified <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="other">The <see cref="Quaternion"/> to compare.</param> | ||||
| 		/// <returns><c>true</c> if the instances are equal; <c>false</c> otherwise.</returns> | ||||
| 		public bool Equals(Quaternion other) | ||||
| 		{ | ||||
| 			return (X == other.X && | ||||
| 					Y == other.Y && | ||||
| 					Z == other.Z && | ||||
| 					W == other.W); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Gets the hash code of this <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		/// <returns>Hash code of this <see cref="Quaternion"/>.</returns> | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return ( | ||||
| 				this.X.GetHashCode() + | ||||
| 				this.Y.GetHashCode() + | ||||
| 				this.Z.GetHashCode() + | ||||
| 				this.W.GetHashCode() | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the magnitude of the quaternion components. | ||||
| 		/// </summary> | ||||
| 		/// <returns>The magnitude of the quaternion components.</returns> | ||||
| 		public Fix64 Length() | ||||
| 		{ | ||||
| 			Fix64 num = ( | ||||
| 				(this.X * this.X) + | ||||
| 				(this.Y * this.Y) + | ||||
| 				(this.Z * this.Z) + | ||||
| 				(this.W * this.W) | ||||
| 			); | ||||
| 			return (Fix64) Fix64.Sqrt(num); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the squared magnitude of the quaternion components. | ||||
| 		/// </summary> | ||||
| 		/// <returns>The squared magnitude of the quaternion components.</returns> | ||||
| 		public Fix64 LengthSquared() | ||||
| 		{ | ||||
| 			return ( | ||||
| 				(this.X * this.X) + | ||||
| 				(this.Y * this.Y) + | ||||
| 				(this.Z * this.Z) + | ||||
| 				(this.W * this.W) | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Scales the quaternion magnitude to unit length. | ||||
| 		/// </summary> | ||||
| 		public void Normalize() | ||||
| 		{ | ||||
| 			Fix64 num = Fix64.One / (Fix64.Sqrt( | ||||
| 				(X * X) + | ||||
| 				(Y * Y) + | ||||
| 				(Z * Z) + | ||||
| 				(W * W) | ||||
| 			)); | ||||
| 			this.X *= num; | ||||
| 			this.Y *= num; | ||||
| 			this.Z *= num; | ||||
| 			this.W *= num; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a <see cref="String"/> representation of this <see cref="Quaternion"/> in the format: | ||||
| 		/// {X:[<see cref="X"/>] Y:[<see cref="Y"/>] Z:[<see cref="Z"/>] W:[<see cref="W"/>]} | ||||
| 		/// </summary> | ||||
| 		/// <returns>A <see cref="String"/> representation of this <see cref="Quaternion"/>.</returns> | ||||
| 		public override string ToString() | ||||
| 		{ | ||||
| 			return ( | ||||
| 				"{X:" + X.ToString() + | ||||
| 				" Y:" + Y.ToString() + | ||||
| 				" Z:" + Z.ToString() + | ||||
| 				" W:" + W.ToString() + | ||||
| 				"}" | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Static Methods | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains the sum of two quaternions. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="quaternion2">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <returns>The result of the quaternion addition.</returns> | ||||
| 		public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Add(ref quaternion1, ref quaternion2, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains the sum of two quaternions. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="quaternion2">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="result">The result of the quaternion addition as an output parameter.</param> | ||||
| 		public static void Add( | ||||
| 			ref Quaternion quaternion1, | ||||
| 			ref Quaternion quaternion2, | ||||
| 			out Quaternion result | ||||
| 		) | ||||
| 		{ | ||||
| 			result.X = quaternion1.X + quaternion2.X; | ||||
| 			result.Y = quaternion1.Y + quaternion2.Y; | ||||
| 			result.Z = quaternion1.Z + quaternion2.Z; | ||||
| 			result.W = quaternion1.W + quaternion2.W; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains concatenation between two quaternion. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first <see cref="Quaternion"/> to concatenate.</param> | ||||
| 		/// <param name="value2">The second <see cref="Quaternion"/> to concatenate.</param> | ||||
| 		/// <returns>The result of rotation of <paramref name="value1"/> followed by <paramref name="value2"/> rotation.</returns> | ||||
| 		public static Quaternion Concatenate(Quaternion value1, Quaternion value2) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Concatenate(ref value1, ref value2, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains concatenation between two quaternion. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first <see cref="Quaternion"/> to concatenate.</param> | ||||
| 		/// <param name="value2">The second <see cref="Quaternion"/> to concatenate.</param> | ||||
| 		/// <param name="result">The result of rotation of <paramref name="value1"/> followed by <paramref name="value2"/> rotation as an output parameter.</param> | ||||
| 		public static void Concatenate( | ||||
| 			ref Quaternion value1, | ||||
| 			ref Quaternion value2, | ||||
| 			out Quaternion result | ||||
| 		) | ||||
| 		{ | ||||
| 			Fix64 x1 = value1.X; | ||||
| 			Fix64 y1 = value1.Y; | ||||
| 			Fix64 z1 = value1.Z; | ||||
| 			Fix64 w1 = value1.W; | ||||
| 
 | ||||
| 			Fix64 x2 = value2.X; | ||||
| 			Fix64 y2 = value2.Y; | ||||
| 			Fix64 z2 = value2.Z; | ||||
| 			Fix64 w2 = value2.W; | ||||
| 
 | ||||
| 			result.X = ((x2 * w1) + (x1 * w2)) + ((y2 * z1) - (z2 * y1)); | ||||
| 			result.Y = ((y2 * w1) + (y1 * w2)) + ((z2 * x1) - (x2 * z1)); | ||||
| 			result.Z = ((z2 * w1) + (z1 * w2)) + ((x2 * y1) - (y2 * x1)); | ||||
| 			result.W = (w2 * w1) - (((x2 * x1) + (y2 * y1)) + (z2 * z1)); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains conjugated version of the specified quaternion. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">The quaternion which values will be used to create the conjugated version.</param> | ||||
| 		/// <returns>The conjugate version of the specified quaternion.</returns> | ||||
| 		public static Quaternion Conjugate(Quaternion value) | ||||
| 		{ | ||||
| 			return new Quaternion(-value.X, -value.Y, -value.Z, value.W); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains conjugated version of the specified quaternion. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">The quaternion which values will be used to create the conjugated version.</param> | ||||
| 		/// <param name="result">The conjugated version of the specified quaternion as an output parameter.</param> | ||||
| 		public static void Conjugate(ref Quaternion value, out Quaternion result) | ||||
| 		{ | ||||
| 			result.X = -value.X; | ||||
| 			result.Y = -value.Y; | ||||
| 			result.Z = -value.Z; | ||||
| 			result.W = value.W; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> from the specified axis and angle. | ||||
| 		/// </summary> | ||||
| 		/// <param name="axis">The axis of rotation.</param> | ||||
| 		/// <param name="angle">The angle in radians.</param> | ||||
| 		/// <returns>The new quaternion builded from axis and angle.</returns> | ||||
| 		public static Quaternion CreateFromAxisAngle(Vector3 axis, Fix64 angle) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			CreateFromAxisAngle(ref axis, angle, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> from the specified axis and angle. | ||||
| 		/// </summary> | ||||
| 		/// <param name="axis">The axis of rotation.</param> | ||||
| 		/// <param name="angle">The angle in radians.</param> | ||||
| 		/// <param name="result">The new quaternion builded from axis and angle as an output parameter.</param> | ||||
| 		public static void CreateFromAxisAngle( | ||||
| 			ref Vector3 axis, | ||||
| 			Fix64 angle, | ||||
| 			out Quaternion result | ||||
| 		) | ||||
| 		{ | ||||
| 			Fix64 half = angle / new Fix64(2); | ||||
| 			Fix64 sin = Fix64.Sin(half); | ||||
| 			Fix64 cos = Fix64.Cos(half); | ||||
| 			result.X = axis.X * sin; | ||||
| 			result.Y = axis.Y * sin; | ||||
| 			result.Z = axis.Z * sin; | ||||
| 			result.W = cos; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> from the specified <see cref="Matrix4x4"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="matrix">The rotation matrix.</param> | ||||
| 		/// <returns>A quaternion composed from the rotation part of the matrix.</returns> | ||||
| 		public static Quaternion CreateFromRotationMatrix(Matrix4x4 matrix) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			CreateFromRotationMatrix(ref matrix, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> from the specified <see cref="Matrix4x4"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="matrix">The rotation matrix.</param> | ||||
| 		/// <param name="result">A quaternion composed from the rotation part of the matrix as an output parameter.</param> | ||||
| 		public static void CreateFromRotationMatrix(ref Matrix4x4 matrix, out Quaternion result) | ||||
| 		{ | ||||
| 			Fix64 sqrt; | ||||
| 			Fix64 half; | ||||
| 			Fix64 scale = matrix.M11 + matrix.M22 + matrix.M33; | ||||
| 			Fix64 two = new Fix64(2); | ||||
| 
 | ||||
| 			if (scale > Fix64.Zero) | ||||
| 			{ | ||||
| 				sqrt = Fix64.Sqrt(scale + Fix64.One); | ||||
| 				result.W = sqrt / two; | ||||
| 				sqrt = Fix64.One / (sqrt * two); | ||||
| 
 | ||||
| 				result.X = (matrix.M23 - matrix.M32) * sqrt; | ||||
| 				result.Y = (matrix.M31 - matrix.M13) * sqrt; | ||||
| 				result.Z = (matrix.M12 - matrix.M21) * sqrt; | ||||
| 			} | ||||
| 			else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33)) | ||||
| 			{ | ||||
| 				sqrt = Fix64.Sqrt(Fix64.One + matrix.M11 - matrix.M22 - matrix.M33); | ||||
| 				half = Fix64.One / (sqrt * two); | ||||
| 
 | ||||
| 				result.X = sqrt / two; | ||||
| 				result.Y = (matrix.M12 + matrix.M21) * half; | ||||
| 				result.Z = (matrix.M13 + matrix.M31) * half; | ||||
| 				result.W = (matrix.M23 - matrix.M32) * half; | ||||
| 			} | ||||
| 			else if (matrix.M22 > matrix.M33) | ||||
| 			{ | ||||
| 				sqrt = Fix64.Sqrt(Fix64.One + matrix.M22 - matrix.M11 - matrix.M33); | ||||
| 				half = Fix64.One / (sqrt * two); | ||||
| 
 | ||||
| 				result.X = (matrix.M21 + matrix.M12) * half; | ||||
| 				result.Y = sqrt / two; | ||||
| 				result.Z = (matrix.M32 + matrix.M23) * half; | ||||
| 				result.W = (matrix.M31 - matrix.M13) * half; | ||||
| 			} | ||||
| 			else | ||||
| 			{ | ||||
| 				sqrt = Fix64.Sqrt(Fix64.One + matrix.M33 - matrix.M11 - matrix.M22); | ||||
| 				half = Fix64.One / (sqrt * two); | ||||
| 
 | ||||
| 				result.X = (matrix.M31 + matrix.M13) * half; | ||||
| 				result.Y = (matrix.M32 + matrix.M23) * half; | ||||
| 				result.Z = sqrt / two; | ||||
| 				result.W = (matrix.M12 - matrix.M21) * half; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> from the specified yaw, pitch and roll angles. | ||||
| 		/// </summary> | ||||
| 		/// <param name="yaw">Yaw around the y axis in radians.</param> | ||||
| 		/// <param name="pitch">Pitch around the x axis in radians.</param> | ||||
| 		/// <param name="roll">Roll around the z axis in radians.</param> | ||||
| 		/// <returns>A new quaternion from the concatenated yaw, pitch, and roll angles.</returns> | ||||
| 		public static Quaternion CreateFromYawPitchRoll(Fix64 yaw, Fix64 pitch, Fix64 roll) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			CreateFromYawPitchRoll(yaw, pitch, roll, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> from the specified yaw, pitch and roll angles. | ||||
| 		/// </summary> | ||||
| 		/// <param name="yaw">Yaw around the y axis in radians.</param> | ||||
| 		/// <param name="pitch">Pitch around the x axis in radians.</param> | ||||
| 		/// <param name="roll">Roll around the z axis in radians.</param> | ||||
| 		/// <param name="result">A new quaternion from the concatenated yaw, pitch, and roll angles as an output parameter.</param> | ||||
| 		public static void CreateFromYawPitchRoll( | ||||
| 			Fix64 yaw, | ||||
| 			Fix64 pitch, | ||||
| 			Fix64 roll, | ||||
| 			out Quaternion result) | ||||
| 		{ | ||||
| 			Fix64 two = new Fix64(2); | ||||
| 			Fix64 halfRoll = roll / two;; | ||||
| 			Fix64 sinRoll = Fix64.Sin(halfRoll); | ||||
| 			Fix64 cosRoll = Fix64.Cos(halfRoll); | ||||
| 			Fix64 halfPitch = pitch / two; | ||||
| 			Fix64 sinPitch = Fix64.Sin(halfPitch); | ||||
| 			Fix64 cosPitch = Fix64.Cos(halfPitch); | ||||
| 			Fix64 halfYaw = yaw / two; | ||||
| 			Fix64 sinYaw = Fix64.Sin(halfYaw); | ||||
| 			Fix64 cosYaw = Fix64.Cos(halfYaw); | ||||
| 			result.X = ((cosYaw * sinPitch) * cosRoll) + ((sinYaw * cosPitch) * sinRoll); | ||||
| 			result.Y = ((sinYaw * cosPitch) * cosRoll) - ((cosYaw * sinPitch) * sinRoll); | ||||
| 			result.Z = ((cosYaw * cosPitch) * sinRoll) - ((sinYaw * sinPitch) * cosRoll); | ||||
| 			result.W = ((cosYaw * cosPitch) * cosRoll) + ((sinYaw * sinPitch) * sinRoll); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Divides a <see cref="Quaternion"/> by the other <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="quaternion2">Divisor <see cref="Quaternion"/>.</param> | ||||
| 		/// <returns>The result of dividing the quaternions.</returns> | ||||
| 		public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Divide(ref quaternion1, ref quaternion2, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Divides a <see cref="Quaternion"/> by the other <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="quaternion2">Divisor <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="result">The result of dividing the quaternions as an output parameter.</param> | ||||
| 		public static void Divide( | ||||
| 			ref Quaternion quaternion1, | ||||
| 			ref Quaternion quaternion2, | ||||
| 			out Quaternion result | ||||
| 		) | ||||
| 		{ | ||||
| 			Fix64 x = quaternion1.X; | ||||
| 			Fix64 y = quaternion1.Y; | ||||
| 			Fix64 z = quaternion1.Z; | ||||
| 			Fix64 w = quaternion1.W; | ||||
| 			Fix64 num14 = ( | ||||
| 				(quaternion2.X * quaternion2.X) + | ||||
| 				(quaternion2.Y * quaternion2.Y) + | ||||
| 				(quaternion2.Z * quaternion2.Z) + | ||||
| 				(quaternion2.W * quaternion2.W) | ||||
| 			); | ||||
| 			Fix64 num5 = Fix64.One / num14; | ||||
| 			Fix64 num4 = -quaternion2.X * num5; | ||||
| 			Fix64 num3 = -quaternion2.Y * num5; | ||||
| 			Fix64 num2 = -quaternion2.Z * num5; | ||||
| 			Fix64 num = quaternion2.W * num5; | ||||
| 			Fix64 num13 = (y * num2) - (z * num3); | ||||
| 			Fix64 num12 = (z * num4) - (x * num2); | ||||
| 			Fix64 num11 = (x * num3) - (y * num4); | ||||
| 			Fix64 num10 = ((x * num4) + (y * num3)) + (z * num2); | ||||
| 			result.X = ((x * num) + (num4 * w)) + num13; | ||||
| 			result.Y = ((y * num) + (num3 * w)) + num12; | ||||
| 			result.Z = ((z * num) + (num2 * w)) + num11; | ||||
| 			result.W = (w * num) - num10; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a dot product of two quaternions. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">The first quaternion.</param> | ||||
| 		/// <param name="quaternion2">The second quaternion.</param> | ||||
| 		/// <returns>The dot product of two quaternions.</returns> | ||||
| 		public static Fix64 Dot(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			return ( | ||||
| 				(quaternion1.X * quaternion2.X) + | ||||
| 				(quaternion1.Y * quaternion2.Y) + | ||||
| 				(quaternion1.Z * quaternion2.Z) + | ||||
| 				(quaternion1.W * quaternion2.W) | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a dot product of two quaternions. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">The first quaternion.</param> | ||||
| 		/// <param name="quaternion2">The second quaternion.</param> | ||||
| 		/// <param name="result">The dot product of two quaternions as an output parameter.</param> | ||||
| 		public static void Dot( | ||||
| 			ref Quaternion quaternion1, | ||||
| 			ref Quaternion quaternion2, | ||||
| 			out Fix64 result | ||||
| 		) | ||||
| 		{ | ||||
| 			result = ( | ||||
| 				(quaternion1.X * quaternion2.X) + | ||||
| 				(quaternion1.Y * quaternion2.Y) + | ||||
| 				(quaternion1.Z * quaternion2.Z) + | ||||
| 				(quaternion1.W * quaternion2.W) | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the inverse quaternion which represents the opposite rotation. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <returns>The inverse quaternion.</returns> | ||||
| 		public static Quaternion Inverse(Quaternion quaternion) | ||||
| 		{ | ||||
| 			Quaternion inverse; | ||||
| 			Inverse(ref quaternion, out inverse); | ||||
| 			return inverse; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the inverse quaternion which represents the opposite rotation. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="result">The inverse quaternion as an output parameter.</param> | ||||
| 		public static void Inverse(ref Quaternion quaternion, out Quaternion result) | ||||
| 		{ | ||||
| 			Fix64 num2 = ( | ||||
| 				(quaternion.X * quaternion.X) + | ||||
| 				(quaternion.Y * quaternion.Y) + | ||||
| 				(quaternion.Z * quaternion.Z) + | ||||
| 				(quaternion.W * quaternion.W) | ||||
| 			); | ||||
| 			Fix64 num = Fix64.One / num2; | ||||
| 			result.X = -quaternion.X * num; | ||||
| 			result.Y = -quaternion.Y * num; | ||||
| 			result.Z = -quaternion.Z * num; | ||||
| 			result.W = quaternion.W * num; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains subtraction of one <see cref="Quaternion"/> from another. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="quaternion2">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <returns>The result of the quaternion subtraction.</returns> | ||||
| 		public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Subtract(ref quaternion1, ref quaternion2, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains subtraction of one <see cref="Quaternion"/> from another. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="quaternion2">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="result">The result of the quaternion subtraction as an output parameter.</param> | ||||
| 		public static void Subtract( | ||||
| 			ref Quaternion quaternion1, | ||||
| 			ref Quaternion quaternion2, | ||||
| 			out Quaternion result | ||||
| 		) | ||||
| 		{ | ||||
| 			result.X = quaternion1.X - quaternion2.X; | ||||
| 			result.Y = quaternion1.Y - quaternion2.Y; | ||||
| 			result.Z = quaternion1.Z - quaternion2.Z; | ||||
| 			result.W = quaternion1.W - quaternion2.W; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains a multiplication of two quaternions. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="quaternion2">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <returns>The result of the quaternion multiplication.</returns> | ||||
| 		public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Multiply(ref quaternion1, ref quaternion2, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains a multiplication of <see cref="Quaternion"/> and a scalar. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="scaleFactor">Scalar value.</param> | ||||
| 		/// <returns>The result of the quaternion multiplication with a scalar.</returns> | ||||
| 		public static Quaternion Multiply(Quaternion quaternion1, Fix64 scaleFactor) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Multiply(ref quaternion1, scaleFactor, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains a multiplication of two quaternions. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="quaternion2">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="result">The result of the quaternion multiplication as an output parameter.</param> | ||||
| 		public static void Multiply( | ||||
| 			ref Quaternion quaternion1, | ||||
| 			ref Quaternion quaternion2, | ||||
| 			out Quaternion result | ||||
| 		) | ||||
| 		{ | ||||
| 			Fix64 x = quaternion1.X; | ||||
| 			Fix64 y = quaternion1.Y; | ||||
| 			Fix64 z = quaternion1.Z; | ||||
| 			Fix64 w = quaternion1.W; | ||||
| 			Fix64 num4 = quaternion2.X; | ||||
| 			Fix64 num3 = quaternion2.Y; | ||||
| 			Fix64 num2 = quaternion2.Z; | ||||
| 			Fix64 num = quaternion2.W; | ||||
| 			Fix64 num12 = (y * num2) - (z * num3); | ||||
| 			Fix64 num11 = (z * num4) - (x * num2); | ||||
| 			Fix64 num10 = (x * num3) - (y * num4); | ||||
| 			Fix64 num9 = ((x * num4) + (y * num3)) + (z * num2); | ||||
| 			result.X = ((x * num) + (num4 * w)) + num12; | ||||
| 			result.Y = ((y * num) + (num3 * w)) + num11; | ||||
| 			result.Z = ((z * num) + (num2 * w)) + num10; | ||||
| 			result.W = (w * num) - num9; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Quaternion"/> that contains a multiplication of <see cref="Quaternion"/> and a scalar. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="scaleFactor">Scalar value.</param> | ||||
| 		/// <param name="result">The result of the quaternion multiplication with a scalar as an output parameter.</param> | ||||
| 		public static void Multiply( | ||||
| 			ref Quaternion quaternion1, | ||||
| 			Fix64 scaleFactor, | ||||
| 			out Quaternion result | ||||
| 		) | ||||
| 		{ | ||||
| 			result.X = quaternion1.X * scaleFactor; | ||||
| 			result.Y = quaternion1.Y * scaleFactor; | ||||
| 			result.Z = quaternion1.Z * scaleFactor; | ||||
| 			result.W = quaternion1.W * scaleFactor; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Flips the sign of the all the quaternion components. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <returns>The result of the quaternion negation.</returns> | ||||
| 		public static Quaternion Negate(Quaternion quaternion) | ||||
| 		{ | ||||
| 			return new Quaternion( | ||||
| 				-quaternion.X, | ||||
| 				-quaternion.Y, | ||||
| 				-quaternion.Z, | ||||
| 				-quaternion.W | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Flips the sign of the all the quaternion components. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="result">The result of the quaternion negation as an output parameter.</param> | ||||
| 		public static void Negate(ref Quaternion quaternion, out Quaternion result) | ||||
| 		{ | ||||
| 			result.X = -quaternion.X; | ||||
| 			result.Y = -quaternion.Y; | ||||
| 			result.Z = -quaternion.Z; | ||||
| 			result.W = -quaternion.W; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Scales the quaternion magnitude to unit length. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <returns>The unit length quaternion.</returns> | ||||
| 		public static Quaternion Normalize(Quaternion quaternion) | ||||
| 		{ | ||||
| 			Quaternion quaternion2; | ||||
| 			Normalize(ref quaternion, out quaternion2); | ||||
| 			return quaternion2; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Scales the quaternion magnitude to unit length. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion">Source <see cref="Quaternion"/>.</param> | ||||
| 		/// <param name="result">The unit length quaternion an output parameter.</param> | ||||
| 		public static void Normalize(ref Quaternion quaternion, out Quaternion result) | ||||
| 		{ | ||||
| 			Fix64 num = Fix64.One / (Fix64.Sqrt( | ||||
| 				(quaternion.X * quaternion.X) + | ||||
| 				(quaternion.Y * quaternion.Y) + | ||||
| 				(quaternion.Z * quaternion.Z) + | ||||
| 				(quaternion.W * quaternion.W) | ||||
| 			)); | ||||
| 			result.X = quaternion.X * num; | ||||
| 			result.Y = quaternion.Y * num; | ||||
| 			result.Z = quaternion.Z * num; | ||||
| 			result.W = quaternion.W * num; | ||||
| 		} | ||||
| 
 | ||||
| 		public static Quaternion LookAt(in Vector3 forward, in Vector3 up) | ||||
| 		{ | ||||
| 			Matrix4x4 orientation = Matrix4x4.Identity; | ||||
| 			orientation.Forward = forward; | ||||
| 			orientation.Right = Vector3.Normalize(Vector3.Cross(forward, up)); | ||||
| 			orientation.Up = Vector3.Cross(orientation.Right, forward); | ||||
| 
 | ||||
| 			return Quaternion.CreateFromRotationMatrix(orientation); | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Static Operator Overloads | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Adds two quaternions. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/> on the left of the add sign.</param> | ||||
| 		/// <param name="quaternion2">Source <see cref="Quaternion"/> on the right of the add sign.</param> | ||||
| 		/// <returns>Sum of the vectors.</returns> | ||||
| 		public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Add(ref quaternion1, ref quaternion2, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Divides a <see cref="Quaternion"/> by the other <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/> on the left of the div sign.</param> | ||||
| 		/// <param name="quaternion2">Divisor <see cref="Quaternion"/> on the right of the div sign.</param> | ||||
| 		/// <returns>The result of dividing the quaternions.</returns> | ||||
| 		public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Divide(ref quaternion1, ref quaternion2, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Compares whether two <see cref="Quaternion"/> instances are equal. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1"><see cref="Quaternion"/> instance on the left of the equal sign.</param> | ||||
| 		/// <param name="quaternion2"><see cref="Quaternion"/> instance on the right of the equal sign.</param> | ||||
| 		/// <returns><c>true</c> if the instances are equal; <c>false</c> otherwise.</returns> | ||||
| 		public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			return quaternion1.Equals(quaternion2); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Compares whether two <see cref="Quaternion"/> instances are not equal. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1"><see cref="Quaternion"/> instance on the left of the not equal sign.</param> | ||||
| 		/// <param name="quaternion2"><see cref="Quaternion"/> instance on the right of the not equal sign.</param> | ||||
| 		/// <returns><c>true</c> if the instances are not equal; <c>false</c> otherwise.</returns> | ||||
| 		public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			return !quaternion1.Equals(quaternion2); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Multiplies two quaternions. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Quaternion"/> on the left of the mul sign.</param> | ||||
| 		/// <param name="quaternion2">Source <see cref="Quaternion"/> on the right of the mul sign.</param> | ||||
| 		/// <returns>Result of the quaternions multiplication.</returns> | ||||
| 		public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Multiply(ref quaternion1, ref quaternion2, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Multiplies the components of quaternion by a scalar. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Vector3"/> on the left of the mul sign.</param> | ||||
| 		/// <param name="scaleFactor">Scalar value on the right of the mul sign.</param> | ||||
| 		/// <returns>Result of the quaternion multiplication with a scalar.</returns> | ||||
| 		public static Quaternion operator *(Quaternion quaternion1, Fix64 scaleFactor) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Multiply(ref quaternion1, scaleFactor, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Subtracts a <see cref="Quaternion"/> from a <see cref="Quaternion"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion1">Source <see cref="Vector3"/> on the left of the sub sign.</param> | ||||
| 		/// <param name="quaternion2">Source <see cref="Vector3"/> on the right of the sub sign.</param> | ||||
| 		/// <returns>Result of the quaternion subtraction.</returns> | ||||
| 		public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2) | ||||
| 		{ | ||||
| 			Quaternion quaternion; | ||||
| 			Subtract(ref quaternion1, ref quaternion2, out quaternion); | ||||
| 			return quaternion; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Flips the sign of the all the quaternion components. | ||||
| 		/// </summary> | ||||
| 		/// <param name="quaternion">Source <see cref="Quaternion"/> on the right of the sub sign.</param> | ||||
| 		/// <returns>The result of the quaternion negation.</returns> | ||||
| 		public static Quaternion operator -(Quaternion quaternion) | ||||
| 		{ | ||||
| 			Quaternion quaternion2; | ||||
| 			Negate(ref quaternion, out quaternion2); | ||||
| 			return quaternion2; | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 	} | ||||
| } | ||||
|  | @ -0,0 +1,105 @@ | |||
| namespace MoonWorks.Math.Fixed | ||||
| { | ||||
| 	public struct Transform2D : System.IEquatable<Transform2D> | ||||
| 	{ | ||||
| 		public Vector2 Position { get; } | ||||
| 		public Fix64 Rotation { get; } | ||||
| 		public Vector2 Scale { get; } | ||||
| 
 | ||||
| 		private bool transformMatrixCalculated = false; | ||||
| 		private Matrix3x2 transformMatrix = Matrix3x2.Identity; | ||||
| 		public Matrix3x2 TransformMatrix | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				if (!transformMatrixCalculated) | ||||
| 				{ | ||||
| 					transformMatrix = CreateTransformMatrix(Position, Rotation, Scale); | ||||
| 					transformMatrixCalculated = true; | ||||
| 				} | ||||
| 
 | ||||
| 				return transformMatrix; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		public bool IsAxisAligned => Rotation % Fix64.PiOver2 == Fix64.Zero; | ||||
| 		public bool IsUniformScale => Scale.X == Scale.Y; | ||||
| 
 | ||||
| 		public static Transform2D Identity = new Transform2D(Vector2.Zero, Fix64.Zero, Vector2.One); | ||||
| 
 | ||||
| 		public Transform2D() | ||||
| 		{ | ||||
| 			Position = Vector2.Zero; | ||||
| 			Rotation = Fix64.Zero; | ||||
| 			Scale = Vector2.One; | ||||
| 		} | ||||
| 
 | ||||
| 		public Transform2D(Vector2 position) | ||||
| 		{ | ||||
| 			Position = position; | ||||
| 			Rotation = Fix64.Zero; | ||||
| 			Scale = Vector2.One; | ||||
| 		} | ||||
| 
 | ||||
| 		public Transform2D(Vector2 position, Fix64 rotation) | ||||
| 		{ | ||||
| 			Position = position; | ||||
| 			Rotation = rotation; | ||||
| 			Scale = Vector2.One; | ||||
| 		} | ||||
| 
 | ||||
| 		public Transform2D(Vector2 position, Fix64 rotation, Vector2 scale) | ||||
| 		{ | ||||
| 			Position = position; | ||||
| 			Rotation = rotation; | ||||
| 			Scale = scale; | ||||
| 		} | ||||
| 
 | ||||
| 		public Transform2D Compose(Transform2D other) | ||||
| 		{ | ||||
| 			return new Transform2D(Position + other.Position, Rotation + other.Rotation, Scale * other.Scale); | ||||
| 		} | ||||
| 
 | ||||
| 		private static Matrix3x2 CreateTransformMatrix(Vector2 position, Fix64 rotation, Vector2 scale) | ||||
| 		{ | ||||
| 			return | ||||
| 				Matrix3x2.CreateScale(scale) * | ||||
| 				Matrix3x2.CreateRotation(rotation) * | ||||
| 				Matrix3x2.CreateTranslation(position); | ||||
| 		} | ||||
| 
 | ||||
| 		public bool Equals(Transform2D other) | ||||
| 		{ | ||||
| 			return | ||||
| 				Position == other.Position && | ||||
| 				Rotation == other.Rotation && | ||||
| 				Scale == other.Scale; | ||||
| 		} | ||||
| 
 | ||||
| 
 | ||||
|         public override bool Equals(System.Object other) | ||||
|         { | ||||
|             if (other is Transform2D otherTransform) | ||||
|             { | ||||
|                 return Equals(otherTransform); | ||||
|             } | ||||
| 
 | ||||
|             return false; | ||||
|         } | ||||
| 
 | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return System.HashCode.Combine(Position, Rotation, Scale); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator ==(Transform2D a, Transform2D b) | ||||
| 		{ | ||||
| 			return a.Equals(b); | ||||
| 		} | ||||
| 
 | ||||
| 		public static bool operator !=(Transform2D a, Transform2D b) | ||||
| 		{ | ||||
| 			return !a.Equals(b); | ||||
| 		} | ||||
| 	} | ||||
| } | ||||
|  | @ -0,0 +1,810 @@ | |||
| #region License | ||||
| 
 | ||||
| /* MoonWorks - Game Development Framework | ||||
|  * Copyright 2022 Evan Hemsley | ||||
|  */ | ||||
| 
 | ||||
| /* Derived from code by Ethan Lee (Copyright 2009-2021). | ||||
|  * Released under the Microsoft Public License. | ||||
|  * See fna.LICENSE for details. | ||||
| 
 | ||||
|  * Derived from code by the Mono.Xna Team (Copyright 2006). | ||||
|  * Released under the MIT License. See monoxna.LICENSE for details. | ||||
|  */ | ||||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| #region Using Statements | ||||
| using System; | ||||
| using System.Diagnostics; | ||||
| using System.Runtime.InteropServices; | ||||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math.Fixed | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Describes a fixed point 2D-vector. | ||||
| 	/// </summary> | ||||
| 	[Serializable] | ||||
| 	[DebuggerDisplay("{DebugDisplayString,nq}")] | ||||
| 	[StructLayout(LayoutKind.Explicit)] | ||||
| 	public struct Vector2 : IEquatable<Vector2> | ||||
| 	{ | ||||
| 		#region Public Static Properties | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a <see cref="Vector2"/> with components 0, 0. | ||||
| 		/// </summary> | ||||
| 		public static Vector2 Zero | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				return zeroVector; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a <see cref="Vector2"/> with components 1, 1. | ||||
| 		/// </summary> | ||||
| 		public static Vector2 One | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				return unitVector; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a <see cref="Vector2"/> with components 1, 0. | ||||
| 		/// </summary> | ||||
| 		public static Vector2 UnitX | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				return unitXVector; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a <see cref="Vector2"/> with components 0, 1. | ||||
| 		/// </summary> | ||||
| 		public static Vector2 UnitY | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				return unitYVector; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Internal Properties | ||||
| 
 | ||||
| 		internal string DebugDisplayString | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				return string.Concat( | ||||
| 					X.ToString(), " ", | ||||
| 					Y.ToString() | ||||
| 				); | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Fields | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// The x coordinate of this <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		[FieldOffset(0)] | ||||
| 		public Fix64 X; | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// The y coordinate of this <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		[FieldOffset(8)] | ||||
| 		public Fix64 Y; | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Private Static Fields | ||||
| 
 | ||||
| 		private static readonly Vector2 zeroVector = new Vector2(0, 0); | ||||
| 		private static readonly Vector2 unitVector = new Vector2(1, 1); | ||||
| 		private static readonly Vector2 unitXVector = new Vector2(1, 0); | ||||
| 		private static readonly Vector2 unitYVector = new Vector2(0, 1); | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Constructors | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Constructs a 2d vector with X and Y from two values. | ||||
| 		/// </summary> | ||||
| 		/// <param name="x">The x coordinate in 2d-space.</param> | ||||
| 		/// <param name="y">The y coordinate in 2d-space.</param> | ||||
| 		public Vector2(Fix64 x, Fix64 y) | ||||
| 		{ | ||||
| 			this.X = x; | ||||
| 			this.Y = y; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Constructs a 2d vector with X and Y set to the same value. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">The x and y coordinates in 2d-space.</param> | ||||
| 		public Vector2(Fix64 value) | ||||
| 		{ | ||||
| 			this.X = value; | ||||
| 			this.Y = value; | ||||
| 		} | ||||
| 
 | ||||
| 		public Vector2(int x, int y) | ||||
| 		{ | ||||
| 			this.X = new Fix64(x); | ||||
| 			this.Y = new Fix64(y); | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Methods | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Compares whether current instance is equal to specified <see cref="Object"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="obj">The <see cref="Object"/> to compare.</param> | ||||
| 		/// <returns><c>true</c> if the instances are equal; <c>false</c> otherwise.</returns> | ||||
| 		public override bool Equals(object obj) | ||||
| 		{ | ||||
| 			return obj is Vector2 fixVector && Equals(fixVector); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Compares whether current instance is equal to specified <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="other">The <see cref="Vector2"/> to compare.</param> | ||||
| 		/// <returns><c>true</c> if the instances are equal; <c>false</c> otherwise.</returns> | ||||
| 		public bool Equals(Vector2 other) | ||||
| 		{ | ||||
| 			return (X == other.X && | ||||
| 					Y == other.Y); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Gets the hash code of this <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <returns>Hash code of this <see cref="Vector2"/>.</returns> | ||||
| 		public override int GetHashCode() | ||||
| 		{ | ||||
| 			return X.GetHashCode() + Y.GetHashCode(); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the length of this <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <returns>The length of this <see cref="Vector2"/>.</returns> | ||||
| 		public Fix64 Length() | ||||
| 		{ | ||||
| 			return Fix64.Sqrt((X * X) + (Y * Y)); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the squared length of this <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <returns>The squared length of this <see cref="Vector2"/>.</returns> | ||||
| 		public Fix64 LengthSquared() | ||||
| 		{ | ||||
| 			return (X * X) + (Y * Y); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Turns this <see cref="Vector2"/> to a unit vector with the same direction. | ||||
| 		/// </summary> | ||||
| 		public void Normalize() | ||||
| 		{ | ||||
| 			Fix64 val = Fix64.One / Fix64.Sqrt((X * X) + (Y * Y)); | ||||
| 			X *= val; | ||||
| 			Y *= val; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Turns this <see cref="Vector2"/> to an angle in radians. | ||||
| 		/// </summary> | ||||
| 		public Fix64 Angle() | ||||
| 		{ | ||||
| 			return Fix64.Atan2(Y, X); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns this Vector2 with the fractional components cut off. | ||||
| 		/// </summary> | ||||
| 		public Vector2 Truncated() | ||||
| 		{ | ||||
| 			return new Vector2((int) X, (int) Y); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a <see cref="String"/> representation of this <see cref="Vector2"/> in the format: | ||||
| 		/// {X:[<see cref="X"/>] Y:[<see cref="Y"/>]} | ||||
| 		/// </summary> | ||||
| 		/// <returns>A <see cref="String"/> representation of this <see cref="Vector2"/>.</returns> | ||||
| 		public override string ToString() | ||||
| 		{ | ||||
| 			return ( | ||||
| 				"{X:" + X.ToString() + | ||||
| 				" Y:" + Y.ToString() + | ||||
| 				"}" | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Static Methods | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Performs vector addition on <paramref name="value1"/> and <paramref name="value2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first vector to add.</param> | ||||
| 		/// <param name="value2">The second vector to add.</param> | ||||
| 		/// <returns>The result of the vector addition.</returns> | ||||
| 		public static Vector2 Add(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			value1.X += value2.X; | ||||
| 			value1.Y += value2.Y; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Clamps the specified value within a range. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The value to clamp.</param> | ||||
| 		/// <param name="min">The min value.</param> | ||||
| 		/// <param name="max">The max value.</param> | ||||
| 		/// <returns>The clamped value.</returns> | ||||
| 		public static Vector2 Clamp(Vector2 value1, Vector2 min, Vector2 max) | ||||
| 		{ | ||||
| 			return new Vector2( | ||||
| 				Fix64.Clamp(value1.X, min.X, max.X), | ||||
| 				Fix64.Clamp(value1.Y, min.Y, max.Y) | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the distance between two vectors. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first vector.</param> | ||||
| 		/// <param name="value2">The second vector.</param> | ||||
| 		/// <returns>The distance between two vectors.</returns> | ||||
| 		public static Fix64 Distance(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			Fix64 v1 = value1.X - value2.X, v2 = value1.Y - value2.Y; | ||||
| 			return Fix64.Sqrt((v1 * v1) + (v2 * v2)); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns the squared distance between two vectors. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first vector.</param> | ||||
| 		/// <param name="value2">The second vector.</param> | ||||
| 		/// <returns>The squared distance between two vectors.</returns> | ||||
| 		public static Fix64 DistanceSquared(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			Fix64 v1 = value1.X - value2.X, v2 = value1.Y - value2.Y; | ||||
| 			return (v1 * v1) + (v2 * v2); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Divides the components of a <see cref="Vector2"/> by the components of another <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="value2">Divisor <see cref="Vector2"/>.</param> | ||||
| 		/// <returns>The result of dividing the vectors.</returns> | ||||
| 		public static Vector2 Divide(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			value1.X /= value2.X; | ||||
| 			value1.Y /= value2.Y; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Divides the components of a <see cref="Vector2"/> by a scalar. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="divider">Divisor scalar.</param> | ||||
| 		/// <returns>The result of dividing a vector by a scalar.</returns> | ||||
| 		public static Vector2 Divide(Vector2 value1, Fix64 divider) | ||||
| 		{ | ||||
| 			Fix64 factor = Fix64.One / divider; | ||||
| 			value1.X *= factor; | ||||
| 			value1.Y *= factor; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Returns a dot product of two vectors. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first vector.</param> | ||||
| 		/// <param name="value2">The second vector.</param> | ||||
| 		/// <returns>The dot product of two vectors.</returns> | ||||
| 		public static Fix64 Dot(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			return (value1.X * value2.X) + (value1.Y * value2.Y); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a maximal values from the two vectors. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first vector.</param> | ||||
| 		/// <param name="value2">The second vector.</param> | ||||
| 		/// <returns>The <see cref="Vector2"/> with maximal values from the two vectors.</returns> | ||||
| 		public static Vector2 Max(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			return new Vector2( | ||||
| 				value1.X > value2.X ? value1.X : value2.X, | ||||
| 				value1.Y > value2.Y ? value1.Y : value2.Y | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a minimal values from the two vectors. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">The first vector.</param> | ||||
| 		/// <param name="value2">The second vector.</param> | ||||
| 		/// <returns>The <see cref="Vector2"/> with minimal values from the two vectors.</returns> | ||||
| 		public static Vector2 Min(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			return new Vector2( | ||||
| 				value1.X < value2.X ? value1.X : value2.X, | ||||
| 				value1.Y < value2.Y ? value1.Y : value2.Y | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a multiplication of two vectors. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="value2">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <returns>The result of the vector multiplication.</returns> | ||||
| 		public static Vector2 Multiply(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			value1.X *= value2.X; | ||||
| 			value1.Y *= value2.Y; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a multiplication of <see cref="Vector2"/> and a scalar. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="scaleFactor">Scalar value.</param> | ||||
| 		/// <returns>The result of the vector multiplication with a scalar.</returns> | ||||
| 		public static Vector2 Multiply(Vector2 value1, Fix64 scaleFactor) | ||||
| 		{ | ||||
| 			value1.X *= scaleFactor; | ||||
| 			value1.Y *= scaleFactor; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains the specified vector inversion. | ||||
| 		/// direction of <paramref name="value"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <returns>The result of the vector inversion.</returns> | ||||
| 		public static Vector2 Negate(Vector2 value) | ||||
| 		{ | ||||
| 			value.X = -value.X; | ||||
| 			value.Y = -value.Y; | ||||
| 			return value; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a normalized values from another vector. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <returns>Unit vector.</returns> | ||||
| 		public static Vector2 Normalize(Vector2 value) | ||||
| 		{ | ||||
| 			Fix64 val = Fix64.One / Fix64.Sqrt((value.X * value.X) + (value.Y * value.Y)); | ||||
| 			value.X *= val; | ||||
| 			value.Y *= val; | ||||
| 			return value; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains reflect vector of the given vector and normal. | ||||
| 		/// </summary> | ||||
| 		/// <param name="vector">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="normal">Reflection normal.</param> | ||||
| 		/// <returns>Reflected vector.</returns> | ||||
| 		public static Vector2 Reflect(Vector2 vector, Vector2 normal) | ||||
| 		{ | ||||
| 			Vector2 result; | ||||
| 			Fix64 val = new Fix64(2) * ((vector.X * normal.X) + (vector.Y * normal.Y)); | ||||
| 			result.X = vector.X - (normal.X * val); | ||||
| 			result.Y = vector.Y - (normal.Y * val); | ||||
| 			return result; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains subtraction of on <see cref="Vector2"/> from a another. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="value2">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <returns>The result of the vector subtraction.</returns> | ||||
| 		public static Vector2 Subtract(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			value1.X -= value2.X; | ||||
| 			value1.Y -= value2.Y; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a transformation of 2d-vector by the specified <see cref="Matrix4x4"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="position">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="matrix">The transformation <see cref="Matrix4x4"/>.</param> | ||||
| 		/// <returns>Transformed <see cref="Vector2"/>.</returns> | ||||
| 		public static Vector2 Transform(Vector2 position, Matrix4x4 matrix) | ||||
| 		{ | ||||
| 			return new Vector2( | ||||
| 				(position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, | ||||
| 				(position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42 | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a transformation of 2d-vector by the specified <see cref="Quaternion"/>, representing the rotation. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param> | ||||
| 		/// <returns>Transformed <see cref="Vector2"/>.</returns> | ||||
| 		public static Vector2 Transform(Vector2 value, Quaternion rotation) | ||||
| 		{ | ||||
| 			Transform(ref value, ref rotation, out value); | ||||
| 			return value; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a transformation of 2d-vector by the specified <see cref="Quaternion"/>, representing the rotation. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param> | ||||
| 		/// <param name="result">Transformed <see cref="Vector2"/> as an output parameter.</param> | ||||
| 		public static void Transform( | ||||
| 			ref Vector2 value, | ||||
| 			ref Quaternion rotation, | ||||
| 			out Vector2 result | ||||
| 		) | ||||
| 		{ | ||||
| 			Fix64 two = new Fix64(2); | ||||
| 			Fix64 x = two * -(rotation.Z * value.Y); | ||||
| 			Fix64 y = two * (rotation.Z * value.X); | ||||
| 			Fix64 z = two * (rotation.X * value.Y - rotation.Y * value.X); | ||||
| 
 | ||||
| 			result.X = value.X + x * rotation.W + (rotation.Y * z - rotation.Z * y); | ||||
| 			result.Y = value.Y + y * rotation.W + (rotation.Z * x - rotation.X * z); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a transformation of 2d-vector by the specified <see cref="Math.Matrix3x2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="position">Source <see cref="Vector2"/>.</param> | ||||
| 		/// <param name="matrix">The transformation <see cref="Math.Matrix3x2"/>.</param> | ||||
| 		/// <returns>Transformed <see cref="Vector2"/>.</returns> | ||||
| 		public static Vector2 Transform(Vector2 position, Matrix3x2 matrix) | ||||
| 		{ | ||||
| 			return new Vector2( | ||||
| 				(position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M31, | ||||
| 				(position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M32 | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Apply transformation on all vectors within array of <see cref="Vector2"/> by the specified <see cref="Matrix4x4"/> and places the results in an another array. | ||||
| 		/// </summary> | ||||
| 		/// <param name="sourceArray">Source array.</param> | ||||
| 		/// <param name="matrix">The transformation <see cref="Matrix4x4"/>.</param> | ||||
| 		/// <param name="destinationArray">Destination array.</param> | ||||
| 		public static void Transform( | ||||
| 			Vector2[] sourceArray, | ||||
| 			ref Matrix4x4 matrix, | ||||
| 			Vector2[] destinationArray | ||||
| 		) | ||||
| 		{ | ||||
| 			Transform(sourceArray, 0, ref matrix, destinationArray, 0, sourceArray.Length); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Apply transformation on vectors within array of <see cref="Vector2"/> by the specified <see cref="Matrix4x4"/> and places the results in an another array. | ||||
| 		/// </summary> | ||||
| 		/// <param name="sourceArray">Source array.</param> | ||||
| 		/// <param name="sourceIndex">The starting index of transformation in the source array.</param> | ||||
| 		/// <param name="matrix">The transformation <see cref="Matrix4x4"/>.</param> | ||||
| 		/// <param name="destinationArray">Destination array.</param> | ||||
| 		/// <param name="destinationIndex">The starting index in the destination array, where the first <see cref="Vector2"/> should be written.</param> | ||||
| 		/// <param name="length">The number of vectors to be transformed.</param> | ||||
| 		public static void Transform( | ||||
| 			Vector2[] sourceArray, | ||||
| 			int sourceIndex, | ||||
| 			ref Matrix4x4 matrix, | ||||
| 			Vector2[] destinationArray, | ||||
| 			int destinationIndex, | ||||
| 			int length | ||||
| 		) | ||||
| 		{ | ||||
| 			for (int x = 0; x < length; x += 1) | ||||
| 			{ | ||||
| 				Vector2 position = sourceArray[sourceIndex + x]; | ||||
| 				Vector2 destination = destinationArray[destinationIndex + x]; | ||||
| 				destination.X = (position.X * matrix.M11) + (position.Y * matrix.M21) | ||||
| 						+ matrix.M41; | ||||
| 				destination.Y = (position.X * matrix.M12) + (position.Y * matrix.M22) | ||||
| 						+ matrix.M42; | ||||
| 				destinationArray[destinationIndex + x] = destination; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Apply transformation on all vectors within array of <see cref="Vector2"/> by the specified <see cref="Quaternion"/> and places the results in an another array. | ||||
| 		/// </summary> | ||||
| 		/// <param name="sourceArray">Source array.</param> | ||||
| 		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param> | ||||
| 		/// <param name="destinationArray">Destination array.</param> | ||||
| 		public static void Transform( | ||||
| 			Vector2[] sourceArray, | ||||
| 			ref Quaternion rotation, | ||||
| 			Vector2[] destinationArray | ||||
| 		) | ||||
| 		{ | ||||
| 			Transform( | ||||
| 				sourceArray, | ||||
| 				0, | ||||
| 				ref rotation, | ||||
| 				destinationArray, | ||||
| 				0, | ||||
| 				sourceArray.Length | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Apply transformation on vectors within array of <see cref="Vector2"/> by the specified <see cref="Quaternion"/> and places the results in an another array. | ||||
| 		/// </summary> | ||||
| 		/// <param name="sourceArray">Source array.</param> | ||||
| 		/// <param name="sourceIndex">The starting index of transformation in the source array.</param> | ||||
| 		/// <param name="rotation">The <see cref="Quaternion"/> which contains rotation transformation.</param> | ||||
| 		/// <param name="destinationArray">Destination array.</param> | ||||
| 		/// <param name="destinationIndex">The starting index in the destination array, where the first <see cref="Vector2"/> should be written.</param> | ||||
| 		/// <param name="length">The number of vectors to be transformed.</param> | ||||
| 		public static void Transform( | ||||
| 			Vector2[] sourceArray, | ||||
| 			int sourceIndex, | ||||
| 			ref Quaternion rotation, | ||||
| 			Vector2[] destinationArray, | ||||
| 			int destinationIndex, | ||||
| 			int length | ||||
| 		) | ||||
| 		{ | ||||
| 			for (int i = 0; i < length; i += 1) | ||||
| 			{ | ||||
| 				Vector2 position = sourceArray[sourceIndex + i]; | ||||
| 				Vector2 v; | ||||
| 				Transform(ref position, ref rotation, out v); | ||||
| 				destinationArray[destinationIndex + i] = v; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a transformation of the specified normal by the specified <see cref="Matrix4x4"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="normal">Source <see cref="Vector2"/> which represents a normal vector.</param> | ||||
| 		/// <param name="matrix">The transformation <see cref="Matrix4x4"/>.</param> | ||||
| 		/// <returns>Transformed normal.</returns> | ||||
| 		public static Vector2 TransformNormal(Vector2 normal, Matrix4x4 matrix) | ||||
| 		{ | ||||
| 			return new Vector2( | ||||
| 				(normal.X * matrix.M11) + (normal.Y * matrix.M21), | ||||
| 				(normal.X * matrix.M12) + (normal.Y * matrix.M22) | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Creates a new <see cref="Vector2"/> that contains a transformation of the specified normal by the specified <see cref="Math.Matrix3x2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="normal">Source <see cref="Vector2"/> which represents a normal vector.</param> | ||||
| 		/// <param name="matrix">The transformation <see cref="Math.Matrix3x2"/>.</param> | ||||
| 		/// <returns>Transformed normal.</returns> | ||||
| 		public static Vector2 TransformNormal(Vector2 normal, Matrix3x2 matrix) | ||||
|         { | ||||
|             return new Vector2( | ||||
|                 normal.X * matrix.M11 + normal.Y * matrix.M21, | ||||
|                 normal.X * matrix.M12 + normal.Y * matrix.M22); | ||||
|         } | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Apply transformation on all normals within array of <see cref="Vector2"/> by the specified <see cref="Matrix4x4"/> and places the results in an another array. | ||||
| 		/// </summary> | ||||
| 		/// <param name="sourceArray">Source array.</param> | ||||
| 		/// <param name="matrix">The transformation <see cref="Matrix4x4"/>.</param> | ||||
| 		/// <param name="destinationArray">Destination array.</param> | ||||
| 		public static void TransformNormal( | ||||
| 			Vector2[] sourceArray, | ||||
| 			ref Matrix4x4 matrix, | ||||
| 			Vector2[] destinationArray | ||||
| 		) | ||||
| 		{ | ||||
| 			TransformNormal( | ||||
| 				sourceArray, | ||||
| 				0, | ||||
| 				ref matrix, | ||||
| 				destinationArray, | ||||
| 				0, | ||||
| 				sourceArray.Length | ||||
| 			); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Apply transformation on normals within array of <see cref="Vector2"/> by the specified <see cref="Matrix4x4"/> and places the results in an another array. | ||||
| 		/// </summary> | ||||
| 		/// <param name="sourceArray">Source array.</param> | ||||
| 		/// <param name="sourceIndex">The starting index of transformation in the source array.</param> | ||||
| 		/// <param name="matrix">The transformation <see cref="Matrix4x4"/>.</param> | ||||
| 		/// <param name="destinationArray">Destination array.</param> | ||||
| 		/// <param name="destinationIndex">The starting index in the destination array, where the first <see cref="Vector2"/> should be written.</param> | ||||
| 		/// <param name="length">The number of normals to be transformed.</param> | ||||
| 		public static void TransformNormal( | ||||
| 			Vector2[] sourceArray, | ||||
| 			int sourceIndex, | ||||
| 			ref Matrix4x4 matrix, | ||||
| 			Vector2[] destinationArray, | ||||
| 			int destinationIndex, | ||||
| 			int length | ||||
| 		) | ||||
| 		{ | ||||
| 			for (int i = 0; i < length; i += 1) | ||||
| 			{ | ||||
| 				Vector2 position = sourceArray[sourceIndex + i]; | ||||
| 				Vector2 result; | ||||
| 				result.X = (position.X * matrix.M11) + (position.Y * matrix.M21); | ||||
| 				result.Y = (position.X * matrix.M12) + (position.Y * matrix.M22); | ||||
| 				destinationArray[destinationIndex + i] = result; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Public Static Operators | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Inverts values in the specified <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">Source <see cref="Vector2"/> on the right of the sub sign.</param> | ||||
| 		/// <returns>Result of the inversion.</returns> | ||||
| 		public static Vector2 operator -(Vector2 value) | ||||
| 		{ | ||||
| 			value.X = -value.X; | ||||
| 			value.Y = -value.Y; | ||||
| 			return value; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Compares whether two <see cref="Vector2"/> instances are equal. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1"><see cref="Vector2"/> instance on the left of the equal sign.</param> | ||||
| 		/// <param name="value2"><see cref="Vector2"/> instance on the right of the equal sign.</param> | ||||
| 		/// <returns><c>true</c> if the instances are equal; <c>false</c> otherwise.</returns> | ||||
| 		public static bool operator ==(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			return (value1.X == value2.X && | ||||
| 					value1.Y == value2.Y); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Compares whether two <see cref="Vector2"/> instances are equal. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1"><see cref="Vector2"/> instance on the left of the equal sign.</param> | ||||
| 		/// <param name="value2"><see cref="Vector2"/> instance on the right of the equal sign.</param> | ||||
| 		/// <returns><c>true</c> if the instances are equal; <c>false</c> otherwise.</returns> | ||||
| 		public static bool operator !=(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			return !(value1 == value2); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Adds two vectors. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/> on the left of the add sign.</param> | ||||
| 		/// <param name="value2">Source <see cref="Vector2"/> on the right of the add sign.</param> | ||||
| 		/// <returns>Sum of the vectors.</returns> | ||||
| 		public static Vector2 operator +(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			value1.X += value2.X; | ||||
| 			value1.Y += value2.Y; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Subtracts a <see cref="Vector2"/> from a <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/> on the left of the sub sign.</param> | ||||
| 		/// <param name="value2">Source <see cref="Vector2"/> on the right of the sub sign.</param> | ||||
| 		/// <returns>Result of the vector subtraction.</returns> | ||||
| 		public static Vector2 operator -(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			value1.X -= value2.X; | ||||
| 			value1.Y -= value2.Y; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Multiplies the components of two vectors by each other. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/> on the left of the mul sign.</param> | ||||
| 		/// <param name="value2">Source <see cref="Vector2"/> on the right of the mul sign.</param> | ||||
| 		/// <returns>Result of the vector multiplication.</returns> | ||||
| 		public static Vector2 operator *(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			value1.X *= value2.X; | ||||
| 			value1.Y *= value2.Y; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Multiplies the components of vector by a scalar. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value">Source <see cref="Vector2"/> on the left of the mul sign.</param> | ||||
| 		/// <param name="scaleFactor">Scalar value on the right of the mul sign.</param> | ||||
| 		/// <returns>Result of the vector multiplication with a scalar.</returns> | ||||
| 		public static Vector2 operator *(Vector2 value, Fix64 scaleFactor) | ||||
| 		{ | ||||
| 			value.X *= scaleFactor; | ||||
| 			value.Y *= scaleFactor; | ||||
| 			return value; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Multiplies the components of vector by a scalar. | ||||
| 		/// </summary> | ||||
| 		/// <param name="scaleFactor">Scalar value on the left of the mul sign.</param> | ||||
| 		/// <param name="value">Source <see cref="Vector2"/> on the right of the mul sign.</param> | ||||
| 		/// <returns>Result of the vector multiplication with a scalar.</returns> | ||||
| 		public static Vector2 operator *(Fix64 scaleFactor, Vector2 value) | ||||
| 		{ | ||||
| 			value.X *= scaleFactor; | ||||
| 			value.Y *= scaleFactor; | ||||
| 			return value; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Divides the components of a <see cref="Vector2"/> by the components of another <see cref="Vector2"/>. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/> on the left of the div sign.</param> | ||||
| 		/// <param name="value2">Divisor <see cref="Vector2"/> on the right of the div sign.</param> | ||||
| 		/// <returns>The result of dividing the vectors.</returns> | ||||
| 		public static Vector2 operator /(Vector2 value1, Vector2 value2) | ||||
| 		{ | ||||
| 			value1.X /= value2.X; | ||||
| 			value1.Y /= value2.Y; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
| 		/// Divides the components of a <see cref="Vector2"/> by a scalar. | ||||
| 		/// </summary> | ||||
| 		/// <param name="value1">Source <see cref="Vector2"/> on the left of the div sign.</param> | ||||
| 		/// <param name="divider">Divisor scalar on the right of the div sign.</param> | ||||
| 		/// <returns>The result of dividing a vector by a scalar.</returns> | ||||
| 		public static Vector2 operator /(Vector2 value1, Fix64 divider) | ||||
| 		{ | ||||
| 			Fix64 factor = Fix64.One / divider; | ||||
| 			value1.X *= factor; | ||||
| 			value1.Y *= factor; | ||||
| 			return value1; | ||||
| 		} | ||||
| 
 | ||||
| 		#endregion | ||||
| 	} | ||||
| } | ||||
										
											
												File diff suppressed because it is too large
												Load Diff
											
										
									
								
							|  | @ -21,7 +21,7 @@ using System.Diagnostics; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	[Serializable] | ||||
| 	[DebuggerDisplay("{DebugDisplayString,nq}")] | ||||
|  | @ -20,7 +20,7 @@ using System.Diagnostics; | |||
| using System.Text; | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Defines a viewing frustum for intersection operations. | ||||
|  | @ -21,7 +21,7 @@ using System.Diagnostics; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Describes a sphere in 3D-space for bounding operations. | ||||
|  | @ -14,7 +14,7 @@ | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Defines how the bounding volumes intersects or contain one another. | ||||
|  | @ -10,7 +10,7 @@ | |||
| using System; | ||||
| using System.Globalization; | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// A structure encapsulating a 3x2 matrix. | ||||
|  | @ -21,7 +21,7 @@ using System.Runtime.InteropServices; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Represents the right-handed 4x4 floating point matrix, which can store translation, scale and rotation information. | ||||
|  | @ -20,7 +20,7 @@ using System.Diagnostics; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	[Serializable] | ||||
| 	[DebuggerDisplay("{DebugDisplayString,nq}")] | ||||
|  | @ -14,7 +14,7 @@ | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Defines the intersection between a <see cref="Plane"/> and a bounding volume. | ||||
|  | @ -20,7 +20,7 @@ using System.Diagnostics; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Describes a 2D-point. | ||||
|  | @ -20,7 +20,7 @@ using System.Diagnostics; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// An efficient mathematical representation for three dimensional rotations. | ||||
|  | @ -20,7 +20,7 @@ using System.Diagnostics; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	[Serializable] | ||||
| 	[DebuggerDisplay("{DebugDisplayString,nq}")] | ||||
|  | @ -20,7 +20,7 @@ using System.Diagnostics; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Describes a 2D-rectangle. | ||||
|  | @ -1,4 +1,4 @@ | |||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	public struct Transform2D : System.IEquatable<Transform2D> | ||||
| 	{ | ||||
|  | @ -6,7 +6,21 @@ namespace MoonWorks.Math | |||
| 		public float Rotation { get; } | ||||
| 		public Vector2 Scale { get; } | ||||
| 
 | ||||
| 		public Matrix3x2 TransformMatrix { get; } | ||||
| 		private bool transformMatrixCalculated = false; | ||||
| 		private Matrix3x2 transformMatrix = Matrix3x2.Identity; | ||||
| 		public Matrix3x2 TransformMatrix | ||||
| 		{ | ||||
| 			get | ||||
| 			{ | ||||
| 				if (!transformMatrixCalculated) | ||||
| 				{ | ||||
| 					transformMatrix = CreateTransformMatrix(Position, Rotation, Scale); | ||||
| 					transformMatrixCalculated = true; | ||||
| 				} | ||||
| 
 | ||||
| 				return transformMatrix; | ||||
| 			} | ||||
| 		} | ||||
| 
 | ||||
| 		public bool IsAxisAligned => Rotation % MathHelper.PiOver2 == 0; | ||||
| 		public bool IsUniformScale => Scale.X == Scale.Y; | ||||
|  | @ -16,7 +30,6 @@ namespace MoonWorks.Math | |||
| 			Position = Vector2.Zero; | ||||
| 			Rotation = 0; | ||||
| 			Scale = Vector2.One; | ||||
| 			TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale); | ||||
| 		} | ||||
| 
 | ||||
| 		public Transform2D(Vector2 position) | ||||
|  | @ -24,7 +37,6 @@ namespace MoonWorks.Math | |||
| 			Position = position; | ||||
| 			Rotation = 0; | ||||
| 			Scale = Vector2.One; | ||||
| 			TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale); | ||||
| 		} | ||||
| 
 | ||||
| 		public Transform2D(Vector2 position, float rotation) | ||||
|  | @ -32,7 +44,6 @@ namespace MoonWorks.Math | |||
| 			Position = position; | ||||
| 			Rotation = rotation; | ||||
| 			Scale = Vector2.One; | ||||
| 			TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale); | ||||
| 		} | ||||
| 
 | ||||
| 		public Transform2D(Vector2 position, float rotation, Vector2 scale) | ||||
|  | @ -40,7 +51,6 @@ namespace MoonWorks.Math | |||
| 			Position = position; | ||||
| 			Rotation = rotation; | ||||
| 			Scale = scale; | ||||
| 			TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale); | ||||
| 		} | ||||
| 
 | ||||
| 		public Transform2D Compose(Transform2D other) | ||||
|  | @ -21,7 +21,7 @@ using System.Runtime.InteropServices; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Describes a 2D-vector. | ||||
|  | @ -22,7 +22,7 @@ using System.Text; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Describes a 3D-vector. | ||||
|  | @ -21,7 +21,7 @@ using System.Runtime.InteropServices; | |||
| 
 | ||||
| #endregion | ||||
| 
 | ||||
| namespace MoonWorks.Math | ||||
| namespace MoonWorks.Math.Float | ||||
| { | ||||
| 	/// <summary> | ||||
| 	/// Describes a 4D-vector. | ||||
|  | @ -334,7 +334,7 @@ namespace MoonWorks.Math | |||
| 		/// <summary> | ||||
| 		/// Rescales a value within a given range to a new range. | ||||
| 		/// </summary> | ||||
| 		public static float Normalize(float value, short min, short max, short newMin, short newMax) | ||||
| 		public static float Normalize(short value, short min, short max, short newMin, short newMax) | ||||
| 		{ | ||||
| 			return ((value - min) * (newMax - newMin)) / (max - min) + newMin; | ||||
| 		} | ||||
|  | @ -373,6 +373,19 @@ namespace MoonWorks.Math | |||
|                 System.Math.Max(start - change, end); | ||||
| 		} | ||||
| 
 | ||||
| 		/// <summary> | ||||
|         /// Step from start towards end by change. | ||||
|         /// </summary> | ||||
|         /// <param name="start">Start value.</param> | ||||
|         /// <param name="end">End value.</param> | ||||
|         /// <param name="change">Change value.</param> | ||||
|         public static Fixed.Fix64 Approach(Fixed.Fix64 start, Fixed.Fix64 end, Fixed.Fix64 change) | ||||
|         { | ||||
|             return start < end ? | ||||
|                 Fixed.Fix64.Min(start + change, end) : | ||||
|                 Fixed.Fix64.Max(start - change, end); | ||||
|         } | ||||
| 
 | ||||
| 		#endregion | ||||
| 
 | ||||
| 		#region Internal Static Methods | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue