/*
* Jacobian recursive worker method
*/
void Solver::computeJ(Matd &J, TransformNode *node, Vec4d &pos, Mat4d &trans) {
// Get initial transform data
std::vector<Transform *> transforms = node->mTransforms;
// LEFT MATRIX
Mat4d leftMat;
leftMat.MakeDiag(1.0); // identity
for (int i = 0; i < transforms.size(); i++)
{
Transform *tr = transforms[i];
// Only need to calculate if DOF
if (tr->IsDof())
{
for (int j = 0; j < tr->GetDofCount(); j++)
{
// Get DOF
Dof *dof = tr->GetDof(j);
int dofId = dof->mId;
// Add to our map
leftMap[dofId] = leftMat;
}
}
// Now update leftMat
leftMat = leftMat * tr->GetTransform();
}
// RIGHT MATRIX
Mat4d rightMat;
rightMat.MakeDiag(1.0); // identity
for (int i = transforms.size()-1; i >= 0; i--)
{
Transform *tr = transforms[i];
// Only need to calculate if DOF
if (tr->IsDof())
{
for (int j = tr->GetDofCount()- 1; j >= 0 ; j--)
{
// Get DOF
Dof *dof = tr->GetDof(j);
int dofId = dof->mId;
// Add to our map
rightMap[dofId] = rightMat;
}
}
// Now update leftMat
rightMat = tr->GetTransform() * rightMat;
}
// JACOBIAN
Mat4d pTrans = node->mParentTransform;
// New identity matrix for later use
Mat4d newTransform;
newTransform.MakeDiag(1.0);
for (int i = 0; i < transforms.size(); i++)
{
// Check if DOF, if so compute derivative
Transform *tr = transforms[i];
if (tr->IsDof())
{
for (int j = 0; j < tr->GetDofCount(); j++)
{
Mat4d deriv = tr->GetDeriv(j);
// Get row
Dof * dof = tr->GetDof(j);
int dofId = dof->mId;
Mat4d leftMat = leftMap[dofId];
Mat4d rightMat = rightMap[dofId];
Vec4d value = pTrans * leftMat * deriv * rightMat * trans * pos;
J[dofId] = value;
}
}
newTransform = newTransform * tr->GetTransform();
}
// Calculate J[] values for parent if necessary
TransformNode *parent = node->mParentNode;
// If the parent is non-null and not the current node
if (parent != NULL && parent != node)
{
newTransform = newTransform * trans;
computeJ(J, parent, pos, newTransform);
}
}
