public void GetColumn()
{
Matrix3x3d m = Indexed3x3();
Assert.AreEqual(new Vector3d(0, 1, 2), m.GetColumn(0));
Assert.AreEqual(new Vector3d(3, 4, 5), m.GetColumn(1));
Assert.AreEqual(new Vector3d(6, 7, 8), m.GetColumn(2));
}
public void SetColumn()
{
Matrix3x3d m = new Matrix3x3d();
m.SetColumn(0, new Vector3d(0, 1, 2));
m.SetColumn(1, new Vector3d(3, 4, 5));
m.SetColumn(2, new Vector3d(6, 7, 8));
Assert.AreEqual(new Vector3d(0, 1, 2), m.GetColumn(0));
Assert.AreEqual(new Vector3d(3, 4, 5), m.GetColumn(1));
Assert.AreEqual(new Vector3d(6, 7, 8), m.GetColumn(2));
}
/// <summary>
/// Perform polar decomposition A = (U D U^T) R
/// </summary>
public static void PolarDecomposition(Matrix3x3d A, out Matrix3x3d R, out Matrix3x3d U, out Matrix3x3d D)
{
// A = SR, where S is symmetric and R is orthonormal
// -> S = (A A^T)^(1/2)
// A = U D U^T R
Matrix3x3d AAT = new Matrix3x3d();
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 = Matrix3x3d.Identity;
Vector3d eigenVals;
EigenDecomposition(AAT, out U, out eigenVals);
double d0 = Math.Sqrt(eigenVals.x);
double d1 = Math.Sqrt(eigenVals.y);
double d2 = Math.Sqrt(eigenVals.z);
D = new Matrix3x3d();
D.m00 = d0;
D.m11 = d1;
D.m22 = d2;
const double eps = 1e-15;
double l0 = eigenVals.x; if (l0 <= eps)
{
l0 = 0.0;
}
else
{
l0 = 1.0 / d0;
}
double l1 = eigenVals.y; if (l1 <= eps)
{
l1 = 0.0;
}
else
{
l1 = 1.0 / d1;
}
double l2 = eigenVals.z; if (l2 <= eps)
{
l2 = 0.0;
}
else
{
l2 = 1.0 / d2;
}
Matrix3x3d S1 = new Matrix3x3d();
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
Vector3d 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);
}
private bool SolveConstraint(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, double invMass)
{
double eps = 1e-9;
Correction[0] = Vector3.zero;
Correction[1] = Vector3.zero;
Correction[2] = Vector3.zero;
Correction[3] = Vector3.zero;
Vector3[] c = new Vector3[3];
c[0] = MathConverter.ToVector3(InvRestMatrix.GetColumn(0));
c[1] = MathConverter.ToVector3(InvRestMatrix.GetColumn(1));
c[2] = MathConverter.ToVector3(InvRestMatrix.GetColumn(2));
for (int i = 0; i < 3; i++)
{
for (int j = 0; j <= i; j++)
{
Matrix3x3d P = new Matrix3x3d();
// Jacobi
//P.col(0) = p1 - p0;
//P.col(1) = p2 - p0;
//P.col(2) = p3 - p0;
// Gauss - Seidel
P.SetColumn(0, MathConverter.ToVector3d((p1 + Correction[1]) - (p0 + Correction[0])));
P.SetColumn(1, MathConverter.ToVector3d((p2 + Correction[2]) - (p0 + Correction[0])));
P.SetColumn(2, MathConverter.ToVector3d((p3 + Correction[3]) - (p0 + Correction[0])));
Vector3 fi = MathConverter.ToVector3(P * MathConverter.ToVector3d(c[i]));
Vector3 fj = MathConverter.ToVector3(P * MathConverter.ToVector3d(c[j]));
double Sij = Vector3.Dot(fi, fj);
double wi = 0.0, wj = 0.0, s1 = 0.0, s3 = 0.0;
if (NormalizeShear && i != j)
{
wi = fi.magnitude;
wj = fj.magnitude;
s1 = 1.0f / (wi * wj);
s3 = s1 * s1 * s1;
}
Vector3[] d = new Vector3[4];
d[0] = Vector3.zero;
for (int k = 0; k < 3; k++)
{
d[k + 1] = fj * (float)InvRestMatrix[k, i] + fi * (float)InvRestMatrix[k, j];
if (NormalizeShear && i != j)
{
d[k + 1] = (float)s1 * d[k + 1] - (float)Sij * (float)s3 * ((float)(wj * wj) * fi * (float)InvRestMatrix[k, i] + (float)(wi * wi) * fj * (float)InvRestMatrix[k, j]);
}
d[0] -= d[k + 1];
}
if (NormalizeShear && i != j)
{
Sij *= s1;
}
double lambda = (d[0].sqrMagnitude + d[1].sqrMagnitude + d[2].sqrMagnitude + d[3].sqrMagnitude) * invMass;
if (Math.Abs(lambda) < eps)
{
continue;
}
if (i == j)
{
// diagonal, stretch
if (NormalizeStretch)
{
double s = Math.Sqrt(Sij);
lambda = 2.0 * s * (s - 1.0) / lambda * Stiffness;
}
else
{
lambda = (Sij - 1.0) / lambda * Stiffness;
}
}
else
{
// off diagonal, shear
lambda = Sij / lambda * Stiffness;
}
Correction[0] -= (float)lambda * (float)invMass * d[0];
Correction[1] -= (float)lambda * (float)invMass * d[1];
Correction[2] -= (float)lambda * (float)invMass * d[2];
Correction[3] -= (float)lambda * (float)invMass * d[3];
}
}
return(true);
}
private bool SolveConstraint(Vector3d p0, Vector3d p1, Vector3d p2, Vector3d p3, double invMass)
{
double eps = 1e-9;
Correction[0] = Vector3d.Zero;
Correction[1] = Vector3d.Zero;
Correction[2] = Vector3d.Zero;
Correction[3] = Vector3d.Zero;
Vector3d[] c = new Vector3d[3];
c[0] = InvRestMatrix.GetColumn(0);
c[1] = InvRestMatrix.GetColumn(1);
c[2] = InvRestMatrix.GetColumn(2);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j <= i; j++)
{
Matrix3x3d P = new Matrix3x3d();
// Jacobi
//P.col(0) = p1 - p0;
//P.col(1) = p2 - p0;
//P.col(2) = p3 - p0;
// Gauss - Seidel
P.SetColumn(0, (p1 + Correction[1]) - (p0 + Correction[0]));
P.SetColumn(1, (p2 + Correction[2]) - (p0 + Correction[0]));
P.SetColumn(2, (p3 + Correction[3]) - (p0 + Correction[0]));
Vector3d fi = P * c[i];
Vector3d fj = P * c[j];
double Sij = Vector3d.Dot(fi, fj);
double wi = 0.0, wj = 0.0, s1 = 0.0, s3 = 0.0;
if (NormalizeShear && i != j)
{
wi = fi.Magnitude;
wj = fj.Magnitude;
s1 = 1.0 / (wi * wj);
s3 = s1 * s1 * s1;
}
Vector3d[] d = new Vector3d[4];
d[0] = new Vector3d(0.0, 0.0, 0.0);
for (int k = 0; k < 3; k++)
{
d[k + 1] = fj * InvRestMatrix[k, i] + fi * InvRestMatrix[k, j];
if (NormalizeShear && i != j)
{
d[k + 1] = s1 * d[k + 1] - Sij * s3 * (wj * wj * fi * InvRestMatrix[k, i] + wi * wi * fj * InvRestMatrix[k, j]);
}
d[0] -= d[k + 1];
}
if (NormalizeShear && i != j)
{
Sij *= s1;
}
double lambda = (d[0].SqrMagnitude + d[1].SqrMagnitude + d[2].SqrMagnitude + d[3].SqrMagnitude) * invMass;
if (Math.Abs(lambda) < eps)
{
continue;
}
if (i == j)
{
// diagonal, stretch
if (NormalizeStretch)
{
double s = Math.Sqrt(Sij);
lambda = 2.0 * s * (s - 1.0) / lambda * Stiffness;
}
else
{
lambda = (Sij - 1.0) / lambda * Stiffness;
}
}
else
{
// off diagonal, shear
lambda = Sij / lambda * Stiffness;
}
Correction[0] -= lambda * invMass * d[0];
Correction[1] -= lambda * invMass * d[1];
Correction[2] -= lambda * invMass * d[2];
Correction[3] -= lambda * invMass * d[3];
}
}
return(true);
}
