So we have a system of connected joints and we want to find out the the relative delta rotation of the joints from one frame to another. I'll call the relative rotation the local rotation since the "relative rotation" on it's own doesn't tell us what it's relative to. (Is it relative to an object, to the center of the universe, etc?)
Assumptions
I'm going to assume a tree structure of joints so that each joint only has one parent and we only have one root joint (without parents). If you have several parents per joint you should be able to use the same solution, but you'll then each joint will one relative rotation per parent and you need to do the calcualtion once for each parent. If you have several joints without parents then each one can be thought of as a root in it's own tree made up of the connected joints.
I'll assume you have a quaternion math library that can; create from an axis and an angle, set to identity, inverse, and accumulate quaternions. If you don't you should find all the info you need to implement them from Wikipedia or Google.
Calulating the rotations
The code below first calculates the local rotations of the joint for the start and the end of the frame. It does this calculation using two vector; the vector from it's parent and the vector from the grandparent to the parent. Then in order to calculate the delta rotation it uses the inverted start rotation to "remove" the start rotation from the end rotation by applying it's inverse rotation. So we end up with the local delta rotation for that frame.
For the first two levels of the joint hierarchy we have special cases which we can solve directly.
Pseudocode
The out parameter is a multidimensional array named result.
NB: startPosition, endPosition, parentStartPosition, parentEndPosition, grandParentStartPosition, grandParentStartPosition all have to be updated for each iteration of the loops. That update is not shown in order to focus on the core of the problem.
for each frame {
for each joint {
if no parent {
// no parent means no local rotation
result[joint,frame] = identityQuaternion
}
else {
startLink = startPosition - parentStartPosition
endLink = endPosition - parentEndPosition
if no grandParent {
// no grand parent - we can calculate the local rotation directly
result[joint,frame] = QuaternionFromVectors( startLink, endLink )
}
else {
parentStartLink = parentStartPosition - grandParentStartPosition
parentEndLink = parentEndPosition - grandParentEndPosition
// calculate the local rotations
// = the difference in rotation between parent link and child link
startRotation = QuaternionFromVectors( parentStartLink, startLink )
endRotation = QuaternionFromVectors( parentEndLink, endLink )
// calculate the delta local rotation
// = the difference between start and end local rotations
invertedStartRotation = Inverse( startRotation )
deltaRotation = invertedStartRotation.Rotate( endRotation )
result[joint,frame] = deltaRotation
}
}
}
}
QuaternionFromVectors( fromVector, toVector )
{
axis = Normalize( fromVector.Cross( toVector ) )
angle = Acos( fromVector.Dot( toVector ) )
return Quaternion( axis, angle )
}
C++ implementation
Below is an untested recursive implementation in C++. For each frame we start at the root of our JointData tree and then traverse the tree by recursivly calling the JointData::calculateRotations() function.
In order to make the code easier to read I have an accessor from the joint tree nodes JointData to the FrameData. You probably don't want to have such a direct dependency in your implementation.
// Frame data holds the individual frame data for a joint
struct FrameData
{
Vector3 m_positionStart;
Vector3 m_positionEnd;
// this is our unknown
Quaternion m_localDeltaRotation;
}
class JointData
{
public:
...
JointData *getChild( int index );
int getNumberOfChildren();
FrameData *getFrame( int frameIndex );
void calculateDeltaRotation( int frameIndex, JointData *parent = NULL,
Vector3& parentV1 = Vector3(0),
Vector3& parentV2 = Vector3(0) );
...
}
void JointData::calculateDeltaRotation( int frameIndex, JointData *parent,
Vector3& parentV1, Vector3& parentV2 )
{
FrameData *frameData = getFrame( frameIndex );
if( !parent )
{
// this is the root, it has no local rotation
frameData->m_localDeltaRotation.setIdentity();
return;
}
FrameData *parentFrameData = parent->getFrame( frameIndex );
// calculate the vector from our parent
// for the start (v1) and the end (v2) of the frame
Vector3 v1 = frameData->m_positionStart - parentFrameData->m_positionStart;
Vector3 v2 = frameData->m_positionEnd - parentFrameData->m_positionEnd;
if( !getParent()->getParent() )
{
// child of the root is a special case,
// we can calculate it's rotation directly
frameData->m_localDeltaRotation = calculateQuaternion( v1, v2 );
}
else
{
// calculate start and end rotations
// apply inverse start rotation to end rotation
Quaternion startRotation = calculateQuaternion( parentV1, v1 );
Quaternion endRotation = calculateQuaternion( parentV2, v2 );
Quaternion invStartRot = startRotation.inverse();
frameData->m_localDeltaRotation = invStartRot.rotate( endRotation );
}
for( int i = 0; i < getNumberOfChildren(); ++i )
{
getChild( i )->calculateRotations( frameIndex, this, v1, v2 );
}
}
// helper function to calulate a quaternion from two vector
Quaternion calculateQuaternion( Vector3& fromVector, Vector3& toVector )
{
float angle = acos( fromVector.dot( toVector ) );
Vector3 axis = fromVector.cross( toVector );
axis.normalize();
return Quaternion( axis, angle );
}
The code is written for readability and not to be optimal.
