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 );
}
}
