407 lines
13 KiB
C#
407 lines
13 KiB
C#
/*
|
|
* Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
using System;
|
|
|
|
/*
|
|
* Author: Andrey Pokidov
|
|
* Date: 10 Feb 2019
|
|
*/
|
|
|
|
namespace Geometry
|
|
{
|
|
public struct DPMatrix2x2
|
|
{
|
|
public double r1c1, r1c2;
|
|
|
|
public double r2c1, r2c2;
|
|
|
|
public DPMatrix2x2()
|
|
{
|
|
this.r1c1 = 0.0;
|
|
this.r1c2 = 0.0;
|
|
|
|
this.r2c1 = 0.0;
|
|
this.r2c2 = 0.0;
|
|
}
|
|
|
|
public DPMatrix2x2(double d1, double d2)
|
|
{
|
|
this.r1c1 = d1;
|
|
this.r1c2 = 0.0;
|
|
|
|
this.r2c1 = 0.0;
|
|
this.r2c2 = d2;
|
|
}
|
|
|
|
public DPMatrix2x2(in DPMatrix2x2 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r1c2 = matrix.r1c2;
|
|
|
|
this.r2c1 = matrix.r2c1;
|
|
this.r2c2 = matrix.r2c2;
|
|
}
|
|
|
|
public DPMatrix2x2(in SPMatrix2x2 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r1c2 = matrix.r1c2;
|
|
|
|
this.r2c1 = matrix.r2c1;
|
|
this.r2c2 = matrix.r2c2;
|
|
}
|
|
|
|
public readonly double GetDeterminant()
|
|
{
|
|
return this.r1c1 * this.r2c2 - this.r1c2 * this.r2c1;
|
|
}
|
|
|
|
public readonly bool IsSingular()
|
|
{
|
|
double determinant = this.GetDeterminant();
|
|
return -DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON;
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
this.r1c1 = 0.0;
|
|
this.r1c2 = 0.0;
|
|
|
|
this.r2c1 = 0.0;
|
|
this.r2c2 = 0.0;
|
|
}
|
|
|
|
public void MakeIdentity()
|
|
{
|
|
this.r1c1 = 1.0;
|
|
this.r1c2 = 0.0;
|
|
|
|
this.r2c1 = 0.0;
|
|
this.r2c2 = 1.0;
|
|
}
|
|
|
|
public void MakeDiagonal(double d1, double d2)
|
|
{
|
|
this.r1c1 = d1;
|
|
this.r1c2 = 0.0;
|
|
|
|
this.r2c1 = 0.0;
|
|
this.r2c2 = d2;
|
|
}
|
|
|
|
public void Transpose()
|
|
{
|
|
(this.r1c2, this.r2c1) = (this.r2c1, this.r1c2);
|
|
}
|
|
|
|
public bool Invert()
|
|
{
|
|
double determinant = this.GetDeterminant();
|
|
|
|
if (-DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double r1c1 = this.r2c2;
|
|
double r1c2 = -this.r1c2;
|
|
|
|
double r2c1 = -this.r2c1;
|
|
double r2c2 = this.r1c1;
|
|
|
|
this.r1c1 = r1c1 / determinant;
|
|
this.r1c2 = r1c2 / determinant;
|
|
|
|
this.r2c1 = r2c1 / determinant;
|
|
this.r2c2 = r2c2 / determinant;
|
|
|
|
return true;
|
|
}
|
|
|
|
public void MakeTransposedOf(in DPMatrix2x2 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r2c2 = matrix.r2c2;
|
|
(this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2);
|
|
}
|
|
|
|
public void MakeTransposedOf(in SPMatrix2x2 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r1c2 = matrix.r2c1;
|
|
|
|
this.r2c1 = matrix.r1c2;
|
|
this.r2c2 = matrix.r2c2;
|
|
}
|
|
|
|
public bool MakeInvertedOf(in DPMatrix2x2 matrix)
|
|
{
|
|
double determinant = matrix.GetDeterminant();
|
|
|
|
if (-DPUtility.EPSYLON <= determinant && determinant <= DPUtility.EPSYLON)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double r1c1 = matrix.r2c2;
|
|
double r1c2 = -matrix.r1c2;
|
|
|
|
double r2c1 = -matrix.r2c1;
|
|
double r2c2 = matrix.r1c1;
|
|
|
|
this.r1c1 = r1c1 / determinant;
|
|
this.r1c2 = r1c2 / determinant;
|
|
|
|
this.r2c1 = r2c1 / determinant;
|
|
this.r2c2 = r2c2 / determinant;
|
|
|
|
return true;
|
|
}
|
|
|
|
public void SetValues(in DPMatrix2x2 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r1c2 = matrix.r1c2;
|
|
|
|
this.r2c1 = matrix.r2c1;
|
|
this.r2c2 = matrix.r2c2;
|
|
}
|
|
|
|
public void SetValues(in SPMatrix2x2 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r1c2 = matrix.r1c2;
|
|
|
|
this.r2c1 = matrix.r2c1;
|
|
this.r2c2 = matrix.r2c2;
|
|
}
|
|
|
|
public void SetMainDiagonal(double d1, double d2)
|
|
{
|
|
this.r1c1 = d1;
|
|
this.r2c2 = d2;
|
|
}
|
|
|
|
public void SetRow1(double c1, double c2)
|
|
{
|
|
this.r1c1 = c1;
|
|
this.r1c2 = c2;
|
|
}
|
|
|
|
public void SetRow2(double c1, double c2)
|
|
{
|
|
this.r2c1 = c1;
|
|
this.r2c2 = c2;
|
|
}
|
|
|
|
public void SetColumn1(double r1, double r2)
|
|
{
|
|
this.r1c1 = r1;
|
|
this.r2c1 = r2;
|
|
}
|
|
|
|
public void SetColumn2(double r1, double r2)
|
|
{
|
|
this.r1c2 = r1;
|
|
this.r2c2 = r2;
|
|
}
|
|
|
|
public static void Add(in DPMatrix2x2 matrix1, in DPMatrix2x2 matrix2, out DPMatrix2x2 sum)
|
|
{
|
|
sum.r1c1 = matrix1.r1c1 + matrix2.r1c1;
|
|
sum.r1c2 = matrix1.r1c2 + matrix2.r1c2;
|
|
|
|
sum.r2c1 = matrix1.r2c1 + matrix2.r2c1;
|
|
sum.r2c2 = matrix1.r2c2 + matrix2.r2c2;
|
|
}
|
|
|
|
public static void Add3(
|
|
in DPMatrix2x2 matrix1,
|
|
in DPMatrix2x2 matrix2,
|
|
in DPMatrix2x2 matrix3,
|
|
out DPMatrix2x2 sum
|
|
)
|
|
{
|
|
sum.r1c1 = matrix1.r1c1 + matrix2.r1c1 + matrix3.r1c1;
|
|
sum.r1c2 = matrix1.r1c2 + matrix2.r1c2 + matrix3.r1c2;
|
|
|
|
sum.r2c1 = matrix1.r2c1 + matrix2.r2c1 + matrix3.r2c1;
|
|
sum.r2c2 = matrix1.r2c2 + matrix2.r2c2 + matrix3.r2c2;
|
|
}
|
|
|
|
public static void Add4(
|
|
in DPMatrix2x2 matrix1,
|
|
in DPMatrix2x2 matrix2,
|
|
in DPMatrix2x2 matrix3,
|
|
in DPMatrix2x2 matrix4,
|
|
out DPMatrix2x2 sum
|
|
)
|
|
{
|
|
sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1);
|
|
sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2);
|
|
|
|
sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1);
|
|
sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2);
|
|
}
|
|
|
|
public static void Add5(
|
|
in DPMatrix2x2 matrix1,
|
|
in DPMatrix2x2 matrix2,
|
|
in DPMatrix2x2 matrix3,
|
|
in DPMatrix2x2 matrix4,
|
|
in DPMatrix2x2 matrix5,
|
|
out DPMatrix2x2 sum
|
|
)
|
|
{
|
|
sum.r1c1 = (matrix1.r1c1 + matrix2.r1c1) + (matrix3.r1c1 + matrix4.r1c1) + matrix5.r1c1;
|
|
sum.r1c2 = (matrix1.r1c2 + matrix2.r1c2) + (matrix3.r1c2 + matrix4.r1c2) + matrix5.r1c2;
|
|
|
|
sum.r2c1 = (matrix1.r2c1 + matrix2.r2c1) + (matrix3.r2c1 + matrix4.r2c1) + matrix5.r2c1;
|
|
sum.r2c2 = (matrix1.r2c2 + matrix2.r2c2) + (matrix3.r2c2 + matrix4.r2c2) + matrix5.r2c2;
|
|
}
|
|
|
|
public static void Subtract(in DPMatrix2x2 minuend, in DPMatrix2x2 subtrahend, out DPMatrix2x2 difference)
|
|
{
|
|
difference.r1c1 = minuend.r1c1 - subtrahend.r1c1;
|
|
difference.r1c2 = minuend.r1c2 - subtrahend.r1c2;
|
|
|
|
difference.r2c1 = minuend.r2c1 - subtrahend.r2c1;
|
|
difference.r2c2 = minuend.r2c2 - subtrahend.r2c2;
|
|
}
|
|
|
|
public static void GetWeightedSum2(
|
|
double weight1, in DPMatrix2x2 matrix1,
|
|
double weight2, in DPMatrix2x2 matrix2,
|
|
out DPMatrix2x2 sum
|
|
)
|
|
{
|
|
sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2;
|
|
sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2;
|
|
|
|
sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2;
|
|
sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2;
|
|
}
|
|
|
|
public static void GetWeightedSum3(
|
|
float weight1, in SPMatrix2x2 matrix1,
|
|
float weight2, in SPMatrix2x2 matrix2,
|
|
float weight3, in SPMatrix2x2 matrix3,
|
|
out DPMatrix2x2 sum
|
|
)
|
|
{
|
|
sum.r1c1 = matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2 + matrix3.r1c1 * weight3;
|
|
sum.r1c2 = matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2 + matrix3.r1c2 * weight3;
|
|
|
|
sum.r2c1 = matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2 + matrix3.r2c1 * weight3;
|
|
sum.r2c2 = matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2 + matrix3.r2c2 * weight3;
|
|
}
|
|
|
|
public static void GetWeightedSum4(
|
|
float weight1, in SPMatrix2x2 matrix1,
|
|
float weight2, in SPMatrix2x2 matrix2,
|
|
float weight3, in SPMatrix2x2 matrix3,
|
|
float weight4, in SPMatrix2x2 matrix4,
|
|
out DPMatrix2x2 sum
|
|
)
|
|
{
|
|
sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4);
|
|
sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4);
|
|
|
|
sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4);
|
|
sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4);
|
|
}
|
|
|
|
public static void GetWeightedSum5(
|
|
double weight1, in DPMatrix2x2 matrix1,
|
|
double weight2, in DPMatrix2x2 matrix2,
|
|
double weight3, in DPMatrix2x2 matrix3,
|
|
double weight4, in DPMatrix2x2 matrix4,
|
|
double weight5, in DPMatrix2x2 matrix5,
|
|
out DPMatrix2x2 sum
|
|
)
|
|
{
|
|
sum.r1c1 = (matrix1.r1c1 * weight1 + matrix2.r1c1 * weight2) + (matrix3.r1c1 * weight3 + matrix4.r1c1 * weight4) + matrix5.r1c1 * weight5;
|
|
sum.r1c2 = (matrix1.r1c2 * weight1 + matrix2.r1c2 * weight2) + (matrix3.r1c2 * weight3 + matrix4.r1c2 * weight4) + matrix5.r1c2 * weight5;
|
|
|
|
sum.r2c1 = (matrix1.r2c1 * weight1 + matrix2.r2c1 * weight2) + (matrix3.r2c1 * weight3 + matrix4.r2c1 * weight4) + matrix5.r2c1 * weight5;
|
|
sum.r2c2 = (matrix1.r2c2 * weight1 + matrix2.r2c2 * weight2) + (matrix3.r2c2 * weight3 + matrix4.r2c2 * weight4) + matrix5.r2c2 * weight5;
|
|
}
|
|
|
|
public static void Multiply(in DPMatrix2x2 multiplicand, double multiplier, out DPMatrix2x2 product)
|
|
{
|
|
product.r1c1 = multiplicand.r1c1 * multiplier;
|
|
product.r1c2 = multiplicand.r1c2 * multiplier;
|
|
|
|
product.r2c1 = multiplicand.r2c1 * multiplier;
|
|
product.r2c2 = multiplicand.r2c2 * multiplier;
|
|
}
|
|
|
|
public static void Divide(in DPMatrix2x2 dividend, double divisor, out DPMatrix2x2 quotient)
|
|
{
|
|
quotient.r1c1 = dividend.r1c1 / divisor;
|
|
quotient.r1c2 = dividend.r1c2 / divisor;
|
|
|
|
quotient.r2c1 = dividend.r2c1 / divisor;
|
|
quotient.r2c2 = dividend.r2c2 / divisor;
|
|
}
|
|
|
|
public static void GetRightProduct(in DPMatrix2x2 matrix, in DPVector2 vector, out DPVector2 result)
|
|
{
|
|
double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2;
|
|
double x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2;
|
|
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
}
|
|
|
|
public static void GetLeftProduct(in DPVector2 vector, in DPMatrix2x2 matrix, out DPVector2 result)
|
|
{
|
|
double x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1;
|
|
double x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2;
|
|
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
}
|
|
|
|
public static void LoadZero(out DPMatrix2x2 matrix)
|
|
{
|
|
matrix.r1c1 = 0.0;
|
|
matrix.r1c2 = 0.0;
|
|
|
|
matrix.r2c1 = 0.0;
|
|
matrix.r2c2 = 0.0;
|
|
}
|
|
|
|
public static void LoadIdentity(out DPMatrix2x2 matrix)
|
|
{
|
|
matrix.r1c1 = 1.0;
|
|
matrix.r1c2 = 0.0;
|
|
|
|
matrix.r2c1 = 0.0;
|
|
matrix.r2c2 = 1.0;
|
|
}
|
|
|
|
public static void LoadDiagonal(double d1, double d2, out DPMatrix2x2 matrix)
|
|
{
|
|
matrix.r1c1 = d1;
|
|
matrix.r1c2 = 0.0;
|
|
|
|
matrix.r2c1 = 0.0;
|
|
matrix.r2c2 = d2;
|
|
}
|
|
}
|
|
}
|