这就是我使用的,它的工作速度出奇的快。
public struct Matrix3
{
public readonly double a11, a12, a13;
public readonly double a21, a22, a23;
public readonly double a31, a32, a33;
...
public vec3 Multiply(vec3 rhs)
{
// y= A*x
// fill vector by element
return new vec3(
(a11*rhs.X+a12*rhs.Y+a13*rhs.Z),
(a21*rhs.X+a22*rhs.Y+a23*rhs.Z),
(a31*rhs.X+a32*rhs.Y+a33*rhs.Z));
}
public mat3 Multiply(mat3 rhs)
{
// Y = A*X
// fill matrix by row
return new mat3(
(a11*rhs.a11+a12*rhs.a21+a13*rhs.a31),
(a11*rhs.a12+a12*rhs.a22+a13*rhs.a32),
(a11*rhs.a13+a12*rhs.a23+a13*rhs.a33),
(a21*rhs.a11+a22*rhs.a21+a23*rhs.a31),
(a21*rhs.a12+a22*rhs.a22+a23*rhs.a32),
(a21*rhs.a13+a22*rhs.a23+a23*rhs.a33),
(a31*rhs.a11+a32*rhs.a21+a33*rhs.a31),
(a31*rhs.a12+a32*rhs.a22+a33*rhs.a32),
(a31*rhs.a13+a32*rhs.a23+a33*rhs.a33));
}
}
wherevec3
和mat3
是我自己的别名Vector3
,Matrix3
存储元素的结构是字段。同样适用于 4 元素结构。我也把它编码成这样的倒数:
public double Determinant()
{
return a11*(a22*a33-a23*a32)
+a12*(a23*a31-a21*a33)
+a13*(a21*a32-a22*a31);
}
/// <summary>
/// Solves the system of equations this*x=rhs for x
/// </summary>
public vec3 Solve(vec3 rhs)
{
double D=Determinant();
double ID=1/D;
return new vec3(
(((a22*a33-a23*a32)*rhs.X+(a13*a32-a12*a33)*rhs.Y+(a12*a23-a13*a22)*rhs.Z)*ID),
-(((a21*a33-a23*a31)*rhs.X+(a13*a31-a11*a33)*rhs.Y+(a11*a23-a13*a21)*rhs.Z)*ID),
(((a21*a32-a22*a31)*rhs.X+(a12*a31-a11*a32)*rhs.Y+(a11*a22-a12*a21)*rhs.Z)*ID));
}
/// <summary>
/// Solves the system of equations this*X = rhs for X
/// </summary>
public mat3 Solve(mat3 rhs)
{
double D=Determinant();
double ID=1/D;
return new mat3(
(((a22*a33-a23*a32)*rhs.a11+(a13*a32-a12*a33)*rhs.a21+(a12*a23-a13*a22)*rhs.a31)*ID),
(((a22*a33-a23*a32)*rhs.a12+(a13*a32-a12*a33)*rhs.a22+(a12*a23-a13*a22)*rhs.a32)*ID),
(((a22*a33-a23*a32)*rhs.a13+(a13*a32-a12*a33)*rhs.a23+(a12*a23-a13*a22)*rhs.a33)*ID),
-(((a21*a33-a23*a31)*rhs.a11+(a13*a31-a11*a33)*rhs.a21+(a11*a23-a13*a21)*rhs.a31)*ID),
-(((a21*a33-a23*a31)*rhs.a12+(a13*a31-a11*a33)*rhs.a22+(a11*a23-a13*a21)*rhs.a32)*ID),
-(((a21*a33-a23*a31)*rhs.a13+(a13*a31-a11*a33)*rhs.a23+(a11*a23-a13*a21)*rhs.a33)*ID),
(((a21*a32-a22*a31)*rhs.a11+(a12*a31-a11*a32)*rhs.a21+(a11*a22-a12*a21)*rhs.a31)*ID),
(((a21*a32-a22*a31)*rhs.a12+(a12*a31-a11*a32)*rhs.a22+(a11*a22-a12*a21)*rhs.a32)*ID),
(((a21*a32-a22*a31)*rhs.a13+(a12*a31-a11*a32)*rhs.a23+(a11*a22-a12*a21)*rhs.a33)*ID));
}