void IKTree::reset(int frame) { MT_Quaternion q; float rad; BVHNode *n; for (int i=0; i<numBones; i++) { bone[i].pos = origin; bone[i].lRot = identity; bone[i].gRot = identity; n = bone[i].node; for (int k=0; k<3; k++) { // rotate each axis in order rad = n->frame[frame][k] * M_PI / 180; q = identity; switch (n->channelType[k]) { case BVH_XROT: q.setRotation(xAxis, rad); break; case BVH_YROT: q.setRotation(yAxis, rad); break; case BVH_ZROT: q.setRotation(zAxis, rad); break; case BVH_XPOS: bone[i].pos[0] = n->frame[frame][k]; break; case BVH_YPOS: bone[i].pos[1] = n->frame[frame][k]; break; case BVH_ZPOS: bone[i].pos[2] = n->frame[frame][k]; break; } bone[i].lRot = q * bone[i].lRot; } } updateBones(0); }
void IKTree::reset(int frame) { MT_Quaternion q; BVHNode *n; for (int i=0; i<numBones; i++) { bone[i].pos = origin; bone[i].lRot = identity; bone[i].gRot = identity; n = bone[i].node; Rotation rot=n->frameData(frame).rotation(); Position pos=n->frameData(frame).position(); for (int k=0; k<n->numChannels; k++) { // rotate each axis in order q = identity; switch (n->channelType[k]) { case BVH_XROT: q.setRotation(xAxis, rot.x * M_PI / 180); break; case BVH_YROT: q.setRotation(yAxis, rot.y * M_PI / 180); break; case BVH_ZROT: q.setRotation(zAxis, rot.z * M_PI / 180); break; case BVH_XPOS: bone[i].pos[0] = pos.x; break; case BVH_YPOS: bone[i].pos[1] = pos.y; break; case BVH_ZPOS: bone[i].pos[2] = pos.z; break; } bone[i].lRot = q * bone[i].lRot; } /* for (int k=0; k<3; k++) { // rotate each axis in order rad = n->frame[frame][k] * M_PI / 180; q = identity; switch (n->channelType[k]) { case BVH_XROT: q.setRotation(xAxis, rad); break; case BVH_YROT: q.setRotation(yAxis, rad); break; case BVH_ZROT: q.setRotation(zAxis, rad); break; case BVH_XPOS: bone[i].pos[0] = n->frame[frame][k]; break; case BVH_YPOS: bone[i].pos[1] = n->frame[frame][k]; break; case BVH_ZPOS: bone[i].pos[2] = n->frame[frame][k]; break; } bone[i].lRot = q * bone[i].lRot; } */ } updateBones(0); }
void MT_ExpMap:: setRotation( const MT_Quaternion &q ) { // ok first normalize the quaternion // then compute theta the axis-angle and the normalized axis v // scale v by theta and that's it hopefully! m_q = q.normalized(); m_v = MT_Vector3(m_q.x(), m_q.y(), m_q.z()); MT_Scalar cosp = m_q.w(); m_sinp = m_v.length(); m_v /= m_sinp; m_theta = atan2(double(m_sinp),double(cosp)); m_v *= m_theta; }
void MT_ExpMap:: compute_dRdVi( const MT_Quaternion &dQdvi, MT_Matrix3x3 & dRdvi ) const { MT_Scalar prod[9]; /* This efficient formulation is arrived at by writing out the * entire chain rule product dRdq * dqdv in terms of 'q' and * noticing that all the entries are formed from sums of just * nine products of 'q' and 'dqdv' */ prod[0] = -MT_Scalar(4)*m_q.x()*dQdvi.x(); prod[1] = -MT_Scalar(4)*m_q.y()*dQdvi.y(); prod[2] = -MT_Scalar(4)*m_q.z()*dQdvi.z(); prod[3] = MT_Scalar(2)*(m_q.y()*dQdvi.x() + m_q.x()*dQdvi.y()); prod[4] = MT_Scalar(2)*(m_q.w()*dQdvi.z() + m_q.z()*dQdvi.w()); prod[5] = MT_Scalar(2)*(m_q.z()*dQdvi.x() + m_q.x()*dQdvi.z()); prod[6] = MT_Scalar(2)*(m_q.w()*dQdvi.y() + m_q.y()*dQdvi.w()); prod[7] = MT_Scalar(2)*(m_q.z()*dQdvi.y() + m_q.y()*dQdvi.z()); prod[8] = MT_Scalar(2)*(m_q.w()*dQdvi.x() + m_q.x()*dQdvi.w()); /* first row, followed by second and third */ dRdvi[0][0] = prod[1] + prod[2]; dRdvi[0][1] = prod[3] - prod[4]; dRdvi[0][2] = prod[5] + prod[6]; dRdvi[1][0] = prod[3] + prod[4]; dRdvi[1][1] = prod[0] + prod[2]; dRdvi[1][2] = prod[7] - prod[8]; dRdvi[2][0] = prod[5] - prod[6]; dRdvi[2][1] = prod[7] + prod[8]; dRdvi[2][2] = prod[0] + prod[1]; }
int main(int argc, char **argv) { const int seg_num = 5; const MT_Scalar seg_length = 15; const float seg_startA[3] = {0,0,0}; const float seg_startB[3] = {0,-20,0}; // create some segments to solve with // First chain ////////////// IK_Segment_ExternPtr const segmentsA = new IK_Segment_Extern[seg_num]; IK_Segment_ExternPtr const segmentsB = new IK_Segment_Extern[seg_num]; IK_Segment_ExternPtr seg_it = segmentsA; IK_Segment_ExternPtr seg_itB = segmentsB; { // MT_Quaternion qmat(MT_Vector3(0,0,1),-3.141/2); MT_Quaternion qmat(MT_Vector3(0,0,1),0); MT_Matrix3x3 mat(qmat); seg_it->seg_start[0] = seg_startA[0]; seg_it->seg_start[1] = seg_startA[1]; seg_it->seg_start[2] = seg_startA[2]; float temp[12]; mat.getValue(temp); seg_it->basis[0] = temp[0]; seg_it->basis[1] = temp[1]; seg_it->basis[2] = temp[2]; seg_it->basis[3] = temp[4]; seg_it->basis[4] = temp[5]; seg_it->basis[5] = temp[6]; seg_it->basis[6] = temp[8]; seg_it->basis[7] = temp[9]; seg_it->basis[8] = temp[10]; seg_it->length = seg_length; MT_Quaternion q; q.setEuler(0,0,0); MT_Matrix3x3 qrot(q); seg_it->basis_change[0] = 1; seg_it->basis_change[1] = 0; seg_it->basis_change[2] = 0; seg_it->basis_change[3] = 0; seg_it->basis_change[4] = 1; seg_it->basis_change[5] = 0; seg_it->basis_change[6] = 0; seg_it->basis_change[7] = 0; seg_it->basis_change[8] = 1; seg_it ++; seg_itB->seg_start[0] = seg_startA[0]; seg_itB->seg_start[1] = seg_startA[1]; seg_itB->seg_start[2] = seg_startA[2]; seg_itB->basis[0] = temp[0]; seg_itB->basis[1] = temp[1]; seg_itB->basis[2] = temp[2]; seg_itB->basis[3] = temp[4]; seg_itB->basis[4] = temp[5]; seg_itB->basis[5] = temp[6]; seg_itB->basis[6] = temp[8]; seg_itB->basis[7] = temp[9]; seg_itB->basis[8] = temp[10]; seg_itB->length = seg_length; seg_itB->basis_change[0] = 1; seg_itB->basis_change[1] = 0; seg_itB->basis_change[2] = 0; seg_itB->basis_change[3] = 0; seg_itB->basis_change[4] = 1; seg_itB->basis_change[5] = 0; seg_itB->basis_change[6] = 0; seg_itB->basis_change[7] = 0; seg_itB->basis_change[8] = 1; seg_itB ++; } int i; for (i=1; i < seg_num; ++i, ++seg_it,++seg_itB) { MT_Quaternion qmat(MT_Vector3(0,0,1),0.3); MT_Matrix3x3 mat(qmat); seg_it->seg_start[0] = 0; seg_it->seg_start[1] = 0; seg_it->seg_start[2] = 0; float temp[12]; mat.getValue(temp); seg_it->basis[0] = temp[0]; seg_it->basis[1] = temp[1]; seg_it->basis[2] = temp[2]; seg_it->basis[3] = temp[4]; seg_it->basis[4] = temp[5]; seg_it->basis[5] = temp[6]; seg_it->basis[6] = temp[8]; seg_it->basis[7] = temp[9]; seg_it->basis[8] = temp[10]; seg_it->length = seg_length; MT_Quaternion q; q.setEuler(0,0,0); MT_Matrix3x3 qrot(q); seg_it->basis_change[0] = 1; seg_it->basis_change[1] = 0; seg_it->basis_change[2] = 0; seg_it->basis_change[3] = 0; seg_it->basis_change[4] = 1; seg_it->basis_change[5] = 0; seg_it->basis_change[6] = 0; seg_it->basis_change[7] = 0; seg_it->basis_change[8] = 1; /////////////////////////////// seg_itB->seg_start[0] = 0; seg_itB->seg_start[1] = 0; seg_itB->seg_start[2] = 0; seg_itB->basis[0] = temp[0]; seg_itB->basis[1] = temp[1]; seg_itB->basis[2] = temp[2]; seg_itB->basis[3] = temp[4]; seg_itB->basis[4] = temp[5]; seg_itB->basis[5] = temp[6]; seg_itB->basis[6] = temp[8]; seg_itB->basis[7] = temp[9]; seg_itB->basis[8] = temp[10]; seg_itB->length = seg_length; seg_itB->basis_change[0] = 1; seg_itB->basis_change[1] = 0; seg_itB->basis_change[2] = 0; seg_itB->basis_change[3] = 0; seg_itB->basis_change[4] = 1; seg_itB->basis_change[5] = 0; seg_itB->basis_change[6] = 0; seg_itB->basis_change[7] = 0; seg_itB->basis_change[8] = 1; } // create the chains const int num_chains = 2; IK_Chain_ExternPtr chains[num_chains]; chains[0] = IK_CreateChain(); chains[1] = IK_CreateChain(); // load segments into chain IK_LoadChain(chains[0],segmentsA,seg_num); IK_LoadChain(chains[1],segmentsB,seg_num); // make and install a mouse handler MEM_SmartPtr<MyGlutMouseHandler> mouse_handler (MyGlutMouseHandler::New()); GlutMouseManager::Instance()->InstallHandler(mouse_handler); mouse_handler->SetChain(chains,num_chains); // make and install a keyhandler MEM_SmartPtr<MyGlutKeyHandler> key_handler (MyGlutKeyHandler::New()); GlutKeyboardManager::Instance()->InstallHandler(key_handler); // instantiate the drawing class MEM_SmartPtr<ChainDrawer> drawer (ChainDrawer::New()); GlutDrawManager::Instance()->InstallDrawer(drawer); drawer->SetMouseHandler(mouse_handler); drawer->SetChain(chains,num_chains); drawer->SetKeyHandler(key_handler); glutInit(&argc, argv); glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); glutCreateWindow("ik"); glutDisplayFunc(GlutDrawManager::Draw); glutMouseFunc(GlutMouseManager::Mouse); glutMotionFunc(GlutMouseManager::Motion); glutKeyboardFunc(GlutKeyboardManager::HandleKeyboard); init(MT_Vector3(-50,-50,-50),MT_Vector3(50,50,50)); glutMainLoop(); return 0; /* ANSI C requires main to return int. */ }
void IKTree::solveJoint(int frame, int i, IKEffectorList &effList) { double x, y, z; double ang = 0; MT_Quaternion q; MT_Quaternion totalPosRot = MT_Quaternion(0,0,0,0); MT_Quaternion totalDirRot = MT_Quaternion(0,0,0,0); MT_Vector3 axis(0,0,0); BVHNode *n; int numPosRot = 0, numDirRot = 0; if (bone[i].numChildren == 0) { // reached end site if (bone[i].node->ikOn) { effList.index[effList.num++] = i; } return; } for (int j=0; j<bone[i].numChildren; j++) { IKEffectorList el; el.num = 0; solveJoint(frame, bone[i].child[j], el); for (int k=0; k<el.num; k++) { effList.index[effList.num++] = el.index[k]; } } updateBones(i); for (int j=0; j<effList.num; j++) { int effIndex = effList.index[j]; n = bone[effIndex].node; MT_Vector3 effGoalPos(n->ikGoalPos[0], n->ikGoalPos[1], n->ikGoalPos[2]); const MT_Vector3 pC = (bone[effIndex].pos - bone[i].pos).safe_normalized(); const MT_Vector3 pD = (effGoalPos - bone[i].pos).safe_normalized(); MT_Vector3 rotAxis = pC.cross(pD); if (rotAxis.length2() > MT_EPSILON) { totalPosRot += MT_Quaternion(rotAxis, bone[i].weight * acos(pC.dot(pD))); numPosRot++; } const MT_Vector3 uC = (bone[effIndex].pos - bone[effIndex-1].pos).safe_normalized(); const MT_Vector3 uD = (MT_Vector3(n->ikGoalDir[0], n->ikGoalDir[1], n->ikGoalDir[2])).safe_normalized(); rotAxis = uC.cross(uD); if (rotAxis.length2() > MT_EPSILON) { double weight = 0.0; if (i == effIndex-1) weight = 0.5; totalDirRot += MT_Quaternion(rotAxis, weight * acos(uC.dot(uD))); numDirRot++; } } if ((numPosRot + numDirRot) > MT_EPSILON) { n = bone[i].node; n->ikOn = true; // average the quaternions from all effectors if (numPosRot) totalPosRot /= numPosRot; else totalPosRot = identity; if (numDirRot) totalDirRot /= numDirRot; else totalDirRot = identity; MT_Quaternion targetRot = 0.9 * totalPosRot + 0.1 * totalDirRot; targetRot = targetRot * bone[i].lRot; toEuler(targetRot, n->channelOrder, x, y, z); if (jointLimits) { bone[i].lRot = identity; for (int k=0; k<n->numChannels; k++) { // clamp each axis in order switch (n->channelType[k]) { case BVH_XROT: ang = x; axis = xAxis; break; case BVH_YROT: ang = y; axis = yAxis; break; case BVH_ZROT: ang = z; axis = zAxis; break; default: break; } // null axis leads to crash in q.setRotation(), so check first if(axis.length()) { if (ang < n->channelMin[k]) ang = n->channelMin[k]; else if (ang > n->channelMax[k]) ang = n->channelMax[k]; q.setRotation(axis, ang * M_PI / 180); bone[i].lRot = q * bone[i].lRot; } } } else bone[i].lRot = targetRot; } }