Kimelas/bgc-net
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Исправления в матрицах 2x3 и 3x2 / Several fixes in 2x3 and 3x2 matrixes

Browse source
This commit is contained in:
Andrey Pokidov 2024-11-14 14:13:29 +07:00
parent 3ba55c7524
commit 301cabe8de
22 changed files with 3805 additions and 3744 deletions
Download patch file Download diff file Expand all files Collapse all files

74
Geometry.Net.sln
View file

@ -1,37 +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

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

96
Geometry/Angle.cs
View file

@ -1,48 +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
}
}
/*
* 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
}
}

814
Geometry/DPMatrix2x2.cs
View file

@ -1,407 +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;
}
}
}
/*
* 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;
}
}
}

40
Geometry/DPMatrix2x3.cs
View file

@ -28,7 +28,8 @@ namespace Geometry
public double r2c1, r2c2;
public double r3c1, r3c2;
public DPMatrix2x3() {
public DPMatrix2x3()
{
this.r1c1 = 0.0;
this.r1c2 = 0.0;
@ -39,7 +40,8 @@ namespace Geometry
this.r3c2 = 0.0;
}
public DPMatrix2x3(in DPMatrix2x3 matrix) {
public DPMatrix2x3(in DPMatrix2x3 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
@ -50,7 +52,8 @@ namespace Geometry
this.r3c2 = matrix.r3c2;
}
public DPMatrix2x3(in SPMatrix2x3 matrix) {
public DPMatrix2x3(in SPMatrix2x3 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
@ -194,7 +197,7 @@ namespace Geometry
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;
@ -212,7 +215,7 @@ namespace Geometry
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);
@ -231,7 +234,7 @@ namespace Geometry
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;
@ -259,7 +262,7 @@ namespace Geometry
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;
@ -276,7 +279,7 @@ namespace Geometry
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;
@ -294,7 +297,7 @@ namespace Geometry
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);
@ -313,7 +316,7 @@ namespace Geometry
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;
@ -351,22 +354,15 @@ namespace Geometry
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;
result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2;
result.x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2;
result.x3 = matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2;
}
public static void GetLeftProduct(in 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;
result.x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1;
result.x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2;
}
}
}

524
Geometry/DPMatrix3x2.cs
View file

@ -22,308 +22,326 @@ using System;
*/
namespace Geometry
{
public struct DPMatrix3x2
{
public double r1c1, r1c2, r1c3;
public double r2c1, r2c2, r2c3;
public DPMatrix3x2()
public struct DPMatrix3x2
{
this.r1c1 = 0.0;
this.r1c2 = 0.0;
this.r1c3 = 0.0;
public double r1c1, r1c2, r1c3;
public double r2c1, r2c2, r2c3;
this.r2c1 = 0.0;
this.r2c2 = 0.0;
this.r2c3 = 0.0;
}
public DPMatrix3x2()
{
this.r1c1 = 0.0;
this.r1c2 = 0.0;
this.r1c3 = 0.0;
public DPMatrix3x2(in DPMatrix3x2 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
this.r1c3 = matrix.r1c3;
this.r2c1 = 0.0;
this.r2c2 = 0.0;
this.r2c3 = 0.0;
}
this.r2c1 = matrix.r2c1;
this.r2c2 = matrix.r2c2;
this.r2c3 = matrix.r2c3;
}
public DPMatrix3x2(in DPMatrix3x2 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
this.r1c3 = matrix.r1c3;
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;
}
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;
public DPMatrix3x2(in DPMatrix2x3 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r2c1;
this.r1c3 = matrix.r3c1;
this.r2c1 = matrix.r2c1;
this.r2c2 = matrix.r2c2;
this.r2c3 = matrix.r2c3;
}
this.r2c1 = matrix.r1c2;
this.r2c2 = matrix.r2c2;
this.r2c3 = matrix.r2c2;
}
public DPMatrix3x2(in DPMatrix2x3 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r2c1;
this.r1c3 = matrix.r3c1;
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;
}
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;
public void Reset() {
this.r1c1 = 0.0;
this.r1c2 = 0.0;
this.r1c3 = 0.0;
this.r2c1 = matrix.r1c2;
this.r2c2 = matrix.r2c2;
this.r2c3 = matrix.r2c2;
}
this.r2c1 = 0.0;
this.r2c2 = 0.0;
this.r2c3 = 0.0;
}
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;
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;
}
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;
public void SetValues(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;
}
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;
public void SetTransposed(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;
}
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;
public void SetTransposed(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;
}
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 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 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 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 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 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;
public static void Add(in DPMatrix3x2 matrix1, in DPMatrix3x2 matrix2, out 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;
}
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;
public static void Add3(
in DPMatrix3x2 matrix1,
in DPMatrix3x2 matrix2,
in DPMatrix3x2 matrix3,
out 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;
}
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);
public static void Add4(
in DPMatrix3x2 matrix1,
in DPMatrix3x2 matrix2,
in DPMatrix3x2 matrix3,
in DPMatrix3x2 matrix4,
out 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);
}
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;
public static void Add5(
in DPMatrix3x2 matrix1,
in DPMatrix3x2 matrix2,
in DPMatrix3x2 matrix3,
in DPMatrix3x2 matrix4,
in DPMatrix3x2 matrix5,
out 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;
}
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;
public static void Subtract(in DPMatrix3x2 minuend, in DPMatrix3x2 subtrahend, out 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;
}
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;
public static void GetWeightedSum2(
double weight1, in DPMatrix3x2 matrix1,
double weight2, in DPMatrix3x2 matrix2,
out 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;
}
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;
public static void GetWeightedSum3(
double weight1, in DPMatrix3x2 matrix1,
double weight2, in DPMatrix3x2 matrix2,
double weight3, in DPMatrix3x2 matrix3,
out 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;
}
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);
public static void GetWeightedSum4(
double weight1, in DPMatrix3x2 matrix1,
double weight2, in DPMatrix3x2 matrix2,
double weight3, in DPMatrix3x2 matrix3,
double weight4, in DPMatrix3x2 matrix4,
out 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);
}
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;
public static void GetWeightedSum5(
double weight1, in DPMatrix3x2 matrix1,
double weight2, in DPMatrix3x2 matrix2,
double weight3, in DPMatrix3x2 matrix3,
double weight4, in DPMatrix3x2 matrix4,
double weight5, in DPMatrix3x2 matrix5,
out 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;
}
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;
public static void Multiply(in DPMatrix3x2 multiplicand, double multiplier, out 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;
}
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;
public static void Divide(in DPMatrix3x2 dividend, double divisor, out 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;
}
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 GetRightProduct(in DPMatrix3x2 matrix, in DPVector3 vector, out DPVector2 result)
{
result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3;
result.x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3;
}
public static void GetLeftProduct(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
);
public static void GetLeftProduct(in DPVector2 vector, in DPMatrix3x2 matrix, out DPVector3 result)
{
result.x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1;
result.x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2;
result.x3 = vector.x1 * matrix.r1c3 + vector.x2 * matrix.r2c3;
}
}
}
}

