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