338 lines
11 KiB
C#
338 lines
11 KiB
C#
/*
|
|
* Author: Andrey Pokidov
|
|
* License: Apache-2.0
|
|
* Date: 20 Oct 2024
|
|
*/
|
|
|
|
namespace BGC
|
|
{
|
|
public struct VersorFP64
|
|
{
|
|
private double s0 = 1.0;
|
|
private double x1 = 0.0;
|
|
private double x2 = 0.0;
|
|
private double x3 = 0.0;
|
|
|
|
public VersorFP64(double s0, double x1, double x2, double x3)
|
|
{
|
|
this.s0 = s0;
|
|
this.x1 = x1;
|
|
this.x2 = x2;
|
|
this.x3 = x3;
|
|
|
|
double squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
|
|
|
|
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
|
{
|
|
this.Normalize(squareModulus);
|
|
}
|
|
}
|
|
|
|
public VersorFP64(in VersorFP64 versor)
|
|
{
|
|
this.s0 = versor.s0;
|
|
this.x1 = versor.x1;
|
|
this.x2 = versor.x2;
|
|
this.x3 = versor.x3;
|
|
}
|
|
|
|
public VersorFP64(in VersorFP32 versor)
|
|
{
|
|
this.s0 = versor.GetScalar();
|
|
this.x1 = versor.GetX1();
|
|
this.x2 = versor.GetX2();
|
|
this.x3 = versor.GetX3();
|
|
|
|
double squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
|
|
|
|
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
|
{
|
|
this.Normalize(squareModulus);
|
|
}
|
|
}
|
|
|
|
public readonly double GetScalar()
|
|
{
|
|
return this.s0;
|
|
}
|
|
|
|
public readonly double GetX1()
|
|
{
|
|
return this.x1;
|
|
}
|
|
|
|
public readonly double GetX2()
|
|
{
|
|
return this.x2;
|
|
}
|
|
|
|
public readonly double GetX3()
|
|
{
|
|
return this.x3;
|
|
}
|
|
|
|
public readonly double GetAngle(AngleUnit unit)
|
|
{
|
|
if (this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || 1.0 - UtilityFP64.EPSYLON <= this.s0) {
|
|
return 0.0;
|
|
}
|
|
|
|
if (UtilityFP64.IsZero(this.s0))
|
|
{
|
|
return AngleFP64.GetHalfCircle(unit);
|
|
}
|
|
|
|
return RadianFP64.ToUnits(2.0 * Math.Acos(this.s0), unit);
|
|
}
|
|
|
|
public readonly bool IsIdentity()
|
|
{
|
|
return this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || (1.0 - UtilityFP64.EPSYLON) <= this.s0;
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
this.s0 = 1.0;
|
|
this.x1 = 0.0;
|
|
this.x2 = 0.0;
|
|
this.x3 = 0.0;
|
|
}
|
|
|
|
public void MakeOpposite()
|
|
{
|
|
}
|
|
|
|
public void Shorten()
|
|
{
|
|
if (this.s0 < 0.0)
|
|
{
|
|
this.s0 = -this.s0;
|
|
this.x1 = -this.x1;
|
|
this.x2 = -this.x2;
|
|
this.x3 = -this.x3;
|
|
}
|
|
}
|
|
|
|
public void Invert()
|
|
{
|
|
this.x1 = -this.x1;
|
|
this.x2 = -this.x2;
|
|
this.x3 = -this.x3;
|
|
}
|
|
|
|
public void SetValues(double s0, double x1, double x2, double x3)
|
|
{
|
|
this.s0 = s0;
|
|
this.x1 = x1;
|
|
this.x2 = x2;
|
|
this.x3 = x3;
|
|
|
|
double squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
|
|
|
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
|
{
|
|
this.Normalize(squareModulus);
|
|
}
|
|
}
|
|
|
|
public void Set(in VersorFP64 versor)
|
|
{
|
|
this.s0 = versor.s0;
|
|
this.x1 = versor.x1;
|
|
this.x2 = versor.x2;
|
|
this.x3 = versor.x3;
|
|
}
|
|
|
|
public void Set(in VersorFP32 versor)
|
|
{
|
|
this.SetValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3());
|
|
}
|
|
|
|
private void Normalize(double squareModulus)
|
|
{
|
|
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
|
|
{
|
|
this.Reset();
|
|
return;
|
|
}
|
|
|
|
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
|
|
|
this.s0 *= multiplier;
|
|
this.x1 *= multiplier;
|
|
this.x2 *= multiplier;
|
|
this.x3 *= multiplier;
|
|
}
|
|
|
|
public static void Combine(in VersorFP64 second, in VersorFP64 first, out VersorFP64 result)
|
|
{
|
|
LoadValues(
|
|
(second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3),
|
|
(second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3),
|
|
(second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1),
|
|
(second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2),
|
|
out result
|
|
);
|
|
}
|
|
|
|
public static void Combine3(in VersorFP64 third, in VersorFP64 second, in VersorFP64 first, out VersorFP64 result)
|
|
{
|
|
double s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3);
|
|
double x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3);
|
|
double x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1);
|
|
double x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2);
|
|
|
|
LoadValues(
|
|
(third.s0 * s0 - third.x1 * x1) - (third.x2 * x2 + third.x3 * x3),
|
|
(third.x1 * s0 + third.s0 * x1) - (third.x3 * x2 - third.x2 * x3),
|
|
(third.x2 * s0 + third.s0 * x2) - (third.x1 * x3 - third.x3 * x1),
|
|
(third.x3 * s0 + third.s0 * x3) - (third.x2 * x1 - third.x1 * x2),
|
|
out result
|
|
);
|
|
}
|
|
|
|
public static void Exclude(in VersorFP64 basic, in VersorFP64 excludant, out VersorFP64 result)
|
|
{
|
|
LoadValues(
|
|
(basic.s0 * excludant.s0 + basic.x1 * excludant.x1) + (basic.x2 * excludant.x2 + basic.x3 * excludant.x3),
|
|
(basic.x1 * excludant.s0 + basic.x3 * excludant.x2) - (basic.s0 * excludant.x1 + basic.x2 * excludant.x3),
|
|
(basic.x2 * excludant.s0 + basic.x1 * excludant.x3) - (basic.s0 * excludant.x2 + basic.x3 * excludant.x1),
|
|
(basic.x3 * excludant.s0 + basic.x2 * excludant.x1) - (basic.s0 * excludant.x3 + basic.x1 * excludant.x2),
|
|
out result
|
|
);
|
|
}
|
|
|
|
public static void MakeInverted(in VersorFP64 versor, out VersorFP64 conjugate)
|
|
{
|
|
conjugate.s0 = versor.s0;
|
|
conjugate.x1 = -versor.x1;
|
|
conjugate.x2 = -versor.x2;
|
|
conjugate.x3 = -versor.x3;
|
|
}
|
|
|
|
public static void MakeShortened(in VersorFP64 versor, out VersorFP64 shortened)
|
|
{
|
|
if (versor.s0 < 0.0) {
|
|
shortened.s0 = -versor.s0;
|
|
shortened.x1 = -versor.x1;
|
|
shortened.x2 = -versor.x2;
|
|
shortened.x3 = -versor.x3;
|
|
}
|
|
else {
|
|
shortened.s0 = versor.s0;
|
|
shortened.x1 = versor.x1;
|
|
shortened.x2 = versor.x2;
|
|
shortened.x3 = versor.x3;
|
|
}
|
|
}
|
|
|
|
public static void MakeRotationMatrix(in VersorFP64 versor, out Matrix3x3FP64 matrix)
|
|
{
|
|
double s0s0 = versor.s0 * versor.s0;
|
|
double x1x1 = versor.x1 * versor.x1;
|
|
double x2x2 = versor.x1 * versor.x2;
|
|
double x3x3 = versor.x1 * versor.x3;
|
|
|
|
double s0x1 = versor.s0 * versor.x1;
|
|
double s0x2 = versor.s0 * versor.x2;
|
|
double s0x3 = versor.s0 * versor.x3;
|
|
|
|
double x1x2 = versor.x1 * versor.x2;
|
|
double x1x3 = versor.x1 * versor.x3;
|
|
double x2x3 = versor.x2 * versor.x3;
|
|
|
|
matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3);
|
|
matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3);
|
|
matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2);
|
|
|
|
matrix.r1c2 = 2.0 * (x1x2 - s0x3);
|
|
matrix.r2c3 = 2.0 * (x2x3 - s0x1);
|
|
matrix.r3c1 = 2.0 * (x1x3 - s0x2);
|
|
|
|
matrix.r2c1 = 2.0 * (x1x2 + s0x3);
|
|
matrix.r3c2 = 2.0 * (x2x3 + s0x1);
|
|
matrix.r1c3 = 2.0 * (x1x3 + s0x2);
|
|
}
|
|
|
|
public static void MakeReverseMatrix(in VersorFP64 versor, out Matrix3x3FP64 matrix)
|
|
{
|
|
double s0s0 = versor.s0 * versor.s0;
|
|
double x1x1 = versor.x1 * versor.x1;
|
|
double x2x2 = versor.x1 * versor.x2;
|
|
double x3x3 = versor.x1 * versor.x3;
|
|
|
|
double s0x1 = versor.s0 * versor.x1;
|
|
double s0x2 = versor.s0 * versor.x2;
|
|
double s0x3 = versor.s0 * versor.x3;
|
|
|
|
double x1x2 = versor.x1 * versor.x2;
|
|
double x1x3 = versor.x1 * versor.x3;
|
|
double x2x3 = versor.x2 * versor.x3;
|
|
|
|
matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3);
|
|
matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3);
|
|
matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2);
|
|
|
|
matrix.r1c2 = 2.0 * (x1x2 + s0x3);
|
|
matrix.r2c3 = 2.0 * (x2x3 + s0x1);
|
|
matrix.r3c1 = 2.0 * (x1x3 + s0x2);
|
|
|
|
matrix.r2c1 = 2.0 * (x1x2 - s0x3);
|
|
matrix.r3c2 = 2.0 * (x2x3 - s0x1);
|
|
matrix.r1c3 = 2.0 * (x1x3 - s0x2);
|
|
}
|
|
|
|
public static void Turn(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result)
|
|
{
|
|
double tx1 = 2.0 * (versor.x2 * vector.x3 - versor.x3 * vector.x2);
|
|
double tx2 = 2.0 * (versor.x3 * vector.x1 - versor.x1 * vector.x3);
|
|
double tx3 = 2.0 * (versor.x1 * vector.x2 - versor.x2 * vector.x1);
|
|
|
|
double x1 = vector.x1 + tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2);
|
|
double x2 = vector.x2 + tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3);
|
|
double x3 = vector.x3 + tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1);
|
|
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
result.x3 = x3;
|
|
}
|
|
|
|
public static void TurnBack(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result)
|
|
{
|
|
double tx1 = 2.0 * (versor.x2 * vector.x3 - versor.x3 * vector.x2);
|
|
double tx2 = 2.0 * (versor.x3 * vector.x1 - versor.x1 * vector.x3);
|
|
double tx3 = 2.0 * (versor.x1 * vector.x2 - versor.x2 * vector.x1);
|
|
|
|
double x1 = vector.x1 - tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2);
|
|
double x2 = vector.x2 - tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3);
|
|
double x3 = vector.x3 - tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1);
|
|
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
result.x3 = x3;
|
|
}
|
|
|
|
public static void LoadIdentity(out VersorFP64 result)
|
|
{
|
|
result.s0 = 1.0;
|
|
result.x1 = 0.0;
|
|
result.x2 = 0.0;
|
|
result.x3 = 0.0;
|
|
}
|
|
|
|
public static void LoadValues(double s0, double x1, double x2, double x3, out VersorFP64 result)
|
|
{
|
|
double squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3);
|
|
|
|
result.s0 = s0;
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
result.x3 = x3;
|
|
|
|
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
|
{
|
|
result.Normalize(squareModulus);
|
|
}
|
|
}
|
|
}
|
|
}
|