MoonsideGames
/
MoonWorks
1
0
Fork 1
Code Issues 4 Pull Requests Projects Releases 1 Activity

start implementing fixed point math structures

pull/19/head
cosmonaut 2022-05-03 18:05:10 -07:00
parent 5e2368bc7d
commit 778e69d145
33 changed files with 59064 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

181
src/Collision/Fixed/AABB2D.cs Normal file
View File

@ -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);
}
}
}

12
src/Collision/Fixed/ICollidable.cs Normal file
View File

@ -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);
}
}

15
src/Collision/Fixed/IShape2D.cs Normal file
View File

@ -0,0 +1,15 @@
using MoonWorks.Math.Fixed;
namespace MoonWorks.Collision.Fixed
{
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);
}
}

57
src/Collision/Fixed/MinkowskiDifference.cs Normal file
View File

@ -0,0 +1,57 @@
using MoonWorks.Math.Fixed;
namespace MoonWorks.Collision.Fixed
{
/// <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);
}
}
}

333
src/Collision/Fixed/NarrowPhase.cs Normal file
View File

@ -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.One / new Fix64(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);
}
}
}

73
src/Collision/Fixed/Shapes/Circle.cs Normal file
View File

@ -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);
}
}
}

83
src/Collision/Fixed/Shapes/Line.cs Normal file
View File

@ -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);
}
}
}

61
src/Collision/Fixed/Shapes/Point.cs Normal file
View File

@ -0,0 +1,61 @@
using System.Collections.Generic;
using MoonWorks.Math.Fixed;
namespace MoonWorks.Collision.Fixed
{
/// <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;
}
}
}

130
src/Collision/Fixed/Shapes/Rectangle.cs Normal file
View File

@ -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);
}
}
}

136
src/Collision/Fixed/Simplex2D.cs Normal file
View File

@ -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);
}
}
}

252
src/Collision/Fixed/SpatialHash2D.cs Normal file
View File

@ -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) Fix64.Floor(position.X / cellSize), (int) Fix64.Floor(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);
}
}
}

2
src/Collision/AABB2D.cs → src/Collision/Float/AABB2D.cs
View File

@ -1,7 +1,7 @@
using System.Collections.Generic;
using MoonWorks.Math;
namespace MoonWorks.Collision
namespace MoonWorks.Collision.Float
{
/// <summary>
/// Axis-aligned bounding box.

12
src/Collision/Float/ICollidable.cs Normal file
View File

@ -0,0 +1,12 @@
using System.Collections.Generic;
using MoonWorks.Math;
namespace MoonWorks.Collision.Float
{
public interface ICollidable
{
IEnumerable<IShape2D> Shapes { get; }
AABB2D AABB { get; }
AABB2D TransformedAABB(Transform2D transform);
}
}

2
src/Collision/IShape2D.cs → src/Collision/Float/IShape2D.cs
View File

@ -1,6 +1,6 @@
using MoonWorks.Math;
namespace MoonWorks.Collision
namespace MoonWorks.Collision.Float
{
public interface IShape2D : ICollidable, System.IEquatable<IShape2D>
{

2
src/Collision/MinkowskiDifference.cs → src/Collision/Float/MinkowskiDifference.cs
View File

@ -1,6 +1,6 @@
using MoonWorks.Math;
namespace MoonWorks.Collision
namespace MoonWorks.Collision.Float
{
/// <summary>
/// A Minkowski difference between two shapes.

2
src/Collision/NarrowPhase.cs → src/Collision/Float/NarrowPhase.cs
View File

@ -1,6 +1,6 @@
using MoonWorks.Math;
namespace MoonWorks.Collision
namespace MoonWorks.Collision.Float
{
public static class NarrowPhase
{

6
src/Collision/Shapes/Circle.cs → src/Collision/Float/Shapes/Circle.cs
View File

@ -1,14 +1,14 @@
using System.Collections.Generic;
using MoonWorks.Math;
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);

2
src/Collision/Shapes/Line.cs → src/Collision/Float/Shapes/Line.cs
View File

@ -1,7 +1,7 @@
using System.Collections.Generic;
using MoonWorks.Math;
namespace MoonWorks.Collision
namespace MoonWorks.Collision.Float
{
/// <summary>
/// A line is a shape defined by exactly two points in space.

2
src/Collision/Shapes/Point.cs → src/Collision/Float/Shapes/Point.cs
View File

@ -1,7 +1,7 @@
using System.Collections.Generic;
using MoonWorks.Math;
namespace MoonWorks.Collision
namespace MoonWorks.Collision.Float
{
/// <summary>
/// A Point is "that which has no part".

16
src/Collision/Shapes/Rectangle.cs → src/Collision/Float/Shapes/Rectangle.cs
View File

@ -1,7 +1,7 @@
using System.Collections.Generic;
using MoonWorks.Math;
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;

2
src/Collision/Simplex2D.cs → src/Collision/Float/Simplex2D.cs
View File

@ -1,7 +1,7 @@
using System.Collections.Generic;
using MoonWorks.Math;
namespace MoonWorks.Collision
namespace MoonWorks.Collision.Float
{
/// <summary>
/// A simplex is a shape with up to n - 2 vertices in the nth dimension.

2
src/Collision/SpatialHash2D.cs → src/Collision/Float/SpatialHash2D.cs
View File

@ -1,7 +1,7 @@
using System.Collections.Generic;
using MoonWorks.Math;
namespace MoonWorks.Collision
namespace MoonWorks.Collision.Float
{
/// <summary>
/// Used to quickly check if two shapes are potentially overlapping.

12
src/Collision/ICollidable.cs
View File

@ -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);
}
}

746
src/Math/Fixed/Fix64.cs Normal file
View File

@ -0,0 +1,746 @@
// 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;
private Fix64(long value)
{
RawValue = value;
}
public Fix64(int value)
{
RawValue = value * ONE;
}
/// <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);
}
public static Fix64 Floor(Fix64 value)
{
// Zero out the fractional part.
return new Fix64((long)((ulong)value.RawValue & 0xFFFFFFFF00000000));
}
public static Fix64 Ceiling(Fix64 value)
{
var hasFractionalPart = (value.RawValue & 0x00000000FFFFFFFF) != 0;
return hasFractionalPart ? Floor(value) + One : value;
}
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;
}
public static Fix64 Min(Fix64 x, Fix64 y)
{
return (x < y) ? x : y;
}
public static Fix64 Max(Fix64 x, Fix64 y)
{
return (x > y) ? x : y;
}
public static Fix64 Clamp(Fix64 value, Fix64 min, Fix64 max)
{
return Fix64.Min(Fix64.Max(value, min), max);
}
// Trigonometry functions
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;
}
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);
}
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));
}
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);
}
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;
}
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 ((int) x) > y;
}
public static bool operator <(Fix64 x, int y)
{
return ((int) x) < 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 ((int) x) >= y;
}
public static bool operator <=(Fix64 x, int y)
{
return ((int) x) <= 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);
}
}
}

