From 791fda6a7f00c3e6b5ca3de5fcdc3a053a4da6ab Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Wed, 26 Nov 2025 22:44:31 +0700 Subject: [PATCH] =?UTF-8?q?=D0=A3=D0=BF=D0=BE=D1=80=D1=8F=D0=B4=D0=BE?= =?UTF-8?q?=D1=87=D0=B8=D0=B2=D0=B0=D0=BD=D0=B8=D0=B5=20=D0=BF=D1=80=D0=BE?= =?UTF-8?q?=D0=B5=D0=BA=D1=82=D0=B0=20=D0=B8=20=D1=81=D0=B8=D0=BD=D1=85?= =?UTF-8?q?=D1=80=D0=BE=D0=BD=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D1=8F=20=D1=81?= =?UTF-8?q?=20BGC-C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/Angle.cs | 2 +- BasicGeometry/AngleFP32.cs | 2 +- BasicGeometry/AngleFP64.cs | 2 +- BasicGeometry/Attitude.cs | 11 + BasicGeometry/Axis.cs | 12 + BasicGeometry/ComplexFP32.cs | 171 +++++++ BasicGeometry/DegreeFP32.cs | 2 +- BasicGeometry/DegreeFP64.cs | 2 +- BasicGeometry/Matrix2x2FP32.cs | 2 +- BasicGeometry/Matrix2x2FP64.cs | 2 +- BasicGeometry/Matrix2x3FP32.cs | 2 +- BasicGeometry/Matrix2x3FP64.cs | 2 +- BasicGeometry/Matrix3x2FP32.cs | 2 +- BasicGeometry/Matrix3x2FP64.cs | 2 +- BasicGeometry/Matrix3x3FP32.cs | 19 +- BasicGeometry/Matrix3x3FP64.cs | 21 +- BasicGeometry/MatrixProductFP32.cs | 2 +- BasicGeometry/MatrixProductFP64.cs | 2 +- BasicGeometry/QuaternionFP32.cs | 425 +++++++++++++---- BasicGeometry/QuaternionFP64.cs | 429 ++++++++++++++---- BasicGeometry/RadianFP32.cs | 2 +- BasicGeometry/RadianFP64.cs | 2 +- BasicGeometry/Rotation3FP32.cs | 2 +- BasicGeometry/Rotation3FP64.cs | 2 +- BasicGeometry/TurnFP32.cs | 2 +- BasicGeometry/TurnFP64.cs | 2 +- BasicGeometry/UtilityFP32.cs | 4 +- BasicGeometry/UtilityFP64.cs | 4 +- BasicGeometry/Vector2FP32.cs | 253 +++++++---- BasicGeometry/Vector2FP64.cs | 257 +++++++---- BasicGeometry/Vector3FP32.cs | 309 +++++++++---- BasicGeometry/Vector3FP64.cs | 307 +++++++++---- BasicGeometry/VersorFP32.cs | 124 +++-- BasicGeometry/VersorFP64.cs | 98 ++-- BasicGeometryDev/Program.cs | 2 +- BasicGeometryTest/Vector2/Vector2InitTest.cs | 2 +- .../Vector2/Vector2IsZeroTest.cs | 2 +- BasicGeometry.Net.sln => GeometryNet.sln | 0 38 files changed, 1838 insertions(+), 650 deletions(-) create mode 100644 BasicGeometry/Attitude.cs create mode 100644 BasicGeometry/Axis.cs create mode 100644 BasicGeometry/ComplexFP32.cs rename BasicGeometry.Net.sln => GeometryNet.sln (100%) diff --git a/BasicGeometry/Angle.cs b/BasicGeometry/Angle.cs index 779404b..ac176f7 100644 --- a/BasicGeometry/Angle.cs +++ b/BasicGeometry/Angle.cs @@ -4,7 +4,7 @@ * Date: 1 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public enum AngleUnit { diff --git a/BasicGeometry/AngleFP32.cs b/BasicGeometry/AngleFP32.cs index 3a59774..5a8fcb0 100644 --- a/BasicGeometry/AngleFP32.cs +++ b/BasicGeometry/AngleFP32.cs @@ -4,7 +4,7 @@ * Date: 1 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public static class AngleFP32 { diff --git a/BasicGeometry/AngleFP64.cs b/BasicGeometry/AngleFP64.cs index 9019085..0cae604 100644 --- a/BasicGeometry/AngleFP64.cs +++ b/BasicGeometry/AngleFP64.cs @@ -4,7 +4,7 @@ * Date: 1 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public static class AngleFP64 { diff --git a/BasicGeometry/Attitude.cs b/BasicGeometry/Attitude.cs new file mode 100644 index 0000000..8f7a35d --- /dev/null +++ b/BasicGeometry/Attitude.cs @@ -0,0 +1,11 @@ +namespace BGC +{ + public enum Attitude + { + ANY = 0, + ZERO = 1, + ORTHOGONAL = 2, + CO_DIRECTIONAL = 3, + COUNTER_DIRECTIONAL = 4 + } +} diff --git a/BasicGeometry/Axis.cs b/BasicGeometry/Axis.cs new file mode 100644 index 0000000..bdbd781 --- /dev/null +++ b/BasicGeometry/Axis.cs @@ -0,0 +1,12 @@ +namespace BGC +{ + public enum Axis + { + X1 = 1, + X2 = 2, + X3 = 3, + REVERSE_X1 = -1, + REVERSE_X2 = -2, + REVERSE_X3 = -3 + } +} diff --git a/BasicGeometry/ComplexFP32.cs b/BasicGeometry/ComplexFP32.cs new file mode 100644 index 0000000..4c469b0 --- /dev/null +++ b/BasicGeometry/ComplexFP32.cs @@ -0,0 +1,171 @@ + +namespace BGC +{ + public struct ComplexFP32 + { + public float real = 0.0f; + public float imaginary = 0.0f; + + public ComplexFP32(float real, float imaginary) + { + this.real = real; + this.imaginary = imaginary; + } + + public ComplexFP32(in ComplexFP32 number) + { + this.real = number.real; + this.imaginary = number.imaginary; + } + + public readonly float GetSquareModulus() + { + return this.real * this.real + this.imaginary * this.imaginary; + } + + public readonly float GetModulus() + { + return MathF.Sqrt(GetSquareModulus()); + } + + public readonly bool IsZero() + { + return GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON; + } + + public readonly bool IsUnit() + { + return UtilityFP32.IsSqareUnit(GetSquareModulus()); + } + + public void Reset() + { + this.real = 0.0f; + this.imaginary = 0.0f; + } + + public void MakeOpposite() + { + this.real = -this.real; + this.imaginary = -this.imaginary; + } + + public bool Normalize() + { + float squareModulus = this.GetSquareModulus(); + + if (UtilityFP32.IsSqareUnit(squareModulus)) + { + return true; + } + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + return false; + } + + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + this.real *= multiplier; + this.imaginary *= multiplier; + + return true; + } + + public void Conjugate() + { + this.imaginary = -this.imaginary; + } + + public bool Invert() + { + float squareModulus = this.GetSquareModulus(); + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + return false; + } + + float multiplicand = 1.0f / squareModulus; + + this.real *= multiplicand; + this.imaginary = -this.imaginary * multiplicand; + + return true; + } + + public void SetValues(float real, float imaginary) + { + this.real = real; + this.imaginary = imaginary; + } + + public void SetValues(in ComplexFP32 number) + { + this.real = number.real; + this.imaginary = number.imaginary; + } + + public static void Reset(out ComplexFP32 number) + { + number.real = 0.0f; + number.imaginary = 0.0f; + } + + public static void GetOpposite(in ComplexFP32 number, out ComplexFP32 opposite) + { + opposite.real = -number.real; + opposite.imaginary = -number.imaginary; + } + + public static bool GetNormalized(in ComplexFP32 number, out ComplexFP32 normalized) + { + float squareModulus = number.GetSquareModulus(); + + if (UtilityFP64.IsSqareUnit(squareModulus)) + { + normalized.real = number.real; + normalized.imaginary = number.imaginary; + return true; + } + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + normalized.real = 0.0f; + normalized.imaginary = 0.0f; + return false; + } + + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + normalized.real = number.real * multiplier; + normalized.imaginary = number.imaginary * multiplier; + return true; + } + + public static void GetConjugate(in ComplexFP32 number, out ComplexFP32 conjugate) + { + conjugate.real = number.real; + conjugate.imaginary = -number.imaginary; + } + + public static bool GetInverse(in ComplexFP32 number, out ComplexFP32 inverse) + { + float squareModulus = number.GetSquareModulus(); + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + inverse.real = 0.0f; + inverse.imaginary = 0.0f; + return false; + } + + float multiplicand = 1.0f / squareModulus; + + inverse.real = number.real * multiplicand; + inverse.imaginary = -number.imaginary * multiplicand; + + return true; + } + } +} diff --git a/BasicGeometry/DegreeFP32.cs b/BasicGeometry/DegreeFP32.cs index 09c299d..d537156 100644 --- a/BasicGeometry/DegreeFP32.cs +++ b/BasicGeometry/DegreeFP32.cs @@ -4,7 +4,7 @@ * Date: 18 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class DegreeFP32 { diff --git a/BasicGeometry/DegreeFP64.cs b/BasicGeometry/DegreeFP64.cs index c47408b..c50dc6a 100644 --- a/BasicGeometry/DegreeFP64.cs +++ b/BasicGeometry/DegreeFP64.cs @@ -4,7 +4,7 @@ * Date: 18 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class DegreeFP64 { diff --git a/BasicGeometry/Matrix2x2FP32.cs b/BasicGeometry/Matrix2x2FP32.cs index 1a1465c..6763e01 100644 --- a/BasicGeometry/Matrix2x2FP32.cs +++ b/BasicGeometry/Matrix2x2FP32.cs @@ -4,7 +4,7 @@ * Date: 10 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Matrix2x2FP32 { diff --git a/BasicGeometry/Matrix2x2FP64.cs b/BasicGeometry/Matrix2x2FP64.cs index e49f3ac..769b1ec 100644 --- a/BasicGeometry/Matrix2x2FP64.cs +++ b/BasicGeometry/Matrix2x2FP64.cs @@ -3,7 +3,7 @@ * Date: 10 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Matrix2x2FP64 { diff --git a/BasicGeometry/Matrix2x3FP32.cs b/BasicGeometry/Matrix2x3FP32.cs index 24360db..7de9d46 100644 --- a/BasicGeometry/Matrix2x3FP32.cs +++ b/BasicGeometry/Matrix2x3FP32.cs @@ -3,7 +3,7 @@ * License: Apache-2.0 * Date: 11 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public struct Matrix2x3FP32 { diff --git a/BasicGeometry/Matrix2x3FP64.cs b/BasicGeometry/Matrix2x3FP64.cs index cbbf95c..b845e45 100644 --- a/BasicGeometry/Matrix2x3FP64.cs +++ b/BasicGeometry/Matrix2x3FP64.cs @@ -3,7 +3,7 @@ * License: Apache-2.0 * Date: 11 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public struct Matrix2x3FP64 { diff --git a/BasicGeometry/Matrix3x2FP32.cs b/BasicGeometry/Matrix3x2FP32.cs index 68dd59c..a6b5eae 100644 --- a/BasicGeometry/Matrix3x2FP32.cs +++ b/BasicGeometry/Matrix3x2FP32.cs @@ -3,7 +3,7 @@ * License: Apache-2.0 * Date: 11 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public struct Matrix3x2FP32 { diff --git a/BasicGeometry/Matrix3x2FP64.cs b/BasicGeometry/Matrix3x2FP64.cs index 5e5eab9..40b0f6d 100644 --- a/BasicGeometry/Matrix3x2FP64.cs +++ b/BasicGeometry/Matrix3x2FP64.cs @@ -3,7 +3,7 @@ * License: Apache-2.0 * Date: 11 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public struct Matrix3x2FP64 { diff --git a/BasicGeometry/Matrix3x3FP32.cs b/BasicGeometry/Matrix3x3FP32.cs index 1486c50..def575b 100644 --- a/BasicGeometry/Matrix3x3FP32.cs +++ b/BasicGeometry/Matrix3x3FP32.cs @@ -4,7 +4,7 @@ * Date: 10 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Matrix3x3FP32 { @@ -223,7 +223,7 @@ namespace BasicGeometry this.r3c3 = r3; } - public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed) + public static void MakeTransposed(in Matrix3x3FP32 matrix, out Matrix3x3FP32 transposed) { transposed.r1c1 = matrix.r1c1; transposed.r2c2 = matrix.r2c2; @@ -317,21 +317,6 @@ 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; diff --git a/BasicGeometry/Matrix3x3FP64.cs b/BasicGeometry/Matrix3x3FP64.cs index cab0ea0..67546e0 100644 --- a/BasicGeometry/Matrix3x3FP64.cs +++ b/BasicGeometry/Matrix3x3FP64.cs @@ -4,7 +4,7 @@ * Date: 10 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Matrix3x3FP64 { @@ -223,7 +223,7 @@ namespace BasicGeometry this.r3c3 = r3; } - public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed) + public static void GetTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed) { transposed.r1c1 = matrix.r1c1; transposed.r2c2 = matrix.r2c2; @@ -234,7 +234,7 @@ namespace BasicGeometry (transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3); } - public static bool MakeInverted(in Matrix3x3FP64 matrix, out Matrix3x3FP64 inverted) + public static bool GetInverted(in Matrix3x3FP64 matrix, out Matrix3x3FP64 inverted) { double determinant = matrix.GetDeterminant(); @@ -317,21 +317,6 @@ 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; diff --git a/BasicGeometry/MatrixProductFP32.cs b/BasicGeometry/MatrixProductFP32.cs index 26a776a..0a766e6 100644 --- a/BasicGeometry/MatrixProductFP32.cs +++ b/BasicGeometry/MatrixProductFP32.cs @@ -4,7 +4,7 @@ * Date: 11 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class MatrixProductFP32 { diff --git a/BasicGeometry/MatrixProductFP64.cs b/BasicGeometry/MatrixProductFP64.cs index 718c4f3..0ab6f0c 100644 --- a/BasicGeometry/MatrixProductFP64.cs +++ b/BasicGeometry/MatrixProductFP64.cs @@ -4,7 +4,7 @@ * Date: 11 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class MatrixProductFP64 { diff --git a/BasicGeometry/QuaternionFP32.cs b/BasicGeometry/QuaternionFP32.cs index 80d189b..3d06f3c 100644 --- a/BasicGeometry/QuaternionFP32.cs +++ b/BasicGeometry/QuaternionFP32.cs @@ -4,7 +4,7 @@ * Date: 4 Dec 2024 */ -namespace BasicGeometry +namespace BGC { public struct QuaternionFP32 { @@ -62,7 +62,7 @@ namespace BasicGeometry this.x3 = 0.0f; } - public void SetToIdentity() + public void MakeUnit() { this.s0 = 1.0f; this.x1 = 0.0f; @@ -77,6 +77,52 @@ namespace BasicGeometry this.x3 = -this.x3; } + public void MakeOpposit() + { + this.s0 = -this.s0; + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public bool Invert() + { + float squareModulus = this.GetSquareModulus(); + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + return false; + } + + float multiplicand = 1.0f / squareModulus; + + this.s0 = this.s0 * multiplicand; + this.x1 = -this.x1 * multiplicand; + this.x2 = -this.x2 * multiplicand; + this.x3 = -this.x3 * multiplicand; + + return true; + } + + public bool Normalize() + { + float squareModulus = this.GetSquareModulus(); + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + return false; + } + + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + this.s0 *= multiplier; + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; + + return true; + } + public void SetValues(float s0, float x1, float x2, float x3) { this.s0 = s0; @@ -85,7 +131,7 @@ namespace BasicGeometry this.x3 = x3; } - public void Set(in QuaternionFP32 quaternion) + public void SetValues(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -101,90 +147,38 @@ namespace BasicGeometry this.x3 = (float)quaternion.x3; } - public static void MakeConjugate(in QuaternionFP32 quaternion, out QuaternionFP32 conjugate) + public static void Reset(out QuaternionFP32 quaternion) { - conjugate.s0 = quaternion.s0; - conjugate.x1 = -quaternion.x1; - conjugate.x2 = -quaternion.x2; - conjugate.x3 = -quaternion.x3; + quaternion.s0 = 0.0f; + quaternion.x1 = 0.0f; + quaternion.x2 = 0.0f; + quaternion.x3 = 0.0f; } - public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix) + public static void MakeUnit(out QuaternionFP32 quaternion) { - float s0s0 = this.s0 * this.s0; - float x1x1 = this.x1 * this.x1; - float x2x2 = this.x2 * this.x2; - float x3x3 = this.x3 * this.x3; - - float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - - if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) - { - Matrix3x3FP32.LoadIdentity(out matrix); - return; - } - - float corrector1 = 1.0f / squareModulus; - - float s0x1 = this.s0 * this.x1; - float s0x2 = this.s0 * this.x2; - float s0x3 = this.s0 * this.x3; - float x1x2 = this.x1 * this.x2; - 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)); - - 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); + quaternion.s0 = 1.0f; + quaternion.x1 = 0.0f; + quaternion.x2 = 0.0f; + quaternion.x3 = 0.0f; } - public readonly void MakeReverseMatrix(out Matrix3x3FP32 matrix) + public static void Swap(ref QuaternionFP32 quaternion1, ref QuaternionFP32 quaternion2) { - float s0s0 = this.s0 * this.s0; - float x1x1 = this.x1 * this.x1; - float x2x2 = this.x2 * this.x2; - float x3x3 = this.x3 * this.x3; + float s0 = quaternion1.s0; + float x1 = quaternion1.x1; + float x2 = quaternion1.x2; + float x3 = quaternion1.x3; - float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); + quaternion1.s0 = quaternion2.s0; + quaternion1.x1 = quaternion2.x1; + quaternion1.x2 = quaternion2.x2; + quaternion1.x3 = quaternion2.x3; - if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) - { - Matrix3x3FP32.LoadIdentity(out matrix); - return; - } - - float corrector1 = 1.0f / squareModulus; - - float s0x1 = this.s0 * this.x1; - float s0x2 = this.s0 * this.x2; - float s0x3 = this.s0 * this.x3; - float x1x2 = this.x1 * this.x2; - 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)); - - 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); + quaternion2.s0 = s0; + quaternion2.x1 = x1; + quaternion2.x2 = x2; + quaternion2.x3 = x3; } public static void Add(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2, out QuaternionFP32 sum) @@ -195,6 +189,14 @@ namespace BasicGeometry sum.x3 = quaternion1.x3 + quaternion2.x3; } + public static void AddScaled(in QuaternionFP32 basic_quaternion, in QuaternionFP32 scalable_quaternion, float scale, out QuaternionFP32 sum) + { + sum.s0 = basic_quaternion.s0 + scale * scalable_quaternion.s0; + sum.x1 = basic_quaternion.x1 + scale * scalable_quaternion.x1; + sum.x2 = basic_quaternion.x2 + scale * scalable_quaternion.x2; + sum.x3 = basic_quaternion.x3 + scale * scalable_quaternion.x3; + } + public static void Subtract(in QuaternionFP32 minuend, in QuaternionFP32 subtrahend, out QuaternionFP32 difference) { difference.s0 = minuend.s0 - subtrahend.s0; @@ -216,7 +218,272 @@ namespace BasicGeometry product.x3 = x3; } - public static bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2) + public static void Multiply(in QuaternionFP32 quaternion, float multiplier, out QuaternionFP32 product) + { + product.s0 = quaternion.s0 * multiplier; + product.x1 = quaternion.x1 * multiplier; + product.x2 = quaternion.x2 * multiplier; + product.x3 = quaternion.x3 * multiplier; + } + + public static bool Divide(in QuaternionFP32 divident, in QuaternionFP32 divisor, out QuaternionFP32 quotient) + { + float squareModulus = divisor.GetSquareModulus(); + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + Reset(out quotient); + return false; + } + + float s0 = (divident.s0 * divisor.s0 + divident.x1 * divisor.x1) + (divident.x2 * divisor.x2 + divident.x3 * divisor.x3); + float x1 = (divident.x1 * divisor.s0 + divident.x3 * divisor.x2) - (divident.s0 * divisor.x1 + divident.x2 * divisor.x3); + float x2 = (divident.x2 * divisor.s0 + divident.x1 * divisor.x3) - (divident.s0 * divisor.x2 + divident.x3 * divisor.x1); + float x3 = (divident.x3 * divisor.s0 + divident.x2 * divisor.x1) - (divident.s0 * divisor.x3 + divident.x1 * divisor.x2); + + float multiplicand = 1.0f / squareModulus; + + quotient.s0 = s0 * multiplicand; + quotient.x1 = x1 * multiplicand; + quotient.x2 = x2 * multiplicand; + quotient.x3 = x3 * multiplicand; + + return true; + } + + public static void Divide(in QuaternionFP32 divident, float divisor, out QuaternionFP32 quotient) + { + Multiply(divident, 1.0f / divisor, out quotient); + } + + public static void GetMeanOfTwo(in QuaternionFP32 vector1, in QuaternionFP32 vector2, out QuaternionFP32 mean) + { + mean.s0 = (vector1.s0 + vector2.s0) * 0.5f; + mean.x1 = (vector1.x1 + vector2.x1) * 0.5f; + mean.x2 = (vector1.x2 + vector2.x2) * 0.5f; + mean.x3 = (vector1.x3 + vector2.x3) * 0.5f; + } + + public static void GetMeanOfThree(in QuaternionFP32 vector1, in QuaternionFP32 vector2, in QuaternionFP32 vector3, out QuaternionFP32 mean) + { + mean.s0 = (vector1.s0 + vector2.s0 + vector3.s0) * UtilityFP32.ONE_THIRD; + mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD; + mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD; + mean.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP32.ONE_THIRD; + } + + public static void Interpolate(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2, float phase, out QuaternionFP32 interpolation) + { + float counterphase = 1.0f - phase; + + interpolation.s0 = quaternion1.s0 * counterphase + quaternion2.s0 * phase; + interpolation.x1 = quaternion1.x1 * counterphase + quaternion2.x1 * phase; + interpolation.x2 = quaternion1.x2 * counterphase + quaternion2.x2 * phase; + interpolation.x3 = quaternion1.x3 * counterphase + quaternion2.x3 * phase; + } + + public static void GetConjugate(in QuaternionFP32 quaternion, out QuaternionFP32 conjugate) + { + conjugate.s0 = quaternion.s0; + conjugate.x1 = -quaternion.x1; + conjugate.x2 = -quaternion.x2; + conjugate.x3 = -quaternion.x3; + } + + public static void GetOpposite(in QuaternionFP32 quaternion, out QuaternionFP32 opposit) + { + opposit.s0 = -quaternion.s0; + opposit.x1 = -quaternion.x1; + opposit.x2 = -quaternion.x2; + opposit.x3 = -quaternion.x3; + } + + public static bool GetInverse(in QuaternionFP32 quaternion, out QuaternionFP32 inverse) + { + float squareModulus = quaternion.GetSquareModulus(); + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + Reset(out inverse); + return false; + } + + float multiplicand = 1.0f / squareModulus; + + inverse.s0 = quaternion.s0 * multiplicand; + inverse.x1 = -quaternion.x1 * multiplicand; + inverse.x2 = -quaternion.x2 * multiplicand; + inverse.x3 = -quaternion.x3 * multiplicand; + + return true; + } + + public static bool GetNormalized(in QuaternionFP32 quaternion, out QuaternionFP32 normalized) + { + float squareModulus = quaternion.GetSquareModulus(); + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + Reset(out normalized); + return false; + } + + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + normalized.s0 = quaternion.s0 * multiplier; + normalized.x1 = quaternion.x1 * multiplier; + normalized.x2 = quaternion.x2 * multiplier; + normalized.x3 = quaternion.x3 * multiplier; + + return true; + } + + public static bool GetExponation(in QuaternionFP32 quaternion, float exponent, out QuaternionFP32 power) + { + float s0s0 = quaternion.s0 * quaternion.s0; + float x1x1 = quaternion.x1 * quaternion.x1; + float x2x2 = quaternion.x2 * quaternion.x2; + float x3x3 = quaternion.x3 * quaternion.x3; + + float squareVector = x1x1 + (x2x2 + x3x3); + float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); + + if (float.IsNaN(squareModulus)) + { + Reset(out power); + return false; + } + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) + { + Reset(out power); + return true; + } + + if (squareVector <= UtilityFP32.SQUARE_EPSYLON) + { + if (quaternion.s0 < 0.0f) + { + Reset(out power); + return false; + } + + power.s0 = MathF.Pow(quaternion.s0, exponent); + power.x1 = 0.0f; + power.x2 = 0.0f; + power.x3 = 0.0f; + + return true; + } + + float sine = MathF.Sqrt(squareVector / squareModulus); + float power_angle = MathF.Atan2(sine, quaternion.s0 / MathF.Sqrt(squareModulus)) * exponent; + float power_modulus = MathF.Pow(squareModulus, 0.5f * exponent); + float multiplier = power_modulus * MathF.Sin(power_angle) / sine; + + power.s0 = power_modulus * MathF.Cos(power_angle); + power.x1 = quaternion.x1 * multiplier; + power.x2 = quaternion.x2 * multiplier; + power.x3 = quaternion.x3 * multiplier; + + return true; + } + + public static bool GetRotationMatrix(in QuaternionFP32 quaternion, out Matrix3x3FP32 matrix) + { + float s0s0 = quaternion.s0 * quaternion.s0; + float x1x1 = quaternion.x1 * quaternion.x1; + float x2x2 = quaternion.x2 * quaternion.x2; + float x3x3 = quaternion.x3 * quaternion.x3; + + float squareModulus = s0s0 + x1x1 + (x2x2 + x3x3); + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) + { + Matrix3x3FP32.LoadIdentity(out matrix); + return false; + } + + float corrector1 = 1.0f / squareModulus; + + float s0x1 = quaternion.s0 * quaternion.x1; + float s0x2 = quaternion.s0 * quaternion.x2; + float s0x3 = quaternion.s0 * quaternion.x3; + float x1x2 = quaternion.x1 * quaternion.x2; + float x1x3 = quaternion.x1 * quaternion.x3; + float x2x3 = quaternion.x2 * quaternion.x3; + + float corrector2 = 2.0f * corrector1; + + matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2)); + + matrix.r1c2 = corrector2 * (x1x2 - s0x3); + matrix.r2c3 = corrector2 * (x2x3 - s0x1); + matrix.r3c1 = corrector2 * (x1x3 - s0x2); + + matrix.r2c1 = corrector2 * (x1x2 + s0x3); + matrix.r3c2 = corrector2 * (x2x3 + s0x1); + matrix.r1c3 = corrector2 * (x1x3 + s0x2); + + return true; + } + + public static bool GetReverseMatrix(in QuaternionFP32 quaternion, out Matrix3x3FP32 matrix) + { + float s0s0 = quaternion.s0 * quaternion.s0; + float x1x1 = quaternion.x1 * quaternion.x1; + float x2x2 = quaternion.x2 * quaternion.x2; + float x3x3 = quaternion.x3 * quaternion.x3; + + float squareModulus = s0s0 + x1x1 + (x2x2 + x3x3); + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) + { + Matrix3x3FP32.LoadIdentity(out matrix); + return false; + } + + float corrector1 = 1.0f / squareModulus; + + float s0x1 = quaternion.s0 * quaternion.x1; + float s0x2 = quaternion.s0 * quaternion.x2; + float s0x3 = quaternion.s0 * quaternion.x3; + float x1x2 = quaternion.x1 * quaternion.x2; + float x1x3 = quaternion.x1 * quaternion.x3; + float x2x3 = quaternion.x2 * quaternion.x3; + + float corrector2 = 2.0f * corrector1; + + matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2)); + + matrix.r1c2 = corrector2 * (x1x2 + s0x3); + matrix.r2c3 = corrector2 * (x2x3 + s0x1); + matrix.r3c1 = corrector2 * (x1x3 + s0x2); + + matrix.r2c1 = corrector2 * (x1x2 - s0x3); + matrix.r3c2 = corrector2 * (x2x3 - s0x1); + matrix.r1c3 = corrector2 * (x1x3 - s0x2); + + return true; + } + + public static bool GetBothMatrices(in QuaternionFP32 quaternion, out Matrix3x3FP32 matrix, out Matrix3x3FP32 reverse) + { + if (GetReverseMatrix(quaternion, out reverse)) + { + Matrix3x3FP32.MakeTransposed(reverse, out matrix); + return true; + } + + Matrix3x3FP32.LoadIdentity(out matrix); + return false; + } + + public static bool AreClose(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2) { float ds0 = quaternion1.s0 - quaternion2.s0; float dx1 = quaternion1.x1 - quaternion2.x1; diff --git a/BasicGeometry/QuaternionFP64.cs b/BasicGeometry/QuaternionFP64.cs index 36f15cc..58f47ff 100644 --- a/BasicGeometry/QuaternionFP64.cs +++ b/BasicGeometry/QuaternionFP64.cs @@ -4,7 +4,7 @@ * Date: 4 Dec 2024 */ -namespace BasicGeometry +namespace BGC { public struct QuaternionFP64 { @@ -18,7 +18,7 @@ namespace BasicGeometry this.x3 = x3; } - public QuaternionFP64(in QuaternionFP32 quaternion) + public QuaternionFP64(in QuaternionFP64 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -26,7 +26,7 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public QuaternionFP64(in QuaternionFP64 quaternion) + public QuaternionFP64(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -62,7 +62,7 @@ namespace BasicGeometry this.x3 = 0.0; } - public void SetToIdentity() + public void MakeUnit() { this.s0 = 1.0; this.x1 = 0.0; @@ -77,6 +77,52 @@ namespace BasicGeometry this.x3 = -this.x3; } + public void MakeOpposite() + { + this.s0 = -this.s0; + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public bool Invert() + { + double squareModulus = this.GetSquareModulus(); + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus)) + { + return false; + } + + double multiplicand = 1.0 / squareModulus; + + this.s0 = this.s0 * multiplicand; + this.x1 = -this.x1 * multiplicand; + this.x2 = -this.x2 * multiplicand; + this.x3 = -this.x3 * multiplicand; + + return true; + } + + public bool Normalize() + { + double squareModulus = this.GetSquareModulus(); + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus)) + { + return false; + } + + double multiplier = Math.Sqrt(1.0 / squareModulus); + + this.s0 *= multiplier; + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; + + return true; + } + public void SetValues(double s0, double x1, double x2, double x3) { this.s0 = s0; @@ -85,7 +131,7 @@ namespace BasicGeometry this.x3 = x3; } - public void Set(in QuaternionFP32 quaternion) + public void SetValues(in QuaternionFP64 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -93,7 +139,7 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public void Set(in QuaternionFP64 quaternion) + public void SetValues(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -101,90 +147,38 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public static void MakeConjugate(in QuaternionFP64 quaternion, out QuaternionFP64 conjugate) + public static void Reset(out QuaternionFP64 quaternion) { - conjugate.s0 = quaternion.s0; - conjugate.x1 = -quaternion.x1; - conjugate.x2 = -quaternion.x2; - conjugate.x3 = -quaternion.x3; + quaternion.s0 = 0.0; + quaternion.x1 = 0.0; + quaternion.x2 = 0.0; + quaternion.x3 = 0.0; } - public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix) + public static void MakeUnit(out QuaternionFP64 quaternion) { - double s0s0 = this.s0 * this.s0; - double x1x1 = this.x1 * this.x1; - double x2x2 = this.x2 * this.x2; - double x3x3 = this.x3 * this.x3; - - double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - - if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus)) - { - Matrix3x3FP64.LoadIdentity(out matrix); - return; - } - - double corrector1 = 1.0 / squareModulus; - - double s0x1 = this.s0 * this.x1; - double s0x2 = this.s0 * this.x2; - double s0x3 = this.s0 * this.x3; - double x1x2 = this.x1 * this.x2; - 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)); - - 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); + quaternion.s0 = 1.0; + quaternion.x1 = 0.0; + quaternion.x2 = 0.0; + quaternion.x3 = 0.0; } - public readonly void MakeReverseMatrix(out Matrix3x3FP64 matrix) + public static void Swap(ref QuaternionFP64 quaternion1, ref QuaternionFP64 quaternion2) { - double s0s0 = this.s0 * this.s0; - double x1x1 = this.x1 * this.x1; - double x2x2 = this.x2 * this.x2; - double x3x3 = this.x3 * this.x3; + double s0 = quaternion1.s0; + double x1 = quaternion1.x1; + double x2 = quaternion1.x2; + double x3 = quaternion1.x3; - double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); + quaternion1.s0 = quaternion2.s0; + quaternion1.x1 = quaternion2.x1; + quaternion1.x2 = quaternion2.x2; + quaternion1.x3 = quaternion2.x3; - if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus)) - { - Matrix3x3FP64.LoadIdentity(out matrix); - return; - } - - double corrector1 = 1.0 / squareModulus; - - double s0x1 = this.s0 * this.x1; - double s0x2 = this.s0 * this.x2; - double s0x3 = this.s0 * this.x3; - double x1x2 = this.x1 * this.x2; - 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)); - - 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); + quaternion2.s0 = s0; + quaternion2.x1 = x1; + quaternion2.x2 = x2; + quaternion2.x3 = x3; } public static void Add(in QuaternionFP64 quaternion1, in QuaternionFP64 quaternion2, out QuaternionFP64 sum) @@ -195,6 +189,14 @@ namespace BasicGeometry sum.x3 = quaternion1.x3 + quaternion2.x3; } + public static void AddScaled(in QuaternionFP64 basic_quaternion, in QuaternionFP64 scalable_quaternion, double scale, out QuaternionFP64 sum) + { + sum.s0 = basic_quaternion.s0 + scale * scalable_quaternion.s0; + sum.x1 = basic_quaternion.x1 + scale * scalable_quaternion.x1; + sum.x2 = basic_quaternion.x2 + scale * scalable_quaternion.x2; + sum.x3 = basic_quaternion.x3 + scale * scalable_quaternion.x3; + } + public static void Subtract(in QuaternionFP64 minuend, in QuaternionFP64 subtrahend, out QuaternionFP64 difference) { difference.s0 = minuend.s0 - subtrahend.s0; @@ -216,6 +218,271 @@ namespace BasicGeometry product.x3 = x3; } + public static void Multiply(in QuaternionFP64 quaternion, double multiplier, out QuaternionFP64 product) + { + product.s0 = quaternion.s0 * multiplier; + product.x1 = quaternion.x1 * multiplier; + product.x2 = quaternion.x2 * multiplier; + product.x3 = quaternion.x3 * multiplier; + } + + public static bool Divide(in QuaternionFP64 divident, in QuaternionFP64 divisor, out QuaternionFP64 quotient) + { + double squareModulus = divisor.GetSquareModulus(); + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus)) + { + Reset(out quotient); + return false; + } + + double s0 = (divident.s0 * divisor.s0 + divident.x1 * divisor.x1) + (divident.x2 * divisor.x2 + divident.x3 * divisor.x3); + double x1 = (divident.x1 * divisor.s0 + divident.x3 * divisor.x2) - (divident.s0 * divisor.x1 + divident.x2 * divisor.x3); + double x2 = (divident.x2 * divisor.s0 + divident.x1 * divisor.x3) - (divident.s0 * divisor.x2 + divident.x3 * divisor.x1); + double x3 = (divident.x3 * divisor.s0 + divident.x2 * divisor.x1) - (divident.s0 * divisor.x3 + divident.x1 * divisor.x2); + + double multiplicand = 1.0 / squareModulus; + + quotient.s0 = s0 * multiplicand; + quotient.x1 = x1 * multiplicand; + quotient.x2 = x2 * multiplicand; + quotient.x3 = x3 * multiplicand; + + return true; + } + + public static void Divide(in QuaternionFP64 divident, double divisor, out QuaternionFP64 quotient) + { + Multiply(divident, 1.0 / divisor, out quotient); + } + + public static void GetMeanOfTwo(in QuaternionFP64 vector1, in QuaternionFP64 vector2, out QuaternionFP64 mean) + { + mean.s0 = (vector1.s0 + vector2.s0) * 0.5; + mean.x1 = (vector1.x1 + vector2.x1) * 0.5; + mean.x2 = (vector1.x2 + vector2.x2) * 0.5; + mean.x3 = (vector1.x3 + vector2.x3) * 0.5; + } + + public static void GetMeanOfThree(in QuaternionFP64 vector1, in QuaternionFP64 vector2, in QuaternionFP64 vector3, out QuaternionFP64 mean) + { + mean.s0 = (vector1.s0 + vector2.s0 + vector3.s0) * UtilityFP64.ONE_THIRD; + mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD; + mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD; + mean.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP64.ONE_THIRD; + } + + public static void Interpolate(in QuaternionFP64 quaternion1, in QuaternionFP64 quaternion2, double phase, out QuaternionFP64 interpolation) + { + double counterphase = 1.0 - phase; + + interpolation.s0 = quaternion1.s0 * counterphase + quaternion2.s0 * phase; + interpolation.x1 = quaternion1.x1 * counterphase + quaternion2.x1 * phase; + interpolation.x2 = quaternion1.x2 * counterphase + quaternion2.x2 * phase; + interpolation.x3 = quaternion1.x3 * counterphase + quaternion2.x3 * phase; + } + + public static void GetConjugate(in QuaternionFP64 quaternion, out QuaternionFP64 conjugate) + { + conjugate.s0 = quaternion.s0; + conjugate.x1 = -quaternion.x1; + conjugate.x2 = -quaternion.x2; + conjugate.x3 = -quaternion.x3; + } + + public static void GetOpposite(in QuaternionFP64 quaternion, out QuaternionFP64 opposit) + { + opposit.s0 = -quaternion.s0; + opposit.x1 = -quaternion.x1; + opposit.x2 = -quaternion.x2; + opposit.x3 = -quaternion.x3; + } + + public static bool GetInverse(in QuaternionFP64 quaternion, out QuaternionFP64 inverse) + { + double squareModulus = quaternion.GetSquareModulus(); + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus)) + { + Reset(out inverse); + return false; + } + + double multiplicand = 1.0 / squareModulus; + + inverse.s0 = quaternion.s0 * multiplicand; + inverse.x1 = -quaternion.x1 * multiplicand; + inverse.x2 = -quaternion.x2 * multiplicand; + inverse.x3 = -quaternion.x3 * multiplicand; + + return true; + } + + public static bool GetNormalized(in QuaternionFP64 quaternion, out QuaternionFP64 normalized) + { + double squareModulus = quaternion.GetSquareModulus(); + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus)) + { + Reset(out normalized); + return false; + } + + double multiplier = Math.Sqrt(1.0 / squareModulus); + + normalized.s0 = quaternion.s0 * multiplier; + normalized.x1 = quaternion.x1 * multiplier; + normalized.x2 = quaternion.x2 * multiplier; + normalized.x3 = quaternion.x3 * multiplier; + + return true; + } + + public static bool GetExponation(in QuaternionFP64 quaternion, double exponent, out QuaternionFP64 power) + { + double s0s0 = quaternion.s0 * quaternion.s0; + double x1x1 = quaternion.x1 * quaternion.x1; + double x2x2 = quaternion.x2 * quaternion.x2; + double x3x3 = quaternion.x3 * quaternion.x3; + + double squareVector = x1x1 + (x2x2 + x3x3); + double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); + + if (double.IsNaN(squareModulus)) + { + Reset(out power); + return false; + } + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON) + { + Reset(out power); + return true; + } + + if (squareVector <= UtilityFP64.SQUARE_EPSYLON) + { + if (quaternion.s0 < 0.0f) + { + Reset(out power); + return false; + } + + power.s0 = Math.Pow(quaternion.s0, exponent); + power.x1 = 0.0f; + power.x2 = 0.0f; + power.x3 = 0.0f; + + return true; + } + + double sine = Math.Sqrt(squareVector / squareModulus); + double power_angle = Math.Atan2(sine, quaternion.s0 / Math.Sqrt(squareModulus)) * exponent; + double power_modulus = Math.Pow(squareModulus, 0.5f * exponent); + double multiplier = power_modulus * Math.Sin(power_angle) / sine; + + power.s0 = power_modulus * Math.Cos(power_angle); + power.x1 = quaternion.x1 * multiplier; + power.x2 = quaternion.x2 * multiplier; + power.x3 = quaternion.x3 * multiplier; + + return true; + } + + public static bool GetRotationMatrix(in QuaternionFP64 quaternion, out Matrix3x3FP64 matrix) + { + double s0s0 = quaternion.s0 * quaternion.s0; + double x1x1 = quaternion.x1 * quaternion.x1; + double x2x2 = quaternion.x2 * quaternion.x2; + double x3x3 = quaternion.x3 * quaternion.x3; + + double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus)) + { + Matrix3x3FP64.LoadIdentity(out matrix); + return false; + } + + double corrector1 = 1.0 / squareModulus; + + double s0x1 = quaternion.s0 * quaternion.x1; + double s0x2 = quaternion.s0 * quaternion.x2; + double s0x3 = quaternion.s0 * quaternion.x3; + double x1x2 = quaternion.x1 * quaternion.x2; + double x1x3 = quaternion.x1 * quaternion.x3; + double x2x3 = quaternion.x2 * quaternion.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)); + + matrix.r1c2 = corrector2 * (x1x2 - s0x3); + matrix.r2c3 = corrector2 * (x2x3 - s0x1); + matrix.r3c1 = corrector2 * (x1x3 - s0x2); + + matrix.r2c1 = corrector2 * (x1x2 + s0x3); + matrix.r3c2 = corrector2 * (x2x3 + s0x1); + matrix.r1c3 = corrector2 * (x1x3 + s0x2); + + return true; + } + + public static bool GetReverseMatrix(in QuaternionFP64 quaternion, out Matrix3x3FP64 matrix) + { + double s0s0 = quaternion.s0 * quaternion.s0; + double x1x1 = quaternion.x1 * quaternion.x1; + double x2x2 = quaternion.x2 * quaternion.x2; + double x3x3 = quaternion.x3 * quaternion.x3; + + double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus)) + { + Matrix3x3FP64.LoadIdentity(out matrix); + return false; + } + + double corrector1 = 1.0 / squareModulus; + + double s0x1 = quaternion.s0 * quaternion.x1; + double s0x2 = quaternion.s0 * quaternion.x2; + double s0x3 = quaternion.s0 * quaternion.x3; + double x1x2 = quaternion.x1 * quaternion.x2; + double x1x3 = quaternion.x1 * quaternion.x3; + double x2x3 = quaternion.x2 * quaternion.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)); + + matrix.r1c2 = corrector2 * (x1x2 + s0x3); + matrix.r2c3 = corrector2 * (x2x3 + s0x1); + matrix.r3c1 = corrector2 * (x1x3 + s0x2); + + matrix.r2c1 = corrector2 * (x1x2 - s0x3); + matrix.r3c2 = corrector2 * (x2x3 - s0x1); + matrix.r1c3 = corrector2 * (x1x3 - s0x2); + + return true; + } + + public static bool GetBothMatrices(in QuaternionFP64 quaternion, out Matrix3x3FP64 matrix, out Matrix3x3FP64 reverse) + { + if (GetReverseMatrix(quaternion, out reverse)) + { + Matrix3x3FP64.GetTransposed(reverse, out matrix); + return true; + } + + Matrix3x3FP64.LoadIdentity(out matrix); + return false; + } + public static bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2) { double ds0 = quaternion1.s0 - quaternion2.s0; diff --git a/BasicGeometry/RadianFP32.cs b/BasicGeometry/RadianFP32.cs index 423a1c3..496cea5 100644 --- a/BasicGeometry/RadianFP32.cs +++ b/BasicGeometry/RadianFP32.cs @@ -4,7 +4,7 @@ * Date: 18 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class RadianFP32 { diff --git a/BasicGeometry/RadianFP64.cs b/BasicGeometry/RadianFP64.cs index e8c7196..c2ef9fb 100644 --- a/BasicGeometry/RadianFP64.cs +++ b/BasicGeometry/RadianFP64.cs @@ -4,7 +4,7 @@ * Date: 18 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class RadianFP64 { diff --git a/BasicGeometry/Rotation3FP32.cs b/BasicGeometry/Rotation3FP32.cs index c322466..0549405 100644 --- a/BasicGeometry/Rotation3FP32.cs +++ b/BasicGeometry/Rotation3FP32.cs @@ -4,7 +4,7 @@ * Date: 2 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Rotation3FP32 { diff --git a/BasicGeometry/Rotation3FP64.cs b/BasicGeometry/Rotation3FP64.cs index e596c0b..38c1d6c 100644 --- a/BasicGeometry/Rotation3FP64.cs +++ b/BasicGeometry/Rotation3FP64.cs @@ -4,7 +4,7 @@ * Date: 2 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Rotation3FP64 { diff --git a/BasicGeometry/TurnFP32.cs b/BasicGeometry/TurnFP32.cs index 82dbae3..d9a7627 100644 --- a/BasicGeometry/TurnFP32.cs +++ b/BasicGeometry/TurnFP32.cs @@ -4,7 +4,7 @@ * Date: 18 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class TurnFP32 { diff --git a/BasicGeometry/TurnFP64.cs b/BasicGeometry/TurnFP64.cs index 2f6a004..8a856f3 100644 --- a/BasicGeometry/TurnFP64.cs +++ b/BasicGeometry/TurnFP64.cs @@ -4,7 +4,7 @@ * Date: 18 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class TurnFP64 { diff --git a/BasicGeometry/UtilityFP32.cs b/BasicGeometry/UtilityFP32.cs index 62982be..fa64a9b 100644 --- a/BasicGeometry/UtilityFP32.cs +++ b/BasicGeometry/UtilityFP32.cs @@ -4,12 +4,12 @@ * Date: 12 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class UtilityFP32 { public const float EPSYLON = 4.76837E-7f; - public const float SQUARE_EPSYLON = 2.27373906E-13f; + public const float SQUARE_EPSYLON = EPSYLON * EPSYLON; public const float EPSYLON_EFFECTIVENESS_LIMIT = 1.0f; diff --git a/BasicGeometry/UtilityFP64.cs b/BasicGeometry/UtilityFP64.cs index 95cc23b..587ad88 100644 --- a/BasicGeometry/UtilityFP64.cs +++ b/BasicGeometry/UtilityFP64.cs @@ -4,12 +4,12 @@ * Date: 12 Nov 2024 */ -namespace BasicGeometry +namespace BGC { public class UtilityFP64 { public const double EPSYLON = 4.996003611E-14; - public const double SQUARE_EPSYLON = 2.496005208112504E-27; + public const double SQUARE_EPSYLON = EPSYLON * EPSYLON; public const double EPSYLON_EFFECTIVENESS_LIMIT = 1.0; diff --git a/BasicGeometry/Vector2FP32.cs b/BasicGeometry/Vector2FP32.cs index 9836ab9..10646f2 100644 --- a/BasicGeometry/Vector2FP32.cs +++ b/BasicGeometry/Vector2FP32.cs @@ -4,7 +4,7 @@ * Date: 1 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Vector2FP32 { @@ -41,35 +41,6 @@ namespace BasicGeometry return MathF.Sqrt(this.GetSquareModulus()); } - public int Normalize() - { - float squareModulus = this.GetSquareModulus(); - - if (UtilityFP32.IsSqareUnit(squareModulus)) - { - return 1; - } - - if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) - { - this.Reset(); - return 0; - } - - float multiplier = MathF.Sqrt(1.0f / squareModulus); - - this.x1 *= multiplier; - this.x2 *= multiplier; - - return 1; - } - - public void Reverse() - { - this.x1 = -this.x1; - this.x2 = -this.x2; - } - public readonly bool IsZero() { return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON; @@ -86,6 +57,39 @@ namespace BasicGeometry this.x2 = 0.0f; } + public void MakeOpposite() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + } + + public readonly Vector2FP32 GetOpposite() + { + return new Vector2FP32(-this.x1, -this.x2); + } + + public bool Normalize() + { + float squareModulus = this.GetSquareModulus(); + + if (UtilityFP32.IsSqareUnit(squareModulus)) + { + return true; + } + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + return false; + } + + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + this.x1 *= multiplier; + this.x2 *= multiplier; + + return true; + } + public void SetValues(float x1, float x2) { this.x1 = x1; @@ -103,28 +107,59 @@ namespace BasicGeometry this.x1 = (float)vector.x1; this.x2 = (float)vector.x2; } - - public void SetReverseOf(in Vector2FP32 vector) - { - this.x1 = -vector.x1; - this.x2 = -vector.x2; - } - - public void SetReverseOf(in Vector2FP64 vector) - { - this.x1 = -(float)vector.x1; - this.x2 = -(float)vector.x2; - } - - public void AppendScaled(Vector2FP32 summand, float scale) - { - this.x1 += summand.x1 * scale; - this.x2 += summand.x2 * scale; - } public readonly override string ToString() { - return String.Format("SPVector2({0}, {1})", this.x1, this.x2); + return String.Format("Vector2FP32({0}, {1})", this.x1, this.x2); + } + + public static void Swap(ref Vector2FP32 vector1, ref Vector2FP32 vector2) + { + float x1 = vector1.x1; + float x2 = vector1.x2; + + vector1.x1 = vector2.x1; + vector1.x2 = vector2.x2; + + vector2.x1 = x1; + vector2.x2 = x2; + } + + public static void Reset(out Vector2FP32 vector) + { + vector.x1 = 0.0f; + vector.x2 = 0.0f; + } + + public static void GetOpposite(in Vector2FP32 vector, out Vector2FP32 reverted) + { + reverted.x1 = -vector.x1; + reverted.x2 = -vector.x2; + } + + public static bool GetNormalized(in Vector2FP32 vector, out Vector2FP32 normalized) + { + float squareModulus = vector.GetSquareModulus(); + + if (UtilityFP32.IsSqareUnit(squareModulus)) + { + normalized.x1 = vector.x1; + normalized.x2 = vector.x2; + return true; + } + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + normalized.x1 = 0.0f; + normalized.x2 = 0.0f; + return false; + } + + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + normalized.x1 = vector.x1 * multiplier; + normalized.x2 = vector.x2 * multiplier; + return true; } public static void Add(in Vector2FP32 vector1, in Vector2FP32 vector2, out Vector2FP32 sum) @@ -133,6 +168,12 @@ namespace BasicGeometry sum.x2 = vector1.x2 + vector2.x2; } + public static void AddScaled(in Vector2FP32 basicVector, in Vector2FP32 scalableVector, float scale, out Vector2FP32 sum) + { + sum.x1 = basicVector.x1 + scalableVector.x1 * scale; + sum.x2 = basicVector.x2 + scalableVector.x2 * scale; + } + public static void Subtract(in Vector2FP32 minuend, in Vector2FP32 subtrahend, out Vector2FP32 difference) { difference.x1 = minuend.x1 - subtrahend.x1; @@ -150,16 +191,24 @@ namespace BasicGeometry Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetMean2(in Vector2FP32 vector1, in Vector2FP32 vector2, out Vector2FP32 result) + public static void GetMeanOfTwo(in Vector2FP32 vector1, in Vector2FP32 vector2, out Vector2FP32 mean) { - result.x1 = (vector1.x1 + vector2.x1) * 0.5f; - result.x2 = (vector1.x2 + vector2.x2) * 0.5f; + mean.x1 = (vector1.x1 + vector2.x1) * 0.5f; + mean.x2 = (vector1.x2 + vector2.x2) * 0.5f; } - public static void GetMean3(in Vector2FP32 vector1, in Vector2FP32 vector2, in Vector2FP32 vector3, out Vector2FP32 result) + public static void GetMeanOfThree(in Vector2FP32 vector1, in Vector2FP32 vector2, in Vector2FP32 vector3, out Vector2FP32 mean) { - result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD; - result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD; + mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD; + mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD; + } + + public static void Interpolate(in Vector2FP32 vector1, in Vector2FP32 vector2, float phase, out Vector2FP32 interpolation) + { + float counterphase = 1.0f - phase; + + interpolation.x1 = vector1.x1 * counterphase + vector2.x1 * phase; + interpolation.x2 = vector1.x2 * counterphase + vector2.x2 * phase; } public static float GetScalarProduct(in Vector2FP32 vector1, in Vector2FP32 vector2) @@ -176,31 +225,24 @@ namespace BasicGeometry { float squareModulus1 = vector1.GetSquareModulus(); - if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus1)) { return 0.0f; } float squareModulus2 = vector2.GetSquareModulus(); - if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus2)) { return 0.0f; } - float cosine = Vector2FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2); + float multiplier = MathF.Sqrt(1.0f / (squareModulus1 * squareModulus2)); - if (1.0f - UtilityFP32.EPSYLON <= cosine) - { - return 0.0f; - } + float x = GetScalarProduct(vector1, vector2); + float y = GetCrossProduct(vector1, vector2); - if (cosine <= -(1.0f - UtilityFP32.EPSYLON)) - { - return AngleFP32.GetHalfCircle(unit); - } - - return RadianFP32.ToUnits(MathF.Acos(cosine), unit); + return RadianFP32.ToUnits(MathF.Atan2(y * multiplier, x * multiplier), unit); } public static float GetSquareDistance(in Vector2FP32 vector1, in Vector2FP32 vector2) @@ -215,25 +257,82 @@ namespace BasicGeometry { return MathF.Sqrt(GetSquareDistance(vector1, vector2)); } - - public static bool AreEqual(in Vector2FP32 vector1, in Vector2FP32 vector2) + + public static bool AreCloseEnough(in Vector2FP32 vector1, in Vector2FP32 vector2, float distanceLimit) + { + return 0.0f <= distanceLimit && GetSquareDistance(vector1, vector2) <= distanceLimit * distanceLimit; + } + + public static bool AreClose(in Vector2FP32 vector1, in Vector2FP32 vector2) { float squareModulus1 = vector1.GetSquareModulus(); float squareModulus2 = vector2.GetSquareModulus(); - float squareModulus3 = GetSquareDistance(vector1, vector2); + float squareDistance = GetSquareDistance(vector1, vector2); - // 2.0f means dimension amount - if (squareModulus1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModulus3 < (2.0f * UtilityFP32.SQUARE_EPSYLON); + return squareDistance <= UtilityFP32.SQUARE_EPSYLON; } - if (squareModulus1 <= squareModulus2) + return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2; + } + + public static bool AreParallel(in Vector2FP32 vector1, in Vector2FP32 vector2) + { + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) { - return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus2; + return true; } - return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus1; + float crossProduct = GetCrossProduct(vector1, vector2); + + return crossProduct * crossProduct <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + } + + public static bool AreOrthogonal(in Vector2FP32 vector1, in Vector2FP32 vector2) + { + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) + { + return true; + } + + float scalarProduct = GetScalarProduct(vector1, vector2); + + return scalarProduct * scalarProduct <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + } + + public static Attitude GetAttitude(in Vector2FP32 vector1, in Vector2FP32 vector2) + { + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) + { + return Attitude.ZERO; + } + + float squareLimit = UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + float scalarProduct = GetScalarProduct(vector1, vector2); + + if (scalarProduct * scalarProduct <= squareLimit) + { + return Attitude.ORTHOGONAL; + } + + float crossProduct = GetCrossProduct(vector1, vector2); + + if (crossProduct * crossProduct > squareLimit) + { + return Attitude.ANY; + } + + return scalarProduct > 0.0f ? Attitude.CO_DIRECTIONAL : Attitude.COUNTER_DIRECTIONAL; } } } diff --git a/BasicGeometry/Vector2FP64.cs b/BasicGeometry/Vector2FP64.cs index 25be102..4f077bc 100644 --- a/BasicGeometry/Vector2FP64.cs +++ b/BasicGeometry/Vector2FP64.cs @@ -4,7 +4,7 @@ * Date: 1 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Vector2FP64 { @@ -41,35 +41,6 @@ namespace BasicGeometry return Math.Sqrt(this.GetSquareModulus()); } - public int Normalize() - { - double squareModulus = this.GetSquareModulus(); - - if (UtilityFP64.IsSqareUnit(squareModulus)) - { - return 1; - } - - if (squareModulus <= UtilityFP64.SQUARE_EPSYLON) - { - this.Reset(); - return 0; - } - - double multiplier = Math.Sqrt(1.0 / squareModulus); - - this.x1 *= multiplier; - this.x2 *= multiplier; - - return 1; - } - - public void Reverse() - { - this.x1 = -this.x1; - this.x2 = -this.x2; - } - public readonly bool IsZero() { return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON; @@ -86,6 +57,39 @@ namespace BasicGeometry this.x2 = 0.0; } + public void MakeOpposite() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + } + + public readonly Vector2FP64 GetOpposite() + { + return new Vector2FP64(-this.x1, -this.x2); + } + + public bool Normalize() + { + double squareModulus = this.GetSquareModulus(); + + if (UtilityFP64.IsSqareUnit(squareModulus)) + { + return true; + } + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus)) + { + return false; + } + + double multiplier = Math.Sqrt(1.0 / squareModulus); + + this.x1 *= multiplier; + this.x2 *= multiplier; + + return true; + } + public void SetValues(double x1, double x2) { this.x1 = x1; @@ -104,27 +108,58 @@ namespace BasicGeometry this.x2 = vector.x2; } - public void SetReverseOf(in Vector2FP64 vector) - { - this.x1 = -vector.x1; - this.x2 = -vector.x2; - } - - public void SetReverseOf(in Vector2FP32 vector) - { - this.x1 = -vector.x1; - this.x2 = -vector.x2; - } - - public void AppendScaled(Vector2FP64 summand, double scale) - { - this.x1 += summand.x1 * scale; - this.x2 += summand.x2 * scale; - } - public readonly override string ToString() { - return String.Format("DPVector2({0}, {1})", this.x1, this.x2); + return String.Format("Vector2FP64({0}, {1})", this.x1, this.x2); + } + + public static void Swap(ref Vector2FP64 vector1, ref Vector2FP64 vector2) + { + double x1 = vector1.x1; + double x2 = vector1.x2; + + vector1.x1 = vector2.x1; + vector1.x2 = vector2.x2; + + vector2.x1 = x1; + vector2.x2 = x2; + } + + public static void Reset(out Vector2FP64 vector) + { + vector.x1 = 0.0; + vector.x2 = 0.0; + } + + public static void GetOpposite(in Vector2FP64 vector, out Vector2FP64 reverted) + { + reverted.x1 = -vector.x1; + reverted.x2 = -vector.x2; + } + + public static bool GetNormalized(in Vector2FP64 vector, out Vector2FP64 normalized) + { + double squareModulus = vector.GetSquareModulus(); + + if (UtilityFP64.IsSqareUnit(squareModulus)) + { + normalized.x1 = vector.x1; + normalized.x2 = vector.x2; + return true; + } + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus)) + { + normalized.x1 = 0.0; + normalized.x2 = 0.0; + return false; + } + + double multiplier = Math.Sqrt(1.0 / squareModulus); + + normalized.x1 = vector.x1 * multiplier; + normalized.x2 = vector.x2 * multiplier; + return true; } public static void Add(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 sum) @@ -133,6 +168,12 @@ namespace BasicGeometry sum.x2 = vector1.x2 + vector2.x2; } + public static void AddScaled(in Vector2FP64 basicVector, in Vector2FP64 scalableVector, double scale, out Vector2FP64 sum) + { + sum.x1 = basicVector.x1 + scalableVector.x1 * scale; + sum.x2 = basicVector.x2 + scalableVector.x2 * scale; + } + public static void Subtract(in Vector2FP64 minuend, in Vector2FP64 subtrahend, out Vector2FP64 difference) { difference.x1 = minuend.x1 - subtrahend.x1; @@ -150,16 +191,24 @@ namespace BasicGeometry Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetMean2(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 result) + public static void GetMeanOfTwo(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 mean) { - result.x1 = (vector1.x1 + vector2.x1) * 0.5; - result.x2 = (vector1.x2 + vector2.x2) * 0.5; + mean.x1 = (vector1.x1 + vector2.x1) * 0.5; + mean.x2 = (vector1.x2 + vector2.x2) * 0.5; } - public static void GetMean3(in Vector2FP64 vector1, in Vector2FP64 vector2, in Vector2FP64 vector3, out Vector2FP64 result) + public static void GetMeanOfThree(in Vector2FP64 vector1, in Vector2FP64 vector2, in Vector2FP64 vector3, out Vector2FP64 mean) { - result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD; - result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD; + mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD; + mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD; + } + + public static void Interpolate(in Vector2FP64 vector1, in Vector2FP64 vector2, double phase, out Vector2FP64 interpolation) + { + double counterphase = 1.0 - phase; + + interpolation.x1 = vector1.x1 * counterphase + vector2.x1 * phase; + interpolation.x2 = vector1.x2 * counterphase + vector2.x2 * phase; } public static double GetScalarProduct(in Vector2FP64 vector1, in Vector2FP64 vector2) @@ -176,31 +225,24 @@ namespace BasicGeometry { double squareModulus1 = vector1.GetSquareModulus(); - if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus1)) { - return 0.0; + return 0.0f; } double squareModulus2 = vector2.GetSquareModulus(); - if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus2)) { - return 0.0; + return 0.0f; } - double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2); + double multiplier = Math.Sqrt(1.0 / (squareModulus1 * squareModulus2)); - if (1.0 - UtilityFP64.EPSYLON <= cosine) - { - return 0.0; - } + double x = GetScalarProduct(vector1, vector2); + double y = GetCrossProduct(vector1, vector2); - if (cosine <= -(1.0 - UtilityFP64.EPSYLON)) - { - return AngleFP64.GetHalfCircle(unit); - } - - return RadianFP64.ToUnits(Math.Acos(cosine), unit); + return RadianFP64.ToUnits(Math.Atan2(y * multiplier, x * multiplier), unit); } public static double GetSquareDistance(in Vector2FP64 vector1, in Vector2FP64 vector2) @@ -215,25 +257,82 @@ namespace BasicGeometry { return Math.Sqrt(GetSquareDistance(vector1, vector2)); } - - public static bool AreEqual(in Vector2FP64 vector1, in Vector2FP64 vector2) + + public static bool AreCloseEnough(in Vector2FP64 vector1, in Vector2FP64 vector2, double distanceLimit) + { + return 0.0 <= distanceLimit && GetSquareDistance(vector1, vector2) <= distanceLimit * distanceLimit; + } + + public static bool AreClose(in Vector2FP64 vector1, in Vector2FP64 vector2) { double squareModulus1 = vector1.GetSquareModulus(); double squareModulus2 = vector2.GetSquareModulus(); - double squareModulus3 = GetSquareDistance(vector1, vector2); + double squareDistance = GetSquareDistance(vector1, vector2); - // 2.0 means dimension amount - if (squareModulus1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModulus3 < (2.0 * UtilityFP64.SQUARE_EPSYLON); + return squareDistance <= UtilityFP64.SQUARE_EPSYLON; } - if (squareModulus1 <= squareModulus2) + return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2; + } + + public static bool AreParallel(in Vector2FP64 vector1, in Vector2FP64 vector2) + { + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) { - return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus2; + return true; } - return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus1; + double crossProduct = GetCrossProduct(vector1, vector2); + + return crossProduct * crossProduct <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + } + + public static bool AreOrthogonal(in Vector2FP64 vector1, in Vector2FP64 vector2) + { + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) + { + return true; + } + + double scalarProduct = GetScalarProduct(vector1, vector2); + + return scalarProduct * scalarProduct <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + } + + public static Attitude GetAttitude(in Vector2FP64 vector1, in Vector2FP64 vector2) + { + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) + { + return Attitude.ZERO; + } + + double squareLimit = UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + double scalarProduct = GetScalarProduct(vector1, vector2); + + if (scalarProduct * scalarProduct <= squareLimit) + { + return Attitude.ORTHOGONAL; + } + + double crossProduct = GetCrossProduct(vector1, vector2); + + if (crossProduct * crossProduct > squareLimit) + { + return Attitude.ANY; + } + + return scalarProduct > 0.0 ? Attitude.CO_DIRECTIONAL : Attitude.COUNTER_DIRECTIONAL; } } } diff --git a/BasicGeometry/Vector3FP32.cs b/BasicGeometry/Vector3FP32.cs index 2b2119d..7c563d9 100644 --- a/BasicGeometry/Vector3FP32.cs +++ b/BasicGeometry/Vector3FP32.cs @@ -4,7 +4,7 @@ * Date: 1 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Vector3FP32 { @@ -45,37 +45,6 @@ namespace BasicGeometry return MathF.Sqrt(this.GetSquareModulus()); } - public int Normalize() - { - float squareModulus = this.GetSquareModulus(); - - if (UtilityFP32.IsSqareUnit(squareModulus)) - { - return 1; - } - - if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) - { - this.Reset(); - return 0; - } - - float multiplier = MathF.Sqrt(1.0f / squareModulus); - - this.x1 *= multiplier; - this.x2 *= multiplier; - this.x3 *= multiplier; - - return 1; - } - - public void Reverse() - { - this.x1 = -this.x1; - this.x2 = -this.x2; - this.x3 = -this.x3; - } - public readonly bool IsZero() { return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON; @@ -93,6 +62,41 @@ namespace BasicGeometry this.x3 = 0.0f; } + public void MakeOpposite() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public readonly Vector3FP32 GetOpposite() + { + return new Vector3FP32(-this.x1, -this.x2, -this.x3); + } + + public bool Normalize() + { + float squareModulus = this.GetSquareModulus(); + + if (UtilityFP32.IsSqareUnit(squareModulus)) + { + return true; + } + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + return false; + } + + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; + + return true; + } + public void SetValues(float x1, float x2, float x3) { this.x1 = x1; @@ -114,30 +118,66 @@ namespace BasicGeometry this.x3 = vector.x3; } - public void SetReverseOf(in Vector3FP32 vector) - { - this.x1 = -vector.x1; - this.x2 = -vector.x2; - this.x3 = -vector.x3; - } - - public void SetReverseOf(in Vector3FP64 vector) - { - this.x1 = -(float)vector.x1; - this.x2 = -(float)vector.x2; - this.x3 = -(float)vector.x3; - } - - public void AppendScaled(Vector3FP32 summand, float scale) - { - this.x1 += summand.x1 * scale; - this.x2 += summand.x2 * scale; - this.x3 += summand.x3 * scale; - } - public readonly override string ToString() { - return String.Format("SPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3); + return String.Format("Vector3FP32({0}, {1}, {2})", this.x1, this.x2, this.x3); + } + + public static void Swap(ref Vector3FP32 vector1, ref Vector3FP32 vector2) + { + float x1 = vector1.x1; + float x2 = vector1.x2; + float x3 = vector1.x3; + + vector1.x1 = vector2.x1; + vector1.x2 = vector2.x2; + vector1.x3 = vector2.x3; + + vector2.x1 = x1; + vector2.x2 = x2; + vector2.x3 = x3; + } + + public static void Reset(out Vector3FP32 vector) + { + vector.x1 = 0.0f; + vector.x2 = 0.0f; + vector.x3 = 0.0f; + } + + public static void GetOpposite(in Vector3FP32 vector, out Vector3FP32 reverted) + { + reverted.x1 = -vector.x1; + reverted.x2 = -vector.x2; + reverted.x3 = -vector.x3; + } + + public static bool GetNormalized(in Vector3FP32 vector, out Vector3FP32 normalized) + { + float squareModulus = vector.GetSquareModulus(); + + if (UtilityFP32.IsSqareUnit(squareModulus)) + { + normalized.x1 = vector.x1; + normalized.x2 = vector.x2; + normalized.x3 = vector.x3; + return true; + } + + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus)) + { + normalized.x1 = 0.0f; + normalized.x2 = 0.0f; + normalized.x3 = 0.0f; + return false; + } + + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + normalized.x1 = vector.x1 * multiplier; + normalized.x2 = vector.x2 * multiplier; + normalized.x3 = vector.x3 * multiplier; + return true; } public static void Add(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 sum) @@ -147,6 +187,13 @@ namespace BasicGeometry sum.x3 = vector1.x3 + vector2.x3; } + public static void AddScaled(in Vector3FP32 basicVector, in Vector3FP32 scalableVector, float scale, out Vector3FP32 sum) + { + sum.x1 = basicVector.x1 + scalableVector.x1 * scale; + sum.x2 = basicVector.x2 + scalableVector.x2 * scale; + sum.x3 = basicVector.x3 + scalableVector.x3 * scale; + } + public static void Subtract(in Vector3FP32 minuend, in Vector3FP32 subtrahend, out Vector3FP32 difference) { difference.x1 = minuend.x1 - subtrahend.x1; @@ -166,35 +213,33 @@ namespace BasicGeometry Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetMean2(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 result) + public static void GetMeanOfTwo(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 mean) { - result.x1 = (vector1.x1 + vector2.x1) * 0.5f; - result.x2 = (vector1.x2 + vector2.x2) * 0.5f; - result.x3 = (vector1.x3 + vector2.x3) * 0.5f; + mean.x1 = (vector1.x1 + vector2.x1) * 0.5f; + mean.x2 = (vector1.x2 + vector2.x2) * 0.5f; + mean.x3 = (vector1.x3 + vector2.x3) * 0.5f; } - public static void GetMean3(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 result) + public static void GetMeanOfThree(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 mean) { - result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD; - result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD; - result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP32.ONE_THIRD; + mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD; + mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD; + mean.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP32.ONE_THIRD; + } + + public static void Interpolate(in Vector3FP32 vector1, in Vector3FP32 vector2, float phase, out Vector3FP32 interpolation) + { + float counterphase = 1.0f - phase; + + interpolation.x1 = vector1.x1 * counterphase + vector2.x1 * phase; + interpolation.x2 = vector1.x2 * counterphase + vector2.x2 * phase; + interpolation.x3 = vector1.x3 * counterphase + vector2.x3 * phase; } public static float GetScalarProduct(in Vector3FP32 vector1, in Vector3FP32 vector2) { return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3; } - - public static void GetCrossProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 result) - { - float x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2; - float x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3; - float x3 = vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1; - - result.x1 = x1; - result.x2 = x2; - result.x3 = x3; - } public static float GetTripleProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3) { @@ -202,47 +247,55 @@ namespace BasicGeometry + vector1.x2 * (vector2.x3 * vector3.x1 - vector2.x1 * vector3.x3) + vector1.x3 * (vector2.x1 * vector3.x2 - vector2.x2 * vector3.x1); } - - public static void GetDoubleCrossProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 result) + + public static void GetCrossProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 product) + { + float x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2; + float x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3; + float x3 = vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1; + + product.x1 = x1; + product.x2 = x2; + product.x3 = x3; + } + + // [a x [b x c]] = b * (a, c) - c * (a, b) + public static void GetDoubleCrossProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 product) { - // [a x [b x c]] = b * (a, c) - c * (a, b) float ac = GetScalarProduct(vector1, vector3); float ab = GetScalarProduct(vector1, vector2); - result.x1 = ac * vector2.x1 - ab * vector3.x1; - result.x2 = ac * vector2.x2 - ab * vector3.x2; - result.x3 = ac * vector2.x3 - ab * vector3.x3; + product.x1 = ac * vector2.x1 - ab * vector3.x1; + product.x2 = ac * vector2.x2 - ab * vector3.x2; + product.x3 = ac * vector2.x3 - ab * vector3.x3; } public static float GetAngle(in Vector3FP32 vector1, in Vector3FP32 vector2, AngleUnit unit) { float squareModulus1 = vector1.GetSquareModulus(); - if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus1)) { return 0.0f; } float squareModulus2 = vector2.GetSquareModulus(); - if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus2)) { return 0.0f; } - float cosine = Vector3FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2); + float multiplier = MathF.Sqrt(1.0f / (squareModulus1 * squareModulus2)); - if (1.0f - UtilityFP32.EPSYLON <= cosine) - { - return 0.0f; - } + Vector3FP32 crossProduct; - if (cosine <= -(1.0f - UtilityFP32.EPSYLON)) - { - return AngleFP32.GetHalfCircle(unit); - } - - return RadianFP32.ToUnits(MathF.Acos(cosine), unit); + GetCrossProduct(vector1, vector2, out crossProduct); + + float x = GetScalarProduct(vector1, vector2); + float y = crossProduct.GetModulus(); + + return RadianFP32.ToUnits(MathF.Atan2(y * multiplier, x * multiplier), unit); } public static float GetSquareDistance(in Vector3FP32 vector1, in Vector3FP32 vector2) @@ -258,14 +311,18 @@ namespace BasicGeometry { return MathF.Sqrt(GetSquareDistance(vector1, vector2)); } - - public static bool AreEqual(in Vector3FP32 vector1, in Vector3FP32 vector2) + + public static bool AreCloseEnough(in Vector3FP32 vector1, in Vector3FP32 vector2, float distanceLimit) + { + return 0.0f <= distanceLimit && GetSquareDistance(vector1, vector2) <= distanceLimit * distanceLimit; + } + + public static bool AreClose(in Vector3FP32 vector1, in Vector3FP32 vector2) { float squareModulus1 = vector1.GetSquareModulus(); float squareModulus2 = vector2.GetSquareModulus(); float squareDistance = GetSquareDistance(vector1, vector2); - // 3.0f means dimension amount if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) { return squareDistance <= UtilityFP32.SQUARE_EPSYLON; @@ -273,5 +330,67 @@ namespace BasicGeometry return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2; } + + public static bool AreParallel(in Vector3FP32 vector1, in Vector3FP32 vector2) + { + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) + { + return true; + } + + Vector3FP32 crossProduct; + + GetCrossProduct(vector1, vector2, out crossProduct); + + return crossProduct.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + } + + public static bool AreOrthogonal(in Vector3FP32 vector1, in Vector3FP32 vector2) + { + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) + { + return true; + } + + float scalarProduct = GetScalarProduct(vector1, vector2); + + return scalarProduct * scalarProduct <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + } + + public static Attitude GetAttitude(in Vector3FP32 vector1, in Vector3FP32 vector2) + { + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) + { + return Attitude.ZERO; + } + + float squareLimit = UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + float scalarProduct = GetScalarProduct(vector1, vector2); + + if (scalarProduct * scalarProduct <= squareLimit) + { + return Attitude.ORTHOGONAL; + } + + Vector3FP32 crossProduct; + + GetCrossProduct(vector1, vector2, out crossProduct); + + if (crossProduct.GetSquareModulus() > squareLimit) + { + return Attitude.ANY; + } + + return scalarProduct > 0.0f ? Attitude.CO_DIRECTIONAL : Attitude.COUNTER_DIRECTIONAL; + } } } diff --git a/BasicGeometry/Vector3FP64.cs b/BasicGeometry/Vector3FP64.cs index 31505f4..f112580 100644 --- a/BasicGeometry/Vector3FP64.cs +++ b/BasicGeometry/Vector3FP64.cs @@ -4,7 +4,7 @@ * Date: 1 Feb 2019 */ -namespace BasicGeometry +namespace BGC { public struct Vector3FP64 { @@ -45,38 +45,6 @@ namespace BasicGeometry return Math.Sqrt(this.GetSquareModulus()); } - public int Normalize() - { - double squareModulus = this.GetSquareModulus(); - - - if (UtilityFP64.IsSqareUnit(squareModulus)) - { - return 1; - } - - if (squareModulus <= UtilityFP64.SQUARE_EPSYLON) - { - this.Reset(); - return 0; - } - - double multiplier = Math.Sqrt(1.0 / squareModulus); - - this.x1 *= multiplier; - this.x2 *= multiplier; - this.x3 *= multiplier; - - return 1; - } - - public void Reverse() - { - this.x1 = -this.x1; - this.x2 = -this.x2; - this.x3 = -this.x3; - } - public readonly bool IsZero() { return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON; @@ -94,6 +62,41 @@ namespace BasicGeometry this.x3 = 0.0; } + public void MakeOpposite() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public readonly Vector3FP64 GetOpposite() + { + return new Vector3FP64(-this.x1, -this.x2, -this.x3); + } + + public bool Normalize() + { + double squareModulus = this.GetSquareModulus(); + + if (UtilityFP64.IsSqareUnit(squareModulus)) + { + return true; + } + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus)) + { + return false; + } + + double multiplier = Math.Sqrt(1.0 / squareModulus); + + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; + + return true; + } + public void SetValues(double x1, double x2, double x3) { this.x1 = x1; @@ -115,30 +118,66 @@ namespace BasicGeometry this.x3 = vector.x3; } - public void SetReverseOf(in Vector3FP64 vector) - { - this.x1 = -vector.x1; - this.x2 = -vector.x2; - this.x3 = -vector.x3; - } - - public void SetReverseOf(in Vector3FP32 vector) - { - this.x1 = -vector.x1; - this.x2 = -vector.x2; - this.x3 = -vector.x3; - } - - public void AppendScaled(Vector3FP64 summand, double scale) - { - this.x1 += summand.x1 * scale; - this.x2 += summand.x2 * scale; - this.x3 += summand.x3 * scale; - } - public readonly override string ToString() { - return String.Format("DPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3); + return String.Format("Vector3FP64({0}, {1}, {2})", this.x1, this.x2, this.x3); + } + + public static void Swap(ref Vector3FP64 vector1, ref Vector3FP64 vector2) + { + double x1 = vector1.x1; + double x2 = vector1.x2; + double x3 = vector1.x3; + + vector1.x1 = vector2.x1; + vector1.x2 = vector2.x2; + vector1.x3 = vector2.x3; + + vector2.x1 = x1; + vector2.x2 = x2; + vector2.x3 = x3; + } + + public static void Reset(out Vector3FP64 vector) + { + vector.x1 = 0.0; + vector.x2 = 0.0; + vector.x3 = 0.0; + } + + public static void GetOpposite(in Vector3FP64 vector, out Vector3FP64 reverted) + { + reverted.x1 = -vector.x1; + reverted.x2 = -vector.x2; + reverted.x3 = -vector.x3; + } + + public static bool GetNormalized(in Vector3FP64 vector, out Vector3FP64 normalized) + { + double squareModulus = vector.GetSquareModulus(); + + if (UtilityFP64.IsSqareUnit(squareModulus)) + { + normalized.x1 = vector.x1; + normalized.x2 = vector.x2; + normalized.x3 = vector.x3; + return true; + } + + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus)) + { + normalized.x1 = 0.0; + normalized.x2 = 0.0; + normalized.x3 = 0.0; + return false; + } + + double multiplier = Math.Sqrt(1.0 / squareModulus); + + normalized.x1 = vector.x1 * multiplier; + normalized.x2 = vector.x2 * multiplier; + normalized.x3 = vector.x3 * multiplier; + return true; } public static void Add(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 sum) @@ -148,6 +187,13 @@ namespace BasicGeometry sum.x3 = vector1.x3 + vector2.x3; } + public static void AddScaled(in Vector3FP64 basicVector, in Vector3FP64 scalableVector, double scale, out Vector3FP64 sum) + { + sum.x1 = basicVector.x1 + scalableVector.x1 * scale; + sum.x2 = basicVector.x2 + scalableVector.x2 * scale; + sum.x3 = basicVector.x3 + scalableVector.x3 * scale; + } + public static void Subtract(in Vector3FP64 minuend, in Vector3FP64 subtrahend, out Vector3FP64 difference) { difference.x1 = minuend.x1 - subtrahend.x1; @@ -167,25 +213,41 @@ namespace BasicGeometry Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetMean2(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 result) + public static void GetMeanOfTwo(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 mean) { - result.x1 = (vector1.x1 + vector2.x1) * 0.5; - result.x2 = (vector1.x2 + vector2.x2) * 0.5; - result.x3 = (vector1.x3 + vector2.x3) * 0.5; + mean.x1 = (vector1.x1 + vector2.x1) * 0.5; + mean.x2 = (vector1.x2 + vector2.x2) * 0.5; + mean.x3 = (vector1.x3 + vector2.x3) * 0.5; } - public static void GetMean3(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 result) + public static void GetMeanOfThree(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 mean) { - result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD; - result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD; - result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP64.ONE_THIRD; + mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD; + mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD; + mean.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP64.ONE_THIRD; + } + + public static void Interpolate(in Vector3FP64 vector1, in Vector3FP64 vector2, double phase, out Vector3FP64 interpolation) + { + double counterphase = 1.0 - phase; + + interpolation.x1 = vector1.x1 * counterphase + vector2.x1 * phase; + interpolation.x2 = vector1.x2 * counterphase + vector2.x2 * phase; + interpolation.x3 = vector1.x3 * counterphase + vector2.x3 * phase; } public static double GetScalarProduct(in Vector3FP64 vector1, in Vector3FP64 vector2) { return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3; } - + + public static double GetTripleProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3) + { + return vector1.x1 * (vector2.x2 * vector3.x3 - vector2.x3 * vector3.x2) + + vector1.x2 * (vector2.x3 * vector3.x1 - vector2.x1 * vector3.x3) + + vector1.x3 * (vector2.x1 * vector3.x2 - vector2.x2 * vector3.x1); + } + public static void GetCrossProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 result) { double x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2; @@ -197,55 +259,45 @@ namespace BasicGeometry result.x3 = x3; } - public static double GetTripleProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3) + // [a x [b x c]] = b * (a, c) - c * (a, b) + public static void GetDoubleCrossProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 product) { - return vector1.x1 * (vector2.x2 * vector3.x3 - vector2.x3 * vector3.x2) - + vector1.x2 * (vector2.x3 * vector3.x1 - vector2.x1 * vector3.x3) - + vector1.x3 * (vector2.x1 * vector3.x2 - vector2.x2 * vector3.x1); - } - - public static void GetDoubleCrossProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 result) - { - // [a x [b x c]] = b * (a, c) - c * (a, b) double ac = GetScalarProduct(vector1, vector3); double ab = GetScalarProduct(vector1, vector2); - result.x1 = ac * vector2.x1 - ab * vector3.x1; - result.x2 = ac * vector2.x2 - ab * vector3.x2; - result.x3 = ac * vector2.x3 - ab * vector3.x3; + product.x1 = ac * vector2.x1 - ab * vector3.x1; + product.x2 = ac * vector2.x2 - ab * vector3.x2; + product.x3 = ac * vector2.x3 - ab * vector3.x3; } public static double GetAngle(in Vector3FP64 vector1, in Vector3FP64 vector2, AngleUnit unit) { double squareModulus1 = vector1.GetSquareModulus(); - if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus1)) { - return 0.0; + return 0.0f; } double squareModulus2 = vector2.GetSquareModulus(); - if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus2)) { - return 0.0; + return 0.0f; } - double cosine = Vector3FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2); + double multiplier = Math.Sqrt(1.0 / (squareModulus1 * squareModulus2)); - if (1.0 - UtilityFP64.EPSYLON <= cosine) - { - return 0.0; - } + Vector3FP64 crossProduct; - if (cosine <= -(1.0 - UtilityFP64.EPSYLON)) - { - return AngleFP64.GetHalfCircle(unit); - } - - return RadianFP64.ToUnits(Math.Acos(cosine), unit); + GetCrossProduct(vector1, vector2, out crossProduct); + + double x = GetScalarProduct(vector1, vector2); + double y = crossProduct.GetModulus(); + + return RadianFP64.ToUnits(Math.Atan2(y * multiplier, x * multiplier), unit); } - + public static double GetSquareDistance(in Vector3FP64 vector1, in Vector3FP64 vector2) { double dx1 = vector1.x1 - vector2.x1; @@ -259,8 +311,13 @@ namespace BasicGeometry { return Math.Sqrt(GetSquareDistance(vector1, vector2)); } - - public static bool AreEqual(in Vector3FP64 vector1, in Vector3FP64 vector2) + + public static bool AreCloseEnough(in Vector3FP64 vector1, in Vector3FP64 vector2, double distanceLimit) + { + return 0.0 <= distanceLimit && GetSquareDistance(vector1, vector2) <= distanceLimit * distanceLimit; + } + + public static bool AreClose(in Vector3FP64 vector1, in Vector3FP64 vector2) { double squareModulus1 = vector1.GetSquareModulus(); double squareModulus2 = vector2.GetSquareModulus(); @@ -273,5 +330,67 @@ namespace BasicGeometry return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2; } + + public static bool AreParallel(in Vector3FP64 vector1, in Vector3FP64 vector2) + { + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) + { + return true; + } + + Vector3FP64 crossProduct; + + GetCrossProduct(vector1, vector2, out crossProduct); + + return crossProduct.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + } + + public static bool AreOrthogonal(in Vector3FP64 vector1, in Vector3FP64 vector2) + { + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) + { + return true; + } + + double scalarProduct = GetScalarProduct(vector1, vector2); + + return scalarProduct * scalarProduct <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + } + + public static Attitude GetAttitude(in Vector3FP64 vector1, in Vector3FP64 vector2) + { + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) + { + return Attitude.ZERO; + } + + double squareLimit = UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2; + double scalarProduct = GetScalarProduct(vector1, vector2); + + if (scalarProduct * scalarProduct <= squareLimit) + { + return Attitude.ORTHOGONAL; + } + + Vector3FP64 crossProduct; + + GetCrossProduct(vector1, vector2, out crossProduct); + + if (crossProduct.GetSquareModulus() > squareLimit) + { + return Attitude.ANY; + } + + return scalarProduct > 0.0f ? Attitude.CO_DIRECTIONAL : Attitude.COUNTER_DIRECTIONAL; + } } } diff --git a/BasicGeometry/VersorFP32.cs b/BasicGeometry/VersorFP32.cs index ac253ee..ccf43b1 100644 --- a/BasicGeometry/VersorFP32.cs +++ b/BasicGeometry/VersorFP32.cs @@ -4,7 +4,7 @@ * Date: 20 Oct 2024 */ -namespace BasicGeometry +namespace BGC { public struct VersorFP32 { @@ -98,6 +98,14 @@ namespace BasicGeometry this.x3 = 0.0f; } + public void MakeOpposite() + { + this.s0 = -this.s0; + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + public void Shorten() { if (this.s0 < 0.0f) @@ -162,25 +170,43 @@ namespace BasicGeometry 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); - } + 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 MakeInverted(in VersorFP32 versor, out VersorFP32 conjugate) + public static void Combine(in VersorFP32 third, 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); + + LoadValues( + (third.s0 * s0 - third.x1 * x1) - (third.x2 * x2 + third.x3 * x3), + (third.x1 * s0 + third.s0 * x1) - (third.x3 * x2 - third.x2 * x3), + (third.x2 * s0 + third.s0 * x2) - (third.x1 * x3 - third.x3 * x1), + (third.x3 * s0 + third.s0 * x3) - (third.x2 * x1 - third.x1 * x2), + out result + ); + } + + public static void Exclude(in VersorFP32 basic, in VersorFP32 excludant, out VersorFP32 result) + { + LoadValues( + (basic.s0 * excludant.s0 + basic.x1 * excludant.x1) + (basic.x2 * excludant.x2 + basic.x3 * excludant.x3), + (basic.x1 * excludant.s0 + basic.x3 * excludant.x2) - (basic.s0 * excludant.x1 + basic.x2 * excludant.x3), + (basic.x2 * excludant.s0 + basic.x1 * excludant.x3) - (basic.s0 * excludant.x2 + basic.x3 * excludant.x1), + (basic.x3 * excludant.s0 + basic.x2 * excludant.x1) - (basic.s0 * excludant.x3 + basic.x1 * excludant.x2), + out result + ); + } + + public static void GetInverted(in VersorFP32 versor, out VersorFP32 conjugate) { conjugate.s0 = versor.s0; conjugate.x1 = -versor.x1; @@ -188,7 +214,7 @@ namespace BasicGeometry conjugate.x3 = -versor.x3; } - public static void MakeShortened(in VersorFP32 versor, out VersorFP32 shortened) + public static void GetShortened(in VersorFP32 versor, out VersorFP32 shortened) { if (versor.s0 < 0.0f) { shortened.s0 = -versor.s0; @@ -204,60 +230,66 @@ namespace BasicGeometry } } - public static void MakeRotationMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix) + public static void GetRotationMatrix(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 s0x1 = versor.s0 * versor.x1; + float s0x2 = versor.s0 * versor.x2; + float s0x3 = 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; + float x1x2 = versor.x1 * versor.x2; + float x1x3 = versor.x1 * versor.x3; + float x2x3 = versor.x2 * versor.x3; matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); - matrix.r1c2 = x1x2 - s0x3; - matrix.r2c3 = x2x3 - s0x1; - matrix.r3c1 = x1x3 - s0x2; + matrix.r1c2 = 2.0f * (x1x2 - s0x3); + matrix.r2c3 = 2.0f * (x2x3 - s0x1); + matrix.r3c1 = 2.0f * (x1x3 - s0x2); - matrix.r2c1 = x1x2 + s0x3; - matrix.r3c2 = x2x3 + s0x1; - matrix.r1c3 = x1x3 + s0x2; + matrix.r2c1 = 2.0f * (x1x2 + s0x3); + matrix.r3c2 = 2.0f * (x2x3 + s0x1); + matrix.r1c3 = 2.0f * (x1x3 + s0x2); } - public static void MakeReverseMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix) + public static void GetReverseMatrix(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 s0x1 = versor.s0 * versor.x1; + float s0x2 = versor.s0 * versor.x2; + float s0x3 = 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; + float x1x2 = versor.x1 * versor.x2; + float x1x3 = versor.x1 * versor.x3; + float x2x3 = versor.x2 * versor.x3; matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); - matrix.r1c2 = x1x2 + s0x3; - matrix.r2c3 = x2x3 + s0x1; - matrix.r3c1 = x1x3 + s0x2; + matrix.r1c2 = 2.0f * (x1x2 + s0x3); + matrix.r2c3 = 2.0f * (x2x3 + s0x1); + matrix.r3c1 = 2.0f * (x1x3 + s0x2); - matrix.r2c1 = x1x2 - s0x3; - matrix.r3c2 = x2x3 - s0x1; - matrix.r1c3 = x1x3 - s0x2; + matrix.r2c1 = 2.0f * (x1x2 - s0x3); + matrix.r3c2 = 2.0f * (x2x3 - s0x1); + matrix.r1c3 = 2.0f * (x1x3 - s0x2); + } + + public static void GetBothMatrices(in VersorFP32 versor, out Matrix3x3FP32 matrix, out Matrix3x3FP32 reverse) + { + GetReverseMatrix(versor, out reverse); + Matrix3x3FP32.MakeTransposed(reverse, out matrix); } public static void Turn(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result) @@ -290,7 +322,7 @@ namespace BasicGeometry result.x3 = x3; } - public static void LoadIdentity(out VersorFP32 result) + public static void Reset(out VersorFP32 result) { result.s0 = 1.0f; result.x1 = 0.0f; diff --git a/BasicGeometry/VersorFP64.cs b/BasicGeometry/VersorFP64.cs index 983ac2f..64c1baf 100644 --- a/BasicGeometry/VersorFP64.cs +++ b/BasicGeometry/VersorFP64.cs @@ -4,7 +4,7 @@ * Date: 20 Oct 2024 */ -namespace BasicGeometry +namespace BGC { public struct VersorFP64 { @@ -98,6 +98,10 @@ namespace BasicGeometry this.x3 = 0.0; } + public void MakeOpposite() + { + } + public void Shorten() { if (this.s0 < 0.0) @@ -162,22 +166,40 @@ namespace BasicGeometry 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); + 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 + ); + } - double squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + public static void Combine3(in VersorFP64 third, in VersorFP64 second, in VersorFP64 first, out VersorFP64 result) + { + double s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3); + double x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3); + double x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1); + double x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2); - result.s0 = s0; - result.x1 = x1; - result.x2 = x2; - result.x3 = x3; + LoadValues( + (third.s0 * s0 - third.x1 * x1) - (third.x2 * x2 + third.x3 * x3), + (third.x1 * s0 + third.s0 * x1) - (third.x3 * x2 - third.x2 * x3), + (third.x2 * s0 + third.s0 * x2) - (third.x1 * x3 - third.x3 * x1), + (third.x3 * s0 + third.s0 * x3) - (third.x2 * x1 - third.x1 * x2), + out result + ); + } - if (!UtilityFP64.IsSqareUnit(squareModulus)) - { - result.Normalize(squareModulus); - } + public static void Exclude(in VersorFP64 basic, in VersorFP64 excludant, out VersorFP64 result) + { + LoadValues( + (basic.s0 * excludant.s0 + basic.x1 * excludant.x1) + (basic.x2 * excludant.x2 + basic.x3 * excludant.x3), + (basic.x1 * excludant.s0 + basic.x3 * excludant.x2) - (basic.s0 * excludant.x1 + basic.x2 * excludant.x3), + (basic.x2 * excludant.s0 + basic.x1 * excludant.x3) - (basic.s0 * excludant.x2 + basic.x3 * excludant.x1), + (basic.x3 * excludant.s0 + basic.x2 * excludant.x1) - (basic.s0 * excludant.x3 + basic.x1 * excludant.x2), + out result + ); } public static void MakeInverted(in VersorFP64 versor, out VersorFP64 conjugate) @@ -211,25 +233,25 @@ namespace BasicGeometry 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 s0x1 = versor.s0 * versor.x1; + double s0x2 = versor.s0 * versor.x2; + double s0x3 = 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; + double x1x2 = versor.x1 * versor.x2; + double x1x3 = versor.x1 * versor.x3; + double x2x3 = versor.x2 * versor.x3; matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); - matrix.r1c2 = x1x2 - s0x3; - matrix.r2c3 = x2x3 - s0x1; - matrix.r3c1 = x1x3 - s0x2; + matrix.r1c2 = 2.0 * (x1x2 - s0x3); + matrix.r2c3 = 2.0 * (x2x3 - s0x1); + matrix.r3c1 = 2.0 * (x1x3 - s0x2); - matrix.r2c1 = x1x2 + s0x3; - matrix.r3c2 = x2x3 + s0x1; - matrix.r1c3 = x1x3 + s0x2; + matrix.r2c1 = 2.0 * (x1x2 + s0x3); + matrix.r3c2 = 2.0 * (x2x3 + s0x1); + matrix.r1c3 = 2.0 * (x1x3 + s0x2); } public static void MakeReverseMatrix(in VersorFP64 versor, out Matrix3x3FP64 matrix) @@ -239,25 +261,25 @@ namespace BasicGeometry 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 s0x1 = versor.s0 * versor.x1; + double s0x2 = versor.s0 * versor.x2; + double s0x3 = 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; + double x1x2 = versor.x1 * versor.x2; + double x1x3 = versor.x1 * versor.x3; + double x2x3 = versor.x2 * versor.x3; matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); - matrix.r1c2 = x1x2 + s0x3; - matrix.r2c3 = x2x3 + s0x1; - matrix.r3c1 = x1x3 + s0x2; + matrix.r1c2 = 2.0 * (x1x2 + s0x3); + matrix.r2c3 = 2.0 * (x2x3 + s0x1); + matrix.r3c1 = 2.0 * (x1x3 + s0x2); - matrix.r2c1 = x1x2 - s0x3; - matrix.r3c2 = x2x3 - s0x1; - matrix.r1c3 = x1x3 - s0x2; + matrix.r2c1 = 2.0 * (x1x2 - s0x3); + matrix.r3c2 = 2.0 * (x2x3 - s0x1); + matrix.r1c3 = 2.0 * (x1x3 - s0x2); } public static void Turn(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result) diff --git a/BasicGeometryDev/Program.cs b/BasicGeometryDev/Program.cs index f9105f4..9db44bc 100644 --- a/BasicGeometryDev/Program.cs +++ b/BasicGeometryDev/Program.cs @@ -3,7 +3,7 @@ using System; using System.ComponentModel; using System.Diagnostics; using System.Numerics; -using BasicGeometry; +using BGC; public static class Program { diff --git a/BasicGeometryTest/Vector2/Vector2InitTest.cs b/BasicGeometryTest/Vector2/Vector2InitTest.cs index 7b6f17a..3f7799b 100644 --- a/BasicGeometryTest/Vector2/Vector2InitTest.cs +++ b/BasicGeometryTest/Vector2/Vector2InitTest.cs @@ -1,6 +1,6 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; -using BasicGeometry; +using BGC; namespace BasicGeometryTest { diff --git a/BasicGeometryTest/Vector2/Vector2IsZeroTest.cs b/BasicGeometryTest/Vector2/Vector2IsZeroTest.cs index 25816f0..79aa93c 100644 --- a/BasicGeometryTest/Vector2/Vector2IsZeroTest.cs +++ b/BasicGeometryTest/Vector2/Vector2IsZeroTest.cs @@ -1,6 +1,6 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; -using BasicGeometry; +using BGC; namespace BasicGeometryTest.Vector2 { diff --git a/BasicGeometry.Net.sln b/GeometryNet.sln similarity index 100% rename from BasicGeometry.Net.sln rename to GeometryNet.sln