public void GetColumn() { Matrix3x3f m = Indexed3x3(); 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)); }
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); }