// Heapify starts at the last internal node of the tree: n/2.
// While it hasn't reached the root, it calls siftdown()
// which recursively compares the node values. The tree will
// be a max heap when heapify() returns
void heapify () {
int *start = rootOf(end());
while(start >= root()) {
siftDown(start);
--start;
}
}
// Siftup() takes a node that has just been added to the heap
// and compares it to its parent. It swaps values if necessary
// and recurses on the parent node until reaching the root.
void siftup(int *node) {
// base case
if(node == root() || size() == 0) return;
int *parent = rootOf(node);
if(*node < *parent) swapValues(node, parent);
// recursion
return siftup(parent);
}
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();
}