504 lines
18 KiB
C#
504 lines
18 KiB
C#
/*
|
|
* Author: Andrey Pokidov
|
|
* License: Apache-2.0
|
|
* Date: 4 Dec 2024
|
|
*/
|
|
|
|
namespace BGC
|
|
{
|
|
public struct QuaternionFP32
|
|
{
|
|
public float s0 = 0.0f, x1 = 0.0f, x2 = 0.0f, x3 = 0.0f;
|
|
|
|
public QuaternionFP32(float s0, float x1, float x2, float x3)
|
|
{
|
|
this.s0 = s0;
|
|
this.x1 = x1;
|
|
this.x2 = x2;
|
|
this.x3 = x3;
|
|
}
|
|
|
|
public QuaternionFP32(in QuaternionFP32 quaternion)
|
|
{
|
|
this.s0 = quaternion.s0;
|
|
this.x1 = quaternion.x1;
|
|
this.x2 = quaternion.x2;
|
|
this.x3 = quaternion.x3;
|
|
}
|
|
|
|
public QuaternionFP32(in QuaternionFP64 quaternion)
|
|
{
|
|
this.s0 = (float)quaternion.s0;
|
|
this.x1 = (float)quaternion.x1;
|
|
this.x2 = (float)quaternion.x2;
|
|
this.x3 = (float)quaternion.x3;
|
|
}
|
|
|
|
public readonly float GetSquareModulus()
|
|
{
|
|
return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
|
|
}
|
|
|
|
public readonly float GetModulus()
|
|
{
|
|
return MathF.Sqrt(this.GetSquareModulus());
|
|
}
|
|
|
|
public readonly bool IsZero()
|
|
{
|
|
return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON;
|
|
}
|
|
|
|
public readonly bool IsUnit()
|
|
{
|
|
return UtilityFP32.IsSqareUnit(this.GetSquareModulus());
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
this.s0 = 0.0f;
|
|
this.x1 = 0.0f;
|
|
this.x2 = 0.0f;
|
|
this.x3 = 0.0f;
|
|
}
|
|
|
|
public void MakeUnit()
|
|
{
|
|
this.s0 = 1.0f;
|
|
this.x1 = 0.0f;
|
|
this.x2 = 0.0f;
|
|
this.x3 = 0.0f;
|
|
}
|
|
|
|
public void Conjugate()
|
|
{
|
|
this.x1 = -this.x1;
|
|
this.x2 = -this.x2;
|
|
this.x3 = -this.x3;
|
|
}
|
|
|
|
public void MakeOpposit()
|
|
{
|
|
this.s0 = -this.s0;
|
|
this.x1 = -this.x1;
|
|
this.x2 = -this.x2;
|
|
this.x3 = -this.x3;
|
|
}
|
|
|
|
public bool Invert()
|
|
{
|
|
float squareModulus = this.GetSquareModulus();
|
|
|
|
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
float multiplicand = 1.0f / squareModulus;
|
|
|
|
this.s0 = this.s0 * multiplicand;
|
|
this.x1 = -this.x1 * multiplicand;
|
|
this.x2 = -this.x2 * multiplicand;
|
|
this.x3 = -this.x3 * multiplicand;
|
|
|
|
return true;
|
|
}
|
|
|
|
public bool Normalize()
|
|
{
|
|
float squareModulus = this.GetSquareModulus();
|
|
|
|
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
|
|
|
this.s0 *= multiplier;
|
|
this.x1 *= multiplier;
|
|
this.x2 *= multiplier;
|
|
this.x3 *= multiplier;
|
|
|
|
return true;
|
|
}
|
|
|
|
public void SetValues(float s0, float x1, float x2, float x3)
|
|
{
|
|
this.s0 = s0;
|
|
this.x1 = x1;
|
|
this.x2 = x2;
|
|
this.x3 = x3;
|
|
}
|
|
|
|
public void SetValues(in QuaternionFP32 quaternion)
|
|
{
|
|
this.s0 = quaternion.s0;
|
|
this.x1 = quaternion.x1;
|
|
this.x2 = quaternion.x2;
|
|
this.x3 = quaternion.x3;
|
|
}
|
|
|
|
public void SetValues(in QuaternionFP64 quaternion)
|
|
{
|
|
this.s0 = (float)quaternion.s0;
|
|
this.x1 = (float)quaternion.x1;
|
|
this.x2 = (float)quaternion.x2;
|
|
this.x3 = (float)quaternion.x3;
|
|
}
|
|
|
|
public static void Reset(out QuaternionFP32 quaternion)
|
|
{
|
|
quaternion.s0 = 0.0f;
|
|
quaternion.x1 = 0.0f;
|
|
quaternion.x2 = 0.0f;
|
|
quaternion.x3 = 0.0f;
|
|
}
|
|
|
|
public static void MakeUnit(out QuaternionFP32 quaternion)
|
|
{
|
|
quaternion.s0 = 1.0f;
|
|
quaternion.x1 = 0.0f;
|
|
quaternion.x2 = 0.0f;
|
|
quaternion.x3 = 0.0f;
|
|
}
|
|
|
|
public static void Swap(ref QuaternionFP32 quaternion1, ref QuaternionFP32 quaternion2)
|
|
{
|
|
float s0 = quaternion1.s0;
|
|
float x1 = quaternion1.x1;
|
|
float x2 = quaternion1.x2;
|
|
float x3 = quaternion1.x3;
|
|
|
|
quaternion1.s0 = quaternion2.s0;
|
|
quaternion1.x1 = quaternion2.x1;
|
|
quaternion1.x2 = quaternion2.x2;
|
|
quaternion1.x3 = quaternion2.x3;
|
|
|
|
quaternion2.s0 = s0;
|
|
quaternion2.x1 = x1;
|
|
quaternion2.x2 = x2;
|
|
quaternion2.x3 = x3;
|
|
}
|
|
|
|
public static void Add(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2, out QuaternionFP32 sum)
|
|
{
|
|
sum.s0 = quaternion1.s0 + quaternion2.s0;
|
|
sum.x1 = quaternion1.x1 + quaternion2.x1;
|
|
sum.x2 = quaternion1.x2 + quaternion2.x2;
|
|
sum.x3 = quaternion1.x3 + quaternion2.x3;
|
|
}
|
|
|
|
public static void AddScaled(in QuaternionFP32 basic_quaternion, in QuaternionFP32 scalable_quaternion, float scale, out QuaternionFP32 sum)
|
|
{
|
|
sum.s0 = basic_quaternion.s0 + scale * scalable_quaternion.s0;
|
|
sum.x1 = basic_quaternion.x1 + scale * scalable_quaternion.x1;
|
|
sum.x2 = basic_quaternion.x2 + scale * scalable_quaternion.x2;
|
|
sum.x3 = basic_quaternion.x3 + scale * scalable_quaternion.x3;
|
|
}
|
|
|
|
public static void Subtract(in QuaternionFP32 minuend, in QuaternionFP32 subtrahend, out QuaternionFP32 difference)
|
|
{
|
|
difference.s0 = minuend.s0 - subtrahend.s0;
|
|
difference.x1 = minuend.x1 - subtrahend.x1;
|
|
difference.x2 = minuend.x2 - subtrahend.x2;
|
|
difference.x3 = minuend.x3 - subtrahend.x3;
|
|
}
|
|
|
|
public static void Multiply(in QuaternionFP32 left, in QuaternionFP32 right, out QuaternionFP32 product)
|
|
{
|
|
float s0 = left.s0 * right.s0 - left.x1 * right.x1 - (left.x2 * right.x2 + left.x3 * right.x3);
|
|
float x1 = left.x1 * right.s0 + left.s0 * right.x1 - (left.x3 * right.x2 - left.x2 * right.x3);
|
|
float x2 = left.x2 * right.s0 + left.s0 * right.x2 - (left.x1 * right.x3 - left.x3 * right.x1);
|
|
float x3 = left.x3 * right.s0 + left.s0 * right.x3 - (left.x2 * right.x1 - left.x1 * right.x2);
|
|
|
|
product.s0 = s0;
|
|
product.x1 = x1;
|
|
product.x2 = x2;
|
|
product.x3 = x3;
|
|
}
|
|
|
|
public static void Multiply(in QuaternionFP32 quaternion, float multiplier, out QuaternionFP32 product)
|
|
{
|
|
product.s0 = quaternion.s0 * multiplier;
|
|
product.x1 = quaternion.x1 * multiplier;
|
|
product.x2 = quaternion.x2 * multiplier;
|
|
product.x3 = quaternion.x3 * multiplier;
|
|
}
|
|
|
|
public static bool Divide(in QuaternionFP32 divident, in QuaternionFP32 divisor, out QuaternionFP32 quotient)
|
|
{
|
|
float squareModulus = divisor.GetSquareModulus();
|
|
|
|
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
|
{
|
|
Reset(out quotient);
|
|
return false;
|
|
}
|
|
|
|
float s0 = (divident.s0 * divisor.s0 + divident.x1 * divisor.x1) + (divident.x2 * divisor.x2 + divident.x3 * divisor.x3);
|
|
float x1 = (divident.x1 * divisor.s0 + divident.x3 * divisor.x2) - (divident.s0 * divisor.x1 + divident.x2 * divisor.x3);
|
|
float x2 = (divident.x2 * divisor.s0 + divident.x1 * divisor.x3) - (divident.s0 * divisor.x2 + divident.x3 * divisor.x1);
|
|
float x3 = (divident.x3 * divisor.s0 + divident.x2 * divisor.x1) - (divident.s0 * divisor.x3 + divident.x1 * divisor.x2);
|
|
|
|
float multiplicand = 1.0f / squareModulus;
|
|
|
|
quotient.s0 = s0 * multiplicand;
|
|
quotient.x1 = x1 * multiplicand;
|
|
quotient.x2 = x2 * multiplicand;
|
|
quotient.x3 = x3 * multiplicand;
|
|
|
|
return true;
|
|
}
|
|
|
|
public static void Divide(in QuaternionFP32 divident, float divisor, out QuaternionFP32 quotient)
|
|
{
|
|
Multiply(divident, 1.0f / divisor, out quotient);
|
|
}
|
|
|
|
public static void GetMeanOfTwo(in QuaternionFP32 vector1, in QuaternionFP32 vector2, out QuaternionFP32 mean)
|
|
{
|
|
mean.s0 = (vector1.s0 + vector2.s0) * 0.5f;
|
|
mean.x1 = (vector1.x1 + vector2.x1) * 0.5f;
|
|
mean.x2 = (vector1.x2 + vector2.x2) * 0.5f;
|
|
mean.x3 = (vector1.x3 + vector2.x3) * 0.5f;
|
|
}
|
|
|
|
public static void GetMeanOfThree(in QuaternionFP32 vector1, in QuaternionFP32 vector2, in QuaternionFP32 vector3, out QuaternionFP32 mean)
|
|
{
|
|
mean.s0 = (vector1.s0 + vector2.s0 + vector3.s0) * UtilityFP32.ONE_THIRD;
|
|
mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD;
|
|
mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD;
|
|
mean.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP32.ONE_THIRD;
|
|
}
|
|
|
|
public static void Interpolate(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2, float phase, out QuaternionFP32 interpolation)
|
|
{
|
|
float counterphase = 1.0f - phase;
|
|
|
|
interpolation.s0 = quaternion1.s0 * counterphase + quaternion2.s0 * phase;
|
|
interpolation.x1 = quaternion1.x1 * counterphase + quaternion2.x1 * phase;
|
|
interpolation.x2 = quaternion1.x2 * counterphase + quaternion2.x2 * phase;
|
|
interpolation.x3 = quaternion1.x3 * counterphase + quaternion2.x3 * phase;
|
|
}
|
|
|
|
public static void GetConjugate(in QuaternionFP32 quaternion, out QuaternionFP32 conjugate)
|
|
{
|
|
conjugate.s0 = quaternion.s0;
|
|
conjugate.x1 = -quaternion.x1;
|
|
conjugate.x2 = -quaternion.x2;
|
|
conjugate.x3 = -quaternion.x3;
|
|
}
|
|
|
|
public static void GetOpposite(in QuaternionFP32 quaternion, out QuaternionFP32 opposit)
|
|
{
|
|
opposit.s0 = -quaternion.s0;
|
|
opposit.x1 = -quaternion.x1;
|
|
opposit.x2 = -quaternion.x2;
|
|
opposit.x3 = -quaternion.x3;
|
|
}
|
|
|
|
public static bool GetInverse(in QuaternionFP32 quaternion, out QuaternionFP32 inverse)
|
|
{
|
|
float squareModulus = quaternion.GetSquareModulus();
|
|
|
|
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
|
{
|
|
Reset(out inverse);
|
|
return false;
|
|
}
|
|
|
|
float multiplicand = 1.0f / squareModulus;
|
|
|
|
inverse.s0 = quaternion.s0 * multiplicand;
|
|
inverse.x1 = -quaternion.x1 * multiplicand;
|
|
inverse.x2 = -quaternion.x2 * multiplicand;
|
|
inverse.x3 = -quaternion.x3 * multiplicand;
|
|
|
|
return true;
|
|
}
|
|
|
|
public static bool GetNormalized(in QuaternionFP32 quaternion, out QuaternionFP32 normalized)
|
|
{
|
|
float squareModulus = quaternion.GetSquareModulus();
|
|
|
|
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
|
{
|
|
Reset(out normalized);
|
|
return false;
|
|
}
|
|
|
|
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
|
|
|
normalized.s0 = quaternion.s0 * multiplier;
|
|
normalized.x1 = quaternion.x1 * multiplier;
|
|
normalized.x2 = quaternion.x2 * multiplier;
|
|
normalized.x3 = quaternion.x3 * multiplier;
|
|
|
|
return true;
|
|
}
|
|
|
|
public static bool GetExponation(in QuaternionFP32 quaternion, float exponent, out QuaternionFP32 power)
|
|
{
|
|
float s0s0 = quaternion.s0 * quaternion.s0;
|
|
float x1x1 = quaternion.x1 * quaternion.x1;
|
|
float x2x2 = quaternion.x2 * quaternion.x2;
|
|
float x3x3 = quaternion.x3 * quaternion.x3;
|
|
|
|
float squareVector = x1x1 + (x2x2 + x3x3);
|
|
float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
|
|
|
if (float.IsNaN(squareModulus))
|
|
{
|
|
Reset(out power);
|
|
return false;
|
|
}
|
|
|
|
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
|
{
|
|
Reset(out power);
|
|
return true;
|
|
}
|
|
|
|
if (squareVector <= UtilityFP32.SQUARE_EPSYLON)
|
|
{
|
|
if (quaternion.s0 < 0.0f)
|
|
{
|
|
Reset(out power);
|
|
return false;
|
|
}
|
|
|
|
power.s0 = MathF.Pow(quaternion.s0, exponent);
|
|
power.x1 = 0.0f;
|
|
power.x2 = 0.0f;
|
|
power.x3 = 0.0f;
|
|
|
|
return true;
|
|
}
|
|
|
|
float sine = MathF.Sqrt(squareVector / squareModulus);
|
|
float power_angle = MathF.Atan2(sine, quaternion.s0 / MathF.Sqrt(squareModulus)) * exponent;
|
|
float power_modulus = MathF.Pow(squareModulus, 0.5f * exponent);
|
|
float multiplier = power_modulus * MathF.Sin(power_angle) / sine;
|
|
|
|
power.s0 = power_modulus * MathF.Cos(power_angle);
|
|
power.x1 = quaternion.x1 * multiplier;
|
|
power.x2 = quaternion.x2 * multiplier;
|
|
power.x3 = quaternion.x3 * multiplier;
|
|
|
|
return true;
|
|
}
|
|
|
|
public static bool GetRotationMatrix(in QuaternionFP32 quaternion, out Matrix3x3FP32 matrix)
|
|
{
|
|
float s0s0 = quaternion.s0 * quaternion.s0;
|
|
float x1x1 = quaternion.x1 * quaternion.x1;
|
|
float x2x2 = quaternion.x2 * quaternion.x2;
|
|
float x3x3 = quaternion.x3 * quaternion.x3;
|
|
|
|
float squareModulus = s0s0 + x1x1 + (x2x2 + x3x3);
|
|
|
|
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
|
{
|
|
Matrix3x3FP32.LoadIdentity(out matrix);
|
|
return false;
|
|
}
|
|
|
|
float corrector1 = 1.0f / squareModulus;
|
|
|
|
float s0x1 = quaternion.s0 * quaternion.x1;
|
|
float s0x2 = quaternion.s0 * quaternion.x2;
|
|
float s0x3 = quaternion.s0 * quaternion.x3;
|
|
float x1x2 = quaternion.x1 * quaternion.x2;
|
|
float x1x3 = quaternion.x1 * quaternion.x3;
|
|
float x2x3 = quaternion.x2 * quaternion.x3;
|
|
|
|
float corrector2 = 2.0f * corrector1;
|
|
|
|
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
|
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
|
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
|
|
|
matrix.r1c2 = corrector2 * (x1x2 - s0x3);
|
|
matrix.r2c3 = corrector2 * (x2x3 - s0x1);
|
|
matrix.r3c1 = corrector2 * (x1x3 - s0x2);
|
|
|
|
matrix.r2c1 = corrector2 * (x1x2 + s0x3);
|
|
matrix.r3c2 = corrector2 * (x2x3 + s0x1);
|
|
matrix.r1c3 = corrector2 * (x1x3 + s0x2);
|
|
|
|
return true;
|
|
}
|
|
|
|
public static bool GetReverseMatrix(in QuaternionFP32 quaternion, out Matrix3x3FP32 matrix)
|
|
{
|
|
float s0s0 = quaternion.s0 * quaternion.s0;
|
|
float x1x1 = quaternion.x1 * quaternion.x1;
|
|
float x2x2 = quaternion.x2 * quaternion.x2;
|
|
float x3x3 = quaternion.x3 * quaternion.x3;
|
|
|
|
float squareModulus = s0s0 + x1x1 + (x2x2 + x3x3);
|
|
|
|
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
|
{
|
|
Matrix3x3FP32.LoadIdentity(out matrix);
|
|
return false;
|
|
}
|
|
|
|
float corrector1 = 1.0f / squareModulus;
|
|
|
|
float s0x1 = quaternion.s0 * quaternion.x1;
|
|
float s0x2 = quaternion.s0 * quaternion.x2;
|
|
float s0x3 = quaternion.s0 * quaternion.x3;
|
|
float x1x2 = quaternion.x1 * quaternion.x2;
|
|
float x1x3 = quaternion.x1 * quaternion.x3;
|
|
float x2x3 = quaternion.x2 * quaternion.x3;
|
|
|
|
float corrector2 = 2.0f * corrector1;
|
|
|
|
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
|
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
|
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
|
|
|
matrix.r1c2 = corrector2 * (x1x2 + s0x3);
|
|
matrix.r2c3 = corrector2 * (x2x3 + s0x1);
|
|
matrix.r3c1 = corrector2 * (x1x3 + s0x2);
|
|
|
|
matrix.r2c1 = corrector2 * (x1x2 - s0x3);
|
|
matrix.r3c2 = corrector2 * (x2x3 - s0x1);
|
|
matrix.r1c3 = corrector2 * (x1x3 - s0x2);
|
|
|
|
return true;
|
|
}
|
|
|
|
public static bool GetBothMatrices(in QuaternionFP32 quaternion, out Matrix3x3FP32 matrix, out Matrix3x3FP32 reverse)
|
|
{
|
|
if (GetReverseMatrix(quaternion, out reverse))
|
|
{
|
|
Matrix3x3FP32.MakeTransposed(reverse, out matrix);
|
|
return true;
|
|
}
|
|
|
|
Matrix3x3FP32.LoadIdentity(out matrix);
|
|
return false;
|
|
}
|
|
|
|
public static bool AreClose(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2)
|
|
{
|
|
float ds0 = quaternion1.s0 - quaternion2.s0;
|
|
float dx1 = quaternion1.x1 - quaternion2.x1;
|
|
float dx2 = quaternion1.x2 - quaternion2.x2;
|
|
float dx3 = quaternion1.x3 - quaternion2.x3;
|
|
|
|
float squareModulus1 = quaternion1.GetSquareModulus();
|
|
float squareModulus2 = quaternion2.GetSquareModulus();
|
|
float squareDistance = (ds0 * ds0 + dx1 * dx1) + (dx2 * dx2 + dx3 * dx3);
|
|
|
|
if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) {
|
|
return squareDistance <= UtilityFP32.SQUARE_EPSYLON;
|
|
}
|
|
|
|
return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2;
|
|
}
|
|
}
|
|
}
|