public void SetRow()
{
Matrix3x3f m = new Matrix3x3f();
m.SetRow(0, new Vector3f(0, 3, 6));
m.SetRow(1, new Vector3f(1, 4, 7));
m.SetRow(2, new Vector3f(2, 5, 8));
Assert.AreEqual(new Vector3f(0, 3, 6), m.GetRow(0));
Assert.AreEqual(new Vector3f(1, 4, 7), m.GetRow(1));
Assert.AreEqual(new Vector3f(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(Matrix3x3f M, float tolerance, out Matrix3x3f R)
{
Matrix3x3f Mt = M.Transpose;
float Mone = OneNorm(M);
float Minf = InfNorm(M);
float Eone;
Matrix3x3f MadjTt = new Matrix3x3f();
Matrix3x3f Et = new Matrix3x3f();
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)));
float det = Mt.m00 * MadjTt.m00 + Mt.m01 * MadjTt.m01 + Mt.m02 * MadjTt.m02;
if (Math.Abs(det) < 1.0e-12f)
{
int index = int.MaxValue;
for (int i = 0; i < 3; i++)
{
float len = MadjTt.GetRow(i).SqrMagnitude;
if (len > 1.0e-12f)
{
// 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 = Matrix3x3f.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)));
Matrix3x3f M2 = Mt.Transpose;
Mone = OneNorm(M2);
Minf = InfNorm(M2);
det = Mt.m00 * MadjTt.m00 + Mt.m01 * MadjTt.m01 + Mt.m02 * MadjTt.m02;
}
}
float MadjTone = OneNorm(MadjTt);
float MadjTinf = InfNorm(MadjTt);
float gamma = (float)Math.Sqrt(Math.Sqrt((MadjTone * MadjTinf) / (Mone * Minf)) / Math.Abs(det));
float g1 = gamma * 0.5f;
float g2 = 0.5f / (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
}
