From c38c5ac857525a7ee26f8283468d159467125dd7 Mon Sep 17 00:00:00 2001 From: Andrey Pokidov Date: Fri, 15 Nov 2024 19:11:49 +0700 Subject: [PATCH] =?UTF-8?q?=D0=A3=D0=B4=D0=B0=D0=BB=D0=B5=D0=BD=D0=B8?= =?UTF-8?q?=D0=B5=20=D0=B8=D0=B7=D0=B1=D1=8B=D1=82=D0=BE=D1=87=D0=BD=D1=8B?= =?UTF-8?q?=D1=85=20=D0=BC=D0=B5=D1=82=D0=BE=D0=B4=D0=BE=D0=B2=20=D0=B8=20?= =?UTF-8?q?=D1=80=D0=B5=D0=BE=D1=80=D0=B3=D0=B0=D0=BD=D0=B8=D0=B7=D0=B0?= =?UTF-8?q?=D1=86=D0=B8=D1=8F=20=D0=B1=D0=B8=D0=B1=D0=BB=D0=B8=D0=BE=D1=82?= =?UTF-8?q?=D0=B5=D0=BA=D0=B8=20/=20Removing=20of=20extra=20methods=20and?= =?UTF-8?q?=20reorganization=20of=20the=20library?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Geometry/DPMatrix2x2.cs | 259 ++++++++++--------------- Geometry/DPMatrix2x3.cs | 190 +++++++----------- Geometry/DPMatrix3x2.cs | 179 ++++++----------- Geometry/DPMatrix3x3.cs | 402 ++++++++++++++++---------------------- Geometry/DPQuaternion.cs | 13 +- Geometry/DPVector2.cs | 103 ++++------ Geometry/DPVector3.cs | 176 +++++++---------- Geometry/SPMatrix2x2.cs | 259 ++++++++++--------------- Geometry/SPMatrix2x3.cs | 190 +++++++----------- Geometry/SPMatrix3x2.cs | 179 ++++++----------- Geometry/SPMatrix3x3.cs | 408 ++++++++++++++++----------------------- Geometry/SPQuaternion.cs | 16 +- Geometry/SPVector2.cs | 90 +++------ Geometry/SPVector3.cs | 88 +++------ 14 files changed, 969 insertions(+), 1583 deletions(-) diff --git a/Geometry/DPMatrix2x2.cs b/Geometry/DPMatrix2x2.cs index d1858e4..37d5b6c 100644 --- a/Geometry/DPMatrix2x2.cs +++ b/Geometry/DPMatrix2x2.cs @@ -76,33 +76,6 @@ namespace Geometry 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); @@ -132,14 +105,59 @@ namespace Geometry return true; } - public void MakeTransposedOf(in DPMatrix2x2 matrix) + public void Reset() + { + this.r1c1 = 0.0; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 0.0; + } + + public void SetToIdentity() + { + this.r1c1 = 1.0; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = 1.0; + } + + public void SetToDiagonal(double d1, double d2) + { + this.r1c1 = d1; + this.r1c2 = 0.0; + + this.r2c1 = 0.0; + this.r2c2 = d2; + } + + 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 SetTransposedOf(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) + public void SetTransposedOf(in SPMatrix2x2 matrix) { this.r1c1 = matrix.r1c1; this.r1c2 = matrix.r2c1; @@ -148,7 +166,7 @@ namespace Geometry this.r2c2 = matrix.r2c2; } - public bool MakeInvertedOf(in DPMatrix2x2 matrix) + public bool SetInvertedOf(in DPMatrix2x2 matrix) { double determinant = matrix.GetDeterminant(); @@ -172,30 +190,6 @@ namespace Geometry 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; @@ -229,51 +223,6 @@ namespace Geometry 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; @@ -283,64 +232,6 @@ namespace Geometry 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; @@ -359,6 +250,60 @@ namespace Geometry quotient.r2c2 = dividend.r2c2 / divisor; } + 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 GetRightProduct(in DPMatrix2x2 matrix, in DPVector2 vector, out DPVector2 result) { double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; diff --git a/Geometry/DPMatrix2x3.cs b/Geometry/DPMatrix2x3.cs index 0dab538..a6ba25e 100644 --- a/Geometry/DPMatrix2x3.cs +++ b/Geometry/DPMatrix2x3.cs @@ -192,60 +192,6 @@ namespace Geometry sum.r3c2 = matrix1.r3c2 + matrix2.r3c2; } - public static void Add3( - in DPMatrix2x3 matrix1, - in DPMatrix2x3 matrix2, - in DPMatrix2x3 matrix3, - out DPMatrix2x3 sum - ) - { - sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; - sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; - - sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; - sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; - - sum.r3c1 = matrix1.r3c1 + matrix2.r3c1 + matrix3.r3c1; - sum.r3c2 = matrix1.r3c2 + matrix2.r3c2 + matrix3.r3c2; - } - - public static void Add4( - in DPMatrix2x3 matrix1, - in DPMatrix2x3 matrix2, - in DPMatrix2x3 matrix3, - in DPMatrix2x3 matrix4, - out DPMatrix2x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); - sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); - - sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); - sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); - - sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1); - sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2); - } - - public static void Add5( - in DPMatrix2x3 matrix1, - in DPMatrix2x3 matrix2, - in DPMatrix2x3 matrix3, - in DPMatrix2x3 matrix4, - in DPMatrix2x3 matrix5, - out DPMatrix2x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; - sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; - - sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; - sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; - - sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1) + matrix5.r3c1; - sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2) + matrix5.r3c2; - } - public static void Subtract(in DPMatrix2x3 minuend, in DPMatrix2x3 subtrahend, out DPMatrix2x3 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; @@ -258,76 +204,6 @@ namespace Geometry difference.r3c2 = minuend.r3c2 - subtrahend.r3c2; } - public static void GetWeightedSum2( - double weight1, in DPMatrix2x3 matrix1, - double weight2, in DPMatrix2x3 matrix2, - out DPMatrix2x3 sum - ) - { - sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; - sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; - - sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; - sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; - - sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; - sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; - } - - public static void GetWeightedSum3( - double weight1, in DPMatrix2x3 matrix1, - double weight2, in DPMatrix2x3 matrix2, - double weight3, in DPMatrix2x3 matrix3, - out DPMatrix2x3 sum - ) - { - sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; - sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; - - sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; - sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; - - sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; - sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; - } - - public static void GetWeightedSum4( - double weight1, in DPMatrix2x3 matrix1, - double weight2, in DPMatrix2x3 matrix2, - double weight3, in DPMatrix2x3 matrix3, - double weight4, in DPMatrix2x3 matrix4, - out DPMatrix2x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); - sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); - - sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); - sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); - - sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); - sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); - } - - public static void GetWeightedSum5( - double weight1, in DPMatrix2x3 matrix1, - double weight2, in DPMatrix2x3 matrix2, - double weight3, in DPMatrix2x3 matrix3, - double weight4, in DPMatrix2x3 matrix4, - double weight5, in DPMatrix2x3 matrix5, - out DPMatrix2x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; - sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; - - sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; - sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; - - sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; - sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; - } - public static void Multiply(in DPMatrix2x3 multiplicand, double multiplier, out DPMatrix2x3 product) { product.r1c1 = multiplicand.r1c1 * multiplier; @@ -352,6 +228,72 @@ namespace Geometry quotient.r3c2 = dividend.r3c2 / divisor; } + public static void GetWeightedSum2( + double weight1, in DPMatrix2x3 matrix1, + double weight2, in DPMatrix2x3 matrix2, + out DPMatrix2x3 sum) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; + } + + public static void GetWeightedSum3( + double weight1, in DPMatrix2x3 matrix1, + double weight2, in DPMatrix2x3 matrix2, + double weight3, in DPMatrix2x3 matrix3, + out DPMatrix2x3 sum) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; + } + + public static void GetWeightedSum4( + double weight1, in DPMatrix2x3 matrix1, + double weight2, in DPMatrix2x3 matrix2, + double weight3, in DPMatrix2x3 matrix3, + double weight4, in DPMatrix2x3 matrix4, + out DPMatrix2x3 sum) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); + } + + public static void GetWeightedSum5( + double weight1, in DPMatrix2x3 matrix1, + double weight2, in DPMatrix2x3 matrix2, + double weight3, in DPMatrix2x3 matrix3, + double weight4, in DPMatrix2x3 matrix4, + double weight5, in DPMatrix2x3 matrix5, + out DPMatrix2x3 sum) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; + } + public static void GetRightProduct(in DPMatrix2x3 matrix, in DPVector2 vector, out DPVector3 result) { result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; diff --git a/Geometry/DPMatrix3x2.cs b/Geometry/DPMatrix3x2.cs index c7be9cc..95939e7 100644 --- a/Geometry/DPMatrix3x2.cs +++ b/Geometry/DPMatrix3x2.cs @@ -181,57 +181,6 @@ namespace Geometry sum.r2c3 = matrix1.r2c3 + matrix2.r2c3; } - 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; - } - - 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); - } - - 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; - } - public static void Subtract(in DPMatrix3x2 minuend, in DPMatrix3x2 subtrahend, out DPMatrix3x2 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; @@ -243,72 +192,6 @@ namespace Geometry difference.r2c3 = minuend.r2c3 - subtrahend.r2c3; } - 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; - } - - 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; - } - - 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); - } - - 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; - } - public static void Multiply(in DPMatrix3x2 multiplicand, double multiplier, out DPMatrix3x2 product) { product.r1c1 = multiplicand.r1c1 * multiplier; @@ -331,6 +214,68 @@ namespace Geometry quotient.r2c3 = dividend.r2c3 / divisor; } + 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; + } + + 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; + } + + 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); + } + + 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; + } + 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; diff --git a/Geometry/DPMatrix3x3.cs b/Geometry/DPMatrix3x3.cs index 16b547a..4b67bd5 100644 --- a/Geometry/DPMatrix3x3.cs +++ b/Geometry/DPMatrix3x3.cs @@ -104,51 +104,6 @@ namespace Geometry return -DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON; } - public void Reset() - { - this.r1c1 = 0.0; - this.r1c2 = 0.0; - this.r1c3 = 0.0; - - this.r2c1 = 0.0; - this.r2c2 = 0.0; - this.r2c3 = 0.0; - - this.r3c1 = 0.0; - this.r3c2 = 0.0; - this.r3c3 = 0.0; - } - - public void MakeIdentity() - { - this.r1c1 = 1.0; - this.r1c2 = 0.0; - this.r1c3 = 0.0; - - this.r2c1 = 0.0; - this.r2c2 = 1.0; - this.r2c3 = 0.0; - - this.r3c1 = 0.0; - this.r3c2 = 0.0; - this.r3c3 = 1.0; - } - - public void MakeDiagonal(double d1, double d2, double d3) - { - this.r1c1 = d1; - this.r1c2 = 0.0; - this.r1c3 = 0.0; - - this.r2c1 = 0.0; - this.r2c2 = d2; - this.r2c3 = 0.0; - - this.r2c1 = 0.0; - this.r2c2 = 0.0; - this.r2c3 = d3; - } - public void Transpose() { (this.r1c2, this.r2c1) = (this.r2c1, this.r1c2); @@ -191,61 +146,49 @@ namespace Geometry return true; } - public void MakeTransposedOf(in DPMatrix3x3 matrix) + public void Reset() { - this.r1c1 = matrix.r1c1; - this.r2c2 = matrix.r2c2; - this.r3c3 = matrix.r3c3; + this.r1c1 = 0.0; + this.r1c2 = 0.0; + this.r1c3 = 0.0; - (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); - (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); - (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); + this.r2c1 = 0.0; + this.r2c2 = 0.0; + this.r2c3 = 0.0; + + this.r3c1 = 0.0; + this.r3c2 = 0.0; + this.r3c3 = 0.0; } - public void MakeTransposedOf(in SPMatrix3x3 matrix) + public void SetToIdentity() { - this.r1c1 = matrix.r1c1; - this.r2c2 = matrix.r2c2; - this.r3c3 = matrix.r3c3; + this.r1c1 = 1.0; + this.r1c2 = 0.0; + this.r1c3 = 0.0; - (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); - (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); - (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); + this.r2c1 = 0.0; + this.r2c2 = 1.0; + this.r2c3 = 0.0; + + this.r3c1 = 0.0; + this.r3c2 = 0.0; + this.r3c3 = 1.0; } - public bool MakeInvertedOf(in DPMatrix3x3 matrix) + public void SetToDiagonal(double d1, double d2, double d3) { - double determinant = matrix.GetDeterminant(); - - if (-DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON) { - return false; - } + this.r1c1 = d1; + this.r1c2 = 0.0; + this.r1c3 = 0.0; - double r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; - double r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; - double r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; + this.r2c1 = 0.0; + this.r2c2 = d2; + this.r2c3 = 0.0; - double r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; - double r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; - double r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; - - double r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; - double r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; - double r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; - - this.r1c1 = r1c1 / determinant; - this.r1c2 = r1c2 / determinant; - this.r1c3 = r1c3 / determinant; - - this.r2c1 = r2c1 / determinant; - this.r2c2 = r2c2 / determinant; - this.r2c3 = r2c3 / determinant; - - this.r3c1 = r3c1 / determinant; - this.r3c2 = r3c2 / determinant; - this.r3c3 = r3c3 / determinant; - - return true; + this.r2c1 = 0.0; + this.r2c2 = 0.0; + this.r2c3 = d3; } public void SetValues(DPMatrix3x3 matrix) @@ -278,11 +221,61 @@ namespace Geometry this.r3c3 = matrix.r3c3; } - public void SetMainDiagonal(double d1, double d2, double d3) + public void SetTransposedOf(in DPMatrix3x3 matrix) { - this.r1c1 = d1; - this.r2c2 = d2; - this.r3c3 = d3; + this.r1c1 = matrix.r1c1; + this.r2c2 = matrix.r2c2; + this.r3c3 = matrix.r3c3; + + (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); + (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); + (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); + } + + public void SetTransposedOf(in SPMatrix3x3 matrix) + { + this.r1c1 = matrix.r1c1; + this.r2c2 = matrix.r2c2; + this.r3c3 = matrix.r3c3; + + (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); + (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); + (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); + } + + public bool SetInvertedOf(in DPMatrix3x3 matrix) + { + double determinant = matrix.GetDeterminant(); + + if (-DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON) { + return false; + } + + double r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; + double r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; + double r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; + + double r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; + double r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; + double r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; + + double r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; + double r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; + double r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + this.r1c3 = r1c3 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + this.r2c3 = r2c3 / determinant; + + this.r3c1 = r3c1 / determinant; + this.r3c2 = r3c2 / determinant; + this.r3c3 = r3c3 / determinant; + + return true; } public void SetRow1(double c1, double c2, double c3) @@ -342,69 +335,6 @@ namespace Geometry sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; } - public static void Add3( - in DPMatrix3x3 matrix1, - in DPMatrix3x3 matrix2, - in DPMatrix3x3 matrix3, - out DPMatrix3x3 sum - ) - { - sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; - sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; - sum.r1c3 = matrix1.r1c3 + matrix2.r1c3 + matrix3.r1c3; - - sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; - sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; - sum.r2c3 = matrix1.r2c3 + matrix2.r2c3 + matrix3.r2c3; - - sum.r3c1 = matrix1.r3c1 + matrix2.r3c1 + matrix3.r3c1; - sum.r3c2 = matrix1.r3c2 + matrix2.r3c2 + matrix3.r3c2; - sum.r3c3 = matrix1.r3c3 + matrix2.r3c3 + matrix3.r3c3; - } - - public static void Add4( - in DPMatrix3x3 matrix1, - in DPMatrix3x3 matrix2, - in DPMatrix3x3 matrix3, - in DPMatrix3x3 matrix4, - out DPMatrix3x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); - sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); - sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3); - - sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); - sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); - sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3); - - sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1); - sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2); - sum.r3c3 = (matrix1.r3c3 + matrix2.r3c3) + (matrix3.r3c3 + matrix4.r3c3); - } - - public static void Add5( - in DPMatrix3x3 matrix1, - in DPMatrix3x3 matrix2, - in DPMatrix3x3 matrix3, - in DPMatrix3x3 matrix4, - in DPMatrix3x3 matrix5, - out DPMatrix3x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; - sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; - sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3) + matrix5.r1c3; - - sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; - sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; - sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3) + matrix5.r2c3; - - sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1) + matrix5.r3c1; - sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2) + matrix5.r3c2; - sum.r3c3 = (matrix1.r3c3 + matrix2.r3c3) + (matrix3.r3c3 + matrix4.r3c3) + matrix5.r3c3; - } - public static void Subtract(in DPMatrix3x3 minuend, in DPMatrix3x3 subtrahend, out DPMatrix3x3 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; @@ -420,88 +350,6 @@ namespace Geometry difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; } - public static void GetWeightedSum2( - double weight1, in DPMatrix3x3 matrix1, - double weight2, in DPMatrix3x3 matrix2, - out DPMatrix3x3 sum - ) - { - sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; - sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; - sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2; - - sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; - sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; - sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; - - sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; - sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; - sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2; - } - - public static void GetWeightedSum3( - double weight1, in DPMatrix3x3 matrix1, - double weight2, in DPMatrix3x3 matrix2, - double weight3, in DPMatrix3x3 matrix3, - out DPMatrix3x3 sum - ) - { - sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; - sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; - sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2 + matrix3.r1c3 * weight3; - - sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; - sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; - sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; - - sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; - sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; - sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2 + matrix3.r3c3 * weight3; - } - - public static void GetWeightedSum4( - double weight1, in DPMatrix3x3 matrix1, - double weight2, in DPMatrix3x3 matrix2, - double weight3, in DPMatrix3x3 matrix3, - double weight4, in DPMatrix3x3 matrix4, - out DPMatrix3x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); - sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); - sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4); - - sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); - sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); - sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); - - sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); - sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); - sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4); - } - - public static void GetWeightedSum5( - double weight1, in DPMatrix3x3 matrix1, - double weight2, in DPMatrix3x3 matrix2, - double weight3, in DPMatrix3x3 matrix3, - double weight4, in DPMatrix3x3 matrix4, - double weight5, in DPMatrix3x3 matrix5, - out DPMatrix3x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; - sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; - sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4) + matrix5.r1c3 * weight5; - - sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; - sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; - sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4) + matrix5.r2c3 * weight5; - - sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; - sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; - sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4) + matrix5.r3c3 * weight5; - } - public static void Multiply(in DPMatrix3x3 multiplicand, double multiplier, out DPMatrix3x3 product) { product.r1c1 = multiplicand.r1c1 * multiplier; @@ -532,6 +380,84 @@ namespace Geometry quotient.r3c3 = dividend.r3c3 / divisor; } + public static void GetWeightedSum2( + double weight1, in DPMatrix3x3 matrix1, + double weight2, in DPMatrix3x3 matrix2, + out DPMatrix3x3 sum) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; + sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2; + } + + public static void GetWeightedSum3( + double weight1, in DPMatrix3x3 matrix1, + double weight2, in DPMatrix3x3 matrix2, + double weight3, in DPMatrix3x3 matrix3, + out DPMatrix3x3 sum) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2 + matrix3.r1c3 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; + sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2 + matrix3.r3c3 * weight3; + } + + public static void GetWeightedSum4( + double weight1, in DPMatrix3x3 matrix1, + double weight2, in DPMatrix3x3 matrix2, + double weight3, in DPMatrix3x3 matrix3, + double weight4, in DPMatrix3x3 matrix4, + out DPMatrix3x3 sum) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); + sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4); + } + + public static void GetWeightedSum5( + double weight1, in DPMatrix3x3 matrix1, + double weight2, in DPMatrix3x3 matrix2, + double weight3, in DPMatrix3x3 matrix3, + double weight4, in DPMatrix3x3 matrix4, + double weight5, in DPMatrix3x3 matrix5, + out DPMatrix3x3 sum) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4) + matrix5.r1c3 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4) + matrix5.r2c3 * weight5; + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; + sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4) + matrix5.r3c3 * weight5; + } + public static void GetRightProduct(in DPMatrix3x3 matrix, in DPVector3 vector, out DPVector3 result) { double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; diff --git a/Geometry/DPQuaternion.cs b/Geometry/DPQuaternion.cs index 24853c9..5051d3c 100644 --- a/Geometry/DPQuaternion.cs +++ b/Geometry/DPQuaternion.cs @@ -46,11 +46,6 @@ namespace Geometry 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; @@ -75,6 +70,14 @@ namespace Geometry this.x3 = quaternion.x3; } + public void SetConjugateOf(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; diff --git a/Geometry/DPVector2.cs b/Geometry/DPVector2.cs index c4c43f4..0c711d1 100644 --- a/Geometry/DPVector2.cs +++ b/Geometry/DPVector2.cs @@ -103,17 +103,12 @@ namespace Geometry ); } - public void Invert() + public void Reverse() { this.x1 = -this.x1; this.x2 = -this.x2; } - public readonly DPVector2 GetInverted() - { - return new DPVector2(-this.x1, -this.x2); - } - public readonly bool IsZero() { return this.GetSquareModule() <= DPUtility.SQUARE_EPSYLON; @@ -137,16 +132,28 @@ namespace Geometry this.x2 = x2; } + public void SetValues(in DPVector2 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + } + public void SetValues(in SPVector2 vector) { this.x1 = vector.x1; this.x2 = vector.x2; } - public void SetValues(in DPVector2 vector) + public void SetReverseOf(in DPVector2 vector) { - this.x1 = vector.x1; - this.x2 = vector.x2; + this.x1 = -vector.x1; + this.x2 = -vector.x2; + } + + public void SetReverseOf(in SPVector2 vector) + { + this.x1 = -vector.x1; + this.x2 = -vector.x2; } public readonly override string ToString() @@ -160,48 +167,24 @@ namespace Geometry sum.x2 = vector1.x2 + vector2.x2; } - public static void Add3( - in DPVector2 vector1, - in DPVector2 vector2, - in DPVector2 vector3, - out DPVector2 sum - ) - { - sum.x1 = vector1.x1 + vector2.x1 + vector3.x1; - sum.x2 = vector1.x2 + vector2.x2 + vector3.x2; - } - - public static void Add4( - in DPVector2 vector1, - in DPVector2 vector2, - in DPVector2 vector3, - in DPVector2 vector4, - out DPVector2 sum - ) - { - sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1); - sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2); - } - - public static void Add5( - in DPVector2 vector1, - in DPVector2 vector2, - in DPVector2 vector3, - in DPVector2 vector4, - in DPVector2 vector5, - out DPVector2 sum - ) - { - sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1; - sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2; - } - public static void Subtract(in DPVector2 minuend, in DPVector2 subtrahend, out DPVector2 difference) { difference.x1 = minuend.x1 - subtrahend.x1; difference.x2 = minuend.x2 - subtrahend.x2; } + public static void Muliply(in DPVector2 multiplicand, double multiplier, out DPVector2 product) + { + product.x1 = multiplicand.x1 * multiplier; + product.x2 = multiplicand.x2 * multiplier; + } + + public static void Divide(in DPVector2 dividend, double divisor, out DPVector2 quotient) + { + quotient.x1 = dividend.x1 / divisor; + quotient.x2 = dividend.x2 / divisor; + } + public static void GetWeightedSum2( double weight1, in DPVector2 vector1, double weight2, in DPVector2 vector2, @@ -228,8 +211,7 @@ namespace Geometry double weight2, in DPVector2 vector2, double weight3, in DPVector2 vector3, double weight4, in DPVector2 vector4, - out DPVector2 sum - ) + out DPVector2 sum) { sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4); sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4); @@ -241,30 +223,16 @@ namespace Geometry double weight3, in DPVector2 vector3, double weight4, in DPVector2 vector4, double weight5, in DPVector2 vector5, - out DPVector2 sum - ) + out DPVector2 sum) { sum.x1 = (vector1.x1 * weight1 + vector2.x1 * weight2) + (vector3.x1 * weight3 + vector4.x1 * weight4) + vector5.x1 * weight5; sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4) + vector5.x2 * weight5; } - public static void Muliply(in DPVector2 multiplicand, double multiplier, out DPVector2 product) - { - product.x1 = multiplicand.x1 * multiplier; - product.x2 = multiplicand.x2 * multiplier; - } - - public static void Divide(in DPVector2 dividend, double divisor, out DPVector2 quotient) - { - quotient.x1 = dividend.x1 / divisor; - quotient.x2 = dividend.x2 / divisor; - } - public static void GetMean2( in DPVector2 vector1, in DPVector2 vector2, - out DPVector2 result - ) + out DPVector2 result) { result.x1 = (vector1.x1 + vector2.x1) * 0.5; result.x2 = (vector1.x2 + vector2.x2) * 0.5; @@ -274,8 +242,7 @@ namespace Geometry in DPVector2 vector1, in DPVector2 vector2, in DPVector2 vector3, - out DPVector2 result - ) + out DPVector2 result) { result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * DPUtility.ONE_THIRD; result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * DPUtility.ONE_THIRD; @@ -286,8 +253,7 @@ namespace Geometry in DPVector2 vector2, in DPVector2 vector3, in DPVector2 vector4, - out DPVector2 result - ) + out DPVector2 result) { result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1)) * 0.25; result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2)) * 0.25; @@ -299,8 +265,7 @@ namespace Geometry in DPVector2 vector3, in DPVector2 vector4, in DPVector2 vector5, - out DPVector2 result - ) + out DPVector2 result) { result.x1 = ((vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1) * 0.2; result.x2 = ((vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2) * 0.2; diff --git a/Geometry/DPVector3.cs b/Geometry/DPVector3.cs index 0fa2d35..ed21551 100644 --- a/Geometry/DPVector3.cs +++ b/Geometry/DPVector3.cs @@ -109,18 +109,13 @@ namespace Geometry ); } - public void Invert() + public void Reverse() { 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; @@ -146,6 +141,13 @@ namespace Geometry this.x3 = x3; } + public void SetValues(in DPVector3 vector) + { + this.x1 = vector.x1; + this.x2 = vector.x2; + this.x3 = vector.x3; + } + public void SetValues(in SPVector3 vector) { this.x1 = vector.x1; @@ -153,11 +155,18 @@ namespace Geometry this.x3 = vector.x3; } - public void SetValues(in DPVector3 vector) + public void SetReverseOf(in DPVector3 vector) { - this.x1 = vector.x1; - this.x2 = vector.x2; - this.x3 = vector.x3; + this.x1 = -vector.x1; + this.x2 = -vector.x2; + this.x3 = -vector.x3; + } + + public void SetReverseOf(in SPVector3 vector) + { + this.x1 = -vector.x1; + this.x2 = -vector.x2; + this.x3 = -vector.x3; } public readonly override string ToString() @@ -172,45 +181,6 @@ namespace Geometry 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; @@ -218,56 +188,6 @@ namespace Geometry 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; @@ -282,11 +202,56 @@ namespace Geometry quotient.x3 = dividend.x3 / divisor; } + 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 GetMean2( in DPVector3 vector1, in DPVector3 vector2, - out DPVector3 result - ) + out DPVector3 result) { result.x1 = (vector1.x1 + vector2.x1) * 0.5; result.x2 = (vector1.x2 + vector2.x2) * 0.5; @@ -297,8 +262,7 @@ namespace Geometry in DPVector3 vector1, in DPVector3 vector2, in DPVector3 vector3, - out DPVector3 result - ) + 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; @@ -310,8 +274,7 @@ namespace Geometry in DPVector3 vector2, in DPVector3 vector3, in DPVector3 vector4, - out DPVector3 result - ) + 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; @@ -324,8 +287,7 @@ namespace Geometry in DPVector3 vector3, in DPVector3 vector4, in DPVector3 vector5, - out DPVector3 result - ) + 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; diff --git a/Geometry/SPMatrix2x2.cs b/Geometry/SPMatrix2x2.cs index 1fbac95..5812376 100644 --- a/Geometry/SPMatrix2x2.cs +++ b/Geometry/SPMatrix2x2.cs @@ -76,33 +76,6 @@ namespace Geometry 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); @@ -132,14 +105,59 @@ namespace Geometry return true; } - public void MakeTransposedOf(in SPMatrix2x2 matrix) + public void Reset() + { + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + } + + public void SetToIdentity() + { + this.r1c1 = 1.0f; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = 1.0f; + } + + public void SetToDiagonal(float d1, float d2) + { + this.r1c1 = d1; + this.r1c2 = 0.0f; + + this.r2c1 = 0.0f; + this.r2c2 = d2; + } + + 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 SetTransposedOf(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) + public void SetTransposedOf(in DPMatrix2x2 matrix) { this.r1c1 = (float)matrix.r1c1; this.r1c2 = (float)matrix.r2c1; @@ -148,7 +166,7 @@ namespace Geometry this.r2c2 = (float)matrix.r2c2; } - public bool MakeInvertedOf(in SPMatrix2x2 matrix) + public bool SetInvertedOf(in SPMatrix2x2 matrix) { float determinant = matrix.GetDeterminant(); @@ -172,30 +190,6 @@ namespace Geometry 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; @@ -229,51 +223,6 @@ namespace Geometry 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; @@ -283,64 +232,6 @@ namespace Geometry 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; @@ -359,6 +250,60 @@ namespace Geometry quotient.r2c2 = dividend.r2c2 / divisor; } + 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 GetRightProduct(in SPMatrix2x2 matrix, in SPVector2 vector, out SPVector2 result) { float x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; diff --git a/Geometry/SPMatrix2x3.cs b/Geometry/SPMatrix2x3.cs index 6c3d7b3..bf7e134 100644 --- a/Geometry/SPMatrix2x3.cs +++ b/Geometry/SPMatrix2x3.cs @@ -192,60 +192,6 @@ namespace Geometry 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; @@ -258,76 +204,6 @@ namespace Geometry 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; @@ -352,6 +228,72 @@ namespace Geometry quotient.r3c2 = dividend.r3c2 / divisor; } + 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 GetRightProduct(in SPMatrix2x3 matrix, in SPVector2 vector, out SPVector3 result) { result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2; diff --git a/Geometry/SPMatrix3x2.cs b/Geometry/SPMatrix3x2.cs index 37d0090..838d356 100644 --- a/Geometry/SPMatrix3x2.cs +++ b/Geometry/SPMatrix3x2.cs @@ -178,57 +178,6 @@ namespace Geometry sum.r2c3 = matrix1.r2c3 + matrix2.r2c3; } - 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; - sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; - sum.r2c3 = matrix1.r2c3 + matrix2.r2c3 + matrix3.r2c3; - } - - 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); - sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); - sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3); - } - - 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; - - 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(in SPMatrix3x2 minuend, in SPMatrix3x2 subtrahend, out SPMatrix3x2 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; @@ -240,72 +189,6 @@ namespace Geometry difference.r2c3 = minuend.r2c3 - subtrahend.r2c3; } - 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; - sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; - sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; - } - - 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; - sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; - sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; - } - - public static void GetWeightedSum4( - float weight1, 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); - sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); - sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); - } - - public static void GetWeightedSum5( - float weight1, 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; - - 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(in SPMatrix3x2 multiplicand, float multiplier, out SPMatrix3x2 product) { product.r1c1 = multiplicand.r1c1 * multiplier; @@ -328,6 +211,68 @@ namespace Geometry quotient.r2c3 = dividend.r2c3 / divisor; } + 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; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; + } + + 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; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; + } + + public static void GetWeightedSum4( + float weight1, 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); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); + } + + public static void GetWeightedSum5( + float weight1, 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; + + 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 GetRightProduct(in SPMatrix3x2 matrix, in SPVector3 vector, out SPVector2 result) { result.x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; diff --git a/Geometry/SPMatrix3x3.cs b/Geometry/SPMatrix3x3.cs index e82afff..bb1ec2f 100644 --- a/Geometry/SPMatrix3x3.cs +++ b/Geometry/SPMatrix3x3.cs @@ -104,51 +104,6 @@ namespace Geometry return -SPUtility.EPSYLON <= determinant && determinant <= SPUtility.EPSYLON; } - public void Reset() - { - this.r1c1 = 0.0f; - this.r1c2 = 0.0f; - this.r1c3 = 0.0f; - - this.r2c1 = 0.0f; - this.r2c2 = 0.0f; - this.r2c3 = 0.0f; - - this.r3c1 = 0.0f; - this.r3c2 = 0.0f; - this.r3c3 = 0.0f; - } - - public void MakeIdentity() - { - this.r1c1 = 1.0f; - this.r1c2 = 0.0f; - this.r1c3 = 0.0f; - - this.r2c1 = 0.0f; - this.r2c2 = 1.0f; - this.r2c3 = 0.0f; - - this.r3c1 = 0.0f; - this.r3c2 = 0.0f; - this.r3c3 = 1.0f; - } - - public void MakeDiagonal(float d1, float d2, float d3) - { - this.r1c1 = d1; - this.r1c2 = 0.0f; - this.r1c3 = 0.0f; - - this.r2c1 = 0.0f; - this.r2c2 = d2; - this.r2c3 = 0.0f; - - this.r2c1 = 0.0f; - this.r2c2 = 0.0f; - this.r2c3 = d3; - } - public void Transpose() { (this.r1c2, this.r2c1) = (this.r2c1, this.r1c2); @@ -191,65 +146,49 @@ namespace Geometry return true; } - public void MakeTransposedOf(in SPMatrix3x3 matrix) + public void Reset() { - this.r1c1 = matrix.r1c1; - this.r2c2 = matrix.r2c2; - this.r3c3 = matrix.r3c3; + this.r1c1 = 0.0f; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; - (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); - (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); - (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + this.r2c3 = 0.0f; + + this.r3c1 = 0.0f; + this.r3c2 = 0.0f; + this.r3c3 = 0.0f; } - public void MakeTransposedOf(in DPMatrix3x3 matrix) + public void SetToIdentity() { - this.r1c1 = (float)matrix.r1c1; - this.r1c2 = (float)matrix.r2c1; - this.r1c3 = (float)matrix.r3c1; + this.r1c1 = 1.0f; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; - this.r2c1 = (float)matrix.r1c2; - this.r2c2 = (float)matrix.r2c2; - this.r2c3 = (float)matrix.r3c2; + this.r2c1 = 0.0f; + this.r2c2 = 1.0f; + this.r2c3 = 0.0f; - this.r3c1 = (float)matrix.r1c3; - this.r3c2 = (float)matrix.r2c3; - this.r3c3 = (float)matrix.r3c3; + this.r3c1 = 0.0f; + this.r3c2 = 0.0f; + this.r3c3 = 1.0f; } - public bool MakeInvertedOf(in SPMatrix3x3 matrix) + public void SetToDiagonal(float d1, float d2, float d3) { - float determinant = matrix.GetDeterminant(); - - if (-SPUtility.EPSYLON <= determinant && determinant <= SPUtility.EPSYLON) { - return false; - } + this.r1c1 = d1; + this.r1c2 = 0.0f; + this.r1c3 = 0.0f; - float r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; - float r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; - float r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; + this.r2c1 = 0.0f; + this.r2c2 = d2; + this.r2c3 = 0.0f; - float r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; - float r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; - float r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; - - float r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; - float r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; - float r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; - - this.r1c1 = r1c1 / determinant; - this.r1c2 = r1c2 / determinant; - this.r1c3 = r1c3 / determinant; - - this.r2c1 = r2c1 / determinant; - this.r2c2 = r2c2 / determinant; - this.r2c3 = r2c3 / determinant; - - this.r3c1 = r3c1 / determinant; - this.r3c2 = r3c2 / determinant; - this.r3c3 = r3c3 / determinant; - - return true; + this.r2c1 = 0.0f; + this.r2c2 = 0.0f; + this.r2c3 = d3; } public void SetValues(in SPMatrix3x3 matrix) @@ -282,11 +221,65 @@ namespace Geometry this.r3c3 = (float)matrix.r3c3; } - public void SetMainDiagonal(float d1, float d2, float d3) + public void SetTransposedOf(in SPMatrix3x3 matrix) { - this.r1c1 = d1; - this.r2c2 = d2; - this.r3c3 = d3; + this.r1c1 = matrix.r1c1; + this.r2c2 = matrix.r2c2; + this.r3c3 = matrix.r3c3; + + (this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2); + (this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3); + (this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3); + } + + public void SetTransposedOf(in DPMatrix3x3 matrix) + { + this.r1c1 = (float)matrix.r1c1; + this.r1c2 = (float)matrix.r2c1; + this.r1c3 = (float)matrix.r3c1; + + this.r2c1 = (float)matrix.r1c2; + this.r2c2 = (float)matrix.r2c2; + this.r2c3 = (float)matrix.r3c2; + + this.r3c1 = (float)matrix.r1c3; + this.r3c2 = (float)matrix.r2c3; + this.r3c3 = (float)matrix.r3c3; + } + + public bool SetInvertedOf(in SPMatrix3x3 matrix) + { + float determinant = matrix.GetDeterminant(); + + if (-SPUtility.EPSYLON <= determinant && determinant <= SPUtility.EPSYLON) { + return false; + } + + float r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2; + float r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3; + float r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2; + + float r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3; + float r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1; + float r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3; + + float r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1; + float r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2; + float r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1; + + this.r1c1 = r1c1 / determinant; + this.r1c2 = r1c2 / determinant; + this.r1c3 = r1c3 / determinant; + + this.r2c1 = r2c1 / determinant; + this.r2c2 = r2c2 / determinant; + this.r2c3 = r2c3 / determinant; + + this.r3c1 = r3c1 / determinant; + this.r3c2 = r3c2 / determinant; + this.r3c3 = r3c3 / determinant; + + return true; } public void SetRow1(float c1, float c2, float c3) @@ -346,69 +339,6 @@ namespace Geometry sum.r3c3 = matrix1.r3c3 + matrix2.r3c3; } - public static void Add3( - in SPMatrix3x3 matrix1, - in SPMatrix3x3 matrix2, - in SPMatrix3x3 matrix3, - out SPMatrix3x3 sum - ) - { - sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1; - sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2; - sum.r1c3 = matrix1.r1c3 + matrix2.r1c3 + matrix3.r1c3; - - sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1; - sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2; - sum.r2c3 = matrix1.r2c3 + matrix2.r2c3 + matrix3.r2c3; - - sum.r3c1 = matrix1.r3c1 + matrix2.r3c1 + matrix3.r3c1; - sum.r3c2 = matrix1.r3c2 + matrix2.r3c2 + matrix3.r3c2; - sum.r3c3 = matrix1.r3c3 + matrix2.r3c3 + matrix3.r3c3; - } - - public static void Add4( - in SPMatrix3x3 matrix1, - in SPMatrix3x3 matrix2, - in SPMatrix3x3 matrix3, - in SPMatrix3x3 matrix4, - out SPMatrix3x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1); - sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2); - sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3); - - sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1); - sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2); - sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3); - - sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1); - sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2); - sum.r3c3 = (matrix1.r3c3 + matrix2.r3c3) + (matrix3.r3c3 + matrix4.r3c3); - } - - public static void Add5( - in SPMatrix3x3 matrix1, - in SPMatrix3x3 matrix2, - in SPMatrix3x3 matrix3, - in SPMatrix3x3 matrix4, - in SPMatrix3x3 matrix5, - out SPMatrix3x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1; - sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2; - sum.r1c3 = (matrix1.r1c3 + matrix2.r1c3) + (matrix3.r1c3 + matrix4.r1c3) + matrix5.r1c3; - - sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1; - sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2; - sum.r2c3 = (matrix1.r2c3 + matrix2.r2c3) + (matrix3.r2c3 + matrix4.r2c3) + matrix5.r2c3; - - sum.r3c1 = (matrix1.r3c1 + matrix2.r3c1) + (matrix3.r3c1 + matrix4.r3c1) + matrix5.r3c1; - sum.r3c2 = (matrix1.r3c2 + matrix2.r3c2) + (matrix3.r3c2 + matrix4.r3c2) + matrix5.r3c2; - sum.r3c3 = (matrix1.r3c3 + matrix2.r3c3) + (matrix3.r3c3 + matrix4.r3c3) + matrix5.r3c3; - } - public static void Subtract(in SPMatrix3x3 minuend, in SPMatrix3x3 subtrahend, out SPMatrix3x3 difference) { difference.r1c1 = minuend.r1c1 - subtrahend.r1c1; @@ -424,88 +354,6 @@ namespace Geometry difference.r3c3 = minuend.r3c3 - subtrahend.r3c3; } - public static void GetWeightedSum2( - float weight1, in SPMatrix3x3 matrix1, - float weight2, in SPMatrix3x3 matrix2, - out SPMatrix3x3 sum - ) - { - sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; - sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; - sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2; - - sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; - sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; - sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; - - sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; - sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; - sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2; - } - - public static void GetWeightedSum3( - float weight1, in SPMatrix3x3 matrix1, - float weight2, in SPMatrix3x3 matrix2, - float weight3, in SPMatrix3x3 matrix3, - out SPMatrix3x3 sum - ) - { - sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; - sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; - sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2 + matrix3.r1c3 * weight3; - - sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; - sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; - sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; - - sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; - sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; - sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2 + matrix3.r3c3 * weight3; - } - - public static void GetWeightedSum4( - float weight1, in SPMatrix3x3 matrix1, - float weight2, in SPMatrix3x3 matrix2, - float weight3, in SPMatrix3x3 matrix3, - float weight4, in SPMatrix3x3 matrix4, - out SPMatrix3x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); - sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); - sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4); - - sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); - sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); - sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); - - sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); - sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); - sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4); - } - - public static void GetWeightedSum5( - float weight1, in SPMatrix3x3 matrix1, - float weight2, in SPMatrix3x3 matrix2, - float weight3, in SPMatrix3x3 matrix3, - float weight4, in SPMatrix3x3 matrix4, - float weight5, in SPMatrix3x3 matrix5, - out SPMatrix3x3 sum - ) - { - sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; - sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; - sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4) + matrix5.r1c3 * weight5; - - sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; - sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; - sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4) + matrix5.r2c3 * weight5; - - sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; - sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; - sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4) + matrix5.r3c3 * weight5; - } - public static void Multiply(in SPMatrix3x3 multiplicand, float multiplier, out SPMatrix3x3 product) { product.r1c1 = multiplicand.r1c1 * multiplier; @@ -536,6 +384,84 @@ namespace Geometry quotient.r3c3 = dividend.r3c3 / divisor; } + public static void GetWeightedSum2( + float weight1, in SPMatrix3x3 matrix1, + float weight2, in SPMatrix3x3 matrix2, + out SPMatrix3x3 sum) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2; + sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2; + } + + public static void GetWeightedSum3( + float weight1, in SPMatrix3x3 matrix1, + float weight2, in SPMatrix3x3 matrix2, + float weight3, in SPMatrix3x3 matrix3, + out SPMatrix3x3 sum) + { + sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3; + sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3; + sum.r1c3 = matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2 + matrix3.r1c3 * weight3; + + sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3; + sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3; + sum.r2c3 = matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2 + matrix3.r2c3 * weight3; + + sum.r3c1 = matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2 + matrix3.r3c1 * weight3; + sum.r3c2 = matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2 + matrix3.r3c2 * weight3; + sum.r3c3 = matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2 + matrix3.r3c3 * weight3; + } + + public static void GetWeightedSum4( + float weight1, in SPMatrix3x3 matrix1, + float weight2, in SPMatrix3x3 matrix2, + float weight3, in SPMatrix3x3 matrix3, + float weight4, in SPMatrix3x3 matrix4, + out SPMatrix3x3 sum) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4); + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4); + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4); + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4); + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4); + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4); + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4); + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4); + sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4); + } + + public static void GetWeightedSum5( + float weight1, in SPMatrix3x3 matrix1, + float weight2, in SPMatrix3x3 matrix2, + float weight3, in SPMatrix3x3 matrix3, + float weight4, in SPMatrix3x3 matrix4, + float weight5, in SPMatrix3x3 matrix5, + out SPMatrix3x3 sum) + { + sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5; + sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5; + sum.r1c3 = (matrix1.r1c3 * weight1 + matrix2.r1c3 * weight2) + (matrix3.r1c3 * weight3 + matrix4.r1c3 * weight4) + matrix5.r1c3 * weight5; + + sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5; + sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5; + sum.r2c3 = (matrix1.r2c3 * weight1 + matrix2.r2c3 * weight2) + (matrix3.r2c3 * weight3 + matrix4.r2c3 * weight4) + matrix5.r2c3 * weight5; + + sum.r3c1 = (matrix1.r3c1 * weight1 + matrix2.r3c1 * weight2) + (matrix3.r3c1 * weight3 + matrix4.r3c1 * weight4) + matrix5.r3c1 * weight5; + sum.r3c2 = (matrix1.r3c2 * weight1 + matrix2.r3c2 * weight2) + (matrix3.r3c2 * weight3 + matrix4.r3c2 * weight4) + matrix5.r3c2 * weight5; + sum.r3c3 = (matrix1.r3c3 * weight1 + matrix2.r3c3 * weight2) + (matrix3.r3c3 * weight3 + matrix4.r3c3 * weight4) + matrix5.r3c3 * weight5; + } + public static void GetRightProduct(in SPMatrix3x3 matrix, in SPVector3 vector, out SPVector3 result) { float x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3; diff --git a/Geometry/SPQuaternion.cs b/Geometry/SPQuaternion.cs index 4d3b256..13bd4a5 100644 --- a/Geometry/SPQuaternion.cs +++ b/Geometry/SPQuaternion.cs @@ -45,14 +45,6 @@ namespace Geometry 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; @@ -77,6 +69,14 @@ namespace Geometry this.x3 = (float)quaternion.x3; } + public void SetConjugateOf(in SPQuaternion quaternion) + { + this.s0 = quaternion.s0; + this.x1 = -quaternion.x1; + this.x2 = -quaternion.x2; + this.x3 = -quaternion.x3; + } + public readonly void MakeRotationMatrix(out SPMatrix3x3 matrix) { float s0s0 = this.s0 * this.s0; diff --git a/Geometry/SPVector2.cs b/Geometry/SPVector2.cs index 07098c8..8528097 100644 --- a/Geometry/SPVector2.cs +++ b/Geometry/SPVector2.cs @@ -103,17 +103,12 @@ namespace Geometry ); } - public void Invert() + public void Reverse() { this.x1 = -this.x1; this.x2 = -this.x2; } - public readonly SPVector2 GetInverted() - { - return new SPVector2(-this.x1, -this.x2); - } - public readonly bool IsZero() { return this.GetSquareModule() <= SPUtility.SQUARE_EPSYLON; @@ -137,18 +132,29 @@ namespace Geometry this.x2 = x2; } - public void SetValues(in DPVector2 vector) - { - this.x1 = (float)vector.x1; - this.x2 = (float)vector.x2; - } - public void SetValues(in SPVector2 vector) { this.x1 = vector.x1; this.x2 = vector.x2; } + public void SetValues(in DPVector2 vector) + { + this.x1 = (float)vector.x1; + this.x2 = (float)vector.x2; + } + + public void SetReverseOf(in SPVector2 vector) + { + this.x1 = -vector.x1; + this.x2 = -vector.x2; + } + + public void SetReverseOf(in DPVector2 vector) + { + this.x1 = -(float)vector.x1; + this.x2 = -(float)vector.x2; + } public readonly override string ToString() { return String.Format("SPVector2({0}, {1})", this.x1, this.x2); @@ -160,48 +166,24 @@ namespace Geometry sum.x2 = vector1.x2 + vector2.x2; } - public static void Add3( - in SPVector2 vector1, - in SPVector2 vector2, - in SPVector2 vector3, - out SPVector2 sum - ) - { - sum.x1 = vector1.x1 + vector2.x1 + vector3.x1; - sum.x2 = vector1.x2 + vector2.x2 + vector3.x2; - } - - public static void Add4( - in SPVector2 vector1, - in SPVector2 vector2, - in SPVector2 vector3, - in SPVector2 vector4, - out SPVector2 sum - ) - { - sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1); - sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2); - } - - public static void Add5( - in SPVector2 vector1, - in SPVector2 vector2, - in SPVector2 vector3, - in SPVector2 vector4, - in SPVector2 vector5, - out SPVector2 sum - ) - { - sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1; - sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2; - } - public static void Subtract(in SPVector2 minuend, in SPVector2 subtrahend, out SPVector2 difference) { difference.x1 = minuend.x1 - subtrahend.x1; difference.x2 = minuend.x2 - subtrahend.x2; } + public static void Muliply(in SPVector2 multiplicand, float multiplier, out SPVector2 product) + { + product.x1 = multiplicand.x1 * multiplier; + product.x2 = multiplicand.x2 * multiplier; + } + + public static void Divide(in SPVector2 dividend, float divisor, out SPVector2 quotient) + { + quotient.x1 = dividend.x1 / divisor; + quotient.x2 = dividend.x2 / divisor; + } + public static void GetWeightedSum2( float weight1, in SPVector2 vector1, float weight2, in SPVector2 vector2, @@ -248,18 +230,6 @@ namespace Geometry sum.x2 = (vector1.x2 * weight1 + vector2.x2 * weight2) + (vector3.x2 * weight3 + vector4.x2 * weight4); } - public static void Muliply(in SPVector2 multiplicand, float multiplier, out SPVector2 product) - { - product.x1 = multiplicand.x1 * multiplier; - product.x2 = multiplicand.x2 * multiplier; - } - - public static void Divide(in SPVector2 dividend, float divisor, out SPVector2 quotient) - { - quotient.x1 = dividend.x1 / divisor; - quotient.x2 = dividend.x2 / divisor; - } - public static void GetMean2( in SPVector2 vector1, in SPVector2 vector2, diff --git a/Geometry/SPVector3.cs b/Geometry/SPVector3.cs index eaff507..d911809 100644 --- a/Geometry/SPVector3.cs +++ b/Geometry/SPVector3.cs @@ -109,18 +109,13 @@ namespace Geometry ); } - public void Invert() + public void Reverse() { this.x1 = -this.x1; this.x2 = -this.x2; this.x3 = -this.x3; } - public readonly SPVector3 GetInverted() - { - return new SPVector3(-this.x1, -this.x2, -this.x3); - } - public readonly bool IsZero() { return this.GetSquareModule() <= SPUtility.SQUARE_EPSYLON; @@ -160,6 +155,20 @@ namespace Geometry this.x3 = vector.x3; } + public void SetReverseOf(in SPVector3 vector) + { + this.x1 = -vector.x1; + this.x2 = -vector.x2; + this.x3 = -vector.x3; + } + + public void SetReverseOf(in DPVector3 vector) + { + this.x1 = -(float)vector.x1; + this.x2 = -(float)vector.x2; + this.x3 = -(float)vector.x3; + } + public readonly override string ToString() { return String.Format("SPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3); @@ -172,45 +181,6 @@ namespace Geometry sum.x3 = vector1.x3 + vector2.x3; } - public static void Add3( - in SPVector3 vector1, - in SPVector3 vector2, - in SPVector3 vector3, - out SPVector3 sum - ) - { - sum.x1 = vector1.x1 + vector2.x1 + vector3.x1; - sum.x2 = vector1.x2 + vector2.x2 + vector3.x2; - sum.x3 = vector1.x3 + vector2.x3 + vector3.x3; - } - - public static void Add4( - in SPVector3 vector1, - in SPVector3 vector2, - in SPVector3 vector3, - in SPVector3 vector4, - out SPVector3 sum - ) - { - sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1); - sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2); - sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3); - } - - public static void Add5( - in SPVector3 vector1, - in SPVector3 vector2, - in SPVector3 vector3, - in SPVector3 vector4, - in SPVector3 vector5, - out SPVector3 sum - ) - { - sum.x1 = (vector1.x1 + vector2.x1) + (vector3.x1 + vector4.x1) + vector5.x1; - sum.x2 = (vector1.x2 + vector2.x2) + (vector3.x2 + vector4.x2) + vector5.x2; - sum.x3 = (vector1.x3 + vector2.x3) + (vector3.x3 + vector4.x3) + vector5.x3; - } - public static void Subtract(in SPVector3 minuend, in SPVector3 subtrahend, out SPVector3 difference) { difference.x1 = minuend.x1 - subtrahend.x1; @@ -218,6 +188,20 @@ namespace Geometry difference.x3 = minuend.x3 - subtrahend.x3; } + public static void Muliply(in SPVector3 multiplicand, float multiplier, out SPVector3 product) + { + product.x1 = multiplicand.x1 * multiplier; + product.x2 = multiplicand.x2 * multiplier; + product.x3 = multiplicand.x3 * multiplier; + } + + public static void Divide(in SPVector3 dividend, float divisor, out SPVector3 quotient) + { + quotient.x1 = dividend.x1 / divisor; + quotient.x2 = dividend.x2 / divisor; + quotient.x3 = dividend.x3 / divisor; + } + public static void GetWeightedSum2( float weight1, in SPVector3 vector1, float weight2, in SPVector3 vector2, @@ -268,20 +252,6 @@ namespace Geometry sum.x3 = (vector1.x3 * weight1 + vector2.x3 * weight2) + (vector3.x3 * weight3 + vector4.x3 * weight4) + vector5.x3 * weight5; } - public static void Muliply(in SPVector3 multiplicand, float multiplier, out SPVector3 product) - { - product.x1 = multiplicand.x1 * multiplier; - product.x2 = multiplicand.x2 * multiplier; - product.x3 = multiplicand.x3 * multiplier; - } - - public static void Divide(in SPVector3 dividend, float divisor, out SPVector3 quotient) - { - quotient.x1 = dividend.x1 / divisor; - quotient.x2 = dividend.x2 / divisor; - quotient.x3 = dividend.x3 / divisor; - } - public static void GetMean2( in SPVector3 vector1, in SPVector3 vector2,