Vector3d CSkeleton::fitToLocation(Joint_handle hJoint, Vector3d target_loc)
{
Joint_handle hRoot = rootOf(hJoint);
updateGlobalPostures(hRoot);
if(hRoot == parents[hJoint])
{
joints[hJoint].setOffset(target_loc);
updateGlobalPostures(hRoot);
return globals[hJoint].getOffset();
}
//updateGlobalPostures(0);
Joint_handle hParent = parents[hJoint];
Vector3d dJoint, dTarget;
Vector3d pJoint, pParent;
pJoint = getGlobalPosition(hJoint);
pParent = getGlobalPosition(hParent);
dJoint = (pJoint - pParent).Normalize();
dTarget = (target_loc - pParent).Normalize();
Quaterniond qParent = getGlobalRotation(hParent);
// dJoint = cast_to_vector(!qParent*dJoint*qParent);
// dTarget = cast_to_vector(!qParent*dTarget*qParent);
Vector3d axis = Vector3d::Cross(dJoint, dTarget);
double dot = Vector3d::Dot(dJoint, dTarget);
double angle = acos(Vector3d::Dot(dJoint, dTarget));
Quaterniond qtemp = !qParent*Quaterniond(0, axis.X(), axis.Y(), axis.Z())*qParent;
axis = Vector3d(qtemp.X(), qtemp.Y(), qtemp.Z());
Quaterniond rot;
rot.FromAxisAngle(angle, axis.X(), axis.Y(), axis.Z());
qtemp = rot*Quaterniond(0, dJoint.X(), dJoint.Y(), dJoint.Z())*!rot;
Vector3d pos(qtemp.X(), qtemp.Y(), qtemp.Z());
Vector3d err = pos - dTarget;
if( angle == 0.f ||
(axis.X() == 0 && axis.Y() == 0 && axis.Z() ==0) ||
dJoint == dTarget)
return globals[hJoint].getOffset();
joints[hParent].Rotate(rot);
//rotateJoint(hParent, rot);
updateGlobalPostures(hParent);
//updateGlobalPostures(0);
return globals[hJoint].getOffset();
}
void CSkeleton::updateGlobalPostures(Joint_handle hJoint)
{
if(isRoot(hJoint))
globals[hJoint] = joints[hJoint];
else
{
size_t parent_idx = parents[hJoint];
globals[hJoint].setOffset(globals[parent_idx].getOffset());
Quaterniond q = globals[parent_idx].getRotation();
Vector3d offset = joints[hJoint].getOffset();
Quaterniond qtemp = q*Quaterniond(0, offset.X(), offset.Y(), offset.Z())*!q;
Vector3d pos(qtemp.X(), qtemp.Y(), qtemp.Z());
globals[hJoint].Translate(pos);
globals[hJoint].setRotation(q*joints[hJoint].getRotation());
}
Joint_handle_iterator jh_it, jh_end = children[hJoint].end();
for(jh_it = children[hJoint].begin(); jh_it != jh_end; ++jh_it)
{
updateGlobalPostures(*jh_it);
}
}