Rotation.cpp

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Micha Hersch, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #include "Rotation.h"
00020 
00021 
00022 //#define RODRIGUES
00023 
00024 Rotation::Rotation(){
00025   base_eps_b = eps_b =0.005;
00026   base_eps_a = eps_a =0.005;
00027   alpha = 1;
00028   norm = 0;
00029   v_clear(beta);
00030   v_clear(axis);
00031   axis_flag=0;
00032 }
00033 
00034 Rotation::~Rotation(){}
00035 
00036 void Rotation::Copy(const Rotation& r){
00037   r.GetRotationParam(beta);
00038   alpha = GetAlpha();
00039   SetAxis();
00040 }
00041 
00042 int Rotation::GetRotationAxis(CVector3_t out) const
00043 {
00044   if(AxisOk()){
00045     v_copy(axis,out);
00046     return 1;
00047   }
00048   else{
00049     int ret = SetAxis();
00050     if(ret){
00051       v_copy(axis,out);
00052     }
00053     return ret;
00054   }
00055 }
00056 
00057 
00058 
00059 float Rotation::GetRotationAngle() const
00060 {
00061   float tmp;
00062   int si;
00063   tmp = v_length(beta);
00064   if(tmp>1){
00065     cout<<"error: beta is bigger than one"<<endl;
00066     return 0.0;
00067   }
00068   else{
00069     if(!AxisOk()){
00070       cout<<"error: axis direction is not defined"<<endl;
00071     }
00072     si = sign(v_dot(beta,axis));
00073     return si*2.0 *(float)asin(tmp);
00074   }
00075 }
00076 
00080 int Rotation::SetTransfo(CVector3_t new_beta){
00081   float sq_len = v_squ_length(new_beta);
00082   if(sq_len>1){
00083     return 0;
00084   }
00085   //  norm = sq_len;
00086   alpha = sqrt(1-sq_len);
00087   v_copy(new_beta,beta);
00088   axis_flag =0;
00089   return 1;
00090 }
00091 
00095 void Rotation::SetQuaternion(CQuat_t q){
00096   v_copy(q,beta);
00097   alpha = q[3];
00098   axis_flag =0;
00099 }
00100 
00101 
00102 void Rotation::TransformRotation(const CVector3_t beta_in, CVector3_t beta_out){
00103     CQuat_t q1,q2,q3;
00104     q_complete(beta,q1);
00105     q_complete(beta_in,q2);
00106     q_multiply(q2,q1,q3);
00107     v_copy(q3,beta_out);
00108 }
00109 
00110 
00122 float Rotation::AimingAngle(const CVector3_t tar,const CVector3_t with){
00123   CVector3_t vec;
00124   float f1,f2,f3,f4,angle,c;
00125   if(!AxisOk()){
00126     SetAxis();
00127   }
00128   v_cross(axis,with,vec);
00129   f1= v_dot(vec,tar);
00130   f2 = v_dot(with,axis);
00131   f3 = v_dot(tar,axis);
00132   f4 = v_dot(tar,with);
00133   f2 = f4-f2*f3;
00134   angle = atan2(f1,f2);
00135   c= cos(angle);
00136   if(c*f2+(f1*f1)/f2<0){ //checking it is a minimization
00137     angle = pi+angle;
00138   }
00139   return angle>pi?angle-2*pi:angle; //putting back to [-pi pi]
00140 }
00141 
00146 void Rotation::MinimizeRotation(Rotation& rot){
00147   CQuat_t b1;
00148   rot.GetQuaternion(b1);
00149   MinimizeRotation(b1);
00150 }
00151 
00155 void Rotation::MinimizeRotation(const CQuat_t q){
00156   float num,den,a1,angle;
00157   CVector3_t v,b1;//rotation param of rot
00158  if(!AxisOk()){
00159     SetAxis();
00160   }
00161   v_copy(q,b1);
00162   a1 = q[3];
00163   //  a1 =sqrt(1-v_dot(b1,b1));;
00164   num=-2*a1*v_dot(axis,b1);
00165   v_cross(b1,beta,v);//it should be axis instead of beta
00166   den = 2*a1*a1-1+v_squ_length(v);
00167   angle = atan2(num,den);
00168  
00169 //  s= sin(angle/2);
00170 //  c= sin(angle/2);
00171 //   if(-2*(s/c)*num+(2*s/c-s/(c*c*c))*den < 0 && 0){ //checking it is a minimaization
00172 //     angle = pi+angle;
00173 //     cout<<"+";
00174 //   }
00175 //   cout<<" angle "<<angle*rad2deg<<endl;
00176 
00177   SetAngle(angle);
00178 }
00179 
00180 //#define FF
00181 
00182 float Rotation::MinimizePositionAndRotationAngle(const CVector3_t tar,const CVector3_t with, const CQuat_t q,float k){
00183   float num_rot,den_rot,num_pos,den_pos,angle,num,den,c,s;
00184   CVector3_t vec;
00185   if(!AxisOk()){
00186     SetAxis();
00187   }
00188   //rotation
00189 
00190 
00191 
00192 #ifndef FF
00193   v_cross(q,axis,vec);// v_cross(q,beta,vec);
00194   num_rot=0.5*q[3]*v_dot(axis,q);//-2
00195   den_rot = -0.25*(2*q[3]*q[3]-1+v_squ_length(vec));
00196   
00197   //position
00198   v_cross(axis,with,vec); 
00199   num_pos= -v_dot(vec,tar);
00200   den_pos =-v_dot(tar,with) +  v_dot(tar,axis) * v_dot(with,axis);
00201   
00202 #else
00203   v_cross(q,axis,vec);// v_cross(q,beta,vec);
00204   num_rot=-1*q[3]*v_dot(axis,q);//-2
00205   den_rot = 0.5*(2*q[3]*q[3]-1+v_squ_length(vec));
00206   
00207   //position
00208   v_cross(axis,with,vec); 
00209   num_pos= v_dot(vec,tar);
00210   den_pos =v_dot(tar,with) -  v_dot(tar,axis) * v_dot(with,axis);
00211 
00212 #endif
00213 
00214 
00215  
00216   // 
00217 
00218   num =k*num_pos+(1-k)*num_rot;
00219   den = k*den_pos+(1-k)*den_rot;
00220   
00221 
00222 angle = atan2(num,den); 
00223  
00224   // check that it's a minimization
00225    s =sin(angle);
00226    c= cos(angle);
00227    //  if(c*den+(num*num)/den<0){
00228    
00229    if(-s*num-c*den<0){
00230 #ifndef FF     
00231      angle += pi;
00232 #endif
00233      //     cout<<"not ok ";
00234    }
00235    //   cout<<0.001*((int)(1000*(angle-GetAngle()))%int(2000*pi))<<" ";
00236   return angle>pi?angle-2*pi:angle; //putting back to [-pi pi]
00237 
00238 }
00239 
00240 void Rotation::SetRate(float rate){
00241   base_eps_b = eps_b = rate;
00242 }
00243 
00244 void Rotation::ScaleRate(float factor){
00245   eps_b = factor*base_eps_b;
00246 }
00247 
00248 
00249 void Rotation::Derivative(CMatrix3_t out)const{
00250   float f1;
00251   CMatrix3_t m1,m2;
00252 
00253   f1= v_dot(beta,beta);//beta'*beta
00254   m_identity(m1);//I
00255   m_rescale(1-2*f1,m1,m1);// (1-2beta'*beta)*I
00256   ComputeMatrixB(beta,m2);//beta|
00257   m_rescale(2*sqrt(1-f1),m2,m2); // 2*sqrt(1-beta'*beta)beta|
00258   m_sub(m1,m2,m2);// (1-2beta'*beta)*I- 2*sqrt(1-beta'*beta)beta|
00259   v_mult(beta,beta,m1);
00260   m_rescale(2,m1,m1);
00261   m_add(m2,m1,out);//(1-2beta'*beta)*I- 2*sqrt(1-beta'*beta)beta|+2beta*beta'
00262 }
00263 
00264 
00265 void Rotation::AngleDerivative(CVector3_t v, CVector3_t out){
00266   CVector3_t v1,v2,v3;
00267   float f;               // b = axis
00268   CheckAxis();
00269   v_scale(v,-2*norm*alpha,v1);//-4k*v
00270   f=(1-2*norm*norm);//(1-2*k^2)
00271   v_cross(axis,v,v2); //bxv
00272   v_scale(v2,f,v2); //2*(1-2*k^2)/sqrt(1-k^2)*(b x v)
00273   v_add(v1,v2,v3);;//-4k*v + (1-2*k^2)/sqrt(1-k^2)*(b x v)
00274   f=2*alpha*norm*v_dot(axis,v);//2*alpha*k*b'*v
00275   v_scale(axis,f,v1);
00276   v_add(v3,v1,out); //-4k*v+2*(1-2*k^2)/sqrt(1-k^2)*(b x v)+ 4*k*(b'*v)*b
00277 
00278 
00279 }
00280 
00281 
00282 void Rotation::NormDerivative(CVector3_t v, CVector3_t out){
00283   CVector3_t v1,v2,v3;
00284   float f;               // b = axis
00285   CheckAxis();
00286   if(alpha>epsilon){
00287     v_scale(v,-4*norm,v1);//-4k*v
00288     f= 2*(1-2*norm*norm)/alpha;//2*(1-2*k^2)/sqrt(1-k^2)
00289     v_cross(axis,v,v2); //bxv
00290     v_scale(v2,f,v2); //2*(1-2*k^2)/sqrt(1-k^2)*(b x v)
00291     v_add(v1,v2,v3);;//-4k*v + (1-2*k^2)/sqrt(1-k^2)*(b x v)
00292     f=4*norm*v_dot(axis,v);//4*k*b'*v
00293     v_scale(axis,f,v1);
00294     v_add(v3,v1,out); //-4k*v+2*(1-2*k^2)/sqrt(1-k^2)*(b x v)+ 4*k*(b'*v)*b
00295   }
00296   else{ //alpha is zero
00297      v_cross(axis,v,v2);
00298      v_scale(v2,-1,out);
00299   }
00300   //  coutvec(out);
00301 }
00302 
00307 void Rotation::Jacobian(CMatrix3_t out)const{
00308   CMatrix3_t m1,m2;
00309   float f;
00310   CheckAxis();
00311   m_identity(out);
00312   ComputeMatrixB(axis,m1);
00313   f=-(1-2*norm*norm);// - I think because computeMatrixB yields axis|
00314   m_rescale(f,m1,m1);
00315   f=2*norm*alpha;
00316   m_rescale(f,out,m2);
00317   m_sub(m1,m2,m1);
00318   v_mult(axis,axis,m2);
00319   m_rescale(f,m2,m2);
00320   m_add(m1,m2,out);
00321 }
00322 
00326 void Rotation::InverseNormDerivative(CVector3_t v, CVector3_t out){
00327   CVector3_t v1,v2,v3;
00328   float f;               // b = axis
00329   if(!AxisOk()){
00330     SetAxis();
00331   }
00332  if(alpha>epsilon){
00333     v_scale(v,-4*norm,v1);//-4k*v
00334     f= -2*(1-2*norm*norm)/alpha;//-2*(1-2*k^2)/sqrt(1-k^2) only the sine part is inverted
00335     v_cross(axis,v,v2); //bxv
00336     v_scale(v2,f,v2); //2*(1-2*k^2)/sqrt(1-k^2)*(b x v)
00337     v_add(v1,v2,v3);;//-4k*v + (1-2*k^2)/sqrt(1-k^2)*(b x v)
00338     f=4*norm*v_dot(axis,v);//4*k*b'*v
00339     v_scale(axis,f,v1);
00340     v_add(v3,v1,out); //-4k*v-(1-2*k^2)/sqrt(1-k^2)*(b x v)+ 4*k*(b'*v)*b
00341   }
00342   else{ //alpha is zero
00343      v_cross(axis,v,v2);
00344      v_scale(v2,-1,out);
00345   }
00346 }
00347 
00348 
00349 void Rotation::BetaDerivative(CVector3_t v, CMatrix3_t out)const{
00350   CMatrix3_t B,C,m1; 
00351   CVector3_t v1;
00352   ComputeMatrixC(v,C);
00353   ComputeMatrixB(v,B);
00354 //   cout<<"C: "<<C<<endl;
00355 //   cout<<"B: "<<B<<endl;
00356   v_cross(beta,v,v1);//beta x v
00357   v_mult(v1,beta,m1);//(beta x v)*beta'
00358   if(alpha>epsilon){
00359     m_rescale(-2.0/alpha,m1,m1);//2/alpha * (beta x v)*beta'
00360     m_rescale(2*alpha,B,B);// 2*alpha*B
00361     m_add(m1,B,m1);// 
00362     m_sub(m1,C,out); 
00363   }
00364   else{//only that component
00365     m_rescale(-1,m1,out);
00366   }
00367 }
00368 
00369 
00370 
00371 
00372 void Rotation::AxisDerivative(CVector3_t v, CMatrix3_t out){
00373   CMatrix3_t B,m1,m2;
00374   float f;
00375   if(!AxisOk()){
00376     SetAxis();
00377   }
00378   ComputeMatrixB(v,B);
00379   m_rescale(2*norm*alpha,B,B); //2*k*sqrt(1-k²)*B
00380   v_mult(axis,v,m1); //b*v'
00381   f=v_dot(axis,v); //b'*v
00382   m_identity(m2);   
00383   m_rescale(f,m2,m2);//b'*v*I
00384   m_add(m1,m2,m1);// b*v+b'v*I
00385   m_rescale(2*norm*norm,m1,m1);//2k²*( b*v'+b'v*I )
00386   m_add(B,m1,out); //2*k*sqrt(1-k²)*B + 2k²*( b*v'+b'v*I )
00387 }
00388 
00392 void Rotation::InverseAxisDerivative(CVector3_t v, CMatrix3_t out){
00393   CMatrix3_t B,m1,m2;
00394   float f;
00395   if(!AxisOk()){
00396     SetAxis();
00397   }
00398   ComputeMatrixB(v,B);
00399   m_rescale(-2*norm*alpha,B,B); //-2*k*sqrt(1-k²)*B
00400   v_mult(axis,v,m1); //b*v'
00401   f=v_dot(axis,v); //b'*v
00402   m_identity(m2);   
00403   m_rescale(f,m2,m2);//b'*v*I
00404   m_add(m1,m2,m1);// b*v+b'v*I
00405   m_rescale(2*norm*norm,m1,m1);//2k²*( b*v'+b'v*I )
00406   m_add(B,m1,out); //-2*k*sqrt(1-k²)*B + 2k²*( b*v'+b'v*I )
00407 }
00408 
00409 
00410 void Rotation::AngleAxisDerivative(CVector3_t v, CMatrix3_t out){
00411   CMatrix3_t B,m1,m2;
00412   float f;
00413   if(!AxisOk()){
00414     SetAxis();
00415   }
00416   ComputeMatrixB(v,B);
00417   m_rescale(1-2*norm*norm,B,B); //1-2*k²*B = (cos(theta)B
00418   v_mult(axis,v,m1); //b*v'
00419   f=v_dot(axis,v); //b'*v
00420   m_identity(m2);   
00421   m_rescale(f,m2,m2);//b'*v*I
00422   m_add(m1,m2,m1);// b*v+b'v*I
00423   m_rescale(2*alpha*norm,m1,m1);//2k*alpha*( b*v'+b'v*I ) = sin(theta)*(...)
00424   m_add(B,m1,out); //-2*k*sqrt(1-k²)*B + 2k²*( b*v'+b'v*I )
00425 }
00426 
00427 void Rotation::AddToAxis(CVector3_t daxis){
00428   float l,f;
00429   if(!AxisOk()){
00430     SetAxis();
00431   }
00432   l = v_normalize(daxis,daxis);
00433   f = min(l*eps_a,pi/180);
00434   v_scale(daxis,f,daxis);
00435   v_add(axis,daxis,axis);
00436   if(v_normalize(axis,axis)<epsilon){
00437     cout<<"axis undefined"<<endl;
00438   }
00439   v_scale(axis,norm,beta);
00440 }
00441 
00442 
00443 void Rotation::AddToNorm(float dnorm){
00444   //  cout<<" "<<dnorm;
00445   norm = norm+dnorm;
00446 
00447   while(abs(norm)>(1+1e-2)){
00448     dnorm *= 0.5;
00449     norm -=dnorm;
00450   }
00451 
00452   if(abs(norm)>1 && abs(norm)<(1+1e-2)){
00453     norm = norm-sign(norm)*2;
00454   }
00455   if(!AxisOk()){
00456     SetAxis();
00457   }
00458 
00459   v_scale(axis,norm,beta);
00460   alpha = sqrt(1-norm*norm);
00461 }
00462 
00463 void Rotation::Add(CVector3_t dbeta){
00464   float dbeta_len,beta_len,f;
00465   dbeta_len = v_normalize(dbeta,dbeta);
00466   f =  min(dbeta_len*eps_b,pi/180);
00467   v_scale(dbeta,f,dbeta);
00468   //  v_scale(dbeta,eps_b,dbeta);
00469   v_add(beta,dbeta,beta);//v_add
00470   cout<<"beta ";coutvec(beta);
00471   beta_len = v_length(beta);
00472   if(beta_len>1 && beta_len<(1+1e-2)){  //closing the manifold
00473     v_scale(beta,beta_len-2,beta);
00474     cout<<"cross"<<endl;
00475   }
00476   while(v_length(beta)>(1+1e-2)){
00477     v_scale(dbeta,0.5,dbeta);
00478     v_sub(beta,dbeta,beta);   
00479   }
00480   SetTransfo(beta);
00481 }
00482 
00483 void Rotation::Update(CVector3_t v, CVector3_t v_tr){
00484   // PD = -2/sqrt(1-p_beta'*p_beta) * cross(p_beta,x)*p_beta' +...
00485   //         2*sqrt(1-p_beta'*p_beta)*B-C;
00486   CVector3_t y,v1,dbeta;
00487   CMatrix3_t B,C,m1; 
00488   float beta_len,dbeta_len;
00489   //BETA_DERIVATIVE  
00490   ComputeMatrixC(v,C);
00491   ComputeMatrixB(v,B);
00492 //   cout<<"C: "<<C<<endl;
00493 //   cout<<"B: "<<B<<endl;
00494   v_cross(beta,v,v1);//beta x v
00495   v_mult(v1,beta,m1);//(beta x v)*beta'
00496   if(alpha>1e-7){
00497     m_rescale(-2.0/alpha,m1,m1);//2/alpha * (beta x v)*beta'
00498     m_rescale(2*alpha,B,B);// 2*alpha*B
00499     m_add(m1,B,m1);// 
00500     m_sub(m1,C,m1); 
00501   }
00502   //
00503 
00504   Transform(v,y);
00505   v_sub(v_tr,y,v1); // (y-RvR)
00506   v_transform_normal2(v1,m1,dbeta);
00507 
00508   // UPDATE
00509   // fixed increment size
00510   dbeta_len = v_length(dbeta);
00511   v_scale(dbeta,eps_b/dbeta_len,dbeta);
00512   //  v_scale(dbeta,eps_b,dbeta);
00513   v_add(beta,dbeta,beta);//v_add
00514   beta_len = v_length(beta);
00515   if(beta_len>1 && beta_len<(1+1e-2)){  //closing the manifold
00516     v_scale(beta,beta_len-2,beta);
00517     cout<<"cross"<<endl;
00518   }
00519   while(v_length(beta)>(1+1e-2)){
00520     v_scale(dbeta,0.5,dbeta);
00521     v_sub(beta,dbeta,beta);   
00522   }
00523 //   while((v_length(beta)>(1+1e-7))){ //avoids beta>1
00524 //     // cout<<(v_length(beta)>1+1e-10)<<endl;
00525 //     v_scale(dbeta,0.5,dbeta);
00526 //     v_sub(beta,dbeta,beta);//v_sub
00527 //   }
00528   alpha = sqrt(1-v_squ_length(beta));
00529 } 
00530 
00531 
00532 
00533 
00534 #ifndef OLD_UPDATE_AXIS
00535 
00541 void Rotation::UpdateAxis(CVector3_t v, CVector3_t v_tr, float angle){
00542   CMatrix3_t B,m1,m2;
00543   CVector3_t v1,y,db;
00544   float f;//,d_phi,d_psi;
00545 
00546 
00547 
00548  SetAngle(angle);
00549   if(!AxisOk()){
00550     SetAxis();
00551   }
00552 
00553 
00554  // computing d/daxis of euclidean distance (y-RvR)²
00555  ComputeMatrixB(v,B);
00556  m_rescale(2*norm*alpha,B,B); //2*k*sqrt(1-k²)*B
00557  v_mult(axis,v,m1); //b*v'
00558  f=v_dot(axis,v); //b'*v
00559  m_identity(m2);   
00560  m_rescale(f,m2,m2);//b'*v*I
00561  m_add(m1,m2,m1);// b*v+b'v*I
00562  m_rescale(2*norm*norm,m1,m1);//2k²*( b*v'+b'v*I )
00563  m_add(B,m1,m1); //2*k*sqrt(1-k²)*B + 2k²*( b*v'+b'v*I )
00564 
00565  Transform(v,y);
00566 //  cout<<"from " ;coutvec(v);
00567 //  cout<<"to " ;coutvec(y);
00568 //  cout<<"instead of " ;coutvec(v_tr);
00569   v_sub(v_tr,y,v1); // (y-RvR)
00570 //  cout<<"diff "<<v1<<endl;
00571 //  // v_normalize(v1,v1);//should not have an influence
00572   v_transform_normal2(v1,m1,db);
00573   f = v_length(db);
00574   f=f>pi/180?eps_a/f:eps_a;
00575   cout<<"norm "<<f<<endl;
00576   v_scale(db,f,db);
00577   v_add(axis,db,axis);
00578 
00579   f = v_normalize(axis,axis);
00580   axis_flag = 1;
00581   SetAngle(angle);
00582     
00583 //  cout<<"db ";coutvec(db);
00584 //  v_set(-sin(phi)*cos(psi), cos(phi)*cos(psi),0,v1);//db/dphi
00585 //  v_set(-cos(phi)*sin(psi),-sin(phi)*sin(psi),cos(psi),v2);//db/dpsi
00586  
00587 //  d_phi = v_dot(v1,db);
00588 //  d_psi = v_dot(v2,db);
00589  
00590 //  cout<<"d_phi "<<d_phi<<" d_psi "<<d_psi<<endl;
00591 //  while((f=d_phi*d_phi+d_psi*d_psi)>eps_a){
00592 //    d_phi /=2.0f;
00593 //    d_psi /=2.0f;
00594 //  }
00595  
00596 //  phi += d_phi;
00597 //  psi += d_psi;
00598 //  v_set(cos(phi)*cos(psi),sin(phi)*cos(psi),sin(psi),axis);
00599 }
00600 
00601 #else
00602 
00603 #ifndef OLD_UPDATE_AXIS2
00604 void Rotation::UpdateAxis(CVector3_t v, CVector3_t v_tr, float angle){
00605   CVector3_t diff,v1,v2,c;
00606   CVector4_t q1;
00607   float f,rn,d,d1,d2,l; 
00608   //  CMatrix3_t A,B,C;
00609   cout<<"v ";coutvec(v);
00610   cout<<"v_tr ";coutvec(v_tr);
00611   v_sub(v_tr,v,diff);
00612   v_scale(diff,0.5,diff);
00613   v_add(v,diff,c); //center of circle
00614   if((l=v_normalize(diff,diff))){
00615     cout<<"l "<<l<<endl;
00616     if(angle<0){v_scale(diff,-1,diff);}
00617     rn = l/tan(angle/2); //radius of circle
00618     if(rn>epsilon){
00619 
00620 //       f = v_dot(c,axis);
00621 //       v_scale(axis,f,r);
00622 //       v_sub(r,c,r);//(b'*c)b-c -(c minus its projection on present axis)
00623 //       v_normalize(r,r);
00624 //       v_cross(r,axis,n); //pre-image of diff
00625 //       cout<<"n ";coutvec(n);
00626 //       cout<<"diff ";coutvec(diff);
00627 
00628       d= v_normalize(c,v1); // local 2d base vector 1
00629       
00630       d1 = rn*rn/d;
00631       //d1 = (d*d-rn*rn)/d; // height of top of cerf-volant
00632       //d2 = rn*sqrt(d*d-rn*rn)/d; // heigth of rectangular triangle
00633       f = (d*d)-(rn*rn);
00634       d2 = f>=0?sqrt(f)*(rn/d):0;
00635       v_cross(v1,diff,v2);//local 2d base vector 2
00636 //       cout<<"d  "<<d<<" d1 "<<d1<<" d2 "<<d2<<endl;
00637 //       cout<<"v2 ";coutvec(v2);
00638      //  v_scale(v1,d1,v1);
00639 //       v_sub(c,v1,c1);
00640 //       v_scale(v2,d2,v1);
00641 //       v_add(c1,v1,c);// rotation point on the axis
00642 //         cout<<"rot point 1 ";coutvec(c);
00643 //       if(v_length(c)<0.001*epsilon){//0  the rotation point
00644 //      v_copy(v2,c);
00645 //       }
00646 //        cout<<"rot point ";coutvec(c);
00647 //       v_normalize(c,c);// new (desired) axis
00648 //       cout<<"des axis ";coutvec(c);
00649       v_scale(v1,d1,v1);
00650       v_scale(v2,d2,v2);
00651       v_add(v1,v2,v1);
00652       v_cross(v1,diff,c);
00653       v_normalize(c,c);// new (desired) axis
00654       cout<<"des axis ";coutvec(c);
00655       v_cross(axis,c,v1);
00656       f = v_dot(axis,c);
00657       if(abs(f-1)>epsilon){
00658         f = acos(f);
00659         q_set(v1,eps_a*f,q1); // we only take a small rotation
00660         q_v_mult(q1,axis,v1); //rotating the axis
00661         v_normalize(v1,axis); // v_copy should theoretically be enough
00662         v_scale(axis,norm,beta);
00663       }
00664    //    m_set_v3_column(r,0,A);
00665 //       m_set_v3_column(axis,1,A);
00666 //       m_set_v3_column(n,2,A);
00667 
00668 //       v_cross(diff,c,r); //can be taken somewhere else
00669 // ;
00670 //       m_set_v3_column(r,0,B);
00671 //       m_set_v3_column(c,1,B);
00672 //       m_set_v3_column(diff,2,B);
00673 
00674 //       m_inverse(A,C);//A^-1
00675 //       m_multiply(B,C,A);//R=B*Ai rotation matrix
00676       
00677 //       f= m_rotation_axis(A,v1);
00678     }
00679   }
00680 }
00681 
00682 
00683 
00684 
00685 #else
00686 
00690 void Rotation::UpdateAxis(CVector3_t v, CVector3_t v_tr, float angle){
00691   CVector3_t dax,diff,c,r,n;
00692   float f;
00693 
00694   if(!AxisOk()){
00695     SetAxis();
00696   }
00697   v_sub(v_tr,v,diff);
00698   v_scale(diff,2,diff);
00699   v_add(v,diff,c); //center 
00700   if(v_normalize(diff,diff)){
00701     if(angle<0){v_scale(diff,-1,diff);}
00702     f = v_dot(c,axis);
00703     v_scale(axis,f,r);
00704     v_sub(r,c,r);//(b'*c)b-c 
00705     v_cross(axis,r,n);
00706     // v_cross(r,axis,n);
00707     v_normalize(n,n);
00708     v_cross(n,diff,c);//rotation axis for axis
00709     //   if(v_squ_length(c)>1e-6)
00710     v_copy(axis,n);//backup;
00711     v_scale(c,eps_a,beta);
00712     alpha = sqrt(1-v_dot(beta,beta));
00713     Rotation::Transform(n,axis);
00714 //     v_cross(c,axis,dax);
00715 //     v_scale(dax,eps_a,dax);
00716 //     v_add(axis,dax,axis);
00717     v_normalize(axis,axis);
00718     v_scale(axis,norm,beta);
00719   }
00720 }
00721 #endif
00722 
00723 #endif
00724 
00725 
00726 
00727 
00728 
00729 
00730 int Rotation::SetAngle(float angle){
00731   if(angle>pi){
00732     return SetAngle(angle-2*pi)&& 0;
00733   }
00734   if(angle<=-pi){
00735     return SetAngle(angle+2*pi)&& 0;
00736   }
00737  //setting the rotation amplitude
00738   if(!AxisOk()){ //do this before modifying norm
00739    SetAxis();
00740  }
00741 
00742  norm = sin(angle/2.0f);
00743  alpha = cos(angle/2.0f);
00744  float n=norm;
00745 // n = norm/v_length(axis);//axis should already be scaled 
00746 //  assert(!isnan(n));
00747  v_scale(axis,n,beta);
00748 
00749  return 1;
00750 }
00751 
00752 
00753 int Rotation::SetRotationParam(CVector3_t nbeta){
00754   float f = v_squ_length(nbeta);
00755   if(f>1){
00756     return 0;
00757   }
00758   else{
00759     v_copy(nbeta,beta);  
00760     alpha = sqrt(1-f);
00761     axis_flag=0;
00762     return 1;
00763   }
00764 
00765 }
00766 
00767 
00768 int Rotation::SetRotationAxis(const CVector3_t a){
00769   if(v_normalize(a,axis)>epsilon){
00770     //    psi=asin(axis[2]);
00771     //phi=atan2(axis[1],axis[0]);
00772     v_scale(axis,norm,beta);
00773     axis_flag=1;
00774     return 1;
00775   }
00776   else{
00777     axis_flag=0;
00778     return 0;
00779   }
00780 }
00781 
00782 void Rotation::RandomAxis(){
00783   CVector3_t v;
00784   do{
00785     for(int i=0;i<3;i++){
00786       v[i] = sign(RND(1)-0.5)*RND(1);
00787     }
00788   } while(!SetRotationAxis(v));
00789 }
00790 
00791 void Rotation::ComputeMatrixC(const CVector3_t v, CMatrix3_t out)const{
00792   CMatrix3_t m1,m2;
00793   float f;
00794   f=v_dot(beta,v);
00795   v_mult(v,beta,m1);
00796   m_rescale(2.0,m1,m2);
00797   v_mult(beta,v,m1);
00798   m_sub(m2,m1,m1);
00799   m_identity(m2);
00800   m_rescale(f,m2,m2);
00801   m_sub(m1,m2,out);
00802   m_rescale(2,out,out); // new correct version
00803 }
00804 
00805 
00811 void Rotation::ComputeMatrixB(const CVector3_t v, CMatrix3_t out)const{
00812   CVector3_t v1;
00813   //  cout<<"v: ";coutvec(v);
00814   v_set(0,-v[2],v[1],v1);
00815   m_set_v3_column(v1,0,out);
00816 
00817   v_set(v[2],0,-v[0],v1);
00818   m_set_v3_column(v1,1,out);
00819 
00820   v_set(-v[1],v[0],0,v1);
00821   m_set_v3_column(v1,2,out);
00822 }
00823 
00824 
00825 
00826 void Rotation::RotFromMatrix(const CMatrix3_t m){
00827   CVector3_t v;
00828   float angle = m_rotation_axis(m,v); //should be normalized already
00829   v_normalize(v,axis);
00830   norm = sin(angle/2);
00831   v_scale(axis,norm,beta);
00832   alpha = cos(angle/2);
00833   axis_flag=1;
00834 }
00835 
00836 
00837 //  p_alpha^2*x + 2*p_alpha*cross(p_beta,x) + ...
00838 //         (p_beta'*x)*p_beta - cross(cross(p_beta,x),p_beta)+p_trans;
00839 void Rotation::Transform(const CVector3_t in, CVector3_t out)const{
00840 CVector3_t v1,v2,v3;
00841   float f1;
00842   f1 = alpha*alpha;
00843   v_scale(in,f1,v1); //alpha^2*v
00844   v_cross(beta,in,v2); //beta x v
00845   v_scale(v2,2*alpha,v3); // 2*alpha*(beta x v)
00846   v_add(v1,v3,v3); // alpha^2*v+2*alpha*(beta x v)
00847   f1 = v_dot(beta,in); //beta'*v
00848   v_scale(beta,f1,v1); //(beta'*v)*beta 
00849   v_add(v3,v1,v3);// alpha^2*v+2*alpha*(beta x v)+(beta'*v)*beta 
00850   v_cross(v2,beta,v1); //(beta x v) x beta
00851   v_sub(v3,v1,out);//alpha^2*v+2*alpha*(beta x v)+(beta'*v)*beta -(beta x v) x beta
00852 
00853 }
00854 
00855 // should be eqivalent to Derivative, also valid for homogeneous matrices
00856 void Rotation::RotToMatrix(CMatrix3_t mat)const{
00857   m_identity(mat);
00858   Rotation::Transform(mat,mat);
00859   Rotation::Transform(mat+4, mat+4);
00860   Rotation::Transform(mat+8,mat+8);
00861 }
00862 
00863 
00864 void Rotation::InverseTransform(const CVector3_t in, CVector3_t out){
00865   v_scale(beta,-1,beta);     // inverting rotation sign
00866   Rotation::Transform(in,out);           // rotating back
00867   v_scale(beta,-1,beta);     // resetting rotation sign
00868 }
00869 
00870 Rotation& Rotation::operator*(const Rotation& r1){
00871   CQuat_t q1,q2,q3;
00872   r1.GetQuaternion(q1);
00873   GetQuaternion(q2);
00874   q_multiply(q1,q2,q3);
00875   SetQuaternion(q3);
00876   //  cout<<"q1 "<<q1[2]<<" q2 "<<q2[2]<<" q3 "<<q3[2]<<" beta "<<beta<<endl;
00877   return *this;
00878 }
00879 
00880 
00881 
00882 ostream& operator<<(ostream& out, const Rotation& rt)
00883 {
00884   CVector3_t tmp;
00885   rt.GetRotationParam(tmp);
00886   out << rt.GetRotationAngle()<<" : "<<tmp[0]<<" "<<tmp[1]<<" "<<tmp[2];
00887   return out;
00888 }
00889 
00890 
00891 istream& operator>>(istream& in, Rotation& rt)
00892 {
00893   CVector3_t b;
00894   float f,b1,b2,b3;
00895   char a1;
00896   //  char line[200];
00897 
00898 //   in>>f>>a>>b[0]>>b[1]>>b[2]>>a>>t[0]>>t[1]>>t[2];
00899 //   in.getline(line,200);
00900 //   cout<<"hum "<<line<<endl;
00901 //   sscanf(line,"%f : %f %f %f",&f,b,b+1,b+2);
00902 //   cout<<"reading"<<endl;
00903 //   coutvec(b);
00904   in>>f>>a1>>b1>>b2>>b3;
00905   v_set(b1,b2,b3,b);
00906   rt.SetTransfo(b);
00907   rt.SetAngle(f);
00908   return in;
00909 }
00910 
00911 //#define TEST_RIGID_TRANSFO
00912 #ifdef TEST_RIGID_TRANSFO
00913 // #include <iostream>
00914 // #include <fstream>
00915 int main(int argc, char *argv[]){
00916   CVector3_t v1,v2;
00917   ofstream mout("bf");
00918   Rotation rt,rt2;
00919   v_set(0.1,0.2,0.2,v1);
00920   v_set(0.5,0.4,0.3,v2);
00921   rt.SetTransfo(v1,v2);
00922   mout<<rt;
00923   mout.close();
00924   ifstream mmin("outrt.txt");
00925   mmin>>rt2;
00926   mmin.close();
00927   cout<<rt2;
00928   return 1;
00929 }
00930 #endif
00931   
00932   
00933 
00934 
00935 
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:26 2010 for Body_Schema_Learning by  doxygen 1.6.1