void Display::drawBone(Bone *pBone,int skelNum){ static float z_dir[3] = {0., 0., 1.}; float r_axis[3], mag, theta; //Tranform (rotate) from the local coordinate system of this bone to it's parent //This step corresponds to doing: ModelviewMatrix = M_k * (rot_parent_current) glMultMatrixd((double*)&pBone->rot_parent_current); //Draw the local coordinate system for the selected bone. if(pBone->idx == m_SpotJoint) draw_bone_axis(); //rotate AMC //This step corresponds to doing: ModelviewMatrix *= R_k+1 if(pBone->doftz) glTranslatef(0.,0.,pBone->tz); if(pBone->dofty) glTranslatef(0.,pBone->ty,0.); if(pBone->doftx) glTranslatef(pBone->tx,0.,0.); if(pBone->dofz) glRotatef(pBone->drz, 0., 0., 1.); if(pBone->dofy) glRotatef(pBone->dry, 0., 1, 0.); if(pBone->dofx) glRotatef(pBone->drx, 1., 0., 0.); glColor3f(1., 1., 0.1); //Store the current ModelviewMatrix (before adding the translation part) glPushMatrix(); //Compute tx, ty, tz - translation from pBone to it's child (in local coordinate system of pBone) float tx = pBone->dir[0]*pBone->length; float ty = pBone->dir[1]*pBone->length; float tz = pBone->dir[2]*pBone->length; // Use the current ModelviewMatrix to display the current bone // Rotate the bone from its canonical position (elongated sphere // with its major axis parallel to X axis) to its correct orientation if(pBone->idx == root) glCallList(m_BoneList[skelNum] + pBone->idx); else { //translate to the center of the bone glTranslatef(tx/2.0, ty/2.0, tz/2.0); //Compute the angle between the canonical pose and the correct orientation //(specified in pBone->dir) using cross product. //Using the formula: r_axis = z_dir x pBone->dir v3_cross(z_dir, pBone->dir, r_axis); theta = GetAngle(z_dir, pBone->dir, r_axis); glRotatef(theta*180./M_PI, r_axis[0], r_axis[1], r_axis[2]);; glCallList(m_BoneList[skelNum] + pBone->idx); } glPopMatrix(); // Finally, add the translation component to the ModelviewMatrix // This step corresponds to doing: M_k+1 = ModelviewMatrix += T_k+1 glTranslatef(tx, ty, tz);}