public void SetColumn()
{
Matrix3x3f m = new Matrix3x3f();
m.SetColumn(0, new Vector3f(0, 1, 2));
m.SetColumn(1, new Vector3f(3, 4, 5));
m.SetColumn(2, new Vector3f(6, 7, 8));
Assert.AreEqual(new Vector3f(0, 1, 2), m.GetColumn(0));
Assert.AreEqual(new Vector3f(3, 4, 5), m.GetColumn(1));
Assert.AreEqual(new Vector3f(6, 7, 8), m.GetColumn(2));
}
/// <summary>
/// Perform polar decomposition A = (U D U^T) R
/// </summary>
public static void PolarDecomposition(Matrix3x3f A, out Matrix3x3f R, out Matrix3x3f U, out Matrix3x3f D)
{
// A = SR, where S is symmetric and R is orthonormal
// -> S = (A A^T)^(1/2)
// A = U D U^T R
Matrix3x3f AAT = new Matrix3x3f();
AAT.m00 = A.m00 * A.m00 + A.m01 * A.m01 + A.m02 * A.m02;
AAT.m11 = A.m10 * A.m10 + A.m11 * A.m11 + A.m12 * A.m12;
AAT.m22 = A.m20 * A.m20 + A.m21 * A.m21 + A.m22 * A.m22;
AAT.m01 = A.m00 * A.m10 + A.m01 * A.m11 + A.m02 * A.m12;
AAT.m02 = A.m00 * A.m20 + A.m01 * A.m21 + A.m02 * A.m22;
AAT.m12 = A.m10 * A.m20 + A.m11 * A.m21 + A.m12 * A.m22;
AAT.m10 = AAT.m01;
AAT.m20 = AAT.m02;
AAT.m21 = AAT.m12;
R = Matrix3x3f.Identity;
Vector3f eigenVals;
EigenDecomposition(AAT, out U, out eigenVals);
float d0 = (float)Math.Sqrt(eigenVals.x);
float d1 = (float)Math.Sqrt(eigenVals.y);
float d2 = (float)Math.Sqrt(eigenVals.z);
D = new Matrix3x3f();
D.m00 = d0;
D.m11 = d1;
D.m22 = d2;
const float eps = 1e-15f;
float l0 = eigenVals.x; if (l0 <= eps)
{
l0 = 0.0f;
}
else
{
l0 = 1.0f / d0;
}
float l1 = eigenVals.y; if (l1 <= eps)
{
l1 = 0.0f;
}
else
{
l1 = 1.0f / d1;
}
float l2 = eigenVals.z; if (l2 <= eps)
{
l2 = 0.0f;
}
else
{
l2 = 1.0f / d2;
}
Matrix3x3f S1 = new Matrix3x3f();
S1.m00 = l0 * U.m00 * U.m00 + l1 * U.m01 * U.m01 + l2 * U.m02 * U.m02;
S1.m11 = l0 * U.m10 * U.m10 + l1 * U.m11 * U.m11 + l2 * U.m12 * U.m12;
S1.m22 = l0 * U.m20 * U.m20 + l1 * U.m21 * U.m21 + l2 * U.m22 * U.m22;
S1.m01 = l0 * U.m00 * U.m10 + l1 * U.m01 * U.m11 + l2 * U.m02 * U.m12;
S1.m02 = l0 * U.m00 * U.m20 + l1 * U.m01 * U.m21 + l2 * U.m02 * U.m22;
S1.m12 = l0 * U.m10 * U.m20 + l1 * U.m11 * U.m21 + l2 * U.m12 * U.m22;
S1.m10 = S1.m01;
S1.m20 = S1.m02;
S1.m21 = S1.m12;
R = S1 * A;
// stabilize
Vector3f c0, c1, c2;
c0 = R.GetColumn(0);
c1 = R.GetColumn(1);
c2 = R.GetColumn(2);
if (c0.SqrMagnitude < eps)
{
c0 = c1.Cross(c2);
}
else if (c1.SqrMagnitude < eps)
{
c1 = c2.Cross(c0);
}
else
{
c2 = c0.Cross(c1);
}
R.SetColumn(0, c0);
R.SetColumn(1, c1);
R.SetColumn(2, c2);
}