/*
* 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);
}
}
std::vector<Vec4d*> TransformNode::ComputeJacobian(Matd* J, C3dFileInfo* c3d, int frameNum) {
// Initialize handles list (transformed)
std::vector<Vec4d*> hLocals (c3d->GetHandleCount());
for (int i = 0; i < hLocals.size(); i++) {
hLocals[i] = (Vec4d*)NULL;
}
// std::cout << mName << " Working on TransformNode " << std::endl;
// Call all children first
for (int i = 0; i < mChildren.size(); i++) {
// We should get all child handles and their transforms
std::vector<Vec4d*> cLocals = static_cast<TransformNode*>(mChildren[i])->ComputeJacobian(J, c3d, frameNum);
// Copy handle transforms
for (int j = 0; j < cLocals.size(); j++) {
if (cLocals[j] != (Vec4d*)NULL) {
hLocals[j] = new Vec4d(*cLocals[j]);
}
}
}
// std::cout << mName << " Called children of " << std::endl;
// Now add all our handles to the list
for (int i = 0; i < mHandles.size(); i++) {
hLocals[mHandles[i]->mMarkerOrder] = new Vec4d(mHandles[i]->mOffset, 1);
}
// std::cout << mName << " has handle list:" << std::endl;
// for (int i = 0; i < hLocals.size(); i++) {
// if (hLocals[i] != (Vec4d*)NULL) {
// std::cout << *hLocals[i] << std::endl;
// } else {
// std::cout << "NULL" << std::endl;
// }
// }
for (int i = 0; i < GetSize(); i++) {
if (!mTransforms[i]->IsDof()) continue;
for (int j = 0; j < mTransforms[i]->GetDofCount(); j++) {
// std::cout << mName << " Working on " << i << " transform " << j << " DOF" << std::endl;
// Which column of J should this DOF be in?
Transform* trans = mTransforms[i];
int c = trans->GetDof(j)->mId;
// Derivative of this local transform
// Some transforms have many DOFs
// Multiply all transforms;
// If we come to this current transform i,
// Take the derivative of it with respect to jth DOF.
// Otherwise, just get regular transform
Mat4d T = vl_I;
for (int k = 0; k < GetSize(); k++) {
if (k == i)
T *= mTransforms[i]->GetDeriv(j);
else
T *= mTransforms[i]->GetTransform();
}
// Now T is our local transform with derivative
// taken with respect to the jth DOF.
// Now the partial transforms of all handles
// on children is populated on model for use.
for (int r = 0; r < hLocals.size(); r++) {
// Make homogeneous coordinate of child handles
// Vec4d h = Vec4d(c3d->GetMarkerPos(frameNum, r), 1);
Vec4d h = vl_zero;
if (hLocals[r] != (Vec4d*)NULL)
h = *hLocals[r];
// Derivative transform
h = T * h;
// Each row is an x,y, or z of a handle
// All child handles use the transform
// Otherwise, all 0s are output
(*J)[r*3+0][c] = h[0];
(*J)[r*3+1][c] = h[1];
(*J)[r*3+2][c] = h[2];
}
}
}
// Now apply local transform to all handles
for (int i = 0; i < hLocals.size(); i++) {
if (hLocals[i] == (Vec4d*)NULL) continue;
Vec4d* ht = new Vec4d(mLocalTransform * *hLocals[i]);
hLocals[i] = ht;
}
return hLocals;
}