public void Transpose()
{
Matrix33D m = new Matrix33D(rowMajor, MatrixOrder.RowMajor);
m.Transpose();
Matrix33D mt = new Matrix33D(rowMajor, MatrixOrder.ColumnMajor);
Assert.AreEqual(mt, m);
Matrix33D i = Matrix33D.Identity;
i.Transpose();
Assert.AreEqual(Matrix33D.Identity, i);
}
/// <summary>
/// Inverts the pose.
/// </summary>
public void Invert()
{
// Let
// R = Rotation as matrix
// t = translation as vector
// v = any vector
// v' = v transformed by the current pose
// Pose(v) = current pose
// Pose'(v) = inverse of current pose
//
// Pose(v) = Rv + t
// v' = Rv + t
// v' - t = Rv
// R'(v' - t) = v
// v = R'v' + R'(-t)
// => Pose'(v) = R'v + R'(-t)
// Calculate the inverse of the orientation.
// (The inverse of an orthogonal is the same as the transposed matrix.)
Orientation.Transpose(); // R'
// Calculate the new translation
Position = Orientation * -Position; // R'(-t)
}
public void Transpose()
{
Matrix33D m = new Matrix33D(rowMajor, MatrixOrder.RowMajor);
m.Transpose();
Matrix33D mt = new Matrix33D(rowMajor, MatrixOrder.ColumnMajor);
Assert.AreEqual(mt, m);
Matrix33D i = Matrix33D.Identity;
i.Transpose();
Assert.AreEqual(Matrix33D.Identity, i);
}