Applying rotation/translation to a hierarchy of joints

Let’s say i have a skeleton with several joints. I want to make it so i can rotate a joint and all of its child joints will rotate with it.

Firstly, you grab the joint and move the mouse. An axis and angle is calculated based on the mouse and sent to the skeleton to update the joint which was moved.


void Skeleton::updateJointRotation(Vector4 rot)
{
	for (unsigned i=0; i<joints.size(); i++)
    {
        if (joints[i].isPicked)
		{
			joints[i].rotation = rot;
			joints[i].p = Matrix4().identity().translate(joints[i].position);
			joints[i].r = Matrix4().identity().rotate(rot.w, rot.x, rot.y, rot.z);
		}
    }
}

Note that “p” is the joint position in 4x4 matrix notation, “r” is its rotation axis/angle represented as a matrix.

Here is my draw code. It doesn’t currently work. If i rotate joints around, their children do not rotate with them. If the bone is an immediate child of the root bone, it seems to rotate around it, however it doesn’t rotate in a circular motion so maybe i am calling the rotate code incorrectly?


for (unsigned i=0; i<joints.size(); i++)
    {
        if (joints[i].isPicked)
            glColor3f(1.0, 0.0, 0.0);
        else if (joints[i].isHovered)
            glColor3f(0.8, 0.8, 0.7);
        else
            glColor3f(0.3, 0.3, 0.3);


		Joint cur = joints[i];

		if (cur.parent == -1) // it is the root, just draw the joint.
		{
			glTranslated(joints[i].position.x, joints[i].position.y, joints[i].position.z);
			glutSolidSphere(0.01, 15, 15);
			glTranslated(-joints[i].position.x, -joints[i].position.y, -joints[i].position.z);
		}
		else
		{
			Matrix4 newM = Matrix4().identity();

			if (cur.parent >= 0) // the cur joint is not the root
			{
				Matrix4 pp = joints[cur.parent].p; //parent position matrix

				newM = newM * (cur.r * cur.p - cur.r * pp + pp);
				
				//cur = joints[cur.parent]; // should this be a while loop, not an if statement?
			}

			Quaternion q;
			Vector4 axAng;
			CalculateQuaternion(q,newM);
			QuaternionToAxisAng(q,axAng);

			glRotated(axAng.w, axAng.x, axAng.y, axAng.z);
			glTranslated(newM[12], newM[13], newM[14]);
			glutSolidSphere(0.01, 15, 15);
			glTranslated(-newM[12], -newM[13], -newM[14]);
			glRotated(-axAng.w, axAng.x, axAng.y, axAng.z);

		}

Any help is appreciated