internal FEMTetConstraint3d(Body3d body, int p0, int p1, int p2, int p3, double youngsModulus) : base(body) { P0 = p0; P1 = p1; P2 = p2; P3 = p3; YoungsModulus = youngsModulus; PoissonRatio = 0.3; Correction = new Vector3d[4]; Vector3d x0 = Body.Positions[P0]; Vector3d x1 = Body.Positions[P1]; Vector3d x2 = Body.Positions[P2]; Vector3d x3 = Body.Positions[P3]; RestVolume = Math.Abs((1.0f / 6.0) * Vector3d.Dot(x3 - x0, Vector3d.Cross(x2 - x0, x1 - x0))); Matrix3x3d restMatrix = new Matrix3x3d(); restMatrix.SetColumn(0, x0 - x3); restMatrix.SetColumn(1, x1 - x3); restMatrix.SetColumn(2, x2 - x3); InvRestMatrix = restMatrix.Inverse; }
internal StrainTetConstraint3d(Body3d body, int p0, int p1, int p2, int p3, double stiffness) : base(body) { P0 = p0; P1 = p1; P2 = p2; P3 = p3; Stiffness = stiffness; NormalizeStretch = false; NormalizeShear = false; Correction = new Vector3[4]; Vector3 x0 = body.Positions[P0]; Vector3 x1 = body.Positions[P1]; Vector3 x2 = body.Positions[P2]; Vector3 x3 = body.Positions[P3]; Matrix3x3d restMatrix = new Matrix3x3d(); restMatrix.SetColumn(0, MathConverter.ToVector3d(x1 - x0)); restMatrix.SetColumn(1, MathConverter.ToVector3d(x2 - x0)); restMatrix.SetColumn(2, MathConverter.ToVector3d(x3 - x0)); InvRestMatrix = restMatrix.Inverse; }
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)); }
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); }
/// <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(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); }