private HomogeneousTM InvertHomogenousMatrix(HomogeneousTM hm)
{
Matrix rotMatrix = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
rotMatrix[i, j] = hm.hMat[i, j];
}
}
Matrix traMatrix = new Matrix(3, 1);
for (int i = 0; i < 3; i++)
{
traMatrix[i, 0] = hm.hMat[i, 3];
}
Matrix newRotMat = rotMatrix.Transpose;
Matrix newTraMat = (-1)*rotMatrix.Transpose*traMatrix;
return new HomogeneousTM(newRotMat, newTraMat );
}
/// <summary>
/// Clone an Homogeneous Transformation Matrix.
/// </summary>
/// <param name="hm"></param>
public HomogeneousTM(HomogeneousTM hm)
{
hMat = new Matrix(hm.hMat);
}
public static HomogeneousTM operator *(HomogeneousTM hm1, HomogeneousTM hm2)
{
HomogeneousTM newHM = new HomogeneousTM(0, 0, 0, 0);
newHM.hMat = hm1.hMat * hm1.hMat;
return newHM;
}