There are two main steps for your requirements.
- Rotate joints 0, 1 and 2 around the origin by 90 degrees.
- Rotate joint 2 around joint 1 by 90 degrees.
I write some pseudo code, it almost done, but you still need some updates to use it. see comments in the code for details.
void Rotatation()
{
// Build up the rotation matrix for step 1
D3DXVECTOR3 rotAxis(0, 0, 1);
float angle = -(D3DX_PI / 2);
D3DXMATRIX rotMatrix;
D3DXMatrixRotationAxis(&rotMatrix, &rotAxis, angle);
// rotate joints 0, 1 and 2 by apply the matrix above
for (int i = 0; i < 3; i++)
{
joints[i].matrix *= rotMatrix;
}
// Build up the rotation matrix for joint 2
// Since joint 2 was not rotate around the origin(I mean the axis should pass the origin), so first you need to translate the rotation center to origin
// then rotate joint 2, and last move back
// After the rotation in step 1, joint 1 now locate at (0, 2, 0)
// to translate it to the origin.
D3DXMATRIX transMat;
D3DXMatrixTranslation(&transMat, 0, 2, 0);
// Now joint 2 can rotate around z-axis, so the rotate matrix is same as step 1
// after rotation, move back, this matrix is the inverse of transMat
D3DXMATRIX inverseTransMat;
D3DXMatrixTranslation(&transMat, 0, -2, 0);
// Combine the 3 matrix above
D3DXMATRIX rotMatjoin2 = transMat * rotMatjoin2 * inverseTransMat;
// rotate jonit 2
joints[2].matrix *= rotMatjoin2;
}