From 3ba55c7524bb96241799e6dca2aa6835419bbaa6 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Tue, 12 Nov 2024 01:14:44 +0700 Subject: [PATCH] =?UTF-8?q?=D0=91=D0=B0=D0=B7=D0=BE=D0=B2=D0=B0=D1=8F=20?= =?UTF-8?q?=D0=B2=D0=B5=D1=80=D1=81=D0=B8=D1=8F=20=D0=B1=D0=B8=D0=B1=D0=BB?= =?UTF-8?q?=D0=B8=D0=BE=D1=82=D0=B5=D0=BA=D0=B8.=20=D0=92=D0=B5=D1=80?= =?UTF-8?q?=D1=81=D0=B8=D1=8F=200.2.0-dev?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 6 + Geometry.Net.sln | 37 ++ Geometry/Angle.cs | 48 +++ Geometry/DPAngle.cs | 305 ++++++++++++++++ Geometry/DPMatrix2x2.cs | 407 +++++++++++++++++++++ Geometry/DPMatrix2x3.cs | 372 +++++++++++++++++++ Geometry/DPMatrix3x2.cs | 329 +++++++++++++++++ Geometry/DPMatrix3x3.cs | 602 ++++++++++++++++++++++++++++++ Geometry/DPMatrixProduct.cs | 191 ++++++++++ Geometry/DPQuaternion.cs | 201 ++++++++++ Geometry/DPUtility.cs | 20 + Geometry/DPVector2.cs | 383 +++++++++++++++++++ Geometry/DPVector3.cs | 435 ++++++++++++++++++++++ Geometry/DPVersor.cs | 359 ++++++++++++++++++ Geometry/Geometry.csproj | 16 + Geometry/SPAngle.cs | 306 ++++++++++++++++ Geometry/SPMatrix2x2.cs | 407 +++++++++++++++++++++ Geometry/SPMatrix2x3.cs | 345 ++++++++++++++++++ Geometry/SPMatrix3x2.cs | 284 +++++++++++++++ Geometry/SPMatrix3x3.cs | 606 +++++++++++++++++++++++++++++++ Geometry/SPMatrixProduct.cs | 191 ++++++++++ Geometry/SPQuaternion.cs | 203 +++++++++++ Geometry/SPRotation3.cs | 44 +++ Geometry/SPUtility.cs | 21 ++ Geometry/SPVector2.cs | 383 +++++++++++++++++++ Geometry/SPVector3.cs | 434 ++++++++++++++++++++++ Geometry/SPVersor.cs | 366 +++++++++++++++++++ GeometryDev/GeometryDev.csproj | 14 + GeometryDev/Program.cs | 80 ++++ GeometryTest/GeometryTest.csproj | 18 + GeometryTest/Vector2FloatTest.cs | 19 + 31 files changed, 7432 insertions(+) create mode 100644 .gitignore create mode 100644 Geometry.Net.sln create mode 100644 Geometry/Angle.cs create mode 100644 Geometry/DPAngle.cs create mode 100644 Geometry/DPMatrix2x2.cs create mode 100644 Geometry/DPMatrix2x3.cs create mode 100644 Geometry/DPMatrix3x2.cs create mode 100644 Geometry/DPMatrix3x3.cs create mode 100644 Geometry/DPMatrixProduct.cs create mode 100644 Geometry/DPQuaternion.cs create mode 100644 Geometry/DPUtility.cs create mode 100644 Geometry/DPVector2.cs create mode 100644 Geometry/DPVector3.cs create mode 100644 Geometry/DPVersor.cs create mode 100644 Geometry/Geometry.csproj create mode 100644 Geometry/SPAngle.cs create mode 100644 Geometry/SPMatrix2x2.cs create mode 100644 Geometry/SPMatrix2x3.cs create mode 100644 Geometry/SPMatrix3x2.cs create mode 100644 Geometry/SPMatrix3x3.cs create mode 100644 Geometry/SPMatrixProduct.cs create mode 100644 Geometry/SPQuaternion.cs create mode 100644 Geometry/SPRotation3.cs create mode 100644 Geometry/SPUtility.cs create mode 100644 Geometry/SPVector2.cs create mode 100644 Geometry/SPVector3.cs create mode 100644 Geometry/SPVersor.cs create mode 100644 GeometryDev/GeometryDev.csproj create mode 100644 GeometryDev/Program.cs create mode 100644 GeometryTest/GeometryTest.csproj create mode 100644 GeometryTest/Vector2FloatTest.cs diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a096172 --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +.vs +.vscode +bin +obj +Debug +Release diff --git a/Geometry.Net.sln b/Geometry.Net.sln new file mode 100644 index 0000000..04ef63b --- /dev/null +++ b/Geometry.Net.sln @@ -0,0 +1,37 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.0.31903.59 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Geometry", "Geometry\Geometry.csproj", "{D1869DF0-7B61-4B6F-8C66-6EEF3916FE0A}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "GeometryDev", "GeometryDev\GeometryDev.csproj", "{3D09FF57-02E6-449D-9DE7-0843633FCBA0}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "GeometryTest", "GeometryTest\GeometryTest.csproj", "{51A07B27-43FF-4A12-AEC1-50D32EDA3815}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {D1869DF0-7B61-4B6F-8C66-6EEF3916FE0A}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D1869DF0-7B61-4B6F-8C66-6EEF3916FE0A}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D1869DF0-7B61-4B6F-8C66-6EEF3916FE0A}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D1869DF0-7B61-4B6F-8C66-6EEF3916FE0A}.Release|Any CPU.Build.0 = Release|Any CPU + {3D09FF57-02E6-449D-9DE7-0843633FCBA0}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {3D09FF57-02E6-449D-9DE7-0843633FCBA0}.Debug|Any CPU.Build.0 = Debug|Any CPU + {3D09FF57-02E6-449D-9DE7-0843633FCBA0}.Release|Any CPU.ActiveCfg = Release|Any CPU + {3D09FF57-02E6-449D-9DE7-0843633FCBA0}.Release|Any CPU.Build.0 = Release|Any CPU + {51A07B27-43FF-4A12-AEC1-50D32EDA3815}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {51A07B27-43FF-4A12-AEC1-50D32EDA3815}.Debug|Any CPU.Build.0 = Debug|Any CPU + {51A07B27-43FF-4A12-AEC1-50D32EDA3815}.Release|Any CPU.ActiveCfg = Release|Any CPU + {51A07B27-43FF-4A12-AEC1-50D32EDA3815}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {040DE977-0DDC-4EEA-8EBC-CBCA41289BBE} + EndGlobalSection +EndGlobal diff --git a/Geometry/Angle.cs b/Geometry/Angle.cs new file mode 100644 index 0000000..fcaed8c --- /dev/null +++ b/Geometry/Angle.cs @@ -0,0 +1,48 @@ +/* + * 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: 1 Feb 2019 + */ + +namespace Geometry +{ + public enum AngleUnit + { + RADIANS = 1, + DEGREES = 2, + TURNS = 3, + + } + + public enum AngleRange + { + /// + /// The measure of an angle with a range of: + /// [0, 360) degrees, [0, 2xPI) radians, [0, 1) turns, [0, 400) gradians + /// + UNSIGNED_RANGE = 1, + + /// + /// The measure of an angle with a range of: + /// (-180, 180] degrees, (-PI, PI] radians, (-0.5, 0.5] turns, (-200, 200] gradians + /// + SIGNED_RANGE = 2 + } +} + diff --git a/Geometry/DPAngle.cs b/Geometry/DPAngle.cs new file mode 100644 index 0000000..89e9356 --- /dev/null +++ b/Geometry/DPAngle.cs @@ -0,0 +1,305 @@ +/* + * 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: 1 Feb 2019 + */ + +namespace Geometry +{ + public static class DPAngle + { + public const double PI = 3.14159265358979324; + public const double TWO_PI = 6.28318530717958648; + public const double HALF_OF_PI = 1.57079632679489662; + public const double THIRD_OF_PI = 1.04719755119659775; + public const double FOURTH_OF_PI = 0.78539816339744831; + public const double SIXTH_OF_PI = 0.523598775598298873; + + public const double DEGREES_IN_RADIAN = 57.2957795130823209; + public const double TURNS_IN_RADIAN = 0.159154943091895336; + public const double RADIANS_IN_DEGREE = 1.74532925199432958E-2; + public const double TURNS_IN_DEGREE = 2.77777777777777778E-3; + + public static double RadiansToDegrees(double radians) + { + return radians * DEGREES_IN_RADIAN; + } + + public static double RadiansToTurns(double radians) + { + return radians * TURNS_IN_RADIAN; + } + + public static double DegreesToRadians(double degrees) + { + return degrees * RADIANS_IN_DEGREE; + } + + public static double DegreesToTurns(double degrees) + { + return degrees * TURNS_IN_DEGREE; + } + + public static double TurnsToRadians(double turns) + { + return turns * TWO_PI; + } + + public static double TurnsToDegrees(double turns) + { + return turns * 360.0; + } + + public static double ConvertFromRadians(double radians, AngleUnit toUnit) + { + if (toUnit == AngleUnit.DEGREES) + { + return radians * DEGREES_IN_RADIAN; + } + + if (toUnit == AngleUnit.TURNS) + { + return radians * TURNS_IN_RADIAN; + } + + return radians; + } + + public static double ConvertFromDegrees(double degrees, AngleUnit toUnit) + { + if (toUnit == AngleUnit.RADIANS) + { + return degrees * RADIANS_IN_DEGREE; + } + + if (toUnit == AngleUnit.TURNS) + { + return degrees * TURNS_IN_DEGREE; + } + + return degrees; + } + + public static double ConvertFromTurns(double turns, AngleUnit toUnit) + { + if (toUnit == AngleUnit.RADIANS) + { + return turns * TWO_PI; + } + + if (toUnit == AngleUnit.DEGREES) + { + return turns * 360.0; + } + + return turns; + } + public static double ConvertToRadians(double angle, AngleUnit unit) + { + if (unit == AngleUnit.DEGREES) + { + return angle * RADIANS_IN_DEGREE; + } + + if (unit == AngleUnit.TURNS) + { + return angle * TWO_PI; + } + + return angle; + } + + public static double ConvertToDegrees(double angle, AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return angle * DEGREES_IN_RADIAN; + } + + if (unit == AngleUnit.TURNS) + { + return angle * 360.0; + } + + return angle; + } + + public static double ConvertToTurns(double angle, AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return angle * TURNS_IN_RADIAN; + } + + if (unit == AngleUnit.DEGREES) + { + return angle * TURNS_IN_DEGREE; + } + + return angle; + } + + public static double GetFullCircle(AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return TWO_PI; + } + + if (unit == AngleUnit.DEGREES) + { + return 360.0; + } + + return 1.0; + } + + public static double GetHalfCircle(AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return PI; + } + + if (unit == AngleUnit.DEGREES) + { + return 180.0; + } + + return 0.5; + } + + public static double GetQuarterCircle(AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return HALF_OF_PI; + } + + if (unit == AngleUnit.DEGREES) + { + return 90.0; + } + + return 0.25; + } + + public static double NormalizeRadians(double radians, AngleRange range) + { + if (range == AngleRange.UNSIGNED_RANGE) + { + if (0.0 <= radians && radians < TWO_PI) + { + return radians; + } + + } + else + { + if (-PI < radians && radians <= PI) + { + return radians; + } + } + + double turns = radians * TURNS_IN_RADIAN; + + turns -= Math.Floor(turns); + + if (range == AngleRange.SIGNED_RANGE && turns > 0.5) + { + turns -= 1.0; + } + + return turns * TWO_PI; + } + + public static double NormalizeDegrees(double degrees, AngleRange range) + { + if (range == AngleRange.UNSIGNED_RANGE) + { + if (0.0 <= degrees && degrees < 360.0) + { + return degrees; + } + } + else + { + if (-180.0 < degrees && degrees <= 180.0) + { + return degrees; + } + } + + double turns = degrees * TURNS_IN_DEGREE; + + turns -= Math.Floor(turns); + + if (range == AngleRange.SIGNED_RANGE && turns > 0.5) + { + turns -= 1.0; + } + + return turns * 360.0; + } + + public static double NormalizeTurns(double turns, AngleRange range) + { + if (range == AngleRange.UNSIGNED_RANGE) + { + if (0.0 <= turns && turns < 1.0) + { + return turns; + } + } + else + { + if (-0.5 < turns && turns <= 0.5) + { + return turns; + } + } + + double rest = turns - Math.Floor(turns); + + if (range == AngleRange.SIGNED_RANGE && rest > 0.5) + { + rest -= 1.0; + } + + return rest; + } + + public static double Normalize(double angle, AngleUnit unit, AngleRange range) + { + if (unit == AngleUnit.RADIANS) + { + return NormalizeRadians(angle, range); + } + + if (unit == AngleUnit.DEGREES) + { + return NormalizeDegrees(angle, range); + } + + return NormalizeTurns(angle, range); + } + } +} diff --git a/Geometry/DPMatrix2x2.cs b/Geometry/DPMatrix2x2.cs new file mode 100644 index 0000000..8664be5 --- /dev/null +++ b/Geometry/DPMatrix2x2.cs @@ -0,0 +1,407 @@ +/* + * 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 + */ + +namespace Geometry +{ + public struct DPMatrix2x2 + { + public double r1c1, r1c2; + + public double r2c1, r2c2; + + public DPMatrix2x2() + { + this.r1c1 = 0.0; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + } + + public DPMatrix2x2(double d1, double d2) + { + this.r1c1 = d1; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = d2; + } + + public DPMatrix2x2(in DPMatrix2x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + } + + public DPMatrix2x2(in SPMatrix2x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + } + + public readonly double GetDeterminant() + { + return this.r1c1 * this.r2c2 - this.r1c2 * this.r2c1; + } + + public readonly bool IsSingular() + { + double determinant = this.GetDeterminant(); + return -DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON; + } + + public void Reset() + { + this.r1c1 = 0.0; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + } + + public void MakeIdentity() + { + this.r1c1 = 1.0; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 1.0; + } + + public void MakeDiagonal(double d1, double d2) + { + this.r1c1 = d1; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = d2; + } + + public void Transpose() + { + (this.r1c2, this.r2c1) = (this.r2c1, this.r1c2); + } + + public bool Invert() + { + double determinant = this.GetDeterminant(); + + if (-DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON) + { + return false; + } + + double r1c1 = this.r2c2; + double r1c2 = -this.r1c2; + + double r2c1 = -this.r2c1; + double r2c2 = this.r1c1; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + + return true; + } + + public void MakeTransposedOf(in DPMatrix2x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r2c2 = matrix.r2c2; + (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); + } + + public void MakeTransposedOf(in SPMatrix2x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + } + + public bool MakeInvertedOf(in DPMatrix2x2 matrix) + { + double determinant = matrix.GetDeterminant(); + + if (-DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON) + { + return false; + } + + double r1c1 = matrix.r2c2; + double r1c2 = -matrix.r1c2; + + double r2c1 = -matrix.r2c1; + double r2c2 = matrix.r1c1; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + + return true; + } + + public void SetValues(in DPMatrix2x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + } + + public void SetValues(in SPMatrix2x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + } + + public void SetMainDiagonal(double d1, double d2) + { + this.r1c1 = d1; + this.r2c2 = d2; + } + + public void SetRow1(double c1, double c2) + { + this.r1c1 = c1; + this.r1c2 = c2; + } + + public void SetRow2(double c1, double c2) + { + this.r2c1 = c1; + this.r2c2 = c2; + } + + public void SetColumn1(double r1, double r2) + { + this.r1c1 = r1; + this.r2c1 = r2; + } + + public void SetColumn2(double r1, double r2) + { + this.r1c2 = r1; + this.r2c2 = r2; + } + + public static void Add(in DPMatrix2x2 matrix1, in DPMatrix2x2 matrix2, out DPMatrix2x2 sum) + { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2; + } + + public static void Add3( + in DPMatrix2x2 matrix1, + in DPMatrix2x2 matrix2, + in DPMatrix2x2 matrix3, + out DPMatrix2x2 sum + ) + { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; + } + + public static void Add4( + in DPMatrix2x2 matrix1, + in DPMatrix2x2 matrix2, + in DPMatrix2x2 matrix3, + in DPMatrix2x2 matrix4, + out DPMatrix2x2 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); + } + + public static void Add5( + in DPMatrix2x2 matrix1, + in DPMatrix2x2 matrix2, + in DPMatrix2x2 matrix3, + in DPMatrix2x2 matrix4, + in DPMatrix2x2 matrix5, + out DPMatrix2x2 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; + } + + public static void Subtract(in DPMatrix2x2 minuend, in DPMatrix2x2 subtrahend, out DPMatrix2x2 difference) + { + difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; + difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; + + difference.r2c1 = minuend.r2c1 - subtrahend.r2c1; + difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; + } + + public static void GetWeightedSum2( + double weight1, in DPMatrix2x2 matrix1, + double weight2, in DPMatrix2x2 matrix2, + out DPMatrix2x2 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + } + + public static void GetWeightedSum3( + float weight1, in SPMatrix2x2 matrix1, + float weight2, in SPMatrix2x2 matrix2, + float weight3, in SPMatrix2x2 matrix3, + out DPMatrix2x2 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + } + + public static void GetWeightedSum4( + float weight1, in SPMatrix2x2 matrix1, + float weight2, in SPMatrix2x2 matrix2, + float weight3, in SPMatrix2x2 matrix3, + float weight4, in SPMatrix2x2 matrix4, + out DPMatrix2x2 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + } + + public static void GetWeightedSum5( + double weight1, in DPMatrix2x2 matrix1, + double weight2, in DPMatrix2x2 matrix2, + double weight3, in DPMatrix2x2 matrix3, + double weight4, in DPMatrix2x2 matrix4, + double weight5, in DPMatrix2x2 matrix5, + out DPMatrix2x2 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + } + + public static void Multiply(in DPMatrix2x2 multiplicand, double multiplier, out DPMatrix2x2 product) + { + product.r1c1 = multiplicand.r1c1 * multiplier; + product.r1c2 = multiplicand.r1c2 * multiplier; + + product.r2c1 = multiplicand.r2c1 * multiplier; + product.r2c2 = multiplicand.r2c2 * multiplier; + } + + public static void Divide(in DPMatrix2x2 dividend, double divisor, out DPMatrix2x2 quotient) + { + quotient.r1c1 = dividend.r1c1 / divisor; + quotient.r1c2 = dividend.r1c2 / divisor; + + quotient.r2c1 = dividend.r2c1 / divisor; + quotient.r2c2 = dividend.r2c2 / divisor; + } + + public static void GetRightProduct(in DPMatrix2x2 matrix, in DPVector2 vector, out DPVector2 result) + { + double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; + double x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2; + + result.x1 = x1; + result.x2 = x2; + } + + public static void GetLeftProduct(in DPVector2 vector, in DPMatrix2x2 matrix, out DPVector2 result) + { + double x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1; + double x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2; + + result.x1 = x1; + result.x2 = x2; + } + + public static void LoadZero(out DPMatrix2x2 matrix) + { + matrix.r1c1 = 0.0; + matrix.r1c2 = 0.0; + + matrix.r2c1 = 0.0; + matrix.r2c2 = 0.0; + } + + public static void LoadIdentity(out DPMatrix2x2 matrix) + { + matrix.r1c1 = 1.0; + matrix.r1c2 = 0.0; + + matrix.r2c1 = 0.0; + matrix.r2c2 = 1.0; + } + + public static void LoadDiagonal(double d1, double d2, out DPMatrix2x2 matrix) + { + matrix.r1c1 = d1; + matrix.r1c2 = 0.0; + + matrix.r2c1 = 0.0; + matrix.r2c2 = d2; + } + } +} diff --git a/Geometry/DPMatrix2x3.cs b/Geometry/DPMatrix2x3.cs new file mode 100644 index 0000000..eb2eaa5 --- /dev/null +++ b/Geometry/DPMatrix2x3.cs @@ -0,0 +1,372 @@ +/* + * 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: 11 Nov 2024 + */ +namespace Geometry +{ + public struct DPMatrix2x3 + { + public double r1c1, r1c2; + public double r2c1, r2c2; + public double r3c1, r3c2; + + public DPMatrix2x3() { + this.r1c1 = 0.0; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + + this.r3c1 = 0.0; + this.r3c2 = 0.0; + } + + public DPMatrix2x3(in DPMatrix2x3 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + } + + public DPMatrix2x3(in SPMatrix2x3 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + } + + public DPMatrix2x3(in DPMatrix3x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r1c3; + this.r3c2 = matrix.r2c3; + } + + public DPMatrix2x3(in SPMatrix3x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r1c3; + this.r3c2 = matrix.r2c3; + } + + public void Reset() + { + this.r1c1 = 0.0; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + + this.r3c1 = 0.0; + this.r3c2 = 0.0; + } + + public void SetValues(in DPMatrix2x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + } + + public void SetValues(in SPMatrix2x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + } + + public void SetTransposed(in DPMatrix3x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r1c3; + this.r3c2 = matrix.r2c3; + } + + public void SetTransposed(in SPMatrix3x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r1c3; + this.r3c2 = matrix.r2c3; + } + + public void SetRow1(double c1, double c2) + { + this.r1c1 = c1; + this.r1c2 = c2; + } + + public void SetRow2(double c1, double c2) + { + this.r2c1 = c1; + this.r2c2 = c2; + } + + public void SetRow3(double c1, double c2) + { + this.r3c1 = c1; + this.r3c2 = c2; + } + + public void SetColumn1(double r1, double r2, double r3) + { + this.r1c1 = r1; + this.r2c1 = r2; + this.r3c1 = r3; + } + + public void SetColumn2(double r1, double r2, double r3) + { + this.r1c2 = r1; + this.r2c2 = r2; + this.r3c2 = r3; + } + + public static void Add(in DPMatrix2x3 matrix1, in DPMatrix2x3 matrix2, out DPMatrix2x3 sum) + { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2; + + sum.r3c1 = matrix1.r3c1 + matrix2.r3c1; + sum.r3c2 = matrix1.r3c2 + matrix2.r3c2; + } + + public static void Add3( + in DPMatrix2x3 matrix1, + in DPMatrix2x3 matrix2, + in DPMatrix2x3 matrix3, + out DPMatrix2x3 sum + ) + { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; + + sum.r3c1 = matrix1.r3c1 + matrix2.r3c1 + matrix3.r3c1; + sum.r3c2 = matrix1.r3c2 + matrix2.r3c2 + matrix3.r3c2; + } + + public static void Add4( + in DPMatrix2x3 matrix1, + in DPMatrix2x3 matrix2, + in DPMatrix2x3 matrix3, + in DPMatrix2x3 matrix4, + out DPMatrix2x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); + + sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1); + sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2); + } + + public static void Add5( + in DPMatrix2x3 matrix1, + in DPMatrix2x3 matrix2, + in DPMatrix2x3 matrix3, + in DPMatrix2x3 matrix4, + in DPMatrix2x3 matrix5, + out DPMatrix2x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; + + sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1) + matrix5.r3c1; + sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2) + matrix5.r3c2; + } + + public static void Subtract(in DPMatrix2x3 minuend, in DPMatrix2x3 subtrahend, out DPMatrix2x3 difference) + { + difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; + difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; + + difference.r2c1 = minuend.r2c1 - subtrahend.r2c1; + difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; + + difference.r3c1 = minuend.r3c1 - subtrahend.r3c1; + difference.r3c2 = minuend.r3c2 - subtrahend.r3c2; + } + + public static void GetWeightedSum2( + double weight1, in DPMatrix2x3 matrix1, + double weight2, in DPMatrix2x3 matrix2, + out DPMatrix2x3 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; + } + + public static void GetWeightedSum3( + double weight1, in DPMatrix2x3 matrix1, + double weight2, in DPMatrix2x3 matrix2, + double weight3, in DPMatrix2x3 matrix3, + out DPMatrix2x3 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; + } + + public static void GetWeightedSum4( + double weight1, in DPMatrix2x3 matrix1, + double weight2, in DPMatrix2x3 matrix2, + double weight3, in DPMatrix2x3 matrix3, + double weight4, in DPMatrix2x3 matrix4, + out DPMatrix2x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); + } + + public static void GetWeightedSum5( + double weight1, in DPMatrix2x3 matrix1, + double weight2, in DPMatrix2x3 matrix2, + double weight3, in DPMatrix2x3 matrix3, + double weight4, in DPMatrix2x3 matrix4, + double weight5, in DPMatrix2x3 matrix5, + out DPMatrix2x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; + } + + public static void Multiply(in DPMatrix2x3 multiplicand, double multiplier, out DPMatrix2x3 product) + { + product.r1c1 = multiplicand.r1c1 * multiplier; + product.r1c2 = multiplicand.r1c2 * multiplier; + + product.r2c1 = multiplicand.r2c1 * multiplier; + product.r2c2 = multiplicand.r2c2 * multiplier; + + product.r3c1 = multiplicand.r3c1 * multiplier; + product.r3c2 = multiplicand.r3c2 * multiplier; + } + + public static void Divide(in DPMatrix2x3 dividend, double divisor, out DPMatrix2x3 quotient) + { + quotient.r1c1 = dividend.r1c1 / divisor; + quotient.r1c2 = dividend.r1c2 / divisor; + + quotient.r2c1 = dividend.r2c1 / divisor; + quotient.r2c2 = dividend.r2c2 / divisor; + + quotient.r3c1 = dividend.r3c1 / divisor; + quotient.r3c2 = dividend.r3c2 / divisor; + } + + public static void GetRightProduct(in DPMatrix2x3 matrix, in DPVector2 vector, out DPVector3 result) + { + double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; + double x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2; + double x3 = matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2; + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + public static void GetLeftProduct(in DPVector3 vector, in DPMatrix2x3 matrix, out DPVector2 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; + + result.x1 = x1; + result.x2 = x2; + } + } +} diff --git a/Geometry/DPMatrix3x2.cs b/Geometry/DPMatrix3x2.cs new file mode 100644 index 0000000..83627cf --- /dev/null +++ b/Geometry/DPMatrix3x2.cs @@ -0,0 +1,329 @@ +/* + * 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: 11 Nov 2024 + */ +namespace Geometry +{ +public struct DPMatrix3x2 +{ + + public double r1c1, r1c2, r1c3; + + public double r2c1, r2c2, r2c3; + + public DPMatrix3x2() + { + this.r1c1 = 0.0; + this.r1c2 = 0.0; + this.r1c3 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + this.r2c3 = 0.0; + } + + public DPMatrix3x2(in DPMatrix3x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + } + + public DPMatrix3x2(in SPMatrix3x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + } + + public DPMatrix3x2(in DPMatrix2x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + this.r1c3 = matrix.r3c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c2; + } + + public DPMatrix3x2(in SPMatrix2x3 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + this.r1c3 = matrix.r3c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c2; + } + + public void Reset() { + this.r1c1 = 0.0; + this.r1c2 = 0.0; + this.r1c3 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + this.r2c3 = 0.0; + } + + + public void SetValues(in DPMatrix3x2 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + } + + public void SetValues(SPMatrix3x2 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + } + + public void SetTransposed(DPMatrix2x3 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + this.r1c3 = matrix.r3c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c2; + } + + public void SetTransposed(SPMatrix2x3 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + this.r1c3 = matrix.r3c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c2; + } + + public void SetRow1(double c1, double c2, double c3) { + this.r1c1 = c1; + this.r1c2 = c2; + this.r1c3 = c3; + } + + public void SetRow2(double c1, double c2, double c3) { + this.r2c1 = c1; + this.r2c2 = c2; + this.r2c3 = c3; + } + + public void SetColumn1(double r1, double r2) { + this.r1c1 = r1; + this.r2c1 = r2; + } + + public void SetColumn2(double r1, double r2) { + this.r1c3 = r1; + this.r2c3 = r2; + } + + public void SetColumn3(double r1, double r2) { + this.r1c3 = r1; + this.r2c3 = r2; + } + + public static void Add(DPMatrix3x2 matrix1, DPMatrix3x2 matrix2, DPMatrix3x2 sum) { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; + sum.r1c3 = matrix1.r1c3 + matrix2.r1c3; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2; + sum.r2c3 = matrix1.r2c3 + matrix2.r2c3; + } + + public static void Add3( + DPMatrix3x2 matrix1, + DPMatrix3x2 matrix2, + DPMatrix3x2 matrix3, + DPMatrix3x2 sum + ) { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; + sum.r1c3 = matrix1.r1c3 + matrix2.r1c3 + matrix3.r1c3; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; + sum.r2c3 = matrix1.r2c3 + matrix2.r2c3 + matrix3.r2c3; + } + + public static void Add4( + DPMatrix3x2 matrix1, + DPMatrix3x2 matrix2, + DPMatrix3x2 matrix3, + DPMatrix3x2 matrix4, + DPMatrix3x2 sum + ) { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); + sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3); + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); + sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3); + } + + public static void Add5( + DPMatrix3x2 matrix1, + DPMatrix3x2 matrix2, + DPMatrix3x2 matrix3, + DPMatrix3x2 matrix4, + DPMatrix3x2 matrix5, + DPMatrix3x2 sum + ) { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; + sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3) + matrix5.r1c3; + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; + sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3) + matrix5.r2c3; + } + + public static void Subtract(DPMatrix3x2 minuend, DPMatrix3x2 subtrahend, DPMatrix3x2 difference) { + difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; + difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; + difference.r1c3 = minuend.r1c3 - subtrahend.r1c3; + + difference.r2c1 = minuend.r2c1 - subtrahend.r2c1; + difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; + difference.r2c3 = minuend.r2c3 - subtrahend.r2c3; + } + + public static void GetWeightedSum2( + double weight1, DPMatrix3x2 matrix1, + double weight2, DPMatrix3x2 matrix2, + DPMatrix3x2 sum + ) { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; + } + + public static void GetWeightedSum3( + double weight1, DPMatrix3x2 matrix1, + double weight2, DPMatrix3x2 matrix2, + double weight3, DPMatrix3x2 matrix3, + DPMatrix3x2 sum + ) { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2 + matrix3.r1c3 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; + } + + public static void GetWeightedSum4( + double weight1, DPMatrix3x2 matrix1, + double weight2, DPMatrix3x2 matrix2, + double weight3, DPMatrix3x2 matrix3, + double weight4, DPMatrix3x2 matrix4, + DPMatrix3x2 sum + ) { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); + } + + public static void GetWeightedSum5( + double weight1, DPMatrix3x2 matrix1, + double weight2, DPMatrix3x2 matrix2, + double weight3, DPMatrix3x2 matrix3, + double weight4, DPMatrix3x2 matrix4, + double weight5, DPMatrix3x2 matrix5, + DPMatrix3x2 sum + ) { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4) + matrix5.r1c3 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4) + matrix5.r2c3 * weight5; + } + + public static void Multiply(DPMatrix3x2 multiplicand, double multiplier, DPMatrix3x2 product) { + product.r1c1 = multiplicand.r1c1 * multiplier; + product.r1c2 = multiplicand.r1c2 * multiplier; + product.r1c3 = multiplicand.r1c3 * multiplier; + + product.r2c1 = multiplicand.r2c1 * multiplier; + product.r2c2 = multiplicand.r2c2 * multiplier; + product.r2c3 = multiplicand.r2c3 * multiplier; + } + + public static void Divide(DPMatrix3x2 dividend, double divisor, DPMatrix3x2 quotient) { + quotient.r1c1 = dividend.r1c1 / divisor; + quotient.r1c2 = dividend.r1c2 / divisor; + quotient.r1c3 = dividend.r1c3 / divisor; + + quotient.r2c1 = dividend.r2c1 / divisor; + quotient.r2c2 = dividend.r2c2 / divisor; + quotient.r2c3 = dividend.r2c3 / divisor; + } + + public static void GetRightProduct(DPMatrix3x2 matrix, DPVector3 vector, DPVector2 result) { + result.SetValues( + matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3, + matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3 + ); + } + + public static void GetLeftProduct(DPVector2 vector, DPMatrix3x2 matrix, DPVector3 result) { + result.SetValues( + vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1, + vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2, + vector.x1 * matrix.r1c3 + vector.x2 * matrix.r2c3 + ); + } +} +} diff --git a/Geometry/DPMatrix3x3.cs b/Geometry/DPMatrix3x3.cs new file mode 100644 index 0000000..16b547a --- /dev/null +++ b/Geometry/DPMatrix3x3.cs @@ -0,0 +1,602 @@ +/* + * 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 + */ + +namespace Geometry +{ + public struct DPMatrix3x3 + { + public double r1c1, r1c2, r1c3; + + public double r2c1, r2c2, r2c3; + + public double r3c1, r3c2, r3c3; + + public DPMatrix3x3() + { + this.r1c1 = 0.0; + this.r1c2 = 0.0; + this.r1c3 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + this.r2c3 = 0.0; + + this.r3c1 = 0.0; + this.r3c2 = 0.0; + this.r3c3 = 0.0; + } + + public DPMatrix3x3(double d1, double d2, double d3) + { + this.r1c1 = d1; + this.r1c2 = 0.0; + this.r1c3 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = d2; + this.r2c3 = 0.0; + + this.r3c1 = 0.0; + this.r3c2 = 0.0; + this.r3c3 = d3; + } + + public DPMatrix3x3(in DPMatrix3x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + this.r3c3 = matrix.r3c3; + } + + public DPMatrix3x3(in SPMatrix3x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + this.r3c3 = matrix.r3c3; + } + + public readonly double GetDeterminant() + { + return this.r1c1 * (this.r2c2 * this.r3c3 - this.r2c3 * this.r3c2) + + this.r1c2 * (this.r2c3 * this.r3c1 - this.r2c1 * this.r3c3) + + this.r1c3 * (this.r2c1 * this.r3c2 - this.r2c2 * this.r3c1); + } + + public readonly bool IsSingular() + { + double determinant = this.GetDeterminant(); + return -DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON; + } + + public void Reset() + { + this.r1c1 = 0.0; + this.r1c2 = 0.0; + this.r1c3 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + this.r2c3 = 0.0; + + this.r3c1 = 0.0; + this.r3c2 = 0.0; + this.r3c3 = 0.0; + } + + public void MakeIdentity() + { + this.r1c1 = 1.0; + this.r1c2 = 0.0; + this.r1c3 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 1.0; + this.r2c3 = 0.0; + + this.r3c1 = 0.0; + this.r3c2 = 0.0; + this.r3c3 = 1.0; + } + + public void MakeDiagonal(double d1, double d2, double d3) + { + this.r1c1 = d1; + this.r1c2 = 0.0; + this.r1c3 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = d2; + this.r2c3 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + this.r2c3 = d3; + } + + public void Transpose() + { + (this.r1c2, this.r2c1) = (this.r2c1, this.r1c2); + (this.r1c3, this.r3c1) = (this.r3c1, this.r1c3); + (this.r2c3, this.r3c2) = (this.r3c2, this.r2c3); + } + + public bool Invert() + { + double determinant = this.GetDeterminant(); + + if (-DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON) { + return false; + } + + double r1c1 = this.r2c2 * this.r3c3 - this.r2c3 * this.r3c2; + double r1c2 = this.r1c3 * this.r3c2 - this.r1c2 * this.r3c3; + double r1c3 = this.r1c2 * this.r2c3 - this.r1c3 * this.r2c2; + + double r2c1 = this.r2c3 * this.r3c1 - this.r2c1 * this.r3c3; + double r2c2 = this.r1c1 * this.r3c3 - this.r1c3 * this.r3c1; + double r2c3 = this.r1c3 * this.r2c1 - this.r1c1 * this.r2c3; + + double r3c1 = this.r2c1 * this.r3c2 - this.r2c2 * this.r3c1; + double r3c2 = this.r1c2 * this.r3c1 - this.r1c1 * this.r3c2; + double r3c3 = this.r1c1 * this.r2c2 - this.r1c2 * this.r2c1; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + this.r1c3 = r1c3 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + this.r2c3 = r2c3 / determinant; + + this.r3c1 = r3c1 / determinant; + this.r3c2 = r3c2 / determinant; + this.r3c3 = r3c3 / determinant; + + return true; + } + + public void MakeTransposedOf(in DPMatrix3x3 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 MakeTransposedOf(in SPMatrix3x3 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 MakeInvertedOf(in DPMatrix3x3 matrix) + { + double determinant = matrix.GetDeterminant(); + + if (-DPUtility.EPSYLON <= determinant && determinant <= DPUtility.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; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + this.r1c3 = r1c3 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + this.r2c3 = r2c3 / determinant; + + this.r3c1 = r3c1 / determinant; + this.r3c2 = r3c2 / determinant; + this.r3c3 = r3c3 / determinant; + + return true; + } + + public void SetValues(DPMatrix3x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + this.r3c3 = matrix.r3c3; + } + + public void SetValues(SPMatrix3x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + this.r3c3 = matrix.r3c3; + } + + public void SetMainDiagonal(double d1, double d2, double d3) + { + this.r1c1 = d1; + this.r2c2 = d2; + this.r3c3 = d3; + } + + public void SetRow1(double c1, double c2, double c3) + { + this.r1c1 = c1; + this.r1c2 = c2; + this.r1c3 = c3; + } + + public void SetRow2(double c1, double c2, double c3) + { + this.r2c1 = c1; + this.r2c2 = c2; + this.r2c3 = c3; + } + + public void SetRow3(double c1, double c2, double c3) + { + this.r3c1 = c1; + this.r3c2 = c2; + this.r3c3 = c3; + } + + public void SetColumn1(double r1, double r2, double r3) + { + this.r1c1 = r1; + this.r2c1 = r2; + this.r3c1 = r3; + } + + public void SetColumn2(double r1, double r2, double r3) + { + this.r1c3 = r1; + this.r2c3 = r2; + this.r3c3 = r3; + } + + public void SetColumn3(double r1, double r2, double r3) + { + this.r1c3 = r1; + this.r2c3 = r2; + this.r3c3 = r3; + } + + public static void Add(in DPMatrix3x3 matrix1, in DPMatrix3x3 matrix2, out DPMatrix3x3 sum) + { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; + sum.r1c3 = matrix1.r1c3 + matrix2.r1c3; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2; + sum.r2c3 = matrix1.r2c3 + matrix2.r2c3; + + sum.r3c1 = matrix1.r3c1 + matrix2.r3c1; + sum.r3c2 = matrix1.r3c2 + matrix2.r3c2; + sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; + } + + public static void Add3( + in DPMatrix3x3 matrix1, + in DPMatrix3x3 matrix2, + in DPMatrix3x3 matrix3, + out DPMatrix3x3 sum + ) + { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; + sum.r1c3 = matrix1.r1c3 + matrix2.r1c3 + matrix3.r1c3; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; + sum.r2c3 = matrix1.r2c3 + matrix2.r2c3 + matrix3.r2c3; + + sum.r3c1 = matrix1.r3c1 + matrix2.r3c1 + matrix3.r3c1; + sum.r3c2 = matrix1.r3c2 + matrix2.r3c2 + matrix3.r3c2; + sum.r3c3 = matrix1.r3c3 + matrix2.r3c3 + matrix3.r3c3; + } + + public static void Add4( + in DPMatrix3x3 matrix1, + in DPMatrix3x3 matrix2, + in DPMatrix3x3 matrix3, + in DPMatrix3x3 matrix4, + out DPMatrix3x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); + sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3); + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); + sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3); + + sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1); + sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2); + sum.r3c3 = (matrix1.r3c3 + matrix2.r3c3) + (matrix3.r3c3 + matrix4.r3c3); + } + + public static void Add5( + in DPMatrix3x3 matrix1, + in DPMatrix3x3 matrix2, + in DPMatrix3x3 matrix3, + in DPMatrix3x3 matrix4, + in DPMatrix3x3 matrix5, + out DPMatrix3x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; + sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3) + matrix5.r1c3; + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; + sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3) + matrix5.r2c3; + + sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1) + matrix5.r3c1; + sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2) + matrix5.r3c2; + sum.r3c3 = (matrix1.r3c3 + matrix2.r3c3) + (matrix3.r3c3 + matrix4.r3c3) + matrix5.r3c3; + } + + public static void Subtract(in DPMatrix3x3 minuend, in DPMatrix3x3 subtrahend, out DPMatrix3x3 difference) + { + difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; + difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; + difference.r1c3 = minuend.r1c3 - subtrahend.r1c3; + + difference.r2c1 = minuend.r2c1 - subtrahend.r2c1; + difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; + difference.r2c3 = minuend.r2c3 - subtrahend.r2c3; + + difference.r3c1 = minuend.r3c1 - subtrahend.r3c1; + difference.r3c2 = minuend.r3c2 - subtrahend.r3c2; + difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; + } + + public static void GetWeightedSum2( + double weight1, in DPMatrix3x3 matrix1, + double weight2, in DPMatrix3x3 matrix2, + out DPMatrix3x3 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; + sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2; + } + + public static void GetWeightedSum3( + double weight1, in DPMatrix3x3 matrix1, + double weight2, in DPMatrix3x3 matrix2, + double weight3, in DPMatrix3x3 matrix3, + out DPMatrix3x3 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2 + matrix3.r1c3 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; + sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2 + matrix3.r3c3 * weight3; + } + + public static void GetWeightedSum4( + double weight1, in DPMatrix3x3 matrix1, + double weight2, in DPMatrix3x3 matrix2, + double weight3, in DPMatrix3x3 matrix3, + double weight4, in DPMatrix3x3 matrix4, + out DPMatrix3x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); + sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4); + } + + public static void GetWeightedSum5( + double weight1, in DPMatrix3x3 matrix1, + double weight2, in DPMatrix3x3 matrix2, + double weight3, in DPMatrix3x3 matrix3, + double weight4, in DPMatrix3x3 matrix4, + double weight5, in DPMatrix3x3 matrix5, + out DPMatrix3x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4) + matrix5.r1c3 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4) + matrix5.r2c3 * weight5; + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; + sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4) + matrix5.r3c3 * weight5; + } + + public static void Multiply(in DPMatrix3x3 multiplicand, double multiplier, out DPMatrix3x3 product) + { + product.r1c1 = multiplicand.r1c1 * multiplier; + product.r1c2 = multiplicand.r1c2 * multiplier; + product.r1c3 = multiplicand.r1c3 * multiplier; + + product.r2c1 = multiplicand.r2c1 * multiplier; + product.r2c2 = multiplicand.r2c2 * multiplier; + product.r2c3 = multiplicand.r2c3 * multiplier; + + product.r3c1 = multiplicand.r3c1 * multiplier; + product.r3c2 = multiplicand.r3c2 * multiplier; + product.r3c3 = multiplicand.r3c3 * multiplier; + } + + public static void Divide(in DPMatrix3x3 dividend, double divisor, out DPMatrix3x3 quotient) + { + quotient.r1c1 = dividend.r1c1 / divisor; + quotient.r1c2 = dividend.r1c2 / divisor; + quotient.r1c3 = dividend.r1c3 / divisor; + + quotient.r2c1 = dividend.r2c1 / divisor; + quotient.r2c2 = dividend.r2c2 / divisor; + quotient.r2c3 = dividend.r2c3 / divisor; + + quotient.r3c1 = dividend.r3c1 / divisor; + quotient.r3c2 = dividend.r3c2 / divisor; + quotient.r3c3 = dividend.r3c3 / divisor; + } + + public static void GetRightProduct(in DPMatrix3x3 matrix, in DPVector3 vector, out DPVector3 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; + double x3 = matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2 + matrix.r3c3 * vector.x3; + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + public static void GetLeftProduct(in DPVector3 vector, in DPMatrix3x3 matrix, out DPVector3 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; + double x3 = vector.x1 * matrix.r1c3 + vector.x2 * matrix.r2c3 + vector.x3 * matrix.r3c3; + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + public static void LoadZero(out DPMatrix3x3 matrix) + { + matrix.r1c1 = 0.0; + matrix.r1c2 = 0.0; + matrix.r1c3 = 0.0; + + matrix.r2c1 = 0.0; + matrix.r2c2 = 0.0; + matrix.r2c3 = 0.0; + + matrix.r3c1 = 0.0; + matrix.r3c2 = 0.0; + matrix.r3c3 = 0.0; + } + + public static void LoadIdentity(out DPMatrix3x3 matrix) + { + matrix.r1c1 = 1.0; + matrix.r1c2 = 0.0; + matrix.r1c3 = 0.0; + + matrix.r2c1 = 0.0; + matrix.r2c2 = 1.0; + matrix.r2c3 = 0.0; + + matrix.r3c1 = 0.0; + matrix.r3c2 = 0.0; + matrix.r3c3 = 1.0; + } + + public static void LoadDiagonal(double d1, double d2, double d3, out DPMatrix3x3 matrix) + { + matrix.r1c1 = d1; + matrix.r1c2 = 0.0; + matrix.r1c3 = 0.0; + + matrix.r2c1 = 0.0; + matrix.r2c2 = d2; + matrix.r2c3 = 0.0; + + matrix.r3c1 = 0.0; + matrix.r3c2 = 0.0; + matrix.r3c3 = d3; + } + } +} diff --git a/Geometry/DPMatrixProduct.cs b/Geometry/DPMatrixProduct.cs new file mode 100644 index 0000000..a00f6b8 --- /dev/null +++ b/Geometry/DPMatrixProduct.cs @@ -0,0 +1,191 @@ +/* + * 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: 11 Nov 2024 + */ +namespace Geometry +{ + public class DPMatrixProduct + { + public static void Get2x2At2x2(in DPMatrix2x2 left, in DPMatrix2x2 right, out DPMatrix2x2 product) + { + double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; + double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; + + double r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1; + double r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + } + + public static void Get2x2At3x2(in DPMatrix2x2 left, in DPMatrix3x2 right, out DPMatrix3x2 product) + { + double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; + double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; + double r1c3 = left.r1c1 * right.r1c3 + left.r1c2 * right.r2c3; + + double r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1; + double r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2; + double r2c3 = left.r2c1 * right.r1c3 + left.r2c2 * right.r2c3; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + product.r1c3 = r1c3; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + product.r2c3 = r2c3; + } + + public static void Get2x3At2x2(in DPMatrix2x3 left, in DPMatrix2x2 right, out DPMatrix2x3 product) + { + double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; + double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; + + double r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1; + double r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2; + + double r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1; + double r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + + product.r3c1 = r3c1; + product.r3c2 = r3c2; + } + + public static void Get2x3At3x2(in DPMatrix2x3 left, in DPMatrix3x2 right, out DPMatrix3x3 product) + { + double r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; + double r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; + double r1c3 = left.r1c1 * right.r1c3 + left.r1c2 * right.r2c3; + + double r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1; + double r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2; + double r2c3 = left.r2c1 * right.r1c3 + left.r2c2 * right.r2c3; + + double r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1; + double r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2; + double r3c3 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c3; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + product.r1c3 = r1c3; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + product.r2c3 = r2c3; + + product.r3c1 = r3c1; + product.r3c2 = r3c2; + product.r3c3 = r3c3; + } + + public static void Get3x2At3x3(in DPMatrix3x2 left, in DPMatrix3x3 right, out DPMatrix3x2 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; + double r1c3 = left.r1c1 * right.r1c3 + left.r1c2 * right.r2c3 + left.r1c3 * right.r3c3; + + double r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1; + double r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; + double r2c3 = left.r2c1 * right.r1c3 + left.r2c2 * right.r2c3 + left.r2c3 * right.r3c3; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + product.r1c3 = r1c3; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + product.r2c3 = r2c3; + } + + public static void Get3x2At2x3(in DPMatrix3x2 left, in DPMatrix2x3 right, out DPMatrix2x2 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; + + double r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1; + double r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + } + + public static void Get3x3At2x3(in DPMatrix3x3 left, in DPMatrix2x3 right, out DPMatrix2x3 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; + + double r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1; + double r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; + + double r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1 + left.r3c3 * right.r3c1; + double r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2 + left.r3c3 * right.r3c2; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + + product.r3c1 = r3c1; + product.r3c2 = r3c2; + } + + public static void Get3x3At3x3(in DPMatrix3x3 left, in DPMatrix3x3 right, out DPMatrix3x3 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; + double r1c3 = left.r1c1 * right.r1c3 + left.r1c2 * right.r2c3 + left.r1c3 * right.r3c3; + + double r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1; + double r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; + double r2c3 = left.r2c1 * right.r1c3 + left.r2c2 * right.r2c3 + left.r2c3 * right.r3c3; + + double r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1 + left.r3c3 * right.r3c1; + double r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2 + left.r3c3 * right.r3c2; + double r3c3 = left.r3c1 * right.r1c3 + left.r3c2 * right.r2c3 + left.r3c3 * right.r3c3; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + product.r1c3 = r1c3; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + product.r2c3 = r2c3; + + product.r3c1 = r3c1; + product.r3c2 = r3c2; + product.r3c3 = r3c3; + } + } +} diff --git a/Geometry/DPQuaternion.cs b/Geometry/DPQuaternion.cs new file mode 100644 index 0000000..3dcdd21 --- /dev/null +++ b/Geometry/DPQuaternion.cs @@ -0,0 +1,201 @@ +using System; +using System.Numerics; + +namespace Geometry +{ + public struct DPQuaternion + { + public double s0, x1, x2, x3; + + public DPQuaternion(double s0, double x1, double x2, double x3) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + } + + public DPQuaternion(in SPQuaternion quaternion) + { + this.s0 = quaternion.s0; + this.x1 = quaternion.x1; + this.x2 = quaternion.x2; + this.x3 = quaternion.x3; + } + + public DPQuaternion(in DPQuaternion quaternion) + { + this.s0 = quaternion.s0; + this.x1 = quaternion.x1; + this.x2 = quaternion.x2; + this.x3 = quaternion.x3; + } + + public void Reset() + { + this.s0 = 0.0; + this.x1 = 0.0; + this.x2 = 0.0; + this.x3 = 0.0; + } + + public void Conjugate() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public readonly DPQuaternion GetConjugate() + { + return new DPQuaternion(this.s0, -this.x1, -this.x2, -this.x3); + } + + public void SetValues(double s0, double x1, double x2, double x3) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + } + + public void SetValues(in SPQuaternion quaternion) + { + this.s0 = quaternion.s0; + this.x1 = quaternion.x1; + this.x2 = quaternion.x2; + this.x3 = quaternion.x3; + } + + public void SetValues(in DPQuaternion quaternion) + { + this.s0 = quaternion.s0; + this.x1 = quaternion.x1; + this.x2 = quaternion.x2; + this.x3 = quaternion.x3; + } + + public readonly void MakeRotationMatrix(out DPMatrix3x3 matrix) + { + 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 squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + + if (-DPUtility.EPSYLON <= squareModule && squareModule <= DPUtility.EPSYLON) + { + DPMatrix3x3.LoadIdentity(out matrix); + return; + } + + double corrector1; + double corrector2; + + if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) { + corrector1 = 2.0 - squareModule; + corrector2 = 2.0 * corrector1; + } + else { + corrector1 = 1.0 / squareModule; + corrector2 = 2.0 / squareModule; + } + + 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; + + 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); + } + + public readonly void MakeReverseMatrix(out DPMatrix3x3 matrix) + { + 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 squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + + if (-DPUtility.EPSYLON <= squareModule && squareModule <= DPUtility.EPSYLON) + { + DPMatrix3x3.LoadIdentity(out matrix); + return; + } + + double corrector1; + double corrector2; + + if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) { + corrector1 = 2.0 - squareModule; + corrector2 = 2.0 * corrector1; + } + else { + corrector1 = 1.0 / squareModule; + corrector2 = 2.0 / squareModule; + } + + 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; + + 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); + } + + public static void Add(in DPQuaternion quaternion1, in DPQuaternion quaternion2, out DPQuaternion sum) + { + sum.s0 = quaternion1.s0 + quaternion2.s0; + sum.x1 = quaternion1.x1 + quaternion2.x1; + sum.x2 = quaternion1.x2 + quaternion2.x2; + sum.x3 = quaternion1.x3 + quaternion2.x3; + } + + public static void Subtract(in DPQuaternion minuend, in DPQuaternion subtrahend, out DPQuaternion difference) + { + difference.s0 = minuend.s0 - subtrahend.s0; + difference.x1 = minuend.x1 - subtrahend.x1; + difference.x2 = minuend.x2 - subtrahend.x2; + difference.x3 = minuend.x3 - subtrahend.x3; + } + + public static void Multiply(in DPQuaternion left, in DPQuaternion right, out DPQuaternion 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); + + product.s0 = s0; + product.x1 = x1; + product.x2 = x2; + product.x3 = x3; + } + } +} diff --git a/Geometry/DPUtility.cs b/Geometry/DPUtility.cs new file mode 100644 index 0000000..0f98687 --- /dev/null +++ b/Geometry/DPUtility.cs @@ -0,0 +1,20 @@ +using System; + +namespace Geometry +{ + public class DPUtility + { + 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/Geometry/DPVector2.cs b/Geometry/DPVector2.cs new file mode 100644 index 0000000..c4c43f4 --- /dev/null +++ b/Geometry/DPVector2.cs @@ -0,0 +1,383 @@ +/* + * 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: 1 Feb 2019 + */ + +namespace Geometry +{ + public struct DPVector2 + { + public static readonly DPVector2 ZERO = new DPVector2(0.0, 0.0); + + public double x1; + public double x2; + + public DPVector2(double x1, double x2) + { + this.x1 = x1; + this.x2 = x2; + } + + public DPVector2(in DPVector2 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + } + + public DPVector2(in SPVector2 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + } + + public readonly double GetSquareModule() + { + return this.x1 * this.x1 + this.x2 * this.x2; + } + + public readonly double GetModule() + { + return Math.Sqrt(this.GetSquareModule()); + } + + public int Normalize() + { + double squareModule = this.GetSquareModule(); + + if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) + { + return 1; + } + + if (squareModule <= DPUtility.SQUARE_EPSYLON) + { + this.Reset(); + return 0; + } + + double module = Math.Sqrt(squareModule); + + this.x1 /= module; + this.x2 /= module; + + return 1; + } + + public readonly DPVector2 GetNormalized() + { + double squareModule = this.GetSquareModule(); + + if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) + { + return this; + } + + if (squareModule <= DPUtility.SQUARE_EPSYLON) + { + return ZERO; + } + + double module = Math.Sqrt(squareModule); + + return new DPVector2( + this.x1 / module, + this.x2 / module + ); + } + + public void Invert() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + } + + public readonly DPVector2 GetInverted() + { + return new DPVector2(-this.x1, -this.x2); + } + + public readonly bool IsZero() + { + return this.GetSquareModule() <= DPUtility.SQUARE_EPSYLON; + } + + public readonly bool IsUnit() + { + double squareModule = this.GetSquareModule(); + return 1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= DPUtility.EPSYLON; + } + + public void Reset() + { + this.x1 = 0.0; + this.x2 = 0.0; + } + + public void SetValues(double x1, double x2) + { + this.x1 = x1; + this.x2 = x2; + } + + public void SetValues(in SPVector2 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + } + + public void SetValues(in DPVector2 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + } + + public readonly override string ToString() + { + return String.Format("DPVector2({0}, {1})", this.x1, this.x2); + } + + public static void Add(in DPVector2 vector1, in DPVector2 vector2, out DPVector2 sum) + { + sum.x1 = vector1.x1 + vector2.x1; + sum.x2 = vector1.x2 + vector2.x2; + } + + public static void Add3( + in DPVector2 vector1, + in DPVector2 vector2, + in DPVector2 vector3, + out DPVector2 sum + ) + { + sum.x1 = vector1.x1 + vector2.x1 + vector3.x1; + sum.x2 = vector1.x2 + vector2.x2 + vector3.x2; + } + + public static void Add4( + in DPVector2 vector1, + in DPVector2 vector2, + in DPVector2 vector3, + in DPVector2 vector4, + out DPVector2 sum + ) + { + sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1); + sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2); + } + + public static void Add5( + in DPVector2 vector1, + in DPVector2 vector2, + in DPVector2 vector3, + in DPVector2 vector4, + in DPVector2 vector5, + out DPVector2 sum + ) + { + sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1; + sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2; + } + + public static void Subtract(in DPVector2 minuend, in DPVector2 subtrahend, out DPVector2 difference) + { + difference.x1 = minuend.x1 - subtrahend.x1; + difference.x2 = minuend.x2 - subtrahend.x2; + } + + public static void GetWeightedSum2( + double weight1, in DPVector2 vector1, + double weight2, in DPVector2 vector2, + out DPVector2 sum + ) + { + sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2; + sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2; + } + + public static void GetWeightedSum3( + double weight1, in DPVector2 vector1, + double weight2, in DPVector2 vector2, + double weight3, in DPVector2 vector3, + out DPVector2 sum + ) + { + sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2 + vector3.x1 * weight3; + sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2 + vector3.x2 * weight3; + } + + public static void GetWeightedSum4( + double weight1, in DPVector2 vector1, + double weight2, in DPVector2 vector2, + double weight3, in DPVector2 vector3, + double weight4, in DPVector2 vector4, + out DPVector2 sum + ) + { + sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4); + sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4); + } + + public static void GetWeightedSum5( + double weight1, in DPVector2 vector1, + double weight2, in DPVector2 vector2, + double weight3, in DPVector2 vector3, + double weight4, in DPVector2 vector4, + double weight5, in DPVector2 vector5, + out DPVector2 sum + ) + { + sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4) + vector5.x1 * weight5; + sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4) + vector5.x2 * weight5; + } + + public static void Muliply(in DPVector2 multiplicand, double multiplier, out DPVector2 product) + { + product.x1 = multiplicand.x1 * multiplier; + product.x2 = multiplicand.x2 * multiplier; + } + + public static void Divide(in DPVector2 dividend, double divisor, out DPVector2 quotient) + { + quotient.x1 = dividend.x1 / divisor; + quotient.x2 = dividend.x2 / divisor; + } + + public static void GetMean2( + in DPVector2 vector1, + in DPVector2 vector2, + out DPVector2 result + ) + { + result.x1 = (vector1.x1 + vector2.x1) * 0.5; + result.x2 = (vector1.x2 + vector2.x2) * 0.5; + } + + public static void GetMean3( + in DPVector2 vector1, + in DPVector2 vector2, + in DPVector2 vector3, + out DPVector2 result + ) + { + result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * DPUtility.ONE_THIRD; + result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * DPUtility.ONE_THIRD; + } + + public static void GetMean4( + in DPVector2 vector1, + in DPVector2 vector2, + in DPVector2 vector3, + in DPVector2 vector4, + out DPVector2 result + ) + { + result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1)) * 0.25; + result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2)) * 0.25; + } + + public static void GetMean5( + in DPVector2 vector1, + in DPVector2 vector2, + in DPVector2 vector3, + in DPVector2 vector4, + in DPVector2 vector5, + out DPVector2 result + ) + { + result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1) * 0.2; + result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2) * 0.2; + } + + public static double GetScalarProduct(in DPVector2 vector1, in DPVector2 vector2) + { + return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2; + } + + public static double GetCrossProduct(in DPVector2 vector1, in DPVector2 vector2) + { + return vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1; + } + + public static double GetAngle(in DPVector2 vector1, in DPVector2 vector2, AngleUnit unit) + { + double squareModule1 = vector1.GetSquareModule(); + + if (squareModule1 <= DPUtility.SQUARE_EPSYLON) + { + return 0.0; + } + + double squareModule2 = vector2.GetSquareModule(); + + if (squareModule2 <= DPUtility.SQUARE_EPSYLON) + { + return 0.0; + } + + double cosine = DPVector2.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); + + if (1.0 - DPUtility.EPSYLON <= cosine) + { + return 0.0; + } + + if (cosine <= -(1.0 - DPUtility.EPSYLON)) + { + return DPAngle.GetHalfCircle(unit); + } + + return DPAngle.ConvertFromRadians(Math.Acos(cosine), unit); + } + + public static double GetSquareDistance(in DPVector2 vector1, in DPVector2 vector2) + { + double dx1 = vector1.x1 - vector2.x1; + double dx2 = vector1.x2 - vector2.x2; + + return dx1 * dx1 + dx2 * dx2; + } + + public static double GetDistance(in DPVector2 vector1, in DPVector2 vector2) + { + return Math.Sqrt(GetSquareDistance(vector1, vector2)); + } + + public static bool AreEqual(in DPVector2 vector1, in DPVector2 vector2) + { + double squareModule1 = vector1.GetSquareModule(); + double squareModule2 = vector2.GetSquareModule(); + double squareModule3 = GetSquareDistance(vector1, vector2); + + // 2.0 means dimension amount + if (squareModule1 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT) + { + return squareModule3 < (2.0 * DPUtility.SQUARE_EPSYLON); + } + + if (squareModule1 <= squareModule2) + { + return squareModule3 <= (2.0 * DPUtility.SQUARE_EPSYLON) * squareModule2; + } + + return squareModule3 <= (2.0 * DPUtility.SQUARE_EPSYLON) * squareModule1; + } + } +} diff --git a/Geometry/DPVector3.cs b/Geometry/DPVector3.cs new file mode 100644 index 0000000..cf5bc44 --- /dev/null +++ b/Geometry/DPVector3.cs @@ -0,0 +1,435 @@ +/* + * 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: 1 Feb 2019 + */ + +namespace Geometry +{ + public struct DPVector3 + { + public static readonly DPVector3 ZERO = new DPVector3(0.0, 0.0, 0.0); + + public double x1; + public double x2; + public double x3; + + public DPVector3(double x1, double x2, double x3) + { + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + } + + public DPVector3(in DPVector3 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + this.x3 = vector.x3; + } + + public DPVector3(in SPVector3 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + this.x3 = vector.x3; + } + + public readonly double GetSquareModule() + { + return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3; + } + + public readonly double GetModule() + { + return Math.Sqrt(this.GetSquareModule()); + } + + public int Normalize() + { + double squareModule = this.GetSquareModule(); + + if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) + { + return 1; + } + + if (squareModule <= DPUtility.SQUARE_EPSYLON) + { + this.Reset(); + return 0; + } + + double module = Math.Sqrt(squareModule); + + this.x1 /= module; + this.x2 /= module; + this.x3 /= module; + + return 1; + } + + public readonly DPVector3 GetNormalized() + { + double squareModule = this.GetSquareModule(); + + if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) + { + return this; + } + + if (squareModule <= DPUtility.SQUARE_EPSYLON) + { + return ZERO; + } + + double module = Math.Sqrt(squareModule); + + return new DPVector3( + this.x1 / module, + this.x2 / module, + this.x3 / module + ); + } + + public void Invert() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public readonly DPVector3 GetInverted() + { + return new DPVector3(-this.x1, -this.x2, -this.x3); + } + + public readonly bool IsZero() + { + return this.GetSquareModule() <= DPUtility.SQUARE_EPSYLON; + } + + public readonly bool IsUnit() + { + double squareModule = this.GetSquareModule(); + return 1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= DPUtility.EPSYLON; + } + + public void Reset() + { + this.x1 = 0.0; + this.x2 = 0.0; + this.x3 = 0.0; + } + + public void SetValues(double x1, double x2, double x3) + { + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + } + + public void SetValues(in SPVector3 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + this.x3 = vector.x3; + } + + public void SetValues(in DPVector3 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + this.x3 = vector.x3; + } + + public readonly override string ToString() + { + return String.Format("DPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3); + } + + public static void Add(in DPVector3 vector1, in DPVector3 vector2, out DPVector3 sum) + { + sum.x1 = vector1.x1 + vector2.x1; + sum.x2 = vector1.x2 + vector2.x2; + sum.x3 = vector1.x3 + vector2.x3; + } + + public static void Add3( + in DPVector3 vector1, + in DPVector3 vector2, + in DPVector3 vector3, + out DPVector3 sum + ) + { + sum.x1 = vector1.x1 + vector2.x1 + vector3.x1; + sum.x2 = vector1.x2 + vector2.x2 + vector3.x2; + sum.x3 = vector1.x3 + vector2.x3 + vector3.x3; + } + + public static void Add4( + in DPVector3 vector1, + in DPVector3 vector2, + in DPVector3 vector3, + in DPVector3 vector4, + out DPVector3 sum + ) + { + sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1); + sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2); + sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3); + } + + public static void Add5( + in DPVector3 vector1, + in DPVector3 vector2, + in DPVector3 vector3, + in DPVector3 vector4, + in DPVector3 vector5, + out DPVector3 sum + ) + { + sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1; + sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2; + sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3; + } + + public static void Subtract(in DPVector3 minuend, in DPVector3 subtrahend, out DPVector3 difference) + { + difference.x1 = minuend.x1 - subtrahend.x1; + difference.x2 = minuend.x2 - subtrahend.x2; + difference.x3 = minuend.x3 - subtrahend.x3; + } + + public static void GetWeightedSum2( + double weight1, in DPVector3 vector1, + double weight2, in DPVector3 vector2, + out DPVector3 sum + ) + { + sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2; + sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2; + sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2; + } + + public static void GetWeightedSum3( + double weight1, in DPVector3 vector1, + double weight2, in DPVector3 vector2, + double weight3, in DPVector3 vector3, + out DPVector3 sum + ) + { + sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2 + vector3.x1 * weight3; + sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2 + vector3.x2 * weight3; + sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2 + vector3.x3 * weight3; + } + + public static void GetWeightedSum4( + double weight1, in DPVector3 vector1, + double weight2, in DPVector3 vector2, + double weight3, in DPVector3 vector3, + double weight4, in DPVector3 vector4, + out DPVector3 sum + ) + { + sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4); + sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4); + sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4); + } + + public static void GetWeightedSum5( + double weight1, in DPVector3 vector1, + double weight2, in DPVector3 vector2, + double weight3, in DPVector3 vector3, + double weight4, in DPVector3 vector4, + double weight5, in DPVector3 vector5, + out DPVector3 sum + ) + { + sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4) + vector5.x1 * weight5; + sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4) + vector5.x2 * weight5; + sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4) + vector5.x3 * weight5; + } + + public static void Muliply(in DPVector3 multiplicand, double multiplier, out DPVector3 product) + { + product.x1 = multiplicand.x1 * multiplier; + product.x2 = multiplicand.x2 * multiplier; + product.x3 = multiplicand.x3 * multiplier; + } + + public static void Divide(in DPVector3 dividend, double divisor, out DPVector3 quotient) + { + quotient.x1 = dividend.x1 / divisor; + quotient.x2 = dividend.x2 / divisor; + quotient.x3 = dividend.x3 / divisor; + } + + public static void GetMean2( + in DPVector3 vector1, + in DPVector3 vector2, + out DPVector3 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 DPVector3 vector1, + in DPVector3 vector2, + in DPVector3 vector3, + out DPVector3 result + ) + { + result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * DPUtility.ONE_THIRD; + result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * DPUtility.ONE_THIRD; + result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * DPUtility.ONE_THIRD; + } + + public static void GetMean4( + in DPVector3 vector1, + in DPVector3 vector2, + in DPVector3 vector3, + in DPVector3 vector4, + out DPVector3 result + ) + { + result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1)) * 0.25; + result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2)) * 0.25; + result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3)) * 0.25; + } + + public static void GetMean5( + in DPVector3 vector1, + in DPVector3 vector2, + in DPVector3 vector3, + in DPVector3 vector4, + in DPVector3 vector5, + out DPVector3 result + ) + { + result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1) * 0.2; + result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2) * 0.2; + result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3) * 0.2; + } + + public static double GetScalarProduct(in DPVector3 vector1, in DPVector3 vector2) + { + return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3; + } + + public static void GetCrossProduct(in DPVector3 vector1, in DPVector3 vector2, out DPVector3 result) + { + double x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2; + double x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3; + double x3 = vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1; + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + public static double GetTripleProduct(in DPVector3 vector1, in DPVector3 vector2, in DPVector3 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 DPVector3 vector1, in DPVector3 vector2, in DPVector3 vector3, out DPVector3 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; + } + + public static double GetAngle(in DPVector3 vector1, in DPVector3 vector2, AngleUnit unit) + { + double squareModule1 = vector1.GetSquareModule(); + + if (squareModule1 <= DPUtility.SQUARE_EPSYLON) + { + return 0.0; + } + + double squareModule2 = vector2.GetSquareModule(); + + if (squareModule2 <= DPUtility.SQUARE_EPSYLON) + { + return 0.0; + } + + double cosine = DPVector3.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2); + + if (1.0 - DPUtility.EPSYLON <= cosine) + { + return 0.0; + } + + if (cosine <= -(1.0 - DPUtility.EPSYLON)) + { + return DPAngle.GetHalfCircle(unit); + } + + return DPAngle.ConvertFromRadians(Math.Acos(cosine), unit); + } + + public static double GetSquareDistance(in DPVector3 vector1, in DPVector3 vector2) + { + double dx1 = vector1.x1 - vector2.x1; + double dx2 = vector1.x2 - vector2.x2; + double dx3 = vector1.x3 - vector2.x3; + + return dx1 * dx1 + dx2 * dx2 + dx3 * dx3; + } + + public static double GetDistance(in DPVector3 vector1, in DPVector3 vector2) + { + return Math.Sqrt(GetSquareDistance(vector1, vector2)); + } + + public static bool AreEqual(in DPVector3 vector1, in DPVector3 vector2) + { + double squareModule1 = vector1.GetSquareModule(); + double squareModule2 = vector2.GetSquareModule(); + double squareModule3 = GetSquareDistance(vector1, vector2); + + // 3.0 means dimension amount + if (squareModule1 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < DPUtility.EPSYLON_EFFECTIVENESS_LIMIT) + { + return squareModule3 < (3.0 * DPUtility.SQUARE_EPSYLON); + } + + if (squareModule1 <= squareModule2) + { + return squareModule3 <= (3.0 * DPUtility.SQUARE_EPSYLON) * squareModule2; + } + + return squareModule3 <= (3.0 * DPUtility.SQUARE_EPSYLON) * squareModule1; + } + } +} + diff --git a/Geometry/DPVersor.cs b/Geometry/DPVersor.cs new file mode 100644 index 0000000..8f79f11 --- /dev/null +++ b/Geometry/DPVersor.cs @@ -0,0 +1,359 @@ +/* + * 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 + */ + +namespace Geometry +{ + public struct DPVersor + { + public static readonly DPVersor IDLE = new DPVersor(1.0, 0.0, 0.0, 0.0, 1.0); + + private double s0; + private double x1; + private double x2; + private double x3; + private double corrector; + + private DPVersor(double s0, double x1, double x2, double x3, double corrector) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + this.corrector = corrector; + } + + public DPVersor() + { + this.s0 = 1.0; + this.x1 = 0.0; + this.x2 = 0.0; + this.x3 = 0.0; + this.corrector = 1.0; + } + + public DPVersor(double s0, double x1, double x2, double x3) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + + double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + this.corrector = 2.0 - squareModule; + + if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule) + { + this.Normalize(squareModule); + } + } + + public DPVersor(in DPVersor versor) + { + this.s0 = versor.s0; + this.x1 = versor.x1; + this.x2 = versor.x2; + this.x3 = versor.x3; + this.corrector = versor.corrector; + } + + public DPVersor(in SPVersor versor) + { + this.s0 = versor.GetScalar(); + this.x1 = versor.GetX1(); + this.x2 = versor.GetX2(); + this.x3 = versor.GetX3(); + + double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); + + this.corrector = 2.0 - squareModule; + + if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule) + { + this.Normalize(squareModule); + } + } + + public readonly double GetScalar() + { + return this.s0; + } + + public readonly double GetX1() + { + return this.x1; + } + + public readonly double GetX2() + { + return this.x2; + } + + public readonly double GetX3() + { + return this.x3; + } + + public readonly double GetCorrector() + { + return this.corrector; + } + + public readonly bool IsIdle() + { + return this.s0 <= -(1.0 - DPUtility.EPSYLON) || (1.0 - DPUtility.EPSYLON) <= this.s0; + } + + public void Reset() + { + this.s0 = 1.0; + this.x1 = 0.0; + this.x2 = 0.0; + this.x3 = 0.0; + this.corrector = 1.0; + } + + public void Invert() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public readonly double GetAngle(AngleUnit unit) + { + if (this.s0 <= -(1.0 - DPUtility.TWO_EPSYLON) || 1.0 - DPUtility.TWO_EPSYLON <= this.s0) { + return 0.0; + } + + if (-DPUtility.EPSYLON <= this.s0 && this.s0 <= DPUtility.EPSYLON) + { + return DPAngle.GetHalfCircle(unit); + } + + return DPAngle.ConvertFromRadians(2.0 * Math.Acos(s0), unit); + } + + public readonly void MakeRotationMatrix(out DPMatrix3x3 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 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 * this.corrector; + + matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix.r3c3 = this.corrector * ((s0s0 + x3x3) - (x1x1 + x2x2)); + + matrix.r1c2 = corrector2 * (x1x2 - s0x3); + matrix.r2c3 = corrector2 * (x2x3 - s0x1); + matrix.r3c1 = corrector2 * (x1x3 - s0x2); + + matrix.r2c1 = corrector2 * (x1x2 + s0x3); + matrix.r3c2 = corrector2 * (x2x3 + s0x1); + matrix.r1c3 = corrector2 * (x1x3 + s0x2); + } + + public readonly void MakeReverseMatrix(out DPMatrix3x3 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 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 * this.corrector; + + matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix.r3c3 = this.corrector * ((s0s0 + x3x3) - (x1x1 + x2x2)); + + matrix.r1c2 = corrector2 * (x1x2 + s0x3); + matrix.r2c3 = corrector2 * (x2x3 + s0x1); + matrix.r3c1 = corrector2 * (x1x3 + s0x2); + + matrix.r2c1 = corrector2 * (x1x2 - s0x3); + matrix.r3c2 = corrector2 * (x2x3 - s0x1); + matrix.r1c3 = corrector2 * (x1x3 - s0x2); + } + + public void SetValues(double s0, double x1, double x2, double x3) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + + double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + this.corrector = 2.0 - squareModule; + + if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule) + { + this.Normalize(squareModule); + } + } + + public void SetValues(in DPVersor versor) + { + this.s0 = versor.s0; + this.x1 = versor.x1; + this.x2 = versor.x2; + this.x3 = versor.x3; + this.corrector = versor.corrector; + } + + public void SetValues(in SPVersor versor) + { + this.s0 = versor.GetScalar(); + this.x1 = versor.GetX1(); + this.x2 = versor.GetX2(); + this.x3 = versor.GetX3(); + + double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); + + this.corrector = 2.0 - squareModule; + + if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule) + { + this.Normalize(squareModule); + } + } + + public void SetInverted(in DPVersor versor) + { + this.s0 = versor.s0; + this.x1 = -versor.x1; + this.x2 = -versor.x2; + this.x3 = -versor.x3; + this.corrector = versor.corrector; + } + + public void SetInverted(in SPVersor versor) + { + this.s0 = versor.GetScalar(); + this.x1 = -versor.GetX1(); + this.x2 = -versor.GetX2(); + this.x3 = -versor.GetX3(); + + double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); + + this.corrector = 2.0 - squareModule; + + if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule) + { + this.Normalize(squareModule); + } + } + + public readonly void Turn(in DPVector3 vector, out DPVector3 result) + { + double multiplier = 2.0 * this.corrector; + double tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); + double tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); + double tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1); + + double 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); + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + public readonly void TurnBack(in DPVector3 vector, out DPVector3 result) + { + double multiplier = 2.0 * this.corrector; + double tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); + double tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); + double tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1); + + double 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); + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + private void Normalize(double squareModule) + { + if (squareModule <= DPUtility.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= DPUtility.SQUARE_EPSYLON * squareModule) + { + this.Reset(); + return; + } + + double module = Math.Sqrt(squareModule); + + this.s0 /= module; + this.x1 /= module; + this.x2 /= module; + this.x3 /= module; + + this.corrector = 2.0 - (this.s0 * this.s0 + this.x1 * this.x1) - (this.x2 * this.x2 + this.x3 * this.x3); + } + + public static void Combine(in DPVersor second, in DPVersor first, out DPVersor 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; + + double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + result.corrector = 2.0 - squareModule; + + if (squareModule < 1.0 - DPUtility.TWO_EPSYLON || 1.0 + DPUtility.TWO_EPSYLON < squareModule) + { + result.Normalize(squareModule); + } + } + } +} diff --git a/Geometry/Geometry.csproj b/Geometry/Geometry.csproj new file mode 100644 index 0000000..f0fcd01 --- /dev/null +++ b/Geometry/Geometry.csproj @@ -0,0 +1,16 @@ + + + + net6.0 + enable + enable + + + + + + + + + + diff --git a/Geometry/SPAngle.cs b/Geometry/SPAngle.cs new file mode 100644 index 0000000..d665ec8 --- /dev/null +++ b/Geometry/SPAngle.cs @@ -0,0 +1,306 @@ +/* + * 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: 1 Feb 2019 + */ + +namespace Geometry +{ + public static class SPAngle + { + public const float PI = 3.1415926536f; + public const float TWO_PI = 6.2831853072f; + public const float HALF_OF_PI = 1.5707963268f; + public const float THIRD_OF_PI = 1.0471975512f; + public const float FOURTH_OF_PI = 0.7853981634f; + public const float SIXTH_OF_PI = 0.5235987756f; + + public const float DEGREES_IN_RADIAN = 57.295779513f; + public const float TURNS_IN_RADIAN = 0.1591549431f; + public const float RADIANS_IN_DEGREE = 1.745329252E-2f; + public const float TURNS_IN_DEGREE = 2.7777777778E-3f; + + public static float RadiansToDegrees(float radians) + { + return radians * DEGREES_IN_RADIAN; + } + + public static float RadiansToTurns(float radians) + { + return radians * TURNS_IN_RADIAN; + } + + public static float DegreesToRadians(float degrees) + { + return degrees * RADIANS_IN_DEGREE; + } + + public static float DegreesToTurns(float degrees) + { + return degrees * TURNS_IN_DEGREE; + } + + public static float TurnsToRadians(float turns) + { + return turns * TWO_PI; + } + + public static float TurnsToDegrees(float turns) + { + return turns * 360.0f; + } + + public static float ConvertFromRadians(float radians, AngleUnit toUnit) + { + if (toUnit == AngleUnit.DEGREES) + { + return radians * DEGREES_IN_RADIAN; + } + + if (toUnit == AngleUnit.TURNS) + { + return radians * TURNS_IN_RADIAN; + } + + return radians; + } + + public static float ConvertFromDegrees(float degrees, AngleUnit toUnit) + { + if (toUnit == AngleUnit.RADIANS) + { + return degrees * RADIANS_IN_DEGREE; + } + + if (toUnit == AngleUnit.TURNS) + { + return degrees * TURNS_IN_DEGREE; + } + + return degrees; + } + + public static float ConvertFromTurns(float turns, AngleUnit toUnit) + { + if (toUnit == AngleUnit.RADIANS) + { + return turns * TWO_PI; + } + + if (toUnit == AngleUnit.DEGREES) + { + return turns * 360.0f; + } + + return turns; + } + + public static float ConvertToRadians(float angle, AngleUnit unit) + { + if (unit == AngleUnit.DEGREES) + { + return angle * RADIANS_IN_DEGREE; + } + + if (unit == AngleUnit.TURNS) + { + return angle * TWO_PI; + } + + return angle; + } + + public static float ConvertToDegrees(float angle, AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return angle * DEGREES_IN_RADIAN; + } + + if (unit == AngleUnit.TURNS) + { + return angle * 360.0f; + } + + return angle; + } + + public static float ConvertToTurns(float angle, AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return angle * TURNS_IN_RADIAN; + } + + if (unit == AngleUnit.DEGREES) + { + return angle * TURNS_IN_DEGREE; + } + + return angle; + } + + public static float GetFullCircle(AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return TWO_PI; + } + + if (unit == AngleUnit.DEGREES) + { + return 360.0f; + } + + return 1.0f; + } + + public static float GetHalfCircle(AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return PI; + } + + if (unit == AngleUnit.DEGREES) + { + return 180.0f; + } + + return 0.5f; + } + + public static float GetQuarterCircle(AngleUnit unit) + { + if (unit == AngleUnit.RADIANS) + { + return HALF_OF_PI; + } + + if (unit == AngleUnit.DEGREES) + { + return 90.0f; + } + + return 0.25f; + } + + public static float NormalizeRadians(float radians, AngleRange range) + { + if (range == AngleRange.UNSIGNED_RANGE) + { + if (0.0 <= radians && radians < TWO_PI) + { + return radians; + } + + } + else + { + if (-PI < radians && radians <= PI) + { + return radians; + } + } + + float turns = radians * TURNS_IN_RADIAN; + + turns -= MathF.Floor(turns); + + if (range == AngleRange.SIGNED_RANGE && turns > 0.5f) + { + turns -= 1.0f; + } + + return turns * TWO_PI; + } + + public static float NormalizeDegrees(float radians, AngleRange range) + { + if (range == AngleRange.UNSIGNED_RANGE) + { + if (0.0f <= radians && radians < 360.0f) + { + return radians; + } + } + else + { + if (-180.0f < radians && radians <= 180.0f) + { + return radians; + } + } + + float turns = radians * TURNS_IN_DEGREE; + + turns -= MathF.Floor(turns); + + if (range == AngleRange.SIGNED_RANGE && turns > 0.5f) + { + turns -= 1.0f; + } + + return turns * 360.0f; + } + + public static float NormalizeTurns(float turns, AngleRange range) + { + if (range == AngleRange.UNSIGNED_RANGE) + { + if (0.0f <= turns && turns < 1.0f) + { + return turns; + } + } + else + { + if (-0.5f < turns && turns <= 0.5f) + { + return turns; + } + } + + float rest = turns - MathF.Floor(turns); + + if (range == AngleRange.SIGNED_RANGE && rest > 0.5f) + { + rest -= 1.0f; + } + + return rest; + } + + public static float Normalize(float angle, AngleUnit unit, AngleRange range) + { + if (unit == AngleUnit.RADIANS) + { + return NormalizeRadians(angle, range); + } + + if (unit == AngleUnit.DEGREES) + { + return NormalizeDegrees(angle, range); + } + + return NormalizeTurns(angle, range); + } + } +} diff --git a/Geometry/SPMatrix2x2.cs b/Geometry/SPMatrix2x2.cs new file mode 100644 index 0000000..b99f082 --- /dev/null +++ b/Geometry/SPMatrix2x2.cs @@ -0,0 +1,407 @@ +/* + * 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 + */ + +namespace Geometry +{ + public struct SPMatrix2x2 + { + public float r1c1, r1c2; + + public float r2c1, r2c2; + + public SPMatrix2x2() + { + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + } + + public SPMatrix2x2(float d1, float d2) + { + this.r1c1 = d1; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = d2; + } + + public SPMatrix2x2(in SPMatrix2x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + } + + public SPMatrix2x2(in DPMatrix2x2 matrix) + { + this.r1c1 = (float)matrix.r1c1; + this.r1c2 = (float)matrix.r1c2; + + this.r2c1 = (float)matrix.r2c1; + this.r2c2 = (float)matrix.r2c2; + } + + public readonly float GetDeterminant() + { + return this.r1c1 * this.r2c2 - this.r1c2 * this.r2c1; + } + + public readonly bool IsSingular() + { + float determinant = this.GetDeterminant(); + return -SPUtility.EPSYLON <= determinant && determinant <= SPUtility.EPSYLON; + } + + public void Reset() + { + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + } + + public void MakeIdentity() + { + this.r1c1 = 1.0f; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 1.0f; + } + + public void MakeDiagonal(float d1, float d2) + { + this.r1c1 = d1; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = d2; + } + + public void Transpose() + { + (this.r1c2, this.r2c1) = (this.r2c1, this.r1c2); + } + + public bool Invert() + { + float determinant = this.GetDeterminant(); + + if (-SPUtility.EPSYLON <= determinant && determinant <= SPUtility.EPSYLON) + { + return false; + } + + float r1c1 = this.r2c2; + float r1c2 = -this.r1c2; + + float r2c1 = -this.r2c1; + float r2c2 = this.r1c1; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + + return true; + } + + public void MakeTransposedOf(in SPMatrix2x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r2c2 = matrix.r2c2; + (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); + } + + public void MakeTransposedOf(in DPMatrix2x2 matrix) + { + this.r1c1 = (float)matrix.r1c1; + this.r1c2 = (float)matrix.r2c1; + + this.r2c1 = (float)matrix.r1c2; + this.r2c2 = (float)matrix.r2c2; + } + + public bool MakeInvertedOf(in SPMatrix2x2 matrix) + { + float determinant = matrix.GetDeterminant(); + + if (-SPUtility.EPSYLON <= determinant && determinant <= SPUtility.EPSYLON) + { + return false; + } + + float r1c1 = matrix.r2c2; + float r1c2 = -matrix.r1c2; + + float r2c1 = -matrix.r2c1; + float r2c2 = matrix.r1c1; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + + return true; + } + + public void SetValues(in SPMatrix2x2 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + } + + public void SetValues(in DPMatrix2x2 matrix) + { + this.r1c1 = (float)matrix.r1c1; + this.r1c2 = (float)matrix.r1c2; + + this.r2c1 = (float)matrix.r2c1; + this.r2c2 = (float)matrix.r2c2; + } + + public void SetMainDiagonal(float d1, float d2) + { + this.r1c1 = d1; + this.r2c2 = d2; + } + + public void SetRow1(float c1, float c2) + { + this.r1c1 = c1; + this.r1c2 = c2; + } + + public void SetRow2(float c1, float c2) + { + this.r2c1 = c1; + this.r2c2 = c2; + } + + public void SetColumn1(float r1, float r2) + { + this.r1c1 = r1; + this.r2c1 = r2; + } + + public void SetColumn2(float r1, float r2) + { + this.r1c2 = r1; + this.r2c2 = r2; + } + + public static void Add(in SPMatrix2x2 matrix1, in SPMatrix2x2 matrix2, out SPMatrix2x2 result) + { + result.r1c1 = matrix1.r1c1 + matrix2.r1c1; + result.r1c2 = matrix1.r1c2 + matrix2.r1c2; + + result.r2c1 = matrix1.r2c1 + matrix2.r2c1; + result.r2c2 = matrix1.r2c2 + matrix2.r2c2; + } + + public static void Add3( + in SPMatrix2x2 matrix1, + in SPMatrix2x2 matrix2, + in SPMatrix2x2 matrix3, + out SPMatrix2x2 sum + ) + { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; + } + + public static void Add4( + in SPMatrix2x2 matrix1, + in SPMatrix2x2 matrix2, + in SPMatrix2x2 matrix3, + in SPMatrix2x2 matrix4, + out SPMatrix2x2 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); + } + + public static void Add5( + in SPMatrix2x2 matrix1, + in SPMatrix2x2 matrix2, + in SPMatrix2x2 matrix3, + in SPMatrix2x2 matrix4, + in SPMatrix2x2 matrix5, + out SPMatrix2x2 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; + } + + public static void Subtract(in SPMatrix2x2 minuend, in SPMatrix2x2 subtrahend, out SPMatrix2x2 difference) + { + difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; + difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; + + difference.r2c1 = minuend.r2c1 - subtrahend.r2c1; + difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; + } + + public static void GetWeightedSum2( + float weight1, in SPMatrix2x2 matrix1, + float weight2, in SPMatrix2x2 matrix2, + out SPMatrix2x2 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + } + + public static void GetWeightedSum3( + float weight1, in SPMatrix2x2 matrix1, + float weight2, in SPMatrix2x2 matrix2, + float weight3, in SPMatrix2x2 matrix3, + out SPMatrix2x2 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + } + + public static void GetWeightedSum4( + float weight1, in SPMatrix2x2 matrix1, + float weight2, in SPMatrix2x2 matrix2, + float weight3, in SPMatrix2x2 matrix3, + float weight4, in SPMatrix2x2 matrix4, + out SPMatrix2x2 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + } + + public static void GetWeightedSum5( + float weight1, in SPMatrix2x2 matrix1, + float weight2, in SPMatrix2x2 matrix2, + float weight3, in SPMatrix2x2 matrix3, + float weight4, in SPMatrix2x2 matrix4, + float weight5, in SPMatrix2x2 matrix5, + out SPMatrix2x2 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + } + + public static void Multiply(in SPMatrix2x2 multiplicand, float multiplier, out SPMatrix2x2 product) + { + product.r1c1 = multiplicand.r1c1 * multiplier; + product.r1c2 = multiplicand.r1c2 * multiplier; + + product.r2c1 = multiplicand.r2c1 * multiplier; + product.r2c2 = multiplicand.r2c2 * multiplier; + } + + public static void Divide(in SPMatrix2x2 dividend, float divisor, out SPMatrix2x2 quotient) + { + quotient.r1c1 = dividend.r1c1 / divisor; + quotient.r1c2 = dividend.r1c2 / divisor; + + quotient.r2c1 = dividend.r2c1 / divisor; + quotient.r2c2 = dividend.r2c2 / divisor; + } + + public static void GetRightProduct(in SPMatrix2x2 matrix, in SPVector2 vector, out SPVector2 result) + { + float x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; + float x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2; + + result.x1 = x1; + result.x2 = x2; + } + + public static void GetLeftProduct(in SPVector2 vector, in SPMatrix2x2 matrix, out SPVector2 result) + { + float x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1; + float x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2; + + result.x1 = x1; + result.x2 = x2; + } + + public static void LoadZero(out SPMatrix2x2 matrix) + { + matrix.r1c1 = 0.0f; + matrix.r1c2 = 0.0f; + + matrix.r2c1 = 0.0f; + matrix.r2c2 = 0.0f; + } + + public static void LoadIdentity(out SPMatrix2x2 matrix) + { + matrix.r1c1 = 1.0f; + matrix.r1c2 = 0.0f; + + matrix.r2c1 = 0.0f; + matrix.r2c2 = 1.0f; + } + + public static void LoadDiagonal(float d1, float d2, out SPMatrix2x2 matrix) + { + matrix.r1c1 = d1; + matrix.r1c2 = 0.0f; + + matrix.r2c1 = 0.0f; + matrix.r2c2 = d2; + } + } +} diff --git a/Geometry/SPMatrix2x3.cs b/Geometry/SPMatrix2x3.cs new file mode 100644 index 0000000..01e2e83 --- /dev/null +++ b/Geometry/SPMatrix2x3.cs @@ -0,0 +1,345 @@ +/* + * 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: 11 Nov 2024 + */ +namespace Geometry +{ +public struct SPMatrix2x3 { + + public float r1c1, r1c2; + public float r2c1, r2c2; + public float r3c1, r3c2; + + public SPMatrix2x3() { + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + + this.r3c1 = 0.0f; + this.r3c2 = 0.0f; + } + + public SPMatrix2x3(SPMatrix2x3 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + } + + public SPMatrix2x3(DPMatrix2x3 matrix) { + this.r1c1 = (float) matrix.r1c1; + this.r1c2 = (float) matrix.r1c2; + + this.r2c1 = (float) matrix.r2c1; + this.r2c2 = (float) matrix.r2c2; + + this.r3c1 = (float) matrix.r3c1; + this.r3c2 = (float) matrix.r3c2; + } + + public SPMatrix2x3(SPMatrix3x2 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r1c3; + this.r3c2 = matrix.r2c3; + } + + public SPMatrix2x3(DPMatrix3x2 matrix) { + this.r1c1 = (float) matrix.r1c1; + this.r1c2 = (float) matrix.r2c1; + + this.r2c1 = (float) matrix.r1c2; + this.r2c2 = (float) matrix.r2c2; + + this.r3c1 = (float) matrix.r1c3; + this.r3c2 = (float) matrix.r2c3; + } + + public void Reset() { + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + + this.r3c1 = 0.0f; + this.r3c2 = 0.0f; + } + + public void SetValues(SPMatrix2x3 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + } + + public void SetValues(DPMatrix2x3 matrix) { + this.r1c1 = (float) matrix.r1c1; + this.r1c2 = (float) matrix.r1c2; + + this.r2c1 = (float) matrix.r2c1; + this.r2c2 = (float) matrix.r2c2; + + this.r3c1 = (float) matrix.r3c1; + this.r3c2 = (float) matrix.r3c2; + } + + public void SetTransposed(SPMatrix3x2 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r2c1; + + this.r2c1 = matrix.r1c2; + this.r2c2 = matrix.r2c2; + + this.r3c1 = matrix.r1c3; + this.r3c2 = matrix.r2c3; + } + + public void SetTransposed(DPMatrix3x2 matrix) { + this.r1c1 = (float) matrix.r1c1; + this.r1c2 = (float) matrix.r2c1; + + this.r2c1 = (float) matrix.r1c2; + this.r2c2 = (float) matrix.r2c2; + + this.r3c1 = (float) matrix.r1c3; + this.r3c2 = (float) matrix.r2c3; + } + + public void SetRow1(float c1, float c2) { + this.r1c1 = c1; + this.r1c2 = c2; + } + + public void SetRow2(float c1, float c2) { + this.r2c1 = c1; + this.r2c2 = c2; + } + + public void SetRow3(float c1, float c2) { + this.r3c1 = c1; + this.r3c2 = c2; + } + + public void SetColumn1(float r1, float r2, float r3) { + this.r1c1 = r1; + this.r2c1 = r2; + this.r3c1 = r3; + } + + public void SetColumn2(float r1, float r2, float r3) { + this.r1c2 = r1; + this.r2c2 = r2; + this.r3c2 = r3; + } + + public static void Add(SPMatrix2x3 matrix1, SPMatrix2x3 matrix2, SPMatrix2x3 result) { + result.r1c1 = matrix1.r1c1 + matrix2.r1c1; + result.r1c2 = matrix1.r1c2 + matrix2.r1c2; + + result.r2c1 = matrix1.r2c1 + matrix2.r2c1; + result.r2c2 = matrix1.r2c2 + matrix2.r2c2; + + result.r3c1 = matrix1.r3c1 + matrix2.r3c1; + result.r3c2 = matrix1.r3c2 + matrix2.r3c2; + } + + public static void Add3( + SPMatrix2x3 matrix1, + SPMatrix2x3 matrix2, + SPMatrix2x3 matrix3, + SPMatrix2x3 sum + ) { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; + + sum.r3c1 = matrix1.r3c1 + matrix2.r3c1 + matrix3.r3c1; + sum.r3c2 = matrix1.r3c2 + matrix2.r3c2 + matrix3.r3c2; + } + + public static void Add4( + SPMatrix2x3 matrix1, + SPMatrix2x3 matrix2, + SPMatrix2x3 matrix3, + SPMatrix2x3 matrix4, + SPMatrix2x3 sum + ) { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); + + sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1); + sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2); + } + + public static void Add5( + SPMatrix2x3 matrix1, + SPMatrix2x3 matrix2, + SPMatrix2x3 matrix3, + SPMatrix2x3 matrix4, + SPMatrix2x3 matrix5, + SPMatrix2x3 sum + ) { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; + + sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1) + matrix5.r3c1; + sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2) + matrix5.r3c2; + } + + public static void Subtract(SPMatrix2x3 minuend, SPMatrix2x3 subtrahend, SPMatrix2x3 difference) { + difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; + difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; + + difference.r2c1 = minuend.r2c1 - subtrahend.r2c1; + difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; + + difference.r3c1 = minuend.r3c1 - subtrahend.r3c1; + difference.r3c2 = minuend.r3c2 - subtrahend.r3c2; + } + + public static void GetWeightedSum2( + float weight1, SPMatrix2x3 matrix1, + float weight2, SPMatrix2x3 matrix2, + SPMatrix2x3 sum + ) { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; + } + + public static void GetWeightedSum3( + float weight1, SPMatrix2x3 matrix1, + float weight2, SPMatrix2x3 matrix2, + float weight3, SPMatrix2x3 matrix3, + SPMatrix2x3 sum + ) { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; + } + + public static void GetWeightedSum4( + float weight1, SPMatrix2x3 matrix1, + float weight2, SPMatrix2x3 matrix2, + float weight3, SPMatrix2x3 matrix3, + float weight4, SPMatrix2x3 matrix4, + SPMatrix2x3 sum + ) { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); + } + + public static void GetWeightedSum5( + float weight1, SPMatrix2x3 matrix1, + float weight2, SPMatrix2x3 matrix2, + float weight3, SPMatrix2x3 matrix3, + float weight4, SPMatrix2x3 matrix4, + float weight5, SPMatrix2x3 matrix5, + SPMatrix2x3 sum + ) { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; + } + + public static void Multiply(SPMatrix2x3 multiplicand, float multiplier, SPMatrix2x3 product) { + product.r1c1 = multiplicand.r1c1 * multiplier; + product.r1c2 = multiplicand.r1c2 * multiplier; + + product.r2c1 = multiplicand.r2c1 * multiplier; + product.r2c2 = multiplicand.r2c2 * multiplier; + + product.r3c1 = multiplicand.r3c1 * multiplier; + product.r3c2 = multiplicand.r3c2 * multiplier; + } + + public static void Divide(SPMatrix2x3 dividend, float divisor, SPMatrix2x3 quotient) { + quotient.r1c1 = dividend.r1c1 / divisor; + quotient.r1c2 = dividend.r1c2 / divisor; + + quotient.r2c1 = dividend.r2c1 / divisor; + quotient.r2c2 = dividend.r2c2 / divisor; + + quotient.r3c1 = dividend.r3c1 / divisor; + quotient.r3c2 = dividend.r3c2 / divisor; + } + + public static void GetRightProduct(SPMatrix2x3 matrix, SPVector2 vector, SPVector3 result) { + result.SetValues( + matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2, + matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2, + matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2 + + ); + } + + public static void GetLeftProduct(SPVector3 vector, SPMatrix2x3 matrix, SPVector2 result) { + result.SetValues( + vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1, + vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2 + ); + } +} +} \ No newline at end of file diff --git a/Geometry/SPMatrix3x2.cs b/Geometry/SPMatrix3x2.cs new file mode 100644 index 0000000..1ee9fe1 --- /dev/null +++ b/Geometry/SPMatrix3x2.cs @@ -0,0 +1,284 @@ +/* + * 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: 11 Nov 2024 + */ +namespace Geometry +{ +public class SPMatrix3x2 { + + public float r1c1, r1c2, r1c3; + + public float r2c1, r2c2, r2c3; + + public SPMatrix3x2() { + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + this.r2c3 = 0.0f; + } + + public SPMatrix3x2(SPMatrix3x2 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + } + + public SPMatrix3x2(DPMatrix3x2 matrix) { + this.r1c1 = (float) matrix.r1c1; + this.r1c2 = (float) matrix.r1c2; + this.r1c3 = (float) matrix.r1c3; + + this.r2c1 = (float) matrix.r2c1; + this.r2c2 = (float) matrix.r2c2; + this.r2c3 = (float) matrix.r2c3; + } + + public void Reset() { + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + this.r2c3 = 0.0f; + } + + + public void SetValues(SPMatrix3x2 matrix) { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + } + +/* + public void SetValues(DPMatrix3x2 matrix) { + this.r1c1 = (float) matrix.r1c1; + this.r1c2 = (float) matrix.r1c2; + this.r1c3 = (float) matrix.r1c3; + + this.r2c1 = (float) matrix.r2c1; + this.r2c2 = (float) matrix.r2c2; + this.r2c3 = (float) matrix.r2c3; + } +*/ + + public void SetRow1(float c1, float c2, float c3) { + this.r1c1 = c1; + this.r1c2 = c2; + this.r1c3 = c3; + } + + public void SetRow2(float c1, float c2, float c3) { + this.r2c1 = c1; + this.r2c2 = c2; + this.r2c3 = c3; + } + + public void SetColumn1(float r1, float r2) { + this.r1c1 = r1; + this.r2c1 = r2; + } + + public void SetColumn2(float r1, float r2) { + this.r1c3 = r1; + this.r2c3 = r2; + } + + public void SetColumn3(float r1, float r2) { + this.r1c3 = r1; + this.r2c3 = r2; + } + + public static void Add(SPMatrix3x2 matrix1, SPMatrix3x2 matrix2, SPMatrix3x2 sum) { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; + sum.r1c3 = matrix1.r1c3 + matrix2.r1c3; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2; + sum.r2c3 = matrix1.r2c3 + matrix2.r2c3; + } + + public static void Add3( + SPMatrix3x2 matrix1, + SPMatrix3x2 matrix2, + SPMatrix3x2 matrix3, + SPMatrix3x2 sum + ) { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; + sum.r1c3 = matrix1.r1c3 + matrix2.r1c3 + matrix3.r1c3; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; + sum.r2c3 = matrix1.r2c3 + matrix2.r2c3 + matrix3.r2c3; + } + + public static void Add4( + SPMatrix3x2 matrix1, + SPMatrix3x2 matrix2, + SPMatrix3x2 matrix3, + SPMatrix3x2 matrix4, + SPMatrix3x2 sum + ) { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); + sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3); + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); + sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3); + } + + public static void Add5( + SPMatrix3x2 matrix1, + SPMatrix3x2 matrix2, + SPMatrix3x2 matrix3, + SPMatrix3x2 matrix4, + SPMatrix3x2 matrix5, + SPMatrix3x2 sum + ) { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; + sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3) + matrix5.r1c3; + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; + sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3) + matrix5.r2c3; + } + + public static void Subtract(SPMatrix3x2 minuend, SPMatrix3x2 subtrahend, SPMatrix3x2 difference) { + difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; + difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; + difference.r1c3 = minuend.r1c3 - subtrahend.r1c3; + + difference.r2c1 = minuend.r2c1 - subtrahend.r2c1; + difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; + difference.r2c3 = minuend.r2c3 - subtrahend.r2c3; + } + + public static void GetWeightedSum2( + float weight1, SPMatrix3x2 matrix1, + float weight2, SPMatrix3x2 matrix2, + SPMatrix3x2 sum + ) { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; + } + + public static void GetWeightedSum3( + float weight1, SPMatrix3x2 matrix1, + float weight2, SPMatrix3x2 matrix2, + float weight3, SPMatrix3x2 matrix3, + SPMatrix3x2 sum + ) { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2 + matrix3.r1c3 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; + } + + public static void GetWeightedSum4( + float weight1, SPMatrix3x2 matrix1, + float weight2, SPMatrix3x2 matrix2, + float weight3, SPMatrix3x2 matrix3, + float weight4, SPMatrix3x2 matrix4, + SPMatrix3x2 sum + ) { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); + } + + public static void GetWeightedSum5( + float weight1, SPMatrix3x2 matrix1, + float weight2, SPMatrix3x2 matrix2, + float weight3, SPMatrix3x2 matrix3, + float weight4, SPMatrix3x2 matrix4, + float weight5, SPMatrix3x2 matrix5, + SPMatrix3x2 sum + ) { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4) + matrix5.r1c3 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4) + matrix5.r2c3 * weight5; + } + + public static void Multiply(SPMatrix3x2 multiplicand, float multiplier, SPMatrix3x2 product) { + product.r1c1 = multiplicand.r1c1 * multiplier; + product.r1c2 = multiplicand.r1c2 * multiplier; + product.r1c3 = multiplicand.r1c3 * multiplier; + + product.r2c1 = multiplicand.r2c1 * multiplier; + product.r2c2 = multiplicand.r2c2 * multiplier; + product.r2c3 = multiplicand.r2c3 * multiplier; + } + + public static void Divide(SPMatrix3x2 dividend, float divisor, SPMatrix3x2 quotient) { + quotient.r1c1 = dividend.r1c1 / divisor; + quotient.r1c2 = dividend.r1c2 / divisor; + quotient.r1c3 = dividend.r1c3 / divisor; + + quotient.r2c1 = dividend.r2c1 / divisor; + quotient.r2c2 = dividend.r2c2 / divisor; + quotient.r2c3 = dividend.r2c3 / divisor; + } + + public static void GetRightProduct(SPMatrix3x2 matrix, SPVector3 vector, SPVector2 result) { + result.SetValues( + matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3, + matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3 + ); + } + + public static void GetLeftProduct(SPVector2 vector, SPMatrix3x2 matrix, SPVector3 result) { + result.SetValues( + vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1, + vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2, + vector.x1 * matrix.r1c3 + vector.x2 * matrix.r2c3 + ); + } +} +} \ No newline at end of file diff --git a/Geometry/SPMatrix3x3.cs b/Geometry/SPMatrix3x3.cs new file mode 100644 index 0000000..6481ac9 --- /dev/null +++ b/Geometry/SPMatrix3x3.cs @@ -0,0 +1,606 @@ +/* + * 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 + */ + +namespace Geometry +{ + public struct SPMatrix3x3 + { + public float r1c1, r1c2, r1c3; + + public float r2c1, r2c2, r2c3; + + public float r3c1, r3c2, r3c3; + + public SPMatrix3x3() + { + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + this.r2c3 = 0.0f; + + this.r3c1 = 0.0f; + this.r3c2 = 0.0f; + this.r3c3 = 0.0f; + } + + public SPMatrix3x3(float d1, float d2, float d3) + { + this.r1c1 = d1; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = d2; + this.r2c3 = 0.0f; + + this.r3c1 = 0.0f; + this.r3c2 = 0.0f; + this.r3c3 = d3; + } + + public SPMatrix3x3(in SPMatrix3x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + this.r3c3 = matrix.r3c3; + } + + public SPMatrix3x3(in DPMatrix3x3 matrix) + { + this.r1c1 = (float)matrix.r1c1; + this.r1c2 = (float)matrix.r1c2; + this.r1c3 = (float)matrix.r1c3; + + this.r2c1 = (float)matrix.r2c1; + this.r2c2 = (float)matrix.r2c2; + this.r2c3 = (float)matrix.r2c3; + + this.r3c1 = (float)matrix.r3c1; + this.r3c2 = (float)matrix.r3c2; + this.r3c3 = (float)matrix.r3c3; + } + + public readonly float GetDeterminant() + { + return this.r1c1 * (this.r2c2 * this.r3c3 - this.r2c3 * this.r3c2) + + this.r1c2 * (this.r2c3 * this.r3c1 - this.r2c1 * this.r3c3) + + this.r1c3 * (this.r2c1 * this.r3c2 - this.r2c2 * this.r3c1); + } + + public readonly bool IsSingular() + { + float determinant = this.GetDeterminant(); + return -SPUtility.EPSYLON <= determinant && determinant <= SPUtility.EPSYLON; + } + + public void Reset() + { + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + this.r2c3 = 0.0f; + + this.r3c1 = 0.0f; + this.r3c2 = 0.0f; + this.r3c3 = 0.0f; + } + + public void MakeIdentity() + { + this.r1c1 = 1.0f; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 1.0f; + this.r2c3 = 0.0f; + + this.r3c1 = 0.0f; + this.r3c2 = 0.0f; + this.r3c3 = 1.0f; + } + + public void MakeDiagonal(float d1, float d2, float d3) + { + this.r1c1 = d1; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = d2; + this.r2c3 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + this.r2c3 = d3; + } + + public void Transpose() + { + (this.r1c2, this.r2c1) = (this.r2c1, this.r1c2); + (this.r1c3, this.r3c1) = (this.r3c1, this.r1c3); + (this.r2c3, this.r3c2) = (this.r3c2, this.r2c3); + } + + public bool Invert() + { + float determinant = this.GetDeterminant(); + + if (-SPUtility.EPSYLON <= determinant && determinant <= SPUtility.EPSYLON) { + return false; + } + + float r1c1 = this.r2c2 * this.r3c3 - this.r2c3 * this.r3c2; + float r1c2 = this.r1c3 * this.r3c2 - this.r1c2 * this.r3c3; + float r1c3 = this.r1c2 * this.r2c3 - this.r1c3 * this.r2c2; + + float r2c1 = this.r2c3 * this.r3c1 - this.r2c1 * this.r3c3; + float r2c2 = this.r1c1 * this.r3c3 - this.r1c3 * this.r3c1; + float r2c3 = this.r1c3 * this.r2c1 - this.r1c1 * this.r2c3; + + float r3c1 = this.r2c1 * this.r3c2 - this.r2c2 * this.r3c1; + float r3c2 = this.r1c2 * this.r3c1 - this.r1c1 * this.r3c2; + float r3c3 = this.r1c1 * this.r2c2 - this.r1c2 * this.r2c1; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + this.r1c3 = r1c3 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + this.r2c3 = r2c3 / determinant; + + this.r3c1 = r3c1 / determinant; + this.r3c2 = r3c2 / determinant; + this.r3c3 = r3c3 / determinant; + + return true; + } + + public void MakeTransposedOf(in SPMatrix3x3 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 MakeTransposedOf(in DPMatrix3x3 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 MakeInvertedOf(in SPMatrix3x3 matrix) + { + float determinant = matrix.GetDeterminant(); + + if (-SPUtility.EPSYLON <= determinant && determinant <= SPUtility.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; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + this.r1c3 = r1c3 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + this.r2c3 = r2c3 / determinant; + + this.r3c1 = r3c1 / determinant; + this.r3c2 = r3c2 / determinant; + this.r3c3 = r3c3 / determinant; + + return true; + } + + public void SetValues(in SPMatrix3x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r1c2 = matrix.r1c2; + this.r1c3 = matrix.r1c3; + + this.r2c1 = matrix.r2c1; + this.r2c2 = matrix.r2c2; + this.r2c3 = matrix.r2c3; + + this.r3c1 = matrix.r3c1; + this.r3c2 = matrix.r3c2; + this.r3c3 = matrix.r3c3; + } + + public void SetValues(in DPMatrix3x3 matrix) + { + this.r1c1 = (float)matrix.r1c1; + this.r1c2 = (float)matrix.r1c2; + this.r1c3 = (float)matrix.r1c3; + + this.r2c1 = (float)matrix.r2c1; + this.r2c2 = (float)matrix.r2c2; + this.r2c3 = (float)matrix.r2c3; + + this.r3c1 = (float)matrix.r3c1; + this.r3c2 = (float)matrix.r3c2; + this.r3c3 = (float)matrix.r3c3; + } + + public void SetMainDiagonal(float d1, float d2, float d3) + { + this.r1c1 = d1; + this.r2c2 = d2; + this.r3c3 = d3; + } + + public void SetRow1(float c1, float c2, float c3) + { + this.r1c1 = c1; + this.r1c2 = c2; + this.r1c3 = c3; + } + + public void SetRow2(float c1, float c2, float c3) + { + this.r2c1 = c1; + this.r2c2 = c2; + this.r2c3 = c3; + } + + public void SetRow3(float c1, float c2, float c3) + { + this.r3c1 = c1; + this.r3c2 = c2; + this.r3c3 = c3; + } + + public void SetColumn1(float r1, float r2, float r3) + { + this.r1c1 = r1; + this.r2c1 = r2; + this.r3c1 = r3; + } + + public void SetColumn2(float r1, float r2, float r3) + { + this.r1c3 = r1; + this.r2c3 = r2; + this.r3c3 = r3; + } + + public void SetColumn3(float r1, float r2, float r3) + { + this.r1c3 = r1; + this.r2c3 = r2; + this.r3c3 = r3; + } + + public static void Add(in SPMatrix3x3 matrix1, in SPMatrix3x3 matrix2, out SPMatrix3x3 sum) + { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2; + sum.r1c3 = matrix1.r1c3 + matrix2.r1c3; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2; + sum.r2c3 = matrix1.r2c3 + matrix2.r2c3; + + sum.r3c1 = matrix1.r3c1 + matrix2.r3c1; + sum.r3c2 = matrix1.r3c2 + matrix2.r3c2; + sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; + } + + public static void Add3( + in SPMatrix3x3 matrix1, + in SPMatrix3x3 matrix2, + in SPMatrix3x3 matrix3, + out SPMatrix3x3 sum + ) + { + sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; + sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; + sum.r1c3 = matrix1.r1c3 + matrix2.r1c3 + matrix3.r1c3; + + sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; + sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; + sum.r2c3 = matrix1.r2c3 + matrix2.r2c3 + matrix3.r2c3; + + sum.r3c1 = matrix1.r3c1 + matrix2.r3c1 + matrix3.r3c1; + sum.r3c2 = matrix1.r3c2 + matrix2.r3c2 + matrix3.r3c2; + sum.r3c3 = matrix1.r3c3 + matrix2.r3c3 + matrix3.r3c3; + } + + public static void Add4( + in SPMatrix3x3 matrix1, + in SPMatrix3x3 matrix2, + in SPMatrix3x3 matrix3, + in SPMatrix3x3 matrix4, + out SPMatrix3x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); + sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3); + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); + sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3); + + sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1); + sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2); + sum.r3c3 = (matrix1.r3c3 + matrix2.r3c3) + (matrix3.r3c3 + matrix4.r3c3); + } + + public static void Add5( + in SPMatrix3x3 matrix1, + in SPMatrix3x3 matrix2, + in SPMatrix3x3 matrix3, + in SPMatrix3x3 matrix4, + in SPMatrix3x3 matrix5, + out SPMatrix3x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; + sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; + sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3) + matrix5.r1c3; + + sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; + sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; + sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3) + matrix5.r2c3; + + sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1) + matrix5.r3c1; + sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2) + matrix5.r3c2; + sum.r3c3 = (matrix1.r3c3 + matrix2.r3c3) + (matrix3.r3c3 + matrix4.r3c3) + matrix5.r3c3; + } + + public static void Subtract(in SPMatrix3x3 minuend, in SPMatrix3x3 subtrahend, out SPMatrix3x3 difference) + { + difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; + difference.r1c2 = minuend.r1c2 - subtrahend.r1c2; + difference.r1c3 = minuend.r1c3 - subtrahend.r1c3; + + difference.r2c1 = minuend.r2c1 - subtrahend.r2c1; + difference.r2c2 = minuend.r2c2 - subtrahend.r2c2; + difference.r2c3 = minuend.r2c3 - subtrahend.r2c3; + + difference.r3c1 = minuend.r3c1 - subtrahend.r3c1; + difference.r3c2 = minuend.r3c2 - subtrahend.r3c2; + difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; + } + + public static void GetWeightedSum2( + float weight1, in SPMatrix3x3 matrix1, + float weight2, in SPMatrix3x3 matrix2, + out SPMatrix3x3 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; + sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2; + } + + public static void GetWeightedSum3( + float weight1, in SPMatrix3x3 matrix1, + float weight2, in SPMatrix3x3 matrix2, + float weight3, in SPMatrix3x3 matrix3, + out SPMatrix3x3 sum + ) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2 + matrix3.r1c3 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; + sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2 + matrix3.r3c3 * weight3; + } + + public static void GetWeightedSum4( + float weight1, in SPMatrix3x3 matrix1, + float weight2, in SPMatrix3x3 matrix2, + float weight3, in SPMatrix3x3 matrix3, + float weight4, in SPMatrix3x3 matrix4, + out SPMatrix3x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); + sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4); + } + + public static void GetWeightedSum5( + float weight1, in SPMatrix3x3 matrix1, + float weight2, in SPMatrix3x3 matrix2, + float weight3, in SPMatrix3x3 matrix3, + float weight4, in SPMatrix3x3 matrix4, + float weight5, in SPMatrix3x3 matrix5, + out SPMatrix3x3 sum + ) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4) + matrix5.r1c3 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4) + matrix5.r2c3 * weight5; + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; + sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4) + matrix5.r3c3 * weight5; + } + + public static void Multiply(in SPMatrix3x3 multiplicand, float multiplier, out SPMatrix3x3 product) + { + product.r1c1 = multiplicand.r1c1 * multiplier; + product.r1c2 = multiplicand.r1c2 * multiplier; + product.r1c3 = multiplicand.r1c3 * multiplier; + + product.r2c1 = multiplicand.r2c1 * multiplier; + product.r2c2 = multiplicand.r2c2 * multiplier; + product.r2c3 = multiplicand.r2c3 * multiplier; + + product.r3c1 = multiplicand.r3c1 * multiplier; + product.r3c2 = multiplicand.r3c2 * multiplier; + product.r3c3 = multiplicand.r3c3 * multiplier; + } + + public static void Divide(in SPMatrix3x3 dividend, float divisor, out SPMatrix3x3 quotient) + { + quotient.r1c1 = dividend.r1c1 / divisor; + quotient.r1c2 = dividend.r1c2 / divisor; + quotient.r1c3 = dividend.r1c3 / divisor; + + quotient.r2c1 = dividend.r2c1 / divisor; + quotient.r2c2 = dividend.r2c2 / divisor; + quotient.r2c3 = dividend.r2c3 / divisor; + + quotient.r3c1 = dividend.r3c1 / divisor; + quotient.r3c2 = dividend.r3c2 / divisor; + quotient.r3c3 = dividend.r3c3 / divisor; + } + + public static void GetRightProduct(in SPMatrix3x3 matrix, in SPVector3 vector, out SPVector3 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; + float x3 = matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2 + matrix.r3c3 * vector.x3; + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + public static void GetLeftProduct(in SPVector3 vector, in SPMatrix3x3 matrix, out SPVector3 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; + float x3 = vector.x1 * matrix.r1c3 + vector.x2 * matrix.r2c3 + vector.x3 * matrix.r3c3; + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + public static void LoadZero(out SPMatrix3x3 matrix) + { + matrix.r1c1 = 0.0f; + matrix.r1c2 = 0.0f; + matrix.r1c3 = 0.0f; + + matrix.r2c1 = 0.0f; + matrix.r2c2 = 0.0f; + matrix.r2c3 = 0.0f; + + matrix.r3c1 = 0.0f; + matrix.r3c2 = 0.0f; + matrix.r3c3 = 0.0f; + } + + public static void LoadIdentity(out SPMatrix3x3 matrix) + { + matrix.r1c1 = 1.0f; + matrix.r1c2 = 0.0f; + matrix.r1c3 = 0.0f; + + matrix.r2c1 = 0.0f; + matrix.r2c2 = 1.0f; + matrix.r2c3 = 0.0f; + + matrix.r3c1 = 0.0f; + matrix.r3c2 = 0.0f; + matrix.r3c3 = 1.0f; + } + + public static void LoadDiagonal(float d1, float d2, float d3, out SPMatrix3x3 matrix) + { + matrix.r1c1 = d1; + matrix.r1c2 = 0.0f; + matrix.r1c3 = 0.0f; + + matrix.r2c1 = 0.0f; + matrix.r2c2 = d2; + matrix.r2c3 = 0.0f; + + matrix.r3c1 = 0.0f; + matrix.r3c2 = 0.0f; + matrix.r3c3 = d3; + } + } +} diff --git a/Geometry/SPMatrixProduct.cs b/Geometry/SPMatrixProduct.cs new file mode 100644 index 0000000..cf39127 --- /dev/null +++ b/Geometry/SPMatrixProduct.cs @@ -0,0 +1,191 @@ +/* + * 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: 11 Nov 2024 + */ +namespace Geometry +{ + public class SPMatrixProduct + { + public static void Get2x2At2x2(in SPMatrix2x2 left, in SPMatrix2x2 right, out SPMatrix2x2 product) + { + float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; + float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; + + float r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1; + float r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + } + + public static void Get2x2At3x2(in SPMatrix2x2 left, in SPMatrix3x2 right, out SPMatrix3x2 product) + { + float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; + float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; + float r1c3 = left.r1c1 * right.r1c3 + left.r1c2 * right.r2c3; + + float r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1; + float r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2; + float r2c3 = left.r2c1 * right.r1c3 + left.r2c2 * right.r2c3; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + product.r1c3 = r1c3; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + product.r2c3 = r2c3; + } + + public static void Get2x3At2x2(in SPMatrix2x3 left, in SPMatrix2x2 right, out SPMatrix2x3 product) + { + float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; + float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; + + float r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1; + float r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2; + + float r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1; + float r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + + product.r3c1 = r3c1; + product.r3c2 = r3c2; + } + + public static void Get2x3At3x2(in SPMatrix2x3 left, in SPMatrix3x2 right, out SPMatrix3x3 product) + { + float r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1; + float r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2; + float r1c3 = left.r1c1 * right.r1c3 + left.r1c2 * right.r2c3; + + float r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1; + float r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2; + float r2c3 = left.r2c1 * right.r1c3 + left.r2c2 * right.r2c3; + + float r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1; + float r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2; + float r3c3 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c3; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + product.r1c3 = r1c3; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + product.r2c3 = r2c3; + + product.r3c1 = r3c1; + product.r3c2 = r3c2; + product.r3c3 = r3c3; + } + + public static void Get3x2At3x3(in SPMatrix3x2 left, in SPMatrix3x3 right, out SPMatrix3x2 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; + float r1c3 = left.r1c1 * right.r1c3 + left.r1c2 * right.r2c3 + left.r1c3 * right.r3c3; + + float r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1; + float r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; + float r2c3 = left.r2c1 * right.r1c3 + left.r2c2 * right.r2c3 + left.r2c3 * right.r3c3; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + product.r1c3 = r1c3; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + product.r2c3 = r2c3; + } + + public static void Get3x2At2x3(in SPMatrix3x2 left, in SPMatrix2x3 right, out SPMatrix2x2 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; + + float r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1; + float r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + } + + public static void Get3x3At2x3(in SPMatrix3x3 left, in SPMatrix2x3 right, out SPMatrix2x3 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; + + float r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1; + float r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; + + float r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1 + left.r3c3 * right.r3c1; + float r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2 + left.r3c3 * right.r3c2; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + + product.r3c1 = r3c1; + product.r3c2 = r3c2; + } + + public static void Get3x3At3x3(in SPMatrix3x3 left, in SPMatrix3x3 right, out SPMatrix3x3 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; + float r1c3 = left.r1c1 * right.r1c3 + left.r1c2 * right.r2c3 + left.r1c3 * right.r3c3; + + float r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1; + float r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2; + float r2c3 = left.r2c1 * right.r1c3 + left.r2c2 * right.r2c3 + left.r2c3 * right.r3c3; + + float r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1 + left.r3c3 * right.r3c1; + float r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2 + left.r3c3 * right.r3c2; + float r3c3 = left.r3c1 * right.r1c3 + left.r3c2 * right.r2c3 + left.r3c3 * right.r3c3; + + product.r1c1 = r1c1; + product.r1c2 = r1c2; + product.r1c3 = r1c3; + + product.r2c1 = r2c1; + product.r2c2 = r2c2; + product.r2c3 = r2c3; + + product.r3c1 = r3c1; + product.r3c2 = r3c2; + product.r3c3 = r3c3; + } + } +} diff --git a/Geometry/SPQuaternion.cs b/Geometry/SPQuaternion.cs new file mode 100644 index 0000000..2cee991 --- /dev/null +++ b/Geometry/SPQuaternion.cs @@ -0,0 +1,203 @@ +using System; + +namespace Geometry +{ + public struct SPQuaternion + { + public float s0, x1, x2, x3; + + public SPQuaternion(float s0, float x1, float x2, float x3) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + } + + public SPQuaternion(in SPQuaternion quaternion) + { + this.s0 = quaternion.s0; + this.x1 = quaternion.x1; + this.x2 = quaternion.x2; + this.x3 = quaternion.x3; + } + + public SPQuaternion(in DPQuaternion quaternion) + { + this.s0 = (float)quaternion.s0; + this.x1 = (float)quaternion.x1; + this.x2 = (float)quaternion.x2; + this.x3 = (float)quaternion.x3; + } + + public void Reset() + { + this.s0 = 0.0f; + this.x1 = 0.0f; + this.x2 = 0.0f; + this.x3 = 0.0f; + } + + public void Conjugate() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public readonly void GetConjugate(out SPQuaternion conjugated) + { + conjugated.s0 = this.s0; + conjugated.x1 = -this.x1; + conjugated.x2 = -this.x2; + conjugated.x3 = -this.x3; + } + + public void SetValues(float s0, float x1, float x2, float x3) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + } + + public void SetValues(in SPQuaternion quaternion) + { + this.s0 = quaternion.s0; + this.x1 = quaternion.x1; + this.x2 = quaternion.x2; + this.x3 = quaternion.x3; + } + + public void SetValues(in DPQuaternion quaternion) + { + this.s0 = (float)quaternion.s0; + this.x1 = (float)quaternion.x1; + this.x2 = (float)quaternion.x2; + this.x3 = (float)quaternion.x3; + } + + public readonly void MakeRotationMatrix(out SPMatrix3x3 matrix) + { + 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 squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + + if (-SPUtility.EPSYLON <= squareModule && squareModule <= SPUtility.EPSYLON) + { + SPMatrix3x3.LoadIdentity(out matrix); + return; + } + + float corrector1; + float corrector2; + + if (1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + SPUtility.TWO_EPSYLON) { + corrector1 = 2.0f - squareModule; + corrector2 = 2.0f * corrector1; + } + else { + corrector1 = 1.0f / squareModule; + corrector2 = 2.0f / squareModule; + } + + 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; + + 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); + } + + public readonly void MakeReverseMatrix(out SPMatrix3x3 matrix) + { + 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 squareModule = (s0s0 + x1x1) + (x2x2 + x3x3); + + if (-SPUtility.EPSYLON <= squareModule && squareModule <= SPUtility.EPSYLON) + { + SPMatrix3x3.LoadIdentity(out matrix); + return; + } + + float corrector1; + float corrector2; + + if (1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + SPUtility.TWO_EPSYLON) { + corrector1 = 2.0f - squareModule; + corrector2 = 2.0f * corrector1; + } + else { + corrector1 = 1.0f / squareModule; + corrector2 = 2.0f / squareModule; + } + + 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; + + 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); + } + + public static void Add(in SPQuaternion quaternion1, in SPQuaternion quaternion2, out SPQuaternion sum) + { + sum.s0 = quaternion1.s0 + quaternion2.s0; + sum.x1 = quaternion1.x1 + quaternion2.x1; + sum.x2 = quaternion1.x2 + quaternion2.x2; + sum.x3 = quaternion1.x3 + quaternion2.x3; + } + + public static void Subtract(in SPQuaternion minuend, in SPQuaternion subtrahend, out SPQuaternion difference) + { + difference.s0 = minuend.s0 - subtrahend.s0; + difference.x1 = minuend.x1 - subtrahend.x1; + difference.x2 = minuend.x2 - subtrahend.x2; + difference.x3 = minuend.x3 - subtrahend.x3; + } + + public static void Multiply(in SPQuaternion left, in SPQuaternion right, out SPQuaternion 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); + + product.s0 = s0; + product.x1 = x1; + product.x2 = x2; + product.x3 = x3; + } + } +} diff --git a/Geometry/SPRotation3.cs b/Geometry/SPRotation3.cs new file mode 100644 index 0000000..ce96b64 --- /dev/null +++ b/Geometry/SPRotation3.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 Geometry +{ + public struct SPRotation3 + { + private float angle; + + private SPVector3 axis; + + public SPRotation3(SPRotation3 rotation) + { + this.angle = rotation.angle; + this.axis = rotation.axis; + } + + public void Reset() + { + this.angle = 0.0f; + this.axis.Reset(); + } + } +} diff --git a/Geometry/SPUtility.cs b/Geometry/SPUtility.cs new file mode 100644 index 0000000..203d540 --- /dev/null +++ b/Geometry/SPUtility.cs @@ -0,0 +1,21 @@ +using System; + +namespace Geometry +{ + public class SPUtility + { + 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/Geometry/SPVector2.cs b/Geometry/SPVector2.cs new file mode 100644 index 0000000..07098c8 --- /dev/null +++ b/Geometry/SPVector2.cs @@ -0,0 +1,383 @@ +/* + * 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: 1 Feb 2019 + */ + +namespace Geometry +{ + public struct SPVector2 + { + public static readonly SPVector2 ZERO = new SPVector2(0.0f, 0.0f); + + public float x1; + public float x2; + + public SPVector2(float x1, float x2) + { + this.x1 = x1; + this.x2 = x2; + } + + public SPVector2(in SPVector2 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + } + + public SPVector2(in DPVector2 vector) + { + this.x1 = (float)vector.x1; + this.x2 = (float)vector.x2; + } + + public readonly float GetSquareModule() + { + return this.x1 * this.x1 + this.x2 * this.x2; + } + + public readonly float GetModule() + { + return MathF.Sqrt(this.GetSquareModule()); + } + + public int Normalize() + { + float squareModule = this.GetSquareModule(); + + if (1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + SPUtility.TWO_EPSYLON) + { + return 1; + } + + if (squareModule <= SPUtility.SQUARE_EPSYLON) + { + this.Reset(); + return 0; + } + + float module = MathF.Sqrt(squareModule); + + this.x1 /= module; + this.x2 /= module; + + return 1; + } + + public readonly SPVector2 GetNormalized() + { + float squareModule = this.GetSquareModule(); + + if (1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + SPUtility.TWO_EPSYLON) + { + return this; + } + + if (squareModule <= SPUtility.SQUARE_EPSYLON) + { + return ZERO; + } + + float module = MathF.Sqrt(squareModule); + + return new SPVector2( + this.x1 / module, + this.x2 / module + ); + } + + public void Invert() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + } + + public readonly SPVector2 GetInverted() + { + return new SPVector2(-this.x1, -this.x2); + } + + public readonly bool IsZero() + { + return this.GetSquareModule() <= SPUtility.SQUARE_EPSYLON; + } + + public readonly bool IsUnit() + { + float squareModule = this.GetSquareModule(); + return 1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= SPUtility.EPSYLON; + } + + public void Reset() + { + this.x1 = 0.0f; + this.x2 = 0.0f; + } + + public void SetValues(float x1, float x2) + { + this.x1 = x1; + this.x2 = x2; + } + + public void SetValues(in DPVector2 vector) + { + this.x1 = (float)vector.x1; + this.x2 = (float)vector.x2; + } + + public void SetValues(in SPVector2 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + } + + public readonly override string ToString() + { + return String.Format("SPVector2({0}, {1})", this.x1, this.x2); + } + + public static void Add(in SPVector2 vector1, in SPVector2 vector2, out SPVector2 sum) + { + sum.x1 = vector1.x1 + vector2.x1; + sum.x2 = vector1.x2 + vector2.x2; + } + + public static void Add3( + in SPVector2 vector1, + in SPVector2 vector2, + in SPVector2 vector3, + out SPVector2 sum + ) + { + sum.x1 = vector1.x1 + vector2.x1 + vector3.x1; + sum.x2 = vector1.x2 + vector2.x2 + vector3.x2; + } + + public static void Add4( + in SPVector2 vector1, + in SPVector2 vector2, + in SPVector2 vector3, + in SPVector2 vector4, + out SPVector2 sum + ) + { + sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1); + sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2); + } + + public static void Add5( + in SPVector2 vector1, + in SPVector2 vector2, + in SPVector2 vector3, + in SPVector2 vector4, + in SPVector2 vector5, + out SPVector2 sum + ) + { + sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1; + sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2; + } + + public static void Subtract(in SPVector2 minuend, in SPVector2 subtrahend, out SPVector2 difference) + { + difference.x1 = minuend.x1 - subtrahend.x1; + difference.x2 = minuend.x2 - subtrahend.x2; + } + + public static void GetWeightedSum2( + float weight1, in SPVector2 vector1, + float weight2, in SPVector2 vector2, + out SPVector2 sum + ) + { + sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2; + sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2; + } + + public static void GetWeightedSum3( + float weight1, in SPVector2 vector1, + float weight2, in SPVector2 vector2, + float weight3, in SPVector2 vector3, + out SPVector2 sum + ) + { + sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2 + vector3.x1 * weight3; + sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2 + vector3.x2 * weight3; + } + + public static void GetWeightedSum4( + float weight1, in SPVector2 vector1, + float weight2, in SPVector2 vector2, + float weight3, in SPVector2 vector3, + float weight4, in SPVector2 vector4, + out SPVector2 sum + ) + { + sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4); + sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4); + } + + public static void GetWeightedSum5( + float weight1, in SPVector2 vector1, + float weight2, in SPVector2 vector2, + float weight3, in SPVector2 vector3, + float weight4, in SPVector2 vector4, + float weight5, in SPVector2 vector5, + out SPVector2 sum + ) + { + sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4); + sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4); + } + + public static void Muliply(in SPVector2 multiplicand, float multiplier, out SPVector2 product) + { + product.x1 = multiplicand.x1 * multiplier; + product.x2 = multiplicand.x2 * multiplier; + } + + public static void Divide(in SPVector2 dividend, float divisor, out SPVector2 quotient) + { + quotient.x1 = dividend.x1 / divisor; + quotient.x2 = dividend.x2 / divisor; + } + + public static void GetMean2( + in SPVector2 vector1, + in SPVector2 vector2, + out SPVector2 result + ) + { + result.x1 = (vector1.x1 + vector2.x1) * 0.5f; + result.x2 = (vector1.x2 + vector2.x2) * 0.5f; + } + + public static void GetMean3( + in SPVector2 vector1, + in SPVector2 vector2, + in SPVector2 vector3, + out SPVector2 result + ) + { + result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * SPUtility.ONE_THIRD; + result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * SPUtility.ONE_THIRD; + } + + public static void GetMean4( + in SPVector2 vector1, + in SPVector2 vector2, + in SPVector2 vector3, + in SPVector2 vector4, + out SPVector2 result + ) + { + result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1)) * 0.25f; + result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2)) * 0.25f; + } + + public static void GetMean5( + in SPVector2 vector1, + in SPVector2 vector2, + in SPVector2 vector3, + in SPVector2 vector4, + in SPVector2 vector5, + out SPVector2 result + ) + { + result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1) * 0.2f; + result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2) * 0.2f; + } + + public static float GetScalarProduct(in SPVector2 vector1, in SPVector2 vector2) + { + return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2; + } + + public static float GetCrossProduct(in SPVector2 vector1, in SPVector2 vector2) + { + return vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1; + } + + public static float GetAngle(in SPVector2 vector1, in SPVector2 vector2, AngleUnit unit) + { + float squareModule1 = vector1.GetSquareModule(); + + if (squareModule1 <= SPUtility.SQUARE_EPSYLON) + { + return 0.0f; + } + + float squareModule2 = vector2.GetSquareModule(); + + if (squareModule2 <= SPUtility.SQUARE_EPSYLON) + { + return 0.0f; + } + + float cosine = SPVector2.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); + + if (1.0f - SPUtility.EPSYLON <= cosine) + { + return 0.0f; + } + + if (cosine <= -(1.0f - SPUtility.EPSYLON)) + { + return SPAngle.GetHalfCircle(unit); + } + + return SPAngle.ConvertFromRadians(MathF.Acos(cosine), unit); + } + + public static float GetSquareDistance(in SPVector2 vector1, in SPVector2 vector2) + { + float dx1 = vector1.x1 - vector2.x1; + float dx2 = vector1.x2 - vector2.x2; + + return dx1 * dx1 + dx2 * dx2; + } + + public static float GetDistance(in SPVector2 vector1, in SPVector2 vector2) + { + return MathF.Sqrt(GetSquareDistance(vector1, vector2)); + } + + public static bool AreEqual(in SPVector2 vector1, in SPVector2 vector2) + { + float squareModule1 = vector1.GetSquareModule(); + float squareModule2 = vector2.GetSquareModule(); + float squareModule3 = GetSquareDistance(vector1, vector2); + + // 2.0f means dimension amount + if (squareModule1 < SPUtility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < SPUtility.EPSYLON_EFFECTIVENESS_LIMIT) + { + return squareModule3 < (2.0f * SPUtility.SQUARE_EPSYLON); + } + + if (squareModule1 <= squareModule2) + { + return squareModule3 <= (2.0f * SPUtility.SQUARE_EPSYLON) * squareModule2; + } + + return squareModule3 <= (2.0f * SPUtility.SQUARE_EPSYLON) * squareModule1; + } + } +} diff --git a/Geometry/SPVector3.cs b/Geometry/SPVector3.cs new file mode 100644 index 0000000..eaff507 --- /dev/null +++ b/Geometry/SPVector3.cs @@ -0,0 +1,434 @@ +/* + * 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: 1 Feb 2019 + */ + +namespace Geometry +{ + public struct SPVector3 + { + public static readonly SPVector3 ZERO = new SPVector3(0.0f, 0.0f, 0.0f); + + public float x1; + public float x2; + public float x3; + + public SPVector3(float x1, float x2, float x3) + { + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + } + + public SPVector3(in SPVector3 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + this.x3 = vector.x3; + } + + public SPVector3(in DPVector3 vector) + { + this.x1 = (float)vector.x1; + this.x2 = (float)vector.x2; + this.x3 = (float)vector.x3; + } + + public readonly float GetSquareModule() + { + return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3; + } + + public readonly float GetModule() + { + return MathF.Sqrt(this.GetSquareModule()); + } + + public int Normalize() + { + float squareModule = this.GetSquareModule(); + + if (1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + SPUtility.TWO_EPSYLON) + { + return 1; + } + + if (squareModule <= SPUtility.SQUARE_EPSYLON) + { + this.Reset(); + return 0; + } + + float module = MathF.Sqrt(squareModule); + + this.x1 /= module; + this.x2 /= module; + this.x3 /= module; + + return 1; + } + + public readonly SPVector3 GetNormalized() + { + float squareModule = this.GetSquareModule(); + + if (1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + SPUtility.TWO_EPSYLON) + { + return this; + } + + if (squareModule <= SPUtility.SQUARE_EPSYLON) + { + return ZERO; + } + + float module = MathF.Sqrt(squareModule); + + return new SPVector3( + this.x1 / module, + this.x2 / module, + this.x3 / module + ); + } + + public void Invert() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public readonly SPVector3 GetInverted() + { + return new SPVector3(-this.x1, -this.x2, -this.x3); + } + + public readonly bool IsZero() + { + return this.GetSquareModule() <= SPUtility.SQUARE_EPSYLON; + } + + public readonly bool IsUnit() + { + float squareModule = this.GetSquareModule(); + return 1.0f - SPUtility.TWO_EPSYLON <= squareModule && squareModule <= SPUtility.EPSYLON; + } + + public void Reset() + { + this.x1 = 0.0f; + this.x2 = 0.0f; + this.x3 = 0.0f; + } + + public void SetValues(float x1, float x2, float x3) + { + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + } + + public void SetValues(in DPVector3 vector) + { + this.x1 = (float)vector.x1; + this.x2 = (float)vector.x2; + this.x3 = (float)vector.x3; + } + + public void SetValues(in SPVector3 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + this.x3 = vector.x3; + } + + public readonly override string ToString() + { + return String.Format("SPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3); + } + + public static void Add(in SPVector3 vector1, in SPVector3 vector2, out SPVector3 sum) + { + sum.x1 = vector1.x1 + vector2.x1; + sum.x2 = vector1.x2 + vector2.x2; + sum.x3 = vector1.x3 + vector2.x3; + } + + public static void Add3( + in SPVector3 vector1, + in SPVector3 vector2, + in SPVector3 vector3, + out SPVector3 sum + ) + { + sum.x1 = vector1.x1 + vector2.x1 + vector3.x1; + sum.x2 = vector1.x2 + vector2.x2 + vector3.x2; + sum.x3 = vector1.x3 + vector2.x3 + vector3.x3; + } + + public static void Add4( + in SPVector3 vector1, + in SPVector3 vector2, + in SPVector3 vector3, + in SPVector3 vector4, + out SPVector3 sum + ) + { + sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1); + sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2); + sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3); + } + + public static void Add5( + in SPVector3 vector1, + in SPVector3 vector2, + in SPVector3 vector3, + in SPVector3 vector4, + in SPVector3 vector5, + out SPVector3 sum + ) + { + sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1; + sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2; + sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3; + } + + public static void Subtract(in SPVector3 minuend, in SPVector3 subtrahend, out SPVector3 difference) + { + difference.x1 = minuend.x1 - subtrahend.x1; + difference.x2 = minuend.x2 - subtrahend.x2; + difference.x3 = minuend.x3 - subtrahend.x3; + } + + public static void GetWeightedSum2( + float weight1, in SPVector3 vector1, + float weight2, in SPVector3 vector2, + out SPVector3 sum + ) + { + sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2; + sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2; + sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2; + } + + public static void GetWeightedSum3( + float weight1, in SPVector3 vector1, + float weight2, in SPVector3 vector2, + float weight3, in SPVector3 vector3, + out SPVector3 sum + ) + { + sum.x1 = vector1.x1 * weight1 + vector2.x1 * weight2 + vector3.x1 * weight3; + sum.x2 = vector1.x2 * weight1 + vector2.x2 * weight2 + vector3.x2 * weight3; + sum.x3 = vector1.x3 * weight1 + vector2.x3 * weight2 + vector3.x3 * weight3; + } + + public static void GetWeightedSum4( + float weight1, in SPVector3 vector1, + float weight2, in SPVector3 vector2, + float weight3, in SPVector3 vector3, + float weight4, in SPVector3 vector4, + out SPVector3 sum + ) + { + sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4); + sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4); + sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4); + } + + public static void GetWeightedSum5( + float weight1, in SPVector3 vector1, + float weight2, in SPVector3 vector2, + float weight3, in SPVector3 vector3, + float weight4, in SPVector3 vector4, + float weight5, in SPVector3 vector5, + out SPVector3 sum + ) + { + sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4) + vector5.x1 * weight5; + sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4) + vector5.x2 * weight5; + sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4) + vector5.x3 * weight5; + } + + public static void Muliply(in SPVector3 multiplicand, float multiplier, out SPVector3 product) + { + product.x1 = multiplicand.x1 * multiplier; + product.x2 = multiplicand.x2 * multiplier; + product.x3 = multiplicand.x3 * multiplier; + } + + public static void Divide(in SPVector3 dividend, float divisor, out SPVector3 quotient) + { + quotient.x1 = dividend.x1 / divisor; + quotient.x2 = dividend.x2 / divisor; + quotient.x3 = dividend.x3 / divisor; + } + + public static void GetMean2( + in SPVector3 vector1, + in SPVector3 vector2, + out SPVector3 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 SPVector3 vector1, + in SPVector3 vector2, + in SPVector3 vector3, + out SPVector3 result + ) + { + result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * SPUtility.ONE_THIRD; + result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * SPUtility.ONE_THIRD; + result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * SPUtility.ONE_THIRD; + } + + public static void GetMean4( + in SPVector3 vector1, + in SPVector3 vector2, + in SPVector3 vector3, + in SPVector3 vector4, + out SPVector3 result + ) + { + result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1)) * 0.25f; + result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2)) * 0.25f; + result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3)) * 0.25f; + } + + public static void GetMean5( + in SPVector3 vector1, + in SPVector3 vector2, + in SPVector3 vector3, + in SPVector3 vector4, + in SPVector3 vector5, + out SPVector3 result + ) + { + result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1) * 0.2f; + result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2) * 0.2f; + result.x3 = ((vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3) * 0.2f; + } + + public static float GetScalarProduct(in SPVector3 vector1, in SPVector3 vector2) + { + return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3; + } + + public static void GetCrossProduct(in SPVector3 vector1, in SPVector3 vector2, out SPVector3 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 SPVector3 vector1, in SPVector3 vector2, in SPVector3 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 SPVector3 vector1, in SPVector3 vector2, in SPVector3 vector3, out SPVector3 result) + { + // [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; + } + + public static float GetAngle(in SPVector3 vector1, in SPVector3 vector2, AngleUnit unit) + { + float squareModule1 = vector1.GetSquareModule(); + + if (squareModule1 <= SPUtility.SQUARE_EPSYLON) + { + return 0.0f; + } + + float squareModule2 = vector2.GetSquareModule(); + + if (squareModule2 <= SPUtility.SQUARE_EPSYLON) + { + return 0.0f; + } + + float cosine = SPVector3.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2); + + if (1.0f - SPUtility.EPSYLON <= cosine) + { + return 0.0f; + } + + if (cosine <= -(1.0f - SPUtility.EPSYLON)) + { + return SPAngle.GetHalfCircle(unit); + } + + return SPAngle.ConvertFromRadians(MathF.Acos(cosine), unit); + } + + public static float GetSquareDistance(in SPVector3 vector1, in SPVector3 vector2) + { + float dx1 = vector1.x1 - vector2.x1; + float dx2 = vector1.x2 - vector2.x2; + float dx3 = vector1.x3 - vector2.x3; + + return dx1 * dx1 + dx2 * dx2 + dx3 * dx3; + } + + public static float GetDistance(in SPVector3 vector1, in SPVector3 vector2) + { + return MathF.Sqrt(GetSquareDistance(vector1, vector2)); + } + + public static bool AreEqual(in SPVector3 vector1, in SPVector3 vector2) + { + float squareModule1 = vector1.GetSquareModule(); + float squareModule2 = vector2.GetSquareModule(); + float squareModule3 = GetSquareDistance(vector1, vector2); + + // 3.0f means dimension amount + if (squareModule1 < SPUtility.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < SPUtility.EPSYLON_EFFECTIVENESS_LIMIT) + { + return squareModule3 < (3.0f * SPUtility.SQUARE_EPSYLON); + } + + if (squareModule1 <= squareModule2) + { + return squareModule3 <= (3.0f * SPUtility.SQUARE_EPSYLON) * squareModule2; + } + + return squareModule3 <= (3.0f * SPUtility.SQUARE_EPSYLON) * squareModule1; + } + } +} diff --git a/Geometry/SPVersor.cs b/Geometry/SPVersor.cs new file mode 100644 index 0000000..8338b5a --- /dev/null +++ b/Geometry/SPVersor.cs @@ -0,0 +1,366 @@ +/* + * 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 + */ + +namespace Geometry +{ + public struct SPVersor + { + public static readonly SPVersor IDLE = new SPVersor(1.0f, 0.0f, 0.0f, 0.0f, 1.0f); + + private float s0; + private float x1; + private float x2; + private float x3; + private float corrector; + + private SPVersor(float s0, float x1, float x2, float x3, float corrector) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + this.corrector = corrector; + } + + public SPVersor() + { + this.s0 = 1.0f; + this.x1 = 0.0f; + this.x2 = 0.0f; + this.x3 = 0.0f; + this.corrector = 1.0f; + } + + public SPVersor(float s0, float x1, float x2, float x3) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + + float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + this.corrector = 2.0f - squareModule; + + if (squareModule < 1.0f - SPUtility.TWO_EPSYLON || 1.0f + SPUtility.TWO_EPSYLON < squareModule) + { + this.Normalize(squareModule); + } + else if (s0 < -1.0f + SPUtility.EPSYLON || 1.0f - SPUtility.EPSYLON < s0) + { + this.Reset(); + } + } + + public SPVersor(in SPVersor versor) + { + this.s0 = versor.s0; + this.x1 = versor.x1; + this.x2 = versor.x2; + this.x3 = versor.x3; + this.corrector = versor.corrector; + } + + public SPVersor(in DPVersor versor) + { + this.s0 = (float)versor.GetScalar(); + this.x1 = (float)versor.GetX1(); + this.x2 = (float)versor.GetX2(); + this.x3 = (float)versor.GetX3(); + + float squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); + + this.corrector = 2.0f - squareModule; + + if (squareModule < 1.0f - SPUtility.TWO_EPSYLON || 1.0f + SPUtility.TWO_EPSYLON < squareModule) + { + this.Normalize(squareModule); + } + else if (s0 < -1.0f + SPUtility.EPSYLON || 1.0f - SPUtility.EPSYLON < s0) + { + this.Reset(); + } + } + + public readonly float GetScalar() + { + return this.s0; + } + + public readonly float GetX1() + { + return this.x1; + } + + public readonly float GetX2() + { + return this.x2; + } + + public readonly float GetX3() + { + return this.x3; + } + + public readonly float GetCorrector() + { + return this.corrector; + } + + public readonly bool IsIdle() + { + return this.s0 <= -(1.0f - SPUtility.EPSYLON) || (1.0f - SPUtility.EPSYLON) <= this.s0; + } + + public void Reset() + { + this.s0 = 1.0f; + this.x1 = 0.0f; + this.x2 = 0.0f; + this.x3 = 0.0f; + this.corrector = 1.0f; + } + + public void Invert() + { + this.x1 = -this.x1; + this.x2 = -this.x2; + this.x3 = -this.x3; + } + + public readonly float GetAngle(AngleUnit unit) + { + if (this.s0 <= -(1.0f - SPUtility.TWO_EPSYLON) || 1.0f - SPUtility.TWO_EPSYLON <= this.s0) { + return 0.0f; + } + + if (-SPUtility.EPSYLON <= this.s0 && this.s0 <= SPUtility.EPSYLON) + { + return SPAngle.GetHalfCircle(unit); + } + + return SPAngle.ConvertFromRadians(2.0f * MathF.Acos(s0), unit); + } + + public readonly void MakeRotationMatrix(out SPMatrix3x3 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 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 * this.corrector; + + matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix.r3c3 = this.corrector * ((s0s0 + x3x3) - (x1x1 + x2x2)); + + matrix.r1c2 = corrector2 * (x1x2 - s0x3); + matrix.r2c3 = corrector2 * (x2x3 - s0x1); + matrix.r3c1 = corrector2 * (x1x3 - s0x2); + + matrix.r2c1 = corrector2 * (x1x2 + s0x3); + matrix.r3c2 = corrector2 * (x2x3 + s0x1); + matrix.r1c3 = corrector2 * (x1x3 + s0x2); + } + + public readonly void MakeReverseMatrix(out SPMatrix3x3 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 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 * this.corrector; + + matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix.r3c3 = this.corrector * ((s0s0 + x3x3) - (x1x1 + x2x2)); + + matrix.r1c2 = corrector2 * (x1x2 + s0x3); + matrix.r2c3 = corrector2 * (x2x3 + s0x1); + matrix.r3c1 = corrector2 * (x1x3 + s0x2); + + matrix.r2c1 = corrector2 * (x1x2 - s0x3); + matrix.r3c2 = corrector2 * (x2x3 - s0x1); + matrix.r1c3 = corrector2 * (x1x3 - s0x2); + } + + public void SetValues(float s0, float x1, float x2, float x3) + { + this.s0 = s0; + this.x1 = x1; + this.x2 = x2; + this.x3 = x3; + + float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + if (squareModule < 1.0f - SPUtility.TWO_EPSYLON || 1.0f + SPUtility.TWO_EPSYLON < squareModule) + { + this.Normalize(squareModule); + return; + } + + if (s0 < -1.0f + SPUtility.EPSYLON || 1.0f - SPUtility.EPSYLON < s0) { + this.Reset(); + return; + } + + this.corrector = 2.0f - squareModule; + } + + public void SetValues(in SPVersor versor) + { + this.s0 = versor.s0; + this.x1 = versor.x1; + this.x2 = versor.x2; + this.x3 = versor.x3; + this.corrector = versor.corrector; + } + + public void SetValues(in DPVersor versor) + { + this.SetValues( + (float)versor.GetScalar(), + (float)versor.GetX1(), + (float)versor.GetX2(), + (float)versor.GetX3() + ); + } + + public void SetInverted(in SPVersor versor) + { + this.s0 = versor.s0; + this.x1 = -versor.x1; + this.x2 = -versor.x2; + this.x3 = -versor.x3; + this.corrector = versor.corrector; + } + + public void SetInverted(in DPVersor versor) + { + this.SetValues( + (float)versor.GetScalar(), + -(float)versor.GetX1(), + -(float)versor.GetX2(), + -(float)versor.GetX3() + ); + } + + public readonly void Turn(in SPVector3 vector, out SPVector3 result) + { + float multiplier = 2.0f * this.corrector; + float tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); + float tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); + float tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1); + + float 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); + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + public readonly void TurnBack(in SPVector3 vector, out SPVector3 result) + { + float multiplier = 2.0f * this.corrector; + float tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); + float tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); + float tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1); + + float 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); + + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + } + + private void Normalize(float squareModule) + { + if (squareModule <= SPUtility.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= SPUtility.SQUARE_EPSYLON * squareModule) + { + this.Reset(); + return; + } + + float module = MathF.Sqrt(squareModule); + + this.s0 /= module; + this.x1 /= module; + this.x2 /= module; + this.x3 /= module; + + this.corrector = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); + } + + public static void Combine(in SPVersor second, in SPVersor first, out SPVersor result) + { + float s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3); + float x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3); + float x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1); + float x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2); + + float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + result.corrector = 2.0f - squareModule; + result.s0 = s0; + result.x1 = x1; + result.x2 = x2; + result.x3 = x3; + + if (squareModule < 1.0f - SPUtility.TWO_EPSYLON || 1.0f + SPUtility.TWO_EPSYLON < squareModule) + { + result.Normalize(squareModule); + } + } + + public static void LoadIdle(out SPVersor versor) + { + versor.corrector = 1.0f; + versor.s0 = 1.0f; + versor.x1 = 0.0f; + versor.x2 = 0.0f; + versor.x3 = 0.0f; + } + } +} diff --git a/GeometryDev/GeometryDev.csproj b/GeometryDev/GeometryDev.csproj new file mode 100644 index 0000000..3ced9b4 --- /dev/null +++ b/GeometryDev/GeometryDev.csproj @@ -0,0 +1,14 @@ + + + + Exe + net8.0 + enable + enable + + + + + + + diff --git a/GeometryDev/Program.cs b/GeometryDev/Program.cs new file mode 100644 index 0000000..8ff67f0 --- /dev/null +++ b/GeometryDev/Program.cs @@ -0,0 +1,80 @@ +// See https://aka.ms/new-console-template for more information +using System; +using System.ComponentModel; +using System.Diagnostics; +using Geometry; + +public static class Program +{ + private static SPVersor[] AllocateVersors(int amount) + { + return new SPVersor[amount]; + } + + private static SPVersor[] MakeZeroVersors(int amount) + { + SPVersor[] versors = AllocateVersors(amount); + + for (int i = 0; i < amount; i++) + { + versors[i] = SPVersor.IDLE; + } + + return versors; + } + + private static SPVersor[] MakeRandomVersors(int amount) + { + Random randomizer = new Random(Environment.TickCount); + + SPVersor[] versors = AllocateVersors(amount); + + for (int i = 0; i < amount; i++) + { + versors[i] = new SPVersor( + randomizer.NextSingle(), + randomizer.NextSingle(), + randomizer.NextSingle(), + randomizer.NextSingle() + ); + } + + return versors; + } + + private static void PrintVersor(in SPVersor versor) + { + Console.WriteLine("({0}, {1}, {2}, {3}) / {4:E}", versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3(), versor.GetCorrector() - 1.0f); + } + + public static int Main() + { + int amount = 1000000; + + SPVersor[] versors1 = MakeRandomVersors(amount); + SPVersor[] versors2 = MakeRandomVersors(amount); + SPVersor[] results = MakeZeroVersors(amount); + + long start, end; + + start = Environment.TickCount64; + + for (int j = 0; j < 1000; j++) + { + for (int i = 0; i < amount; i++) + { + SPVersor.Combine(versors1[i], versors2[i], out results[i]); + } + } + + end = Environment.TickCount64; + + Console.WriteLine("Time: {0}", end - start); + + PrintVersor(versors1[10]); + PrintVersor(versors2[10]); + PrintVersor(results[10]); + + return 0; + } +} \ No newline at end of file diff --git a/GeometryTest/GeometryTest.csproj b/GeometryTest/GeometryTest.csproj new file mode 100644 index 0000000..09678fe --- /dev/null +++ b/GeometryTest/GeometryTest.csproj @@ -0,0 +1,18 @@ + + + + net8.0 + enable + + false + + + + + + + + + + + diff --git a/GeometryTest/Vector2FloatTest.cs b/GeometryTest/Vector2FloatTest.cs new file mode 100644 index 0000000..d2c2377 --- /dev/null +++ b/GeometryTest/Vector2FloatTest.cs @@ -0,0 +1,19 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; + +using System; +using System.IO; + +using Geometry; + +namespace GeometryTest +{ + [TestClass] + public class Vector2FloatTest + { + [TestMethod] + public void TestInitialization() + { + SPVector2 vector = new SPVector2(1.0f, 2.0f); + } + } +} \ No newline at end of file