44
Geometry/DPMatrixProduct.cs
View file

@ -81,29 +81,17 @@ namespace Geometry
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;
product.r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1;
product.r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2;
product.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.r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1;
product.r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2;
product.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;
product.r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1;
product.r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2;
product.r3c3 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c3;
}
public static void Get3x2At3x3(in DPMatrix3x2 left, in DPMatrix3x3 right, out DPMatrix3x2 product)
@ -127,17 +115,11 @@ namespace Geometry
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;
product.r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1;
product.r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2;
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;
product.r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1;
product.r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2;
}
public static void Get3x3At2x3(in DPMatrix3x3 left, in DPMatrix2x3 right, out DPMatrix2x3 product)

402
Geometry/DPQuaternion.cs
View file

@ -1,201 +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;
}
}
}
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;
}
}
}

40
Geometry/DPUtility.cs
View file

@ -1,20 +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;
}
}
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;
}
}

870
Geometry/DPVector3.cs
View file

@ -1,435 +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;
}
}
}
/*
* 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;
}
}
}

32
Geometry/Geometry.csproj
View file

@ -1,16 +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>
<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>

814
Geometry/SPMatrix2x2.cs
View file

@ -1,407 +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;
}
}
}
/*
* 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;
}
}
}

611
Geometry/SPMatrix2x3.cs
View file

@ -22,324 +22,347 @@ using System;
*/
namespace Geometry
{
public struct SPMatrix2x3 {
public struct SPMatrix2x3
{
public float r1c1, r1c2;
public float r2c1, r2c2;
public float r3c1, r3c2;
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() {
this.r1c1 = 0.0f;
this.r1c2 = 0.0f;
public SPMatrix2x3(in SPMatrix2x3 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
this.r2c1 = matrix.r2c1;
this.r2c2 = matrix.r2c2;
this.r2c1 = 0.0f;
this.r2c2 = 0.0f;
this.r3c1 = 0.0f;
this.r3c2 = 0.0f;
}
this.r3c1 = matrix.r3c1;
this.r3c2 = matrix.r3c2;
}
public SPMatrix2x3(SPMatrix2x3 matrix) {
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
public SPMatrix2x3(in DPMatrix2x3 matrix)
{
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r1c2;
this.r2c1 = matrix.r2c1;
this.r2c2 = matrix.r2c2;
this.r2c1 = (float) matrix.r2c1;
this.r2c2 = (float) matrix.r2c2;
this.r3c1 = matrix.r3c1;
this.r3c2 = matrix.r3c2;
}
this.r3c1 = (float) matrix.r3c1;
this.r3c2 = (float) matrix.r3c2;
}
public SPMatrix2x3(DPMatrix2x3 matrix) {
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r1c2;
public SPMatrix2x3(in SPMatrix3x2 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r2c1;
this.r2c1 = (float) matrix.r2c1;
this.r2c2 = (float) matrix.r2c2;
this.r2c1 = matrix.r1c2;
this.r2c2 = matrix.r2c2;
this.r3c1 = (float) matrix.r3c1;
this.r3c2 = (float) matrix.r3c2;
}
this.r3c1 = matrix.r1c3;
this.r3c2 = matrix.r2c3;
}
public SPMatrix2x3(SPMatrix3x2 matrix) {
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r2c1;
public SPMatrix2x3(in DPMatrix3x2 matrix)
{
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r2c1;
this.r2c1 = matrix.r1c2;
this.r2c2 = matrix.r2c2;
this.r2c1 = (float) matrix.r1c2;
this.r2c2 = (float) matrix.r2c2;
this.r3c1 = matrix.r1c3;
this.r3c2 = matrix.r2c3;
}
this.r3c1 = (float) matrix.r1c3;
this.r3c2 = (float) 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;
public void Reset()
{
this.r1c1 = 0.0f;
this.r1c2 = 0.0f;
this.r3c1 = (float) matrix.r1c3;
this.r3c2 = (float) matrix.r2c3;
}
this.r2c1 = 0.0f;
this.r2c2 = 0.0f;
this.r3c1 = 0.0f;
this.r3c2 = 0.0f;
}
public void Reset() {
this.r1c1 = 0.0f;
this.r1c2 = 0.0f;
public void SetValues(in SPMatrix2x3 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
this.r2c1 = matrix.r2c1;
this.r2c2 = matrix.r2c2;
this.r2c1 = 0.0f;
this.r2c2 = 0.0f;
this.r3c1 = 0.0f;
this.r3c2 = 0.0f;
}
this.r3c1 = matrix.r3c1;
this.r3c2 = matrix.r3c2;
}
public void SetValues(SPMatrix2x3 matrix) {
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
public void SetValues(in DPMatrix2x3 matrix)
{
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r1c2;
this.r2c1 = matrix.r2c1;
this.r2c2 = matrix.r2c2;
this.r2c1 = (float) matrix.r2c1;
this.r2c2 = (float) matrix.r2c2;
this.r3c1 = matrix.r3c1;
this.r3c2 = matrix.r3c2;
}
this.r3c1 = (float) matrix.r3c1;
this.r3c2 = (float) matrix.r3c2;
}
public void SetValues(DPMatrix2x3 matrix) {
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r1c2;
public void SetTransposed(in SPMatrix3x2 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r2c1;
this.r2c1 = (float) matrix.r2c1;
this.r2c2 = (float) matrix.r2c2;
this.r2c1 = matrix.r1c2;
this.r2c2 = matrix.r2c2;
this.r3c1 = (float) matrix.r3c1;
this.r3c2 = (float) matrix.r3c2;
}
this.r3c1 = matrix.r1c3;
this.r3c2 = matrix.r2c3;
}
public void SetTransposed(SPMatrix3x2 matrix) {
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r2c1;
public void SetTransposed(in DPMatrix3x2 matrix)
{
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r2c1;
this.r2c1 = matrix.r1c2;
this.r2c2 = matrix.r2c2;
this.r2c1 = (float) matrix.r1c2;
this.r2c2 = (float) matrix.r2c2;
this.r3c1 = matrix.r1c3;
this.r3c2 = matrix.r2c3;
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(in SPMatrix2x3 matrix1, in SPMatrix2x3 matrix2, out SPMatrix2x3 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 SPMatrix2x3 matrix1,
in SPMatrix2x3 matrix2,
in SPMatrix2x3 matrix3,
out 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(
in SPMatrix2x3 matrix1,
in SPMatrix2x3 matrix2,
in SPMatrix2x3 matrix3,
in SPMatrix2x3 matrix4,
out 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(
in SPMatrix2x3 matrix1,
in SPMatrix2x3 matrix2,
in SPMatrix2x3 matrix3,
in SPMatrix2x3 matrix4,
in SPMatrix2x3 matrix5,
out 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(in SPMatrix2x3 minuend, in SPMatrix2x3 subtrahend, out 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, in SPMatrix2x3 matrix1,
float weight2, in SPMatrix2x3 matrix2,
out 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, in SPMatrix2x3 matrix1,
float weight2, in SPMatrix2x3 matrix2,
float weight3, in SPMatrix2x3 matrix3,
out 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, in SPMatrix2x3 matrix1,
float weight2, in SPMatrix2x3 matrix2,
float weight3, in SPMatrix2x3 matrix3,
float weight4, in SPMatrix2x3 matrix4,
out 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, in SPMatrix2x3 matrix1,
float weight2, in SPMatrix2x3 matrix2,
float weight3, in SPMatrix2x3 matrix3,
float weight4, in SPMatrix2x3 matrix4,
float weight5, in SPMatrix2x3 matrix5,
out 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(in SPMatrix2x3 multiplicand, float multiplier, out 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(in SPMatrix2x3 dividend, float divisor, out 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(in SPMatrix2x3 matrix, in SPVector2 vector, out SPVector3 result)
{
result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2;
result.x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2;
result.x3 = matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2;
}
public static void GetLeftProduct(in SPVector3 vector, in SPMatrix2x3 matrix, out SPVector2 result)
{
result.x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1;
result.x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2;
}
}
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
);
}
}
}

492
Geometry/SPMatrix3x2.cs
View file

@ -20,265 +20,325 @@
*/
namespace Geometry
{
public class SPMatrix3x2 {
public struct SPMatrix3x2
{
public float r1c1, r1c2, r1c3;
public float r2c1, r2c2, r2c3;
public float r1c1, r1c2, r1c3;
public SPMatrix3x2()
{
this.r1c1 = 0.0f;
this.r1c2 = 0.0f;
this.r1c3 = 0.0f;
public float r2c1, r2c2, r2c3;
this.r2c1 = 0.0f;
this.r2c2 = 0.0f;
this.r2c3 = 0.0f;
}
public SPMatrix3x2() {
this.r1c1 = 0.0f;
this.r1c2 = 0.0f;
this.r1c3 = 0.0f;
public SPMatrix3x2(in SPMatrix3x2 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
this.r1c3 = matrix.r1c3;
this.r2c1 = 0.0f;
this.r2c2 = 0.0f;
this.r2c3 = 0.0f;
}
this.r2c1 = matrix.r2c1;
this.r2c2 = matrix.r2c2;
this.r2c3 = matrix.r2c3;
}
public SPMatrix3x2(SPMatrix3x2 matrix) {
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
this.r1c3 = matrix.r1c3;
public SPMatrix3x2(in DPMatrix3x2 matrix)
{
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r1c2;
this.r1c3 = (float) matrix.r1c3;
this.r2c1 = matrix.r2c1;
this.r2c2 = matrix.r2c2;
this.r2c3 = matrix.r2c3;
}
this.r2c1 = (float) matrix.r2c1;
this.r2c2 = (float) matrix.r2c2;
this.r2c3 = (float) matrix.r2c3;
}
public SPMatrix3x2(DPMatrix3x2 matrix) {
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r1c2;
this.r1c3 = (float) matrix.r1c3;
public SPMatrix3x2(in SPMatrix2x3 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r2c1;
this.r1c3 = matrix.r3c1;
this.r2c1 = (float) matrix.r2c1;
this.r2c2 = (float) matrix.r2c2;
this.r2c3 = (float) matrix.r2c3;
}
this.r2c1 = matrix.r1c2;
this.r2c2 = matrix.r2c2;
this.r2c3 = matrix.r3c2;
}
public void Reset() {
this.r1c1 = 0.0f;
this.r1c2 = 0.0f;
this.r1c3 = 0.0f;
public SPMatrix3x2(in DPMatrix2x3 matrix)
{
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r2c1;
this.r1c3 = (float) matrix.r3c1;
this.r2c1 = 0.0f;
this.r2c2 = 0.0f;
this.r2c3 = 0.0f;
}
this.r2c1 = (float) matrix.r1c2;
this.r2c2 = (float) matrix.r2c2;
this.r2c3 = (float) matrix.r3c2;
}
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(in SPMatrix3x2 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r1c2;
this.r1c3 = matrix.r1c3;
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;
}
this.r2c1 = matrix.r2c1;
this.r2c2 = matrix.r2c2;
this.r2c3 = matrix.r2c3;
}
public void SetValues(in DPMatrix3x2 matrix)
{
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r1c2;
this.r1c3 = (float) matrix.r1c3;
/*
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 SetTransposed(in SPMatrix2x3 matrix)
{
this.r1c1 = matrix.r1c1;
this.r1c2 = matrix.r2c1;
this.r1c3 = matrix.r3c1;
this.r2c1 = (float) matrix.r2c1;
this.r2c2 = (float) matrix.r2c2;
this.r2c3 = (float) matrix.r2c3;
}
*/
this.r2c1 = matrix.r1c2;
this.r2c2 = matrix.r2c2;
this.r2c3 = matrix.r3c2;
}
public void SetRow1(float c1, float c2, float c3) {
this.r1c1 = c1;
this.r1c2 = c2;
this.r1c3 = c3;
}
public void SetTransposed(in DPMatrix2x3 matrix)
{
this.r1c1 = (float) matrix.r1c1;
this.r1c2 = (float) matrix.r2c1;
this.r1c3 = (float) matrix.r3c1;
public void SetRow2(float c1, float c2, float c3) {
this.r2c1 = c1;
this.r2c2 = c2;
this.r2c3 = c3;
}
this.r2c1 = (float) matrix.r1c2;
this.r2c2 = (float) matrix.r2c2;
this.r2c3 = (float) matrix.r3c2;
}
public void SetColumn1(float r1, float r2) {
this.r1c1 = r1;
this.r2c1 = r2;
}
public void SetRow1(float c1, float c2, float c3)
{
this.r1c1 = c1;
this.r1c2 = c2;
this.r1c3 = c3;
}
public void SetColumn2(float r1, float r2) {
this.r1c3 = r1;
this.r2c3 = r2;
}
public void SetRow2(float c1, float c2, float c3)
{
this.r2c1 = c1;
this.r2c2 = c2;
this.r2c3 = c3;
}
public void SetColumn3(float r1, float r2) {
this.r1c3 = r1;
this.r2c3 = r2;
}
public void SetColumn1(float r1, float r2)
{
this.r1c1 = r1;
this.r2c1 = 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;
public void SetColumn2(float r1, float r2)
{
this.r1c3 = r1;
this.r2c3 = r2;
}
sum.r2c1 = matrix1.r2c1 + matrix2.r2c1;
sum.r2c2 = matrix1.r2c2 + matrix2.r2c2;
sum.r2c3 = matrix1.r2c3 + matrix2.r2c3;
}
public void SetColumn3(float r1, float r2)
{
this.r1c3 = r1;
this.r2c3 = r2;
}
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;
public static void Add(in SPMatrix3x2 matrix1, in SPMatrix3x2 matrix2, out 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 + matrix3.r2c1;
sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2;
sum.r2c3 = matrix1.r2c3 + matrix2.r2c3 + matrix3.r2c3;
}
sum.r2c1 = matrix1.r2c1 + matrix2.r2c1;
sum.r2c2 = matrix1.r2c2 + matrix2.r2c2;
sum.r2c3 = matrix1.r2c3 + matrix2.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);
public static void Add3(
in SPMatrix3x2 matrix1,
in SPMatrix3x2 matrix2,
in SPMatrix3x2 matrix3,
out 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 + matrix4.r2c1);
sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2);
sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3);
}
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 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;
public static void Add4(
in SPMatrix3x2 matrix1,
in SPMatrix3x2 matrix2,
in SPMatrix3x2 matrix3,
in SPMatrix3x2 matrix4,
out 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) + 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.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 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;
public static void Add5(
in SPMatrix3x2 matrix1,
in SPMatrix3x2 matrix2,
in SPMatrix3x2 matrix3,
in SPMatrix3x2 matrix4,
in SPMatrix3x2 matrix5,
out 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;
difference.r2c1 = minuend.r2c1 - subtrahend.r2c1;
difference.r2c2 = minuend.r2c2 - subtrahend.r2c2;
difference.r2c3 = minuend.r2c3 - subtrahend.r2c3;
}
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 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;
public static void Subtract(in SPMatrix3x2 minuend, in SPMatrix3x2 subtrahend, out SPMatrix3x2 difference)
{
difference.r1c1 = minuend.r1c1 - subtrahend.r1c1;
difference.r1c2 = minuend.r1c2 - subtrahend.r1c2;
difference.r1c3 = minuend.r1c3 - subtrahend.r1c3;
sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2;
sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2;
sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2;
}
difference.r2c1 = minuend.r2c1 - subtrahend.r2c1;
difference.r2c2 = minuend.r2c2 - subtrahend.r2c2;
difference.r2c3 = minuend.r2c3 - subtrahend.r2c3;
}
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;
public static void GetWeightedSum2(
float weight1, in SPMatrix3x2 matrix1,
float weight2, in SPMatrix3x2 matrix2,
out 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 + 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.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 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);
public static void GetWeightedSum3(
float weight1, in SPMatrix3x2 matrix1,
float weight2, in SPMatrix3x2 matrix2,
float weight3, in SPMatrix3x2 matrix3,
out 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 + 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.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 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;
public static void GetWeightedSum4(
float weight1, in SPMatrix3x2 matrix1,
float weight2, in SPMatrix3x2 matrix2,
float weight3, in SPMatrix3x2 matrix3,
float weight4, in SPMatrix3x2 matrix4,
out 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) + 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.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 Multiply(SPMatrix3x2 multiplicand, float multiplier, SPMatrix3x2 product) {
product.r1c1 = multiplicand.r1c1 * multiplier;
product.r1c2 = multiplicand.r1c2 * multiplier;
product.r1c3 = multiplicand.r1c3 * multiplier;
public static void GetWeightedSum5(
float weight1, in SPMatrix3x2 matrix1,
float weight2, in SPMatrix3x2 matrix2,
float weight3, in SPMatrix3x2 matrix3,
float weight4, in SPMatrix3x2 matrix4,
float weight5, in SPMatrix3x2 matrix5,
out 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;
product.r2c1 = multiplicand.r2c1 * multiplier;
product.r2c2 = multiplicand.r2c2 * multiplier;
product.r2c3 = multiplicand.r2c3 * multiplier;
}
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 Divide(SPMatrix3x2 dividend, float divisor, SPMatrix3x2 quotient) {
quotient.r1c1 = dividend.r1c1 / divisor;
quotient.r1c2 = dividend.r1c2 / divisor;
quotient.r1c3 = dividend.r1c3 / divisor;
public static void Multiply(in SPMatrix3x2 multiplicand, float multiplier, out SPMatrix3x2 product)
{
product.r1c1 = multiplicand.r1c1 * multiplier;
product.r1c2 = multiplicand.r1c2 * multiplier;
product.r1c3 = multiplicand.r1c3 * multiplier;
quotient.r2c1 = dividend.r2c1 / divisor;
quotient.r2c2 = dividend.r2c2 / divisor;
quotient.r2c3 = dividend.r2c3 / divisor;
}
product.r2c1 = multiplicand.r2c1 * multiplier;
product.r2c2 = multiplicand.r2c2 * multiplier;
product.r2c3 = multiplicand.r2c3 * multiplier;
}
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 Divide(in SPMatrix3x2 dividend, float divisor, out SPMatrix3x2 quotient)
{
quotient.r1c1 = dividend.r1c1 / divisor;
quotient.r1c2 = dividend.r1c2 / divisor;
quotient.r1c3 = dividend.r1c3 / divisor;
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
);
quotient.r2c1 = dividend.r2c1 / divisor;
quotient.r2c2 = dividend.r2c2 / divisor;
quotient.r2c3 = dividend.r2c3 / divisor;
}
public static void GetRightProduct(in SPMatrix3x2 matrix, in SPVector3 vector, out SPVector2 result)
{
result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3;
result.x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3;
}
public static void GetLeftProduct(in SPVector2 vector, in SPMatrix3x2 matrix, out SPVector3 result)
{
result.x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1;
result.x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2;
result.x3 = vector.x1 * matrix.r1c3 + vector.x2 * matrix.r2c3;
}
}
}
}

1212
Geometry/SPMatrix3x3.cs
View file

File diff suppressed because it is too large Load diff

48
Geometry/SPMatrixProduct.cs
View file

@ -81,29 +81,17 @@ namespace Geometry
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;
product.r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1;
product.r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2;
product.r1c3 = left.r1c1 * right.r1c3 + left.r1c2 * right.r2c3;
product.r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1;
product.r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2;
product.r2c3 = left.r2c1 * right.r1c3 + left.r2c2 * right.r2c3;
product.r3c1 = left.r3c1 * right.r1c1 + left.r3c2 * right.r2c1;
product.r3c2 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c2;
product.r3c3 = left.r3c1 * right.r1c2 + left.r3c2 * right.r2c3;
}
public static void Get3x2At3x3(in SPMatrix3x2 left, in SPMatrix3x3 right, out SPMatrix3x2 product)
@ -127,17 +115,11 @@ namespace Geometry
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;
product.r1c1 = left.r1c1 * right.r1c1 + left.r1c2 * right.r2c1 + left.r1c3 * right.r3c1;
product.r1c2 = left.r1c1 * right.r1c2 + left.r1c2 * right.r2c2 + left.r1c3 * right.r3c2;
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;
product.r2c1 = left.r2c1 * right.r1c1 + left.r2c2 * right.r2c1 + left.r2c3 * right.r3c1;
product.r2c2 = left.r2c1 * right.r1c2 + left.r2c2 * right.r2c2 + left.r2c3 * right.r3c2;
}
public static void Get3x3At2x3(in SPMatrix3x3 left, in SPMatrix2x3 right, out SPMatrix2x3 product)

406
Geometry/SPQuaternion.cs
View file

@ -1,203 +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;
}
}
}
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;
}
}
}

42
Geometry/SPUtility.cs
View file

@ -1,21 +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;
}
}
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;
}
}

732
Geometry/SPVersor.cs
View file

@ -1,366 +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;
}
}
}
/*
* 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;
}
}
}

28
GeometryDev/GeometryDev.csproj
View file

@ -1,14 +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>
<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>

158
GeometryDev/Program.cs
View file

@ -1,80 +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;
}
// 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;
}
}

34
GeometryTest/GeometryTest.csproj
View file

@ -1,18 +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" />
<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>
</ItemGroup>
</Project>

36
GeometryTest/Vector2FloatTest.cs
View file

@ -1,19 +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);
}
}
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);
}
}
}