/// <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 <T>(Matrix3X2 <T> matrix, out Matrix3X2 <T> result)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
T det = Scalar.Subtract(Scalar.Multiply(matrix.M11, matrix.M22), Scalar.Multiply(matrix.M21, matrix.M12));
if (!Scalar.GreaterThanOrEqual(Scalar.Abs(det), Scalar <T> .Epsilon))
{
result = new(Scalar <T> .NaN, Scalar <T> .NaN, Scalar <T> .NaN, Scalar <T> .NaN, Scalar <T> .NaN, Scalar <T> .NaN);
return(false);
}
T invDet = Scalar.Reciprocal(det);
result = default;
result.M11 = Scalar.Multiply(matrix.M22, invDet);
result.M12 = Scalar.Negate(Scalar.Multiply(matrix.M12, invDet));
result.M21 = Scalar.Negate(Scalar.Multiply(matrix.M21, invDet));
result.M22 = Scalar.Multiply(matrix.M11, invDet);
result.M31 = Scalar.Multiply(Scalar.Subtract(Scalar.Multiply(matrix.M21, matrix.M32), Scalar.Multiply(matrix.M31, matrix.M22)), invDet);
result.M32 = Scalar.Multiply(Scalar.Subtract(Scalar.Multiply(matrix.M31, matrix.M12), Scalar.Multiply(matrix.M11, matrix.M32)), invDet);
return(true);
}
/// <summary>
/// Calculates whether this circle contains another circle
/// </summary>
/// <param name="other">The circle.</param>
/// <returns>True if this circle contains the given circle; False otherwise.</returns>
/// <remarks>This does consider a circle that touches the edge contained.</remarks>
public bool Contains(Circle <T> other)
{
var distanceSquared = Vector2D.DistanceSquared(Center, other.Center);
var radiusDiff = Scalar.Subtract(Radius, other.Radius);
return(Scalar.LessThanOrEqual(distanceSquared, Scalar.Multiply(radiusDiff, radiusDiff)));
}
/// <summary>Creates a rotation matrix from the given Quaternion rotation value.</summary>
/// <param name="quaternion">The source Quaternion.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix2X3 <T> CreateFromQuaternion <T>(Quaternion <T> quaternion)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
Matrix2X3 <T> result = Matrix2X3 <T> .Identity;
T xx = Scalar.Multiply(quaternion.X, quaternion.X);
T yy = Scalar.Multiply(quaternion.Y, quaternion.Y);
T zz = Scalar.Multiply(quaternion.Z, quaternion.Z);
T xy = Scalar.Multiply(quaternion.X, quaternion.Y);
T wz = Scalar.Multiply(quaternion.Z, quaternion.W);
T xz = Scalar.Multiply(quaternion.Z, quaternion.X);
T wy = Scalar.Multiply(quaternion.Y, quaternion.W);
T yz = Scalar.Multiply(quaternion.Y, quaternion.Z);
T wx = Scalar.Multiply(quaternion.X, quaternion.W);
result.M11 = Scalar.Subtract(Scalar <T> .One, Scalar.Multiply(Scalar <T> .Two, Scalar.Add(yy, zz)));
result.M12 = Scalar.Multiply(Scalar <T> .Two, Scalar.Add(xy, wz));
result.M13 = Scalar.Multiply(Scalar <T> .Two, Scalar.Subtract(xz, wy));
result.M21 = Scalar.Multiply(Scalar <T> .Two, Scalar.Subtract(xy, wz));
result.M22 = Scalar.Subtract(Scalar <T> .One, Scalar.Multiply(Scalar <T> .Two, Scalar.Add(zz, xx)));
result.M23 = Scalar.Multiply(Scalar <T> .Two, Scalar.Add(yz, wx));
return(result);
}
public static T Dot <T>(Plane <T> plane, Vector4D <T> value)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
=> Scalar.Add(
Scalar.Add(
Scalar.Add(Scalar.Multiply(plane.Normal.X, value.X),
Scalar.Multiply(plane.Normal.Y, value.Y)), Scalar.Multiply(plane.Normal.Z, value.Z)),
Scalar.Multiply(plane.Distance, value.W));
/// <summary>
/// Calculates the distance to the nearest edge from the point.
/// </summary>
/// <param name="point">The point.</param>
/// <returns>The distance.</returns>
public T GetDistanceToNearestEdge(Vector2D <T> point)
{
var dx = Scalar.Max(Scalar.Max(Scalar.Subtract(Min.X, point.X), Scalar <T> .Zero), Scalar.Subtract(point.X, Max.X));
var dy = Scalar.Max(Scalar.Max(Scalar.Subtract(Min.Y, point.Y), Scalar <T> .Zero), Scalar.Subtract(point.Y, Max.Y));
return(Scalar.Sqrt(Scalar.Add(Scalar.Multiply(dx, dx), Scalar.Multiply(dy, dy))));
}
/// <summary>
/// Calculates the distance to the nearest edge from the point.
/// </summary>
/// <param name="point">The point.</param>
/// <returns>The distance.</returns>
public T GetDistanceToNearestEdge(Vector3D <T> point)
{
var max = Max;
var dx = Scalar.Max(Scalar.Max(Scalar.Subtract(Origin.X, point.X), Scalar <T> .Zero), Scalar.Subtract(point.X, max.X));
var dy = Scalar.Max(Scalar.Max(Scalar.Subtract(Origin.Y, point.Y), Scalar <T> .Zero), Scalar.Subtract(point.Y, max.Y));
var dz = Scalar.Max(Scalar.Max(Scalar.Subtract(Origin.Z, point.Z), Scalar <T> .Zero), Scalar.Subtract(point.Z, max.Z));
return(Scalar.Sqrt(Scalar.Add(Scalar.Add(Scalar.Multiply(dx, dx), Scalar.Multiply(dy, dy)), Scalar.Multiply(dz, dz))));
}
public static Plane <T> Transform <T>(Plane <T> plane, Matrix4X4 <T> matrix)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
Matrix4X4.Invert(matrix, out Matrix4X4 <T> m);
T x = plane.Normal.X, y = plane.Normal.Y, z = plane.Normal.Z, w = plane.Distance;
return(new(
Scalar.Add(Scalar.Add(Scalar.Add(Scalar.Multiply(x, m.M11), Scalar.Multiply(y, m.M12)), Scalar.Multiply(z, m.M13)), Scalar.Multiply(w, m.M14)),
Scalar.Add(Scalar.Add(Scalar.Add(Scalar.Multiply(x, m.M21), Scalar.Multiply(y, m.M22)), Scalar.Multiply(z, m.M23)), Scalar.Multiply(w, m.M24)),
Scalar.Add(Scalar.Add(Scalar.Add(Scalar.Multiply(x, m.M31), Scalar.Multiply(y, m.M32)), Scalar.Multiply(z, m.M33)), Scalar.Multiply(w, m.M34)),
Scalar.Add(Scalar.Add(Scalar.Add(Scalar.Multiply(x, m.M41), Scalar.Multiply(y, m.M42)), Scalar.Multiply(z, m.M43)), Scalar.Multiply(w, m.M44))));
}
/// <summary>Linearly interpolates between the corresponding values of two matrices.</summary>
/// <param name="matrix1">The first source matrix.</param>
/// <param name="matrix2">The second source matrix.</param>
/// <param name="amount">The relative weight of the second source matrix.</param>
/// <returns>The interpolated matrix.</returns>
public static unsafe Matrix2X2<T> Lerp<T>(Matrix2X2<T> matrix1, Matrix2X2<T> matrix2, T amount) where T : unmanaged, IFormattable, IEquatable<T>, IComparable<T>
{
Matrix2X2<T> result = default;
// First row
result.M11 = Scalar.Add(matrix1.M11, Scalar.Multiply(Scalar.Subtract(matrix2.M11, matrix1.M11), amount));
result.M12 = Scalar.Add(matrix1.M12, Scalar.Multiply(Scalar.Subtract(matrix2.M12, matrix1.M12), amount));
// Second row
result.M21 = Scalar.Add(matrix1.M21, Scalar.Multiply(Scalar.Subtract(matrix2.M21, matrix1.M21), amount));
result.M22 = Scalar.Add(matrix1.M22, Scalar.Multiply(Scalar.Subtract(matrix2.M22, matrix1.M22), amount));
return result;
}
/// <summary>Creates a matrix that rotates around an arbitrary vector.</summary>
/// <param name="axis">The axis to rotate around.</param>
/// <param name="angle">The angle to rotate around the given axis, in radians.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix3X3 <T> CreateFromAxisAngle <T>(Vector3D <T> axis, T angle)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
// a: angle
// x, y, z: unit vector for axis.
//
// Rotation matrix M can compute by using below equation.
//
// T T
// M = uu + (cos a)( I-uu ) + (sin a)S
//
// Where:
//
// u = ( x, y, z )
//
// [ 0 -z y ]
// S = [ z 0 -x ]
// [ -y x 0 ]
//
// [ 1 0 0 ]
// I = [ 0 1 0 ]
// [ 0 0 1 ]
//
//
// [ xx+cosa*(1-xx) yx-cosa*yx-sina*z zx-cosa*xz+sina*y ]
// M = [ xy-cosa*yx+sina*z yy+cosa(1-yy) yz-cosa*yz-sina*x ]
// [ zx-cosa*zx-sina*y zy-cosa*zy+sina*x zz+cosa*(1-zz) ]
//
T x = axis.X, y = axis.Y, z = axis.Z;
T sa = Scalar.Sin(angle), ca = Scalar.Cos(angle);
T xx = Scalar.Multiply(x, x), yy = Scalar.Multiply(y, y), zz = Scalar.Multiply(z, z);
T xy = Scalar.Multiply(x, y), xz = Scalar.Multiply(x, z), yz = Scalar.Multiply(y, z);
Matrix3X3 <T> result = Matrix3X3 <T> .Identity;
result.M11 = Scalar.Add(xx, Scalar.Multiply(ca, Scalar.Subtract(Scalar <T> .One, xx)));
result.M12 = Scalar.Add(Scalar.Subtract(xy, Scalar.Multiply(ca, xy)), Scalar.Multiply(sa, z));
result.M13 = Scalar.Subtract(Scalar.Subtract(xz, Scalar.Multiply(ca, xz)), Scalar.Multiply(sa, y));
result.M21 = Scalar.Subtract(Scalar.Subtract(xy, Scalar.Multiply(ca, xy)), Scalar.Multiply(sa, z));
result.M22 = Scalar.Add(yy, Scalar.Multiply(ca, Scalar.Subtract(Scalar <T> .One, yy)));
result.M23 = Scalar.Add(Scalar.Subtract(yz, Scalar.Multiply(ca, yz)), Scalar.Multiply(sa, x));
result.M31 = Scalar.Add(Scalar.Subtract(xz, Scalar.Multiply(ca, xz)), Scalar.Multiply(sa, y));
result.M32 = Scalar.Subtract(Scalar.Subtract(yz, Scalar.Multiply(ca, yz)), Scalar.Multiply(sa, x));
result.M33 = Scalar.Add(zz, Scalar.Multiply(ca, Scalar.Subtract(Scalar <T> .One, zz)));
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 <T> CreateScale <T>(T scale, Vector2D <T> centerPoint)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
Matrix3X2 <T> result = Matrix3X2 <T> .Identity;
T tx = Scalar.Multiply(centerPoint.X, Scalar.Subtract(Scalar <T> .One, scale));
T ty = Scalar.Multiply(centerPoint.Y, Scalar.Subtract(Scalar <T> .One, scale));
result.M11 = scale;
result.M22 = scale;
result.M31 = tx;
result.M32 = ty;
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 <T> CreateSkew <T>(T radiansX, T radiansY, Vector2D <T> centerPoint)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
Matrix3X2 <T> result = Matrix3X2 <T> .Identity;
T xTan = Scalar.Tan(radiansX);
T yTan = Scalar.Tan(radiansY);
T tx = Scalar.Negate(Scalar.Multiply(centerPoint.Y, xTan));
T ty = Scalar.Negate(Scalar.Multiply(centerPoint.X, yTan));
result.M12 = yTan;
result.M21 = xTan;
result.M31 = tx;
result.M32 = ty;
return(result);
}
public static Plane <T> Transform <T>(Plane <T> plane, Quaternion <T> rotation)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
// Compute rotation matrix.
T x2 = Scalar.Add(rotation.X, rotation.X);
T y2 = Scalar.Add(rotation.Y, rotation.Y);
T z2 = Scalar.Add(rotation.Z, rotation.Z);
T wx2 = Scalar.Multiply(rotation.W, x2);
T wy2 = Scalar.Multiply(rotation.W, y2);
T wz2 = Scalar.Multiply(rotation.W, z2);
T xx2 = Scalar.Multiply(rotation.X, x2);
T xy2 = Scalar.Multiply(rotation.X, y2);
T xz2 = Scalar.Multiply(rotation.X, z2);
T yy2 = Scalar.Multiply(rotation.Y, y2);
T yz2 = Scalar.Multiply(rotation.Y, z2);
T zz2 = Scalar.Multiply(rotation.Z, z2);
T m11 = Scalar.Subtract(Scalar.Subtract(Scalar <T> .One, yy2), zz2);
T m21 = Scalar.Subtract(xy2, wz2);
T m31 = Scalar.Add(xz2, wy2);
T m12 = Scalar.Add(xy2, wz2);
T m22 = Scalar.Subtract(Scalar.Subtract(Scalar <T> .One, xx2), zz2);
T m32 = Scalar.Subtract(yz2, wx2);
T m13 = Scalar.Subtract(xz2, wy2);
T m23 = Scalar.Add(yz2, wx2);
T m33 = Scalar.Subtract(Scalar.Subtract(Scalar <T> .One, xx2), yy2);
T x = plane.Normal.X, y = plane.Normal.Y, z = plane.Normal.Z;
return(new(
Scalar.Add(Scalar.Add(Scalar.Multiply(x, m11), Scalar.Multiply(y, m21)), Scalar.Multiply(z, m31)),
Scalar.Add(Scalar.Add(Scalar.Multiply(x, m12), Scalar.Multiply(y, m22)), Scalar.Multiply(z, m32)),
Scalar.Add(Scalar.Add(Scalar.Multiply(x, m13), Scalar.Multiply(y, m23)), Scalar.Multiply(z, m33)),
plane.Distance));
}
public static Plane <T> Normalize <T>(Plane <T> value)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
/*if (Vector.IsHardwareAccelerated)
* {
* T normalLengthSquared = value.Normal.LengthSquared();
* if (MathF.Abs(normalLengthSquared - 1.0f) < NormalizeEpsilon)
* {
* // It already normalized, so we don't need to farther process.
* return value;
* }
* T normalLength = MathF.Sqrt(normalLengthSquared);
* return new Plane(
* value.Normal / normalLength,
* value.D / normalLength);
* }
* else*/
{
T f = Scalar.Add(
Scalar.Add(Scalar.Multiply(value.Normal.X, value.Normal.X),
Scalar.Multiply(value.Normal.Y, value.Normal.Y)),
Scalar.Multiply(value.Normal.Z, value.Normal.Z));
if (!Scalar.GreaterThanOrEqual(Scalar.Abs(Scalar.Subtract(f, Scalar <T> .One)), Scalar.As <float, T>(NormalizeEpsilon)))
{
return(value); // It already normalized, so we don't need to further process.
}
T fInv = Scalar.Reciprocal(Scalar.Sqrt(f));
return(new(
Scalar.Multiply(value.Normal.X, fInv),
Scalar.Multiply(value.Normal.Y, fInv),
Scalar.Multiply(value.Normal.Z, fInv),
Scalar.Multiply(value.Distance, fInv)));
}
}
/// <summary>Transforms the given matrix by applying the given Quaternion rotation.</summary>
/// <param name="value">The source matrix to transform.</param>
/// <param name="rotation">The rotation to apply.</param>
/// <returns>The transformed matrix.</returns>
public static Matrix2X3 <T> Transform <T>(Matrix2X3 <T> value, Quaternion <T> rotation)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
// Compute rotation matrix.
T x2 = Scalar.Add(rotation.X, rotation.X);
T y2 = Scalar.Add(rotation.Y, rotation.Y);
T z2 = Scalar.Add(rotation.Z, rotation.Z);
T wx2 = Scalar.Multiply(rotation.W, x2);
T wy2 = Scalar.Multiply(rotation.W, y2);
T wz2 = Scalar.Multiply(rotation.W, z2);
T xx2 = Scalar.Multiply(rotation.X, x2);
T xy2 = Scalar.Multiply(rotation.X, y2);
T xz2 = Scalar.Multiply(rotation.X, z2);
T yy2 = Scalar.Multiply(rotation.Y, y2);
T yz2 = Scalar.Multiply(rotation.Y, z2);
T zz2 = Scalar.Multiply(rotation.Z, z2);
T q11 = Scalar.Subtract(Scalar.Subtract(Scalar <T> .One, yy2), zz2);
T q21 = Scalar.Subtract(xy2, wz2);
T q31 = Scalar.Add(xz2, wy2);
T q12 = Scalar.Add(xy2, wz2);
T q22 = Scalar.Subtract(Scalar.Subtract(Scalar <T> .One, xx2), zz2);
T q32 = Scalar.Subtract(yz2, wx2);
T q13 = Scalar.Subtract(xz2, wy2);
T q23 = Scalar.Add(yz2, wx2);
T q33 = Scalar.Subtract(Scalar.Subtract(Scalar <T> .One, xx2), yy2);
var q1 = new Vector3D <T>(q11, q12, q13);
var q2 = new Vector3D <T>(q21, q22, q23);
var q3 = new Vector3D <T>(q31, q32, q33);
return(new(value.M11 * q1 + value.M12 * q2 + value.M13 * q3, value.M21 *q1 + value.M22 * q2 + value.M23 * q3));
}
#pragma warning disable 8618 // unitialized fields
static Scalar()
#pragma warning restore 8618
{
// This won't inline as nicely on platforms that aren't .NET 5, however there's no other way to yield the
// constant folding benefits that come with the fields being static readonly.
//
// We have used local functions elsewhere to get around this elsewhere, however there's no sane way we can
// do that with local functions.
//
// This will inline fine on .NET 5, though. See also: https://github.com/dotnet/runtime/issues/38106
if (typeof(T) == typeof(Half))
{
Epsilon = (T)(object)Half.Epsilon;
MaxValue = (T)(object)Half.MaxValue;
MinValue = (T)(object)Half.MinValue;
NaN = (T)(object)Half.NaN;
NegativeInfinity = (T)(object)Half.NegativeInfinity;
PositiveInfinity = (T)(object)Half.PositiveInfinity;
One = (T)(object)(Half)1;
Two = (T)(object)(Half)2;
MinusOne = (T)(object)(Half)(-1f);
MinusTwo = (T)(object)(Half)(-2f);
E = (T)(object)(Half)FloatE;
Pi = (T)(object)(Half)FloatPi;
Tau = (T)(object)(Half)FloatTau;
}
else if (typeof(T) == typeof(float))
{
Epsilon = (T)(object)float.Epsilon;
MaxValue = (T)(object)float.MaxValue;
MinValue = (T)(object)float.MinValue;
NaN = (T)(object)float.NaN;
NegativeInfinity = (T)(object)float.NegativeInfinity;
PositiveInfinity = (T)(object)float.PositiveInfinity;
One = (T)(object)1f;
Two = (T)(object)2f;
MinusOne = (T)(object)-1f;
MinusTwo = (T)(object)-2f;
E = (T)(object)FloatE;
Pi = (T)(object)FloatPi;
Tau = (T)(object)FloatTau;
}
else if (typeof(T) == typeof(double))
{
Epsilon = (T)(object)double.Epsilon;
MaxValue = (T)(object)double.MaxValue;
MinValue = (T)(object)double.MinValue;
NaN = (T)(object)double.NaN;
NegativeInfinity = (T)(object)double.NegativeInfinity;
PositiveInfinity = (T)(object)double.PositiveInfinity;
One = (T)(object)1d;
Two = (T)(object)2d;
MinusOne = (T)(object)-1d;
MinusTwo = (T)(object)-2d;
E = (T)(object)Math.E;
Pi = (T)(object)Math.PI;
#if !NET5_0
Tau = Scalar.Multiply(Pi, Two);
#else
Tau = (T)(object)Math.Tau;
#endif
}
else if (typeof(T) == typeof(decimal))
{
Epsilon = default !;
public static Plane <T> CreateFromVertices <T>(Vector3D <T> point1, Vector3D <T> point2, Vector3D <T> point3)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
var a = point1;
var b = point2;
var c = point3;
var ab = b - a;
var ac = c - a;
var cross = Vector3D.Cross(ab, ac);
Plane <T> p;
p.Normal = cross;
p.Distance = Scalar.Negate(Scalar.Add(
Scalar.Add(Scalar.Multiply(p.Normal.X, a.X), Scalar.Multiply(p.Normal.Y, a.Y)),
Scalar.Multiply(p.Normal.Z, a.Z)));
return(p);
/*if (Vector.IsHardwareAccelerated)
* {
* Vector3D<T> a = point2 - point1;
* Vector3D<T> b = point3 - point1;
*
* // N = Cross(a, b)
* Vector3D<T> n = Vector3D.Cross(a, b);
* Vector3D<T> normal = Vector3D.Normalize(n);
*
* // D = - Dot(N, point1)
* T d = Scalar.Negate(Vector3D.Dot(normal, point1));
*
* return new Plane<T>(normal, d);
* }
* else
* {
* T ax = Scalar.Subtract(point2.X, point1.X);
* T ay = Scalar.Subtract(point2.Y, point1.Y);
* T az = Scalar.Subtract(point2.Z, point1.Z);
*
* T bx = Scalar.Subtract(point3.X, point1.X);
* T by = Scalar.Subtract(point3.Y, point1.Y);
* T bz = Scalar.Subtract(point3.Z, point1.Z);
*
* // N=Cross(a,b)
* T nx = Scalar.Subtract(Scalar.Multiply(ay, bz), Scalar.Multiply(az, by));
* T ny = Scalar.Subtract(Scalar.Multiply(az, bx), Scalar.Multiply(ax, bz));
* T nz = Scalar.Subtract(Scalar.Multiply(ax, by), Scalar.Multiply(ay, bx));
*
* // Normalize(N)
* T ls = Scalar.Add(Scalar.Add(Scalar.Multiply(nx, nx), Scalar.Multiply(ny, ny)), Scalar.Multiply(nz, nz));
* T invNorm = Scalar.Inverse(Scalar.Sqrt(ls));
*
* Vector3D<T> normal = new Vector3D<T>(
* Scalar.Multiply(nx, invNorm),
* Scalar.Multiply(ny, invNorm),
* Scalar.Multiply(nz, invNorm));
*
* return new(normal,
* Scalar.Negate(Scalar.Add(
* Scalar.Add(Scalar.Multiply(normal.X, point1.X),
* Scalar.Multiply(normal.Y, point1.Y)), Scalar.Multiply(normal.Z, point1.Z))));
* }*/
}
/// <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 <T> CreateRotation <T>(T radians, Vector2D <T> centerPoint)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
radians = Scalar.IEEERemainder(radians, Scalar <T> .Tau);
T c, s;
if (Scalar.GreaterThan(radians, Scalar.As <float, T>(-RotationEpsilon)) && !Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(RotationEpsilon)))
{
// Exact case for zero rotation.
c = Scalar <T> .One;
s = Scalar <T> .Zero;
}
else if (Scalar.GreaterThan(radians, Scalar.As <float, T>(
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 - RotationEpsilon)) && !Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 + RotationEpsilon)))
{
// Exact case for 90 degree rotation.
c = Scalar <T> .Zero;
s = Scalar <T> .One;
}
else if (!Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(-
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
+ RotationEpsilon)) || Scalar.GreaterThan(radians, Scalar.As <float, T>(
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
- RotationEpsilon)))
{
// Exact case for 180 degree rotation.
c = Scalar <T> .MinusOne;
s = Scalar <T> .Zero;
}
else if (Scalar.GreaterThan(radians, Scalar.As <float, T>(-
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 - RotationEpsilon)) && !Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(-
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 + RotationEpsilon)))
{
// Exact case for 270 degree rotation.
c = Scalar <T> .Zero;
s = Scalar <T> .MinusOne;
}
else
{
// Arbitrary rotation.
c = Scalar.Cos(radians);
s = Scalar.Sin(radians);
}
T x = Scalar.Add(Scalar.Multiply(centerPoint.X, Scalar.Subtract(Scalar <T> .One, c)), Scalar.Multiply(centerPoint.Y, s));
T y = Scalar.Subtract(Scalar.Multiply(centerPoint.Y, Scalar.Subtract(Scalar <T> .One, c)), Scalar.Multiply(centerPoint.X, s));
// [ c s ]
// [ -s c ]
// [ x y ]
return(new(
new(c, s),
new(Scalar.Negate(s), c),
new(x, y)));
}
static Scalar()
{
// This won't inline as nicely on platforms that aren't .NET 5, however there's no other way to yield the
// constant folding benefits that come with the fields being static readonly.
//
// We have used local functions elsewhere to get around this elsewhere, however there's no sane way we can
// do that with local functions.
//
// This will inline fine on .NET 5, though. See also: https://github.com/dotnet/runtime/issues/38106
if (typeof(T) == typeof(Half))
{
Epsilon = (T)(object)Half.Epsilon;
MaxValue = (T)(object)Half.MaxValue;
MinValue = (T)(object)Half.MinValue;
NaN = (T)(object)Half.NaN;
NegativeInfinity = (T)(object)Half.NegativeInfinity;
PositiveInfinity = (T)(object)Half.PositiveInfinity;
One = (T)(object)(Half)1;
Two = (T)(object)(Half)2;
MinusOne = (T)(object)(Half)(-1f);
MinusTwo = (T)(object)(Half)(-2f);
E = (T)(object)(Half)FloatE;
Pi = (T)(object)(Half)FloatPi;
Tau = (T)(object)(Half)FloatTau;
}
else if (typeof(T) == typeof(float))
{
Epsilon = (T)(object)float.Epsilon;
MaxValue = (T)(object)float.MaxValue;
MinValue = (T)(object)float.MinValue;
NaN = (T)(object)float.NaN;
NegativeInfinity = (T)(object)float.NegativeInfinity;
PositiveInfinity = (T)(object)float.PositiveInfinity;
One = (T)(object)1f;
Two = (T)(object)2f;
MinusOne = (T)(object)-1f;
MinusTwo = (T)(object)-2f;
E = (T)(object)FloatE;
Pi = (T)(object)FloatPi;
Tau = (T)(object)FloatTau;
}
else if (typeof(T) == typeof(double))
{
Epsilon = (T)(object)double.Epsilon;
MaxValue = (T)(object)double.MaxValue;
MinValue = (T)(object)double.MinValue;
NaN = (T)(object)double.NaN;
NegativeInfinity = (T)(object)double.NegativeInfinity;
PositiveInfinity = (T)(object)double.PositiveInfinity;
One = (T)(object)1d;
Two = (T)(object)2d;
MinusOne = (T)(object)-1d;
MinusTwo = (T)(object)-2d;
E = (T)(object)Math.E;
Pi = (T)(object)Math.PI;
#if !NET5_0
Tau = Scalar.Multiply(Pi, Two);
#else
Tau = (T)(object)Math.Tau;
#endif
}
else if (typeof(T) == typeof(decimal))
{
Epsilon = default;
MaxValue = (T)(object)decimal.MaxValue;
MinValue = (T)(object)decimal.MinValue;
NaN = default;
NegativeInfinity = default;
PositiveInfinity = default;
One = (T)(object)(decimal)1;
Two = (T)(object)(decimal)2;
MinusOne = (T)(object)(decimal) - 1;
MinusTwo = (T)(object)(decimal) - 2;
E = (T)(object)(decimal)Math.E;
Pi = (T)(object)(decimal)Math.PI;
Tau = Scalar.Multiply(Pi, Two);
}
else if (typeof(T) == typeof(short))
{
Epsilon = default;
MaxValue = (T)(object)short.MaxValue;
MinValue = (T)(object)short.MinValue;
NaN = default;
NegativeInfinity = default;
PositiveInfinity = default;
One = (T)(object)(short)1;
Two = (T)(object)(short)2;
MinusOne = (T)(object)(short)-1;
MinusTwo = (T)(object)(short)-2;
E = (T)(object)(short)FloatE;
Pi = (T)(object)(short)FloatPi;
Tau = (T)(object)(short)FloatTau;
}
else if (typeof(T) == typeof(ushort))
{
Epsilon = default;
MaxValue = (T)(object)ushort.MaxValue;
MinValue = (T)(object)ushort.MinValue;
NaN = default;
NegativeInfinity = default;
PositiveInfinity = default;
One = (T)(object)(ushort)1;
Two = (T)(object)(ushort)2;
MinusOne = default;
MinusTwo = default;
E = (T)(object)(ushort)FloatE;
Pi = (T)(object)(ushort)FloatPi;
Tau = (T)(object)(ushort)FloatTau;
}
else if (typeof(T) == typeof(sbyte))
{
Epsilon = default;
MaxValue = (T)(object)sbyte.MaxValue;
MinValue = (T)(object)sbyte.MinValue;
NaN = default;
NegativeInfinity = default;
PositiveInfinity = default;
One = (T)(object)(sbyte)1;
Two = (T)(object)(sbyte)2;
MinusOne = (T)(object)(sbyte)-1;
MinusTwo = (T)(object)(sbyte)-2;
E = (T)(object)(sbyte)FloatE;
Pi = (T)(object)(sbyte)FloatPi;
Tau = (T)(object)(sbyte)FloatTau;
}
else if (typeof(T) == typeof(byte))
{
Epsilon = default;
MaxValue = (T)(object)byte.MaxValue;
MinValue = (T)(object)byte.MinValue;
NaN = default;
NegativeInfinity = default;
PositiveInfinity = default;
One = (T)(object)(byte)1;
Two = (T)(object)(byte)2;
MinusOne = default;
MinusTwo = default;
E = (T)(object)(byte)FloatE;
Pi = (T)(object)(byte)FloatPi;
Tau = (T)(object)(byte)FloatTau;
}
else if (typeof(T) == typeof(int))
{
Epsilon = default;
MaxValue = (T)(object)int.MaxValue;
MinValue = (T)(object)int.MinValue;
NaN = default;
NegativeInfinity = default;
PositiveInfinity = default;
One = (T)(object)1;
Two = (T)(object)2;
MinusOne = (T)(object)-1;
MinusTwo = (T)(object)-2;
E = (T)(object)(int)FloatE;
Pi = (T)(object)(int)FloatPi;
Tau = (T)(object)(int)FloatTau;
}
else if (typeof(T) == typeof(uint))
{
Epsilon = default;
MaxValue = (T)(object)uint.MaxValue;
MinValue = (T)(object)uint.MinValue;
NaN = default;
NegativeInfinity = default;
PositiveInfinity = default;
One = (T)(object)1u;
Two = (T)(object)2u;
MinusOne = default;
MinusTwo = default;
E = (T)(object)(uint)FloatE;
Pi = (T)(object)(uint)FloatPi;
Tau = (T)(object)(uint)FloatTau;
}
else if (typeof(T) == typeof(long))
{
Epsilon = default;
MaxValue = (T)(object)long.MaxValue;
MinValue = (T)(object)long.MinValue;
NaN = default;
NegativeInfinity = default;
PositiveInfinity = default;
One = (T)(object)1L;
Two = (T)(object)2L;
MinusOne = (T)(object)-1L;
MinusTwo = (T)(object)-2L;
E = (T)(object)(long)FloatE;
Pi = (T)(object)(long)FloatPi;
Tau = (T)(object)(long)FloatTau;
}
else if (typeof(T) == typeof(ulong))
{
Epsilon = default;
MaxValue = (T)(object)ulong.MaxValue;
MinValue = (T)(object)ulong.MinValue;
NaN = default;
NegativeInfinity = default;
PositiveInfinity = default;
One = (T)(object)1ul;
Two = (T)(object)2ul;
MinusOne = default;
MinusTwo = default;
E = (T)(object)(ulong)FloatE;
Pi = (T)(object)(ulong)FloatPi;
Tau = (T)(object)(ulong)FloatTau;
}
else
{
// if it's none of these cases, don't do the general cases.
return;
}
PiOver2 = Scalar.Divide(Pi, Two);
DegreesPerRadian = Scalar.Divide(Scalar.As <float, T>(180), Pi);
RadiansPerDegree = Scalar.Divide(Pi, Scalar.As <float, T>(180));
}
/// <summary>Attempts to extract the scale, translation, and rotation components from the given scale/rotation/translation matrix.
/// If successful, the out parameters will contained the extracted values.</summary>
/// <param name="matrix">The source matrix.</param>
/// <param name="scale">The scaling component of the transformation matrix.</param>
/// <param name="rotation">The rotation component of the transformation matrix.</param>
/// <returns>True if the source matrix was successfully decomposed; False otherwise.</returns>
public static bool Decompose <T>(Matrix3X3 <T> matrix, out Vector3D <T> scale, out Quaternion <T> rotation)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
bool result = true;
unsafe
{
fixed(Vector3D <T> *scaleBase = &scale)
{
T *pfScales = (T *)scaleBase;
T det;
VectorBasis <T> vectorBasis;
Vector3D <T> ** pVectorBasis = (Vector3D <T> **) & vectorBasis;
Matrix3X3 <T> matTemp = Matrix3X3 <T> .Identity;
CanonicalBasis <T> canonicalBasis = default;
Vector3D <T> * pCanonicalBasis = &canonicalBasis.Row0;
canonicalBasis.Row0 = new Vector3D <T>(Scalar <T> .One, Scalar <T> .Zero, Scalar <T> .Zero);
canonicalBasis.Row1 = new Vector3D <T>(Scalar <T> .Zero, Scalar <T> .One, Scalar <T> .Zero);
canonicalBasis.Row2 = new Vector3D <T>(Scalar <T> .Zero, Scalar <T> .Zero, Scalar <T> .One);
pVectorBasis[0] = &matTemp.Row1;
pVectorBasis[1] = &matTemp.Row2;
pVectorBasis[2] = &matTemp.Row3;
*(pVectorBasis[0]) = new Vector3D <T>(matrix.M11, matrix.M12, matrix.M13);
*(pVectorBasis[1]) = new Vector3D <T>(matrix.M21, matrix.M22, matrix.M23);
*(pVectorBasis[2]) = new Vector3D <T>(matrix.M31, matrix.M32, matrix.M33);
scale.X = pVectorBasis[0]->Length;
scale.Y = pVectorBasis[1]->Length;
scale.Z = pVectorBasis[2]->Length;
uint a, b, c;
#region Ranking
T x = pfScales[0], y = pfScales[1], z = pfScales[2];
if (!Scalar.GreaterThanOrEqual(x, y))
{
if (!Scalar.GreaterThanOrEqual(y, z))
{
a = 2;
b = 1;
c = 0;
}
else
{
a = 1;
if (!Scalar.GreaterThanOrEqual(x, z))
{
b = 2;
c = 0;
}
else
{
b = 0;
c = 2;
}
}
}
else
{
if (!Scalar.GreaterThanOrEqual(x, z))
{
a = 2;
b = 0;
c = 1;
}
else
{
a = 0;
if (!Scalar.GreaterThanOrEqual(y, z))
{
b = 2;
c = 1;
}
else
{
b = 1;
c = 2;
}
}
}
#endregion
if (!Scalar.GreaterThanOrEqual(pfScales[a], Scalar.As <float, T>(DecomposeEpsilon)))
{
*(pVectorBasis[a]) = pCanonicalBasis[a];
}
*pVectorBasis[a] = Vector3D.Normalize(*pVectorBasis[a]);
if (!Scalar.GreaterThanOrEqual(pfScales[b], Scalar.As <float, T>(DecomposeEpsilon)))
{
uint cc;
T fAbsX, fAbsY, fAbsZ;
fAbsX = Scalar.Abs(pVectorBasis[a]->X);
fAbsY = Scalar.Abs(pVectorBasis[a]->Y);
fAbsZ = Scalar.Abs(pVectorBasis[a]->Z);
#region Ranking
if (!Scalar.GreaterThanOrEqual(fAbsX, fAbsY))
{
if (!Scalar.GreaterThanOrEqual(fAbsY, fAbsZ))
{
cc = 0;
}
else
{
if (!Scalar.GreaterThanOrEqual(fAbsX, fAbsZ))
{
cc = 0;
}
else
{
cc = 2;
}
}
}
else
{
if (!Scalar.GreaterThanOrEqual(fAbsX, fAbsZ))
{
cc = 1;
}
else
{
if (!Scalar.GreaterThanOrEqual(fAbsY, fAbsZ))
{
cc = 1;
}
else
{
cc = 2;
}
}
}
#endregion
*pVectorBasis[b] = Vector3D.Cross(*pVectorBasis[a], *(pCanonicalBasis + cc));
}
*pVectorBasis[b] = Vector3D.Normalize(*pVectorBasis[b]);
if (!Scalar.GreaterThanOrEqual(pfScales[c], Scalar.As <float, T>(DecomposeEpsilon)))
{
*pVectorBasis[c] = Vector3D.Cross(*pVectorBasis[a], *pVectorBasis[b]);
}
*pVectorBasis[c] = Vector3D.Normalize(*pVectorBasis[c]);
det = matTemp.GetDeterminant();
// use Kramer's rule to check for handedness of coordinate system
if (!Scalar.GreaterThanOrEqual(det, Scalar <T> .Zero))
{
// switch coordinate system by negating the scale and inverting the basis vector on the x-axis
pfScales[a] = Scalar.Negate(pfScales[a]);
*pVectorBasis[a] = -(*pVectorBasis[a]);
det = Scalar.Negate(det);
}
det = Scalar.Subtract(det, Scalar <T> .One);
det = Scalar.Multiply(det, det);
if (!Scalar.GreaterThanOrEqual(Scalar.As <float, T>(DecomposeEpsilon), det))
{
// Non-SRT matrix encountered
rotation = Quaternion <T> .Identity;
result = false;
}
else
{
// generate the quaternion from the matrix
rotation = Quaternion <T> .CreateFromRotationMatrix(matTemp);
}
}
}
return(result);
}
