/// <summary>Creates a spherical billboard that rotates around a specified object position.</summary>
/// <param name="objectPosition">Position of the object the billboard will rotate around.</param>
/// <param name="cameraPosition">Position of the camera.</param>
/// <param name="cameraUpVector">The up vector of the camera.</param>
/// <param name="cameraForwardVector">The forward vector of the camera.</param>
/// <returns>The created billboard matrix</returns>
public static Matrix2X3 <T> CreateBillboard <T>(Vector3D <T> objectPosition, Vector3D <T> cameraPosition, Vector3D <T> cameraUpVector, Vector3D <T> cameraForwardVector)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
Vector3D <T> zaxis = objectPosition - cameraPosition;
var norm = zaxis.LengthSquared;
if (!Scalar.GreaterThanOrEqual(norm, Scalar.As <float, T>(BillboardEpsilon)))
{
zaxis = -cameraForwardVector;
}
else
{
zaxis = Vector3D.Multiply(zaxis, Scalar.Reciprocal(Scalar.Sqrt(norm)));
}
Vector3D <T> xaxis = Vector3D.Normalize(Vector3D.Cross(cameraUpVector, zaxis));
Vector3D <T> yaxis = Vector3D.Cross(zaxis, xaxis);
return(new(xaxis, yaxis));
}
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>
/// Returns this circle casted to <typeparamref name="TOther"></typeparamref>
/// </summary>
/// <typeparam name="TOther">The type to cast to</typeparam>
/// <returns>The casted circle</returns>
public Circle <TOther> As <TOther>() where TOther : unmanaged, IFormattable, IEquatable <TOther>, IComparable <TOther>
{
return(new(Center.As <TOther>(), Scalar.As <T, TOther>(Radius)));
}
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>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 <T> CreateRotation <T>(T radians)
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);
}
// [ c s ]
// [ -s c ]
// [ 0 0 ]
Matrix3X2 <T> result = Matrix3X2 <T> .Identity;
result.M11 = c;
result.M12 = s;
result.M21 = Scalar.Negate(s);
result.M22 = c;
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 <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)));
}
/// <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);
}