void IKSolver::solve(std::vector<Joint*>* bones, qglviewer::Vec goal, int type)
{
//cout << type << endl;
Joint* effector = bones->front();
qglviewer::Vec posEffector = effector->globalEffectorPosition();
if ((goal-posEffector).norm()<GOAL_DISTANCE_ERROR) return;
Joint *root;
root = effector;
// int joint_count=0;
// if(!(root==NULL)){
// while(root!=NULL){
// joint_count++;
// root = root->parent();
// }
// }else{
// joint_count=1;
// }
// root = effector;
GenericMatrix jacobianMatrix = pseudoJacobian(bones,type);
qglviewer::Vec e;
if(type==1){
if((goal-posEffector).norm()<MAX_DISTANCE_FRAME){
return;
}else{
e = ((goal-posEffector)*MAX_DISTANCE_FRAME);
//e = ( ((goal-posEffector)*MAX_DISTANCE_FRAME) / ((goal-posEffector).norm()) );
}
}else{
e = (goal-posEffector)*D;
}
GenericMatrix position = GenericMatrix(3,1);
position.set( 0 , 0 , e.x);
position.set( 1 , 0 , e.y);
position.set( 2 , 0 , e.z);
GenericMatrix application = (jacobianMatrix * position);
// application.debugPrint("application");
int i = 0;
for(int j = 0 ; j < bones->size() ; j++ ) {
// Smoothing Factor s
Joint* root = bones->at(j);
double s=1;
double angle;
qglviewer::Vec vector;
qglviewer::Quaternion qresult = root->orientation();
if(type == 2){
vector = qglviewer::Vec(1,0,0);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qglviewer::Quaternion(vector,angle) * qresult;
i++;
vector = qglviewer::Vec(0,1,0);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qglviewer::Quaternion(vector,angle) * qresult;
i++;
vector = qglviewer::Vec(0,0,1);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qglviewer::Quaternion(vector,angle) * qresult;
i++;
}else{
vector = qglviewer::Vec(1,0,0);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qresult * qglviewer::Quaternion(vector,angle);
i++;
vector = qglviewer::Vec(0,1,0);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qresult * qglviewer::Quaternion(vector,angle);
i++;
vector = qglviewer::Vec(0,0,1);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qresult * qglviewer::Quaternion(vector,angle);
i++;
}
root->setNewOrientation( qresult );
}
}
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;
}
