Updating a Joint IK Chain

I'm trying to create an IK chain of (rigid body) bones, linked together with custom hinge joints.

I've managed to get the IK working reasonably well, here's a screenshot:



So the 'snake' is attempting to touch the green target with its tip (i.e the end effector).


Here's the creation code (hacked together from the tutorial):


for (int x=0;x<numBones-1;x++)
{

// make the rigid body.
child[x] = makeSimpleBox(size, pos, orient);

//Custom callback, no gravity
child[x]->setCustomForceAndTorqueCallback<OgreNewtonApplication>( &OgreNewtonApplication::defaultCallback, this );

// make the joint right between the bodies...
if (parent)
{
joint[x] = new myCustomJoint(mSceneMgr, child[x], parent, pos-Ogre::Vector3(size.x/2,0,0), Ogre::Vector3(Ogre::Vector3::UNIT_Z));
}
else
{
joint[x] = new myCustomJoint(mSceneMgr, child[x], NULL, pos-Ogre::Vector3(size.x/2,0,0), Ogre::Vector3(Ogre::Vector3::UNIT_Z));
}
}

pos += Ogre::Vector3(size.x,0,0);

// save the last body for the next loop!
parent = child[x];


And the joint callback (based on the Rigid Joint code):


void myCustomJoint::submitConstraint()
{
// get globals.
Ogre::Vector3 globalPos0, globalPos1;
Ogre::Quaternion globalOrient0, globalOrient1;

localToGlobal( mLocalOrient0, mLocalPos0, globalOrient0, globalPos0, 0 );
localToGlobal( mLocalOrient1, mLocalPos1, globalOrient1, globalPos1, 1 );

//Update position of next joint
Ogre::Vector3 dir = globalOrient0 * Ogre::Vector3::UNIT_Z;
Vector3 boneVector = globalPos1 + (dir * -mLength*2);
box1node->setPosition(boneVector);// + (getAngleVector() * mLength));

// apply the constraints!
addLinearRow( globalPos0, globalPos1, globalOrient0 * Ogre::Vector3::UNIT_X );
addLinearRow( globalPos0, globalPos1, globalOrient0 * Ogre::Vector3::UNIT_Y );
addLinearRow( globalPos0, globalPos1, globalOrient0 * Ogre::Vector3::UNIT_Z );

// now find a point off 10 units away.
globalPos0 = globalPos0 + (globalOrient0 * (Ogre::Vector3::UNIT_X * 10.0f));
globalPos1 = globalPos1 + (globalOrient1 * (Ogre::Vector3::UNIT_X * 10.0f));

// apply the constraints!
addLinearRow( globalPos0, globalPos1, globalOrient0 * Ogre::Vector3::UNIT_Y );
addLinearRow( globalPos0, globalPos1, globalOrient0 * Ogre::Vector3::UNIT_Z );


// add the IK solver angle
addAngularRow(Radian(mAngle), globalOrient0 * Ogre::Vector3::UNIT_X);
}


The last line is the angle passed to the joint by the IK solver, for minimizing the error between end-effector and target.

-------------------

I'm using the CCD algorithm, which aims to rotate each joint so that the end-effector is a little closer to the target each step. The chain is processed in reverse, starting at the last joint and working its way from child to parent all the way back to the root.


Lets say we start at the last joint (JOINT 14 - i.e the last red sphere before the green target in the image). The incremental angle for this joint is calculated , which will rotate the hinge a little closer to the target. Then the parent joint (JOINT 13) is processed, then 12 etc.

The problem I have is that for this algorithm to work efficiently, the preceeding joints need to know the updated angle of the child joints. I.e in the example above, JOINT 14 needs to actually rotate before JOINT 13 is processed.

First I tested the program by updating the joints in a loop, i.e:



for(int i=numberOfJoints-1; i>=0; i++)
{
...
joint[i]->setAngleToRotate( calculateCCDJointRotation(...) );
...
}


As I believe it, the physics are calculated once per frame for all the rigid bodies, so the preceeding joints aren't aware of the changes applied to previous ones higher up the chain, and therefore in effect the joints update at the same time.


However, if I do this :

for(int i=numberOfJoints-1; i>=0; i++)
{
...
joint[i]->setAngleToRotate( calculateCCDJointRotation(...) );
m_World->update(...);
...
}


The IK chain works better, but the framerate is reduced and all other rigid bodies are updated 15 times per frame!

I've also tried having a counter, so that the joints are updated sequentially, one each frame. But this means that the IK chain is only updated as a whole every 14 frames..

My current (non-Newton) simulation solved the problem like this :



for(size_t bone = numBones; bone > 0; --bone)
{

//Calculate angle to rotate each joint
float angle = calculateCCDJointRotation(legBones[bone]);
...
...
ACTUALLY ROTATE THE BONE by angle
...
...
//update the current bone, for next
legBones[bone]->_update(true, false);
}


So I guess what I'm asking is how would I implement the last line (update the bone rotation, without updating anything else), in OgreNewt. Can anyone help?