protected Transform Multiply(Transform t1, Transform t2)
{
Matrix4x4 m1 = Multiply(t1.m, t2.m);
Matrix4x4 m2 = Multiply(t1.mInv, t2.mInv);
return new Transform(m1, m2);
}
/// <summary>
/// Transpose 值传递
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
protected Matrix4x4 Transpose(Matrix4x4 m) {
return new Matrix4x4(m.m[0,0], m.m[1,0], m.m[2,0], m.m[3,0],
m.m[0,1], m.m[1,1], m.m[2,1], m.m[3,1],
m.m[0,2], m.m[1,2], m.m[2,2], m.m[3,2],
m.m[0,3], m.m[1,3], m.m[2,3], m.m[3,3]);
}
/// <summary>
/// Inverse
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
protected Matrix4x4 Inverse( Matrix4x4 m)
{
int[] indxc = new int[4];
int[] indxr = new int[4];
int[] ipiv = new int[4];
for (int i = 0; i < 4; i++)
ipiv[i] = 0;
float[,] minv = new float[4, 4];
for (int i = 0; i < 4; i++)//使用值传递
for (int j = 0; j < 4; j++)
minv[i, j] = m.m[i, j];
for (int i = 0; i< 4; i++)
{
int irow = -1, icol = -1;
float big = 0.0f;
// Choose pivot
for (int j = 0; j< 4; j++)
{
if (ipiv[j] != 1)
{
for (int k = 0; k< 4; k++)
{
if (ipiv[k] == 0)
{
if ((float)Math.Abs(minv[j, k]) >= big)
{
big = ((float)Math.Abs(minv[j, k]));
irow = j;
icol = k;
}
}
else if (ipiv[k] > 1)
return null;
// Error("Singular matrix in MatrixInvert");
}
}
}
++ipiv[icol];
// Swap rows _irow_ and _icol_ for pivot
if (irow != icol)
{
for (int k = 0; k< 4; ++k)
Swap(minv[irow,k], minv[icol,k]);
}
indxr[i] = irow;
indxc[i] = icol;
if (minv[icol, icol] == 0.0f)
return null;
//Error("Singular matrix in MatrixInvert");
// Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
float pivinv = 1.0f / minv[icol,icol];
minv[icol,icol] = 1.0f;
for (int j = 0; j< 4; j++)
minv[icol,j] *= pivinv;
// Subtract this row from others to zero out their columns
for (int j = 0; j< 4; j++)
{
if (j != icol)
{
float save = minv[j,icol];
minv[j,icol] = 0;
for (int k = 0; k< 4; k++)
minv[j,k] -= minv[icol,k]* save;
}
}
}
// Swap columns to reflect permutation
for (int j = 3; j >= 0; j--)
{
if (indxr[j] != indxc[j])
{
for (int k = 0; k< 4; k++)
Swap(minv[k,indxr[j]], minv[k,indxc[j]]);
}
}
return new Matrix4x4(minv);
}
protected Matrix4x4 Multiply(Matrix4x4 m1, Matrix4x4 m2,Matrix4x4 m3)
{
return Multiply(Multiply(m1,m2),m3);
}
