void TetMesh::computeGradient(int element, const double * U, int numFields, double * grad) const
{
// grad is 9 x numFields
// grad is constant inside a tet
Vec3d vtx[4];
for(int i=0; i<4; i++)
vtx[i] = *getVertex(element,i);
// form M =
// [b - a]
// [c - a]
// [d - a]
Mat3d M(vtx[1] - vtx[0], vtx[2] - vtx[0], vtx[3] - vtx[0]);
Mat3d MInv = inv(M);
//printf("M=\n");
//M.print();
for(int field=0; field<numFields; field++)
{
// form rhs =
// [U1 - U0]
// [U2 - U0]
// [U3 - U0]
const double * u[4];
for(int i=0; i<4; i++)
u[i] = &U[3 * numVertices * field + 3 * getVertexIndex(element, i)];
Vec3d rows[3];
for(int i=0; i<3; i++)
rows[i] = Vec3d(u[i+1]) - Vec3d(u[0]);
Mat3d rhs(rows);
//printf("rhs=\n");
//rhs.print();
Mat3d gradM = trans(MInv * rhs);
gradM.convertToArray(&grad[9 * field]);
/*
// test gradient
if (field == 0)
{
printf("----\n");
printf("0: pos: %.15f %.15f %.15f | uExact: %.15f %.15f %.15f\n", vtx[0][0], vtx[0][1], vtx[0][2], u[0][0], u[0][1], u[0][2]);
for(int vertex=0; vertex<3; vertex++)
{
Vec3d u1 = Vec3d(u[vertex+1]);
printf("%d: ", vertex+1);
printf("pos: %.15f %.15f %.15f | uExact: %.15f %.15f %.15f | ", vtx[vertex+1][0], vtx[vertex+1][1], vtx[vertex+1][2], u1[0], u1[1], u1[2]);
Vec3d u1approx = Vec3d(u[0]) + gradM * (vtx[vertex+1] - vtx[0]);
printf("uApprox: %.15f %.15f %.15f\n", u1approx[0], u1approx[1], u1approx[2]);
}
printf("----\n");
}
*/
}
}
void PolarDecompositionGradient::Compute(const double * M, const double * Q, const double * S, const double * MDot, double * omega, double * QDot, double * SDot, const double * MDotDot, double * omegaDot, double * QDotDot)
{
// compute omega = G^{-1} (2 * skew(Q^T * MDot)), where G = (tr(S)I - S) * Q^T
// (see Barbic and Zhao, SIGGRAPH 2011)
// first, construct G, and invert it
// tempMatrix = tr(S)I - S
double tempMatrix[9];
for(int i=0; i<9; i++)
tempMatrix[i] = -S[i];
double trace = S[0] + S[4] + S[8];
tempMatrix[0] += trace;
tempMatrix[4] += trace;
tempMatrix[8] += trace;
double G[9]; // G = (tr(S)I - S) * Q^T
MATRIX_MULTIPLY3X3ABT(tempMatrix, Q, G);
Mat3d GM(G);
Mat3d GInvM = inv(GM);
double GInv[9];
GInvM.convertToArray(GInv);
// omega = GInv * (2 * skew(R^T * Mdot))
MATRIX_MULTIPLY3X3ATB(Q, MDot, tempMatrix);
double rhs[3];
SKEW_PART(tempMatrix, rhs);
VECTOR_SCALE3(rhs, 2.0);
MATRIX_VECTOR_MULTIPLY3X3(GInv, rhs, omega);
// compute QDot = tilde(omega) * Q
double omegaTilde[9];
SKEW_MATRIX(omega, omegaTilde);
//double QDot[9];
MATRIX_MULTIPLY3X3(omegaTilde, Q, QDot);
// compute SDot = Q^T * (MDot - QDot * S)
// tempMatrix = MDot - QDot * S
MATRIX_MULTIPLY3X3(QDot, S, tempMatrix);
for(int i=0; i<9; i++)
tempMatrix[i] = MDot[i] - tempMatrix[i];
// SDot = Q^T * tempMatrix
MATRIX_MULTIPLY3X3ATB(Q, tempMatrix, SDot);
if ((MDotDot != NULL) && (omegaDot != NULL))
{
// compute omegaDot = GInv * ( 2 skew(Q^T (ADotDot - omegaTilde * ADot)) - (tr(SDot) I - SDot) * Q^T * omega )
// (see Barbic and Zhao, SIGGRAPH 2011)
// tempMatrix = MDotDot - omegaTilde * MDot
MATRIX_MULTIPLY3X3(omegaTilde, MDot, tempMatrix);
for(int i=0; i<9; i++)
tempMatrix[i] = MDotDot[i] - tempMatrix[i];
double tempMatrix2[9];
// tempVector = 2 * skew(Q^T * tempMatrix)
MATRIX_MULTIPLY3X3ATB(Q, tempMatrix, tempMatrix2);
double tempVector[3];
SKEW_PART(tempMatrix2, tempVector);
VECTOR_SCALE3(tempVector, 2.0);
// tempMatrix = tr(SDot)I - SDot
for(int i=0; i<9; i++)
tempMatrix[i] = -SDot[i];
double trace = SDot[0] + SDot[4] + SDot[8];
tempMatrix[0] += trace;
tempMatrix[4] += trace;
tempMatrix[8] += trace;
// tempVector2 = (tempMatrix * Q^T) * omega
double tempVector2[3];
MATRIX_MULTIPLY3X3ABT(tempMatrix, Q, tempMatrix2);
MATRIX_VECTOR_MULTIPLY3X3(tempMatrix2, omega, tempVector2);
// tempVector -= tempVector2
VECTOR_SUBTRACTEQUAL3(tempVector, tempVector2);
// tempVector2 = GInv * tempVector
MATRIX_VECTOR_MULTIPLY3X3(GInv, tempVector, omegaDot);
if (QDotDot != NULL)
{
double tempMatrix[9];
SKEW_MATRIX(omegaDot, tempMatrix);
MATRIX_MULTIPLY3X3(omegaTilde, omegaTilde, tempMatrix2);
for(int i=0;i<9;i++)
tempMatrix[i] += tempMatrix2[i];
MATRIX_MULTIPLY3X3(tempMatrix, Q, QDotDot);
}
}
}
