diff --git a/BGC/F32Versor.cs b/BGC/F32Versor.cs index f28306c..cb3127d 100644 --- a/BGC/F32Versor.cs +++ b/BGC/F32Versor.cs @@ -23,22 +23,10 @@ namespace BGC { public struct F32Versor { - public static readonly F32Versor IDLE = new F32Versor(1.0f, 0.0f, 0.0f, 0.0f, 1.0f); - private float s0; private float x1; private float x2; private float x3; - private float corrector; - - private F32Versor(float s0, float x1, float x2, float x3, float corrector) - { - this.s0 = s0; - this.x1 = x1; - this.x2 = x2; - this.x3 = x3; - this.corrector = corrector; - } public F32Versor() { @@ -46,28 +34,11 @@ namespace BGC this.x1 = 0.0f; this.x2 = 0.0f; this.x3 = 0.0f; - this.corrector = 1.0f; } public F32Versor(float s0, float x1, float x2, float x3) { - this.s0 = s0; - this.x1 = x1; - this.x2 = x2; - this.x3 = x3; - - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - this.corrector = 2.0f - squareModule; - - if (squareModule < 1.0f - F32Utility.TWO_EPSYLON || 1.0f + F32Utility.TWO_EPSYLON < squareModule) - { - this.Normalize(squareModule); - } - else if (s0 < -1.0f + F32Utility.EPSYLON || 1.0f - F32Utility.EPSYLON < s0) - { - this.Reset(); - } + LoadValues(s0, x1, x2, x3, out this); } public F32Versor(in F32Versor versor) @@ -76,7 +47,6 @@ namespace BGC this.x1 = versor.x1; this.x2 = versor.x2; this.x3 = versor.x3; - this.corrector = versor.corrector; } public F32Versor(in F64Versor versor) @@ -85,19 +55,6 @@ namespace BGC this.x1 = (float)versor.GetX1(); this.x2 = (float)versor.GetX2(); this.x3 = (float)versor.GetX3(); - - float squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); - - this.corrector = 2.0f - squareModule; - - if (squareModule < 1.0f - F32Utility.TWO_EPSYLON || 1.0f + F32Utility.TWO_EPSYLON < squareModule) - { - this.Normalize(squareModule); - } - else if (s0 < -1.0f + F32Utility.EPSYLON || 1.0f - F32Utility.EPSYLON < s0) - { - this.Reset(); - } } public readonly float GetScalar() @@ -120,11 +77,6 @@ namespace BGC return this.x3; } - public readonly float GetCorrector() - { - return this.corrector; - } - public readonly bool IsIdle() { return this.s0 <= -(1.0f - F32Utility.EPSYLON) || (1.0f - F32Utility.EPSYLON) <= this.s0; @@ -136,7 +88,6 @@ namespace BGC this.x1 = 0.0f; this.x2 = 0.0f; this.x3 = 0.0f; - this.corrector = 1.0f; } public void Invert() @@ -167,28 +118,25 @@ namespace BGC float x2x2 = this.x1 * this.x2; float x3x3 = this.x1 * this.x3; + float s0x1 = 2.0f * this.s0 * this.x1; + float s0x2 = 2.0f * this.s0 * this.x2; + float s0x3 = 2.0f * this.s0 * this.x3; - float s0x1 = this.s0 * this.x1; - float s0x2 = this.s0 * this.x2; - float s0x3 = this.s0 * this.x3; + float x1x2 = 2.0f * this.x1 * this.x2; + float x1x3 = 2.0f * this.x1 * this.x3; + float x2x3 = 2.0f * this.x2 * this.x3; - float x1x2 = this.x1 * this.x2; - float x1x3 = this.x1 * this.x3; - float x2x3 = this.x2 * this.x3; + matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); + matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); + matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - float corrector2 = 2.0f * this.corrector; + matrix.r1c2 = x1x2 - s0x3; + matrix.r2c3 = x2x3 - s0x1; + matrix.r3c1 = x1x3 - s0x2; - matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); - matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); - matrix.r3c3 = this.corrector * ((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); + matrix.r2c1 = x1x2 + s0x3; + matrix.r3c2 = x2x3 + s0x1; + matrix.r1c3 = x1x3 + s0x2; } public readonly void MakeReverseMatrix(out F32Matrix3x3 matrix) @@ -198,28 +146,25 @@ namespace BGC float x2x2 = this.x1 * this.x2; float x3x3 = this.x1 * this.x3; + float s0x1 = 2.0f * this.s0 * this.x1; + float s0x2 = 2.0f * this.s0 * this.x2; + float s0x3 = 2.0f * this.s0 * this.x3; - float s0x1 = this.s0 * this.x1; - float s0x2 = this.s0 * this.x2; - float s0x3 = this.s0 * this.x3; + float x1x2 = 2.0f * this.x1 * this.x2; + float x1x3 = 2.0f * this.x1 * this.x3; + float x2x3 = 2.0f * this.x2 * this.x3; - float x1x2 = this.x1 * this.x2; - float x1x3 = this.x1 * this.x3; - float x2x3 = this.x2 * this.x3; + matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); + matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); + matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - float corrector2 = 2.0f * this.corrector; + matrix.r1c2 = x1x2 + s0x3; + matrix.r2c3 = x2x3 + s0x1; + matrix.r3c1 = x1x3 + s0x2; - matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); - matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); - matrix.r3c3 = this.corrector * ((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); + matrix.r2c1 = x1x2 - s0x3; + matrix.r3c2 = x2x3 - s0x1; + matrix.r1c3 = x1x3 - s0x2; } public void SetValues(float s0, float x1, float x2, float x3) @@ -228,21 +173,21 @@ namespace BGC this.x1 = x1; this.x2 = x2; this.x3 = x3; - - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - if (squareModule < 1.0f - F32Utility.TWO_EPSYLON || 1.0f + F32Utility.TWO_EPSYLON < squareModule) - { - this.Normalize(squareModule); - return; - } + float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - if (s0 < -1.0f + F32Utility.EPSYLON || 1.0f - F32Utility.EPSYLON < s0) { + if (1.0f - F32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + F32Utility.TWO_EPSYLON) + { + if (-1.0f + F32Utility.EPSYLON < s0) + { + return; + } + this.Reset(); return; } - this.corrector = 2.0f - squareModule; + this.Normalize(squareModule); } public void SetValues(in F32Versor versor) @@ -251,17 +196,14 @@ namespace BGC this.x1 = versor.x1; this.x2 = versor.x2; this.x3 = versor.x3; - this.corrector = versor.corrector; } public void SetValues(in F64Versor versor) { - this.SetValues( - (float)versor.GetScalar(), - (float)versor.GetX1(), - (float)versor.GetX2(), - (float)versor.GetX3() - ); + this.s0 = (float) versor.GetScalar(); + this.x1 = (float) versor.GetX1(); + this.x2 = (float) versor.GetX2(); + this.x3 = (float) versor.GetX3(); } public void SetInverted(in F32Versor versor) @@ -270,25 +212,21 @@ namespace BGC this.x1 = -versor.x1; this.x2 = -versor.x2; this.x3 = -versor.x3; - this.corrector = versor.corrector; } public void SetInverted(in F64Versor versor) { - this.SetValues( - (float)versor.GetScalar(), - -(float)versor.GetX1(), - -(float)versor.GetX2(), - -(float)versor.GetX3() - ); + this.s0 = (float) versor.GetScalar(); + this.x1 = (float) -versor.GetX1(); + this.x2 = (float) -versor.GetX2(); + this.x3 = (float) -versor.GetX3(); } public readonly void Turn(in F32Vector3 vector, out F32Vector3 result) { - float multiplier = 2.0f * this.corrector; - float tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); - float tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); - float tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1); + float tx1 = 2.0f * (this.x2 * vector.x3 - this.x3 * vector.x2); + float tx2 = 2.0f * (this.x3 * vector.x1 - this.x1 * vector.x3); + float tx3 = 2.0f * (this.x1 * vector.x2 - this.x2 * vector.x1); float x1 = (vector.x1 + tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); float x2 = (vector.x2 + tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3); @@ -301,10 +239,9 @@ namespace BGC public readonly void TurnBack(in F32Vector3 vector, out F32Vector3 result) { - float multiplier = 2.0f * this.corrector; - float tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); - float tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); - float tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1); + float tx1 = 2.0f * (this.x2 * vector.x3 - this.x3 * vector.x2); + float tx2 = 2.0f * (this.x3 * vector.x1 - this.x1 * vector.x3); + float tx3 = 2.0f * (this.x1 * vector.x2 - this.x2 * vector.x1); float x1 = (vector.x1 - tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); float x2 = (vector.x2 - tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3); @@ -329,38 +266,47 @@ namespace BGC this.x1 /= module; this.x2 /= module; this.x3 /= module; - - this.corrector = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); } public static void Combine(in F32Versor second, in F32Versor first, out F32Versor result) { - float s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3); - float x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3); - float x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1); - float x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2); - - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - result.corrector = 2.0f - squareModule; - result.s0 = s0; - result.x1 = x1; - result.x2 = x2; - result.x3 = x3; - - if (squareModule < 1.0f - F32Utility.TWO_EPSYLON || 1.0f + F32Utility.TWO_EPSYLON < squareModule) - { - result.Normalize(squareModule); - } + 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 LoadIdle(out F32Versor versor) { - versor.corrector = 1.0f; versor.s0 = 1.0f; versor.x1 = 0.0f; versor.x2 = 0.0f; versor.x3 = 0.0f; } + + public static void LoadValues(float s0, float x1, float x2, float x3, out F32Versor versor) + { + versor.s0 = s0; + versor.x1 = x1; + versor.x2 = x2; + versor.x3 = x3; + + float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + if (1.0f - F32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + F32Utility.TWO_EPSYLON) + { + if (-1.0f + F32Utility.EPSYLON < s0) + { + return; + } + + versor.Reset(); + return; + } + + versor.Normalize(squareModule); + } } } diff --git a/BGC/F64Versor.cs b/BGC/F64Versor.cs index f16c91e..95292ab 100644 --- a/BGC/F64Versor.cs +++ b/BGC/F64Versor.cs @@ -24,22 +24,10 @@ namespace BGC { public struct F64Versor { - public static readonly F64Versor IDLE = new F64Versor(1.0, 0.0, 0.0, 0.0, 1.0); - private double s0; private double x1; private double x2; private double x3; - private double corrector; - - private F64Versor(double s0, double x1, double x2, double x3, double corrector) - { - this.s0 = s0; - this.x1 = x1; - this.x2 = x2; - this.x3 = x3; - this.corrector = corrector; - } public F64Versor() { @@ -47,24 +35,11 @@ namespace BGC this.x1 = 0.0; this.x2 = 0.0; this.x3 = 0.0; - this.corrector = 1.0; } public F64Versor(double s0, double x1, double x2, double x3) { - this.s0 = s0; - this.x1 = x1; - this.x2 = x2; - this.x3 = x3; - - double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - this.corrector = 2.0 - squareModule; - - if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule) - { - this.Normalize(squareModule); - } + LoadValues(s0, x1, x2, x3, out this); } public F64Versor(in F64Versor versor) @@ -73,24 +48,16 @@ namespace BGC this.x1 = versor.x1; this.x2 = versor.x2; this.x3 = versor.x3; - this.corrector = versor.corrector; } public F64Versor(in F32Versor versor) { - this.s0 = versor.GetScalar(); - this.x1 = versor.GetX1(); - this.x2 = versor.GetX2(); - this.x3 = versor.GetX3(); - - double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); - - this.corrector = 2.0 - squareModule; - - if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule) - { - this.Normalize(squareModule); - } + LoadValues( + versor.GetScalar(), + versor.GetX1(), + versor.GetX2(), + versor.GetX3(), + out this); } public readonly double GetScalar() @@ -113,11 +80,6 @@ namespace BGC return this.x3; } - public readonly double GetCorrector() - { - return this.corrector; - } - public readonly bool IsIdle() { return this.s0 <= -(1.0 - F64Utility.EPSYLON) || (1.0 - F64Utility.EPSYLON) <= this.s0; @@ -129,7 +91,6 @@ namespace BGC this.x1 = 0.0; this.x2 = 0.0; this.x3 = 0.0; - this.corrector = 1.0; } public void Invert() @@ -161,27 +122,25 @@ namespace BGC double x3x3 = this.x1 * this.x3; - double s0x1 = this.s0 * this.x1; - double s0x2 = this.s0 * this.x2; - double s0x3 = this.s0 * this.x3; + double s0x1 = 2.0 * this.s0 * this.x1; + double s0x2 = 2.0 * this.s0 * this.x2; + double s0x3 = 2.0 * this.s0 * this.x3; - double x1x2 = this.x1 * this.x2; - double x1x3 = this.x1 * this.x3; - double x2x3 = this.x2 * this.x3; + double x1x2 = 2.0 * this.x1 * this.x2; + double x1x3 = 2.0 * this.x1 * this.x3; + double x2x3 = 2.0 * this.x2 * this.x3; - double corrector2 = 2.0 * this.corrector; + matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); + matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); + matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); - matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); - matrix.r3c3 = this.corrector * ((s0s0 + x3x3) - (x1x1 + x2x2)); + matrix.r1c2 = x1x2 - s0x3; + matrix.r2c3 = x2x3 - s0x1; + matrix.r3c1 = x1x3 - s0x2; - 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); + matrix.r2c1 = x1x2 + s0x3; + matrix.r3c2 = x2x3 + s0x1; + matrix.r1c3 = x1x3 + s0x2; } public readonly void MakeReverseMatrix(out F64Matrix3x3 matrix) @@ -191,28 +150,25 @@ namespace BGC double x2x2 = this.x1 * this.x2; double x3x3 = this.x1 * this.x3; + double s0x1 = 2.0 * this.s0 * this.x1; + double s0x2 = 2.0 * this.s0 * this.x2; + double s0x3 = 2.0 * this.s0 * this.x3; - double s0x1 = this.s0 * this.x1; - double s0x2 = this.s0 * this.x2; - double s0x3 = this.s0 * this.x3; + double x1x2 = 2.0 * this.x1 * this.x2; + double x1x3 = 2.0 * this.x1 * this.x3; + double x2x3 = 2.0 * this.x2 * this.x3; - double x1x2 = this.x1 * this.x2; - double x1x3 = this.x1 * this.x3; - double x2x3 = this.x2 * this.x3; + matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); + matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); + matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); - double corrector2 = 2.0 * this.corrector; + matrix.r1c2 = x1x2 + s0x3; + matrix.r2c3 = x2x3 + s0x1; + matrix.r3c1 = x1x3 + s0x2; - matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); - matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); - matrix.r3c3 = this.corrector * ((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); + matrix.r2c1 = x1x2 - s0x3; + matrix.r3c2 = x2x3 - s0x1; + matrix.r1c3 = x1x3 - s0x2; } public void SetValues(double s0, double x1, double x2, double x3) @@ -223,13 +179,19 @@ namespace BGC this.x3 = x3; double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - this.corrector = 2.0 - squareModule; - - if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule) + + if (1.0 - F64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + F64Utility.TWO_EPSYLON) { - this.Normalize(squareModule); + if (-1.0 + F64Utility.EPSYLON < s0) + { + return; + } + + this.Reset(); + return; } + + this.Normalize(squareModule); } public void SetValues(in F64Versor versor) @@ -238,24 +200,16 @@ namespace BGC this.x1 = versor.x1; this.x2 = versor.x2; this.x3 = versor.x3; - this.corrector = versor.corrector; } public void SetValues(in F32Versor versor) { - this.s0 = versor.GetScalar(); - this.x1 = versor.GetX1(); - this.x2 = versor.GetX2(); - this.x3 = versor.GetX3(); - - double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); - - this.corrector = 2.0 - squareModule; - - if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule) - { - this.Normalize(squareModule); - } + LoadValues( + versor.GetScalar(), + versor.GetX1(), + versor.GetX2(), + versor.GetX3(), + out this); } public void SetInverted(in F64Versor versor) @@ -264,32 +218,23 @@ namespace BGC this.x1 = -versor.x1; this.x2 = -versor.x2; this.x3 = -versor.x3; - this.corrector = versor.corrector; } public void SetInverted(in F32Versor versor) { - this.s0 = versor.GetScalar(); - this.x1 = -versor.GetX1(); - this.x2 = -versor.GetX2(); - this.x3 = -versor.GetX3(); - - double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); - - this.corrector = 2.0 - squareModule; - - if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule) - { - this.Normalize(squareModule); - } + LoadValues( + versor.GetScalar(), + versor.GetX1(), + versor.GetX2(), + versor.GetX3(), + out this); } public readonly void Turn(in F64Vector3 vector, out F64Vector3 result) { - double multiplier = 2.0 * this.corrector; - double tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); - double tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); - double tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1); + double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2); + double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3); + double tx3 = 2.0 * (this.x1 * vector.x2 - this.x2 * vector.x1); double x1 = (vector.x1 + tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); double x2 = (vector.x2 + tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3); @@ -302,10 +247,9 @@ namespace BGC public readonly void TurnBack(in F64Vector3 vector, out F64Vector3 result) { - double multiplier = 2.0 * this.corrector; - double tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); - double tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); - double tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1); + double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2); + double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3); + double tx3 = 2.0 * (this.x1 * vector.x2 - this.x2 * vector.x1); double x1 = (vector.x1 - tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); double x2 = (vector.x2 - tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3); @@ -330,30 +274,47 @@ namespace BGC this.x1 /= module; this.x2 /= module; this.x3 /= module; - - this.corrector = 2.0 - (this.s0 * this.s0 + this.x1 * this.x1) - (this.x2 * this.x2 + this.x3 * this.x3); } public static void Combine(in F64Versor second, in F64Versor first, out F64Versor 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); - - result.s0 = s0; - result.x1 = x1; - result.x2 = x2; - result.x3 = x3; + 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 LoadIdle(out F64Versor versor) + { + versor.s0 = 1.0; + versor.x1 = 0.0; + versor.x2 = 0.0; + versor.x3 = 0.0; + } + + public static void LoadValues(double s0, double x1, double x2, double x3, out F64Versor versor) + { + versor.s0 = s0; + versor.x1 = x1; + versor.x2 = x2; + versor.x3 = x3; double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - result.corrector = 2.0 - squareModule; - - if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule) + + if (1.0 - F64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + F64Utility.TWO_EPSYLON) { - result.Normalize(squareModule); + if (-1.0 + F64Utility.EPSYLON < s0) + { + return; + } + + versor.Reset(); + return; } + + versor.Normalize(squareModule); } } } diff --git a/BGCDev/Program.cs b/BGCDev/Program.cs index 4d41a79..8885d2b 100644 --- a/BGCDev/Program.cs +++ b/BGCDev/Program.cs @@ -17,7 +17,7 @@ public static class Program for (int i = 0; i < amount; i++) { - versors[i] = F32Versor.IDLE; + versors[i].Reset(); } return versors; @@ -44,7 +44,7 @@ public static class Program private static void PrintVersor(in F32Versor versor) { - Console.WriteLine("({0}, {1}, {2}, {3}) / {4:E}", versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3(), versor.GetCorrector() - 1.0f); + Console.WriteLine("({0}, {1}, {2}, {3})", versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3()); } public static int Main()