// 用角度和旋转轴构造矩阵
public static Matrix33 AngleAxis(float angle, Vector3 axis)
{
float rad = angle * (float)Math.PI / 180f;
float S = (float)Math.Sin(rad);
float C = (float)Math.Cos(rad);
float C1 = 1 - C;
float x = axis.x;
float y = axis.y;
float z = axis.z;
Matrix33 matrix = new Matrix33();
matrix[0, 0] = x * x * C1 + C;
matrix[0, 1] = x * y * C1 + z * S;
matrix[0, 2] = x * z * C1 - y * S;
matrix[1, 0] = x * y * C1 - z * S;
matrix[1, 1] = y * y * C1 + C;
matrix[1, 2] = y * z * C1 + x * S;
matrix[2, 0] = x * z * C1 + y * S;
matrix[2, 1] = y * z * C1 - x * S;
matrix[2, 2] = z * z * C1 + C;
return(matrix);
}
// 获取逆矩阵
public Matrix33 Inverse()
{
float det = this.Determinant();
Matrix33 adj = this._GetAdj();
return(adj * (1f / det));
}
// 获取代数余子式
public float _GetRemianValue(int trimRow, int trimCol)
{
int remainNumber = mNumber - 1;
// 余子式
Matrix33 remain = new Matrix33();
int index = 0;
for (int row = 0; row < mNumber; row++)
{
for (int col = 0; col < mNumber; col++)
{
if (row != trimRow && col != trimCol)
{
int rowIndex = index / remainNumber;
int colIndex = index % remainNumber;
++index;
remain[rowIndex, colIndex] = this[row, col];
}
}
}
// 代数余子式
float remainValue = remain.Determinant() * (float)Math.Pow(-1, trimRow + trimCol);
return(remainValue);
}
public static Matrix33 operator *(Matrix33 lhs, float k)
{
Matrix33 matrix = new Matrix33();
for (int row = 0; row < mNumber; row++)
{
for (int col = 0; col < mNumber; col++)
{
matrix[row, col] = lhs[row, col] * k;
}
}
return(matrix);
}
// 获取伴随矩阵
public Matrix33 _GetAdj()
{
Matrix33 matrix = new Matrix33();
for (int i = 0; i < mNumber; i++)
{
for (int j = 0; j < mNumber; j++)
{
matrix[j, i] = this._GetRemianValue(i, j);
}
}
return(matrix);
}
public static Matrix33 operator *(Matrix33 m1, Matrix33 m2)
{
Matrix33 matrix = new Matrix33();
for (int row = 0; row < mNumber; row++)
{
for (int col = 0; col < mNumber; col++)
{
float value = 0;
for (int i = 0; i < mNumber; i++)
{
value = value + m1[row, i] * m2[i, col];
}
matrix[row, col] = value;
}
}
return(matrix);
}
