GenericMatrix IKSolver::jacobian(std::vector<Joint*>* bones, int type) { // Joint * effector = bones->back(); Joint * effector = bones->front(); Joint * joint = effector; //int joint_count=0; // if(!(root==NULL)){ /*while(joint!=NULL){ // if(root->getParent()==NULL){ // root->DrawObject(); // } joint_count++; joint = joint->parent(); }*/ // }else{ // joint_count=1; // } //joint = effector; GenericMatrix jacobian = GenericMatrix(3,3*bones->size()); qglviewer::Vec posEffector = effector->globalEffectorPosition(); for(unsigned int i = 0 ; i < bones->size() ; i++) { joint = bones->at(i); qglviewer::Vec derivatex,derivatey,derivatez; qglviewer::Vec posJoint = joint->globalPosition(); qglviewer::Vec posrelative = posEffector - posJoint; GenericMatrix globalTransform = joint->globalTransformationMatrix().transpose(); if(type!=2){ derivatex.setValue(globalTransform.get(0),globalTransform.get(1),globalTransform.get(2)); derivatey.setValue(globalTransform.get(4),globalTransform.get(5),globalTransform.get(6)); derivatez.setValue(globalTransform.get(8),globalTransform.get(9),globalTransform.get(10)); // Cross Product derivatex = derivatex^posrelative; derivatey = derivatey^posrelative; derivatez = derivatez^posrelative; double vetx[3] = {derivatex.x,derivatey.x,derivatez.x}; double vety[3] = {derivatex.y,derivatey.y,derivatez.y}; double vetz[3] = {derivatex.z,derivatey.z,derivatez.z}; for(int k=0;k<3;k++){ jacobian.set( 0 , k+(3*i) , vetx[k] ); jacobian.set( 1 , k+(3*i) , vety[k] ); jacobian.set( 2 , k+(3*i) , vetz[k] ); } }else{ posrelative = ( effector->globalEffectorPosition() - joint->globalPosition() ); jacobian.set(0, (3*i), 0); jacobian.set(0, 1+(3*i), posrelative.z); jacobian.set(0, 2+(3*i), posrelative.y); jacobian.set(1, (3*i), -posrelative.z); jacobian.set(1, 1+(3*i), 0); jacobian.set(1, 2+(3*i), posrelative.x); jacobian.set(2, (3*i), posrelative.y); jacobian.set(2, 1+(3*i), -posrelative.x); jacobian.set(2, 2+(3*i), 0); } } // jacobian.debugPrint("jacobian"); return jacobian; }