From 320e20997cc36317f313fce0eabc94ee74a66848 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Thu, 5 Dec 2024 00:48:12 +0700 Subject: [PATCH 1/9] =?UTF-8?q?=D0=9E=D0=BF=D1=82=D0=B8=D0=BC=D0=B8=D0=B7?= =?UTF-8?q?=D0=B0=D1=86=D0=B8=D1=8F=20=D0=BD=D0=BE=D1=80=D0=BC=D0=B0=D0=BB?= =?UTF-8?q?=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D0=B8=20=D0=B2=D0=B5=D0=BA=D1=82?= =?UTF-8?q?=D0=BE=D1=80=D0=B0=20/=20Optimization=20of=20vector=20normaliza?= =?UTF-8?q?tion?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/FP32Vector2.cs | 6 +++--- BasicGeometry/FP32Vector3.cs | 8 ++++---- BasicGeometry/FP64Vector2.cs | 6 +++--- BasicGeometry/FP64Vector3.cs | 8 ++++---- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/BasicGeometry/FP32Vector2.cs b/BasicGeometry/FP32Vector2.cs index ea3e8a2..c629f74 100644 --- a/BasicGeometry/FP32Vector2.cs +++ b/BasicGeometry/FP32Vector2.cs @@ -73,10 +73,10 @@ namespace BasicGeometry return 0; } - float module = MathF.Sqrt(squareModule); + float multiplier = Math.Sqrt(1.0f / squareModule); - this.x1 /= module; - this.x2 /= module; + this.x1 *= multiplier; + this.x2 *= multiplier; return 1; } diff --git a/BasicGeometry/FP32Vector3.cs b/BasicGeometry/FP32Vector3.cs index 8b5b51f..6ebb53d 100644 --- a/BasicGeometry/FP32Vector3.cs +++ b/BasicGeometry/FP32Vector3.cs @@ -77,11 +77,11 @@ namespace BasicGeometry return 0; } - float module = MathF.Sqrt(squareModule); + float multiplier = Math.Sqrt(1.0f / squareModule); - this.x1 /= module; - this.x2 /= module; - this.x3 /= module; + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; return 1; } diff --git a/BasicGeometry/FP64Vector2.cs b/BasicGeometry/FP64Vector2.cs index 82d9f84..7628ba7 100644 --- a/BasicGeometry/FP64Vector2.cs +++ b/BasicGeometry/FP64Vector2.cs @@ -73,10 +73,10 @@ namespace BasicGeometry return 0; } - double module = Math.Sqrt(squareModule); + double multiplier = Math.Sqrt(1.0 / squareModule); - this.x1 /= module; - this.x2 /= module; + this.x1 *= multiplier; + this.x2 *= multiplier; return 1; } diff --git a/BasicGeometry/FP64Vector3.cs b/BasicGeometry/FP64Vector3.cs index 9bbf09b..c1e72d1 100644 --- a/BasicGeometry/FP64Vector3.cs +++ b/BasicGeometry/FP64Vector3.cs @@ -77,11 +77,11 @@ namespace BasicGeometry return 0; } - double module = Math.Sqrt(squareModule); + double multiplier = Math.Sqrt(1.0 / squareModule); - this.x1 /= module; - this.x2 /= module; - this.x3 /= module; + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; return 1; } From 7b40d23f196066dc4ed3bdb0c1e6f9a74e8a5fee Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Thu, 5 Dec 2024 12:30:24 +0700 Subject: [PATCH 2/9] =?UTF-8?q?=D0=98=D1=81=D0=BF=D1=80=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B8=D0=B5=20=D0=BD=D0=BE=D1=80=D0=BC=D0=B0=D0=BB?= =?UTF-8?q?=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D0=B8=20=D0=B2=D0=B5=D0=BA=D1=82?= =?UTF-8?q?=D0=BE=D1=80=D0=BE=D0=B2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/FP32Vector2.cs | 2 +- BasicGeometry/FP32Vector3.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/BasicGeometry/FP32Vector2.cs b/BasicGeometry/FP32Vector2.cs index c629f74..7009986 100644 --- a/BasicGeometry/FP32Vector2.cs +++ b/BasicGeometry/FP32Vector2.cs @@ -73,7 +73,7 @@ namespace BasicGeometry return 0; } - float multiplier = Math.Sqrt(1.0f / squareModule); + float multiplier = MathF.Sqrt(1.0f / squareModule); this.x1 *= multiplier; this.x2 *= multiplier; diff --git a/BasicGeometry/FP32Vector3.cs b/BasicGeometry/FP32Vector3.cs index 6ebb53d..607d047 100644 --- a/BasicGeometry/FP32Vector3.cs +++ b/BasicGeometry/FP32Vector3.cs @@ -77,7 +77,7 @@ namespace BasicGeometry return 0; } - float multiplier = Math.Sqrt(1.0f / squareModule); + float multiplier = MathF.Sqrt(1.0f / squareModule); this.x1 *= multiplier; this.x2 *= multiplier; From df827ffe0e618012129313a8d6affddcdb60bb19 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Thu, 5 Dec 2024 13:50:15 +0700 Subject: [PATCH 3/9] =?UTF-8?q?=D0=9E=D0=BF=D1=82=D0=B8=D0=BC=D0=B8=D0=B7?= =?UTF-8?q?=D0=B0=D1=86=D0=B8=D1=8F=20=D0=B2=D1=8B=D1=87=D0=B8=D1=81=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B8=D0=B9=20/=20Optimization=20of=20computations?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/FP32Versor.cs | 2 +- BasicGeometry/FP64Versor.cs | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/BasicGeometry/FP32Versor.cs b/BasicGeometry/FP32Versor.cs index 0912aab..ae923c3 100644 --- a/BasicGeometry/FP32Versor.cs +++ b/BasicGeometry/FP32Versor.cs @@ -100,7 +100,7 @@ namespace BasicGeometry return FP32Angle.GetHalfCircle(unit); } - return FP32Radians.ToUnits(2.0f * MathF.Acos(s0), unit); + return FP32Radians.ToUnits(2.0f * MathF.Acos(this.s0), unit); } public readonly void MakeRotationMatrix(out FP32Matrix3x3 matrix) diff --git a/BasicGeometry/FP64Versor.cs b/BasicGeometry/FP64Versor.cs index bd1396e..b750c4e 100644 --- a/BasicGeometry/FP64Versor.cs +++ b/BasicGeometry/FP64Versor.cs @@ -103,7 +103,7 @@ namespace BasicGeometry return FP64Angle.GetHalfCircle(unit); } - return FP64Radians.ToUnits(2.0 * Math.Acos(s0), unit); + return FP64Radians.ToUnits(2.0 * Math.Acos(this.s0), unit); } public readonly void MakeRotationMatrix(out FP64Matrix3x3 matrix) @@ -254,12 +254,12 @@ namespace BasicGeometry return; } - double module = Math.Sqrt(squareModule); + double multiplier = Math.Sqrt(1.0 / squareModule); - this.s0 /= module; - this.x1 /= module; - this.x2 /= module; - this.x3 /= module; + this.s0 *= multiplier; + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; } public static void Combine(in FP64Versor second, in FP64Versor first, out FP64Versor result) From 233c027d46b299e62afff9778d4a25a2d35796cf Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Sat, 21 Dec 2024 21:38:45 +0700 Subject: [PATCH 4/9] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=D0=B8=D0=B5=20=D0=BE=D0=BF=D0=B5=D1=80=D0=B0=D1=86=D0=B8?= =?UTF-8?q?=D0=B8=20Shorten=20=D0=B4=D0=BB=D1=8F=20=D0=B2=D0=B5=D1=80?= =?UTF-8?q?=D1=81=D0=BE=D1=80=D0=BE=D0=B2=20/=20Adding=20of=20shortening?= =?UTF-8?q?=20operation=20for=20versors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/FP32Versor.cs | 52 ++++++++++++++++++++++++++++------ BasicGeometry/FP64Versor.cs | 56 +++++++++++++++++++++++++------------ 2 files changed, 82 insertions(+), 26 deletions(-) diff --git a/BasicGeometry/FP32Versor.cs b/BasicGeometry/FP32Versor.cs index ae923c3..9c3007c 100644 --- a/BasicGeometry/FP32Versor.cs +++ b/BasicGeometry/FP32Versor.cs @@ -82,6 +82,16 @@ namespace BasicGeometry this.x3 = 0.0f; } + public void Shorten() + { + if (this.s0 < 0.0f) { + this.s0 = -this.s0; + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + } + public void Invert() { this.x1 = -this.x1; @@ -186,10 +196,11 @@ namespace BasicGeometry public void SetValues(in FP64Versor versor) { - this.s0 = (float) versor.GetScalar(); - this.x1 = (float) versor.GetX1(); - this.x2 = (float) versor.GetX2(); - this.x3 = (float) versor.GetX3(); + this.SetValues( + (float) versor.GetScalar(), + (float) versor.GetX1(), + (float) versor.GetX2(), + (float) versor.GetX3()); } public void SetInverted(in FP32Versor versor) @@ -202,10 +213,35 @@ namespace BasicGeometry public void SetInverted(in FP64Versor versor) { - this.s0 = (float) versor.GetScalar(); - this.x1 = (float) -versor.GetX1(); - this.x2 = (float) -versor.GetX2(); - this.x3 = (float) -versor.GetX3(); + this.SetValues( + (float)versor.GetScalar(), + (float)-versor.GetX1(), + (float)-versor.GetX2(), + (float)-versor.GetX3()); + } + + public void SetShortened(in FP32Versor versor) + { + if (versor.s0 < 0.0f) + { + this.s0 = -versor.s0; + this.x1 = -versor.x1; + this.x2 = -versor.x2; + this.x3 = -versor.x3; + } + else + { + this.s0 = versor.s0; + this.x1 = versor.x1; + this.x2 = versor.x2; + this.x3 = versor.x3; + } + } + + public void SetShortened(in FP64Versor versor) + { + this.SetValues(versor); + this.Shorten(); } public readonly void Turn(in FP32Vector3 vector, out FP32Vector3 result) diff --git a/BasicGeometry/FP64Versor.cs b/BasicGeometry/FP64Versor.cs index b750c4e..4995bec 100644 --- a/BasicGeometry/FP64Versor.cs +++ b/BasicGeometry/FP64Versor.cs @@ -44,12 +44,7 @@ namespace BasicGeometry public FP64Versor(in FP32Versor versor) { - LoadValues( - versor.GetScalar(), - versor.GetX1(), - versor.GetX2(), - versor.GetX3(), - out this); + LoadValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3(), out this); } public readonly double GetScalar() @@ -85,6 +80,17 @@ namespace BasicGeometry this.x3 = 0.0; } + public void Shorten() + { + if (this.s0 < 0.0) + { + this.s0 = -this.s0; + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + } + public void Invert() { this.x1 = -this.x1; @@ -190,12 +196,31 @@ namespace BasicGeometry public void SetValues(in FP32Versor versor) { - LoadValues( - versor.GetScalar(), - versor.GetX1(), - versor.GetX2(), - versor.GetX3(), - out this); + this.SetValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3()); + } + + public void SetShortened(in FP64Versor versor) + { + if (versor.s0 < 0.0) + { + this.s0 = -versor.s0; + this.x1 = -versor.x1; + this.x2 = -versor.x2; + this.x3 = -versor.x3; + } + else + { + this.s0 = versor.s0; + this.x1 = versor.x1; + this.x2 = versor.x2; + this.x3 = versor.x3; + } + } + + public void SetShortened(in FP32Versor versor) + { + this.SetValues(versor); + this.Shorten(); } public void SetInverted(in FP64Versor versor) @@ -208,12 +233,7 @@ namespace BasicGeometry public void SetInverted(in FP32Versor versor) { - LoadValues( - versor.GetScalar(), - versor.GetX1(), - versor.GetX2(), - versor.GetX3(), - out this); + this.SetValues(versor.GetScalar(), -versor.GetX1(), -versor.GetX2(), -versor.GetX3()); } public readonly void Turn(in FP64Vector3 vector, out FP64Vector3 result) From f3b0232dd41ead88a5fdbaf84e04224714961456 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Tue, 11 Feb 2025 19:38:07 +0700 Subject: [PATCH 5/9] =?UTF-8?q?=D0=91=D0=BE=D0=BB=D1=8C=D1=88=D0=B0=D1=8F?= =?UTF-8?q?=20=D1=80=D0=B5=D0=BE=D1=80=D0=B3=D0=B0=D0=BD=D0=B8=D0=B7=D0=B0?= =?UTF-8?q?=D1=86=D0=B8=D1=8F=20=D0=BF=D1=80=D0=BE=D0=B5=D0=BA=D1=82=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/{FP32Angle.cs => AngleFP32.cs} | 24 +++--- BasicGeometry/{FP64Angle.cs => AngleFP64.cs} | 24 +++--- .../{FP32Degrees.cs => DegreeFP32.cs} | 2 +- .../{FP64Degrees.cs => DegreeFP64.cs} | 2 +- BasicGeometry/FP32Utility.cs | 21 ----- BasicGeometry/FP64Utility.cs | 20 ----- .../{FP32Matrix2x2.cs => Matrix2x2FP32.cs} | 44 +++++----- .../{FP64Matrix2x2.cs => Matrix2x2FP64.cs} | 44 +++++----- .../{FP32Matrix2x3.cs => Matrix2x3FP32.cs} | 32 +++---- .../{FP64Matrix2x3.cs => Matrix2x3FP64.cs} | 32 +++---- .../{FP32Matrix3x2.cs => Matrix3x2FP32.cs} | 32 +++---- .../{FP64Matrix3x2.cs => Matrix3x2FP64.cs} | 32 +++---- .../{FP32Matrix3x3.cs => Matrix3x3FP32.cs} | 44 +++++----- .../{FP64Matrix3x3.cs => Matrix3x3FP64.cs} | 44 +++++----- ...2MatrixProduct.cs => MatrixProductFP32.cs} | 18 ++-- ...4MatrixProduct.cs => MatrixProductFP64.cs} | 18 ++-- .../{FP32Quaternion.cs => QuaternionFP32.cs} | 86 +++++++++++-------- .../{FP64Quaternion.cs => QuaternionFP64.cs} | 86 +++++++++++-------- .../{FP32Radians.cs => RadianFP32.cs} | 2 +- .../{FP64Radians.cs => RadianFP64.cs} | 2 +- .../{FP32Rotation3.cs => Rotation3FP32.cs} | 6 +- BasicGeometry/Rotation3FP64.cs | 44 ++++++++++ BasicGeometry/{FP32Turns.cs => TurnFP32.cs} | 6 +- BasicGeometry/{FP64Turns.cs => TurnFP64.cs} | 10 +-- BasicGeometry/UtilityFP32.cs | 48 +++++++++++ BasicGeometry/UtilityFP64.cs | 48 +++++++++++ .../{FP32Vector2.cs => Vector2FP32.cs} | 79 +++++++++-------- .../{FP64Vector2.cs => Vector2FP64.cs} | 79 +++++++++-------- .../{FP32Vector3.cs => Vector3FP32.cs} | 85 +++++++++--------- .../{FP64Vector3.cs => Vector3FP64.cs} | 86 +++++++++---------- .../{FP32Versor.cs => VersorFP32.cs} | 64 +++++++------- .../{FP64Versor.cs => VersorFP64.cs} | 64 +++++++------- BasicGeometryDev/Program.cs | 24 +++--- BasicGeometryTest/F32Vector2Test.cs | 19 ---- BasicGeometryTest/TestVector2FP32.cs | 27 ++++++ 35 files changed, 705 insertions(+), 593 deletions(-) rename BasicGeometry/{FP32Angle.cs => AngleFP32.cs} (81%) rename BasicGeometry/{FP64Angle.cs => AngleFP64.cs} (81%) rename BasicGeometry/{FP32Degrees.cs => DegreeFP32.cs} (98%) rename BasicGeometry/{FP64Degrees.cs => DegreeFP64.cs} (98%) delete mode 100644 BasicGeometry/FP32Utility.cs delete mode 100644 BasicGeometry/FP64Utility.cs rename BasicGeometry/{FP32Matrix2x2.cs => Matrix2x2FP32.cs} (79%) rename BasicGeometry/{FP64Matrix2x2.cs => Matrix2x2FP64.cs} (79%) rename BasicGeometry/{FP32Matrix2x3.cs => Matrix2x3FP32.cs} (83%) rename BasicGeometry/{FP64Matrix2x3.cs => Matrix2x3FP64.cs} (82%) rename BasicGeometry/{FP32Matrix3x2.cs => Matrix3x2FP32.cs} (82%) rename BasicGeometry/{FP64Matrix3x2.cs => Matrix3x2FP64.cs} (82%) rename BasicGeometry/{FP32Matrix3x3.cs => Matrix3x3FP32.cs} (88%) rename BasicGeometry/{FP64Matrix3x3.cs => Matrix3x3FP64.cs} (88%) rename BasicGeometry/{FP32MatrixProduct.cs => MatrixProductFP32.cs} (87%) rename BasicGeometry/{FP64MatrixProduct.cs => MatrixProductFP64.cs} (87%) rename BasicGeometry/{FP32Quaternion.cs => QuaternionFP32.cs} (70%) rename BasicGeometry/{FP64Quaternion.cs => QuaternionFP64.cs} (70%) rename BasicGeometry/{FP32Radians.cs => RadianFP32.cs} (98%) rename BasicGeometry/{FP64Radians.cs => RadianFP64.cs} (98%) rename BasicGeometry/{FP32Rotation3.cs => Rotation3FP32.cs} (89%) create mode 100644 BasicGeometry/Rotation3FP64.cs rename BasicGeometry/{FP32Turns.cs => TurnFP32.cs} (90%) rename BasicGeometry/{FP64Turns.cs => TurnFP64.cs} (82%) create mode 100644 BasicGeometry/UtilityFP32.cs create mode 100644 BasicGeometry/UtilityFP64.cs rename BasicGeometry/{FP32Vector2.cs => Vector2FP32.cs} (62%) rename BasicGeometry/{FP64Vector2.cs => Vector2FP64.cs} (62%) rename BasicGeometry/{FP32Vector3.cs => Vector3FP32.cs} (66%) rename BasicGeometry/{FP64Vector3.cs => Vector3FP64.cs} (66%) rename BasicGeometry/{FP32Versor.cs => VersorFP32.cs} (80%) rename BasicGeometry/{FP64Versor.cs => VersorFP64.cs} (79%) delete mode 100644 BasicGeometryTest/F32Vector2Test.cs create mode 100644 BasicGeometryTest/TestVector2FP32.cs diff --git a/BasicGeometry/FP32Angle.cs b/BasicGeometry/AngleFP32.cs similarity index 81% rename from BasicGeometry/FP32Angle.cs rename to BasicGeometry/AngleFP32.cs index 6962498..01f5787 100644 --- a/BasicGeometry/FP32Angle.cs +++ b/BasicGeometry/AngleFP32.cs @@ -23,18 +23,18 @@ using System; namespace BasicGeometry { - public static class FP32Angle + public static class AngleFP32 { public static float ToRadians(float angle, AngleUnit unit) { if (unit == AngleUnit.DEGREES) { - return angle * FP32Degrees.RADIANS_IN_DEGREE; + return angle * DegreeFP32.RADIANS_IN_DEGREE; } if (unit == AngleUnit.TURNS) { - return angle * FP32Radians.TWO_PI; + return angle * RadianFP32.TWO_PI; } return angle; @@ -44,7 +44,7 @@ namespace BasicGeometry { if (unit == AngleUnit.RADIANS) { - return angle * FP32Radians.DEGREES_IN_RADIAN; + return angle * RadianFP32.DEGREES_IN_RADIAN; } if (unit == AngleUnit.TURNS) @@ -59,12 +59,12 @@ namespace BasicGeometry { if (unit == AngleUnit.RADIANS) { - return angle * FP32Radians.TURNS_IN_RADIAN; + return angle * RadianFP32.TURNS_IN_RADIAN; } if (unit == AngleUnit.DEGREES) { - return angle * FP32Degrees.TURNS_IN_DEGREE; + return angle * DegreeFP32.TURNS_IN_DEGREE; } return angle; @@ -82,7 +82,7 @@ namespace BasicGeometry return 1.0f; } - return FP32Radians.TWO_PI; + return RadianFP32.TWO_PI; } public static float GetHalfCircle(AngleUnit unit) @@ -97,7 +97,7 @@ namespace BasicGeometry return 0.5f; } - return FP32Radians.PI; + return RadianFP32.PI; } public static float GetQuarterCircle(AngleUnit unit) @@ -112,22 +112,22 @@ namespace BasicGeometry return 0.25f; } - return FP32Radians.HALF_OF_PI; + return RadianFP32.HALF_OF_PI; } public static float Normalize(float angle, AngleUnit unit, AngleRange range) { if (unit == AngleUnit.DEGREES) { - return FP32Degrees.Normalize(angle, range); + return DegreeFP32.Normalize(angle, range); } if (unit == AngleUnit.TURNS) { - return FP32Turns.Normalize(angle, range); + return TurnFP32.Normalize(angle, range); } - return FP32Radians.Normalize(angle, range); + return RadianFP32.Normalize(angle, range); } } } diff --git a/BasicGeometry/FP64Angle.cs b/BasicGeometry/AngleFP64.cs similarity index 81% rename from BasicGeometry/FP64Angle.cs rename to BasicGeometry/AngleFP64.cs index 3b58553..088b45e 100644 --- a/BasicGeometry/FP64Angle.cs +++ b/BasicGeometry/AngleFP64.cs @@ -23,18 +23,18 @@ using System; namespace BasicGeometry { - public static class FP64Angle + public static class AngleFP64 { public static double ToRadians(double angle, AngleUnit unit) { if (unit == AngleUnit.DEGREES) { - return angle * FP64Degrees.RADIANS_IN_DEGREE; + return angle * DegreeFP64.RADIANS_IN_DEGREE; } if (unit == AngleUnit.TURNS) { - return angle * FP64Radians.TWO_PI; + return angle * RadianFP64.TWO_PI; } return angle; @@ -44,7 +44,7 @@ namespace BasicGeometry { if (unit == AngleUnit.RADIANS) { - return angle * FP64Radians.DEGREES_IN_RADIAN; + return angle * RadianFP64.DEGREES_IN_RADIAN; } if (unit == AngleUnit.TURNS) @@ -59,12 +59,12 @@ namespace BasicGeometry { if (unit == AngleUnit.RADIANS) { - return angle * FP64Radians.TURNS_IN_RADIAN; + return angle * RadianFP64.TURNS_IN_RADIAN; } if (unit == AngleUnit.DEGREES) { - return angle * FP64Degrees.TURNS_IN_DEGREE; + return angle * DegreeFP64.TURNS_IN_DEGREE; } return angle; @@ -82,7 +82,7 @@ namespace BasicGeometry return 1.0; } - return FP64Radians.TWO_PI; + return RadianFP64.TWO_PI; } public static double GetHalfCircle(AngleUnit unit) @@ -97,7 +97,7 @@ namespace BasicGeometry return 0.5; } - return FP64Radians.PI; + return RadianFP64.PI; } public static double GetQuarterCircle(AngleUnit unit) @@ -112,22 +112,22 @@ namespace BasicGeometry return 0.25; } - return FP64Radians.HALF_OF_PI; + return RadianFP64.HALF_OF_PI; } public static double Normalize(double angle, AngleUnit unit, AngleRange range) { if (unit == AngleUnit.DEGREES) { - return FP64Degrees.Normalize(angle, range); + return DegreeFP64.Normalize(angle, range); } if (unit == AngleUnit.TURNS) { - return FP64Turns.Normalize(angle, range); + return TurnFP64.Normalize(angle, range); } - return FP64Radians.Normalize(angle, range); + return RadianFP64.Normalize(angle, range); } } } diff --git a/BasicGeometry/FP32Degrees.cs b/BasicGeometry/DegreeFP32.cs similarity index 98% rename from BasicGeometry/FP32Degrees.cs rename to BasicGeometry/DegreeFP32.cs index 4fa98b0..a458294 100644 --- a/BasicGeometry/FP32Degrees.cs +++ b/BasicGeometry/DegreeFP32.cs @@ -6,7 +6,7 @@ namespace BasicGeometry { - public class FP32Degrees + public class DegreeFP32 { public const float RADIANS_IN_DEGREE = 1.745329252E-2f; public const float TURNS_IN_DEGREE = 2.7777777778E-3f; diff --git a/BasicGeometry/FP64Degrees.cs b/BasicGeometry/DegreeFP64.cs similarity index 98% rename from BasicGeometry/FP64Degrees.cs rename to BasicGeometry/DegreeFP64.cs index 075b600..b7c1785 100644 --- a/BasicGeometry/FP64Degrees.cs +++ b/BasicGeometry/DegreeFP64.cs @@ -6,7 +6,7 @@ namespace BasicGeometry { - public class FP64Degrees + public class DegreeFP64 { public const double RADIANS_IN_DEGREE = 1.74532925199432958E-2; public const double TURNS_IN_DEGREE = 2.77777777777777778E-3; diff --git a/BasicGeometry/FP32Utility.cs b/BasicGeometry/FP32Utility.cs deleted file mode 100644 index eb5ce94..0000000 --- a/BasicGeometry/FP32Utility.cs +++ /dev/null @@ -1,21 +0,0 @@ -using System; - -namespace BasicGeometry -{ - public class FP32Utility - { - public const float EPSYLON = 5E-7f; - public const float TWO_EPSYLON = 1E-6f; - public const float SQUARE_EPSYLON = 2.5E-13f; - - public const float EPSYLON_EFFECTIVENESS_LIMIT = 1.0f; - - public const float ONE_THIRD = 0.333333333f; - public const float ONE_SIXTH = 0.166666667f; - public const float ONE_NINETH = 0.111111111f; - - public const float GOLDEN_RATIO_HIGH = 1.618034f; - public const float GOLDEN_RATIO_LOW = 0.618034f; - } -} - diff --git a/BasicGeometry/FP64Utility.cs b/BasicGeometry/FP64Utility.cs deleted file mode 100644 index bfa9ce0..0000000 --- a/BasicGeometry/FP64Utility.cs +++ /dev/null @@ -1,20 +0,0 @@ -using System; - -namespace BasicGeometry -{ - public class FP64Utility - { - public const double EPSYLON = 5E-14; - public const double TWO_EPSYLON = 1E-13; - public const double SQUARE_EPSYLON = 2.5E-27; - - public const double EPSYLON_EFFECTIVENESS_LIMIT = 1.0; - - public const double ONE_THIRD = 0.333333333333333333; - public const double ONE_SIXTH = 0.166666666666666667; - public const double ONE_NINETH = 0.111111111111111111; - - public const double GOLDEN_RATIO_HIGH = 1.61803398874989485; - public const double GOLDEN_RATIO_LOW = 0.61803398874989485; - } -} diff --git a/BasicGeometry/FP32Matrix2x2.cs b/BasicGeometry/Matrix2x2FP32.cs similarity index 79% rename from BasicGeometry/FP32Matrix2x2.cs rename to BasicGeometry/Matrix2x2FP32.cs index e8ab41c..10c520b 100644 --- a/BasicGeometry/FP32Matrix2x2.cs +++ b/BasicGeometry/Matrix2x2FP32.cs @@ -23,18 +23,18 @@ using System; namespace BasicGeometry { - public struct FP32Matrix2x2 + public struct Matrix2x2FP32 { public float r1c1 = 0.0f, r1c2 = 0.0f; public float r2c1 = 0.0f, r2c2 = 0.0f; - public FP32Matrix2x2(float d1, float d2) + public Matrix2x2FP32(float d1, float d2) { this.r1c1 = d1; this.r2c2 = d2; } - public FP32Matrix2x2(in FP32Matrix2x2 matrix) + public Matrix2x2FP32(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -43,7 +43,7 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public FP32Matrix2x2(in FP64Matrix2x2 matrix) + public Matrix2x2FP32(in Matrix2x2FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r1c2; @@ -60,7 +60,7 @@ namespace BasicGeometry public readonly bool IsSingular() { float determinant = this.GetDeterminant(); - return -FP32Utility.EPSYLON <= determinant && determinant <= FP32Utility.EPSYLON; + return -UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON; } public void Transpose() @@ -72,7 +72,7 @@ namespace BasicGeometry { float determinant = this.GetDeterminant(); - if (-FP32Utility.EPSYLON <= determinant && determinant <= FP32Utility.EPSYLON) + if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) { return false; } @@ -121,7 +121,7 @@ namespace BasicGeometry this.r2c2 = d2; } - public void SetValues(in FP32Matrix2x2 matrix) + public void SetValues(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -130,7 +130,7 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public void SetValues(in FP64Matrix2x2 matrix) + public void SetValues(in Matrix2x2FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r1c2; @@ -139,14 +139,14 @@ namespace BasicGeometry this.r2c2 = (float)matrix.r2c2; } - public void SetTransposedOf(in FP32Matrix2x2 matrix) + public void SetTransposedOf(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r2c2 = matrix.r2c2; (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); } - public void SetTransposedOf(in FP64Matrix2x2 matrix) + public void SetTransposedOf(in Matrix2x2FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r2c1; @@ -155,11 +155,11 @@ namespace BasicGeometry this.r2c2 = (float)matrix.r2c2; } - public bool SetInvertedOf(in FP32Matrix2x2 matrix) + public bool SetInvertedOf(in Matrix2x2FP32 matrix) { float determinant = matrix.GetDeterminant(); - if (-FP32Utility.EPSYLON <= determinant && determinant <= FP32Utility.EPSYLON) + if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) { return false; } @@ -205,7 +205,7 @@ namespace BasicGeometry this.r2c2 = r2; } - public void AppendScaled(in FP32Matrix2x2 matrix, float scale) + public void AppendScaled(in Matrix2x2FP32 matrix, float scale) { this.r1c1 += matrix.r1c1 * scale; this.r1c2 += matrix.r1c2 * scale; @@ -214,7 +214,7 @@ namespace BasicGeometry this.r2c2 += matrix.r2c2 * scale; } - public static void Add(in FP32Matrix2x2 matrix1, in FP32Matrix2x2 matrix2, out FP32Matrix2x2 result) + public static void Add(in Matrix2x2FP32 matrix1, in Matrix2x2FP32 matrix2, out Matrix2x2FP32 result) { result.r1c1 = matrix1.r1c1 + matrix2.r1c1; result.r1c2 = matrix1.r1c2 + matrix2.r1c2; @@ -223,7 +223,7 @@ namespace BasicGeometry result.r2c2 = matrix1.r2c2 + matrix2.r2c2; } - public static void Subtract(in FP32Matrix2x2 minuend, in FP32Matrix2x2 subtrahend, out FP32Matrix2x2 difference) + public static void Subtract(in Matrix2x2FP32 minuend, in Matrix2x2FP32 subtrahend, out Matrix2x2FP32 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; @@ -232,7 +232,7 @@ namespace BasicGeometry difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; } - public static void Multiply(in FP32Matrix2x2 multiplicand, float multiplier, out FP32Matrix2x2 product) + public static void Multiply(in Matrix2x2FP32 multiplicand, float multiplier, out Matrix2x2FP32 product) { product.r1c1 = multiplicand.r1c1 * multiplier; product.r1c2 = multiplicand.r1c2 * multiplier; @@ -241,12 +241,12 @@ namespace BasicGeometry product.r2c2 = multiplicand.r2c2 * multiplier; } - public static void Divide(in FP32Matrix2x2 dividend, float divisor, out FP32Matrix2x2 quotient) + public static void Divide(in Matrix2x2FP32 dividend, float divisor, out Matrix2x2FP32 quotient) { Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetRightProduct(in FP32Matrix2x2 matrix, in FP32Vector2 vector, out FP32Vector2 result) + public static void GetRightProduct(in Matrix2x2FP32 matrix, in Vector2FP32 vector, out Vector2FP32 result) { float x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; float x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2; @@ -255,7 +255,7 @@ namespace BasicGeometry result.x2 = x2; } - public static void GetLeftProduct(in FP32Vector2 vector, in FP32Matrix2x2 matrix, out FP32Vector2 result) + public static void GetLeftProduct(in Vector2FP32 vector, in Matrix2x2FP32 matrix, out Vector2FP32 result) { float x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1; float x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2; @@ -264,7 +264,7 @@ namespace BasicGeometry result.x2 = x2; } - public static void LoadZero(out FP32Matrix2x2 matrix) + public static void LoadZero(out Matrix2x2FP32 matrix) { matrix.r1c1 = 0.0f; matrix.r1c2 = 0.0f; @@ -273,7 +273,7 @@ namespace BasicGeometry matrix.r2c2 = 0.0f; } - public static void LoadIdentity(out FP32Matrix2x2 matrix) + public static void LoadIdentity(out Matrix2x2FP32 matrix) { matrix.r1c1 = 1.0f; matrix.r1c2 = 0.0f; @@ -282,7 +282,7 @@ namespace BasicGeometry matrix.r2c2 = 1.0f; } - public static void LoadDiagonal(float d1, float d2, out FP32Matrix2x2 matrix) + public static void LoadDiagonal(float d1, float d2, out Matrix2x2FP32 matrix) { matrix.r1c1 = d1; matrix.r1c2 = 0.0f; diff --git a/BasicGeometry/FP64Matrix2x2.cs b/BasicGeometry/Matrix2x2FP64.cs similarity index 79% rename from BasicGeometry/FP64Matrix2x2.cs rename to BasicGeometry/Matrix2x2FP64.cs index f4bdd97..11f20d0 100644 --- a/BasicGeometry/FP64Matrix2x2.cs +++ b/BasicGeometry/Matrix2x2FP64.cs @@ -23,18 +23,18 @@ using System; namespace BasicGeometry { - public struct FP64Matrix2x2 + public struct Matrix2x2FP64 { public double r1c1 = 0.0, r1c2 = 0.0; public double r2c1 = 0.0, r2c2 = 0.0; - public FP64Matrix2x2(double d1, double d2) + public Matrix2x2FP64(double d1, double d2) { this.r1c1 = d1; this.r2c2 = d2; } - public FP64Matrix2x2(in FP64Matrix2x2 matrix) + public Matrix2x2FP64(in Matrix2x2FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -43,7 +43,7 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public FP64Matrix2x2(in FP32Matrix2x2 matrix) + public Matrix2x2FP64(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -60,7 +60,7 @@ namespace BasicGeometry public readonly bool IsSingular() { double determinant = this.GetDeterminant(); - return -FP64Utility.EPSYLON <= determinant && determinant <= FP64Utility.EPSYLON; + return -UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON; } public void Transpose() @@ -72,7 +72,7 @@ namespace BasicGeometry { double determinant = this.GetDeterminant(); - if (-FP64Utility.EPSYLON <= determinant && determinant <= FP64Utility.EPSYLON) + if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) { return false; } @@ -121,7 +121,7 @@ namespace BasicGeometry this.r2c2 = d2; } - public void SetValues(in FP64Matrix2x2 matrix) + public void SetValues(in Matrix2x2FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -130,7 +130,7 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public void SetValues(in FP32Matrix2x2 matrix) + public void SetValues(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -139,14 +139,14 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public void SetTransposedOf(in FP64Matrix2x2 matrix) + public void SetTransposedOf(in Matrix2x2FP64 matrix) { this.r1c1 = matrix.r1c1; this.r2c2 = matrix.r2c2; (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); } - public void SetTransposedOf(in FP32Matrix2x2 matrix) + public void SetTransposedOf(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -155,11 +155,11 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public bool SetInvertedOf(in FP64Matrix2x2 matrix) + public bool SetInvertedOf(in Matrix2x2FP64 matrix) { double determinant = matrix.GetDeterminant(); - if (-FP64Utility.EPSYLON <= determinant && determinant <= FP64Utility.EPSYLON) + if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) { return false; } @@ -205,7 +205,7 @@ namespace BasicGeometry this.r2c2 = r2; } - public void AppendScaled(in FP64Matrix2x2 matrix, double scale) + public void AppendScaled(in Matrix2x2FP64 matrix, double scale) { this.r1c1 += matrix.r1c1 * scale; this.r1c2 += matrix.r1c2 * scale; @@ -214,7 +214,7 @@ namespace BasicGeometry this.r2c2 += matrix.r2c2 * scale; } - public static void Add(in FP64Matrix2x2 matrix1, in FP64Matrix2x2 matrix2, out FP64Matrix2x2 sum) + public static void Add(in Matrix2x2FP64 matrix1, in Matrix2x2FP64 matrix2, out Matrix2x2FP64 sum) { sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; @@ -223,7 +223,7 @@ namespace BasicGeometry sum.r2c2 = matrix1.r2c2 + matrix2.r2c2; } - public static void Subtract(in FP64Matrix2x2 minuend, in FP64Matrix2x2 subtrahend, out FP64Matrix2x2 difference) + public static void Subtract(in Matrix2x2FP64 minuend, in Matrix2x2FP64 subtrahend, out Matrix2x2FP64 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; @@ -232,7 +232,7 @@ namespace BasicGeometry difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; } - public static void Multiply(in FP64Matrix2x2 multiplicand, double multiplier, out FP64Matrix2x2 product) + public static void Multiply(in Matrix2x2FP64 multiplicand, double multiplier, out Matrix2x2FP64 product) { product.r1c1 = multiplicand.r1c1 * multiplier; product.r1c2 = multiplicand.r1c2 * multiplier; @@ -241,12 +241,12 @@ namespace BasicGeometry product.r2c2 = multiplicand.r2c2 * multiplier; } - public static void Divide(in FP64Matrix2x2 dividend, double divisor, out FP64Matrix2x2 quotient) + public static void Divide(in Matrix2x2FP64 dividend, double divisor, out Matrix2x2FP64 quotient) { Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetRightProduct(in FP64Matrix2x2 matrix, in FP64Vector2 vector, out FP64Vector2 result) + public static void GetRightProduct(in Matrix2x2FP64 matrix, in Vector2FP64 vector, out Vector2FP64 result) { double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; double x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2; @@ -255,7 +255,7 @@ namespace BasicGeometry result.x2 = x2; } - public static void GetLeftProduct(in FP64Vector2 vector, in FP64Matrix2x2 matrix, out FP64Vector2 result) + public static void GetLeftProduct(in Vector2FP64 vector, in Matrix2x2FP64 matrix, out Vector2FP64 result) { double x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1; double x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2; @@ -264,7 +264,7 @@ namespace BasicGeometry result.x2 = x2; } - public static void LoadZero(out FP64Matrix2x2 matrix) + public static void LoadZero(out Matrix2x2FP64 matrix) { matrix.r1c1 = 0.0; matrix.r1c2 = 0.0; @@ -273,7 +273,7 @@ namespace BasicGeometry matrix.r2c2 = 0.0; } - public static void LoadIdentity(out FP64Matrix2x2 matrix) + public static void LoadIdentity(out Matrix2x2FP64 matrix) { matrix.r1c1 = 1.0; matrix.r1c2 = 0.0; @@ -282,7 +282,7 @@ namespace BasicGeometry matrix.r2c2 = 1.0; } - public static void LoadDiagonal(double d1, double d2, out FP64Matrix2x2 matrix) + public static void LoadDiagonal(double d1, double d2, out Matrix2x2FP64 matrix) { matrix.r1c1 = d1; matrix.r1c2 = 0.0; diff --git a/BasicGeometry/FP32Matrix2x3.cs b/BasicGeometry/Matrix2x3FP32.cs similarity index 83% rename from BasicGeometry/FP32Matrix2x3.cs rename to BasicGeometry/Matrix2x3FP32.cs index 90f1f63..87e33aa 100644 --- a/BasicGeometry/FP32Matrix2x3.cs +++ b/BasicGeometry/Matrix2x3FP32.cs @@ -22,13 +22,13 @@ using System; */ namespace BasicGeometry { - public struct FP32Matrix2x3 + public struct Matrix2x3FP32 { public float r1c1 = 0.0f, r1c2 = 0.0f; public float r2c1 = 0.0f, r2c2 = 0.0f; public float r3c1 = 0.0f, r3c2 = 0.0f; - public FP32Matrix2x3(in FP32Matrix2x3 matrix) + public Matrix2x3FP32(in Matrix2x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -40,7 +40,7 @@ namespace BasicGeometry this.r3c2 = matrix.r3c2; } - public FP32Matrix2x3(in FP64Matrix2x3 matrix) + public Matrix2x3FP32(in Matrix2x3FP64 matrix) { this.r1c1 = (float) matrix.r1c1; this.r1c2 = (float) matrix.r1c2; @@ -52,7 +52,7 @@ namespace BasicGeometry this.r3c2 = (float) matrix.r3c2; } - public FP32Matrix2x3(in FP32Matrix3x2 matrix) + public Matrix2x3FP32(in Matrix3x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -64,7 +64,7 @@ namespace BasicGeometry this.r3c2 = matrix.r2c3; } - public FP32Matrix2x3(in FP64Matrix3x2 matrix) + public Matrix2x3FP32(in Matrix3x2FP64 matrix) { this.r1c1 = (float) matrix.r1c1; this.r1c2 = (float) matrix.r2c1; @@ -88,7 +88,7 @@ namespace BasicGeometry this.r3c2 = 0.0f; } - public void SetValues(in FP32Matrix2x3 matrix) + public void SetValues(in Matrix2x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -100,7 +100,7 @@ namespace BasicGeometry this.r3c2 = matrix.r3c2; } - public void SetValues(in FP64Matrix2x3 matrix) + public void SetValues(in Matrix2x3FP64 matrix) { this.r1c1 = (float) matrix.r1c1; this.r1c2 = (float) matrix.r1c2; @@ -112,7 +112,7 @@ namespace BasicGeometry this.r3c2 = (float) matrix.r3c2; } - public void SetTransposed(in FP32Matrix3x2 matrix) + public void SetTransposed(in Matrix3x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -124,7 +124,7 @@ namespace BasicGeometry this.r3c2 = matrix.r2c3; } - public void SetTransposed(in FP64Matrix3x2 matrix) + public void SetTransposed(in Matrix3x2FP64 matrix) { this.r1c1 = (float) matrix.r1c1; this.r1c2 = (float) matrix.r2c1; @@ -168,7 +168,7 @@ namespace BasicGeometry this.r3c2 = r3; } - public void AppendScaled(in FP32Matrix2x3 matrix, float scale) + public void AppendScaled(in Matrix2x3FP32 matrix, float scale) { this.r1c1 += matrix.r1c1 * scale; this.r1c2 += matrix.r1c2 * scale; @@ -180,7 +180,7 @@ namespace BasicGeometry this.r3c2 += matrix.r3c2 * scale; } - public static void Add(in FP32Matrix2x3 matrix1, in FP32Matrix2x3 matrix2, out FP32Matrix2x3 sum) + public static void Add(in Matrix2x3FP32 matrix1, in Matrix2x3FP32 matrix2, out Matrix2x3FP32 sum) { sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; @@ -192,7 +192,7 @@ namespace BasicGeometry sum.r3c2 = matrix1.r3c2 + matrix2.r3c2; } - public static void Subtract(in FP32Matrix2x3 minuend, in FP32Matrix2x3 subtrahend, out FP32Matrix2x3 difference) + public static void Subtract(in Matrix2x3FP32 minuend, in Matrix2x3FP32 subtrahend, out Matrix2x3FP32 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; @@ -204,7 +204,7 @@ namespace BasicGeometry difference.r3c2 = minuend.r3c2 - subtrahend.r3c2; } - public static void Multiply(in FP32Matrix2x3 multiplicand, float multiplier, out FP32Matrix2x3 product) + public static void Multiply(in Matrix2x3FP32 multiplicand, float multiplier, out Matrix2x3FP32 product) { product.r1c1 = multiplicand.r1c1 * multiplier; product.r1c2 = multiplicand.r1c2 * multiplier; @@ -216,19 +216,19 @@ namespace BasicGeometry product.r3c2 = multiplicand.r3c2 * multiplier; } - public static void Divide(in FP32Matrix2x3 dividend, float divisor, out FP32Matrix2x3 quotient) + public static void Divide(in Matrix2x3FP32 dividend, float divisor, out Matrix2x3FP32 quotient) { Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetRightProduct(in FP32Matrix2x3 matrix, in FP32Vector2 vector, out FP32Vector3 result) + public static void GetRightProduct(in Matrix2x3FP32 matrix, in Vector2FP32 vector, out Vector3FP32 result) { result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; result.x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2; result.x3 = matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2; } - public static void GetLeftProduct(in FP32Vector3 vector, in FP32Matrix2x3 matrix, out FP32Vector2 result) + public static void GetLeftProduct(in Vector3FP32 vector, in Matrix2x3FP32 matrix, out Vector2FP32 result) { result.x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1; result.x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2; diff --git a/BasicGeometry/FP64Matrix2x3.cs b/BasicGeometry/Matrix2x3FP64.cs similarity index 82% rename from BasicGeometry/FP64Matrix2x3.cs rename to BasicGeometry/Matrix2x3FP64.cs index 3605e4a..8de76be 100644 --- a/BasicGeometry/FP64Matrix2x3.cs +++ b/BasicGeometry/Matrix2x3FP64.cs @@ -22,13 +22,13 @@ using System; */ namespace BasicGeometry { - public struct FP64Matrix2x3 + public struct Matrix2x3FP64 { public double r1c1 = 0.0, r1c2 = 0.0; public double r2c1 = 0.0, r2c2 = 0.0; public double r3c1 = 0.0, r3c2 = 0.0; - public FP64Matrix2x3(in FP64Matrix2x3 matrix) + public Matrix2x3FP64(in Matrix2x3FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -40,7 +40,7 @@ namespace BasicGeometry this.r3c2 = matrix.r3c2; } - public FP64Matrix2x3(in FP32Matrix2x3 matrix) + public Matrix2x3FP64(in Matrix2x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -52,7 +52,7 @@ namespace BasicGeometry this.r3c2 = matrix.r3c2; } - public FP64Matrix2x3(in FP64Matrix3x2 matrix) + public Matrix2x3FP64(in Matrix3x2FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -64,7 +64,7 @@ namespace BasicGeometry this.r3c2 = matrix.r2c3; } - public FP64Matrix2x3(in FP32Matrix3x2 matrix) + public Matrix2x3FP64(in Matrix3x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -88,7 +88,7 @@ namespace BasicGeometry this.r3c2 = 0.0; } - public void SetValues(in FP64Matrix2x3 matrix) + public void SetValues(in Matrix2x3FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -100,7 +100,7 @@ namespace BasicGeometry this.r3c2 = matrix.r3c2; } - public void SetValues(in FP32Matrix2x3 matrix) + public void SetValues(in Matrix2x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -112,7 +112,7 @@ namespace BasicGeometry this.r3c2 = matrix.r3c2; } - public void SetTransposed(in FP64Matrix3x2 matrix) + public void SetTransposed(in Matrix3x2FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -124,7 +124,7 @@ namespace BasicGeometry this.r3c2 = matrix.r2c3; } - public void SetTransposed(in FP32Matrix3x2 matrix) + public void SetTransposed(in Matrix3x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -168,7 +168,7 @@ namespace BasicGeometry this.r3c2 = r3; } - public void AppendScaled(in FP64Matrix2x3 matrix, double scale) + public void AppendScaled(in Matrix2x3FP64 matrix, double scale) { this.r1c1 += matrix.r1c1 * scale; this.r1c2 += matrix.r1c2 * scale; @@ -180,7 +180,7 @@ namespace BasicGeometry this.r3c2 += matrix.r3c2 * scale; } - public static void Add(in FP64Matrix2x3 matrix1, in FP64Matrix2x3 matrix2, out FP64Matrix2x3 sum) + public static void Add(in Matrix2x3FP64 matrix1, in Matrix2x3FP64 matrix2, out Matrix2x3FP64 sum) { sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; @@ -192,7 +192,7 @@ namespace BasicGeometry sum.r3c2 = matrix1.r3c2 + matrix2.r3c2; } - public static void Subtract(in FP64Matrix2x3 minuend, in FP64Matrix2x3 subtrahend, out FP64Matrix2x3 difference) + public static void Subtract(in Matrix2x3FP64 minuend, in Matrix2x3FP64 subtrahend, out Matrix2x3FP64 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; @@ -204,7 +204,7 @@ namespace BasicGeometry difference.r3c2 = minuend.r3c2 - subtrahend.r3c2; } - public static void Multiply(in FP64Matrix2x3 multiplicand, double multiplier, out FP64Matrix2x3 product) + public static void Multiply(in Matrix2x3FP64 multiplicand, double multiplier, out Matrix2x3FP64 product) { product.r1c1 = multiplicand.r1c1 * multiplier; product.r1c2 = multiplicand.r1c2 * multiplier; @@ -216,19 +216,19 @@ namespace BasicGeometry product.r3c2 = multiplicand.r3c2 * multiplier; } - public static void Divide(in FP64Matrix2x3 dividend, double divisor, out FP64Matrix2x3 quotient) + public static void Divide(in Matrix2x3FP64 dividend, double divisor, out Matrix2x3FP64 quotient) { Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetRightProduct(in FP64Matrix2x3 matrix, in FP64Vector2 vector, out FP64Vector3 result) + public static void GetRightProduct(in Matrix2x3FP64 matrix, in Vector2FP64 vector, out Vector3FP64 result) { result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; result.x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2; result.x3 = matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2; } - public static void GetLeftProduct(in FP64Vector3 vector, in FP64Matrix2x3 matrix, out FP64Vector2 result) + public static void GetLeftProduct(in Vector3FP64 vector, in Matrix2x3FP64 matrix, out Vector2FP64 result) { result.x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1; result.x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2; diff --git a/BasicGeometry/FP32Matrix3x2.cs b/BasicGeometry/Matrix3x2FP32.cs similarity index 82% rename from BasicGeometry/FP32Matrix3x2.cs rename to BasicGeometry/Matrix3x2FP32.cs index eeac152..5f1ce37 100644 --- a/BasicGeometry/FP32Matrix3x2.cs +++ b/BasicGeometry/Matrix3x2FP32.cs @@ -20,12 +20,12 @@ */ namespace BasicGeometry { - public struct FP32Matrix3x2 + public struct Matrix3x2FP32 { public float r1c1 = 0.0f, r1c2 = 0.0f, r1c3 = 0.0f; public float r2c1 = 0.0f, r2c2 = 0.0f, r2c3 = 0.0f; - public FP32Matrix3x2(in FP32Matrix3x2 matrix) + public Matrix3x2FP32(in Matrix3x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -36,7 +36,7 @@ namespace BasicGeometry this.r2c3 = matrix.r2c3; } - public FP32Matrix3x2(in FP64Matrix3x2 matrix) + public Matrix3x2FP32(in Matrix3x2FP64 matrix) { this.r1c1 = (float) matrix.r1c1; this.r1c2 = (float) matrix.r1c2; @@ -47,7 +47,7 @@ namespace BasicGeometry this.r2c3 = (float) matrix.r2c3; } - public FP32Matrix3x2(in FP32Matrix2x3 matrix) + public Matrix3x2FP32(in Matrix2x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -58,7 +58,7 @@ namespace BasicGeometry this.r2c3 = matrix.r3c2; } - public FP32Matrix3x2(in FP64Matrix2x3 matrix) + public Matrix3x2FP32(in Matrix2x3FP64 matrix) { this.r1c1 = (float) matrix.r1c1; this.r1c2 = (float) matrix.r2c1; @@ -80,7 +80,7 @@ namespace BasicGeometry this.r2c3 = 0.0f; } - public void SetValues(in FP32Matrix3x2 matrix) + public void SetValues(in Matrix3x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -91,7 +91,7 @@ namespace BasicGeometry this.r2c3 = matrix.r2c3; } - public void SetValues(in FP64Matrix3x2 matrix) + public void SetValues(in Matrix3x2FP64 matrix) { this.r1c1 = (float) matrix.r1c1; this.r1c2 = (float) matrix.r1c2; @@ -102,7 +102,7 @@ namespace BasicGeometry this.r2c3 = (float) matrix.r2c3; } - public void SetTransposed(in FP32Matrix2x3 matrix) + public void SetTransposed(in Matrix2x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -113,7 +113,7 @@ namespace BasicGeometry this.r2c3 = matrix.r3c2; } - public void SetTransposed(in FP64Matrix2x3 matrix) + public void SetTransposed(in Matrix2x3FP64 matrix) { this.r1c1 = (float) matrix.r1c1; this.r1c2 = (float) matrix.r2c1; @@ -156,7 +156,7 @@ namespace BasicGeometry this.r2c3 = r2; } - public void AppendScaled(in FP32Matrix3x2 matrix, float scale) + public void AppendScaled(in Matrix3x2FP32 matrix, float scale) { this.r1c1 += matrix.r1c1 * scale; this.r1c2 += matrix.r1c2 * scale; @@ -167,7 +167,7 @@ namespace BasicGeometry this.r2c3 += matrix.r2c3 * scale; } - public static void Add(in FP32Matrix3x2 matrix1, in FP32Matrix3x2 matrix2, out FP32Matrix3x2 sum) + public static void Add(in Matrix3x2FP32 matrix1, in Matrix3x2FP32 matrix2, out Matrix3x2FP32 sum) { sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; @@ -178,7 +178,7 @@ namespace BasicGeometry sum.r2c3 = matrix1.r2c3 + matrix2.r2c3; } - public static void Subtract(in FP32Matrix3x2 minuend, in FP32Matrix3x2 subtrahend, out FP32Matrix3x2 difference) + public static void Subtract(in Matrix3x2FP32 minuend, in Matrix3x2FP32 subtrahend, out Matrix3x2FP32 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; @@ -189,7 +189,7 @@ namespace BasicGeometry difference.r2c3 = minuend.r2c3 - subtrahend.r2c3; } - public static void Multiply(in FP32Matrix3x2 multiplicand, float multiplier, out FP32Matrix3x2 product) + public static void Multiply(in Matrix3x2FP32 multiplicand, float multiplier, out Matrix3x2FP32 product) { product.r1c1 = multiplicand.r1c1 * multiplier; product.r1c2 = multiplicand.r1c2 * multiplier; @@ -200,18 +200,18 @@ namespace BasicGeometry product.r2c3 = multiplicand.r2c3 * multiplier; } - public static void Divide(in FP32Matrix3x2 dividend, float divisor, out FP32Matrix3x2 quotient) + public static void Divide(in Matrix3x2FP32 dividend, float divisor, out Matrix3x2FP32 quotient) { Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetRightProduct(in FP32Matrix3x2 matrix, in FP32Vector3 vector, out FP32Vector2 result) + public static void GetRightProduct(in Matrix3x2FP32 matrix, in Vector3FP32 vector, out Vector2FP32 result) { result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; result.x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3; } - public static void GetLeftProduct(in FP32Vector2 vector, in FP32Matrix3x2 matrix, out FP32Vector3 result) + public static void GetLeftProduct(in Vector2FP32 vector, in Matrix3x2FP32 matrix, out Vector3FP32 result) { result.x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1; result.x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2; diff --git a/BasicGeometry/FP64Matrix3x2.cs b/BasicGeometry/Matrix3x2FP64.cs similarity index 82% rename from BasicGeometry/FP64Matrix3x2.cs rename to BasicGeometry/Matrix3x2FP64.cs index 84bd384..c3e4862 100644 --- a/BasicGeometry/FP64Matrix3x2.cs +++ b/BasicGeometry/Matrix3x2FP64.cs @@ -22,12 +22,12 @@ using System; */ namespace BasicGeometry { - public struct FP64Matrix3x2 + public struct Matrix3x2FP64 { public double r1c1 = 0.0, r1c2 = 0.0, r1c3 = 0.0; public double r2c1 = 0.0, r2c2 = 0.0, r2c3 = 0.0; - public FP64Matrix3x2(in FP64Matrix3x2 matrix) + public Matrix3x2FP64(in Matrix3x2FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -38,7 +38,7 @@ namespace BasicGeometry this.r2c3 = matrix.r2c3; } - public FP64Matrix3x2(in FP32Matrix3x2 matrix) + public Matrix3x2FP64(in Matrix3x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -49,7 +49,7 @@ namespace BasicGeometry this.r2c3 = matrix.r2c3; } - public FP64Matrix3x2(in FP64Matrix2x3 matrix) + public Matrix3x2FP64(in Matrix2x3FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -60,7 +60,7 @@ namespace BasicGeometry this.r2c3 = matrix.r2c2; } - public FP64Matrix3x2(in FP32Matrix2x3 matrix) + public Matrix3x2FP64(in Matrix2x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -83,7 +83,7 @@ namespace BasicGeometry } - public void SetValues(in FP64Matrix3x2 matrix) + public void SetValues(in Matrix3x2FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -94,7 +94,7 @@ namespace BasicGeometry this.r2c3 = matrix.r2c3; } - public void SetValues(in FP32Matrix3x2 matrix) + public void SetValues(in Matrix3x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -105,7 +105,7 @@ namespace BasicGeometry this.r2c3 = matrix.r2c3; } - public void SetTransposed(in FP64Matrix2x3 matrix) + public void SetTransposed(in Matrix2x3FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -116,7 +116,7 @@ namespace BasicGeometry this.r2c3 = matrix.r2c2; } - public void SetTransposed(in FP32Matrix2x3 matrix) + public void SetTransposed(in Matrix2x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -159,7 +159,7 @@ namespace BasicGeometry this.r2c3 = r2; } - public void AppendScaled(in FP64Matrix3x2 matrix, double scale) + public void AppendScaled(in Matrix3x2FP64 matrix, double scale) { this.r1c1 += matrix.r1c1 * scale; this.r1c2 += matrix.r1c2 * scale; @@ -170,7 +170,7 @@ namespace BasicGeometry this.r2c3 += matrix.r2c3 * scale; } - public static void Add(in FP64Matrix3x2 matrix1, in FP64Matrix3x2 matrix2, out FP64Matrix3x2 sum) + public static void Add(in Matrix3x2FP64 matrix1, in Matrix3x2FP64 matrix2, out Matrix3x2FP64 sum) { sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; @@ -181,7 +181,7 @@ namespace BasicGeometry sum.r2c3 = matrix1.r2c3 + matrix2.r2c3; } - public static void Subtract(in FP64Matrix3x2 minuend, in FP64Matrix3x2 subtrahend, out FP64Matrix3x2 difference) + public static void Subtract(in Matrix3x2FP64 minuend, in Matrix3x2FP64 subtrahend, out Matrix3x2FP64 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; @@ -192,7 +192,7 @@ namespace BasicGeometry difference.r2c3 = minuend.r2c3 - subtrahend.r2c3; } - public static void Multiply(in FP64Matrix3x2 multiplicand, double multiplier, out FP64Matrix3x2 product) + public static void Multiply(in Matrix3x2FP64 multiplicand, double multiplier, out Matrix3x2FP64 product) { product.r1c1 = multiplicand.r1c1 * multiplier; product.r1c2 = multiplicand.r1c2 * multiplier; @@ -203,18 +203,18 @@ namespace BasicGeometry product.r2c3 = multiplicand.r2c3 * multiplier; } - public static void Divide(in FP64Matrix3x2 dividend, double divisor, out FP64Matrix3x2 quotient) + public static void Divide(in Matrix3x2FP64 dividend, double divisor, out Matrix3x2FP64 quotient) { Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetRightProduct(in FP64Matrix3x2 matrix, in FP64Vector3 vector, out FP64Vector2 result) + public static void GetRightProduct(in Matrix3x2FP64 matrix, in Vector3FP64 vector, out Vector2FP64 result) { result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; result.x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3; } - public static void GetLeftProduct(in FP64Vector2 vector, in FP64Matrix3x2 matrix, out FP64Vector3 result) + public static void GetLeftProduct(in Vector2FP64 vector, in Matrix3x2FP64 matrix, out Vector3FP64 result) { result.x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1; result.x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2; diff --git a/BasicGeometry/FP32Matrix3x3.cs b/BasicGeometry/Matrix3x3FP32.cs similarity index 88% rename from BasicGeometry/FP32Matrix3x3.cs rename to BasicGeometry/Matrix3x3FP32.cs index 5eb1f00..665c900 100644 --- a/BasicGeometry/FP32Matrix3x3.cs +++ b/BasicGeometry/Matrix3x3FP32.cs @@ -23,20 +23,20 @@ using System; namespace BasicGeometry { - public struct FP32Matrix3x3 + public struct Matrix3x3FP32 { public float r1c1 = 0.0f, r1c2 = 0.0f, r1c3 = 0.0f; public float r2c1 = 0.0f, r2c2 = 0.0f, r2c3 = 0.0f; public float r3c1 = 0.0f, r3c2 = 0.0f, r3c3 = 0.0f; - public FP32Matrix3x3(float d1, float d2, float d3) + public Matrix3x3FP32(float d1, float d2, float d3) { this.r1c1 = d1; this.r2c2 = d2; this.r3c3 = d3; } - public FP32Matrix3x3(in FP32Matrix3x3 matrix) + public Matrix3x3FP32(in Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -51,7 +51,7 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public FP32Matrix3x3(in FP64Matrix3x3 matrix) + public Matrix3x3FP32(in Matrix3x3FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r1c2; @@ -76,7 +76,7 @@ namespace BasicGeometry public readonly bool IsSingular() { float determinant = this.GetDeterminant(); - return -FP32Utility.EPSYLON <= determinant && determinant <= FP32Utility.EPSYLON; + return -UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON; } public void Transpose() @@ -90,7 +90,7 @@ namespace BasicGeometry { float determinant = this.GetDeterminant(); - if (-FP32Utility.EPSYLON <= determinant && determinant <= FP32Utility.EPSYLON) { + if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) { return false; } @@ -168,7 +168,7 @@ namespace BasicGeometry this.r2c3 = d3; } - public void SetValues(in FP32Matrix3x3 matrix) + public void SetValues(in Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -183,7 +183,7 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public void SetValues(in FP64Matrix3x3 matrix) + public void SetValues(in Matrix3x3FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r1c2; @@ -198,7 +198,7 @@ namespace BasicGeometry this.r3c3 = (float)matrix.r3c3; } - public void SetTransposedOf(in FP32Matrix3x3 matrix) + public void SetTransposedOf(in Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r2c2 = matrix.r2c2; @@ -209,7 +209,7 @@ namespace BasicGeometry (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); } - public void SetTransposedOf(in FP64Matrix3x3 matrix) + public void SetTransposedOf(in Matrix3x3FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r2c1; @@ -224,11 +224,11 @@ namespace BasicGeometry this.r3c3 = (float)matrix.r3c3; } - public bool SetInvertedOf(in FP32Matrix3x3 matrix) + public bool SetInvertedOf(in Matrix3x3FP32 matrix) { float determinant = matrix.GetDeterminant(); - if (-FP32Utility.EPSYLON <= determinant && determinant <= FP32Utility.EPSYLON) { + if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) { return false; } @@ -303,7 +303,7 @@ namespace BasicGeometry this.r3c3 = r3; } - public void AppendScaled(in FP32Matrix3x3 matrix, float scale) + public void AppendScaled(in Matrix3x3FP32 matrix, float scale) { this.r1c1 += matrix.r1c1* scale; this.r1c2 += matrix.r1c2* scale; @@ -318,7 +318,7 @@ namespace BasicGeometry this.r3c3 += matrix.r3c3* scale; } - public static void Add(in FP32Matrix3x3 matrix1, in FP32Matrix3x3 matrix2, out FP32Matrix3x3 sum) + public static void Add(in Matrix3x3FP32 matrix1, in Matrix3x3FP32 matrix2, out Matrix3x3FP32 sum) { sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; @@ -333,7 +333,7 @@ namespace BasicGeometry sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; } - public static void Subtract(in FP32Matrix3x3 minuend, in FP32Matrix3x3 subtrahend, out FP32Matrix3x3 difference) + public static void Subtract(in Matrix3x3FP32 minuend, in Matrix3x3FP32 subtrahend, out Matrix3x3FP32 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; @@ -348,7 +348,7 @@ namespace BasicGeometry difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; } - public static void Multiply(in FP32Matrix3x3 multiplicand, float multiplier, out FP32Matrix3x3 product) + public static void Multiply(in Matrix3x3FP32 multiplicand, float multiplier, out Matrix3x3FP32 product) { product.r1c1 = multiplicand.r1c1 * multiplier; product.r1c2 = multiplicand.r1c2 * multiplier; @@ -363,12 +363,12 @@ namespace BasicGeometry product.r3c3 = multiplicand.r3c3 * multiplier; } - public static void Divide(in FP32Matrix3x3 dividend, float divisor, out FP32Matrix3x3 quotient) + public static void Divide(in Matrix3x3FP32 dividend, float divisor, out Matrix3x3FP32 quotient) { Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetRightProduct(in FP32Matrix3x3 matrix, in FP32Vector3 vector, out FP32Vector3 result) + public static void GetRightProduct(in Matrix3x3FP32 matrix, in Vector3FP32 vector, out Vector3FP32 result) { float x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; float x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3; @@ -379,7 +379,7 @@ namespace BasicGeometry result.x3 = x3; } - public static void GetLeftProduct(in FP32Vector3 vector, in FP32Matrix3x3 matrix, out FP32Vector3 result) + public static void GetLeftProduct(in Vector3FP32 vector, in Matrix3x3FP32 matrix, out Vector3FP32 result) { float x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1; float x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2; @@ -390,7 +390,7 @@ namespace BasicGeometry result.x3 = x3; } - public static void LoadZero(out FP32Matrix3x3 matrix) + public static void LoadZero(out Matrix3x3FP32 matrix) { matrix.r1c1 = 0.0f; matrix.r1c2 = 0.0f; @@ -405,7 +405,7 @@ namespace BasicGeometry matrix.r3c3 = 0.0f; } - public static void LoadIdentity(out FP32Matrix3x3 matrix) + public static void LoadIdentity(out Matrix3x3FP32 matrix) { matrix.r1c1 = 1.0f; matrix.r1c2 = 0.0f; @@ -420,7 +420,7 @@ namespace BasicGeometry matrix.r3c3 = 1.0f; } - public static void LoadDiagonal(float d1, float d2, float d3, out FP32Matrix3x3 matrix) + public static void LoadDiagonal(float d1, float d2, float d3, out Matrix3x3FP32 matrix) { matrix.r1c1 = d1; matrix.r1c2 = 0.0f; diff --git a/BasicGeometry/FP64Matrix3x3.cs b/BasicGeometry/Matrix3x3FP64.cs similarity index 88% rename from BasicGeometry/FP64Matrix3x3.cs rename to BasicGeometry/Matrix3x3FP64.cs index fafad05..434cd85 100644 --- a/BasicGeometry/FP64Matrix3x3.cs +++ b/BasicGeometry/Matrix3x3FP64.cs @@ -23,20 +23,20 @@ using System; namespace BasicGeometry { - public struct FP64Matrix3x3 + public struct Matrix3x3FP64 { public double r1c1 = 0.0, r1c2 = 0.0, r1c3 = 0.0; public double r2c1 = 0.0, r2c2 = 0.0, r2c3 = 0.0; public double r3c1 = 0.0, r3c2 = 0.0, r3c3 = 0.0; - public FP64Matrix3x3(double d1, double d2, double d3) + public Matrix3x3FP64(double d1, double d2, double d3) { this.r1c1 = d1; this.r2c2 = d2; this.r3c3 = d3; } - public FP64Matrix3x3(in FP64Matrix3x3 matrix) + public Matrix3x3FP64(in Matrix3x3FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -51,7 +51,7 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public FP64Matrix3x3(in FP32Matrix3x3 matrix) + public Matrix3x3FP64(in Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -76,7 +76,7 @@ namespace BasicGeometry public readonly bool IsSingular() { double determinant = this.GetDeterminant(); - return -FP64Utility.EPSYLON <= determinant && determinant <= FP64Utility.EPSYLON; + return -UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON; } public void Transpose() @@ -90,7 +90,7 @@ namespace BasicGeometry { double determinant = this.GetDeterminant(); - if (-FP64Utility.EPSYLON <= determinant && determinant <= FP64Utility.EPSYLON) { + if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) { return false; } @@ -168,7 +168,7 @@ namespace BasicGeometry this.r2c3 = d3; } - public void SetValues(FP64Matrix3x3 matrix) + public void SetValues(Matrix3x3FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -183,7 +183,7 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public void SetValues(FP32Matrix3x3 matrix) + public void SetValues(Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -198,7 +198,7 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public void SetTransposedOf(in FP64Matrix3x3 matrix) + public void SetTransposedOf(in Matrix3x3FP64 matrix) { this.r1c1 = matrix.r1c1; this.r2c2 = matrix.r2c2; @@ -209,7 +209,7 @@ namespace BasicGeometry (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); } - public void SetTransposedOf(in FP32Matrix3x3 matrix) + public void SetTransposedOf(in Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r2c2 = matrix.r2c2; @@ -220,11 +220,11 @@ namespace BasicGeometry (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); } - public bool SetInvertedOf(in FP64Matrix3x3 matrix) + public bool SetInvertedOf(in Matrix3x3FP64 matrix) { double determinant = matrix.GetDeterminant(); - if (-FP64Utility.EPSYLON <= determinant && determinant <= FP64Utility.EPSYLON) { + if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) { return false; } @@ -299,7 +299,7 @@ namespace BasicGeometry this.r3c3 = r3; } - public void AppendScaled(in FP64Matrix3x3 matrix, double scale) + public void AppendScaled(in Matrix3x3FP64 matrix, double scale) { this.r1c1 += matrix.r1c1 * scale; this.r1c2 += matrix.r1c2 * scale; @@ -314,7 +314,7 @@ namespace BasicGeometry this.r3c3 += matrix.r3c3 * scale; } - public static void Add(in FP64Matrix3x3 matrix1, in FP64Matrix3x3 matrix2, out FP64Matrix3x3 sum) + public static void Add(in Matrix3x3FP64 matrix1, in Matrix3x3FP64 matrix2, out Matrix3x3FP64 sum) { sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; @@ -329,7 +329,7 @@ namespace BasicGeometry sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; } - public static void Subtract(in FP64Matrix3x3 minuend, in FP64Matrix3x3 subtrahend, out FP64Matrix3x3 difference) + public static void Subtract(in Matrix3x3FP64 minuend, in Matrix3x3FP64 subtrahend, out Matrix3x3FP64 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; @@ -344,7 +344,7 @@ namespace BasicGeometry difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; } - public static void Multiply(in FP64Matrix3x3 multiplicand, double multiplier, out FP64Matrix3x3 product) + public static void Multiply(in Matrix3x3FP64 multiplicand, double multiplier, out Matrix3x3FP64 product) { product.r1c1 = multiplicand.r1c1 * multiplier; product.r1c2 = multiplicand.r1c2 * multiplier; @@ -359,12 +359,12 @@ namespace BasicGeometry product.r3c3 = multiplicand.r3c3 * multiplier; } - public static void Divide(in FP64Matrix3x3 dividend, double divisor, out FP64Matrix3x3 quotient) + public static void Divide(in Matrix3x3FP64 dividend, double divisor, out Matrix3x3FP64 quotient) { Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetRightProduct(in FP64Matrix3x3 matrix, in FP64Vector3 vector, out FP64Vector3 result) + public static void GetRightProduct(in Matrix3x3FP64 matrix, in Vector3FP64 vector, out Vector3FP64 result) { double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; double x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3; @@ -375,7 +375,7 @@ namespace BasicGeometry result.x3 = x3; } - public static void GetLeftProduct(in FP64Vector3 vector, in FP64Matrix3x3 matrix, out FP64Vector3 result) + public static void GetLeftProduct(in Vector3FP64 vector, in Matrix3x3FP64 matrix, out Vector3FP64 result) { double x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1; double x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2; @@ -386,7 +386,7 @@ namespace BasicGeometry result.x3 = x3; } - public static void LoadZero(out FP64Matrix3x3 matrix) + public static void LoadZero(out Matrix3x3FP64 matrix) { matrix.r1c1 = 0.0; matrix.r1c2 = 0.0; @@ -401,7 +401,7 @@ namespace BasicGeometry matrix.r3c3 = 0.0; } - public static void LoadIdentity(out FP64Matrix3x3 matrix) + public static void LoadIdentity(out Matrix3x3FP64 matrix) { matrix.r1c1 = 1.0; matrix.r1c2 = 0.0; @@ -416,7 +416,7 @@ namespace BasicGeometry matrix.r3c3 = 1.0; } - public static void LoadDiagonal(double d1, double d2, double d3, out FP64Matrix3x3 matrix) + public static void LoadDiagonal(double d1, double d2, double d3, out Matrix3x3FP64 matrix) { matrix.r1c1 = d1; matrix.r1c2 = 0.0; diff --git a/BasicGeometry/FP32MatrixProduct.cs b/BasicGeometry/MatrixProductFP32.cs similarity index 87% rename from BasicGeometry/FP32MatrixProduct.cs rename to BasicGeometry/MatrixProductFP32.cs index 75ae41f..9acb4a1 100644 --- a/BasicGeometry/FP32MatrixProduct.cs +++ b/BasicGeometry/MatrixProductFP32.cs @@ -22,9 +22,9 @@ using System; */ namespace BasicGeometry { - public class FP32MatrixProduct + public class MatrixProductFP32 { - public static void Get2x2At2x2(in FP32Matrix2x2 left, in FP32Matrix2x2 right, out FP32Matrix2x2 product) + public static void Get2x2At2x2(in Matrix2x2FP32 left, in Matrix2x2FP32 right, out Matrix2x2FP32 product) { float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; @@ -39,7 +39,7 @@ namespace BasicGeometry product.r2c2 = r2c2; } - public static void Get2x2At3x2(in FP32Matrix2x2 left, in FP32Matrix3x2 right, out FP32Matrix3x2 product) + public static void Get2x2At3x2(in Matrix2x2FP32 left, in Matrix3x2FP32 right, out Matrix3x2FP32 product) { float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; @@ -58,7 +58,7 @@ namespace BasicGeometry product.r2c3 = r2c3; } - public static void Get2x3At2x2(in FP32Matrix2x3 left, in FP32Matrix2x2 right, out FP32Matrix2x3 product) + public static void Get2x3At2x2(in Matrix2x3FP32 left, in Matrix2x2FP32 right, out Matrix2x3FP32 product) { float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; @@ -79,7 +79,7 @@ namespace BasicGeometry product.r3c2 = r3c2; } - public static void Get2x3At3x2(in FP32Matrix2x3 left, in FP32Matrix3x2 right, out FP32Matrix3x3 product) + public static void Get2x3At3x2(in Matrix2x3FP32 left, in Matrix3x2FP32 right, out Matrix3x3FP32 product) { product.r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; product.r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; @@ -94,7 +94,7 @@ namespace BasicGeometry product.r3c3 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c3; } - public static void Get3x2At3x3(in FP32Matrix3x2 left, in FP32Matrix3x3 right, out FP32Matrix3x2 product) + public static void Get3x2At3x3(in Matrix3x2FP32 left, in Matrix3x3FP32 right, out Matrix3x2FP32 product) { float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1; float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2; @@ -113,7 +113,7 @@ namespace BasicGeometry product.r2c3 = r2c3; } - public static void Get3x2At2x3(in FP32Matrix3x2 left, in FP32Matrix2x3 right, out FP32Matrix2x2 product) + public static void Get3x2At2x3(in Matrix3x2FP32 left, in Matrix2x3FP32 right, out Matrix2x2FP32 product) { product.r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1; product.r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2; @@ -122,7 +122,7 @@ namespace BasicGeometry product.r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; } - public static void Get3x3At2x3(in FP32Matrix3x3 left, in FP32Matrix2x3 right, out FP32Matrix2x3 product) + public static void Get3x3At2x3(in Matrix3x3FP32 left, in Matrix2x3FP32 right, out Matrix2x3FP32 product) { float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1; float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2; @@ -143,7 +143,7 @@ namespace BasicGeometry product.r3c2 = r3c2; } - public static void Get3x3At3x3(in FP32Matrix3x3 left, in FP32Matrix3x3 right, out FP32Matrix3x3 product) + public static void Get3x3At3x3(in Matrix3x3FP32 left, in Matrix3x3FP32 right, out Matrix3x3FP32 product) { float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1; float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2; diff --git a/BasicGeometry/FP64MatrixProduct.cs b/BasicGeometry/MatrixProductFP64.cs similarity index 87% rename from BasicGeometry/FP64MatrixProduct.cs rename to BasicGeometry/MatrixProductFP64.cs index 8a6d2fc..e91fd3b 100644 --- a/BasicGeometry/FP64MatrixProduct.cs +++ b/BasicGeometry/MatrixProductFP64.cs @@ -22,9 +22,9 @@ using System; */ namespace BasicGeometry { - public class FP64MatrixProduct + public class MatrixProductFP64 { - public static void Get2x2At2x2(in FP64Matrix2x2 left, in FP64Matrix2x2 right, out FP64Matrix2x2 product) + public static void Get2x2At2x2(in Matrix2x2FP64 left, in Matrix2x2FP64 right, out Matrix2x2FP64 product) { double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; @@ -39,7 +39,7 @@ namespace BasicGeometry product.r2c2 = r2c2; } - public static void Get2x2At3x2(in FP64Matrix2x2 left, in FP64Matrix3x2 right, out FP64Matrix3x2 product) + public static void Get2x2At3x2(in Matrix2x2FP64 left, in Matrix3x2FP64 right, out Matrix3x2FP64 product) { double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; @@ -58,7 +58,7 @@ namespace BasicGeometry product.r2c3 = r2c3; } - public static void Get2x3At2x2(in FP64Matrix2x3 left, in FP64Matrix2x2 right, out FP64Matrix2x3 product) + public static void Get2x3At2x2(in Matrix2x3FP64 left, in Matrix2x2FP64 right, out Matrix2x3FP64 product) { double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; @@ -79,7 +79,7 @@ namespace BasicGeometry product.r3c2 = r3c2; } - public static void Get2x3At3x2(in FP64Matrix2x3 left, in FP64Matrix3x2 right, out FP64Matrix3x3 product) + public static void Get2x3At3x2(in Matrix2x3FP64 left, in Matrix3x2FP64 right, out Matrix3x3FP64 product) { product.r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; product.r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; @@ -94,7 +94,7 @@ namespace BasicGeometry product.r3c3 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c3; } - public static void Get3x2At3x3(in FP64Matrix3x2 left, in FP64Matrix3x3 right, out FP64Matrix3x2 product) + public static void Get3x2At3x3(in Matrix3x2FP64 left, in Matrix3x3FP64 right, out Matrix3x2FP64 product) { double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1; double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2; @@ -113,7 +113,7 @@ namespace BasicGeometry product.r2c3 = r2c3; } - public static void Get3x2At2x3(in FP64Matrix3x2 left, in FP64Matrix2x3 right, out FP64Matrix2x2 product) + public static void Get3x2At2x3(in Matrix3x2FP64 left, in Matrix2x3FP64 right, out Matrix2x2FP64 product) { product.r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1; product.r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2; @@ -122,7 +122,7 @@ namespace BasicGeometry product.r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; } - public static void Get3x3At2x3(in FP64Matrix3x3 left, in FP64Matrix2x3 right, out FP64Matrix2x3 product) + public static void Get3x3At2x3(in Matrix3x3FP64 left, in Matrix2x3FP64 right, out Matrix2x3FP64 product) { double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1; double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2; @@ -143,7 +143,7 @@ namespace BasicGeometry product.r3c2 = r3c2; } - public static void Get3x3At3x3(in FP64Matrix3x3 left, in FP64Matrix3x3 right, out FP64Matrix3x3 product) + public static void Get3x3At3x3(in Matrix3x3FP64 left, in Matrix3x3FP64 right, out Matrix3x3FP64 product) { double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1; double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2; diff --git a/BasicGeometry/FP32Quaternion.cs b/BasicGeometry/QuaternionFP32.cs similarity index 70% rename from BasicGeometry/FP32Quaternion.cs rename to BasicGeometry/QuaternionFP32.cs index 230807e..4d16ec3 100644 --- a/BasicGeometry/FP32Quaternion.cs +++ b/BasicGeometry/QuaternionFP32.cs @@ -2,11 +2,11 @@ namespace BasicGeometry { - public struct FP32Quaternion + public struct QuaternionFP32 { public float s0 = 0.0f, x1 = 0.0f, x2 = 0.0f, x3 = 0.0f; - public FP32Quaternion(float s0, float x1, float x2, float x3) + public QuaternionFP32(float s0, float x1, float x2, float x3) { this.s0 = s0; this.x1 = x1; @@ -14,7 +14,7 @@ namespace BasicGeometry this.x3 = x3; } - public FP32Quaternion(in FP32Quaternion quaternion) + public QuaternionFP32(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -22,7 +22,7 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public FP32Quaternion(in FP64Quaternion quaternion) + public QuaternionFP32(in QuaternionFP64 quaternion) { this.s0 = (float)quaternion.s0; this.x1 = (float)quaternion.x1; @@ -30,6 +30,26 @@ namespace BasicGeometry this.x3 = (float)quaternion.x3; } + public readonly float GetSquareModule() + { + return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); + } + + public readonly float GetModule() + { + return MathF.Sqrt(this.GetSquareModule()); + } + + public readonly bool IsZero() + { + return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON; + } + + public readonly bool IsUnit() + { + return UtilityFP32.IsSqareValueUnit(this.GetSquareModule()); + } + public void Reset() { this.s0 = 0.0f; @@ -38,6 +58,14 @@ namespace BasicGeometry this.x3 = 0.0f; } + public void SetToIdentity() + { + this.s0 = 1.0f; + this.x1 = 0.0f; + this.x2 = 0.0f; + this.x3 = 0.0f; + } + public void Conjugate() { this.x1 = -this.x1; @@ -53,7 +81,7 @@ namespace BasicGeometry this.x3 = x3; } - public void SetValues(in FP32Quaternion quaternion) + public void SetValues(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -61,7 +89,7 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public void SetValues(in FP64Quaternion quaternion) + public void SetValues(in QuaternionFP64 quaternion) { this.s0 = (float)quaternion.s0; this.x1 = (float)quaternion.x1; @@ -69,7 +97,7 @@ namespace BasicGeometry this.x3 = (float)quaternion.x3; } - public void SetConjugateOf(in FP32Quaternion quaternion) + public void SetConjugateOf(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = -quaternion.x1; @@ -77,7 +105,7 @@ namespace BasicGeometry this.x3 = -quaternion.x3; } - public readonly void MakeRotationMatrix(out FP32Matrix3x3 matrix) + public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix) { float s0s0 = this.s0 * this.s0; float x1x1 = this.x1 * this.x1; @@ -86,23 +114,14 @@ namespace BasicGeometry float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); - if (-FP32Utility.EPSYLON <= squareModule && squareModule <= FP32Utility.EPSYLON) + if (squareModule <= UtilityFP32.SQUARE_EPSYLON) { - FP32Matrix3x3.LoadIdentity(out matrix); + Matrix3x3FP32.LoadIdentity(out matrix); return; } - float corrector1; - float corrector2; - - if (1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + FP32Utility.TWO_EPSYLON) { - corrector1 = 2.0f - squareModule; - corrector2 = 2.0f * corrector1; - } - else { - corrector1 = 1.0f / squareModule; - corrector2 = 2.0f / squareModule; - } + float corrector1 = 1.0f / squareModule; + float corrector2 = 2.0f * corrector1; float s0x1 = this.s0 * this.x1; float s0x2 = this.s0 * this.x2; @@ -124,7 +143,7 @@ namespace BasicGeometry matrix.r1c3 = corrector2 * (x1x3 + s0x2); } - public readonly void MakeReverseMatrix(out FP32Matrix3x3 matrix) + public readonly void MakeReverseMatrix(out Matrix3x3FP32 matrix) { float s0s0 = this.s0 * this.s0; float x1x1 = this.x1 * this.x1; @@ -133,23 +152,14 @@ namespace BasicGeometry float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); - if (-FP32Utility.EPSYLON <= squareModule && squareModule <= FP32Utility.EPSYLON) + if (squareModule <= UtilityFP32.SQUARE_EPSYLON) { - FP32Matrix3x3.LoadIdentity(out matrix); + Matrix3x3FP32.LoadIdentity(out matrix); return; } - float corrector1; - float corrector2; - - if (1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + FP32Utility.TWO_EPSYLON) { - corrector1 = 2.0f - squareModule; - corrector2 = 2.0f * corrector1; - } - else { - corrector1 = 1.0f / squareModule; - corrector2 = 2.0f / squareModule; - } + float corrector1 = 1.0f / squareModule; + float corrector2 = 2.0f * corrector1; float s0x1 = this.s0 * this.x1; float s0x2 = this.s0 * this.x2; @@ -171,7 +181,7 @@ namespace BasicGeometry matrix.r1c3 = corrector2 * (x1x3 - s0x2); } - public static void Add(in FP32Quaternion quaternion1, in FP32Quaternion quaternion2, out FP32Quaternion sum) + public static void Add(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2, out QuaternionFP32 sum) { sum.s0 = quaternion1.s0 + quaternion2.s0; sum.x1 = quaternion1.x1 + quaternion2.x1; @@ -179,7 +189,7 @@ namespace BasicGeometry sum.x3 = quaternion1.x3 + quaternion2.x3; } - public static void Subtract(in FP32Quaternion minuend, in FP32Quaternion subtrahend, out FP32Quaternion difference) + public static void Subtract(in QuaternionFP32 minuend, in QuaternionFP32 subtrahend, out QuaternionFP32 difference) { difference.s0 = minuend.s0 - subtrahend.s0; difference.x1 = minuend.x1 - subtrahend.x1; @@ -187,7 +197,7 @@ namespace BasicGeometry difference.x3 = minuend.x3 - subtrahend.x3; } - public static void Multiply(in FP32Quaternion left, in FP32Quaternion right, out FP32Quaternion product) + public static void Multiply(in QuaternionFP32 left, in QuaternionFP32 right, out QuaternionFP32 product) { float s0 = (left.s0 * right.s0 - left.x1 * right.x1) - (left.x2 * right.x2 + left.x3 * right.x3); float x1 = (left.x1 * right.s0 + left.s0 * right.x1) - (left.x3 * right.x2 - left.x2 * right.x3); diff --git a/BasicGeometry/FP64Quaternion.cs b/BasicGeometry/QuaternionFP64.cs similarity index 70% rename from BasicGeometry/FP64Quaternion.cs rename to BasicGeometry/QuaternionFP64.cs index f7ada4c..13f5dd6 100644 --- a/BasicGeometry/FP64Quaternion.cs +++ b/BasicGeometry/QuaternionFP64.cs @@ -3,11 +3,11 @@ using System.Numerics; namespace BasicGeometry { - public struct FP64Quaternion + public struct QuaternionFP64 { public double s0 = 0.0, x1 = 0.0, x2 = 0.0, x3 = 0.0; - public FP64Quaternion(double s0, double x1, double x2, double x3) + public QuaternionFP64(double s0, double x1, double x2, double x3) { this.s0 = s0; this.x1 = x1; @@ -15,7 +15,7 @@ namespace BasicGeometry this.x3 = x3; } - public FP64Quaternion(in FP32Quaternion quaternion) + public QuaternionFP64(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -23,7 +23,7 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public FP64Quaternion(in FP64Quaternion quaternion) + public QuaternionFP64(in QuaternionFP64 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -31,6 +31,26 @@ namespace BasicGeometry this.x3 = quaternion.x3; } + public readonly double GetSquareModule() + { + return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); + } + + public readonly double GetModule() + { + return Math.Sqrt(this.GetSquareModule()); + } + + public readonly bool IsZero() + { + return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON; + } + + public readonly bool IsUnit() + { + return UtilityFP64.IsSqareValueUnit(this.GetSquareModule()); + } + public void Reset() { this.s0 = 0.0; @@ -39,6 +59,14 @@ namespace BasicGeometry this.x3 = 0.0; } + public void SetToIdentity() + { + this.s0 = 1.0; + this.x1 = 0.0; + this.x2 = 0.0; + this.x3 = 0.0; + } + public void Conjugate() { this.x1 = -this.x1; @@ -54,7 +82,7 @@ namespace BasicGeometry this.x3 = x3; } - public void SetValues(in FP32Quaternion quaternion) + public void SetValues(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -62,7 +90,7 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public void SetValues(in FP64Quaternion quaternion) + public void SetValues(in QuaternionFP64 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -70,7 +98,7 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public void SetConjugateOf(in FP64Quaternion quaternion) + public void SetConjugateOf(in QuaternionFP64 quaternion) { this.s0 = quaternion.s0; this.x1 = -quaternion.x1; @@ -78,7 +106,7 @@ namespace BasicGeometry this.x3 = -quaternion.x3; } - public readonly void MakeRotationMatrix(out FP64Matrix3x3 matrix) + public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix) { double s0s0 = this.s0 * this.s0; double x1x1 = this.x1 * this.x1; @@ -87,23 +115,14 @@ namespace BasicGeometry double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); - if (-FP64Utility.EPSYLON <= squareModule && squareModule <= FP64Utility.EPSYLON) + if (squareModule <= UtilityFP64.SQUARE_EPSYLON) { - FP64Matrix3x3.LoadIdentity(out matrix); + Matrix3x3FP64.LoadIdentity(out matrix); return; } - double corrector1; - double corrector2; - - if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON) { - corrector1 = 2.0 - squareModule; - corrector2 = 2.0 * corrector1; - } - else { - corrector1 = 1.0 / squareModule; - corrector2 = 2.0 / squareModule; - } + double corrector1 = 1.0 / squareModule; + double corrector2 = 2.0 * corrector1; double s0x1 = this.s0 * this.x1; double s0x2 = this.s0 * this.x2; @@ -125,7 +144,7 @@ namespace BasicGeometry matrix.r1c3 = corrector2 * (x1x3 + s0x2); } - public readonly void MakeReverseMatrix(out FP64Matrix3x3 matrix) + public readonly void MakeReverseMatrix(out Matrix3x3FP64 matrix) { double s0s0 = this.s0 * this.s0; double x1x1 = this.x1 * this.x1; @@ -134,23 +153,14 @@ namespace BasicGeometry double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); - if (-FP64Utility.EPSYLON <= squareModule && squareModule <= FP64Utility.EPSYLON) + if (squareModule <= UtilityFP64.SQUARE_EPSYLON) { - FP64Matrix3x3.LoadIdentity(out matrix); + Matrix3x3FP64.LoadIdentity(out matrix); return; } - double corrector1; - double corrector2; - - if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON) { - corrector1 = 2.0 - squareModule; - corrector2 = 2.0 * corrector1; - } - else { - corrector1 = 1.0 / squareModule; - corrector2 = 2.0 / squareModule; - } + double corrector1 = 1.0 / squareModule; + double corrector2 = 2.0 * corrector1; double s0x1 = this.s0 * this.x1; double s0x2 = this.s0 * this.x2; @@ -172,7 +182,7 @@ namespace BasicGeometry matrix.r1c3 = corrector2 * (x1x3 - s0x2); } - public static void Add(in FP64Quaternion quaternion1, in FP64Quaternion quaternion2, out FP64Quaternion sum) + public static void Add(in QuaternionFP64 quaternion1, in QuaternionFP64 quaternion2, out QuaternionFP64 sum) { sum.s0 = quaternion1.s0 + quaternion2.s0; sum.x1 = quaternion1.x1 + quaternion2.x1; @@ -180,7 +190,7 @@ namespace BasicGeometry sum.x3 = quaternion1.x3 + quaternion2.x3; } - public static void Subtract(in FP64Quaternion minuend, in FP64Quaternion subtrahend, out FP64Quaternion difference) + public static void Subtract(in QuaternionFP64 minuend, in QuaternionFP64 subtrahend, out QuaternionFP64 difference) { difference.s0 = minuend.s0 - subtrahend.s0; difference.x1 = minuend.x1 - subtrahend.x1; @@ -188,7 +198,7 @@ namespace BasicGeometry difference.x3 = minuend.x3 - subtrahend.x3; } - public static void Multiply(in FP64Quaternion left, in FP64Quaternion right, out FP64Quaternion product) + public static void Multiply(in QuaternionFP64 left, in QuaternionFP64 right, out QuaternionFP64 product) { double s0 = (left.s0 * right.s0 - left.x1 * right.x1) - (left.x2 * right.x2 + left.x3 * right.x3); double x1 = (left.x1 * right.s0 + left.s0 * right.x1) - (left.x3 * right.x2 - left.x2 * right.x3); diff --git a/BasicGeometry/FP32Radians.cs b/BasicGeometry/RadianFP32.cs similarity index 98% rename from BasicGeometry/FP32Radians.cs rename to BasicGeometry/RadianFP32.cs index a6bf46f..1c4a002 100644 --- a/BasicGeometry/FP32Radians.cs +++ b/BasicGeometry/RadianFP32.cs @@ -6,7 +6,7 @@ namespace BasicGeometry { - public class FP32Radians + public class RadianFP32 { public const float PI = 3.1415926536f; public const float TWO_PI = 6.2831853072f; diff --git a/BasicGeometry/FP64Radians.cs b/BasicGeometry/RadianFP64.cs similarity index 98% rename from BasicGeometry/FP64Radians.cs rename to BasicGeometry/RadianFP64.cs index 8163a72..8be238e 100644 --- a/BasicGeometry/FP64Radians.cs +++ b/BasicGeometry/RadianFP64.cs @@ -6,7 +6,7 @@ namespace BasicGeometry { - public class FP64Radians + public class RadianFP64 { public const double PI = 3.14159265358979324; public const double TWO_PI = 6.28318530717958648; diff --git a/BasicGeometry/FP32Rotation3.cs b/BasicGeometry/Rotation3FP32.cs similarity index 89% rename from BasicGeometry/FP32Rotation3.cs rename to BasicGeometry/Rotation3FP32.cs index a020a3d..95d6a1b 100644 --- a/BasicGeometry/FP32Rotation3.cs +++ b/BasicGeometry/Rotation3FP32.cs @@ -23,13 +23,13 @@ using System; namespace BasicGeometry { - public struct FP32Rotation3 + public struct Rotation3FP32 { private float angle = 0.0f; - private FP32Vector3 axis; + private Vector3FP32 axis; - public FP32Rotation3(FP32Rotation3 rotation) + public Rotation3FP32(Rotation3FP32 rotation) { this.angle = rotation.angle; this.axis = rotation.axis; diff --git a/BasicGeometry/Rotation3FP64.cs b/BasicGeometry/Rotation3FP64.cs new file mode 100644 index 0000000..2c2de9d --- /dev/null +++ b/BasicGeometry/Rotation3FP64.cs @@ -0,0 +1,44 @@ +/* + * Copyright 2019-2025 Andrey Pokidov + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +using System; + +/* + * Author: Andrey Pokidov + * Date: 2 Feb 2019 + */ + +namespace BasicGeometry +{ + public struct Rotation3FP64 + { + private double angle = 0.0; + + private Vector3FP64 axis; + + public Rotation3FP64(Rotation3FP64 rotation) + { + this.angle = rotation.angle; + this.axis = rotation.axis; + } + + public void Reset() + { + this.angle = 0.0; + this.axis.Reset(); + } + } +} diff --git a/BasicGeometry/FP32Turns.cs b/BasicGeometry/TurnFP32.cs similarity index 90% rename from BasicGeometry/FP32Turns.cs rename to BasicGeometry/TurnFP32.cs index 36bf85b..8016396 100644 --- a/BasicGeometry/FP32Turns.cs +++ b/BasicGeometry/TurnFP32.cs @@ -6,11 +6,11 @@ namespace BasicGeometry { - public class FP32Turns + public class TurnFP32 { public static float ToRadians(float turns) { - return turns * FP32Radians.TWO_PI; + return turns * RadianFP32.TWO_PI; } public static float ToDegrees(float turns) @@ -22,7 +22,7 @@ namespace BasicGeometry { if (toUnit == AngleUnit.RADIANS) { - return turns * FP32Radians.TWO_PI; + return turns * RadianFP32.TWO_PI; } if (toUnit == AngleUnit.DEGREES) diff --git a/BasicGeometry/FP64Turns.cs b/BasicGeometry/TurnFP64.cs similarity index 82% rename from BasicGeometry/FP64Turns.cs rename to BasicGeometry/TurnFP64.cs index 731f361..5bf15f6 100644 --- a/BasicGeometry/FP64Turns.cs +++ b/BasicGeometry/TurnFP64.cs @@ -6,14 +6,14 @@ namespace BasicGeometry { - public class FP64Turns + public class TurnFP64 { - public static double TurnsToRadians(double turns) + public static double ToRadians(double turns) { - return turns * FP64Radians.TWO_PI; + return turns * RadianFP64.TWO_PI; } - public static double TurnsToDegrees(double turns) + public static double ToDegrees(double turns) { return turns * 360.0; } @@ -22,7 +22,7 @@ namespace BasicGeometry { if (toUnit == AngleUnit.RADIANS) { - return turns * FP64Radians.TWO_PI; + return turns * RadianFP64.TWO_PI; } if (toUnit == AngleUnit.DEGREES) diff --git a/BasicGeometry/UtilityFP32.cs b/BasicGeometry/UtilityFP32.cs new file mode 100644 index 0000000..abc272b --- /dev/null +++ b/BasicGeometry/UtilityFP32.cs @@ -0,0 +1,48 @@ +using System; + +namespace BasicGeometry +{ + public class UtilityFP32 + { + public const float EPSYLON = 4.76837E-7f; + public const float SQUARE_EPSYLON = 2.27373906E-13f; + + public const float EPSYLON_EFFECTIVENESS_LIMIT = 1.0f; + + public const float ONE_THIRD = 0.333333333f; + public const float ONE_SIXTH = 0.166666667f; + public const float ONE_NINETH = 0.111111111f; + + public const float GOLDEN_RATIO_HIGH = 1.618034f; + public const float GOLDEN_RATIO_LOW = 0.618034f; + + public static bool IsZero(float value) + { + return -EPSYLON <= value && value <= EPSYLON; + } + + public static bool IsUnit(float value) + { + return (1.0f - EPSYLON) <= value && value <= (1.0f + EPSYLON); + } + + public static bool IsSqareValueUnit(float square) + { + return (1.0f - 2.0f * EPSYLON) <= square && square <= (1.0f + 2.0f * EPSYLON); + } + + public static bool AreClose(float value1, float value2) + { + float difference = value1 - value2; + float squareValue1 = value1 * value1; + float squareValue2 = value2 * value2; + float squareDifference = difference * difference; + + if (squareValue1 <= EPSYLON_EFFECTIVENESS_LIMIT || squareValue2 <= EPSYLON_EFFECTIVENESS_LIMIT) { + return squareDifference <= SQUARE_EPSYLON; + } + + return squareDifference <= SQUARE_EPSYLON * squareValue1 && squareDifference <= SQUARE_EPSYLON * squareValue2; + } + } +} diff --git a/BasicGeometry/UtilityFP64.cs b/BasicGeometry/UtilityFP64.cs new file mode 100644 index 0000000..a940fb6 --- /dev/null +++ b/BasicGeometry/UtilityFP64.cs @@ -0,0 +1,48 @@ +using System; + +namespace BasicGeometry +{ + public class UtilityFP64 + { + public const double EPSYLON = 4.996003611E-14; + public const double SQUARE_EPSYLON = 2.496005208112504E-27; + + public const double EPSYLON_EFFECTIVENESS_LIMIT = 1.0; + + public const double ONE_THIRD = 0.333333333333333333; + public const double ONE_SIXTH = 0.166666666666666667; + public const double ONE_NINETH = 0.111111111111111111; + + public const double GOLDEN_RATIO_HIGH = 1.61803398874989485; + public const double GOLDEN_RATIO_LOW = 0.61803398874989485; + + public static bool IsZero(double value) + { + return -EPSYLON <= value && value <= EPSYLON; + } + + public static bool IsUnit(double value) + { + return (1.0 - EPSYLON) <= value && value <= (1.0 + EPSYLON); + } + + public static bool IsSqareValueUnit(double square) + { + return (1.0 - 2.0 * EPSYLON) <= square && square <= (1.0 + 2.0 * EPSYLON); + } + + public static bool AreClose(double value1, double value2) + { + double difference = value1 - value2; + double squareValue1 = value1 * value1; + double squareValue2 = value2 * value2; + double squareDifference = difference * difference; + + if (squareValue1 <= EPSYLON_EFFECTIVENESS_LIMIT || squareValue2 <= EPSYLON_EFFECTIVENESS_LIMIT) { + return squareDifference <= SQUARE_EPSYLON; + } + + return squareDifference <= SQUARE_EPSYLON * squareValue1 && squareDifference <= SQUARE_EPSYLON * squareValue2; + } + } +} diff --git a/BasicGeometry/FP32Vector2.cs b/BasicGeometry/Vector2FP32.cs similarity index 62% rename from BasicGeometry/FP32Vector2.cs rename to BasicGeometry/Vector2FP32.cs index 7009986..64f9854 100644 --- a/BasicGeometry/FP32Vector2.cs +++ b/BasicGeometry/Vector2FP32.cs @@ -23,26 +23,26 @@ using System; namespace BasicGeometry { - public struct FP32Vector2 + public struct Vector2FP32 { - public static readonly FP32Vector2 ZERO = new FP32Vector2(0.0f, 0.0f); + public static readonly Vector2FP32 ZERO = new Vector2FP32(0.0f, 0.0f); public float x1 = 0.0f; public float x2 = 0.0f; - public FP32Vector2(float x1, float x2) + public Vector2FP32(float x1, float x2) { this.x1 = x1; this.x2 = x2; } - public FP32Vector2(in FP32Vector2 vector) + public Vector2FP32(in Vector2FP32 vector) { this.x1 = vector.x1; this.x2 = vector.x2; } - public FP32Vector2(in FP64Vector2 vector) + public Vector2FP32(in Vector2FP64 vector) { this.x1 = (float)vector.x1; this.x2 = (float)vector.x2; @@ -62,12 +62,12 @@ namespace BasicGeometry { float squareModule = this.GetSquareModule(); - if (1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + FP32Utility.TWO_EPSYLON) + if (UtilityFP32.IsSqareValueUnit(squareModule)) { return 1; } - if (squareModule <= FP32Utility.SQUARE_EPSYLON) + if (squareModule <= UtilityFP32.SQUARE_EPSYLON) { this.Reset(); return 0; @@ -89,13 +89,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= FP32Utility.SQUARE_EPSYLON; + return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON; } public readonly bool IsUnit() { - float squareModule = this.GetSquareModule(); - return 1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= FP32Utility.EPSYLON; + return UtilityFP32.IsSqareValueUnit(this.GetSquareModule()); } public void Reset() @@ -110,31 +109,31 @@ namespace BasicGeometry this.x2 = x2; } - public void SetValues(in FP32Vector2 vector) + public void SetValues(in Vector2FP32 vector) { this.x1 = vector.x1; this.x2 = vector.x2; } - public void SetValues(in FP64Vector2 vector) + public void SetValues(in Vector2FP64 vector) { this.x1 = (float)vector.x1; this.x2 = (float)vector.x2; } - public void SetReverseOf(in FP32Vector2 vector) + public void SetReverseOf(in Vector2FP32 vector) { this.x1 = -vector.x1; this.x2 = -vector.x2; } - public void SetReverseOf(in FP64Vector2 vector) + public void SetReverseOf(in Vector2FP64 vector) { this.x1 = -(float)vector.x1; this.x2 = -(float)vector.x2; } - public void AppendScaled(FP32Vector2 summand, float scale) + public void AppendScaled(Vector2FP32 summand, float scale) { this.x1 += summand.x1 * scale; this.x2 += summand.x2 * scale; @@ -145,83 +144,83 @@ namespace BasicGeometry return String.Format("SPVector2({0}, {1})", this.x1, this.x2); } - public static void Add(in FP32Vector2 vector1, in FP32Vector2 vector2, out FP32Vector2 sum) + public static void Add(in Vector2FP32 vector1, in Vector2FP32 vector2, out Vector2FP32 sum) { sum.x1 = vector1.x1 + vector2.x1; sum.x2 = vector1.x2 + vector2.x2; } - public static void Subtract(in FP32Vector2 minuend, in FP32Vector2 subtrahend, out FP32Vector2 difference) + public static void Subtract(in Vector2FP32 minuend, in Vector2FP32 subtrahend, out Vector2FP32 difference) { difference.x1 = minuend.x1 - subtrahend.x1; difference.x2 = minuend.x2 - subtrahend.x2; } - public static void Multiply(in FP32Vector2 multiplicand, float multiplier, out FP32Vector2 product) + public static void Multiply(in Vector2FP32 multiplicand, float multiplier, out Vector2FP32 product) { product.x1 = multiplicand.x1 * multiplier; product.x2 = multiplicand.x2 * multiplier; } - public static void Divide(in FP32Vector2 dividend, float divisor, out FP32Vector2 quotient) + public static void Divide(in Vector2FP32 dividend, float divisor, out Vector2FP32 quotient) { Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetMean2(in FP32Vector2 vector1, in FP32Vector2 vector2, out FP32Vector2 result) + public static void GetMean2(in Vector2FP32 vector1, in Vector2FP32 vector2, out Vector2FP32 result) { result.x1 = (vector1.x1 + vector2.x1) * 0.5f; result.x2 = (vector1.x2 + vector2.x2) * 0.5f; } - public static void GetMean3(in FP32Vector2 vector1, in FP32Vector2 vector2, in FP32Vector2 vector3, out FP32Vector2 result) + public static void GetMean3(in Vector2FP32 vector1, in Vector2FP32 vector2, in Vector2FP32 vector3, out Vector2FP32 result) { - result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * FP32Utility.ONE_THIRD; - result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * FP32Utility.ONE_THIRD; + result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD; + result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD; } - public static float GetScalarProduct(in FP32Vector2 vector1, in FP32Vector2 vector2) + public static float GetScalarProduct(in Vector2FP32 vector1, in Vector2FP32 vector2) { return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2; } - public static float GetCrossProduct(in FP32Vector2 vector1, in FP32Vector2 vector2) + public static float GetCrossProduct(in Vector2FP32 vector1, in Vector2FP32 vector2) { return vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1; } - public static float GetAngle(in FP32Vector2 vector1, in FP32Vector2 vector2, AngleUnit unit) + public static float GetAngle(in Vector2FP32 vector1, in Vector2FP32 vector2, AngleUnit unit) { float squareModule1 = vector1.GetSquareModule(); - if (squareModule1 <= FP32Utility.SQUARE_EPSYLON) + if (squareModule1 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } float squareModule2 = vector2.GetSquareModule(); - if (squareModule2 <= FP32Utility.SQUARE_EPSYLON) + if (squareModule2 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float cosine = FP32Vector2.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); + float cosine = Vector2FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); - if (1.0f - FP32Utility.EPSYLON <= cosine) + if (1.0f - UtilityFP32.EPSYLON <= cosine) { return 0.0f; } - if (cosine <= -(1.0f - FP32Utility.EPSYLON)) + if (cosine <= -(1.0f - UtilityFP32.EPSYLON)) { - return FP32Angle.GetHalfCircle(unit); + return AngleFP32.GetHalfCircle(unit); } - return FP32Radians.ToUnits(MathF.Acos(cosine), unit); + return RadianFP32.ToUnits(MathF.Acos(cosine), unit); } - public static float GetSquareDistance(in FP32Vector2 vector1, in FP32Vector2 vector2) + public static float GetSquareDistance(in Vector2FP32 vector1, in Vector2FP32 vector2) { float dx1 = vector1.x1 - vector2.x1; float dx2 = vector1.x2 - vector2.x2; @@ -229,29 +228,29 @@ namespace BasicGeometry return dx1 * dx1 + dx2 * dx2; } - public static float GetDistance(in FP32Vector2 vector1, in FP32Vector2 vector2) + public static float GetDistance(in Vector2FP32 vector1, in Vector2FP32 vector2) { return MathF.Sqrt(GetSquareDistance(vector1, vector2)); } - public static bool AreEqual(in FP32Vector2 vector1, in FP32Vector2 vector2) + public static bool AreEqual(in Vector2FP32 vector1, in Vector2FP32 vector2) { float squareModule1 = vector1.GetSquareModule(); float squareModule2 = vector2.GetSquareModule(); float squareModule3 = GetSquareDistance(vector1, vector2); // 2.0f means dimension amount - if (squareModule1 < FP32Utility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < FP32Utility.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModule1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (2.0f * FP32Utility.SQUARE_EPSYLON); + return squareModule3 < (2.0f * UtilityFP32.SQUARE_EPSYLON); } if (squareModule1 <= squareModule2) { - return squareModule3 <= (2.0f * FP32Utility.SQUARE_EPSYLON) * squareModule2; + return squareModule3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule2; } - return squareModule3 <= (2.0f * FP32Utility.SQUARE_EPSYLON) * squareModule1; + return squareModule3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule1; } } } diff --git a/BasicGeometry/FP64Vector2.cs b/BasicGeometry/Vector2FP64.cs similarity index 62% rename from BasicGeometry/FP64Vector2.cs rename to BasicGeometry/Vector2FP64.cs index 7628ba7..2e40dfe 100644 --- a/BasicGeometry/FP64Vector2.cs +++ b/BasicGeometry/Vector2FP64.cs @@ -23,26 +23,26 @@ using System; namespace BasicGeometry { - public struct FP64Vector2 + public struct Vector2FP64 { - public static readonly FP64Vector2 ZERO = new FP64Vector2(0.0, 0.0); + public static readonly Vector2FP64 ZERO = new Vector2FP64(0.0, 0.0); public double x1 = 0.0; public double x2 = 0.0; - public FP64Vector2(double x1, double x2) + public Vector2FP64(double x1, double x2) { this.x1 = x1; this.x2 = x2; } - public FP64Vector2(in FP64Vector2 vector) + public Vector2FP64(in Vector2FP64 vector) { this.x1 = vector.x1; this.x2 = vector.x2; } - public FP64Vector2(in FP32Vector2 vector) + public Vector2FP64(in Vector2FP32 vector) { this.x1 = vector.x1; this.x2 = vector.x2; @@ -62,12 +62,12 @@ namespace BasicGeometry { double squareModule = this.GetSquareModule(); - if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON) + if (UtilityFP64.IsSqareValueUnit(squareModule)) { return 1; } - if (squareModule <= FP64Utility.SQUARE_EPSYLON) + if (squareModule <= UtilityFP64.SQUARE_EPSYLON) { this.Reset(); return 0; @@ -89,13 +89,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= FP64Utility.SQUARE_EPSYLON; + return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON; } public readonly bool IsUnit() { - double squareModule = this.GetSquareModule(); - return 1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= FP64Utility.EPSYLON; + return UtilityFP64.IsSqareValueUnit(this.GetSquareModule()); } public void Reset() @@ -110,31 +109,31 @@ namespace BasicGeometry this.x2 = x2; } - public void SetValues(in FP64Vector2 vector) + public void SetValues(in Vector2FP64 vector) { this.x1 = vector.x1; this.x2 = vector.x2; } - public void SetValues(in FP32Vector2 vector) + public void SetValues(in Vector2FP32 vector) { this.x1 = vector.x1; this.x2 = vector.x2; } - public void SetReverseOf(in FP64Vector2 vector) + public void SetReverseOf(in Vector2FP64 vector) { this.x1 = -vector.x1; this.x2 = -vector.x2; } - public void SetReverseOf(in FP32Vector2 vector) + public void SetReverseOf(in Vector2FP32 vector) { this.x1 = -vector.x1; this.x2 = -vector.x2; } - public void AppendScaled(FP64Vector2 summand, double scale) + public void AppendScaled(Vector2FP64 summand, double scale) { this.x1 += summand.x1 * scale; this.x2 += summand.x2 * scale; @@ -145,83 +144,83 @@ namespace BasicGeometry return String.Format("DPVector2({0}, {1})", this.x1, this.x2); } - public static void Add(in FP64Vector2 vector1, in FP64Vector2 vector2, out FP64Vector2 sum) + public static void Add(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 sum) { sum.x1 = vector1.x1 + vector2.x1; sum.x2 = vector1.x2 + vector2.x2; } - public static void Subtract(in FP64Vector2 minuend, in FP64Vector2 subtrahend, out FP64Vector2 difference) + public static void Subtract(in Vector2FP64 minuend, in Vector2FP64 subtrahend, out Vector2FP64 difference) { difference.x1 = minuend.x1 - subtrahend.x1; difference.x2 = minuend.x2 - subtrahend.x2; } - public static void Multiply(in FP64Vector2 multiplicand, double multiplier, out FP64Vector2 product) + public static void Multiply(in Vector2FP64 multiplicand, double multiplier, out Vector2FP64 product) { product.x1 = multiplicand.x1 * multiplier; product.x2 = multiplicand.x2 * multiplier; } - public static void Divide(in FP64Vector2 dividend, double divisor, out FP64Vector2 quotient) + public static void Divide(in Vector2FP64 dividend, double divisor, out Vector2FP64 quotient) { Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetMean2(in FP64Vector2 vector1, in FP64Vector2 vector2, out FP64Vector2 result) + public static void GetMean2(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 result) { result.x1 = (vector1.x1 + vector2.x1) * 0.5; result.x2 = (vector1.x2 + vector2.x2) * 0.5; } - public static void GetMean3(in FP64Vector2 vector1, in FP64Vector2 vector2, in FP64Vector2 vector3, out FP64Vector2 result) + public static void GetMean3(in Vector2FP64 vector1, in Vector2FP64 vector2, in Vector2FP64 vector3, out Vector2FP64 result) { - result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * FP64Utility.ONE_THIRD; - result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * FP64Utility.ONE_THIRD; + result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD; + result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD; } - public static double GetScalarProduct(in FP64Vector2 vector1, in FP64Vector2 vector2) + public static double GetScalarProduct(in Vector2FP64 vector1, in Vector2FP64 vector2) { return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2; } - public static double GetCrossProduct(in FP64Vector2 vector1, in FP64Vector2 vector2) + public static double GetCrossProduct(in Vector2FP64 vector1, in Vector2FP64 vector2) { return vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1; } - public static double GetAngle(in FP64Vector2 vector1, in FP64Vector2 vector2, AngleUnit unit) + public static double GetAngle(in Vector2FP64 vector1, in Vector2FP64 vector2, AngleUnit unit) { double squareModule1 = vector1.GetSquareModule(); - if (squareModule1 <= FP64Utility.SQUARE_EPSYLON) + if (squareModule1 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } double squareModule2 = vector2.GetSquareModule(); - if (squareModule2 <= FP64Utility.SQUARE_EPSYLON) + if (squareModule2 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double cosine = FP64Vector2.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); + double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); - if (1.0 - FP64Utility.EPSYLON <= cosine) + if (1.0 - UtilityFP64.EPSYLON <= cosine) { return 0.0; } - if (cosine <= -(1.0 - FP64Utility.EPSYLON)) + if (cosine <= -(1.0 - UtilityFP64.EPSYLON)) { - return FP64Angle.GetHalfCircle(unit); + return AngleFP64.GetHalfCircle(unit); } - return FP64Radians.ToUnits(Math.Acos(cosine), unit); + return RadianFP64.ToUnits(Math.Acos(cosine), unit); } - public static double GetSquareDistance(in FP64Vector2 vector1, in FP64Vector2 vector2) + public static double GetSquareDistance(in Vector2FP64 vector1, in Vector2FP64 vector2) { double dx1 = vector1.x1 - vector2.x1; double dx2 = vector1.x2 - vector2.x2; @@ -229,29 +228,29 @@ namespace BasicGeometry return dx1 * dx1 + dx2 * dx2; } - public static double GetDistance(in FP64Vector2 vector1, in FP64Vector2 vector2) + public static double GetDistance(in Vector2FP64 vector1, in Vector2FP64 vector2) { return Math.Sqrt(GetSquareDistance(vector1, vector2)); } - public static bool AreEqual(in FP64Vector2 vector1, in FP64Vector2 vector2) + public static bool AreEqual(in Vector2FP64 vector1, in Vector2FP64 vector2) { double squareModule1 = vector1.GetSquareModule(); double squareModule2 = vector2.GetSquareModule(); double squareModule3 = GetSquareDistance(vector1, vector2); // 2.0 means dimension amount - if (squareModule1 < FP64Utility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < FP64Utility.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModule1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (2.0 * FP64Utility.SQUARE_EPSYLON); + return squareModule3 < (2.0 * UtilityFP64.SQUARE_EPSYLON); } if (squareModule1 <= squareModule2) { - return squareModule3 <= (2.0 * FP64Utility.SQUARE_EPSYLON) * squareModule2; + return squareModule3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule2; } - return squareModule3 <= (2.0 * FP64Utility.SQUARE_EPSYLON) * squareModule1; + return squareModule3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule1; } } } diff --git a/BasicGeometry/FP32Vector3.cs b/BasicGeometry/Vector3FP32.cs similarity index 66% rename from BasicGeometry/FP32Vector3.cs rename to BasicGeometry/Vector3FP32.cs index 607d047..54d38a0 100644 --- a/BasicGeometry/FP32Vector3.cs +++ b/BasicGeometry/Vector3FP32.cs @@ -23,29 +23,29 @@ using System; namespace BasicGeometry { - public struct FP32Vector3 + public struct Vector3FP32 { - public static readonly FP32Vector3 ZERO = new FP32Vector3(0.0f, 0.0f, 0.0f); + public static readonly Vector3FP32 ZERO = new Vector3FP32(0.0f, 0.0f, 0.0f); public float x1 = 0.0f; public float x2 = 0.0f; public float x3 = 0.0f; - public FP32Vector3(float x1, float x2, float x3) + public Vector3FP32(float x1, float x2, float x3) { this.x1 = x1; this.x2 = x2; this.x3 = x3; } - public FP32Vector3(in FP32Vector3 vector) + public Vector3FP32(in Vector3FP32 vector) { this.x1 = vector.x1; this.x2 = vector.x2; this.x3 = vector.x3; } - public FP32Vector3(in FP64Vector3 vector) + public Vector3FP32(in Vector3FP64 vector) { this.x1 = (float)vector.x1; this.x2 = (float)vector.x2; @@ -66,12 +66,12 @@ namespace BasicGeometry { float squareModule = this.GetSquareModule(); - if (1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + FP32Utility.TWO_EPSYLON) + if (UtilityFP32.IsSqareValueUnit(squareModule)) { return 1; } - if (squareModule <= FP32Utility.SQUARE_EPSYLON) + if (squareModule <= UtilityFP32.SQUARE_EPSYLON) { this.Reset(); return 0; @@ -95,13 +95,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= FP32Utility.SQUARE_EPSYLON; + return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON; } public readonly bool IsUnit() { - float squareModule = this.GetSquareModule(); - return 1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= FP32Utility.EPSYLON; + return UtilityFP32.IsSqareValueUnit(this.GetSquareModule()); } public void Reset() @@ -118,35 +117,35 @@ namespace BasicGeometry this.x3 = x3; } - public void SetValues(in FP64Vector3 vector) + public void SetValues(in Vector3FP64 vector) { this.x1 = (float)vector.x1; this.x2 = (float)vector.x2; this.x3 = (float)vector.x3; } - public void SetValues(in FP32Vector3 vector) + public void SetValues(in Vector3FP32 vector) { this.x1 = vector.x1; this.x2 = vector.x2; this.x3 = vector.x3; } - public void SetReverseOf(in FP32Vector3 vector) + public void SetReverseOf(in Vector3FP32 vector) { this.x1 = -vector.x1; this.x2 = -vector.x2; this.x3 = -vector.x3; } - public void SetReverseOf(in FP64Vector3 vector) + public void SetReverseOf(in Vector3FP64 vector) { this.x1 = -(float)vector.x1; this.x2 = -(float)vector.x2; this.x3 = -(float)vector.x3; } - public void AppendScaled(FP32Vector3 summand, float scale) + public void AppendScaled(Vector3FP32 summand, float scale) { this.x1 += summand.x1 * scale; this.x2 += summand.x2 * scale; @@ -158,52 +157,52 @@ namespace BasicGeometry return String.Format("SPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3); } - public static void Add(in FP32Vector3 vector1, in FP32Vector3 vector2, out FP32Vector3 sum) + public static void Add(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 sum) { sum.x1 = vector1.x1 + vector2.x1; sum.x2 = vector1.x2 + vector2.x2; sum.x3 = vector1.x3 + vector2.x3; } - public static void Subtract(in FP32Vector3 minuend, in FP32Vector3 subtrahend, out FP32Vector3 difference) + public static void Subtract(in Vector3FP32 minuend, in Vector3FP32 subtrahend, out Vector3FP32 difference) { difference.x1 = minuend.x1 - subtrahend.x1; difference.x2 = minuend.x2 - subtrahend.x2; difference.x3 = minuend.x3 - subtrahend.x3; } - public static void Multiply(in FP32Vector3 multiplicand, float multiplier, out FP32Vector3 product) + public static void Multiply(in Vector3FP32 multiplicand, float multiplier, out Vector3FP32 product) { product.x1 = multiplicand.x1 * multiplier; product.x2 = multiplicand.x2 * multiplier; product.x3 = multiplicand.x3 * multiplier; } - public static void Divide(in FP32Vector3 dividend, float divisor, out FP32Vector3 quotient) + public static void Divide(in Vector3FP32 dividend, float divisor, out Vector3FP32 quotient) { Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetMean2(in FP32Vector3 vector1, in FP32Vector3 vector2, out FP32Vector3 result) + public static void GetMean2(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 result) { result.x1 = (vector1.x1 + vector2.x1) * 0.5f; result.x2 = (vector1.x2 + vector2.x2) * 0.5f; result.x3 = (vector1.x3 + vector2.x3) * 0.5f; } - public static void GetMean3(in FP32Vector3 vector1, in FP32Vector3 vector2, in FP32Vector3 vector3, out FP32Vector3 result) + public static void GetMean3(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 result) { - result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * FP32Utility.ONE_THIRD; - result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * FP32Utility.ONE_THIRD; - result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * FP32Utility.ONE_THIRD; + 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; } - public static float GetScalarProduct(in FP32Vector3 vector1, in FP32Vector3 vector2) + 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 FP32Vector3 vector1, in FP32Vector3 vector2, out FP32Vector3 result) + 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; @@ -214,14 +213,14 @@ namespace BasicGeometry result.x3 = x3; } - public static float GetTripleProduct(in FP32Vector3 vector1, in FP32Vector3 vector2, in FP32Vector3 vector3) + public static float GetTripleProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 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 GetDoubleCrossProduct(in FP32Vector3 vector1, in FP32Vector3 vector2, in FP32Vector3 vector3, out FP32Vector3 result) + public static void GetDoubleCrossProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 result) { // [a x [b x c]] = b * (a, c) - c * (a, b) float ac = GetScalarProduct(vector1, vector3); @@ -232,38 +231,38 @@ namespace BasicGeometry result.x3 = ac * vector2.x3 - ab * vector3.x3; } - public static float GetAngle(in FP32Vector3 vector1, in FP32Vector3 vector2, AngleUnit unit) + public static float GetAngle(in Vector3FP32 vector1, in Vector3FP32 vector2, AngleUnit unit) { float squareModule1 = vector1.GetSquareModule(); - if (squareModule1 <= FP32Utility.SQUARE_EPSYLON) + if (squareModule1 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } float squareModule2 = vector2.GetSquareModule(); - if (squareModule2 <= FP32Utility.SQUARE_EPSYLON) + if (squareModule2 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float cosine = FP32Vector3.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); + float cosine = Vector3FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); - if (1.0f - FP32Utility.EPSYLON <= cosine) + if (1.0f - UtilityFP32.EPSYLON <= cosine) { return 0.0f; } - if (cosine <= -(1.0f - FP32Utility.EPSYLON)) + if (cosine <= -(1.0f - UtilityFP32.EPSYLON)) { - return FP32Angle.GetHalfCircle(unit); + return AngleFP32.GetHalfCircle(unit); } - return FP32Radians.ToUnits(MathF.Acos(cosine), unit); + return RadianFP32.ToUnits(MathF.Acos(cosine), unit); } - public static float GetSquareDistance(in FP32Vector3 vector1, in FP32Vector3 vector2) + public static float GetSquareDistance(in Vector3FP32 vector1, in Vector3FP32 vector2) { float dx1 = vector1.x1 - vector2.x1; float dx2 = vector1.x2 - vector2.x2; @@ -272,29 +271,29 @@ namespace BasicGeometry return dx1 * dx1 + dx2 * dx2 + dx3 * dx3; } - public static float GetDistance(in FP32Vector3 vector1, in FP32Vector3 vector2) + public static float GetDistance(in Vector3FP32 vector1, in Vector3FP32 vector2) { return MathF.Sqrt(GetSquareDistance(vector1, vector2)); } - public static bool AreEqual(in FP32Vector3 vector1, in FP32Vector3 vector2) + public static bool AreEqual(in Vector3FP32 vector1, in Vector3FP32 vector2) { float squareModule1 = vector1.GetSquareModule(); float squareModule2 = vector2.GetSquareModule(); float squareModule3 = GetSquareDistance(vector1, vector2); // 3.0f means dimension amount - if (squareModule1 < FP32Utility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < FP32Utility.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModule1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (3.0f * FP32Utility.SQUARE_EPSYLON); + return squareModule3 < (3.0f * UtilityFP32.SQUARE_EPSYLON); } if (squareModule1 <= squareModule2) { - return squareModule3 <= (3.0f * FP32Utility.SQUARE_EPSYLON) * squareModule2; + return squareModule3 <= (3.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule2; } - return squareModule3 <= (3.0f * FP32Utility.SQUARE_EPSYLON) * squareModule1; + return squareModule3 <= (3.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule1; } } } diff --git a/BasicGeometry/FP64Vector3.cs b/BasicGeometry/Vector3FP64.cs similarity index 66% rename from BasicGeometry/FP64Vector3.cs rename to BasicGeometry/Vector3FP64.cs index c1e72d1..1823f86 100644 --- a/BasicGeometry/FP64Vector3.cs +++ b/BasicGeometry/Vector3FP64.cs @@ -23,29 +23,29 @@ using System; namespace BasicGeometry { - public struct FP64Vector3 + public struct Vector3FP64 { - public static readonly FP64Vector3 ZERO = new FP64Vector3(0.0, 0.0, 0.0); + public static readonly Vector3FP64 ZERO = new Vector3FP64(0.0, 0.0, 0.0); public double x1 = 0.0; public double x2 = 0.0; public double x3 = 0.0; - public FP64Vector3(double x1, double x2, double x3) + public Vector3FP64(double x1, double x2, double x3) { this.x1 = x1; this.x2 = x2; this.x3 = x3; } - public FP64Vector3(in FP64Vector3 vector) + public Vector3FP64(in Vector3FP64 vector) { this.x1 = vector.x1; this.x2 = vector.x2; this.x3 = vector.x3; } - public FP64Vector3(in FP32Vector3 vector) + public Vector3FP64(in Vector3FP32 vector) { this.x1 = vector.x1; this.x2 = vector.x2; @@ -66,12 +66,13 @@ namespace BasicGeometry { double squareModule = this.GetSquareModule(); - if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON) + + if (UtilityFP64.IsSqareValueUnit(squareModule)) { return 1; } - if (squareModule <= FP64Utility.SQUARE_EPSYLON) + if (squareModule <= UtilityFP64.SQUARE_EPSYLON) { this.Reset(); return 0; @@ -95,13 +96,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= FP64Utility.SQUARE_EPSYLON; + return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON; } public readonly bool IsUnit() { - double squareModule = this.GetSquareModule(); - return 1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= FP64Utility.EPSYLON; + return UtilityFP64.IsSqareValueUnit(this.GetSquareModule()); } public void Reset() @@ -118,35 +118,35 @@ namespace BasicGeometry this.x3 = x3; } - public void SetValues(in FP64Vector3 vector) + public void SetValues(in Vector3FP64 vector) { this.x1 = vector.x1; this.x2 = vector.x2; this.x3 = vector.x3; } - public void SetValues(in FP32Vector3 vector) + public void SetValues(in Vector3FP32 vector) { this.x1 = vector.x1; this.x2 = vector.x2; this.x3 = vector.x3; } - public void SetReverseOf(in FP64Vector3 vector) + public void SetReverseOf(in Vector3FP64 vector) { this.x1 = -vector.x1; this.x2 = -vector.x2; this.x3 = -vector.x3; } - public void SetReverseOf(in FP32Vector3 vector) + public void SetReverseOf(in Vector3FP32 vector) { this.x1 = -vector.x1; this.x2 = -vector.x2; this.x3 = -vector.x3; } - public void AppendScaled(FP64Vector3 summand, double scale) + public void AppendScaled(Vector3FP64 summand, double scale) { this.x1 += summand.x1 * scale; this.x2 += summand.x2 * scale; @@ -158,52 +158,52 @@ namespace BasicGeometry return String.Format("DPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3); } - public static void Add(in FP64Vector3 vector1, in FP64Vector3 vector2, out FP64Vector3 sum) + public static void Add(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 sum) { sum.x1 = vector1.x1 + vector2.x1; sum.x2 = vector1.x2 + vector2.x2; sum.x3 = vector1.x3 + vector2.x3; } - public static void Subtract(in FP64Vector3 minuend, in FP64Vector3 subtrahend, out FP64Vector3 difference) + public static void Subtract(in Vector3FP64 minuend, in Vector3FP64 subtrahend, out Vector3FP64 difference) { difference.x1 = minuend.x1 - subtrahend.x1; difference.x2 = minuend.x2 - subtrahend.x2; difference.x3 = minuend.x3 - subtrahend.x3; } - public static void Multiply(in FP64Vector3 multiplicand, double multiplier, out FP64Vector3 product) + public static void Multiply(in Vector3FP64 multiplicand, double multiplier, out Vector3FP64 product) { product.x1 = multiplicand.x1 * multiplier; product.x2 = multiplicand.x2 * multiplier; product.x3 = multiplicand.x3 * multiplier; } - public static void Divide(in FP64Vector3 dividend, double divisor, out FP64Vector3 quotient) + public static void Divide(in Vector3FP64 dividend, double divisor, out Vector3FP64 quotient) { Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetMean2(in FP64Vector3 vector1, in FP64Vector3 vector2, out FP64Vector3 result) + public static void GetMean2(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 result) { result.x1 = (vector1.x1 + vector2.x1) * 0.5; result.x2 = (vector1.x2 + vector2.x2) * 0.5; result.x3 = (vector1.x3 + vector2.x3) * 0.5; } - public static void GetMean3(in FP64Vector3 vector1, in FP64Vector3 vector2, in FP64Vector3 vector3, out FP64Vector3 result) + public static void GetMean3(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 result) { - result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * FP64Utility.ONE_THIRD; - result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * FP64Utility.ONE_THIRD; - result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * FP64Utility.ONE_THIRD; + 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; } - public static double GetScalarProduct(in FP64Vector3 vector1, in FP64Vector3 vector2) + 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 void GetCrossProduct(in FP64Vector3 vector1, in FP64Vector3 vector2, out FP64Vector3 result) + public static void GetCrossProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 result) { double x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2; double x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3; @@ -214,14 +214,14 @@ namespace BasicGeometry result.x3 = x3; } - public static double GetTripleProduct(in FP64Vector3 vector1, in FP64Vector3 vector2, in FP64Vector3 vector3) + 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 GetDoubleCrossProduct(in FP64Vector3 vector1, in FP64Vector3 vector2, in FP64Vector3 vector3, out FP64Vector3 result) + 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); @@ -232,38 +232,38 @@ namespace BasicGeometry result.x3 = ac * vector2.x3 - ab * vector3.x3; } - public static double GetAngle(in FP64Vector3 vector1, in FP64Vector3 vector2, AngleUnit unit) + public static double GetAngle(in Vector3FP64 vector1, in Vector3FP64 vector2, AngleUnit unit) { double squareModule1 = vector1.GetSquareModule(); - if (squareModule1 <= FP64Utility.SQUARE_EPSYLON) + if (squareModule1 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } double squareModule2 = vector2.GetSquareModule(); - if (squareModule2 <= FP64Utility.SQUARE_EPSYLON) + if (squareModule2 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double cosine = FP64Vector3.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); + double cosine = Vector3FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); - if (1.0 - FP64Utility.EPSYLON <= cosine) + if (1.0 - UtilityFP64.EPSYLON <= cosine) { return 0.0; } - if (cosine <= -(1.0 - FP64Utility.EPSYLON)) + if (cosine <= -(1.0 - UtilityFP64.EPSYLON)) { - return FP64Angle.GetHalfCircle(unit); + return AngleFP64.GetHalfCircle(unit); } - return FP64Radians.ToUnits(Math.Acos(cosine), unit); + return RadianFP64.ToUnits(Math.Acos(cosine), unit); } - public static double GetSquareDistance(in FP64Vector3 vector1, in FP64Vector3 vector2) + public static double GetSquareDistance(in Vector3FP64 vector1, in Vector3FP64 vector2) { double dx1 = vector1.x1 - vector2.x1; double dx2 = vector1.x2 - vector2.x2; @@ -272,29 +272,29 @@ namespace BasicGeometry return dx1 * dx1 + dx2 * dx2 + dx3 * dx3; } - public static double GetDistance(in FP64Vector3 vector1, in FP64Vector3 vector2) + public static double GetDistance(in Vector3FP64 vector1, in Vector3FP64 vector2) { return Math.Sqrt(GetSquareDistance(vector1, vector2)); } - public static bool AreEqual(in FP64Vector3 vector1, in FP64Vector3 vector2) + public static bool AreEqual(in Vector3FP64 vector1, in Vector3FP64 vector2) { double squareModule1 = vector1.GetSquareModule(); double squareModule2 = vector2.GetSquareModule(); double squareModule3 = GetSquareDistance(vector1, vector2); // 3.0 means dimension amount - if (squareModule1 < FP64Utility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < FP64Utility.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModule1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (3.0 * FP64Utility.SQUARE_EPSYLON); + return squareModule3 < (3.0 * UtilityFP64.SQUARE_EPSYLON); } if (squareModule1 <= squareModule2) { - return squareModule3 <= (3.0 * FP64Utility.SQUARE_EPSYLON) * squareModule2; + return squareModule3 <= (3.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule2; } - return squareModule3 <= (3.0 * FP64Utility.SQUARE_EPSYLON) * squareModule1; + return squareModule3 <= (3.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule1; } } } diff --git a/BasicGeometry/FP32Versor.cs b/BasicGeometry/VersorFP32.cs similarity index 80% rename from BasicGeometry/FP32Versor.cs rename to BasicGeometry/VersorFP32.cs index 9c3007c..30a35bb 100644 --- a/BasicGeometry/FP32Versor.cs +++ b/BasicGeometry/VersorFP32.cs @@ -21,19 +21,19 @@ namespace BasicGeometry { - public struct FP32Versor + public struct VersorFP32 { private float s0 = 1.0f; private float x1 = 0.0f; private float x2 = 0.0f; private float x3 = 0.0f; - public FP32Versor(float s0, float x1, float x2, float x3) + public VersorFP32(float s0, float x1, float x2, float x3) { LoadValues(s0, x1, x2, x3, out this); } - public FP32Versor(in FP32Versor versor) + public VersorFP32(in VersorFP32 versor) { this.s0 = versor.s0; this.x1 = versor.x1; @@ -41,7 +41,7 @@ namespace BasicGeometry this.x3 = versor.x3; } - public FP32Versor(in FP64Versor versor) + public VersorFP32(in VersorFP64 versor) { this.s0 = (float)versor.GetScalar(); this.x1 = (float)versor.GetX1(); @@ -71,7 +71,7 @@ namespace BasicGeometry public readonly bool IsIdle() { - return this.s0 <= -(1.0f - FP32Utility.EPSYLON) || (1.0f - FP32Utility.EPSYLON) <= this.s0; + return this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || (1.0f - UtilityFP32.EPSYLON) <= this.s0; } public void Reset() @@ -101,19 +101,19 @@ namespace BasicGeometry public readonly float GetAngle(AngleUnit unit) { - if (this.s0 <= -(1.0f - FP32Utility.TWO_EPSYLON) || 1.0f - FP32Utility.TWO_EPSYLON <= this.s0) { + if (this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || 1.0f - UtilityFP32.EPSYLON <= this.s0) { return 0.0f; } - if (-FP32Utility.EPSYLON <= this.s0 && this.s0 <= FP32Utility.EPSYLON) + if (UtilityFP32.IsZero(this.s0)) { - return FP32Angle.GetHalfCircle(unit); + return AngleFP32.GetHalfCircle(unit); } - return FP32Radians.ToUnits(2.0f * MathF.Acos(this.s0), unit); + return RadianFP32.ToUnits(2.0f * MathF.Acos(this.s0), unit); } - public readonly void MakeRotationMatrix(out FP32Matrix3x3 matrix) + public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix) { float s0s0 = this.s0 * this.s0; float x1x1 = this.x1 * this.x1; @@ -141,7 +141,7 @@ namespace BasicGeometry matrix.r1c3 = x1x3 + s0x2; } - public readonly void MakeReverseMatrix(out FP32Matrix3x3 matrix) + public readonly void MakeReverseMatrix(out Matrix3x3FP32 matrix) { float s0s0 = this.s0 * this.s0; float x1x1 = this.x1 * this.x1; @@ -178,15 +178,13 @@ namespace BasicGeometry float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - if (1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + FP32Utility.TWO_EPSYLON) + if (!UtilityFP32.IsSqareValueUnit(squareModule)) { - return; + this.Normalize(squareModule); } - - this.Normalize(squareModule); } - public void SetValues(in FP32Versor versor) + public void SetValues(in VersorFP32 versor) { this.s0 = versor.s0; this.x1 = versor.x1; @@ -194,7 +192,7 @@ namespace BasicGeometry this.x3 = versor.x3; } - public void SetValues(in FP64Versor versor) + public void SetValues(in VersorFP64 versor) { this.SetValues( (float) versor.GetScalar(), @@ -203,7 +201,7 @@ namespace BasicGeometry (float) versor.GetX3()); } - public void SetInverted(in FP32Versor versor) + public void SetInverted(in VersorFP32 versor) { this.s0 = versor.s0; this.x1 = -versor.x1; @@ -211,7 +209,7 @@ namespace BasicGeometry this.x3 = -versor.x3; } - public void SetInverted(in FP64Versor versor) + public void SetInverted(in VersorFP64 versor) { this.SetValues( (float)versor.GetScalar(), @@ -220,7 +218,7 @@ namespace BasicGeometry (float)-versor.GetX3()); } - public void SetShortened(in FP32Versor versor) + public void SetShortened(in VersorFP32 versor) { if (versor.s0 < 0.0f) { @@ -238,13 +236,13 @@ namespace BasicGeometry } } - public void SetShortened(in FP64Versor versor) + public void SetShortened(in VersorFP64 versor) { this.SetValues(versor); this.Shorten(); } - public readonly void Turn(in FP32Vector3 vector, out FP32Vector3 result) + public readonly void Turn(in Vector3FP32 vector, out Vector3FP32 result) { float tx1 = 2.0f * (this.x2 * vector.x3 - this.x3 * vector.x2); float tx2 = 2.0f * (this.x3 * vector.x1 - this.x1 * vector.x3); @@ -259,7 +257,7 @@ namespace BasicGeometry result.x3 = x3; } - public readonly void TurnBack(in FP32Vector3 vector, out FP32Vector3 result) + public readonly void TurnBack(in Vector3FP32 vector, out Vector3FP32 result) { float tx1 = 2.0f * (this.x2 * vector.x3 - this.x3 * vector.x2); float tx2 = 2.0f * (this.x3 * vector.x1 - this.x1 * vector.x3); @@ -276,7 +274,7 @@ namespace BasicGeometry private void Normalize(float squareModule) { - if (squareModule <= FP32Utility.SQUARE_EPSYLON) + if (squareModule <= UtilityFP32.SQUARE_EPSYLON) { this.Reset(); return; @@ -290,7 +288,7 @@ namespace BasicGeometry this.x3 *= multiplier; } - public static void Combine(in FP32Versor second, in FP32Versor first, out FP32Versor result) + 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); @@ -304,15 +302,13 @@ namespace BasicGeometry result.x2 = x2; result.x3 = x3; - if (1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + FP32Utility.TWO_EPSYLON) + if (!UtilityFP32.IsSqareValueUnit(squareModule)) { - return; + result.Normalize(squareModule); } - - result.Normalize(squareModule); } - public static void LoadIdle(out FP32Versor versor) + public static void LoadIdle(out VersorFP32 versor) { versor.s0 = 1.0f; versor.x1 = 0.0f; @@ -320,7 +316,7 @@ namespace BasicGeometry versor.x3 = 0.0f; } - public static void LoadValues(float s0, float x1, float x2, float x3, out FP32Versor versor) + public static void LoadValues(float s0, float x1, float x2, float x3, out VersorFP32 versor) { versor.s0 = s0; versor.x1 = x1; @@ -329,12 +325,10 @@ namespace BasicGeometry float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - if (1.0f - FP32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + FP32Utility.TWO_EPSYLON) + if (!UtilityFP32.IsSqareValueUnit(squareModule)) { - return; + versor.Normalize(squareModule); } - - versor.Normalize(squareModule); } } } diff --git a/BasicGeometry/FP64Versor.cs b/BasicGeometry/VersorFP64.cs similarity index 79% rename from BasicGeometry/FP64Versor.cs rename to BasicGeometry/VersorFP64.cs index 4995bec..5026cad 100644 --- a/BasicGeometry/FP64Versor.cs +++ b/BasicGeometry/VersorFP64.cs @@ -22,19 +22,19 @@ namespace BasicGeometry { - public struct FP64Versor + public struct VersorFP64 { private double s0 = 1.0; private double x1 = 0.0; private double x2 = 0.0; private double x3 = 0.0; - public FP64Versor(double s0, double x1, double x2, double x3) + public VersorFP64(double s0, double x1, double x2, double x3) { LoadValues(s0, x1, x2, x3, out this); } - public FP64Versor(in FP64Versor versor) + public VersorFP64(in VersorFP64 versor) { this.s0 = versor.s0; this.x1 = versor.x1; @@ -42,7 +42,7 @@ namespace BasicGeometry this.x3 = versor.x3; } - public FP64Versor(in FP32Versor versor) + public VersorFP64(in VersorFP32 versor) { LoadValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3(), out this); } @@ -69,7 +69,7 @@ namespace BasicGeometry public readonly bool IsIdle() { - return this.s0 <= -(1.0 - FP64Utility.EPSYLON) || (1.0 - FP64Utility.EPSYLON) <= this.s0; + return this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || (1.0 - UtilityFP64.EPSYLON) <= this.s0; } public void Reset() @@ -100,19 +100,19 @@ namespace BasicGeometry public readonly double GetAngle(AngleUnit unit) { - if (this.s0 <= -(1.0 - FP64Utility.TWO_EPSYLON) || 1.0 - FP64Utility.TWO_EPSYLON <= this.s0) { + if (this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || 1.0 - UtilityFP64.EPSYLON <= this.s0) { return 0.0; } - if (-FP64Utility.EPSYLON <= this.s0 && this.s0 <= FP64Utility.EPSYLON) + if (UtilityFP64.IsZero(this.s0)) { - return FP64Angle.GetHalfCircle(unit); + return AngleFP64.GetHalfCircle(unit); } - return FP64Radians.ToUnits(2.0 * Math.Acos(this.s0), unit); + return RadianFP64.ToUnits(2.0 * Math.Acos(this.s0), unit); } - public readonly void MakeRotationMatrix(out FP64Matrix3x3 matrix) + public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix) { double s0s0 = this.s0 * this.s0; double x1x1 = this.x1 * this.x1; @@ -141,7 +141,7 @@ namespace BasicGeometry matrix.r1c3 = x1x3 + s0x2; } - public readonly void MakeReverseMatrix(out FP64Matrix3x3 matrix) + public readonly void MakeReverseMatrix(out Matrix3x3FP64 matrix) { double s0s0 = this.s0 * this.s0; double x1x1 = this.x1 * this.x1; @@ -178,15 +178,13 @@ namespace BasicGeometry double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON) + if (!UtilityFP64.IsSqareValueUnit(squareModule)) { - return; + this.Normalize(squareModule); } - - this.Normalize(squareModule); } - public void SetValues(in FP64Versor versor) + public void SetValues(in VersorFP64 versor) { this.s0 = versor.s0; this.x1 = versor.x1; @@ -194,12 +192,12 @@ namespace BasicGeometry this.x3 = versor.x3; } - public void SetValues(in FP32Versor versor) + public void SetValues(in VersorFP32 versor) { this.SetValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3()); } - public void SetShortened(in FP64Versor versor) + public void SetShortened(in VersorFP64 versor) { if (versor.s0 < 0.0) { @@ -217,13 +215,13 @@ namespace BasicGeometry } } - public void SetShortened(in FP32Versor versor) + public void SetShortened(in VersorFP32 versor) { this.SetValues(versor); this.Shorten(); } - public void SetInverted(in FP64Versor versor) + public void SetInverted(in VersorFP64 versor) { this.s0 = versor.s0; this.x1 = -versor.x1; @@ -231,12 +229,12 @@ namespace BasicGeometry this.x3 = -versor.x3; } - public void SetInverted(in FP32Versor versor) + public void SetInverted(in VersorFP32 versor) { this.SetValues(versor.GetScalar(), -versor.GetX1(), -versor.GetX2(), -versor.GetX3()); } - public readonly void Turn(in FP64Vector3 vector, out FP64Vector3 result) + public readonly void Turn(in Vector3FP64 vector, out Vector3FP64 result) { double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2); double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3); @@ -251,7 +249,7 @@ namespace BasicGeometry result.x3 = x3; } - public readonly void TurnBack(in FP64Vector3 vector, out FP64Vector3 result) + public readonly void TurnBack(in Vector3FP64 vector, out Vector3FP64 result) { double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2); double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3); @@ -268,7 +266,7 @@ namespace BasicGeometry private void Normalize(double squareModule) { - if (squareModule <= FP64Utility.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= FP64Utility.SQUARE_EPSYLON * squareModule) + if (squareModule <= UtilityFP64.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= UtilityFP64.SQUARE_EPSYLON * squareModule) { this.Reset(); return; @@ -282,7 +280,7 @@ namespace BasicGeometry this.x3 *= multiplier; } - public static void Combine(in FP64Versor second, in FP64Versor first, out FP64Versor result) + 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); @@ -296,15 +294,13 @@ namespace BasicGeometry result.x2 = x2; result.x3 = x3; - if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON) + if (!UtilityFP64.IsSqareValueUnit(squareModule)) { - return; + result.Normalize(squareModule); } - - result.Normalize(squareModule); } - public static void LoadIdle(out FP64Versor versor) + public static void LoadIdle(out VersorFP64 versor) { versor.s0 = 1.0; versor.x1 = 0.0; @@ -312,7 +308,7 @@ namespace BasicGeometry versor.x3 = 0.0; } - public static void LoadValues(double s0, double x1, double x2, double x3, out FP64Versor versor) + public static void LoadValues(double s0, double x1, double x2, double x3, out VersorFP64 versor) { versor.s0 = s0; versor.x1 = x1; @@ -321,12 +317,10 @@ namespace BasicGeometry double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON) + if (!UtilityFP64.IsSqareValueUnit(squareModule)) { - return; + versor.Normalize(squareModule); } - - versor.Normalize(squareModule); } } } diff --git a/BasicGeometryDev/Program.cs b/BasicGeometryDev/Program.cs index 4b05e60..14e854a 100644 --- a/BasicGeometryDev/Program.cs +++ b/BasicGeometryDev/Program.cs @@ -6,14 +6,14 @@ using BasicGeometry; public static class Program { - private static FP32Versor[] AllocateVersors(int amount) + private static VersorFP32[] AllocateVersors(int amount) { - return new FP32Versor[amount]; + return new VersorFP32[amount]; } - private static FP32Versor[] MakeZeroVersors(int amount) + private static VersorFP32[] MakeZeroVersors(int amount) { - FP32Versor[] versors = AllocateVersors(amount); + VersorFP32[] versors = AllocateVersors(amount); for (int i = 0; i < amount; i++) { @@ -23,15 +23,15 @@ public static class Program return versors; } - private static FP32Versor[] MakeRandomVersors(int amount) + private static VersorFP32[] MakeRandomVersors(int amount) { Random randomizer = new Random(Environment.TickCount); - FP32Versor[] versors = AllocateVersors(amount); + VersorFP32[] versors = AllocateVersors(amount); for (int i = 0; i < amount; i++) { - versors[i] = new FP32Versor( + versors[i] = new VersorFP32( randomizer.NextSingle(), randomizer.NextSingle(), randomizer.NextSingle(), @@ -42,7 +42,7 @@ public static class Program return versors; } - private static void PrintVersor(in FP32Versor versor) + private static void PrintVersor(in VersorFP32 versor) { Console.WriteLine("({0}, {1}, {2}, {3})", versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3()); } @@ -51,9 +51,9 @@ public static class Program { int amount = 1000000; - FP32Versor[] versors1 = MakeRandomVersors(amount); - FP32Versor[] versors2 = MakeRandomVersors(amount); - FP32Versor[] results = MakeZeroVersors(amount); + VersorFP32[] versors1 = MakeRandomVersors(amount); + VersorFP32[] versors2 = MakeRandomVersors(amount); + VersorFP32[] results = MakeZeroVersors(amount); long start, end; @@ -63,7 +63,7 @@ public static class Program { for (int i = 0; i < amount; i++) { - FP32Versor.Combine(versors1[i], versors2[i], out results[i]); + VersorFP32.Combine(versors1[i], versors2[i], out results[i]); } } diff --git a/BasicGeometryTest/F32Vector2Test.cs b/BasicGeometryTest/F32Vector2Test.cs deleted file mode 100644 index 96ab1c9..0000000 --- a/BasicGeometryTest/F32Vector2Test.cs +++ /dev/null @@ -1,19 +0,0 @@ -using Microsoft.VisualStudio.TestTools.UnitTesting; - -using System; -using System.IO; - -using BasicGeometry; - -namespace BasicGeometryTest -{ - [TestClass] - public class FP32Vector2Test - { - [TestMethod] - public void TestInitialization() - { - FP32Vector2 vector = new FP32Vector2(1.0f, 2.0f); - } - } -} diff --git a/BasicGeometryTest/TestVector2FP32.cs b/BasicGeometryTest/TestVector2FP32.cs new file mode 100644 index 0000000..aa1f48e --- /dev/null +++ b/BasicGeometryTest/TestVector2FP32.cs @@ -0,0 +1,27 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; + +using System; +using System.IO; + +using BasicGeometry; + +namespace BasicGeometryTest +{ + [TestClass] + public class TestVector2FP32 + { + [TestMethod] + public void TestReset() + { + Vector2FP32 vector = new Vector2FP32(1.0f, 2.0f); + + Assert.AreEqual(vector.x1, 1.0f); + Assert.AreEqual(vector.x2, 2.0f); + + vector.Reset(); + + Assert.AreEqual(vector.x1, 0.0f); + Assert.AreEqual(vector.x2, 0.0f); + } + } +} From a3c9e93270939b318a0bdbcd5505e161c5daf048 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Wed, 12 Feb 2025 20:51:48 +0700 Subject: [PATCH 6/9] =?UTF-8?q?=D0=91=D0=BE=D0=BB=D1=8C=D1=88=D0=BE=D0=B5?= =?UTF-8?q?=20=D0=BF=D0=B5=D1=80=D0=B5=D1=83=D0=BF=D0=BE=D1=80=D1=8F=D0=B4?= =?UTF-8?q?=D0=BE=D1=87=D0=B8=D0=B2=D0=B0=D0=BD=D0=B8=D0=B5=20=D0=B8=D1=81?= =?UTF-8?q?=D1=85=D0=BE=D0=B4=D0=BD=D0=BE=D0=B3=D0=BE=20=D0=BA=D0=BE=D0=B4?= =?UTF-8?q?=D0=B0=20(=D0=BD=D0=B5=20=D0=BE=D0=BA=D0=BE=D0=BD=D1=87=D0=B5?= =?UTF-8?q?=D0=BD=D0=BE)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/Matrix2x2FP32.cs | 9 +- BasicGeometry/Matrix2x2FP64.cs | 11 +- BasicGeometry/Matrix3x3FP32.cs | 161 +++++++-------- BasicGeometry/Matrix3x3FP64.cs | 157 ++++++++------- BasicGeometry/QuaternionFP32.cs | 66 ++++--- BasicGeometry/QuaternionFP64.cs | 69 ++++--- BasicGeometry/UtilityFP32.cs | 2 +- BasicGeometry/UtilityFP64.cs | 2 +- BasicGeometry/Vector2FP32.cs | 44 ++--- BasicGeometry/Vector2FP64.cs | 44 ++--- BasicGeometry/Vector3FP32.cs | 45 ++--- BasicGeometry/Vector3FP64.cs | 46 ++--- BasicGeometry/VersorFP32.cs | 338 ++++++++++++++++---------------- BasicGeometry/VersorFP64.cs | 334 +++++++++++++++---------------- BasicGeometryDev/Program.cs | 12 +- 15 files changed, 694 insertions(+), 646 deletions(-) diff --git a/BasicGeometry/Matrix2x2FP32.cs b/BasicGeometry/Matrix2x2FP32.cs index 10c520b..936471c 100644 --- a/BasicGeometry/Matrix2x2FP32.cs +++ b/BasicGeometry/Matrix2x2FP32.cs @@ -59,8 +59,7 @@ namespace BasicGeometry public readonly bool IsSingular() { - float determinant = this.GetDeterminant(); - return -UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON; + return UtilityFP32.IsZero(this.GetDeterminant()); } public void Transpose() @@ -72,7 +71,7 @@ namespace BasicGeometry { float determinant = this.GetDeterminant(); - if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) + if (UtilityFP32.IsZero(determinant)) { return false; } @@ -121,7 +120,7 @@ namespace BasicGeometry this.r2c2 = d2; } - public void SetValues(in Matrix2x2FP32 matrix) + public void Set(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -130,7 +129,7 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public void SetValues(in Matrix2x2FP64 matrix) + public void Set(in Matrix2x2FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r1c2; diff --git a/BasicGeometry/Matrix2x2FP64.cs b/BasicGeometry/Matrix2x2FP64.cs index 11f20d0..4618711 100644 --- a/BasicGeometry/Matrix2x2FP64.cs +++ b/BasicGeometry/Matrix2x2FP64.cs @@ -59,8 +59,7 @@ namespace BasicGeometry public readonly bool IsSingular() { - double determinant = this.GetDeterminant(); - return -UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON; + return UtilityFP64.IsZero(this.GetDeterminant()); } public void Transpose() @@ -72,7 +71,7 @@ namespace BasicGeometry { double determinant = this.GetDeterminant(); - if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) + if (UtilityFP64.IsZero(determinant)) { return false; } @@ -121,7 +120,7 @@ namespace BasicGeometry this.r2c2 = d2; } - public void SetValues(in Matrix2x2FP64 matrix) + public void Set(in Matrix2x2FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -130,7 +129,7 @@ namespace BasicGeometry this.r2c2 = matrix.r2c2; } - public void SetValues(in Matrix2x2FP32 matrix) + public void Set(in Matrix2x2FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -159,7 +158,7 @@ namespace BasicGeometry { double determinant = matrix.GetDeterminant(); - if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) + if (UtilityFP64.IsZero(determinant)) { return false; } diff --git a/BasicGeometry/Matrix3x3FP32.cs b/BasicGeometry/Matrix3x3FP32.cs index 665c900..8201ea3 100644 --- a/BasicGeometry/Matrix3x3FP32.cs +++ b/BasicGeometry/Matrix3x3FP32.cs @@ -75,8 +75,7 @@ namespace BasicGeometry public readonly bool IsSingular() { - float determinant = this.GetDeterminant(); - return -UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON; + return UtilityFP32.IsZero(this.GetDeterminant()); } public void Transpose() @@ -90,7 +89,8 @@ namespace BasicGeometry { float determinant = this.GetDeterminant(); - if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) { + if (UtilityFP32.IsZero(determinant)) + { return false; } @@ -168,7 +168,7 @@ namespace BasicGeometry this.r2c3 = d3; } - public void SetValues(in Matrix3x3FP32 matrix) + public void Set(in Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -183,7 +183,7 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public void SetValues(in Matrix3x3FP64 matrix) + public void Set(in Matrix3x3FP64 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r1c2; @@ -198,69 +198,6 @@ namespace BasicGeometry this.r3c3 = (float)matrix.r3c3; } - public void SetTransposedOf(in Matrix3x3FP32 matrix) - { - this.r1c1 = matrix.r1c1; - this.r2c2 = matrix.r2c2; - this.r3c3 = matrix.r3c3; - - (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); - (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); - (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); - } - - public void SetTransposedOf(in Matrix3x3FP64 matrix) - { - this.r1c1 = (float)matrix.r1c1; - this.r1c2 = (float)matrix.r2c1; - this.r1c3 = (float)matrix.r3c1; - - this.r2c1 = (float)matrix.r1c2; - this.r2c2 = (float)matrix.r2c2; - this.r2c3 = (float)matrix.r3c2; - - this.r3c1 = (float)matrix.r1c3; - this.r3c2 = (float)matrix.r2c3; - this.r3c3 = (float)matrix.r3c3; - } - - public bool SetInvertedOf(in Matrix3x3FP32 matrix) - { - float determinant = matrix.GetDeterminant(); - - if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) { - return false; - } - - float r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; - float r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; - float r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; - - float r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; - float r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; - float r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; - - float r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; - float r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; - float r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; - - float mutiplier = 1.0f / determinant; - - this.r1c1 = r1c1 * mutiplier; - this.r1c2 = r1c2 * mutiplier; - this.r1c3 = r1c3 * mutiplier; - - this.r2c1 = r2c1 * mutiplier; - this.r2c2 = r2c2 * mutiplier; - this.r2c3 = r2c3 * mutiplier; - - this.r3c1 = r3c1 * mutiplier; - this.r3c2 = r3c2 * mutiplier; - this.r3c3 = r3c3 * mutiplier; - - return true; - } - public void SetRow1(float c1, float c2, float c3) { this.r1c1 = c1; @@ -303,19 +240,53 @@ namespace BasicGeometry this.r3c3 = r3; } - public void AppendScaled(in Matrix3x3FP32 matrix, float scale) + public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed) { - this.r1c1 += matrix.r1c1* scale; - this.r1c2 += matrix.r1c2* scale; - this.r1c3 += matrix.r1c3* scale; + transposed.r1c1 = matrix.r1c1; + transposed.r2c2 = matrix.r2c2; + transposed.r3c3 = matrix.r3c3; - this.r2c1 += matrix.r2c1* scale; - this.r2c2 += matrix.r2c2* scale; - this.r2c3 += matrix.r2c3* scale; + (transposed.r1c2, transposed.r2c1) = (matrix.r2c1, matrix.r1c2); + (transposed.r1c3, transposed.r3c1) = (matrix.r3c1, matrix.r1c3); + (transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3); + } - this.r3c1 += matrix.r3c1* scale; - this.r3c2 += matrix.r3c2* scale; - this.r3c3 += matrix.r3c3* scale; + public static bool MakeInverted(in Matrix3x3FP32 matrix, out Matrix3x3FP32 inverted) + { + float determinant = matrix.GetDeterminant(); + + if (UtilityFP32.IsZero(determinant)) { + LoadZero(out inverted); + return false; + } + + float r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; + float r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; + float r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; + + float r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; + float r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; + float r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; + + float r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; + float r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; + float r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; + + float mutiplier = 1.0f / determinant; + + inverted.r1c1 = r1c1 * mutiplier; + inverted.r1c2 = r1c2 * mutiplier; + inverted.r1c3 = r1c3 * mutiplier; + + inverted.r2c1 = r2c1 * mutiplier; + inverted.r2c2 = r2c2 * mutiplier; + inverted.r2c3 = r2c3 * mutiplier; + + inverted.r3c1 = r3c1 * mutiplier; + inverted.r3c2 = r3c2 * mutiplier; + inverted.r3c3 = r3c3 * mutiplier; + + return true; } public static void Add(in Matrix3x3FP32 matrix1, in Matrix3x3FP32 matrix2, out Matrix3x3FP32 sum) @@ -333,6 +304,21 @@ namespace BasicGeometry sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; } + public static void AddScaled(in Matrix3x3FP32 basicMatrix, in Matrix3x3FP32 scalableMatrix, float scale, out Matrix3x3FP32 sum) + { + sum.r1c1 = basicMatrix.r1c1 + scalableMatrix.r1c1 * scale; + sum.r1c2 = basicMatrix.r1c2 + scalableMatrix.r1c2 * scale; + sum.r1c3 = basicMatrix.r1c3 + scalableMatrix.r1c3 * scale; + + sum.r2c1 = basicMatrix.r2c1 + scalableMatrix.r2c1 * scale; + sum.r2c2 = basicMatrix.r2c2 + scalableMatrix.r2c2 * scale; + sum.r2c3 = basicMatrix.r2c3 + scalableMatrix.r2c3 * scale; + + sum.r3c1 = basicMatrix.r3c1 + scalableMatrix.r3c1 * scale; + sum.r3c2 = basicMatrix.r3c2 + scalableMatrix.r3c2 * scale; + sum.r3c3 = basicMatrix.r3c3 + scalableMatrix.r3c3 * scale; + } + public static void Subtract(in Matrix3x3FP32 minuend, in Matrix3x3FP32 subtrahend, out Matrix3x3FP32 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; @@ -348,6 +334,21 @@ namespace BasicGeometry difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; } + public static void SubtractScaled(in Matrix3x3FP32 basicMatrix, in Matrix3x3FP32 scalableMatrix, float scale, out Matrix3x3FP32 difference) + { + difference.r1c1 = basicMatrix.r1c1 - scalableMatrix.r1c1 * scale; + difference.r1c2 = basicMatrix.r1c2 - scalableMatrix.r1c2 * scale; + difference.r1c3 = basicMatrix.r1c3 - scalableMatrix.r1c3 * scale; + + difference.r2c1 = basicMatrix.r2c1 - scalableMatrix.r2c1 * scale; + difference.r2c2 = basicMatrix.r2c2 - scalableMatrix.r2c2 * scale; + difference.r2c3 = basicMatrix.r2c3 - scalableMatrix.r2c3 * scale; + + difference.r3c1 = basicMatrix.r3c1 - scalableMatrix.r3c1 * scale; + difference.r3c2 = basicMatrix.r3c2 - scalableMatrix.r3c2 * scale; + difference.r3c3 = basicMatrix.r3c3 - scalableMatrix.r3c3 * scale; + } + public static void Multiply(in Matrix3x3FP32 multiplicand, float multiplier, out Matrix3x3FP32 product) { product.r1c1 = multiplicand.r1c1 * multiplier; @@ -368,7 +369,7 @@ namespace BasicGeometry Multiply(dividend, 1.0f / divisor, out quotient); } - public static void GetRightProduct(in Matrix3x3FP32 matrix, in Vector3FP32 vector, out Vector3FP32 result) + public static void RightProduct(in Matrix3x3FP32 matrix, in Vector3FP32 vector, out Vector3FP32 result) { float x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; float x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3; @@ -379,7 +380,7 @@ namespace BasicGeometry result.x3 = x3; } - public static void GetLeftProduct(in Vector3FP32 vector, in Matrix3x3FP32 matrix, out Vector3FP32 result) + public static void LeftProduct(in Vector3FP32 vector, in Matrix3x3FP32 matrix, out Vector3FP32 result) { float x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1; float x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2; diff --git a/BasicGeometry/Matrix3x3FP64.cs b/BasicGeometry/Matrix3x3FP64.cs index 434cd85..b9b016e 100644 --- a/BasicGeometry/Matrix3x3FP64.cs +++ b/BasicGeometry/Matrix3x3FP64.cs @@ -75,8 +75,7 @@ namespace BasicGeometry public readonly bool IsSingular() { - double determinant = this.GetDeterminant(); - return -UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON; + return UtilityFP64.IsZero(this.GetDeterminant()); } public void Transpose() @@ -90,7 +89,8 @@ namespace BasicGeometry { double determinant = this.GetDeterminant(); - if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) { + if (UtilityFP64.IsZero(determinant)) + { return false; } @@ -168,7 +168,7 @@ namespace BasicGeometry this.r2c3 = d3; } - public void SetValues(Matrix3x3FP64 matrix) + public void Set(Matrix3x3FP64 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -183,7 +183,7 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public void SetValues(Matrix3x3FP32 matrix) + public void Set(Matrix3x3FP32 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r1c2; @@ -198,65 +198,6 @@ namespace BasicGeometry this.r3c3 = matrix.r3c3; } - public void SetTransposedOf(in Matrix3x3FP64 matrix) - { - this.r1c1 = matrix.r1c1; - this.r2c2 = matrix.r2c2; - this.r3c3 = matrix.r3c3; - - (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); - (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); - (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); - } - - public void SetTransposedOf(in Matrix3x3FP32 matrix) - { - this.r1c1 = matrix.r1c1; - this.r2c2 = matrix.r2c2; - this.r3c3 = matrix.r3c3; - - (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); - (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); - (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); - } - - public bool SetInvertedOf(in Matrix3x3FP64 matrix) - { - double determinant = matrix.GetDeterminant(); - - if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) { - return false; - } - - double r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; - double r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; - double r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; - - double r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; - double r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; - double r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; - - double r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; - double r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; - double r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; - - double mutiplier = 1.0 / determinant; - - this.r1c1 = r1c1 * mutiplier; - this.r1c2 = r1c2 * mutiplier; - this.r1c3 = r1c3 * mutiplier; - - this.r2c1 = r2c1 * mutiplier; - this.r2c2 = r2c2 * mutiplier; - this.r2c3 = r2c3 * mutiplier; - - this.r3c1 = r3c1 * mutiplier; - this.r3c2 = r3c2 * mutiplier; - this.r3c3 = r3c3 * mutiplier; - - return true; - } - public void SetRow1(double c1, double c2, double c3) { this.r1c1 = c1; @@ -299,19 +240,53 @@ namespace BasicGeometry this.r3c3 = r3; } - public void AppendScaled(in Matrix3x3FP64 matrix, double scale) + public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed) { - this.r1c1 += matrix.r1c1 * scale; - this.r1c2 += matrix.r1c2 * scale; - this.r1c3 += matrix.r1c3 * scale; + transposed.r1c1 = matrix.r1c1; + transposed.r2c2 = matrix.r2c2; + transposed.r3c3 = matrix.r3c3; - this.r2c1 += matrix.r2c1 * scale; - this.r2c2 += matrix.r2c2 * scale; - this.r2c3 += matrix.r2c3 * scale; + (transposed.r1c2, transposed.r2c1) = (matrix.r2c1, matrix.r1c2); + (transposed.r1c3, transposed.r3c1) = (matrix.r3c1, matrix.r1c3); + (transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3); + } - this.r3c1 += matrix.r3c1 * scale; - this.r3c2 += matrix.r3c2 * scale; - this.r3c3 += matrix.r3c3 * scale; + public static bool MakeInverted(in Matrix3x3FP64 matrix, out Matrix3x3FP64 inverted) + { + double determinant = matrix.GetDeterminant(); + + if (UtilityFP64.IsZero(determinant)) { + LoadZero(out inverted); + return false; + } + + double r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; + double r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; + double r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; + + double r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; + double r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; + double r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; + + double r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; + double r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; + double r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; + + double mutiplier = 1.0 / determinant; + + inverted.r1c1 = r1c1 * mutiplier; + inverted.r1c2 = r1c2 * mutiplier; + inverted.r1c3 = r1c3 * mutiplier; + + inverted.r2c1 = r2c1 * mutiplier; + inverted.r2c2 = r2c2 * mutiplier; + inverted.r2c3 = r2c3 * mutiplier; + + inverted.r3c1 = r3c1 * mutiplier; + inverted.r3c2 = r3c2 * mutiplier; + inverted.r3c3 = r3c3 * mutiplier; + + return true; } public static void Add(in Matrix3x3FP64 matrix1, in Matrix3x3FP64 matrix2, out Matrix3x3FP64 sum) @@ -329,6 +304,21 @@ namespace BasicGeometry sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; } + public static void AddScaled(in Matrix3x3FP64 basicMatrix, in Matrix3x3FP64 scalableMatrix, double scale, out Matrix3x3FP64 sum) + { + sum.r1c1 = basicMatrix.r1c1 + scalableMatrix.r1c1 * scale; + sum.r1c2 = basicMatrix.r1c2 + scalableMatrix.r1c2 * scale; + sum.r1c3 = basicMatrix.r1c3 + scalableMatrix.r1c3 * scale; + + sum.r2c1 = basicMatrix.r2c1 + scalableMatrix.r2c1 * scale; + sum.r2c2 = basicMatrix.r2c2 + scalableMatrix.r2c2 * scale; + sum.r2c3 = basicMatrix.r2c3 + scalableMatrix.r2c3 * scale; + + sum.r3c1 = basicMatrix.r3c1 + scalableMatrix.r3c1 * scale; + sum.r3c2 = basicMatrix.r3c2 + scalableMatrix.r3c2 * scale; + sum.r3c3 = basicMatrix.r3c3 + scalableMatrix.r3c3 * scale; + } + public static void Subtract(in Matrix3x3FP64 minuend, in Matrix3x3FP64 subtrahend, out Matrix3x3FP64 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; @@ -344,6 +334,21 @@ namespace BasicGeometry difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; } + public static void SubtractScaled(in Matrix3x3FP64 basicMatrix, in Matrix3x3FP64 scalableMatrix, double scale, out Matrix3x3FP64 difference) + { + difference.r1c1 = basicMatrix.r1c1 - scalableMatrix.r1c1 * scale; + difference.r1c2 = basicMatrix.r1c2 - scalableMatrix.r1c2 * scale; + difference.r1c3 = basicMatrix.r1c3 - scalableMatrix.r1c3 * scale; + + difference.r2c1 = basicMatrix.r2c1 - scalableMatrix.r2c1 * scale; + difference.r2c2 = basicMatrix.r2c2 - scalableMatrix.r2c2 * scale; + difference.r2c3 = basicMatrix.r2c3 - scalableMatrix.r2c3 * scale; + + difference.r3c1 = basicMatrix.r3c1 - scalableMatrix.r3c1 * scale; + difference.r3c2 = basicMatrix.r3c2 - scalableMatrix.r3c2 * scale; + difference.r3c3 = basicMatrix.r3c3 - scalableMatrix.r3c3 * scale; + } + public static void Multiply(in Matrix3x3FP64 multiplicand, double multiplier, out Matrix3x3FP64 product) { product.r1c1 = multiplicand.r1c1 * multiplier; @@ -364,7 +369,7 @@ namespace BasicGeometry Multiply(dividend, 1.0 / divisor, out quotient); } - public static void GetRightProduct(in Matrix3x3FP64 matrix, in Vector3FP64 vector, out Vector3FP64 result) + public static void RightProduct(in Matrix3x3FP64 matrix, in Vector3FP64 vector, out Vector3FP64 result) { double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; double x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3; @@ -375,7 +380,7 @@ namespace BasicGeometry result.x3 = x3; } - public static void GetLeftProduct(in Vector3FP64 vector, in Matrix3x3FP64 matrix, out Vector3FP64 result) + public static void LeftProduct(in Vector3FP64 vector, in Matrix3x3FP64 matrix, out Vector3FP64 result) { double x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1; double x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2; diff --git a/BasicGeometry/QuaternionFP32.cs b/BasicGeometry/QuaternionFP32.cs index 4d16ec3..d4b9232 100644 --- a/BasicGeometry/QuaternionFP32.cs +++ b/BasicGeometry/QuaternionFP32.cs @@ -30,24 +30,24 @@ namespace BasicGeometry this.x3 = (float)quaternion.x3; } - public readonly float GetSquareModule() + public readonly float GetSquareModulus() { return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); } - public readonly float GetModule() + public readonly float GetModulus() { - return MathF.Sqrt(this.GetSquareModule()); + return MathF.Sqrt(this.GetSquareModulus()); } public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP32.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP32.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -81,7 +81,7 @@ namespace BasicGeometry this.x3 = x3; } - public void SetValues(in QuaternionFP32 quaternion) + public void Set(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -97,12 +97,12 @@ namespace BasicGeometry this.x3 = (float)quaternion.x3; } - public void SetConjugateOf(in QuaternionFP32 quaternion) + public static void MakeConjugate(in QuaternionFP32 quaternion, out QuaternionFP32 conjugate) { - this.s0 = quaternion.s0; - this.x1 = -quaternion.x1; - this.x2 = -quaternion.x2; - this.x3 = -quaternion.x3; + conjugate.s0 = quaternion.s0; + conjugate.x1 = -quaternion.x1; + conjugate.x2 = -quaternion.x2; + conjugate.x3 = -quaternion.x3; } public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix) @@ -112,16 +112,15 @@ namespace BasicGeometry float x2x2 = this.x2 * this.x2; float x3x3 = this.x3 * this.x3; - float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) { Matrix3x3FP32.LoadIdentity(out matrix); return; } - float corrector1 = 1.0f / squareModule; - float corrector2 = 2.0f * corrector1; + float corrector1 = 1.0f / squareModulus; float s0x1 = this.s0 * this.x1; float s0x2 = this.s0 * this.x2; @@ -130,6 +129,8 @@ namespace BasicGeometry float x1x3 = this.x1 * this.x3; float x2x3 = this.x2 * this.x3; + float corrector2 = 2.0f * corrector1; + matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3)); matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3)); matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2)); @@ -150,16 +151,15 @@ namespace BasicGeometry float x2x2 = this.x2 * this.x2; float x3x3 = this.x3 * this.x3; - float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) { Matrix3x3FP32.LoadIdentity(out matrix); return; } - float corrector1 = 1.0f / squareModule; - float corrector2 = 2.0f * corrector1; + float corrector1 = 1.0f / squareModulus; float s0x1 = this.s0 * this.x1; float s0x2 = this.s0 * this.x2; @@ -168,6 +168,8 @@ namespace BasicGeometry float x1x3 = this.x1 * this.x3; float x2x3 = this.x2 * this.x3; + float corrector2 = 2.0f * corrector1; + matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3)); matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3)); matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2)); @@ -199,15 +201,33 @@ namespace BasicGeometry public static void Multiply(in QuaternionFP32 left, in QuaternionFP32 right, out QuaternionFP32 product) { - float s0 = (left.s0 * right.s0 - left.x1 * right.x1) - (left.x2 * right.x2 + left.x3 * right.x3); - float x1 = (left.x1 * right.s0 + left.s0 * right.x1) - (left.x3 * right.x2 - left.x2 * right.x3); - float x2 = (left.x2 * right.s0 + left.s0 * right.x2) - (left.x1 * right.x3 - left.x3 * right.x1); - float x3 = (left.x3 * right.s0 + left.s0 * right.x3) - (left.x2 * right.x1 - left.x1 * right.x2); + float s0 = left.s0 * right.s0 - left.x1 * right.x1 - (left.x2 * right.x2 + left.x3 * right.x3); + float x1 = left.x1 * right.s0 + left.s0 * right.x1 - (left.x3 * right.x2 - left.x2 * right.x3); + float x2 = left.x2 * right.s0 + left.s0 * right.x2 - (left.x1 * right.x3 - left.x3 * right.x1); + float x3 = left.x3 * right.s0 + left.s0 * right.x3 - (left.x2 * right.x1 - left.x1 * right.x2); product.s0 = s0; product.x1 = x1; product.x2 = x2; product.x3 = x3; } + + public static bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2) + { + float ds0 = quaternion1.s0 - quaternion2.s0; + float dx1 = quaternion1.x1 - quaternion2.x1; + float dx2 = quaternion1.x2 - quaternion2.x2; + float dx3 = quaternion1.x3 - quaternion2.x3; + + float squareModulus1 = quaternion1.GetSquareModulus(); + float squareModulus2 = quaternion2.GetSquareModulus(); + float squareDistance = (ds0 * ds0 + dx1 * dx1) + (dx2 * dx2 + dx3 * dx3); + + if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) { + return squareDistance <= UtilityFP32.SQUARE_EPSYLON; + } + + return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2; + } } } diff --git a/BasicGeometry/QuaternionFP64.cs b/BasicGeometry/QuaternionFP64.cs index 13f5dd6..ba260a8 100644 --- a/BasicGeometry/QuaternionFP64.cs +++ b/BasicGeometry/QuaternionFP64.cs @@ -1,5 +1,4 @@ using System; -using System.Numerics; namespace BasicGeometry { @@ -31,24 +30,24 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public readonly double GetSquareModule() + public readonly double GetSquareModulus() { return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); } - public readonly double GetModule() + public readonly double GetModulus() { - return Math.Sqrt(this.GetSquareModule()); + return Math.Sqrt(this.GetSquareModulus()); } public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP64.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP64.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -82,7 +81,7 @@ namespace BasicGeometry this.x3 = x3; } - public void SetValues(in QuaternionFP32 quaternion) + public void Set(in QuaternionFP32 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -90,7 +89,7 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public void SetValues(in QuaternionFP64 quaternion) + public void Set(in QuaternionFP64 quaternion) { this.s0 = quaternion.s0; this.x1 = quaternion.x1; @@ -98,12 +97,12 @@ namespace BasicGeometry this.x3 = quaternion.x3; } - public void SetConjugateOf(in QuaternionFP64 quaternion) + public static void MakeConjugate(in QuaternionFP64 quaternion, out QuaternionFP64 conjugate) { - this.s0 = quaternion.s0; - this.x1 = -quaternion.x1; - this.x2 = -quaternion.x2; - this.x3 = -quaternion.x3; + conjugate.s0 = quaternion.s0; + conjugate.x1 = -quaternion.x1; + conjugate.x2 = -quaternion.x2; + conjugate.x3 = -quaternion.x3; } public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix) @@ -113,16 +112,15 @@ namespace BasicGeometry double x2x2 = this.x2 * this.x2; double x3x3 = this.x3 * this.x3; - double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - if (squareModule <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus)) { Matrix3x3FP64.LoadIdentity(out matrix); return; } - double corrector1 = 1.0 / squareModule; - double corrector2 = 2.0 * corrector1; + double corrector1 = 1.0 / squareModulus; double s0x1 = this.s0 * this.x1; double s0x2 = this.s0 * this.x2; @@ -131,6 +129,8 @@ namespace BasicGeometry double x1x3 = this.x1 * this.x3; double x2x3 = this.x2 * this.x3; + double corrector2 = 2.0 * corrector1; + matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3)); matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3)); matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2)); @@ -151,16 +151,15 @@ namespace BasicGeometry double x2x2 = this.x2 * this.x2; double x3x3 = this.x3 * this.x3; - double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3); - if (squareModule <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus)) { Matrix3x3FP64.LoadIdentity(out matrix); return; } - double corrector1 = 1.0 / squareModule; - double corrector2 = 2.0 * corrector1; + double corrector1 = 1.0 / squareModulus; double s0x1 = this.s0 * this.x1; double s0x2 = this.s0 * this.x2; @@ -169,6 +168,8 @@ namespace BasicGeometry double x1x3 = this.x1 * this.x3; double x2x3 = this.x2 * this.x3; + double corrector2 = 2.0 * corrector1; + matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3)); matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3)); matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2)); @@ -200,15 +201,33 @@ namespace BasicGeometry public static void Multiply(in QuaternionFP64 left, in QuaternionFP64 right, out QuaternionFP64 product) { - double s0 = (left.s0 * right.s0 - left.x1 * right.x1) - (left.x2 * right.x2 + left.x3 * right.x3); - double x1 = (left.x1 * right.s0 + left.s0 * right.x1) - (left.x3 * right.x2 - left.x2 * right.x3); - double x2 = (left.x2 * right.s0 + left.s0 * right.x2) - (left.x1 * right.x3 - left.x3 * right.x1); - double x3 = (left.x3 * right.s0 + left.s0 * right.x3) - (left.x2 * right.x1 - left.x1 * right.x2); + double s0 = left.s0 * right.s0 - left.x1 * right.x1 - (left.x2 * right.x2 + left.x3 * right.x3); + double x1 = left.x1 * right.s0 + left.s0 * right.x1 - (left.x3 * right.x2 - left.x2 * right.x3); + double x2 = left.x2 * right.s0 + left.s0 * right.x2 - (left.x1 * right.x3 - left.x3 * right.x1); + double x3 = left.x3 * right.s0 + left.s0 * right.x3 - (left.x2 * right.x1 - left.x1 * right.x2); product.s0 = s0; product.x1 = x1; product.x2 = x2; product.x3 = x3; } + + public static bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2) + { + double ds0 = quaternion1.s0 - quaternion2.s0; + double dx1 = quaternion1.x1 - quaternion2.x1; + double dx2 = quaternion1.x2 - quaternion2.x2; + double dx3 = quaternion1.x3 - quaternion2.x3; + + double squareModulus1 = quaternion1.GetSquareModulus(); + double squareModulus2 = quaternion2.GetSquareModulus(); + double squareDistance = (ds0 * ds0 + dx1 * dx1) + (dx2 * dx2 + dx3 * dx3); + + if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) { + return squareDistance <= UtilityFP64.SQUARE_EPSYLON; + } + + return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2; + } } } diff --git a/BasicGeometry/UtilityFP32.cs b/BasicGeometry/UtilityFP32.cs index abc272b..a326deb 100644 --- a/BasicGeometry/UtilityFP32.cs +++ b/BasicGeometry/UtilityFP32.cs @@ -26,7 +26,7 @@ namespace BasicGeometry return (1.0f - EPSYLON) <= value && value <= (1.0f + EPSYLON); } - public static bool IsSqareValueUnit(float square) + public static bool IsSqareUnit(float square) { return (1.0f - 2.0f * EPSYLON) <= square && square <= (1.0f + 2.0f * EPSYLON); } diff --git a/BasicGeometry/UtilityFP64.cs b/BasicGeometry/UtilityFP64.cs index a940fb6..ba61ee9 100644 --- a/BasicGeometry/UtilityFP64.cs +++ b/BasicGeometry/UtilityFP64.cs @@ -26,7 +26,7 @@ namespace BasicGeometry return (1.0 - EPSYLON) <= value && value <= (1.0 + EPSYLON); } - public static bool IsSqareValueUnit(double square) + public static bool IsSqareUnit(double square) { return (1.0 - 2.0 * EPSYLON) <= square && square <= (1.0 + 2.0 * EPSYLON); } diff --git a/BasicGeometry/Vector2FP32.cs b/BasicGeometry/Vector2FP32.cs index 64f9854..9de1992 100644 --- a/BasicGeometry/Vector2FP32.cs +++ b/BasicGeometry/Vector2FP32.cs @@ -48,32 +48,32 @@ namespace BasicGeometry this.x2 = (float)vector.x2; } - public readonly float GetSquareModule() + public readonly float GetSquareModulus() { return this.x1 * this.x1 + this.x2 * this.x2; } - public readonly float GetModule() + public readonly float GetModulus() { - return MathF.Sqrt(this.GetSquareModule()); + return MathF.Sqrt(this.GetSquareModulus()); } public int Normalize() { - float squareModule = this.GetSquareModule(); + float squareModulus = this.GetSquareModulus(); - if (UtilityFP32.IsSqareValueUnit(squareModule)) + if (UtilityFP32.IsSqareUnit(squareModulus)) { return 1; } - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) { this.Reset(); return 0; } - float multiplier = MathF.Sqrt(1.0f / squareModule); + float multiplier = MathF.Sqrt(1.0f / squareModulus); this.x1 *= multiplier; this.x2 *= multiplier; @@ -89,12 +89,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP32.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP32.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -191,21 +191,21 @@ namespace BasicGeometry public static float GetAngle(in Vector2FP32 vector1, in Vector2FP32 vector2, AngleUnit unit) { - float squareModule1 = vector1.GetSquareModule(); + float squareModulus1 = vector1.GetSquareModulus(); - if (squareModule1 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float squareModule2 = vector2.GetSquareModule(); + float squareModulus2 = vector2.GetSquareModulus(); - if (squareModule2 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float cosine = Vector2FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); + float cosine = Vector2FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2); if (1.0f - UtilityFP32.EPSYLON <= cosine) { @@ -235,22 +235,22 @@ namespace BasicGeometry public static bool AreEqual(in Vector2FP32 vector1, in Vector2FP32 vector2) { - float squareModule1 = vector1.GetSquareModule(); - float squareModule2 = vector2.GetSquareModule(); - float squareModule3 = GetSquareDistance(vector1, vector2); + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + float squareModulus3 = GetSquareDistance(vector1, vector2); // 2.0f means dimension amount - if (squareModule1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (2.0f * UtilityFP32.SQUARE_EPSYLON); + return squareModulus3 < (2.0f * UtilityFP32.SQUARE_EPSYLON); } - if (squareModule1 <= squareModule2) + if (squareModulus1 <= squareModulus2) { - return squareModule3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule2; + return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus2; } - return squareModule3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule1; + return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus1; } } } diff --git a/BasicGeometry/Vector2FP64.cs b/BasicGeometry/Vector2FP64.cs index 2e40dfe..ab25936 100644 --- a/BasicGeometry/Vector2FP64.cs +++ b/BasicGeometry/Vector2FP64.cs @@ -48,32 +48,32 @@ namespace BasicGeometry this.x2 = vector.x2; } - public readonly double GetSquareModule() + public readonly double GetSquareModulus() { return this.x1 * this.x1 + this.x2 * this.x2; } - public readonly double GetModule() + public readonly double GetModulus() { - return Math.Sqrt(this.GetSquareModule()); + return Math.Sqrt(this.GetSquareModulus()); } public int Normalize() { - double squareModule = this.GetSquareModule(); + double squareModulus = this.GetSquareModulus(); - if (UtilityFP64.IsSqareValueUnit(squareModule)) + if (UtilityFP64.IsSqareUnit(squareModulus)) { return 1; } - if (squareModule <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON) { this.Reset(); return 0; } - double multiplier = Math.Sqrt(1.0 / squareModule); + double multiplier = Math.Sqrt(1.0 / squareModulus); this.x1 *= multiplier; this.x2 *= multiplier; @@ -89,12 +89,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP64.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP64.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -191,21 +191,21 @@ namespace BasicGeometry public static double GetAngle(in Vector2FP64 vector1, in Vector2FP64 vector2, AngleUnit unit) { - double squareModule1 = vector1.GetSquareModule(); + double squareModulus1 = vector1.GetSquareModulus(); - if (squareModule1 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double squareModule2 = vector2.GetSquareModule(); + double squareModulus2 = vector2.GetSquareModulus(); - if (squareModule2 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); + double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2); if (1.0 - UtilityFP64.EPSYLON <= cosine) { @@ -235,22 +235,22 @@ namespace BasicGeometry public static bool AreEqual(in Vector2FP64 vector1, in Vector2FP64 vector2) { - double squareModule1 = vector1.GetSquareModule(); - double squareModule2 = vector2.GetSquareModule(); - double squareModule3 = GetSquareDistance(vector1, vector2); + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + double squareModulus3 = GetSquareDistance(vector1, vector2); // 2.0 means dimension amount - if (squareModule1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (2.0 * UtilityFP64.SQUARE_EPSYLON); + return squareModulus3 < (2.0 * UtilityFP64.SQUARE_EPSYLON); } - if (squareModule1 <= squareModule2) + if (squareModulus1 <= squareModulus2) { - return squareModule3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule2; + return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus2; } - return squareModule3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule1; + return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus1; } } } diff --git a/BasicGeometry/Vector3FP32.cs b/BasicGeometry/Vector3FP32.cs index 54d38a0..5cd65ea 100644 --- a/BasicGeometry/Vector3FP32.cs +++ b/BasicGeometry/Vector3FP32.cs @@ -52,32 +52,32 @@ namespace BasicGeometry this.x3 = (float)vector.x3; } - public readonly float GetSquareModule() + public readonly float GetSquareModulus() { return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3; } - public readonly float GetModule() + public readonly float GetModulus() { - return MathF.Sqrt(this.GetSquareModule()); + return MathF.Sqrt(this.GetSquareModulus()); } public int Normalize() { - float squareModule = this.GetSquareModule(); + float squareModulus = this.GetSquareModulus(); - if (UtilityFP32.IsSqareValueUnit(squareModule)) + if (UtilityFP32.IsSqareUnit(squareModulus)) { return 1; } - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP32.SQUARE_EPSYLON) { this.Reset(); return 0; } - float multiplier = MathF.Sqrt(1.0f / squareModule); + float multiplier = MathF.Sqrt(1.0f / squareModulus); this.x1 *= multiplier; this.x2 *= multiplier; @@ -95,12 +95,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP32.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP32.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -233,21 +233,21 @@ namespace BasicGeometry public static float GetAngle(in Vector3FP32 vector1, in Vector3FP32 vector2, AngleUnit unit) { - float squareModule1 = vector1.GetSquareModule(); + float squareModulus1 = vector1.GetSquareModulus(); - if (squareModule1 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float squareModule2 = vector2.GetSquareModule(); + float squareModulus2 = vector2.GetSquareModulus(); - if (squareModule2 <= UtilityFP32.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON) { return 0.0f; } - float cosine = Vector3FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); + float cosine = Vector3FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2); if (1.0f - UtilityFP32.EPSYLON <= cosine) { @@ -278,22 +278,17 @@ namespace BasicGeometry public static bool AreEqual(in Vector3FP32 vector1, in Vector3FP32 vector2) { - float squareModule1 = vector1.GetSquareModule(); - float squareModule2 = vector2.GetSquareModule(); - float squareModule3 = GetSquareDistance(vector1, vector2); + float squareModulus1 = vector1.GetSquareModulus(); + float squareModulus2 = vector2.GetSquareModulus(); + float squareDistance = GetSquareDistance(vector1, vector2); // 3.0f means dimension amount - if (squareModule1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (3.0f * UtilityFP32.SQUARE_EPSYLON); + return squareDistance <= UtilityFP32.SQUARE_EPSYLON; } - if (squareModule1 <= squareModule2) - { - return squareModule3 <= (3.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule2; - } - - return squareModule3 <= (3.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule1; + return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2; } } } diff --git a/BasicGeometry/Vector3FP64.cs b/BasicGeometry/Vector3FP64.cs index 1823f86..7e42050 100644 --- a/BasicGeometry/Vector3FP64.cs +++ b/BasicGeometry/Vector3FP64.cs @@ -52,33 +52,33 @@ namespace BasicGeometry this.x3 = vector.x3; } - public readonly double GetSquareModule() + public readonly double GetSquareModulus() { return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3; } - public readonly double GetModule() + public readonly double GetModulus() { - return Math.Sqrt(this.GetSquareModule()); + return Math.Sqrt(this.GetSquareModulus()); } public int Normalize() { - double squareModule = this.GetSquareModule(); + double squareModulus = this.GetSquareModulus(); - if (UtilityFP64.IsSqareValueUnit(squareModule)) + if (UtilityFP64.IsSqareUnit(squareModulus)) { return 1; } - if (squareModule <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus <= UtilityFP64.SQUARE_EPSYLON) { this.Reset(); return 0; } - double multiplier = Math.Sqrt(1.0 / squareModule); + double multiplier = Math.Sqrt(1.0 / squareModulus); this.x1 *= multiplier; this.x2 *= multiplier; @@ -96,12 +96,12 @@ namespace BasicGeometry public readonly bool IsZero() { - return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON; + return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON; } public readonly bool IsUnit() { - return UtilityFP64.IsSqareValueUnit(this.GetSquareModule()); + return UtilityFP64.IsSqareUnit(this.GetSquareModulus()); } public void Reset() @@ -234,21 +234,21 @@ namespace BasicGeometry public static double GetAngle(in Vector3FP64 vector1, in Vector3FP64 vector2, AngleUnit unit) { - double squareModule1 = vector1.GetSquareModule(); + double squareModulus1 = vector1.GetSquareModulus(); - if (squareModule1 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double squareModule2 = vector2.GetSquareModule(); + double squareModulus2 = vector2.GetSquareModulus(); - if (squareModule2 <= UtilityFP64.SQUARE_EPSYLON) + if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON) { return 0.0; } - double cosine = Vector3FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); + double cosine = Vector3FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2); if (1.0 - UtilityFP64.EPSYLON <= cosine) { @@ -279,22 +279,16 @@ namespace BasicGeometry public static bool AreEqual(in Vector3FP64 vector1, in Vector3FP64 vector2) { - double squareModule1 = vector1.GetSquareModule(); - double squareModule2 = vector2.GetSquareModule(); - double squareModule3 = GetSquareDistance(vector1, vector2); + double squareModulus1 = vector1.GetSquareModulus(); + double squareModulus2 = vector2.GetSquareModulus(); + double squareDistance = GetSquareDistance(vector1, vector2); - // 3.0 means dimension amount - if (squareModule1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) + if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) { - return squareModule3 < (3.0 * UtilityFP64.SQUARE_EPSYLON); + return squareDistance <= UtilityFP64.SQUARE_EPSYLON; } - if (squareModule1 <= squareModule2) - { - return squareModule3 <= (3.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule2; - } - - return squareModule3 <= (3.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule1; + return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2; } } } diff --git a/BasicGeometry/VersorFP32.cs b/BasicGeometry/VersorFP32.cs index 30a35bb..9aaf3cd 100644 --- a/BasicGeometry/VersorFP32.cs +++ b/BasicGeometry/VersorFP32.cs @@ -30,7 +30,17 @@ namespace BasicGeometry public VersorFP32(float s0, float x1, float x2, float x3) { - LoadValues(s0, x1, x2, x3, out this); + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + + float squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); + + if (!UtilityFP32.IsSqareUnit(squareModulus)) + { + this.Normalize(squareModulus); + } } public VersorFP32(in VersorFP32 versor) @@ -47,6 +57,13 @@ namespace BasicGeometry this.x1 = (float)versor.GetX1(); this.x2 = (float)versor.GetX2(); this.x3 = (float)versor.GetX3(); + + float squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); + + if (!UtilityFP32.IsSqareUnit(squareModulus)) + { + this.Normalize(squareModulus); + } } public readonly float GetScalar() @@ -69,6 +86,20 @@ namespace BasicGeometry return this.x3; } + public readonly float GetAngle(AngleUnit unit) + { + if (this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || 1.0f - UtilityFP32.EPSYLON <= this.s0) { + return 0.0f; + } + + if (UtilityFP32.IsZero(this.s0)) + { + return AngleFP32.GetHalfCircle(unit); + } + + return RadianFP32.ToUnits(2.0f * MathF.Acos(this.s0), unit); + } + public readonly bool IsIdle() { return this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || (1.0f - UtilityFP32.EPSYLON) <= this.s0; @@ -99,38 +130,116 @@ namespace BasicGeometry this.x3 = -this.x3; } - public readonly float GetAngle(AngleUnit unit) + public void SetValues(float s0, float x1, float x2, float x3) { - if (this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || 1.0f - UtilityFP32.EPSYLON <= this.s0) { - return 0.0f; - } + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; - if (UtilityFP32.IsZero(this.s0)) + float squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + if (!UtilityFP32.IsSqareUnit(squareModulus)) { - return AngleFP32.GetHalfCircle(unit); + this.Normalize(squareModulus); } - - return RadianFP32.ToUnits(2.0f * MathF.Acos(this.s0), unit); } - public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix) + public void Set(in VersorFP32 versor) { - float s0s0 = this.s0 * this.s0; - float x1x1 = this.x1 * this.x1; - float x2x2 = this.x1 * this.x2; - float x3x3 = this.x1 * this.x3; + this.s0 = versor.s0; + this.x1 = versor.x1; + this.x2 = versor.x2; + this.x3 = versor.x3; + } - float s0x1 = 2.0f * this.s0 * this.x1; - float s0x2 = 2.0f * this.s0 * this.x2; - float s0x3 = 2.0f * this.s0 * this.x3; + public void Set(in VersorFP64 versor) + { + this.SetValues( + (float) versor.GetScalar(), + (float) versor.GetX1(), + (float) versor.GetX2(), + (float) versor.GetX3()); + } - float x1x2 = 2.0f * this.x1 * this.x2; - float x1x3 = 2.0f * this.x1 * this.x3; - float x2x3 = 2.0f * this.x2 * this.x3; + private void Normalize(float squareModulus) + { + if (!float.IsFinite(squareModulus) || squareModulus <= UtilityFP32.SQUARE_EPSYLON) + { + this.Reset(); + return; + } - matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + float multiplier = MathF.Sqrt(1.0f / squareModulus); + + this.s0 *= multiplier; + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; + } + + public static void Combine(in VersorFP32 second, in VersorFP32 first, out VersorFP32 result) + { + float s0 = second.s0 * first.s0 - second.x1 * first.x1 - (second.x2 * first.x2 + second.x3 * first.x3); + float x1 = second.x1 * first.s0 + second.s0 * first.x1 - (second.x3 * first.x2 - second.x2 * first.x3); + float x2 = second.x2 * first.s0 + second.s0 * first.x2 - (second.x1 * first.x3 - second.x3 * first.x1); + float x3 = second.x3 * first.s0 + second.s0 * first.x3 - (second.x2 * first.x1 - second.x1 * first.x2); + + float squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3); + + result.s0 = s0; + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + + if (!UtilityFP32.IsSqareUnit(squareModulus)) + { + result.Normalize(squareModulus); + } + } + + public static void MakeInverted(in VersorFP32 versor, out VersorFP32 conjugate) + { + conjugate.s0 = versor.s0; + conjugate.x1 = -versor.x1; + conjugate.x2 = -versor.x2; + conjugate.x3 = -versor.x3; + } + + public static void MakeShortened(in VersorFP32 versor, out VersorFP32 shortened) + { + if (versor.s0 < 0.0f) { + shortened.s0 = -versor.s0; + shortened.x1 = -versor.x1; + shortened.x2 = -versor.x2; + shortened.x3 = -versor.x3; + } + else { + shortened.s0 = versor.s0; + shortened.x1 = versor.x1; + shortened.x2 = versor.x2; + shortened.x3 = versor.x3; + } + } + + public static void MakeRotationMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix) + { + float s0s0 = versor.s0 * versor.s0; + float x1x1 = versor.x1 * versor.x1; + float x2x2 = versor.x1 * versor.x2; + float x3x3 = versor.x1 * versor.x3; + + float s0x1 = 2.0f * versor.s0 * versor.x1; + float s0x2 = 2.0f * versor.s0 * versor.x2; + float s0x3 = 2.0f * versor.s0 * versor.x3; + + float x1x2 = 2.0f * versor.x1 * versor.x2; + float x1x3 = 2.0f * versor.x1 * versor.x3; + float x2x3 = 2.0f * versor.x2 * versor.x3; + + matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); + matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); + matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); matrix.r1c2 = x1x2 - s0x3; matrix.r2c3 = x2x3 - s0x1; @@ -141,24 +250,24 @@ namespace BasicGeometry matrix.r1c3 = x1x3 + s0x2; } - public readonly void MakeReverseMatrix(out Matrix3x3FP32 matrix) + public static void MakeReverseMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix) { - float s0s0 = this.s0 * this.s0; - float x1x1 = this.x1 * this.x1; - float x2x2 = this.x1 * this.x2; - float x3x3 = this.x1 * this.x3; + float s0s0 = versor.s0 * versor.s0; + float x1x1 = versor.x1 * versor.x1; + float x2x2 = versor.x1 * versor.x2; + float x3x3 = versor.x1 * versor.x3; - float s0x1 = 2.0f * this.s0 * this.x1; - float s0x2 = 2.0f * this.s0 * this.x2; - float s0x3 = 2.0f * this.s0 * this.x3; + float s0x1 = 2.0f * versor.s0 * versor.x1; + float s0x2 = 2.0f * versor.s0 * versor.x2; + float s0x3 = 2.0f * versor.s0 * versor.x3; - float x1x2 = 2.0f * this.x1 * this.x2; - float x1x3 = 2.0f * this.x1 * this.x3; - float x2x3 = 2.0f * this.x2 * this.x3; + float x1x2 = 2.0f * versor.x1 * versor.x2; + float x1x3 = 2.0f * versor.x1 * versor.x3; + float x2x3 = 2.0f * versor.x2 * versor.x3; - matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); + matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); + matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); matrix.r1c2 = x1x2 + s0x3; matrix.r2c3 = x2x3 + s0x1; @@ -169,165 +278,56 @@ namespace BasicGeometry matrix.r1c3 = x1x3 - s0x2; } - public void SetValues(float s0, float x1, float x2, float x3) + public static void Turn(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result) { - this.s0 = s0; - this.x1 = x1; - this.x2 = x2; - this.x3 = x3; + float tx1 = 2.0f * (versor.x2 * vector.x3 - versor.x3 * vector.x2); + float tx2 = 2.0f * (versor.x3 * vector.x1 - versor.x1 * vector.x3); + float tx3 = 2.0f * (versor.x1 * vector.x2 - versor.x2 * vector.x1); - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (!UtilityFP32.IsSqareValueUnit(squareModule)) - { - this.Normalize(squareModule); - } - } - - public void SetValues(in VersorFP32 versor) - { - this.s0 = versor.s0; - this.x1 = versor.x1; - this.x2 = versor.x2; - this.x3 = versor.x3; - } - - public void SetValues(in VersorFP64 versor) - { - this.SetValues( - (float) versor.GetScalar(), - (float) versor.GetX1(), - (float) versor.GetX2(), - (float) versor.GetX3()); - } - - public void SetInverted(in VersorFP32 versor) - { - this.s0 = versor.s0; - this.x1 = -versor.x1; - this.x2 = -versor.x2; - this.x3 = -versor.x3; - } - - public void SetInverted(in VersorFP64 versor) - { - this.SetValues( - (float)versor.GetScalar(), - (float)-versor.GetX1(), - (float)-versor.GetX2(), - (float)-versor.GetX3()); - } - - public void SetShortened(in VersorFP32 versor) - { - if (versor.s0 < 0.0f) - { - this.s0 = -versor.s0; - this.x1 = -versor.x1; - this.x2 = -versor.x2; - this.x3 = -versor.x3; - } - else - { - this.s0 = versor.s0; - this.x1 = versor.x1; - this.x2 = versor.x2; - this.x3 = versor.x3; - } - } - - public void SetShortened(in VersorFP64 versor) - { - this.SetValues(versor); - this.Shorten(); - } - - public readonly void Turn(in Vector3FP32 vector, out Vector3FP32 result) - { - float tx1 = 2.0f * (this.x2 * vector.x3 - this.x3 * vector.x2); - float tx2 = 2.0f * (this.x3 * vector.x1 - this.x1 * vector.x3); - float tx3 = 2.0f * (this.x1 * vector.x2 - this.x2 * vector.x1); - - float x1 = (vector.x1 + tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); - float x2 = (vector.x2 + tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3); - float x3 = (vector.x3 + tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1); + float x1 = vector.x1 + tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2); + float x2 = vector.x2 + tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3); + float x3 = vector.x3 + tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1); result.x1 = x1; result.x2 = x2; result.x3 = x3; } - public readonly void TurnBack(in Vector3FP32 vector, out Vector3FP32 result) + public static void TurnBack(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result) { - float tx1 = 2.0f * (this.x2 * vector.x3 - this.x3 * vector.x2); - float tx2 = 2.0f * (this.x3 * vector.x1 - this.x1 * vector.x3); - float tx3 = 2.0f * (this.x1 * vector.x2 - this.x2 * vector.x1); + float tx1 = 2.0f * (versor.x2 * vector.x3 - versor.x3 * vector.x2); + float tx2 = 2.0f * (versor.x3 * vector.x1 - versor.x1 * vector.x3); + float tx3 = 2.0f * (versor.x1 * vector.x2 - versor.x2 * vector.x1); - float x1 = (vector.x1 - tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); - float x2 = (vector.x2 - tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3); - float x3 = (vector.x3 - tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1); + float x1 = vector.x1 - tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2); + float x2 = vector.x2 - tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3); + float x3 = vector.x3 - tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1); result.x1 = x1; result.x2 = x2; result.x3 = x3; } - private void Normalize(float squareModule) + public static void LoadIdentity(out VersorFP32 result) { - if (squareModule <= UtilityFP32.SQUARE_EPSYLON) - { - this.Reset(); - return; - } - - float multiplier = MathF.Sqrt(1.0f / squareModule); - - this.s0 *= multiplier; - this.x1 *= multiplier; - this.x2 *= multiplier; - this.x3 *= multiplier; + result.s0 = 1.0f; + result.x1 = 0.0f; + result.x2 = 0.0f; + result.x3 = 0.0f; } - public static void Combine(in VersorFP32 second, in VersorFP32 first, out VersorFP32 result) + public static void LoadValues(float s0, float x1, float x2, float x3, out VersorFP32 result) { - float s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3); - float x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3); - float x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1); - float x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2); - - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + float squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3); result.s0 = s0; result.x1 = x1; result.x2 = x2; result.x3 = x3; - if (!UtilityFP32.IsSqareValueUnit(squareModule)) + if (!UtilityFP32.IsSqareUnit(squareModulus)) { - result.Normalize(squareModule); - } - } - - public static void LoadIdle(out VersorFP32 versor) - { - versor.s0 = 1.0f; - versor.x1 = 0.0f; - versor.x2 = 0.0f; - versor.x3 = 0.0f; - } - - public static void LoadValues(float s0, float x1, float x2, float x3, out VersorFP32 versor) - { - versor.s0 = s0; - versor.x1 = x1; - versor.x2 = x2; - versor.x3 = x3; - - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (!UtilityFP32.IsSqareValueUnit(squareModule)) - { - versor.Normalize(squareModule); + result.Normalize(squareModulus); } } } diff --git a/BasicGeometry/VersorFP64.cs b/BasicGeometry/VersorFP64.cs index 5026cad..9878c7e 100644 --- a/BasicGeometry/VersorFP64.cs +++ b/BasicGeometry/VersorFP64.cs @@ -31,7 +31,17 @@ namespace BasicGeometry public VersorFP64(double s0, double x1, double x2, double x3) { - LoadValues(s0, x1, x2, x3, out this); + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + + double squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); + + if (!UtilityFP64.IsSqareUnit(squareModulus)) + { + this.Normalize(squareModulus); + } } public VersorFP64(in VersorFP64 versor) @@ -44,7 +54,17 @@ namespace BasicGeometry public VersorFP64(in VersorFP32 versor) { - LoadValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3(), out this); + this.s0 = versor.GetScalar(); + this.x1 = versor.GetX1(); + this.x2 = versor.GetX2(); + this.x3 = versor.GetX3(); + + double squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3); + + if (!UtilityFP64.IsSqareUnit(squareModulus)) + { + this.Normalize(squareModulus); + } } public readonly double GetScalar() @@ -67,7 +87,21 @@ namespace BasicGeometry return this.x3; } - public readonly bool IsIdle() + public readonly double GetAngle(AngleUnit unit) + { + if (this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || 1.0 - UtilityFP64.EPSYLON <= this.s0) { + return 0.0; + } + + if (UtilityFP64.IsZero(this.s0)) + { + return AngleFP64.GetHalfCircle(unit); + } + + return RadianFP64.ToUnits(2.0 * Math.Acos(this.s0), unit); + } + + public readonly bool IsIdentity() { return this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || (1.0 - UtilityFP64.EPSYLON) <= this.s0; } @@ -98,39 +132,112 @@ namespace BasicGeometry this.x3 = -this.x3; } - public readonly double GetAngle(AngleUnit unit) + public void SetValues(double s0, double x1, double x2, double x3) { - if (this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || 1.0 - UtilityFP64.EPSYLON <= this.s0) { - return 0.0; - } - - if (UtilityFP64.IsZero(this.s0)) + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + + double squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + if (!UtilityFP64.IsSqareUnit(squareModulus)) { - return AngleFP64.GetHalfCircle(unit); + this.Normalize(squareModulus); } - - return RadianFP64.ToUnits(2.0 * Math.Acos(this.s0), unit); } - public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix) + public void Set(in VersorFP64 versor) { - double s0s0 = this.s0 * this.s0; - double x1x1 = this.x1 * this.x1; - double x2x2 = this.x1 * this.x2; - double x3x3 = this.x1 * this.x3; + this.s0 = versor.s0; + this.x1 = versor.x1; + this.x2 = versor.x2; + this.x3 = versor.x3; + } + public void Set(in VersorFP32 versor) + { + this.SetValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3()); + } - double s0x1 = 2.0 * this.s0 * this.x1; - double s0x2 = 2.0 * this.s0 * this.x2; - double s0x3 = 2.0 * this.s0 * this.x3; + private void Normalize(double squareModulus) + { + if (!double.IsFinite(squareModulus) || squareModulus <= UtilityFP64.SQUARE_EPSYLON) + { + this.Reset(); + return; + } - double x1x2 = 2.0 * this.x1 * this.x2; - double x1x3 = 2.0 * this.x1 * this.x3; - double x2x3 = 2.0 * this.x2 * this.x3; + double multiplier = Math.Sqrt(1.0 / squareModulus); - matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + this.s0 *= multiplier; + this.x1 *= multiplier; + this.x2 *= multiplier; + this.x3 *= multiplier; + } + + public static void Combine(in VersorFP64 second, in VersorFP64 first, out VersorFP64 result) + { + double s0 = second.s0 * first.s0 - second.x1 * first.x1 - (second.x2 * first.x2 + second.x3 * first.x3); + double x1 = second.x1 * first.s0 + second.s0 * first.x1 - (second.x3 * first.x2 - second.x2 * first.x3); + double x2 = second.x2 * first.s0 + second.s0 * first.x2 - (second.x1 * first.x3 - second.x3 * first.x1); + double x3 = second.x3 * first.s0 + second.s0 * first.x3 - (second.x2 * first.x1 - second.x1 * first.x2); + + double squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + result.s0 = s0; + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + + if (!UtilityFP64.IsSqareUnit(squareModulus)) + { + result.Normalize(squareModulus); + } + } + + public static void MakeInverted(in VersorFP64 versor, out VersorFP64 conjugate) + { + conjugate.s0 = versor.s0; + conjugate.x1 = -versor.x1; + conjugate.x2 = -versor.x2; + conjugate.x3 = -versor.x3; + } + + public static void MakeShortened(in VersorFP64 versor, out VersorFP64 shortened) + { + if (versor.s0 < 0.0) { + shortened.s0 = -versor.s0; + shortened.x1 = -versor.x1; + shortened.x2 = -versor.x2; + shortened.x3 = -versor.x3; + } + else { + shortened.s0 = versor.s0; + shortened.x1 = versor.x1; + shortened.x2 = versor.x2; + shortened.x3 = versor.x3; + } + } + + public static void MakeRotationMatrix(in VersorFP64 versor, out Matrix3x3FP64 matrix) + { + double s0s0 = versor.s0 * versor.s0; + double x1x1 = versor.x1 * versor.x1; + double x2x2 = versor.x1 * versor.x2; + double x3x3 = versor.x1 * versor.x3; + + double s0x1 = 2.0 * versor.s0 * versor.x1; + double s0x2 = 2.0 * versor.s0 * versor.x2; + double s0x3 = 2.0 * versor.s0 * versor.x3; + + double x1x2 = 2.0 * versor.x1 * versor.x2; + double x1x3 = 2.0 * versor.x1 * versor.x3; + double x2x3 = 2.0 * versor.x2 * versor.x3; + + matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); + matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); + matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); matrix.r1c2 = x1x2 - s0x3; matrix.r2c3 = x2x3 - s0x1; @@ -141,24 +248,24 @@ namespace BasicGeometry matrix.r1c3 = x1x3 + s0x2; } - public readonly void MakeReverseMatrix(out Matrix3x3FP64 matrix) + public static void MakeReverseMatrix(in VersorFP64 versor, out Matrix3x3FP64 matrix) { - double s0s0 = this.s0 * this.s0; - double x1x1 = this.x1 * this.x1; - double x2x2 = this.x1 * this.x2; - double x3x3 = this.x1 * this.x3; + double s0s0 = versor.s0 * versor.s0; + double x1x1 = versor.x1 * versor.x1; + double x2x2 = versor.x1 * versor.x2; + double x3x3 = versor.x1 * versor.x3; - double s0x1 = 2.0 * this.s0 * this.x1; - double s0x2 = 2.0 * this.s0 * this.x2; - double s0x3 = 2.0 * this.s0 * this.x3; + double s0x1 = 2.0 * versor.s0 * versor.x1; + double s0x2 = 2.0 * versor.s0 * versor.x2; + double s0x3 = 2.0 * versor.s0 * versor.x3; - double x1x2 = 2.0 * this.x1 * this.x2; - double x1x3 = 2.0 * this.x1 * this.x3; - double x2x3 = 2.0 * this.x2 * this.x3; + double x1x2 = 2.0 * versor.x1 * versor.x2; + double x1x3 = 2.0 * versor.x1 * versor.x3; + double x2x3 = 2.0 * versor.x2 * versor.x3; - matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3); - matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3); - matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2); + matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3); + matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3); + matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2); matrix.r1c2 = x1x2 + s0x3; matrix.r2c3 = x2x3 + s0x1; @@ -169,157 +276,56 @@ namespace BasicGeometry matrix.r1c3 = x1x3 - s0x2; } - public void SetValues(double s0, double x1, double x2, double x3) + public static void Turn(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result) { - this.s0 = s0; - this.x1 = x1; - this.x2 = x2; - this.x3 = x3; - - double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (!UtilityFP64.IsSqareValueUnit(squareModule)) - { - this.Normalize(squareModule); - } - } + double tx1 = 2.0 * (versor.x2 * vector.x3 - versor.x3 * vector.x2); + double tx2 = 2.0 * (versor.x3 * vector.x1 - versor.x1 * vector.x3); + double tx3 = 2.0 * (versor.x1 * vector.x2 - versor.x2 * vector.x1); - public void SetValues(in VersorFP64 versor) - { - this.s0 = versor.s0; - this.x1 = versor.x1; - this.x2 = versor.x2; - this.x3 = versor.x3; - } - - public void SetValues(in VersorFP32 versor) - { - this.SetValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3()); - } - - public void SetShortened(in VersorFP64 versor) - { - if (versor.s0 < 0.0) - { - this.s0 = -versor.s0; - this.x1 = -versor.x1; - this.x2 = -versor.x2; - this.x3 = -versor.x3; - } - else - { - this.s0 = versor.s0; - this.x1 = versor.x1; - this.x2 = versor.x2; - this.x3 = versor.x3; - } - } - - public void SetShortened(in VersorFP32 versor) - { - this.SetValues(versor); - this.Shorten(); - } - - public void SetInverted(in VersorFP64 versor) - { - this.s0 = versor.s0; - this.x1 = -versor.x1; - this.x2 = -versor.x2; - this.x3 = -versor.x3; - } - - public void SetInverted(in VersorFP32 versor) - { - this.SetValues(versor.GetScalar(), -versor.GetX1(), -versor.GetX2(), -versor.GetX3()); - } - - public readonly void Turn(in Vector3FP64 vector, out Vector3FP64 result) - { - double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2); - double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3); - double tx3 = 2.0 * (this.x1 * vector.x2 - this.x2 * vector.x1); - - double x1 = (vector.x1 + tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); - double x2 = (vector.x2 + tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3); - double x3 = (vector.x3 + tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1); + double x1 = vector.x1 + tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2); + double x2 = vector.x2 + tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3); + double x3 = vector.x3 + tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1); result.x1 = x1; result.x2 = x2; result.x3 = x3; } - public readonly void TurnBack(in Vector3FP64 vector, out Vector3FP64 result) + public static void TurnBack(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result) { - double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2); - double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3); - double tx3 = 2.0 * (this.x1 * vector.x2 - this.x2 * vector.x1); + double tx1 = 2.0 * (versor.x2 * vector.x3 - versor.x3 * vector.x2); + double tx2 = 2.0 * (versor.x3 * vector.x1 - versor.x1 * vector.x3); + double tx3 = 2.0 * (versor.x1 * vector.x2 - versor.x2 * vector.x1); - double x1 = (vector.x1 - tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); - double x2 = (vector.x2 - tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3); - double x3 = (vector.x3 - tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1); + double x1 = vector.x1 - tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2); + double x2 = vector.x2 - tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3); + double x3 = vector.x3 - tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1); result.x1 = x1; result.x2 = x2; result.x3 = x3; } - private void Normalize(double squareModule) + public static void LoadIdentity(out VersorFP64 result) { - if (squareModule <= UtilityFP64.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= UtilityFP64.SQUARE_EPSYLON * squareModule) - { - this.Reset(); - return; - } - - double multiplier = Math.Sqrt(1.0 / squareModule); - - this.s0 *= multiplier; - this.x1 *= multiplier; - this.x2 *= multiplier; - this.x3 *= multiplier; + result.s0 = 1.0; + result.x1 = 0.0; + result.x2 = 0.0; + result.x3 = 0.0; } - public static void Combine(in VersorFP64 second, in VersorFP64 first, out VersorFP64 result) + public static void LoadValues(double s0, double x1, double x2, double x3, out VersorFP64 result) { - double s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3); - double x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3); - double x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1); - double x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2); - - double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + double squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3); result.s0 = s0; result.x1 = x1; result.x2 = x2; result.x3 = x3; - if (!UtilityFP64.IsSqareValueUnit(squareModule)) + if (!UtilityFP64.IsSqareUnit(squareModulus)) { - result.Normalize(squareModule); - } - } - - public static void LoadIdle(out VersorFP64 versor) - { - versor.s0 = 1.0; - versor.x1 = 0.0; - versor.x2 = 0.0; - versor.x3 = 0.0; - } - - public static void LoadValues(double s0, double x1, double x2, double x3, out VersorFP64 versor) - { - versor.s0 = s0; - versor.x1 = x1; - versor.x2 = x2; - versor.x3 = x3; - - double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); - - if (!UtilityFP64.IsSqareValueUnit(squareModule)) - { - versor.Normalize(squareModule); + result.Normalize(squareModulus); } } } diff --git a/BasicGeometryDev/Program.cs b/BasicGeometryDev/Program.cs index 14e854a..1d599d3 100644 --- a/BasicGeometryDev/Program.cs +++ b/BasicGeometryDev/Program.cs @@ -2,6 +2,7 @@ using System; using System.ComponentModel; using System.Diagnostics; +using System.Numerics; using BasicGeometry; public static class Program @@ -46,7 +47,7 @@ public static class Program { Console.WriteLine("({0}, {1}, {2}, {3})", versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3()); } - +/* public static int Main() { int amount = 1000000; @@ -75,6 +76,15 @@ public static class Program PrintVersor(versors2[10]); PrintVersor(results[10]); + return 0; + } +*/ + public static int Main() + { + Vector2FP32 vector = Vector2FP32.ZERO; + + Console.WriteLine("{0}", vector.ToString()); + return 0; } } \ No newline at end of file From eb88fb2db46565da4e69d94e47a7ed36c1cecb5b Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Thu, 13 Feb 2025 01:10:31 +0700 Subject: [PATCH 7/9] =?UTF-8?q?=D0=A0=D0=B5=D0=BE=D1=80=D0=B3=D0=B0=D0=BD?= =?UTF-8?q?=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D1=8F=20=D0=BF=D1=80=D0=BE=D0=B5?= =?UTF-8?q?=D0=BA=D1=82=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/VersorFP32.cs | 12 ++--- BasicGeometryDev/Program.cs | 2 +- BasicGeometryTest/TestVector2FP32.cs | 27 ---------- BasicGeometryTest/Vector2/Vector2InitTest.cs | 48 ++++++++++++++++++ .../Vector2/Vector2IsZeroTest.cs | 50 +++++++++++++++++++ 5 files changed, 105 insertions(+), 34 deletions(-) delete mode 100644 BasicGeometryTest/TestVector2FP32.cs create mode 100644 BasicGeometryTest/Vector2/Vector2InitTest.cs create mode 100644 BasicGeometryTest/Vector2/Vector2IsZeroTest.cs diff --git a/BasicGeometry/VersorFP32.cs b/BasicGeometry/VersorFP32.cs index 30a35bb..0b778b4 100644 --- a/BasicGeometry/VersorFP32.cs +++ b/BasicGeometry/VersorFP32.cs @@ -290,12 +290,12 @@ 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 s0 = second.s0 * first.s0 - second.x1 * first.x1 - (second.x2 * first.x2 + second.x3 * first.x3); + float x1 = second.x1 * first.s0 + second.s0 * first.x1 - (second.x3 * first.x2 - second.x2 * first.x3); + float x2 = second.x2 * first.s0 + second.s0 * first.x2 - (second.x1 * first.x3 - second.x3 * first.x1); + float x3 = second.x3 * first.s0 + second.s0 * first.x3 - (second.x2 * first.x1 - second.x1 * first.x2); - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + float squareModule = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3); result.s0 = s0; result.x1 = x1; @@ -323,7 +323,7 @@ namespace BasicGeometry versor.x2 = x2; versor.x3 = x3; - float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + float squareModule = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3); if (!UtilityFP32.IsSqareValueUnit(squareModule)) { diff --git a/BasicGeometryDev/Program.cs b/BasicGeometryDev/Program.cs index 14e854a..1d4e070 100644 --- a/BasicGeometryDev/Program.cs +++ b/BasicGeometryDev/Program.cs @@ -49,7 +49,7 @@ public static class Program public static int Main() { - int amount = 1000000; + const int amount = 1000000; VersorFP32[] versors1 = MakeRandomVersors(amount); VersorFP32[] versors2 = MakeRandomVersors(amount); diff --git a/BasicGeometryTest/TestVector2FP32.cs b/BasicGeometryTest/TestVector2FP32.cs deleted file mode 100644 index aa1f48e..0000000 --- a/BasicGeometryTest/TestVector2FP32.cs +++ /dev/null @@ -1,27 +0,0 @@ -using Microsoft.VisualStudio.TestTools.UnitTesting; - -using System; -using System.IO; - -using BasicGeometry; - -namespace BasicGeometryTest -{ - [TestClass] - public class TestVector2FP32 - { - [TestMethod] - public void TestReset() - { - Vector2FP32 vector = new Vector2FP32(1.0f, 2.0f); - - Assert.AreEqual(vector.x1, 1.0f); - Assert.AreEqual(vector.x2, 2.0f); - - vector.Reset(); - - Assert.AreEqual(vector.x1, 0.0f); - Assert.AreEqual(vector.x2, 0.0f); - } - } -} diff --git a/BasicGeometryTest/Vector2/Vector2InitTest.cs b/BasicGeometryTest/Vector2/Vector2InitTest.cs new file mode 100644 index 0000000..7b6f17a --- /dev/null +++ b/BasicGeometryTest/Vector2/Vector2InitTest.cs @@ -0,0 +1,48 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; + +using BasicGeometry; + +namespace BasicGeometryTest +{ + [TestClass] + public class Vector2InitTest + { + [TestMethod] + public void InitVector2FP32() + { + Vector2FP32 vector = new Vector2FP32(1.0f, 2.0f); + + Assert.AreEqual(vector.x1, 1.0f); + Assert.AreEqual(vector.x2, 2.0f); + + vector.Reset(); + + Assert.AreEqual(vector.x1, 0.0f); + Assert.AreEqual(vector.x2, 0.0f); + + vector.SetValues(-5.02f, -200.7f); + + Assert.AreEqual(vector.x1, -5.02f); + Assert.AreEqual(vector.x2, -200.7f); + } + + [TestMethod] + public void InitVector2FP64() + { + Vector2FP64 vector = new Vector2FP64(1.0, 2.0); + + Assert.AreEqual(vector.x1, 1.0); + Assert.AreEqual(vector.x2, 2.0); + + vector.Reset(); + + Assert.AreEqual(vector.x1, 0.0); + Assert.AreEqual(vector.x2, 0.0); + + vector.SetValues(-5.79, -200.2); + + Assert.AreEqual(vector.x1, -5.79); + Assert.AreEqual(vector.x2, -200.2); + } + } +} diff --git a/BasicGeometryTest/Vector2/Vector2IsZeroTest.cs b/BasicGeometryTest/Vector2/Vector2IsZeroTest.cs new file mode 100644 index 0000000..25816f0 --- /dev/null +++ b/BasicGeometryTest/Vector2/Vector2IsZeroTest.cs @@ -0,0 +1,50 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; + +using BasicGeometry; + +namespace BasicGeometryTest.Vector2 +{ + [TestClass] + public class Vector2IsZeroTest + { + [TestMethod] + public void IsZeroFP32() + { + Vector2FP32 vector = new Vector2FP32(); + + Assert.IsTrue(vector.IsZero()); + + vector.SetValues(UtilityFP32.EPSYLON * 0.75f, 0.0f); + + Assert.IsTrue(vector.IsZero()); + + vector.SetValues(-UtilityFP32.EPSYLON * 0.5f, -UtilityFP32.EPSYLON * 0.5f); + + Assert.IsTrue(vector.IsZero()); + + vector.SetValues(-UtilityFP32.EPSYLON * 1.25f, -UtilityFP32.EPSYLON * 1.25f); + + Assert.IsFalse(vector.IsZero()); + } + + [TestMethod] + public void IsZeroFP64() + { + Vector2FP64 vector = new Vector2FP64(); + + Assert.IsTrue(vector.IsZero()); + + vector.SetValues(UtilityFP64.EPSYLON * 0.75, 0.0); + + Assert.IsTrue(vector.IsZero()); + + vector.SetValues(-UtilityFP64.EPSYLON * 0.5, UtilityFP64.EPSYLON * 0.5); + + Assert.IsTrue(vector.IsZero()); + + vector.SetValues(UtilityFP64.EPSYLON * 1.25, UtilityFP64.EPSYLON * 1.25); + + Assert.IsFalse(vector.IsZero()); + } + } +} From a072da442e6b3613cb151f5b8e5a7164d1f93962 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Fri, 14 Feb 2025 16:18:23 +0700 Subject: [PATCH 8/9] =?UTF-8?q?=D0=91=D0=BE=D0=BB=D1=8C=D1=88=D0=BE=D0=B5?= =?UTF-8?q?=20=D0=BF=D0=B5=D1=80=D0=B5=D1=83=D0=BF=D0=BE=D1=80=D1=8F=D0=B4?= =?UTF-8?q?=D0=BE=D1=87=D0=B8=D0=B2=D0=B0=D0=BD=D0=B8=D0=B5=20=D0=B8=D1=81?= =?UTF-8?q?=D1=85=D0=BE=D0=B4=D0=BD=D0=BE=D0=B3=D0=BE=20=D0=BA=D0=BE=D0=B4?= =?UTF-8?q?=D0=B0=20(=D0=BD=D0=B5=20=D0=BE=D0=BA=D0=BE=D0=BD=D1=87=D0=B5?= =?UTF-8?q?=D0=BD=D0=BE)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BasicGeometry/Angle.cs | 18 +----------------- BasicGeometry/AngleFP32.cs | 19 +------------------ BasicGeometry/AngleFP64.cs | 19 +------------------ BasicGeometry/DegreeFP32.cs | 4 ++-- BasicGeometry/DegreeFP64.cs | 4 ++-- BasicGeometry/Matrix2x2FP32.cs | 19 +------------------ BasicGeometry/Matrix2x2FP64.cs | 18 ------------------ BasicGeometry/Matrix2x3FP32.cs | 19 +------------------ BasicGeometry/Matrix2x3FP64.cs | 19 +------------------ BasicGeometry/Matrix3x2FP32.cs | 17 +---------------- BasicGeometry/Matrix3x2FP64.cs | 19 +------------------ BasicGeometry/Matrix3x3FP32.cs | 19 +------------------ BasicGeometry/Matrix3x3FP64.cs | 19 +------------------ BasicGeometry/MatrixProductFP32.cs | 20 ++------------------ BasicGeometry/MatrixProductFP64.cs | 20 ++------------------ BasicGeometry/QuaternionFP32.cs | 6 +++++- BasicGeometry/QuaternionFP64.cs | 6 +++++- BasicGeometry/RadianFP32.cs | 4 ++-- BasicGeometry/RadianFP64.cs | 4 ++-- BasicGeometry/Rotation3FP32.cs | 19 +------------------ BasicGeometry/Rotation3FP64.cs | 19 +------------------ BasicGeometry/TurnFP32.cs | 4 ++-- BasicGeometry/TurnFP64.cs | 4 ++-- BasicGeometry/UtilityFP32.cs | 6 +++++- BasicGeometry/UtilityFP64.cs | 6 +++++- BasicGeometry/Vector2FP32.cs | 19 +------------------ BasicGeometry/Vector2FP64.cs | 19 +------------------ BasicGeometry/Vector3FP32.cs | 19 +------------------ BasicGeometry/Vector3FP64.cs | 19 +------------------ BasicGeometry/VersorFP32.cs | 19 ++----------------- BasicGeometry/VersorFP64.cs | 17 +---------------- 31 files changed, 55 insertions(+), 388 deletions(-) diff --git a/BasicGeometry/Angle.cs b/BasicGeometry/Angle.cs index 8861d61..779404b 100644 --- a/BasicGeometry/Angle.cs +++ b/BasicGeometry/Angle.cs @@ -1,22 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 1 Feb 2019 */ diff --git a/BasicGeometry/AngleFP32.cs b/BasicGeometry/AngleFP32.cs index 01f5787..3a59774 100644 --- a/BasicGeometry/AngleFP32.cs +++ b/BasicGeometry/AngleFP32.cs @@ -1,23 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 1 Feb 2019 */ diff --git a/BasicGeometry/AngleFP64.cs b/BasicGeometry/AngleFP64.cs index 088b45e..9019085 100644 --- a/BasicGeometry/AngleFP64.cs +++ b/BasicGeometry/AngleFP64.cs @@ -1,23 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 1 Feb 2019 */ diff --git a/BasicGeometry/DegreeFP32.cs b/BasicGeometry/DegreeFP32.cs index a458294..09c299d 100644 --- a/BasicGeometry/DegreeFP32.cs +++ b/BasicGeometry/DegreeFP32.cs @@ -1,6 +1,6 @@ - -/* +/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 18 Nov 2024 */ diff --git a/BasicGeometry/DegreeFP64.cs b/BasicGeometry/DegreeFP64.cs index b7c1785..c47408b 100644 --- a/BasicGeometry/DegreeFP64.cs +++ b/BasicGeometry/DegreeFP64.cs @@ -1,6 +1,6 @@ - -/* +/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 18 Nov 2024 */ diff --git a/BasicGeometry/Matrix2x2FP32.cs b/BasicGeometry/Matrix2x2FP32.cs index 936471c..1a1465c 100644 --- a/BasicGeometry/Matrix2x2FP32.cs +++ b/BasicGeometry/Matrix2x2FP32.cs @@ -1,23 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 10 Feb 2019 */ diff --git a/BasicGeometry/Matrix2x2FP64.cs b/BasicGeometry/Matrix2x2FP64.cs index 4618711..e49f3ac 100644 --- a/BasicGeometry/Matrix2x2FP64.cs +++ b/BasicGeometry/Matrix2x2FP64.cs @@ -1,22 +1,4 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov * Date: 10 Feb 2019 */ diff --git a/BasicGeometry/Matrix2x3FP32.cs b/BasicGeometry/Matrix2x3FP32.cs index 87e33aa..24360db 100644 --- a/BasicGeometry/Matrix2x3FP32.cs +++ b/BasicGeometry/Matrix2x3FP32.cs @@ -1,23 +1,6 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - /* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 11 Nov 2024 */ namespace BasicGeometry diff --git a/BasicGeometry/Matrix2x3FP64.cs b/BasicGeometry/Matrix2x3FP64.cs index 8de76be..cbbf95c 100644 --- a/BasicGeometry/Matrix2x3FP64.cs +++ b/BasicGeometry/Matrix2x3FP64.cs @@ -1,23 +1,6 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - /* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 11 Nov 2024 */ namespace BasicGeometry diff --git a/BasicGeometry/Matrix3x2FP32.cs b/BasicGeometry/Matrix3x2FP32.cs index 5f1ce37..68dd59c 100644 --- a/BasicGeometry/Matrix3x2FP32.cs +++ b/BasicGeometry/Matrix3x2FP32.cs @@ -1,21 +1,6 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - /* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 11 Nov 2024 */ namespace BasicGeometry diff --git a/BasicGeometry/Matrix3x2FP64.cs b/BasicGeometry/Matrix3x2FP64.cs index c3e4862..5e5eab9 100644 --- a/BasicGeometry/Matrix3x2FP64.cs +++ b/BasicGeometry/Matrix3x2FP64.cs @@ -1,23 +1,6 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - /* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 11 Nov 2024 */ namespace BasicGeometry diff --git a/BasicGeometry/Matrix3x3FP32.cs b/BasicGeometry/Matrix3x3FP32.cs index 8201ea3..1486c50 100644 --- a/BasicGeometry/Matrix3x3FP32.cs +++ b/BasicGeometry/Matrix3x3FP32.cs @@ -1,23 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 10 Feb 2019 */ diff --git a/BasicGeometry/Matrix3x3FP64.cs b/BasicGeometry/Matrix3x3FP64.cs index b9b016e..cab0ea0 100644 --- a/BasicGeometry/Matrix3x3FP64.cs +++ b/BasicGeometry/Matrix3x3FP64.cs @@ -1,23 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 10 Feb 2019 */ diff --git a/BasicGeometry/MatrixProductFP32.cs b/BasicGeometry/MatrixProductFP32.cs index 9acb4a1..26a776a 100644 --- a/BasicGeometry/MatrixProductFP32.cs +++ b/BasicGeometry/MatrixProductFP32.cs @@ -1,25 +1,9 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - /* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 11 Nov 2024 */ + namespace BasicGeometry { public class MatrixProductFP32 diff --git a/BasicGeometry/MatrixProductFP64.cs b/BasicGeometry/MatrixProductFP64.cs index e91fd3b..718c4f3 100644 --- a/BasicGeometry/MatrixProductFP64.cs +++ b/BasicGeometry/MatrixProductFP64.cs @@ -1,25 +1,9 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - /* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 11 Nov 2024 */ + namespace BasicGeometry { public class MatrixProductFP64 diff --git a/BasicGeometry/QuaternionFP32.cs b/BasicGeometry/QuaternionFP32.cs index d4b9232..80d189b 100644 --- a/BasicGeometry/QuaternionFP32.cs +++ b/BasicGeometry/QuaternionFP32.cs @@ -1,4 +1,8 @@ -using System; +/* + * Author: Andrey Pokidov + * License: Apache-2.0 + * Date: 4 Dec 2024 + */ namespace BasicGeometry { diff --git a/BasicGeometry/QuaternionFP64.cs b/BasicGeometry/QuaternionFP64.cs index ba260a8..36f15cc 100644 --- a/BasicGeometry/QuaternionFP64.cs +++ b/BasicGeometry/QuaternionFP64.cs @@ -1,4 +1,8 @@ -using System; +/* + * Author: Andrey Pokidov + * License: Apache-2.0 + * Date: 4 Dec 2024 + */ namespace BasicGeometry { diff --git a/BasicGeometry/RadianFP32.cs b/BasicGeometry/RadianFP32.cs index 1c4a002..423a1c3 100644 --- a/BasicGeometry/RadianFP32.cs +++ b/BasicGeometry/RadianFP32.cs @@ -1,6 +1,6 @@ - -/* +/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 18 Nov 2024 */ diff --git a/BasicGeometry/RadianFP64.cs b/BasicGeometry/RadianFP64.cs index 8be238e..e8c7196 100644 --- a/BasicGeometry/RadianFP64.cs +++ b/BasicGeometry/RadianFP64.cs @@ -1,6 +1,6 @@ - -/* +/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 18 Nov 2024 */ diff --git a/BasicGeometry/Rotation3FP32.cs b/BasicGeometry/Rotation3FP32.cs index 95d6a1b..c322466 100644 --- a/BasicGeometry/Rotation3FP32.cs +++ b/BasicGeometry/Rotation3FP32.cs @@ -1,23 +1,6 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - /* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 2 Feb 2019 */ diff --git a/BasicGeometry/Rotation3FP64.cs b/BasicGeometry/Rotation3FP64.cs index 2c2de9d..e596c0b 100644 --- a/BasicGeometry/Rotation3FP64.cs +++ b/BasicGeometry/Rotation3FP64.cs @@ -1,23 +1,6 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - /* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 2 Feb 2019 */ diff --git a/BasicGeometry/TurnFP32.cs b/BasicGeometry/TurnFP32.cs index 8016396..82dbae3 100644 --- a/BasicGeometry/TurnFP32.cs +++ b/BasicGeometry/TurnFP32.cs @@ -1,6 +1,6 @@ - -/* +/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 18 Nov 2024 */ diff --git a/BasicGeometry/TurnFP64.cs b/BasicGeometry/TurnFP64.cs index 5bf15f6..2f6a004 100644 --- a/BasicGeometry/TurnFP64.cs +++ b/BasicGeometry/TurnFP64.cs @@ -1,6 +1,6 @@ - -/* +/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 18 Nov 2024 */ diff --git a/BasicGeometry/UtilityFP32.cs b/BasicGeometry/UtilityFP32.cs index a326deb..62982be 100644 --- a/BasicGeometry/UtilityFP32.cs +++ b/BasicGeometry/UtilityFP32.cs @@ -1,4 +1,8 @@ -using System; +/* + * Author: Andrey Pokidov + * License: Apache-2.0 + * Date: 12 Nov 2024 + */ namespace BasicGeometry { diff --git a/BasicGeometry/UtilityFP64.cs b/BasicGeometry/UtilityFP64.cs index ba61ee9..95cc23b 100644 --- a/BasicGeometry/UtilityFP64.cs +++ b/BasicGeometry/UtilityFP64.cs @@ -1,4 +1,8 @@ -using System; +/* + * Author: Andrey Pokidov + * License: Apache-2.0 + * Date: 12 Nov 2024 + */ namespace BasicGeometry { diff --git a/BasicGeometry/Vector2FP32.cs b/BasicGeometry/Vector2FP32.cs index 9de1992..9836ab9 100644 --- a/BasicGeometry/Vector2FP32.cs +++ b/BasicGeometry/Vector2FP32.cs @@ -1,23 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 1 Feb 2019 */ diff --git a/BasicGeometry/Vector2FP64.cs b/BasicGeometry/Vector2FP64.cs index ab25936..25be102 100644 --- a/BasicGeometry/Vector2FP64.cs +++ b/BasicGeometry/Vector2FP64.cs @@ -1,23 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 1 Feb 2019 */ diff --git a/BasicGeometry/Vector3FP32.cs b/BasicGeometry/Vector3FP32.cs index 5cd65ea..2b2119d 100644 --- a/BasicGeometry/Vector3FP32.cs +++ b/BasicGeometry/Vector3FP32.cs @@ -1,23 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 1 Feb 2019 */ diff --git a/BasicGeometry/Vector3FP64.cs b/BasicGeometry/Vector3FP64.cs index 7e42050..31505f4 100644 --- a/BasicGeometry/Vector3FP64.cs +++ b/BasicGeometry/Vector3FP64.cs @@ -1,23 +1,6 @@ /* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -using System; - -/* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 1 Feb 2019 */ diff --git a/BasicGeometry/VersorFP32.cs b/BasicGeometry/VersorFP32.cs index 3a15756..ac253ee 100644 --- a/BasicGeometry/VersorFP32.cs +++ b/BasicGeometry/VersorFP32.cs @@ -1,22 +1,7 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - /* * Author: Andrey Pokidov -* Date: 20 Oct 2024 + * License: Apache-2.0 + * Date: 20 Oct 2024 */ namespace BasicGeometry diff --git a/BasicGeometry/VersorFP64.cs b/BasicGeometry/VersorFP64.cs index 546bed0..983ac2f 100644 --- a/BasicGeometry/VersorFP64.cs +++ b/BasicGeometry/VersorFP64.cs @@ -1,21 +1,6 @@ -/* - * Copyright 2019-2025 Andrey Pokidov - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - /* * Author: Andrey Pokidov + * License: Apache-2.0 * Date: 20 Oct 2024 */ From 791fda6a7f00c3e6b5ca3de5fcdc3a053a4da6ab Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Wed, 26 Nov 2025 22:44:31 +0700 Subject: [PATCH 9/9] =?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