407 lines
13 KiB
C#
407 lines
13 KiB
C#
/*
|
|
* Author: Andrey Pokidov
|
|
* License: Apache-2.0
|
|
* Date: 10 Feb 2019
|
|
*/
|
|
|
|
namespace BGC
|
|
{
|
|
public struct Matrix3x3FP64
|
|
{
|
|
public double r1c1 = 0.0, r1c2 = 0.0, r1c3 = 0.0;
|
|
public double r2c1 = 0.0, r2c2 = 0.0, r2c3 = 0.0;
|
|
public double r3c1 = 0.0, r3c2 = 0.0, r3c3 = 0.0;
|
|
|
|
public Matrix3x3FP64(double d1, double d2, double d3)
|
|
{
|
|
this.r1c1 = d1;
|
|
this.r2c2 = d2;
|
|
this.r3c3 = d3;
|
|
}
|
|
|
|
public Matrix3x3FP64(in Matrix3x3FP64 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r1c2 = matrix.r1c2;
|
|
this.r1c3 = matrix.r1c3;
|
|
|
|
this.r2c1 = matrix.r2c1;
|
|
this.r2c2 = matrix.r2c2;
|
|
this.r2c3 = matrix.r2c3;
|
|
|
|
this.r3c1 = matrix.r3c1;
|
|
this.r3c2 = matrix.r3c2;
|
|
this.r3c3 = matrix.r3c3;
|
|
}
|
|
|
|
public Matrix3x3FP64(in Matrix3x3FP32 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r1c2 = matrix.r1c2;
|
|
this.r1c3 = matrix.r1c3;
|
|
|
|
this.r2c1 = matrix.r2c1;
|
|
this.r2c2 = matrix.r2c2;
|
|
this.r2c3 = matrix.r2c3;
|
|
|
|
this.r3c1 = matrix.r3c1;
|
|
this.r3c2 = matrix.r3c2;
|
|
this.r3c3 = matrix.r3c3;
|
|
}
|
|
|
|
public readonly double GetDeterminant()
|
|
{
|
|
return this.r1c1 * (this.r2c2 * this.r3c3 - this.r2c3 * this.r3c2)
|
|
+ this.r1c2 * (this.r2c3 * this.r3c1 - this.r2c1 * this.r3c3)
|
|
+ this.r1c3 * (this.r2c1 * this.r3c2 - this.r2c2 * this.r3c1);
|
|
}
|
|
|
|
public readonly bool IsSingular()
|
|
{
|
|
return UtilityFP64.IsZero(this.GetDeterminant());
|
|
}
|
|
|
|
public void Transpose()
|
|
{
|
|
(this.r1c2, this.r2c1) = (this.r2c1, this.r1c2);
|
|
(this.r1c3, this.r3c1) = (this.r3c1, this.r1c3);
|
|
(this.r2c3, this.r3c2) = (this.r3c2, this.r2c3);
|
|
}
|
|
|
|
public bool Invert()
|
|
{
|
|
double determinant = this.GetDeterminant();
|
|
|
|
if (UtilityFP64.IsZero(determinant))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double r1c1 = this.r2c2 * this.r3c3 - this.r2c3 * this.r3c2;
|
|
double r1c2 = this.r1c3 * this.r3c2 - this.r1c2 * this.r3c3;
|
|
double r1c3 = this.r1c2 * this.r2c3 - this.r1c3 * this.r2c2;
|
|
|
|
double r2c1 = this.r2c3 * this.r3c1 - this.r2c1 * this.r3c3;
|
|
double r2c2 = this.r1c1 * this.r3c3 - this.r1c3 * this.r3c1;
|
|
double r2c3 = this.r1c3 * this.r2c1 - this.r1c1 * this.r2c3;
|
|
|
|
double r3c1 = this.r2c1 * this.r3c2 - this.r2c2 * this.r3c1;
|
|
double r3c2 = this.r1c2 * this.r3c1 - this.r1c1 * this.r3c2;
|
|
double r3c3 = this.r1c1 * this.r2c2 - this.r1c2 * this.r2c1;
|
|
|
|
double mutiplier = 1.0 / determinant;
|
|
|
|
this.r1c1 = r1c1 * mutiplier;
|
|
this.r1c2 = r1c2 * mutiplier;
|
|
this.r1c3 = r1c3 * mutiplier;
|
|
|
|
this.r2c1 = r2c1 * mutiplier;
|
|
this.r2c2 = r2c2 * mutiplier;
|
|
this.r2c3 = r2c3 * mutiplier;
|
|
|
|
this.r3c1 = r3c1 * mutiplier;
|
|
this.r3c2 = r3c2 * mutiplier;
|
|
this.r3c3 = r3c3 * mutiplier;
|
|
|
|
return true;
|
|
}
|
|
|
|
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 SetToIdentity()
|
|
{
|
|
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 SetToDiagonal(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 Set(Matrix3x3FP64 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r1c2 = matrix.r1c2;
|
|
this.r1c3 = matrix.r1c3;
|
|
|
|
this.r2c1 = matrix.r2c1;
|
|
this.r2c2 = matrix.r2c2;
|
|
this.r2c3 = matrix.r2c3;
|
|
|
|
this.r3c1 = matrix.r3c1;
|
|
this.r3c2 = matrix.r3c2;
|
|
this.r3c3 = matrix.r3c3;
|
|
}
|
|
|
|
public void Set(Matrix3x3FP32 matrix)
|
|
{
|
|
this.r1c1 = matrix.r1c1;
|
|
this.r1c2 = matrix.r1c2;
|
|
this.r1c3 = matrix.r1c3;
|
|
|
|
this.r2c1 = matrix.r2c1;
|
|
this.r2c2 = matrix.r2c2;
|
|
this.r2c3 = matrix.r2c3;
|
|
|
|
this.r3c1 = matrix.r3c1;
|
|
this.r3c2 = matrix.r3c2;
|
|
this.r3c3 = matrix.r3c3;
|
|
}
|
|
|
|
public void SetRow1(double c1, double c2, double c3)
|
|
{
|
|
this.r1c1 = c1;
|
|
this.r1c2 = c2;
|
|
this.r1c3 = c3;
|
|
}
|
|
|
|
public void SetRow2(double c1, double c2, double c3)
|
|
{
|
|
this.r2c1 = c1;
|
|
this.r2c2 = c2;
|
|
this.r2c3 = c3;
|
|
}
|
|
|
|
public void SetRow3(double c1, double c2, double c3)
|
|
{
|
|
this.r3c1 = c1;
|
|
this.r3c2 = c2;
|
|
this.r3c3 = c3;
|
|
}
|
|
|
|
public void SetColumn1(double r1, double r2, double r3)
|
|
{
|
|
this.r1c1 = r1;
|
|
this.r2c1 = r2;
|
|
this.r3c1 = r3;
|
|
}
|
|
|
|
public void SetColumn2(double r1, double r2, double r3)
|
|
{
|
|
this.r1c2 = r1;
|
|
this.r2c2 = r2;
|
|
this.r3c2 = r3;
|
|
}
|
|
|
|
public void SetColumn3(double r1, double r2, double r3)
|
|
{
|
|
this.r1c3 = r1;
|
|
this.r2c3 = r2;
|
|
this.r3c3 = r3;
|
|
}
|
|
|
|
public static void GetTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed)
|
|
{
|
|
transposed.r1c1 = matrix.r1c1;
|
|
transposed.r2c2 = matrix.r2c2;
|
|
transposed.r3c3 = matrix.r3c3;
|
|
|
|
(transposed.r1c2, transposed.r2c1) = (matrix.r2c1, matrix.r1c2);
|
|
(transposed.r1c3, transposed.r3c1) = (matrix.r3c1, matrix.r1c3);
|
|
(transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3);
|
|
}
|
|
|
|
public static bool GetInverted(in Matrix3x3FP64 matrix, out Matrix3x3FP64 inverted)
|
|
{
|
|
double determinant = matrix.GetDeterminant();
|
|
|
|
if (UtilityFP64.IsZero(determinant)) {
|
|
LoadZero(out inverted);
|
|
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;
|
|
|
|
double mutiplier = 1.0 / determinant;
|
|
|
|
inverted.r1c1 = r1c1 * mutiplier;
|
|
inverted.r1c2 = r1c2 * mutiplier;
|
|
inverted.r1c3 = r1c3 * mutiplier;
|
|
|
|
inverted.r2c1 = r2c1 * mutiplier;
|
|
inverted.r2c2 = r2c2 * mutiplier;
|
|
inverted.r2c3 = r2c3 * mutiplier;
|
|
|
|
inverted.r3c1 = r3c1 * mutiplier;
|
|
inverted.r3c2 = r3c2 * mutiplier;
|
|
inverted.r3c3 = r3c3 * mutiplier;
|
|
|
|
return true;
|
|
}
|
|
|
|
public static void Add(in Matrix3x3FP64 matrix1, in Matrix3x3FP64 matrix2, out Matrix3x3FP64 sum)
|
|
{
|
|
sum.r1c1 = matrix1.r1c1 + matrix2.r1c1;
|
|
sum.r1c2 = matrix1.r1c2 + matrix2.r1c2;
|
|
sum.r1c3 = matrix1.r1c3 + matrix2.r1c3;
|
|
|
|
sum.r2c1 = matrix1.r2c1 + matrix2.r2c1;
|
|
sum.r2c2 = matrix1.r2c2 + matrix2.r2c2;
|
|
sum.r2c3 = matrix1.r2c3 + matrix2.r2c3;
|
|
|
|
sum.r3c1 = matrix1.r3c1 + matrix2.r3c1;
|
|
sum.r3c2 = matrix1.r3c2 + matrix2.r3c2;
|
|
sum.r3c3 = matrix1.r3c3 + matrix2.r3c3;
|
|
}
|
|
|
|
public static void AddScaled(in Matrix3x3FP64 basicMatrix, in Matrix3x3FP64 scalableMatrix, double scale, out Matrix3x3FP64 sum)
|
|
{
|
|
sum.r1c1 = basicMatrix.r1c1 + scalableMatrix.r1c1 * scale;
|
|
sum.r1c2 = basicMatrix.r1c2 + scalableMatrix.r1c2 * scale;
|
|
sum.r1c3 = basicMatrix.r1c3 + scalableMatrix.r1c3 * scale;
|
|
|
|
sum.r2c1 = basicMatrix.r2c1 + scalableMatrix.r2c1 * scale;
|
|
sum.r2c2 = basicMatrix.r2c2 + scalableMatrix.r2c2 * scale;
|
|
sum.r2c3 = basicMatrix.r2c3 + scalableMatrix.r2c3 * scale;
|
|
|
|
sum.r3c1 = basicMatrix.r3c1 + scalableMatrix.r3c1 * scale;
|
|
sum.r3c2 = basicMatrix.r3c2 + scalableMatrix.r3c2 * scale;
|
|
sum.r3c3 = basicMatrix.r3c3 + scalableMatrix.r3c3 * scale;
|
|
}
|
|
|
|
public static void Subtract(in Matrix3x3FP64 minuend, in Matrix3x3FP64 subtrahend, out Matrix3x3FP64 difference)
|
|
{
|
|
difference.r1c1 = minuend.r1c1 - subtrahend.r1c1;
|
|
difference.r1c2 = minuend.r1c2 - subtrahend.r1c2;
|
|
difference.r1c3 = minuend.r1c3 - subtrahend.r1c3;
|
|
|
|
difference.r2c1 = minuend.r2c1 - subtrahend.r2c1;
|
|
difference.r2c2 = minuend.r2c2 - subtrahend.r2c2;
|
|
difference.r2c3 = minuend.r2c3 - subtrahend.r2c3;
|
|
|
|
difference.r3c1 = minuend.r3c1 - subtrahend.r3c1;
|
|
difference.r3c2 = minuend.r3c2 - subtrahend.r3c2;
|
|
difference.r3c3 = minuend.r3c3 - subtrahend.r3c3;
|
|
}
|
|
|
|
public static void Multiply(in Matrix3x3FP64 multiplicand, double multiplier, out Matrix3x3FP64 product)
|
|
{
|
|
product.r1c1 = multiplicand.r1c1 * multiplier;
|
|
product.r1c2 = multiplicand.r1c2 * multiplier;
|
|
product.r1c3 = multiplicand.r1c3 * multiplier;
|
|
|
|
product.r2c1 = multiplicand.r2c1 * multiplier;
|
|
product.r2c2 = multiplicand.r2c2 * multiplier;
|
|
product.r2c3 = multiplicand.r2c3 * multiplier;
|
|
|
|
product.r3c1 = multiplicand.r3c1 * multiplier;
|
|
product.r3c2 = multiplicand.r3c2 * multiplier;
|
|
product.r3c3 = multiplicand.r3c3 * multiplier;
|
|
}
|
|
|
|
public static void Divide(in Matrix3x3FP64 dividend, double divisor, out Matrix3x3FP64 quotient)
|
|
{
|
|
Multiply(dividend, 1.0 / divisor, out quotient);
|
|
}
|
|
|
|
public static void RightProduct(in Matrix3x3FP64 matrix, in Vector3FP64 vector, out Vector3FP64 result)
|
|
{
|
|
double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3;
|
|
double x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3;
|
|
double x3 = matrix.r3c1 * vector.x1 + matrix.r3c2 * vector.x2 + matrix.r3c3 * vector.x3;
|
|
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
result.x3 = x3;
|
|
}
|
|
|
|
public static void LeftProduct(in Vector3FP64 vector, in Matrix3x3FP64 matrix, out Vector3FP64 result)
|
|
{
|
|
double x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1;
|
|
double x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2;
|
|
double x3 = vector.x1 * matrix.r1c3 + vector.x2 * matrix.r2c3 + vector.x3 * matrix.r3c3;
|
|
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
result.x3 = x3;
|
|
}
|
|
|
|
public static void LoadZero(out Matrix3x3FP64 matrix)
|
|
{
|
|
matrix.r1c1 = 0.0;
|
|
matrix.r1c2 = 0.0;
|
|
matrix.r1c3 = 0.0;
|
|
|
|
matrix.r2c1 = 0.0;
|
|
matrix.r2c2 = 0.0;
|
|
matrix.r2c3 = 0.0;
|
|
|
|
matrix.r3c1 = 0.0;
|
|
matrix.r3c2 = 0.0;
|
|
matrix.r3c3 = 0.0;
|
|
}
|
|
|
|
public static void LoadIdentity(out Matrix3x3FP64 matrix)
|
|
{
|
|
matrix.r1c1 = 1.0;
|
|
matrix.r1c2 = 0.0;
|
|
matrix.r1c3 = 0.0;
|
|
|
|
matrix.r2c1 = 0.0;
|
|
matrix.r2c2 = 1.0;
|
|
matrix.r2c3 = 0.0;
|
|
|
|
matrix.r3c1 = 0.0;
|
|
matrix.r3c2 = 0.0;
|
|
matrix.r3c3 = 1.0;
|
|
}
|
|
|
|
public static void LoadDiagonal(double d1, double d2, double d3, out Matrix3x3FP64 matrix)
|
|
{
|
|
matrix.r1c1 = d1;
|
|
matrix.r1c2 = 0.0;
|
|
matrix.r1c3 = 0.0;
|
|
|
|
matrix.r2c1 = 0.0;
|
|
matrix.r2c2 = d2;
|
|
matrix.r2c3 = 0.0;
|
|
|
|
matrix.r3c1 = 0.0;
|
|
matrix.r3c2 = 0.0;
|
|
matrix.r3c3 = d3;
|
|
}
|
|
}
|
|
}
|