Converting Global Hierarchical Rotations to Local and back

Question

I have a hierarchical skeleton, in which translations & rotations are expressed in global space. I need to convert the skeleton into local coordinates and back again. I have no problem with the translations, but I can't seem to get the rotations working.

Initially, I have the rotation matrices, but I can easily convert them to Euler or quaternions, and have tried working with each. I am using the Irrlicht engine math library.

So, my code goes through every bone, and applies the transformation as long as the bone has a next parent.

From Global to Local:

for(; iter != absolutePose.end(); iter++)
{
    if(hierarchy->find(iter->boneID) != hierarchy->end())
    {
        BoneID parentNodeID = hierarchy[iter->boneID];
        relativePose[iter->boneID].position.X = iter->position.X - absolutePose[parentNodeId].position.X;
        relativePose[iter->boneID].position.Y = iter->position.Y - absolutePose[parentNodeId].position.Y;
        relativePose[iter->boneID].position.Z = iter->position.Z - absolutePose[parentNodeId].position.Z;

                    ////Rotation Part/////////
        float pq[4] ={0.0,0.0,0.0,0.0};
        MatrixToQuaternion(absolutePose[parentNodeId].orientation, pq);

        irr::core::quaternion quat(q[0],q[1],q[2],q[3]);
        irr::core::quaternion parentquat(pq[0],pq[1],pq[2],pq[3]);

        quat = quat.makeInverse();

        quat = quat*parentquat;

        float newQuat[4] = {quat.X, quat.Y, quat.Z, quat.W};
        QuaternionToMatrix(newQuat,relativePose[iter->first].orientation);

    }
    else // top bone
    {
        relativePose[iter->boneID].position.X = iter->position.X;
        relativePose[iter->boneID].position.Y = iter->position.Y;
        relativePose[iter->boneID].position.Z = iter->position.Z;

        relativePose[iter->boneID].orientation = iter->second.orientation;
    }
}

Then from Local to Global:

for(; iter != relativePose.end(); iter++)
    {
        absolutePose[iter->boneID].position.position = iter->position;
        auto nextParent = hierarchy->find(iter->boneID);
        while(nextParent != hierarchy->end())
        {
            absolutePose[iter->boneID].position.X += relativePose[nextParent->boneID].position.X;
            absolutePose[iter->boneID].position.Y += relativePose[nextParent->boneID].position.Y;
            absolutePose[iter->boneID].position.Z += relativePose[nextParent->boneID].position.Z;

            ////Rotation part///
            float q[4] ={0.0,0.0,0.0,0.0};
            MatrixToQuaternion(relativePose[iter->boneID].orientation, q);

            float pq[4] ={0.0,0.0,0.0,0.0};
            MatrixToQuaternion(relativePose[nextParent->boneID].orientation, pq);
            irr::core::quaternion quat(q[0],q[1],q[2],q[3]);
            irr::core::quaternion parentquat(pq[0],pq[1],pq[2],pq[3]);

            quat = parentquat*quat;

            float newQuat[4] = {quat.X, quat.Y, quat.Z, quat.W};
            QuaternionToMatrix(newQuat,absolutePose[iter->boneID].orientation);
            //////

            nextParent = hierarchy->find(nextParent->boneID);
        }
    }

I have been having issues with this for a while now, and have also tried staying in matrix mode and switching to Euler angles as well. Can anyone help me figure out what I'm doing wrong?

Answer 1

It doesn't matter if you use matrices or quaternions. I'd recommend against using Euler angles here. It looks to me like the problem is that you're inverting the child quaternion instead of the parent when converting to local coordinates and you're not propagating rotations when converting to global coordinates.

Consider (assuming orientations are applied to column vectors on the right):

q_1, q_2, and q_3 are the global orientations of three bodies in a chain. If we want to use relative orientations r_2 and r_3, then you have to take the parent's rotation out of its descendants. So we can see that q_2 = q_1 * r_2 and q_3 = q_1 * r_2 * r_3 = q_2 * r_3.

Solve for your relative values and you get r_3 = q_2' * q_3 and likewise r_2 = q_1' * q_2 (where q_2' is the matrix or quaternion inverse).

Going the other direction to convert from global to local, you need to remember:

q_3 = q_1 * r_2 * r_3

All of the parent orientations need to propagate down to the final descendant. So you either need to use recursion to grab all the orientation changes from from the child up to the root, or you need to start at the parent an propagate/accumulate all the orientation changes at each link as you traverse down through the children.

As an afterthought, I suppose that you also need to be sure that the root node realizes that its relativePose is equal to its absolutePose. Otherwise you'll have problems.