From a3c9e93270939b318a0bdbcd5505e161c5daf048 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Wed, 12 Feb 2025 20:51:48 +0700 Subject: [PATCH] =?UTF-8?q?=D0=91=D0=BE=D0=BB=D1=8C=D1=88=D0=BE=D0=B5=20?= =?UTF-8?q?=D0=BF=D0=B5=D1=80=D0=B5=D1=83=D0=BF=D0=BE=D1=80=D1=8F=D0=B4?= =?UTF-8?q?=D0=BE=D1=87=D0=B8=D0=B2=D0=B0=D0=BD=D0=B8=D0=B5=20=D0=B8=D1=81?= =?UTF-8?q?=D1=85=D0=BE=D0=B4=D0=BD=D0=BE=D0=B3=D0=BE=20=D0=BA=D0=BE=D0=B4?= =?UTF-8?q?=D0=B0=20(=D0=BD=D0=B5=20=D0=BE=D0=BA=D0=BE=D0=BD=D1=87=D0=B5?= =?UTF-8?q?=D0=BD=D0=BE)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/Matrix2x2FP32.cs | 9 +- BasicGeometry/Matrix2x2FP64.cs | 11 +- BasicGeometry/Matrix3x3FP32.cs | 161 +++++++-------- BasicGeometry/Matrix3x3FP64.cs | 157 ++++++++------- BasicGeometry/QuaternionFP32.cs | 66 ++++--- BasicGeometry/QuaternionFP64.cs | 69 ++++--- BasicGeometry/UtilityFP32.cs | 2 +- BasicGeometry/UtilityFP64.cs | 2 +- BasicGeometry/Vector2FP32.cs | 44 ++--- BasicGeometry/Vector2FP64.cs | 44 ++--- BasicGeometry/Vector3FP32.cs | 45 ++--- BasicGeometry/Vector3FP64.cs | 46 ++--- BasicGeometry/VersorFP32.cs | 338 ++++++++++++++++---------------- BasicGeometry/VersorFP64.cs | 334 +++++++++++++++---------------- BasicGeometryDev/Program.cs | 12 +- 15 files changed, 694 insertions(+), 646 deletions(-) diff --git a/BasicGeometry/Matrix2x2FP32.cs b/BasicGeometry/Matrix2x2FP32.cs index 10c520b..936471c 100644 --- a/BasicGeometry/Matrix2x2FP32.cs +++ b/BasicGeometry/Matrix2x2FP32.cs @@ -59,8 +59,7 @@ namespace BasicGeometry public readonly bool IsSingular() { - float determinant = this.GetDeterminant(); - return -UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON; + return UtilityFP32.IsZero(this.GetDeterminant()); } public void Transpose() @@ -72,7 +71,7 @@ namespace BasicGeometry { float determinant = this.GetDeterminant(); - if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) + if (UtilityFP32.IsZero(determinant)) { return false; } @@ -121,7 +120,7 @@ namespace BasicGeometry this.r2c2 = d2; } - public void SetValues(in Matrix2x2FP32 matrix) + public void Set(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -130,7 +129,7 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public void SetValues(in Matrix2x2FP64 matrix) + public void Set(in Matrix2x2FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r1c2; diff --git a/BasicGeometry/Matrix2x2FP64.cs b/BasicGeometry/Matrix2x2FP64.cs index 11f20d0..4618711 100644 --- a/BasicGeometry/Matrix2x2FP64.cs +++ b/BasicGeometry/Matrix2x2FP64.cs @@ -59,8 +59,7 @@ namespace BasicGeometry public readonly bool IsSingular() { - double determinant = this.GetDeterminant(); - return -UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON; + return UtilityFP64.IsZero(this.GetDeterminant()); } public void Transpose() @@ -72,7 +71,7 @@ namespace BasicGeometry { double determinant = this.GetDeterminant(); - if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) + if (UtilityFP64.IsZero(determinant)) { return false; } @@ -121,7 +120,7 @@ namespace BasicGeometry this.r2c2 = d2; } - public void SetValues(in Matrix2x2FP64 matrix) + public void Set(in Matrix2x2FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -130,7 +129,7 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public void SetValues(in Matrix2x2FP32 matrix) + public void Set(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -159,7 +158,7 @@ namespace BasicGeometry { double determinant = matrix.GetDeterminant(); - if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) + if (UtilityFP64.IsZero(determinant)) { return false; } diff --git a/BasicGeometry/Matrix3x3FP32.cs b/BasicGeometry/Matrix3x3FP32.cs index 665c900..8201ea3 100644 --- a/BasicGeometry/Matrix3x3FP32.cs +++ b/BasicGeometry/Matrix3x3FP32.cs @@ -75,8 +75,7 @@ namespace BasicGeometry public readonly bool IsSingular() { - float determinant = this.GetDeterminant(); - return -UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON; + return UtilityFP32.IsZero(this.GetDeterminant()); } public void Transpose() @@ -90,7 +89,8 @@ namespace BasicGeometry { float determinant = this.GetDeterminant(); - if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) { + if (UtilityFP32.IsZero(determinant)) + { return false; } @@ -168,7 +168,7 @@ namespace BasicGeometry this.r2c3 = d3; } - public void SetValues(in Matrix3x3FP32 matrix) + public void Set(in Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -183,7 +183,7 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public void SetValues(in Matrix3x3FP64 matrix) + public void Set(in Matrix3x3FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r1c2; @@ -198,69 +198,6 @@ namespace BasicGeometry this.r3c3 = (float)matrix.r3c3; } - public void SetTransposedOf(in Matrix3x3FP32 matrix) - { - this.r1c1 = matrix.r1c1; - this.r2c2 = matrix.r2c2; - this.r3c3 = matrix.r3c3; - - (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); - (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); - (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); - } - - public void SetTransposedOf(in Matrix3x3FP64 matrix) - { - this.r1c1 = (float)matrix.r1c1; - this.r1c2 = (float)matrix.r2c1; - this.r1c3 = (float)matrix.r3c1; - - this.r2c1 = (float)matrix.r1c2; - this.r2c2 = (float)matrix.r2c2; - this.r2c3 = (float)matrix.r3c2; - - this.r3c1 = (float)matrix.r1c3; - this.r3c2 = (float)matrix.r2c3; - this.r3c3 = (float)matrix.r3c3; - } - - public bool SetInvertedOf(in Matrix3x3FP32 matrix) - { - float determinant = matrix.GetDeterminant(); - - if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) { - return false; - } - - float r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; - float r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; - float r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; - - float r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; - float r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; - float r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; - - float r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; - float r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; - float r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; - - float mutiplier = 1.0f / determinant; - - this.r1c1 = r1c1 * mutiplier; - this.r1c2 = r1c2 * mutiplier; - this.r1c3 = r1c3 * mutiplier; - - this.r2c1 = r2c1 * mutiplier; - this.r2c2 = r2c2 * mutiplier; - this.r2c3 = r2c3 * mutiplier; - - this.r3c1 = r3c1 * mutiplier; - this.r3c2 = r3c2 * mutiplier; - this.r3c3 = r3c3 * mutiplier; - - return true; - } - public void SetRow1(float c1, float c2, float c3) { this.r1c1 = c1; @@ -303,19 +240,53 @@ namespace BasicGeometry this.r3c3 = r3; } - public void AppendScaled(in Matrix3x3FP32 matrix, float scale) + public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed) { - this.r1c1 += matrix.r1c1* scale; - this.r1c2 += matrix.r1c2* scale; - this.r1c3 += matrix.r1c3* scale; + transposed.r1c1 = matrix.r1c1; + transposed.r2c2 = matrix.r2c2; + transposed.r3c3 = matrix.r3c3; - this.r2c1 += matrix.r2c1* scale; - this.r2c2 += matrix.r2c2* scale; - this.r2c3 += matrix.r2c3* scale; + (transposed.r1c2, transposed.r2c1) = (matrix.r2c1, matrix.r1c2); + (transposed.r1c3, transposed.r3c1) = (matrix.r3c1, matrix.r1c3); + (transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3); + } - this.r3c1 += matrix.r3c1* scale; - this.r3c2 += matrix.r3c2* scale; - this.r3c3 += matrix.r3c3* scale; + public static bool MakeInverted(in Matrix3x3FP32 matrix, out Matrix3x3FP32 inverted) + { + float determinant = matrix.GetDeterminant(); + + if (UtilityFP32.IsZero(determinant)) { + LoadZero(out inverted); + return false; + } + + float r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; + float r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; + float r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; + + float r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; + float r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; + float r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; + + float r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; + float r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; + float r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; + + float mutiplier = 1.0f / determinant; + + inverted.r1c1 = r1c1 * mutiplier; + inverted.r1c2 = r1c2 * mutiplier; + inverted.r1c3 = r1c3 * mutiplier; + + inverted.r2c1 = r2c1 * mutiplier; + inverted.r2c2 = r2c2 * mutiplier; + inverted.r2c3 = r2c3 * mutiplier; + + inverted.r3c1 = r3c1 * mutiplier; + inverted.r3c2 = r3c2 * mutiplier; + inverted.r3c3 = r3c3 * mutiplier; + + return true; } public static void Add(in Matrix3x3FP32 matrix1, in Matrix3x3FP32 matrix2, out Matrix3x3FP32 sum) @@ -333,6 +304,21 @@ namespace BasicGeometry sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; } + public static void AddScaled(in Matrix3x3FP32 basicMatrix, in Matrix3x3FP32 scalableMatrix, float scale, out Matrix3x3FP32 sum) + { + sum.r1c1 = basicMatrix.r1c1 + scalableMatrix.r1c1 * scale; + sum.r1c2 = basicMatrix.r1c2 + scalableMatrix.r1c2 * scale; + sum.r1c3 = basicMatrix.r1c3 + scalableMatrix.r1c3 * scale; + + sum.r2c1 = basicMatrix.r2c1 + scalableMatrix.r2c1 * scale; + sum.r2c2 = basicMatrix.r2c2 + scalableMatrix.r2c2 * scale; + sum.r2c3 = basicMatrix.r2c3 + scalableMatrix.r2c3 * scale; + + sum.r3c1 = basicMatrix.r3c1 + scalableMatrix.r3c1 * scale; + sum.r3c2 = basicMatrix.r3c2 + scalableMatrix.r3c2 * scale; + sum.r3c3 = basicMatrix.r3c3 + scalableMatrix.r3c3 * scale; + } + public static void Subtract(in Matrix3x3FP32 minuend, in Matrix3x3FP32 subtrahend, out Matrix3x3FP32 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; @@ -348,6 +334,21 @@ namespace BasicGeometry difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; } + public static void SubtractScaled(in Matrix3x3FP32 basicMatrix, in Matrix3x3FP32 scalableMatrix, float scale, out Matrix3x3FP32 difference) + { + difference.r1c1 = basicMatrix.r1c1 - scalableMatrix.r1c1 * scale; + difference.r1c2 = basicMatrix.r1c2 - scalableMatrix.r1c2 * scale; + difference.r1c3 = basicMatrix.r1c3 - scalableMatrix.r1c3 * scale; + + difference.r2c1 = basicMatrix.r2c1 - scalableMatrix.r2c1 * scale; + difference.r2c2 = basicMatrix.r2c2 - scalableMatrix.r2c2 * scale; + difference.r2c3 = basicMatrix.r2c3 - scalableMatrix.r2c3 * scale; + + difference.r3c1 = basicMatrix.r3c1 - scalableMatrix.r3c1 * scale; + difference.r3c2 = basicMatrix.r3c2 - scalableMatrix.r3c2 * scale; + difference.r3c3 = basicMatrix.r3c3 - scalableMatrix.r3c3 * scale; + } + public static void Multiply(in Matrix3x3FP32 multiplicand, float multiplier, out Matrix3x3FP32 product) { product.r1c1 = multiplicand.r1c1 * multiplier; @@ -368,7 +369,7 @@ namespace BasicGeometry Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetRightProduct(in Matrix3x3FP32 matrix, in Vector3FP32 vector, out Vector3FP32 result) + public static void RightProduct(in Matrix3x3FP32 matrix, in Vector3FP32 vector, out Vector3FP32 result) { float x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; float x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3; @@ -379,7 +380,7 @@ namespace BasicGeometry result.x3 = x3; } - public static void GetLeftProduct(in Vector3FP32 vector, in Matrix3x3FP32 matrix, out Vector3FP32 result) + public static void LeftProduct(in Vector3FP32 vector, in Matrix3x3FP32 matrix, out Vector3FP32 result) { float x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1; float x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2; diff --git a/BasicGeometry/Matrix3x3FP64.cs b/BasicGeometry/Matrix3x3FP64.cs index 434cd85..b9b016e 100644 --- a/BasicGeometry/Matrix3x3FP64.cs +++ b/BasicGeometry/Matrix3x3FP64.cs @@ -75,8 +75,7 @@ namespace BasicGeometry public readonly bool IsSingular() { - double determinant = this.GetDeterminant(); - return -UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON; + return UtilityFP64.IsZero(this.GetDeterminant()); } public void Transpose() @@ -90,7 +89,8 @@ namespace BasicGeometry { double determinant = this.GetDeterminant(); - if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) { + if (UtilityFP64.IsZero(determinant)) + { return false; } @@ -168,7 +168,7 @@ namespace BasicGeometry this.r2c3 = d3; } - public void SetValues(Matrix3x3FP64 matrix) + public void Set(Matrix3x3FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -183,7 +183,7 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public void SetValues(Matrix3x3FP32 matrix) + public void Set(Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -198,65 +198,6 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public void SetTransposedOf(in Matrix3x3FP64 matrix) - { - this.r1c1 = matrix.r1c1; - this.r2c2 = matrix.r2c2; - this.r3c3 = matrix.r3c3; - - (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); - (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); - (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); - } - - public void SetTransposedOf(in Matrix3x3FP32 matrix) - { - this.r1c1 = matrix.r1c1; - this.r2c2 = matrix.r2c2; - this.r3c3 = matrix.r3c3; - - (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); - (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); - (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); - } - - public bool SetInvertedOf(in Matrix3x3FP64 matrix) - { - double determinant = matrix.GetDeterminant(); - - if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) { - return false; - } - - double r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; - double r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; - double r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; - - double r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; - double r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; - double r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; - - double r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; - double r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; - double r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; - - double mutiplier = 1.0 / determinant; - - this.r1c1 = r1c1 * mutiplier; - this.r1c2 = r1c2 * mutiplier; - this.r1c3 = r1c3 * mutiplier; - - this.r2c1 = r2c1 * mutiplier; - this.r2c2 = r2c2 * mutiplier; - this.r2c3 = r2c3 * mutiplier; - - this.r3c1 = r3c1 * mutiplier; - this.r3c2 = r3c2 * mutiplier; - this.r3c3 = r3c3 * mutiplier; - - return true; - } - public void SetRow1(double c1, double c2, double c3) { this.r1c1 = c1; @@ -299,19 +240,53 @@ namespace BasicGeometry this.r3c3 = r3; } - public void AppendScaled(in Matrix3x3FP64 matrix, double scale) + public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed) { - this.r1c1 += matrix.r1c1 * scale; - this.r1c2 += matrix.r1c2 * scale; - this.r1c3 += matrix.r1c3 * scale; + transposed.r1c1 = matrix.r1c1; + transposed.r2c2 = matrix.r2c2; + transposed.r3c3 = matrix.r3c3; - this.r2c1 += matrix.r2c1 * scale; - this.r2c2 += matrix.r2c2 * scale; - this.r2c3 += matrix.r2c3 * scale; + (transposed.r1c2, transposed.r2c1) = (matrix.r2c1, matrix.r1c2); + (transposed.r1c3, transposed.r3c1) = (matrix.r3c1, matrix.r1c3); + (transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3); + } - this.r3c1 += matrix.r3c1 * scale; - this.r3c2 += matrix.r3c2 * scale; - this.r3c3 += matrix.r3c3 * scale; + public static bool MakeInverted(in Matrix3x3FP64 matrix, out Matrix3x3FP64 inverted) + { + double determinant = matrix.GetDeterminant(); + + if (UtilityFP64.IsZero(determinant)) { + LoadZero(out inverted); + return false; + } + + double r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; + double r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; + double r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; + + double r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; + double r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; + double r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; + + double r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; + double r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; + double r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; + + double mutiplier = 1.0 / determinant; + + inverted.r1c1 = r1c1 * mutiplier; + inverted.r1c2 = r1c2 * mutiplier; + inverted.r1c3 = r1c3 * mutiplier; + + inverted.r2c1 = r2c1 * mutiplier; + inverted.r2c2 = r2c2 * mutiplier; + inverted.r2c3 = r2c3 * mutiplier; + + inverted.r3c1 = r3c1 * mutiplier; + inverted.r3c2 = r3c2 * mutiplier; + inverted.r3c3 = r3c3 * mutiplier; + + return true; } public static void Add(in Matrix3x3FP64 matrix1, in Matrix3x3FP64 matrix2, out Matrix3x3FP64 sum) @@ -329,6 +304,21 @@ namespace BasicGeometry sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; } + public static void AddScaled(in Matrix3x3FP64 basicMatrix, in Matrix3x3FP64 scalableMatrix, double scale, out Matrix3x3FP64 sum) + { + sum.r1c1 = basicMatrix.r1c1 + scalableMatrix.r1c1 * scale; + sum.r1c2 = basicMatrix.r1c2 + scalableMatrix.r1c2 * scale; + sum.r1c3 = basicMatrix.r1c3 + scalableMatrix.r1c3 * scale; + + sum.r2c1 = basicMatrix.r2c1 + scalableMatrix.r2c1 * scale; + sum.r2c2 = basicMatrix.r2c2 + scalableMatrix.r2c2 * scale; + sum.r2c3 = basicMatrix.r2c3 + scalableMatrix.r2c3 * scale; + + sum.r3c1 = basicMatrix.r3c1 + scalableMatrix.r3c1 * scale; + sum.r3c2 = basicMatrix.r3c2 + scalableMatrix.r3c2 * scale; + sum.r3c3 = basicMatrix.r3c3 + scalableMatrix.r3c3 * scale; + } + public static void Subtract(in Matrix3x3FP64 minuend, in Matrix3x3FP64 subtrahend, out Matrix3x3FP64 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; @@ -344,6 +334,21 @@ namespace BasicGeometry difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; } + public static void SubtractScaled(in Matrix3x3FP64 basicMatrix, in Matrix3x3FP64 scalableMatrix, double scale, out Matrix3x3FP64 difference) + { + difference.r1c1 = basicMatrix.r1c1 - scalableMatrix.r1c1 * scale; + difference.r1c2 = basicMatrix.r1c2 - scalableMatrix.r1c2 * scale; + difference.r1c3 = basicMatrix.r1c3 - scalableMatrix.r1c3 * scale; + + difference.r2c1 = basicMatrix.r2c1 - scalableMatrix.r2c1 * scale; + difference.r2c2 = basicMatrix.r2c2 - scalableMatrix.r2c2 * scale; + difference.r2c3 = basicMatrix.r2c3 - scalableMatrix.r2c3 * scale; + + difference.r3c1 = basicMatrix.r3c1 - scalableMatrix.r3c1 * scale; + difference.r3c2 = basicMatrix.r3c2 - scalableMatrix.r3c2 * scale; + difference.r3c3 = basicMatrix.r3c3 - scalableMatrix.r3c3 * scale; + } + public static void Multiply(in Matrix3x3FP64 multiplicand, double multiplier, out Matrix3x3FP64 product) { product.r1c1 = multiplicand.r1c1 * multiplier; @@ -364,7 +369,7 @@ namespace BasicGeometry Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetRightProduct(in Matrix3x3FP64 matrix, in Vector3FP64 vector, out Vector3FP64 result) + public static void RightProduct(in Matrix3x3FP64 matrix, in Vector3FP64 vector, out Vector3FP64 result) { double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; double x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3; @@ -375,7 +380,7 @@ namespace BasicGeometry result.x3 = x3; } - public static void GetLeftProduct(in Vector3FP64 vector, in Matrix3x3FP64 matrix, out Vector3FP64 result) + public static void LeftProduct(in Vector3FP64 vector, in Matrix3x3FP64 matrix, out Vector3FP64 result) { double x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1; double x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2; diff --git a/BasicGeometry/QuaternionFP32.cs b/BasicGeometry/QuaternionFP32.cs index 4d16ec3..d4b9232 100644 --- a/BasicGeometry/QuaternionFP32.cs +++ b/BasicGeometry/QuaternionFP32.cs @@ -30,24 +30,24 @@ namespace BasicGeometry this.x3 = (float)quaternion.x3; } - public readonly float GetSquareModule() + public readonly float GetSquareModulus() { return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); } - public readonly float GetModule() + public readonly float GetModulus() { - return MathF.Sqrt(this.GetSquareModule()); + return MathF.Sqrt(this.GetSquareModulus()); } public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP32.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP32.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -81,7 +81,7 @@ namespace BasicGeometry this.x3 = x3; } - public void SetValues(in QuaternionFP32 quaternion) + public void Set(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -97,12 +97,12 @@ namespace BasicGeometry this.x3 = (float)quaternion.x3; } - public void SetConjugateOf(in QuaternionFP32 quaternion) + public static void MakeConjugate(in QuaternionFP32 quaternion, out QuaternionFP32 conjugate) { - this.s0 = quaternion.s0; - this.x1 = -quaternion.x1; - this.x2 = -quaternion.x2; - this.x3 = -quaternion.x3; + conjugate.s0 = quaternion.s0; + conjugate.x1 = -quaternion.x1; + conjugate.x2 = -quaternion.x2; + conjugate.x3 = -quaternion.x3; } public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix) @@ -112,16 +112,15 @@ namespace BasicGeometry float x2x2 = this.x2 * this.x2; float x3x3 = this.x3 * this.x3; - float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) { Matrix3x3FP32.LoadIdentity(out matrix); return; } - float corrector1 = 1.0f / squareModule; - float corrector2 = 2.0f * corrector1; + float corrector1 = 1.0f / squareModulus; float s0x1 = this.s0 * this.x1; float s0x2 = this.s0 * this.x2; @@ -130,6 +129,8 @@ namespace BasicGeometry float x1x3 = this.x1 * this.x3; float x2x3 = this.x2 * this.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)); @@ -150,16 +151,15 @@ namespace BasicGeometry float x2x2 = this.x2 * this.x2; float x3x3 = this.x3 * this.x3; - float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) { Matrix3x3FP32.LoadIdentity(out matrix); return; } - float corrector1 = 1.0f / squareModule; - float corrector2 = 2.0f * corrector1; + float corrector1 = 1.0f / squareModulus; float s0x1 = this.s0 * this.x1; float s0x2 = this.s0 * this.x2; @@ -168,6 +168,8 @@ namespace BasicGeometry float x1x3 = this.x1 * this.x3; float x2x3 = this.x2 * this.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)); @@ -199,15 +201,33 @@ namespace BasicGeometry 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); + 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 bool AreClose(QuaternionFP32 quaternion1, 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; + } } } diff --git a/BasicGeometry/QuaternionFP64.cs b/BasicGeometry/QuaternionFP64.cs index 13f5dd6..ba260a8 100644 --- a/BasicGeometry/QuaternionFP64.cs +++ b/BasicGeometry/QuaternionFP64.cs @@ -1,5 +1,4 @@ using System; -using System.Numerics; namespace BasicGeometry { @@ -31,24 +30,24 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public readonly double GetSquareModule() + public readonly double GetSquareModulus() { return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); } - public readonly double GetModule() + public readonly double GetModulus() { - return Math.Sqrt(this.GetSquareModule()); + return Math.Sqrt(this.GetSquareModulus()); } public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP64.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP64.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -82,7 +81,7 @@ namespace BasicGeometry this.x3 = x3; } - public void SetValues(in QuaternionFP32 quaternion) + public void Set(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -90,7 +89,7 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public void SetValues(in QuaternionFP64 quaternion) + public void Set(in QuaternionFP64 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -98,12 +97,12 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public void SetConjugateOf(in QuaternionFP64 quaternion) + public static void MakeConjugate(in QuaternionFP64 quaternion, out QuaternionFP64 conjugate) { - this.s0 = quaternion.s0; - this.x1 = -quaternion.x1; - this.x2 = -quaternion.x2; - this.x3 = -quaternion.x3; + conjugate.s0 = quaternion.s0; + conjugate.x1 = -quaternion.x1; + conjugate.x2 = -quaternion.x2; + conjugate.x3 = -quaternion.x3; } public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix) @@ -113,16 +112,15 @@ namespace BasicGeometry double x2x2 = this.x2 * this.x2; double x3x3 = this.x3 * this.x3; - double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - if (squareModule <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus)) { Matrix3x3FP64.LoadIdentity(out matrix); return; } - double corrector1 = 1.0 / squareModule; - double corrector2 = 2.0 * corrector1; + double corrector1 = 1.0 / squareModulus; double s0x1 = this.s0 * this.x1; double s0x2 = this.s0 * this.x2; @@ -131,6 +129,8 @@ namespace BasicGeometry double x1x3 = this.x1 * this.x3; double x2x3 = this.x2 * this.x3; + double corrector2 = 2.0 * corrector1; + matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3)); matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3)); matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2)); @@ -151,16 +151,15 @@ namespace BasicGeometry double x2x2 = this.x2 * this.x2; double x3x3 = this.x3 * this.x3; - double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - if (squareModule <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus)) { Matrix3x3FP64.LoadIdentity(out matrix); return; } - double corrector1 = 1.0 / squareModule; - double corrector2 = 2.0 * corrector1; + double corrector1 = 1.0 / squareModulus; double s0x1 = this.s0 * this.x1; double s0x2 = this.s0 * this.x2; @@ -169,6 +168,8 @@ namespace BasicGeometry double x1x3 = this.x1 * this.x3; double x2x3 = this.x2 * this.x3; + double corrector2 = 2.0 * corrector1; + matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3)); matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3)); matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2)); @@ -200,15 +201,33 @@ namespace BasicGeometry public static void Multiply(in QuaternionFP64 left, in QuaternionFP64 right, out QuaternionFP64 product) { - double s0 = (left.s0 * right.s0 - left.x1 * right.x1) - (left.x2 * right.x2 + left.x3 * right.x3); - double x1 = (left.x1 * right.s0 + left.s0 * right.x1) - (left.x3 * right.x2 - left.x2 * right.x3); - double x2 = (left.x2 * right.s0 + left.s0 * right.x2) - (left.x1 * right.x3 - left.x3 * right.x1); - double x3 = (left.x3 * right.s0 + left.s0 * right.x3) - (left.x2 * right.x1 - left.x1 * right.x2); + double s0 = left.s0 * right.s0 - left.x1 * right.x1 - (left.x2 * right.x2 + left.x3 * right.x3); + double x1 = left.x1 * right.s0 + left.s0 * right.x1 - (left.x3 * right.x2 - left.x2 * right.x3); + double x2 = left.x2 * right.s0 + left.s0 * right.x2 - (left.x1 * right.x3 - left.x3 * right.x1); + double 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 bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2) + { + double ds0 = quaternion1.s0 - quaternion2.s0; + double dx1 = quaternion1.x1 - quaternion2.x1; + double dx2 = quaternion1.x2 - quaternion2.x2; + double dx3 = quaternion1.x3 - quaternion2.x3; + + double squareModulus1 = quaternion1.GetSquareModulus(); + double squareModulus2 = quaternion2.GetSquareModulus(); + double squareDistance = (ds0 * ds0 + dx1 * dx1) + (dx2 * dx2 + dx3 * dx3); + + if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) { + return squareDistance <= UtilityFP64.SQUARE_EPSYLON; + } + + return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2; + } } } diff --git a/BasicGeometry/UtilityFP32.cs b/BasicGeometry/UtilityFP32.cs index abc272b..a326deb 100644 --- a/BasicGeometry/UtilityFP32.cs +++ b/BasicGeometry/UtilityFP32.cs @@ -26,7 +26,7 @@ namespace BasicGeometry return (1.0f - EPSYLON) <= value && value <= (1.0f + EPSYLON); } - public static bool IsSqareValueUnit(float square) + public static bool IsSqareUnit(float square) { return (1.0f - 2.0f * EPSYLON) <= square && square <= (1.0f + 2.0f * EPSYLON); } diff --git a/BasicGeometry/UtilityFP64.cs b/BasicGeometry/UtilityFP64.cs index a940fb6..ba61ee9 100644 --- a/BasicGeometry/UtilityFP64.cs +++ b/BasicGeometry/UtilityFP64.cs @@ -26,7 +26,7 @@ namespace BasicGeometry return (1.0 - EPSYLON) <= value && value <= (1.0 + EPSYLON); } - public static bool IsSqareValueUnit(double square) + public static bool IsSqareUnit(double square) { return (1.0 - 2.0 * EPSYLON) <= square && square <= (1.0 + 2.0 * EPSYLON); } diff --git a/BasicGeometry/Vector2FP32.cs b/BasicGeometry/Vector2FP32.cs index 64f9854..9de1992 100644 --- a/BasicGeometry/Vector2FP32.cs +++ b/BasicGeometry/Vector2FP32.cs @@ -48,32 +48,32 @@ namespace BasicGeometry this.x2 = (float)vector.x2; } - public readonly float GetSquareModule() + public readonly float GetSquareModulus() { return this.x1 * this.x1 + this.x2 * this.x2; } - public readonly float GetModule() + public readonly float GetModulus() { - return MathF.Sqrt(this.GetSquareModule()); + return MathF.Sqrt(this.GetSquareModulus()); } public int Normalize() { - float squareModule = this.GetSquareModule(); + float squareModulus = this.GetSquareModulus(); - if (UtilityFP32.IsSqareValueUnit(squareModule)) + if (UtilityFP32.IsSqareUnit(squareModulus)) { return 1; } - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) { this.Reset(); return 0; } - float multiplier = MathF.Sqrt(1.0f / squareModule); + float multiplier = MathF.Sqrt(1.0f / squareModulus); this.x1 *= multiplier; this.x2 *= multiplier; @@ -89,12 +89,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP32.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP32.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -191,21 +191,21 @@ namespace BasicGeometry public static float GetAngle(in Vector2FP32 vector1, in Vector2FP32 vector2, AngleUnit unit) { - float squareModule1 = vector1.GetSquareModule(); + float squareModulus1 = vector1.GetSquareModulus(); - if (squareModule1 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float squareModule2 = vector2.GetSquareModule(); + float squareModulus2 = vector2.GetSquareModulus(); - if (squareModule2 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float cosine = Vector2FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); + float cosine = Vector2FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2); if (1.0f - UtilityFP32.EPSYLON <= cosine) { @@ -235,22 +235,22 @@ namespace BasicGeometry public static bool AreEqual(in Vector2FP32 vector1, in Vector2FP32 vector2) { - float squareModule1 = vector1.GetSquareModule(); - float squareModule2 = vector2.GetSquareModule(); - float squareModule3 = GetSquareDistance(vector1, vector2); + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + float squareModulus3 = GetSquareDistance(vector1, vector2); // 2.0f means dimension amount - if (squareModule1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (2.0f * UtilityFP32.SQUARE_EPSYLON); + return squareModulus3 < (2.0f * UtilityFP32.SQUARE_EPSYLON); } - if (squareModule1 <= squareModule2) + if (squareModulus1 <= squareModulus2) { - return squareModule3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule2; + return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus2; } - return squareModule3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule1; + return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus1; } } } diff --git a/BasicGeometry/Vector2FP64.cs b/BasicGeometry/Vector2FP64.cs index 2e40dfe..ab25936 100644 --- a/BasicGeometry/Vector2FP64.cs +++ b/BasicGeometry/Vector2FP64.cs @@ -48,32 +48,32 @@ namespace BasicGeometry this.x2 = vector.x2; } - public readonly double GetSquareModule() + public readonly double GetSquareModulus() { return this.x1 * this.x1 + this.x2 * this.x2; } - public readonly double GetModule() + public readonly double GetModulus() { - return Math.Sqrt(this.GetSquareModule()); + return Math.Sqrt(this.GetSquareModulus()); } public int Normalize() { - double squareModule = this.GetSquareModule(); + double squareModulus = this.GetSquareModulus(); - if (UtilityFP64.IsSqareValueUnit(squareModule)) + if (UtilityFP64.IsSqareUnit(squareModulus)) { return 1; } - if (squareModule <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON) { this.Reset(); return 0; } - double multiplier = Math.Sqrt(1.0 / squareModule); + double multiplier = Math.Sqrt(1.0 / squareModulus); this.x1 *= multiplier; this.x2 *= multiplier; @@ -89,12 +89,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP64.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP64.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -191,21 +191,21 @@ namespace BasicGeometry public static double GetAngle(in Vector2FP64 vector1, in Vector2FP64 vector2, AngleUnit unit) { - double squareModule1 = vector1.GetSquareModule(); + double squareModulus1 = vector1.GetSquareModulus(); - if (squareModule1 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double squareModule2 = vector2.GetSquareModule(); + double squareModulus2 = vector2.GetSquareModulus(); - if (squareModule2 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); + double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2); if (1.0 - UtilityFP64.EPSYLON <= cosine) { @@ -235,22 +235,22 @@ namespace BasicGeometry public static bool AreEqual(in Vector2FP64 vector1, in Vector2FP64 vector2) { - double squareModule1 = vector1.GetSquareModule(); - double squareModule2 = vector2.GetSquareModule(); - double squareModule3 = GetSquareDistance(vector1, vector2); + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + double squareModulus3 = GetSquareDistance(vector1, vector2); // 2.0 means dimension amount - if (squareModule1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (2.0 * UtilityFP64.SQUARE_EPSYLON); + return squareModulus3 < (2.0 * UtilityFP64.SQUARE_EPSYLON); } - if (squareModule1 <= squareModule2) + if (squareModulus1 <= squareModulus2) { - return squareModule3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule2; + return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus2; } - return squareModule3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule1; + return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus1; } } } diff --git a/BasicGeometry/Vector3FP32.cs b/BasicGeometry/Vector3FP32.cs index 54d38a0..5cd65ea 100644 --- a/BasicGeometry/Vector3FP32.cs +++ b/BasicGeometry/Vector3FP32.cs @@ -52,32 +52,32 @@ namespace BasicGeometry this.x3 = (float)vector.x3; } - public readonly float GetSquareModule() + public readonly float GetSquareModulus() { return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3; } - public readonly float GetModule() + public readonly float GetModulus() { - return MathF.Sqrt(this.GetSquareModule()); + return MathF.Sqrt(this.GetSquareModulus()); } public int Normalize() { - float squareModule = this.GetSquareModule(); + float squareModulus = this.GetSquareModulus(); - if (UtilityFP32.IsSqareValueUnit(squareModule)) + if (UtilityFP32.IsSqareUnit(squareModulus)) { return 1; } - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) { this.Reset(); return 0; } - float multiplier = MathF.Sqrt(1.0f / squareModule); + float multiplier = MathF.Sqrt(1.0f / squareModulus); this.x1 *= multiplier; this.x2 *= multiplier; @@ -95,12 +95,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP32.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP32.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -233,21 +233,21 @@ namespace BasicGeometry public static float GetAngle(in Vector3FP32 vector1, in Vector3FP32 vector2, AngleUnit unit) { - float squareModule1 = vector1.GetSquareModule(); + float squareModulus1 = vector1.GetSquareModulus(); - if (squareModule1 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float squareModule2 = vector2.GetSquareModule(); + float squareModulus2 = vector2.GetSquareModulus(); - if (squareModule2 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float cosine = Vector3FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); + float cosine = Vector3FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2); if (1.0f - UtilityFP32.EPSYLON <= cosine) { @@ -278,22 +278,17 @@ namespace BasicGeometry public static bool AreEqual(in Vector3FP32 vector1, in Vector3FP32 vector2) { - float squareModule1 = vector1.GetSquareModule(); - float squareModule2 = vector2.GetSquareModule(); - float squareModule3 = GetSquareDistance(vector1, vector2); + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + float squareDistance = GetSquareDistance(vector1, vector2); // 3.0f means dimension amount - if (squareModule1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (3.0f * UtilityFP32.SQUARE_EPSYLON); + return squareDistance <= UtilityFP32.SQUARE_EPSYLON; } - if (squareModule1 <= squareModule2) - { - return squareModule3 <= (3.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule2; - } - - return squareModule3 <= (3.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule1; + return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2; } } } diff --git a/BasicGeometry/Vector3FP64.cs b/BasicGeometry/Vector3FP64.cs index 1823f86..7e42050 100644 --- a/BasicGeometry/Vector3FP64.cs +++ b/BasicGeometry/Vector3FP64.cs @@ -52,33 +52,33 @@ namespace BasicGeometry this.x3 = vector.x3; } - public readonly double GetSquareModule() + public readonly double GetSquareModulus() { return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3; } - public readonly double GetModule() + public readonly double GetModulus() { - return Math.Sqrt(this.GetSquareModule()); + return Math.Sqrt(this.GetSquareModulus()); } public int Normalize() { - double squareModule = this.GetSquareModule(); + double squareModulus = this.GetSquareModulus(); - if (UtilityFP64.IsSqareValueUnit(squareModule)) + if (UtilityFP64.IsSqareUnit(squareModulus)) { return 1; } - if (squareModule <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON) { this.Reset(); return 0; } - double multiplier = Math.Sqrt(1.0 / squareModule); + double multiplier = Math.Sqrt(1.0 / squareModulus); this.x1 *= multiplier; this.x2 *= multiplier; @@ -96,12 +96,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP64.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP64.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -234,21 +234,21 @@ namespace BasicGeometry public static double GetAngle(in Vector3FP64 vector1, in Vector3FP64 vector2, AngleUnit unit) { - double squareModule1 = vector1.GetSquareModule(); + double squareModulus1 = vector1.GetSquareModulus(); - if (squareModule1 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double squareModule2 = vector2.GetSquareModule(); + double squareModulus2 = vector2.GetSquareModulus(); - if (squareModule2 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double cosine = Vector3FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); + double cosine = Vector3FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2); if (1.0 - UtilityFP64.EPSYLON <= cosine) { @@ -279,22 +279,16 @@ namespace BasicGeometry public static bool AreEqual(in Vector3FP64 vector1, in Vector3FP64 vector2) { - double squareModule1 = vector1.GetSquareModule(); - double squareModule2 = vector2.GetSquareModule(); - double squareModule3 = GetSquareDistance(vector1, vector2); + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + double squareDistance = GetSquareDistance(vector1, vector2); - // 3.0 means dimension amount - if (squareModule1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (3.0 * UtilityFP64.SQUARE_EPSYLON); + return squareDistance <= UtilityFP64.SQUARE_EPSYLON; } - if (squareModule1 <= squareModule2) - { - return squareModule3 <= (3.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule2; - } - - return squareModule3 <= (3.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule1; + return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2; } } } diff --git a/BasicGeometry/VersorFP32.cs b/BasicGeometry/VersorFP32.cs index 30a35bb..9aaf3cd 100644 --- a/BasicGeometry/VersorFP32.cs +++ b/BasicGeometry/VersorFP32.cs @@ -30,7 +30,17 @@ namespace BasicGeometry public VersorFP32(float s0, float x1, float x2, float x3) { - LoadValues(s0, x1, x2, x3, out this); + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + + float squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); + + if (!UtilityFP32.IsSqareUnit(squareModulus)) + { + this.Normalize(squareModulus); + } } public VersorFP32(in VersorFP32 versor) @@ -47,6 +57,13 @@ namespace BasicGeometry this.x1 = (float)versor.GetX1(); this.x2 = (float)versor.GetX2(); this.x3 = (float)versor.GetX3(); + + float squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); + + if (!UtilityFP32.IsSqareUnit(squareModulus)) + { + this.Normalize(squareModulus); + } } public readonly float GetScalar() @@ -69,6 +86,20 @@ namespace BasicGeometry return this.x3; } + public readonly float GetAngle(AngleUnit unit) + { + if (this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || 1.0f - UtilityFP32.EPSYLON <= this.s0) { + return 0.0f; + } + + if (UtilityFP32.IsZero(this.s0)) + { + return AngleFP32.GetHalfCircle(unit); + } + + return RadianFP32.ToUnits(2.0f * MathF.Acos(this.s0), unit); + } + public readonly bool IsIdle() { return this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || (1.0f - UtilityFP32.EPSYLON) <= this.s0; @@ -99,38 +130,116 @@ namespace BasicGeometry this.x3 = -this.x3; } - public readonly float GetAngle(AngleUnit unit) + public void SetValues(float s0, float x1, float x2, float x3) { - if (this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || 1.0f - UtilityFP32.EPSYLON <= this.s0) { - return 0.0f; - } + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; - if (UtilityFP32.IsZero(this.s0)) + float squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + if (!UtilityFP32.IsSqareUnit(squareModulus)) { - return AngleFP32.GetHalfCircle(unit); + this.Normalize(squareModulus); } - - return RadianFP32.ToUnits(2.0f * MathF.Acos(this.s0), unit); } - public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix) + public void Set(in VersorFP32 versor) { - float s0s0 = this.s0 * this.s0; - float x1x1 = this.x1 * this.x1; - float x2x2 = this.x1 * this.x2; - float x3x3 = this.x1 * this.x3; + this.s0 = versor.s0; + this.x1 = versor.x1; + this.x2 = versor.x2; + this.x3 = versor.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; + public void Set(in VersorFP64 versor) + { + this.SetValues( + (float) versor.GetScalar(), + (float) versor.GetX1(), + (float) versor.GetX2(), + (float) versor.GetX3()); + } - float x1x2 = 2.0f * this.x1 * this.x2; - float x1x3 = 2.0f * this.x1 * this.x3; - float x2x3 = 2.0f * this.x2 * this.x3; + private void Normalize(float squareModulus) + { + if (!float.IsFinite(squareModulus) || squareModulus <= UtilityFP32.SQUARE_EPSYLON) + { + this.Reset(); + return; + } - matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + this.s0 *= multiplier; + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; + } + + public static void Combine(in VersorFP32 second, in VersorFP32 first, out VersorFP32 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 squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3); + + result.s0 = s0; + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + + if (!UtilityFP32.IsSqareUnit(squareModulus)) + { + result.Normalize(squareModulus); + } + } + + public static void MakeInverted(in VersorFP32 versor, out VersorFP32 conjugate) + { + conjugate.s0 = versor.s0; + conjugate.x1 = -versor.x1; + conjugate.x2 = -versor.x2; + conjugate.x3 = -versor.x3; + } + + public static void MakeShortened(in VersorFP32 versor, out VersorFP32 shortened) + { + if (versor.s0 < 0.0f) { + 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 VersorFP32 versor, out Matrix3x3FP32 matrix) + { + float s0s0 = versor.s0 * versor.s0; + float x1x1 = versor.x1 * versor.x1; + float x2x2 = versor.x1 * versor.x2; + float x3x3 = versor.x1 * versor.x3; + + float s0x1 = 2.0f * versor.s0 * versor.x1; + float s0x2 = 2.0f * versor.s0 * versor.x2; + float s0x3 = 2.0f * versor.s0 * versor.x3; + + float x1x2 = 2.0f * versor.x1 * versor.x2; + float x1x3 = 2.0f * versor.x1 * versor.x3; + float x2x3 = 2.0f * versor.x2 * versor.x3; + + matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); + matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); + matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); matrix.r1c2 = x1x2 - s0x3; matrix.r2c3 = x2x3 - s0x1; @@ -141,24 +250,24 @@ namespace BasicGeometry matrix.r1c3 = x1x3 + s0x2; } - public readonly void MakeReverseMatrix(out Matrix3x3FP32 matrix) + public static void MakeReverseMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix) { - float s0s0 = this.s0 * this.s0; - float x1x1 = this.x1 * this.x1; - float x2x2 = this.x1 * this.x2; - float x3x3 = this.x1 * this.x3; + float s0s0 = versor.s0 * versor.s0; + float x1x1 = versor.x1 * versor.x1; + float x2x2 = versor.x1 * versor.x2; + float x3x3 = versor.x1 * versor.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 = 2.0f * versor.s0 * versor.x1; + float s0x2 = 2.0f * versor.s0 * versor.x2; + float s0x3 = 2.0f * versor.s0 * versor.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 = 2.0f * versor.x1 * versor.x2; + float x1x3 = 2.0f * versor.x1 * versor.x3; + float x2x3 = 2.0f * versor.x2 * versor.x3; - matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); + matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); + matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); matrix.r1c2 = x1x2 + s0x3; matrix.r2c3 = x2x3 + s0x1; @@ -169,165 +278,56 @@ namespace BasicGeometry matrix.r1c3 = x1x3 - s0x2; } - public void SetValues(float s0, float x1, float x2, float x3) + public static void Turn(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result) { - this.s0 = s0; - this.x1 = x1; - this.x2 = x2; - this.x3 = x3; + float tx1 = 2.0f * (versor.x2 * vector.x3 - versor.x3 * vector.x2); + float tx2 = 2.0f * (versor.x3 * vector.x1 - versor.x1 * vector.x3); + float tx3 = 2.0f * (versor.x1 * vector.x2 - versor.x2 * vector.x1); - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (!UtilityFP32.IsSqareValueUnit(squareModule)) - { - this.Normalize(squareModule); - } - } - - public void SetValues(in VersorFP32 versor) - { - this.s0 = versor.s0; - this.x1 = versor.x1; - this.x2 = versor.x2; - this.x3 = versor.x3; - } - - public void SetValues(in VersorFP64 versor) - { - this.SetValues( - (float) versor.GetScalar(), - (float) versor.GetX1(), - (float) versor.GetX2(), - (float) versor.GetX3()); - } - - public void SetInverted(in VersorFP32 versor) - { - this.s0 = versor.s0; - this.x1 = -versor.x1; - this.x2 = -versor.x2; - this.x3 = -versor.x3; - } - - public void SetInverted(in VersorFP64 versor) - { - this.SetValues( - (float)versor.GetScalar(), - (float)-versor.GetX1(), - (float)-versor.GetX2(), - (float)-versor.GetX3()); - } - - public void SetShortened(in VersorFP32 versor) - { - if (versor.s0 < 0.0f) - { - this.s0 = -versor.s0; - this.x1 = -versor.x1; - this.x2 = -versor.x2; - this.x3 = -versor.x3; - } - else - { - this.s0 = versor.s0; - this.x1 = versor.x1; - this.x2 = versor.x2; - this.x3 = versor.x3; - } - } - - public void SetShortened(in VersorFP64 versor) - { - this.SetValues(versor); - this.Shorten(); - } - - public readonly void Turn(in Vector3FP32 vector, out Vector3FP32 result) - { - 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); - float x3 = (vector.x3 + tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1); + float x1 = vector.x1 + tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2); + float x2 = vector.x2 + tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3); + float x3 = vector.x3 + tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1); result.x1 = x1; result.x2 = x2; result.x3 = x3; } - public readonly void TurnBack(in Vector3FP32 vector, out Vector3FP32 result) + public static void TurnBack(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result) { - 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 tx1 = 2.0f * (versor.x2 * vector.x3 - versor.x3 * vector.x2); + float tx2 = 2.0f * (versor.x3 * vector.x1 - versor.x1 * vector.x3); + float tx3 = 2.0f * (versor.x1 * vector.x2 - versor.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); - float x3 = (vector.x3 - tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1); + float x1 = vector.x1 - tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2); + float x2 = vector.x2 - tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3); + float x3 = vector.x3 - tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1); result.x1 = x1; result.x2 = x2; result.x3 = x3; } - private void Normalize(float squareModule) + public static void LoadIdentity(out VersorFP32 result) { - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) - { - this.Reset(); - return; - } - - float multiplier = MathF.Sqrt(1.0f / squareModule); - - this.s0 *= multiplier; - this.x1 *= multiplier; - this.x2 *= multiplier; - this.x3 *= multiplier; + result.s0 = 1.0f; + result.x1 = 0.0f; + result.x2 = 0.0f; + result.x3 = 0.0f; } - public static void Combine(in VersorFP32 second, in VersorFP32 first, out VersorFP32 result) + public static void LoadValues(float s0, float x1, float x2, float x3, out VersorFP32 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); + float squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3); result.s0 = s0; result.x1 = x1; result.x2 = x2; result.x3 = x3; - if (!UtilityFP32.IsSqareValueUnit(squareModule)) + if (!UtilityFP32.IsSqareUnit(squareModulus)) { - result.Normalize(squareModule); - } - } - - public static void LoadIdle(out VersorFP32 versor) - { - 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 VersorFP32 versor) - { - versor.s0 = s0; - versor.x1 = x1; - versor.x2 = x2; - versor.x3 = x3; - - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (!UtilityFP32.IsSqareValueUnit(squareModule)) - { - versor.Normalize(squareModule); + result.Normalize(squareModulus); } } } diff --git a/BasicGeometry/VersorFP64.cs b/BasicGeometry/VersorFP64.cs index 5026cad..9878c7e 100644 --- a/BasicGeometry/VersorFP64.cs +++ b/BasicGeometry/VersorFP64.cs @@ -31,7 +31,17 @@ namespace BasicGeometry public VersorFP64(double s0, double x1, double x2, double x3) { - LoadValues(s0, x1, x2, x3, out this); + 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) @@ -44,7 +54,17 @@ namespace BasicGeometry public VersorFP64(in VersorFP32 versor) { - LoadValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3(), out this); + 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() @@ -67,7 +87,21 @@ namespace BasicGeometry return this.x3; } - public readonly bool IsIdle() + 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; } @@ -98,39 +132,112 @@ namespace BasicGeometry this.x3 = -this.x3; } - public readonly double GetAngle(AngleUnit unit) + public void SetValues(double s0, double x1, double x2, double x3) { - if (this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || 1.0 - UtilityFP64.EPSYLON <= this.s0) { - return 0.0; - } - - if (UtilityFP64.IsZero(this.s0)) + 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)) { - return AngleFP64.GetHalfCircle(unit); + this.Normalize(squareModulus); } - - return RadianFP64.ToUnits(2.0 * Math.Acos(this.s0), unit); } - public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix) + public void Set(in VersorFP64 versor) { - double s0s0 = this.s0 * this.s0; - double x1x1 = this.x1 * this.x1; - double x2x2 = this.x1 * this.x2; - double x3x3 = this.x1 * this.x3; + 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()); + } - double s0x1 = 2.0 * this.s0 * this.x1; - double s0x2 = 2.0 * this.s0 * this.x2; - double s0x3 = 2.0 * this.s0 * this.x3; + private void Normalize(double squareModulus) + { + if (!double.IsFinite(squareModulus) || squareModulus <= UtilityFP64.SQUARE_EPSYLON) + { + this.Reset(); + return; + } - 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 multiplier = Math.Sqrt(1.0 / squareModulus); - matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + 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) + { + 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); + + 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); + } + } + + 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 = 2.0 * versor.s0 * versor.x1; + double s0x2 = 2.0 * versor.s0 * versor.x2; + double s0x3 = 2.0 * versor.s0 * versor.x3; + + double x1x2 = 2.0 * versor.x1 * versor.x2; + double x1x3 = 2.0 * versor.x1 * versor.x3; + double x2x3 = 2.0 * versor.x2 * versor.x3; + + matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); + matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); + matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); matrix.r1c2 = x1x2 - s0x3; matrix.r2c3 = x2x3 - s0x1; @@ -141,24 +248,24 @@ namespace BasicGeometry matrix.r1c3 = x1x3 + s0x2; } - public readonly void MakeReverseMatrix(out Matrix3x3FP64 matrix) + public static void MakeReverseMatrix(in VersorFP64 versor, out Matrix3x3FP64 matrix) { - double s0s0 = this.s0 * this.s0; - double x1x1 = this.x1 * this.x1; - double x2x2 = this.x1 * this.x2; - double x3x3 = this.x1 * this.x3; + 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 = 2.0 * this.s0 * this.x1; - double s0x2 = 2.0 * this.s0 * this.x2; - double s0x3 = 2.0 * this.s0 * this.x3; + double s0x1 = 2.0 * versor.s0 * versor.x1; + double s0x2 = 2.0 * versor.s0 * versor.x2; + double s0x3 = 2.0 * versor.s0 * versor.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 = 2.0 * versor.x1 * versor.x2; + double x1x3 = 2.0 * versor.x1 * versor.x3; + double x2x3 = 2.0 * versor.x2 * versor.x3; - matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); + matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); + matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); matrix.r1c2 = x1x2 + s0x3; matrix.r2c3 = x2x3 + s0x1; @@ -169,157 +276,56 @@ namespace BasicGeometry matrix.r1c3 = x1x3 - s0x2; } - public void SetValues(double s0, double x1, double x2, double x3) + public static void Turn(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result) { - this.s0 = s0; - this.x1 = x1; - this.x2 = x2; - this.x3 = x3; - - double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (!UtilityFP64.IsSqareValueUnit(squareModule)) - { - this.Normalize(squareModule); - } - } + 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); - public void SetValues(in VersorFP64 versor) - { - this.s0 = versor.s0; - this.x1 = versor.x1; - this.x2 = versor.x2; - this.x3 = versor.x3; - } - - public void SetValues(in VersorFP32 versor) - { - this.SetValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3()); - } - - public void SetShortened(in VersorFP64 versor) - { - if (versor.s0 < 0.0) - { - this.s0 = -versor.s0; - this.x1 = -versor.x1; - this.x2 = -versor.x2; - this.x3 = -versor.x3; - } - else - { - this.s0 = versor.s0; - this.x1 = versor.x1; - this.x2 = versor.x2; - this.x3 = versor.x3; - } - } - - public void SetShortened(in VersorFP32 versor) - { - this.SetValues(versor); - this.Shorten(); - } - - public void SetInverted(in VersorFP64 versor) - { - this.s0 = versor.s0; - this.x1 = -versor.x1; - this.x2 = -versor.x2; - this.x3 = -versor.x3; - } - - public void SetInverted(in VersorFP32 versor) - { - this.SetValues(versor.GetScalar(), -versor.GetX1(), -versor.GetX2(), -versor.GetX3()); - } - - public readonly void Turn(in Vector3FP64 vector, out Vector3FP64 result) - { - 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); - double x3 = (vector.x3 + tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1); + 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 readonly void TurnBack(in Vector3FP64 vector, out Vector3FP64 result) + public static void TurnBack(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result) { - 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 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 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); - double x2 = (vector.x2 - tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3); - double x3 = (vector.x3 - tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1); + 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; } - private void Normalize(double squareModule) + public static void LoadIdentity(out VersorFP64 result) { - if (squareModule <= UtilityFP64.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= UtilityFP64.SQUARE_EPSYLON * squareModule) - { - this.Reset(); - return; - } - - double multiplier = Math.Sqrt(1.0 / squareModule); - - this.s0 *= multiplier; - this.x1 *= multiplier; - this.x2 *= multiplier; - this.x3 *= multiplier; + result.s0 = 1.0; + result.x1 = 0.0; + result.x2 = 0.0; + result.x3 = 0.0; } - public static void Combine(in VersorFP64 second, in VersorFP64 first, out VersorFP64 result) + public static void LoadValues(double s0, double x1, double x2, double x3, 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); - - double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + double squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3); result.s0 = s0; result.x1 = x1; result.x2 = x2; result.x3 = x3; - if (!UtilityFP64.IsSqareValueUnit(squareModule)) + if (!UtilityFP64.IsSqareUnit(squareModulus)) { - result.Normalize(squareModule); - } - } - - public static void LoadIdle(out VersorFP64 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 VersorFP64 versor) - { - versor.s0 = s0; - versor.x1 = x1; - versor.x2 = x2; - versor.x3 = x3; - - double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (!UtilityFP64.IsSqareValueUnit(squareModule)) - { - versor.Normalize(squareModule); + result.Normalize(squareModulus); } } } diff --git a/BasicGeometryDev/Program.cs b/BasicGeometryDev/Program.cs index 14e854a..1d599d3 100644 --- a/BasicGeometryDev/Program.cs +++ b/BasicGeometryDev/Program.cs @@ -2,6 +2,7 @@ using System; using System.ComponentModel; using System.Diagnostics; +using System.Numerics; using BasicGeometry; public static class Program @@ -46,7 +47,7 @@ public static class Program { Console.WriteLine("({0}, {1}, {2}, {3})", versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3()); } - +/* public static int Main() { int amount = 1000000; @@ -75,6 +76,15 @@ public static class Program PrintVersor(versors2[10]); PrintVersor(results[10]); + return 0; + } +*/ + public static int Main() + { + Vector2FP32 vector = Vector2FP32.ZERO; + + Console.WriteLine("{0}", vector.ToString()); + return 0; } } \ No newline at end of file