25745
src/Math/Fixed/Fix64SinLut.cs Normal file
View File

File diff suppressed because it is too large Load Diff

25745
src/Math/Fixed/Fix64TanLut.cs Normal file
View File

File diff suppressed because it is too large Load Diff

836
src/Math/Fixed/FixMatrix3x2.cs Normal file
View File

@ -0,0 +1,836 @@
/* 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
);
/// <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 = radians % Fix64.PiTimes2;
Fix64 c, s;
Fix64 epsilon = (Fix64) 0.001f * (Fix64.Pi / new Fix64(180));
if (radians > -epsilon && radians < epsilon)
{
// Exact case for zero rotation.
c = Fix64.One;
s = Fix64.Zero;
}
else if (radians > Fix64.PiOver2 - epsilon && radians < Fix64.PiOver2 + epsilon)
{
// Exact case for 90 degree rotation.
c = Fix64.Zero;
s = Fix64.One;
}
else if (radians < -Fix64.Pi + epsilon || radians > Fix64.Pi - epsilon)
{
// Exact case for 180 degree rotation.
c = -Fix64.One;
s = Fix64.Zero;
}
else if (radians > -Fix64.PiOver2 - epsilon && radians < -Fix64.PiOver2 + epsilon)
{
// 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);
}
// [ 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 = radians % Fix64.PiTimes2;
Fix64 c, s;
Fix64 epsilon = (Fix64) 0.001f * (Fix64.Pi / new Fix64(180));
if (radians > -epsilon && radians < epsilon)
{
// Exact case for zero rotation.
c = Fix64.One;
s = Fix64.Zero;
}
else if (radians > Fix64.PiOver2 - epsilon && radians < Fix64.PiOver2 + epsilon)
{
// Exact case for 90 degree rotation.
c = Fix64.Zero;
s = Fix64.One;
}
else if (radians < -Fix64.Pi + epsilon || radians > Fix64.Pi - epsilon)
{
// Exact case for 180 degree rotation.
c = -Fix64.One;
s = Fix64.Zero;
}
else if (radians > -Fix64.PiOver2 - epsilon && radians < -Fix64.PiOver2 + epsilon)
{
// 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>
/// 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();
}
}
}

1692
src/Math/Fixed/FixMatrix4x4.cs Normal file
View File

File diff suppressed because it is too large Load Diff

889
src/Math/Fixed/Quaternion.cs Normal file
View File

@ -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
}
}

95
src/Math/Fixed/Transform2D.cs Normal file
View File

@ -0,0 +1,95 @@
namespace MoonWorks.Math.Fixed
{
public struct Transform2D : System.IEquatable<Transform2D>
{
public Vector2 Position { get; }
public Fix64 Rotation { get; }
public Vector2 Scale { get; }
public Matrix3x2 TransformMatrix { get; }
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;
TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale);
}
public Transform2D(Vector2 position)
{
Position = position;
Rotation = Fix64.Zero;
Scale = Vector2.One;
TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale);
}
public Transform2D(Vector2 position, Fix64 rotation)
{
Position = position;
Rotation = rotation;
Scale = Vector2.One;
TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale);
}
public Transform2D(Vector2 position, Fix64 rotation, Vector2 scale)
{
Position = position;
Rotation = rotation;
Scale = scale;
TransformMatrix = CreateTransformMatrix(Position, Rotation, 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);
}
}
}

802
src/Math/Fixed/Vector2.cs Normal file
View File

@ -0,0 +1,802 @@
#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 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
}
}

1136
src/Math/Fixed/Vector3.cs Normal file
View File

File diff suppressed because it is too large Load Diff

15
src/Math/MathHelper.cs
View File

@ -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