public void SetRow() { Matrix3x3d m = new Matrix3x3d(); m.SetRow(0, new Vector3d(0, 3, 6)); m.SetRow(1, new Vector3d(1, 4, 7)); m.SetRow(2, new Vector3d(2, 5, 8)); Assert.AreEqual(new Vector3d(0, 3, 6), m.GetRow(0)); Assert.AreEqual(new Vector3d(1, 4, 7), m.GetRow(1)); Assert.AreEqual(new Vector3d(2, 5, 8), m.GetRow(2)); }
private void ComputeGreenStrainAndPiolaStressInversion(Vector3d x1, Vector3d x2, Vector3d x3, Vector3d x4, double mu, double lambda, ref Matrix3x3d epsilon, ref Matrix3x3d sigma, ref double energy) { // Determine \partial x/\partial m_i Matrix3x3d F = new Matrix3x3d(); Vector3d p14 = x1 - x4; Vector3d p24 = x2 - x4; Vector3d p34 = x3 - x4; F.m00 = p14.x * InvRestMatrix.m00 + p24.x * InvRestMatrix.m10 + p34.x * InvRestMatrix.m20; F.m01 = p14.x * InvRestMatrix.m01 + p24.x * InvRestMatrix.m11 + p34.x * InvRestMatrix.m21; F.m02 = p14.x * InvRestMatrix.m02 + p24.x * InvRestMatrix.m12 + p34.x * InvRestMatrix.m22; F.m10 = p14.y * InvRestMatrix.m00 + p24.y * InvRestMatrix.m10 + p34.y * InvRestMatrix.m20; F.m11 = p14.y * InvRestMatrix.m01 + p24.y * InvRestMatrix.m11 + p34.y * InvRestMatrix.m21; F.m12 = p14.y * InvRestMatrix.m02 + p24.y * InvRestMatrix.m12 + p34.y * InvRestMatrix.m22; F.m20 = p14.z * InvRestMatrix.m00 + p24.z * InvRestMatrix.m10 + p34.z * InvRestMatrix.m20; F.m21 = p14.z * InvRestMatrix.m01 + p24.z * InvRestMatrix.m11 + p34.z * InvRestMatrix.m21; F.m22 = p14.z * InvRestMatrix.m02 + p24.z * InvRestMatrix.m12 + p34.z * InvRestMatrix.m22; Matrix3x3d U, VT; Vector3d hatF; Matrix3x3dDecomposition.SVDWithInversionHandling(F, out hatF, out U, out VT); // Clamp small singular values double minXVal = 0.577; if (hatF.x < minXVal) { hatF.x = minXVal; } if (hatF.y < minXVal) { hatF.y = minXVal; } if (hatF.z < minXVal) { hatF.z = minXVal; } // epsilon for hatF Vector3d epsilonHatF = new Vector3d(0.5 * (hatF.x * hatF.x - 1.0), 0.5 * (hatF.y * hatF.y - 1.0), 0.5 * (hatF.z * hatF.z - 1.0)); double trace = epsilonHatF.x + epsilonHatF.y + epsilonHatF.z; double ltrace = lambda * trace; Vector3d sigmaVec = epsilonHatF * 2.0 * mu; sigmaVec.x += ltrace; sigmaVec.y += ltrace; sigmaVec.z += ltrace; sigmaVec.x = hatF.x * sigmaVec.x; sigmaVec.y = hatF.y * sigmaVec.y; sigmaVec.z = hatF.z * sigmaVec.z; Matrix3x3d sigmaDiag = new Matrix3x3d(); Matrix3x3d epsDiag = new Matrix3x3d(); sigmaDiag.SetRow(0, new Vector3d(sigmaVec.x, 0.0, 0.0)); sigmaDiag.SetRow(1, new Vector3d(0.0, sigmaVec.y, 0.0)); sigmaDiag.SetRow(2, new Vector3d(0.0, 0.0, sigmaVec.z)); epsDiag.SetRow(0, new Vector3d(epsilonHatF.x, 0.0, 0.0)); epsDiag.SetRow(1, new Vector3d(0.0, epsilonHatF.y, 0.0)); epsDiag.SetRow(2, new Vector3d(0.0, 0.0f, epsilonHatF.z)); epsilon = U * epsDiag * VT; sigma = U * sigmaDiag * VT; double psi = 0.0f; for (int j = 0; j < 3; j++) { for (int k = 0; k < 3; k++) { psi += epsilon[j, k] * epsilon[j, k]; } } psi = mu * psi + 0.5 * lambda * trace * trace; energy = RestVolume * psi; }
/// <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(Matrix3x3d M, double tolerance, out Matrix3x3d R) { Matrix3x3d Mt = M.Transpose; double Mone = OneNorm(M); double Minf = InfNorm(M); double Eone; Matrix3x3d MadjTt = new Matrix3x3d(); Matrix3x3d Et = new Matrix3x3d(); const double eps = 1.0e-15; 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))); double det = Mt.m00 * MadjTt.m00 + Mt.m01 * MadjTt.m01 + Mt.m02 * MadjTt.m02; if (Math.Abs(det) < eps) { int index = int.MaxValue; for (int i = 0; i < 3; i++) { double len = MadjTt.GetRow(i).SqrMagnitude; if (len > eps) { // 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 = Matrix3x3d.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))); Matrix3x3d M2 = Mt.Transpose; Mone = OneNorm(M2); Minf = InfNorm(M2); det = Mt.m00 * MadjTt.m00 + Mt.m01 * MadjTt.m01 + Mt.m02 * MadjTt.m02; } } double MadjTone = OneNorm(MadjTt); double MadjTinf = InfNorm(MadjTt); double gamma = Math.Sqrt(Math.Sqrt((MadjTone * MadjTinf) / (Mone * Minf)) / Math.Abs(det)); double g1 = gamma * 0.5; double g2 = 0.5 / (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 }