public void GetRow()
{
Matrix3x3d m = Indexed3x3();
Assert.AreEqual(new Vector3d(0, 3, 6), m.GetRow(0));
Assert.AreEqual(new Vector3d(1, 4, 7), m.GetRow(1));
Assert.AreEqual(new Vector3d(2, 5, 8), m.GetRow(2));
}
public void SetRow()
{
Matrix3x3d m = new Matrix3x3d();
m.SetRow(0, new Vector3d(0, 3, 6));
m.SetRow(1, new Vector3d(1, 4, 7));
m.SetRow(2, new Vector3d(2, 5, 8));
Assert.AreEqual(new Vector3d(0, 3, 6), m.GetRow(0));
Assert.AreEqual(new Vector3d(1, 4, 7), m.GetRow(1));
Assert.AreEqual(new Vector3d(2, 5, 8), m.GetRow(2));
}
/// <summary>
/// Perform a polar decomposition of matrix M and return the rotation matrix R. This method handles the degenerated cases.
/// </summary>am>
public static void PolarDecompositionStable(Matrix3x3d M, double tolerance, out Matrix3x3d R)
{
Matrix3x3d Mt = M.Transpose;
double Mone = OneNorm(M);
double Minf = InfNorm(M);
double Eone;
Matrix3x3d MadjTt = new Matrix3x3d();
Matrix3x3d Et = new Matrix3x3d();
const double eps = 1.0e-15;
do
{
MadjTt.SetRow(0, Mt.GetRow(1).Cross(Mt.GetRow(2)));
MadjTt.SetRow(1, Mt.GetRow(2).Cross(Mt.GetRow(0)));
MadjTt.SetRow(2, Mt.GetRow(0).Cross(Mt.GetRow(1)));
double det = Mt.m00 * MadjTt.m00 + Mt.m01 * MadjTt.m01 + Mt.m02 * MadjTt.m02;
if (Math.Abs(det) < eps)
{
int index = int.MaxValue;
for (int i = 0; i < 3; i++)
{
double len = MadjTt.GetRow(i).SqrMagnitude;
if (len > eps)
{
// index of valid cross product
// => is also the index of the vector in Mt that must be exchanged
index = i;
break;
}
}
if (index == int.MaxValue)
{
R = Matrix3x3d.Identity;
return;
}
else
{
Mt.SetRow(index, Mt.GetRow((index + 1) % 3).Cross(Mt.GetRow((index + 2) % 3)));
MadjTt.SetRow((index + 1) % 3, Mt.GetRow((index + 2) % 3).Cross(Mt.GetRow(index)));
MadjTt.SetRow((index + 2) % 3, Mt.GetRow(index).Cross(Mt.GetRow((index + 1) % 3)));
Matrix3x3d M2 = Mt.Transpose;
Mone = OneNorm(M2);
Minf = InfNorm(M2);
det = Mt.m00 * MadjTt.m00 + Mt.m01 * MadjTt.m01 + Mt.m02 * MadjTt.m02;
}
}
double MadjTone = OneNorm(MadjTt);
double MadjTinf = InfNorm(MadjTt);
double gamma = Math.Sqrt(Math.Sqrt((MadjTone * MadjTinf) / (Mone * Minf)) / Math.Abs(det));
double g1 = gamma * 0.5;
double g2 = 0.5 / (gamma * det);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
Et[i, j] = Mt[i, j];
Mt[i, j] = g1 * Mt[i, j] + g2 * MadjTt[i, j];
Et[i, j] -= Mt[i, j];
}
}
Eone = OneNorm(Et);
Mone = OneNorm(Mt);
Minf = InfNorm(Mt);
}while (Eone > Mone * tolerance);
// Q = Mt^T
R = Mt.Transpose;
//end of function
}
