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
}
