Базовая версия библиотеки. Версия 0.2.0-dev

.gitignore

@@ -0,0 +1,6 @@
+.vs
+.vscode
+bin
+obj
+Debug
+Release

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

Geometry/Angle.cs

@@ -0,0 +1,48 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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
+    {
+        /// <summary>
+        /// The measure of an angle with a range of:
+        /// [0, 360) degrees, [0, 2xPI) radians, [0, 1) turns, [0, 400) gradians
+        /// </summary>
+        UNSIGNED_RANGE = 1,
+
+        /// <summary>
+        /// The measure of an angle with a range of:
+        /// (-180, 180] degrees, (-PI, PI] radians, (-0.5, 0.5] turns, (-200, 200] gradians
+        /// </summary>
+        SIGNED_RANGE = 2
+    }
+}
+

Geometry/DPAngle.cs

@@ -0,0 +1,305 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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);
+        }
+    }
+}

Geometry/DPMatrix2x2.cs

@@ -0,0 +1,407 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

Geometry/DPMatrix2x3.cs

@@ -0,0 +1,372 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

Geometry/DPMatrix3x2.cs

@@ -0,0 +1,329 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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
+        );
+    }
+}
+}

Geometry/DPMatrix3x3.cs

@@ -0,0 +1,602 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

Geometry/DPMatrixProduct.cs

@@ -0,0 +1,191 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

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;
+        }
+    }
+}

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;
+    }
+}

Geometry/DPVector2.cs

@@ -0,0 +1,383 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

Geometry/DPVector3.cs

@@ -0,0 +1,435 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}
+

Geometry/DPVersor.cs

@@ -0,0 +1,359 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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);
+            }
+        }
+    }
+}

Geometry/Geometry.csproj

@@ -0,0 +1,16 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net6.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <Compile Remove="DPMatrix4x4.cs" />
+    <Compile Remove="DPVector4.cs" />
+    <Compile Remove="SPMatrix4x4.cs" />
+    <Compile Remove="SPVector4.cs" />
+  </ItemGroup>
+
+</Project>

Geometry/SPAngle.cs

@@ -0,0 +1,306 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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);
+        }
+    }
+}

Geometry/SPMatrix2x2.cs

@@ -0,0 +1,407 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

Geometry/SPMatrix2x3.cs

@@ -0,0 +1,345 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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
+        );
+    }
+}
+}

Geometry/SPMatrix3x2.cs

@@ -0,0 +1,284 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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
+        );
+    }
+}
+}

Geometry/SPMatrix3x3.cs

@@ -0,0 +1,606 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

Geometry/SPMatrixProduct.cs

@@ -0,0 +1,191 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

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;
+        }
+    }
+}

Geometry/SPRotation3.cs

@@ -0,0 +1,44 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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();
+        }
+    }
+}

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;
+    }
+}
+

Geometry/SPVector2.cs

@@ -0,0 +1,383 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

Geometry/SPVector3.cs

@@ -0,0 +1,434 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

Geometry/SPVersor.cs

@@ -0,0 +1,366 @@
+/*
+ * Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
+ *
+ * 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;
+        }
+    }
+}

GeometryDev/GeometryDev.csproj

@@ -0,0 +1,14 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net8.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="..\Geometry\Geometry.csproj" />
+  </ItemGroup>
+
+</Project>

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;
+    }
+}

GeometryTest/GeometryTest.csproj

@@ -0,0 +1,18 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net8.0</TargetFramework>
+    <Nullable>enable</Nullable>
+
+    <IsPackable>false</IsPackable>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="Microsoft.NET.Test.Sdk" Version="16.11.0" />
+    <PackageReference Include="MSTest.TestAdapter" Version="2.2.7" />
+    <PackageReference Include="MSTest.TestFramework" Version="2.2.7" />
+    <PackageReference Include="coverlet.collector" Version="3.1.0" />
+    <ProjectReference Include="..\Geometry\Geometry.csproj" />
+  </ItemGroup>
+
+</Project>

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);
+        }
+    }
+}