425 lines
11 KiB
C#
425 lines
11 KiB
C#
using System;
|
|
|
|
namespace AForge.Math;
|
|
|
|
[Serializable]
|
|
public struct Matrix3x3
|
|
{
|
|
public float V00;
|
|
|
|
public float V01;
|
|
|
|
public float V02;
|
|
|
|
public float V10;
|
|
|
|
public float V11;
|
|
|
|
public float V12;
|
|
|
|
public float V20;
|
|
|
|
public float V21;
|
|
|
|
public float V22;
|
|
|
|
public static Matrix3x3 Identity
|
|
{
|
|
get
|
|
{
|
|
Matrix3x3 result = default(Matrix3x3);
|
|
result.V00 = (result.V11 = (result.V22 = 1f));
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public float Determinant => V00 * V11 * V22 + V01 * V12 * V20 + V02 * V10 * V21 - V00 * V12 * V21 - V01 * V10 * V22 - V02 * V11 * V20;
|
|
|
|
public float[] ToArray()
|
|
{
|
|
return new float[9] { V00, V01, V02, V10, V11, V12, V20, V21, V22 };
|
|
}
|
|
|
|
public static Matrix3x3 CreateRotationY(float radians)
|
|
{
|
|
Matrix3x3 result = default(Matrix3x3);
|
|
float v = (float)System.Math.Cos(radians);
|
|
float num = (float)System.Math.Sin(radians);
|
|
result.V00 = (result.V22 = v);
|
|
result.V02 = num;
|
|
result.V20 = 0f - num;
|
|
result.V11 = 1f;
|
|
return result;
|
|
}
|
|
|
|
public static Matrix3x3 CreateRotationX(float radians)
|
|
{
|
|
Matrix3x3 result = default(Matrix3x3);
|
|
float v = (float)System.Math.Cos(radians);
|
|
float num = (float)System.Math.Sin(radians);
|
|
result.V11 = (result.V22 = v);
|
|
result.V12 = 0f - num;
|
|
result.V21 = num;
|
|
result.V00 = 1f;
|
|
return result;
|
|
}
|
|
|
|
public static Matrix3x3 CreateRotationZ(float radians)
|
|
{
|
|
Matrix3x3 result = default(Matrix3x3);
|
|
float v = (float)System.Math.Cos(radians);
|
|
float num = (float)System.Math.Sin(radians);
|
|
result.V00 = (result.V11 = v);
|
|
result.V01 = 0f - num;
|
|
result.V10 = num;
|
|
result.V22 = 1f;
|
|
return result;
|
|
}
|
|
|
|
public static Matrix3x3 CreateFromYawPitchRoll(float yaw, float pitch, float roll)
|
|
{
|
|
return CreateRotationY(yaw) * CreateRotationX(pitch) * CreateRotationZ(roll);
|
|
}
|
|
|
|
public void ExtractYawPitchRoll(out float yaw, out float pitch, out float roll)
|
|
{
|
|
yaw = (float)System.Math.Atan2(V02, V22);
|
|
pitch = (float)System.Math.Asin(0f - V12);
|
|
roll = (float)System.Math.Atan2(V10, V11);
|
|
}
|
|
|
|
public static Matrix3x3 CreateFromRows(Vector3 row0, Vector3 row1, Vector3 row2)
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = row0.X,
|
|
V01 = row0.Y,
|
|
V02 = row0.Z,
|
|
V10 = row1.X,
|
|
V11 = row1.Y,
|
|
V12 = row1.Z,
|
|
V20 = row2.X,
|
|
V21 = row2.Y,
|
|
V22 = row2.Z
|
|
};
|
|
}
|
|
|
|
public static Matrix3x3 CreateFromColumns(Vector3 column0, Vector3 column1, Vector3 column2)
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = column0.X,
|
|
V10 = column0.Y,
|
|
V20 = column0.Z,
|
|
V01 = column1.X,
|
|
V11 = column1.Y,
|
|
V21 = column1.Z,
|
|
V02 = column2.X,
|
|
V12 = column2.Y,
|
|
V22 = column2.Z
|
|
};
|
|
}
|
|
|
|
public static Matrix3x3 CreateDiagonal(Vector3 vector)
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = vector.X,
|
|
V11 = vector.Y,
|
|
V22 = vector.Z
|
|
};
|
|
}
|
|
|
|
public Vector3 GetRow(int index)
|
|
{
|
|
return index switch
|
|
{
|
|
2 => new Vector3(V20, V21, V22),
|
|
1 => new Vector3(V10, V11, V12),
|
|
0 => new Vector3(V00, V01, V02),
|
|
_ => throw new ArgumentException("Invalid row index was specified.", "index"),
|
|
};
|
|
}
|
|
|
|
public Vector3 GetColumn(int index)
|
|
{
|
|
return index switch
|
|
{
|
|
2 => new Vector3(V02, V12, V22),
|
|
1 => new Vector3(V01, V11, V21),
|
|
0 => new Vector3(V00, V10, V20),
|
|
_ => throw new ArgumentException("Invalid column index was specified.", "index"),
|
|
};
|
|
}
|
|
|
|
public static Matrix3x3 operator *(Matrix3x3 matrix1, Matrix3x3 matrix2)
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = matrix1.V00 * matrix2.V00 + matrix1.V01 * matrix2.V10 + matrix1.V02 * matrix2.V20,
|
|
V01 = matrix1.V00 * matrix2.V01 + matrix1.V01 * matrix2.V11 + matrix1.V02 * matrix2.V21,
|
|
V02 = matrix1.V00 * matrix2.V02 + matrix1.V01 * matrix2.V12 + matrix1.V02 * matrix2.V22,
|
|
V10 = matrix1.V10 * matrix2.V00 + matrix1.V11 * matrix2.V10 + matrix1.V12 * matrix2.V20,
|
|
V11 = matrix1.V10 * matrix2.V01 + matrix1.V11 * matrix2.V11 + matrix1.V12 * matrix2.V21,
|
|
V12 = matrix1.V10 * matrix2.V02 + matrix1.V11 * matrix2.V12 + matrix1.V12 * matrix2.V22,
|
|
V20 = matrix1.V20 * matrix2.V00 + matrix1.V21 * matrix2.V10 + matrix1.V22 * matrix2.V20,
|
|
V21 = matrix1.V20 * matrix2.V01 + matrix1.V21 * matrix2.V11 + matrix1.V22 * matrix2.V21,
|
|
V22 = matrix1.V20 * matrix2.V02 + matrix1.V21 * matrix2.V12 + matrix1.V22 * matrix2.V22
|
|
};
|
|
}
|
|
|
|
public static Matrix3x3 Multiply(Matrix3x3 matrix1, Matrix3x3 matrix2)
|
|
{
|
|
return matrix1 * matrix2;
|
|
}
|
|
|
|
public static Matrix3x3 operator +(Matrix3x3 matrix1, Matrix3x3 matrix2)
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = matrix1.V00 + matrix2.V00,
|
|
V01 = matrix1.V01 + matrix2.V01,
|
|
V02 = matrix1.V02 + matrix2.V02,
|
|
V10 = matrix1.V10 + matrix2.V10,
|
|
V11 = matrix1.V11 + matrix2.V11,
|
|
V12 = matrix1.V12 + matrix2.V12,
|
|
V20 = matrix1.V20 + matrix2.V20,
|
|
V21 = matrix1.V21 + matrix2.V21,
|
|
V22 = matrix1.V22 + matrix2.V22
|
|
};
|
|
}
|
|
|
|
public static Matrix3x3 Add(Matrix3x3 matrix1, Matrix3x3 matrix2)
|
|
{
|
|
return matrix1 + matrix2;
|
|
}
|
|
|
|
public static Matrix3x3 operator -(Matrix3x3 matrix1, Matrix3x3 matrix2)
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = matrix1.V00 - matrix2.V00,
|
|
V01 = matrix1.V01 - matrix2.V01,
|
|
V02 = matrix1.V02 - matrix2.V02,
|
|
V10 = matrix1.V10 - matrix2.V10,
|
|
V11 = matrix1.V11 - matrix2.V11,
|
|
V12 = matrix1.V12 - matrix2.V12,
|
|
V20 = matrix1.V20 - matrix2.V20,
|
|
V21 = matrix1.V21 - matrix2.V21,
|
|
V22 = matrix1.V22 - matrix2.V22
|
|
};
|
|
}
|
|
|
|
public static Matrix3x3 Subtract(Matrix3x3 matrix1, Matrix3x3 matrix2)
|
|
{
|
|
return matrix1 - matrix2;
|
|
}
|
|
|
|
public static Vector3 operator *(Matrix3x3 matrix, Vector3 vector)
|
|
{
|
|
return new Vector3(matrix.V00 * vector.X + matrix.V01 * vector.Y + matrix.V02 * vector.Z, matrix.V10 * vector.X + matrix.V11 * vector.Y + matrix.V12 * vector.Z, matrix.V20 * vector.X + matrix.V21 * vector.Y + matrix.V22 * vector.Z);
|
|
}
|
|
|
|
public static Vector3 Multiply(Matrix3x3 matrix, Vector3 vector)
|
|
{
|
|
return matrix * vector;
|
|
}
|
|
|
|
public static Matrix3x3 operator *(Matrix3x3 matrix, float factor)
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = matrix.V00 * factor,
|
|
V01 = matrix.V01 * factor,
|
|
V02 = matrix.V02 * factor,
|
|
V10 = matrix.V10 * factor,
|
|
V11 = matrix.V11 * factor,
|
|
V12 = matrix.V12 * factor,
|
|
V20 = matrix.V20 * factor,
|
|
V21 = matrix.V21 * factor,
|
|
V22 = matrix.V22 * factor
|
|
};
|
|
}
|
|
|
|
public static Matrix3x3 Multiply(Matrix3x3 matrix, float factor)
|
|
{
|
|
return matrix * factor;
|
|
}
|
|
|
|
public static Matrix3x3 operator +(Matrix3x3 matrix, float value)
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = matrix.V00 + value,
|
|
V01 = matrix.V01 + value,
|
|
V02 = matrix.V02 + value,
|
|
V10 = matrix.V10 + value,
|
|
V11 = matrix.V11 + value,
|
|
V12 = matrix.V12 + value,
|
|
V20 = matrix.V20 + value,
|
|
V21 = matrix.V21 + value,
|
|
V22 = matrix.V22 + value
|
|
};
|
|
}
|
|
|
|
public static Matrix3x3 Add(Matrix3x3 matrix, float value)
|
|
{
|
|
return matrix + value;
|
|
}
|
|
|
|
public static bool operator ==(Matrix3x3 matrix1, Matrix3x3 matrix2)
|
|
{
|
|
if (matrix1.V00 == matrix2.V00 && matrix1.V01 == matrix2.V01 && matrix1.V02 == matrix2.V02 && matrix1.V10 == matrix2.V10 && matrix1.V11 == matrix2.V11 && matrix1.V12 == matrix2.V12 && matrix1.V20 == matrix2.V20 && matrix1.V21 == matrix2.V21)
|
|
{
|
|
return matrix1.V22 == matrix2.V22;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public static bool operator !=(Matrix3x3 matrix1, Matrix3x3 matrix2)
|
|
{
|
|
if (matrix1.V00 == matrix2.V00 && matrix1.V01 == matrix2.V01 && matrix1.V02 == matrix2.V02 && matrix1.V10 == matrix2.V10 && matrix1.V11 == matrix2.V11 && matrix1.V12 == matrix2.V12 && matrix1.V20 == matrix2.V20 && matrix1.V21 == matrix2.V21)
|
|
{
|
|
return matrix1.V22 != matrix2.V22;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
public bool Equals(Matrix3x3 matrix)
|
|
{
|
|
return this == matrix;
|
|
}
|
|
|
|
public override bool Equals(object obj)
|
|
{
|
|
if (obj is Matrix3x3)
|
|
{
|
|
return Equals((Matrix3x3)obj);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public override int GetHashCode()
|
|
{
|
|
return V00.GetHashCode() + V01.GetHashCode() + V02.GetHashCode() + V10.GetHashCode() + V11.GetHashCode() + V12.GetHashCode() + V20.GetHashCode() + V21.GetHashCode() + V22.GetHashCode();
|
|
}
|
|
|
|
public Matrix3x3 Transpose()
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = V00,
|
|
V01 = V10,
|
|
V02 = V20,
|
|
V10 = V01,
|
|
V11 = V11,
|
|
V12 = V21,
|
|
V20 = V02,
|
|
V21 = V12,
|
|
V22 = V22
|
|
};
|
|
}
|
|
|
|
public Matrix3x3 MultiplySelfByTranspose()
|
|
{
|
|
Matrix3x3 result = default(Matrix3x3);
|
|
result.V00 = V00 * V00 + V01 * V01 + V02 * V02;
|
|
result.V10 = (result.V01 = V00 * V10 + V01 * V11 + V02 * V12);
|
|
result.V20 = (result.V02 = V00 * V20 + V01 * V21 + V02 * V22);
|
|
result.V11 = V10 * V10 + V11 * V11 + V12 * V12;
|
|
result.V21 = (result.V12 = V10 * V20 + V11 * V21 + V12 * V22);
|
|
result.V22 = V20 * V20 + V21 * V21 + V22 * V22;
|
|
return result;
|
|
}
|
|
|
|
public Matrix3x3 MultiplyTransposeBySelf()
|
|
{
|
|
Matrix3x3 result = default(Matrix3x3);
|
|
result.V00 = V00 * V00 + V10 * V10 + V20 * V20;
|
|
result.V10 = (result.V01 = V00 * V01 + V10 * V11 + V20 * V21);
|
|
result.V20 = (result.V02 = V00 * V02 + V10 * V12 + V20 * V22);
|
|
result.V11 = V01 * V01 + V11 * V11 + V21 * V21;
|
|
result.V21 = (result.V12 = V01 * V02 + V11 * V12 + V21 * V22);
|
|
result.V22 = V02 * V02 + V12 * V12 + V22 * V22;
|
|
return result;
|
|
}
|
|
|
|
public Matrix3x3 Adjugate()
|
|
{
|
|
return new Matrix3x3
|
|
{
|
|
V00 = V11 * V22 - V12 * V21,
|
|
V01 = 0f - (V01 * V22 - V02 * V21),
|
|
V02 = V01 * V12 - V02 * V11,
|
|
V10 = 0f - (V10 * V22 - V12 * V20),
|
|
V11 = V00 * V22 - V02 * V20,
|
|
V12 = 0f - (V00 * V12 - V02 * V10),
|
|
V20 = V10 * V21 - V11 * V20,
|
|
V21 = 0f - (V00 * V21 - V01 * V20),
|
|
V22 = V00 * V11 - V01 * V10
|
|
};
|
|
}
|
|
|
|
public Matrix3x3 Inverse()
|
|
{
|
|
float determinant = Determinant;
|
|
if (determinant == 0f)
|
|
{
|
|
throw new ArgumentException("Cannot calculate inverse of the matrix since it is singular.");
|
|
}
|
|
float num = 1f / determinant;
|
|
Matrix3x3 result = Adjugate();
|
|
result.V00 *= num;
|
|
result.V01 *= num;
|
|
result.V02 *= num;
|
|
result.V10 *= num;
|
|
result.V11 *= num;
|
|
result.V12 *= num;
|
|
result.V20 *= num;
|
|
result.V21 *= num;
|
|
result.V22 *= num;
|
|
return result;
|
|
}
|
|
|
|
public Matrix3x3 PseudoInverse()
|
|
{
|
|
SVD(out var u, out var e, out var v);
|
|
return v * CreateDiagonal(e.Inverse()) * u.Transpose();
|
|
}
|
|
|
|
public void SVD(out Matrix3x3 u, out Vector3 e, out Matrix3x3 v)
|
|
{
|
|
double[,] array = new double[3, 3]
|
|
{
|
|
{ V00, V01, V02 },
|
|
{ V10, V11, V12 },
|
|
{ V20, V21, V22 }
|
|
};
|
|
svd.svdcmp(array, out var w, out var v2);
|
|
u = default(Matrix3x3);
|
|
u.V00 = (float)array[0, 0];
|
|
u.V01 = (float)array[0, 1];
|
|
u.V02 = (float)array[0, 2];
|
|
u.V10 = (float)array[1, 0];
|
|
u.V11 = (float)array[1, 1];
|
|
u.V12 = (float)array[1, 2];
|
|
u.V20 = (float)array[2, 0];
|
|
u.V21 = (float)array[2, 1];
|
|
u.V22 = (float)array[2, 2];
|
|
v = default(Matrix3x3);
|
|
v.V00 = (float)v2[0, 0];
|
|
v.V01 = (float)v2[0, 1];
|
|
v.V02 = (float)v2[0, 2];
|
|
v.V10 = (float)v2[1, 0];
|
|
v.V11 = (float)v2[1, 1];
|
|
v.V12 = (float)v2[1, 2];
|
|
v.V20 = (float)v2[2, 0];
|
|
v.V21 = (float)v2[2, 1];
|
|
v.V22 = (float)v2[2, 2];
|
|
e = default(Vector3);
|
|
e.X = (float)w[0];
|
|
e.Y = (float)w[1];
|
|
e.Z = (float)w[2];
|
|
}
|
|
}
|