Shape.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 "Shape.h"
00020 #include "Rotation.h"
00021 
00022 
00023 #define FRANCIS_DIST
00024 
00025 Shape::Shape(){
00026   v_clear(position);
00027   v_clear(orientation);
00028   highlight=0;
00029 }
00030 
00031 
00032 float Shape::Distance(Shape& sh, RigidTransfo& transfo){
00033   CVector3_t pos,pos2;
00034   sh.GetPosition(pos);
00035   transfo.Transform(pos,pos2);
00036   v_sub(position,pos2,pos);
00037   return v_length(pos);
00038 }
00039 
00040 
00041 // void Capsule::SetRadius(float r){
00042 //   radius = r;
00043 // }
00044 
00045  Capsule::Capsule(){
00046   radius =10;
00047   v_clear(axis);
00048   cyl = gluNewQuadric();
00049 }
00050 
00051 Capsule::Capsule(pTree config):Shape(){
00052   //  Capsule();
00053   unsigned int i;
00054   float f;
00055   CVector3_t v;
00056   cyl = gluNewQuadric();
00057   Tree_List *subTrees = config->GetSubTrees();
00058   for(i=0;i<(*subTrees).size();i++){
00059      if((*subTrees)[i]->GetName().compare("Axis")==0){
00060        istringstream s((*subTrees)[i]->GetData());
00061        s >> v[0]>>v[1]>>v[2];
00062        SetAxis(v);
00063        cout<<"setting axis "<<v<<endl;
00064      }
00065       if((*subTrees)[i]->GetName().compare("Radius")==0){
00066         istringstream s((*subTrees)[i]->GetData());
00067         s >> f;
00068         SetRadius(f);
00069        cout<<"setting radius "<<f<<endl;
00070     }
00071      if((*subTrees)[i]->GetName().compare("Position")==0){
00072        istringstream s((*subTrees)[i]->GetData());
00073        s >> v[0]>>v[1]>>v[2];
00074        SetPosition(v);
00075        cout<<"setting position "<<v<<endl;
00076      }
00077   }
00078 }
00079 
00080 Capsule::~Capsule(){
00081   gluDeleteQuadric(cyl);
00082 }
00083 
00084 void Capsule::Stream(ostream& out) const{
00085   out<<"<Type> Capsule </Type>"<<endl;
00086   out<<"<Params>"<<endl;
00087   out<<"<Radius> "<<radius<<" </Radius>"<<endl; 
00088   out<<"<Axis> "<< axis <<" </Axis>"<<endl;
00089   out<<"<Position> "<<position<<" </Position>"<<endl;
00090   out<<"</Params>"<<endl;
00091 }
00092 
00093 void Capsule::Render(){
00094   Rotation rot;
00095   CVector3_t v,z; //z axis
00096   CMatrix4_t mat;
00097   float length,angle;
00098   length = v_length(axis);
00099   if(length>epsilon){
00100     v_set(0,0,1,z);
00101     v_cross(z,axis,v);
00102     
00103     if(!rot.SetRotationAxis(v)){
00104       m_identity(mat);
00105     }
00106     else{
00107       angle = acos(v_dot(z,axis)/length);
00108       rot.SetAngle(angle);
00109       rot.RotToMatrix(mat);
00110     }
00111     if(highlight){
00112       glColor3f(1.0,0.6,0.4);
00113     }
00114     else{
00115       glColor3f(1.0,0.8,0.7);
00116     }
00117     glPushMatrix();
00118     glTranslatef(position[0],position[1],position[2]);
00119     glMultMatrixf(mat);
00120     gluCylinder(cyl,radius,radius,length,8,5);
00121   //  glutSolidCylinder(radius,length, 10,10);
00122   }
00123   glColor3f(1.0,0,0);
00124   glutSolidSphere(radius,10,10);
00125   glPushMatrix();
00126   glTranslatef(0,0,length);
00127   glutSolidSphere(radius,10,10);
00128   glPopMatrix();
00129   if(length>epsilon){
00130     glPopMatrix();
00131   }
00132   //  Unhighlight();
00133 }
00134 
00135 
00136 void Capsule::Update(CVector3_t *pos,int nbpos){
00137     if(nbpos==1){
00138         v_copy(pos[0],axis);
00139     }
00140 }
00141 
00142 /*
00143 float Capsule::Distance(Capsule& cap, RigidTransfo& transfo){ 
00144     
00145     CVector3_t vec1, vec2, p1g1,p1g2,p2g1,p2g2;
00146     CVector3_t point[4];
00147     float k1,k2,dist,radius2,min;
00148     
00149     cap.GetParam(p1g2,vec2,radius2); 
00150     
00151         //conversion de referentiel
00152     transfo.InverseTransform(position,p1g1);
00153     transfo.Rotation::InverseTransform(axis,vec1);
00154   
00155     v_add(p1g2,vec2,p2g2);
00156     v_add(p1g1,vec1,p2g1);
00157     
00158     if (segments_nearest_points(p1g1, vec1, p1g2, vec2, &k1 , &k2, &dist)==1){
00159       return (dist-radius-radius2);
00160     }
00161     else{      
00162       v_sub(p1g2,p1g1,point[0]);
00163       v_sub(p1g2,p2g1,point[1]);
00164       v_sub(p2g2,p1g1,point[2]);  
00165       v_sub(p2g2,p2g1,point[3]);
00166       //for (int i=0;i<4;i++)
00167       //cout<<"Distance"<<i<<"=  "<<v_length(point[i])<<endl;
00168       min=MAX_FLT;
00169       for(int i=0;i<4;i++){
00170         if (v_length(point[i])<min){
00171           min=v_length(point[i]);
00172         }
00173       }
00174       dist=min-radius-radius2;
00175       return dist;
00176     }
00177 }
00178 
00179 */
00180 
00181 
00182 void Capsule::GetParam(CVector3_t point, CVector3_t dir, float& radius_cap){
00183   v_copy(position,point);
00184   v_copy(axis,dir);
00185   radius_cap=radius;
00186 }
00187 
00188 float Capsule::Distance(Capsule& cap, RigidTransfo& transfo){ 
00189     
00190     CVector3_t vec1, vec2;
00191     CVector3_t point[4]; // p1,p2 : gelule 1 , p2,p3 :gelule 2
00192     float k1,k2,dist,radius2,min_dist=FLT_MAX;
00193     
00194 
00195     
00196     radius2 = cap.GetRadius();
00197     cap.GetPosition(point[2]);
00198     cap.GetAxis(vec2);
00199 
00200         //conversion de referentiel
00201     transfo.InverseTransform(position,point[0]);
00202     transfo.Rotation::InverseTransform(axis,vec1);
00203   
00204     v_add(point[2],vec2,point[3]);
00205     v_add(point[0],vec1,point[1]);
00206     
00207     if (segments_nearest_points(point[0], vec1, point[2], vec2, &k1 , &k2, &dist)==1){
00208       return (dist-radius-radius2);
00209     }
00210     else{
00211       for(int i=0;i<2;i++){
00212         for(int j=0;j<2;j++){
00213           v_sub(point[i],point[2+j],vec1);
00214           dist = v_squ_length(vec1);
00215           if(dist<min_dist){
00216             min_dist=dist;
00217           }
00218         }
00219       }
00220       return sqrt(dist)-radius-radius2;
00221     }     
00222 }
00223 
00224 
00225 
00226 
00227 
00228 float Capsule::Distance(Shape& sh, RigidTransfo& transfo){
00229   
00230   Shape *psh;
00231   Capsule *cap;
00232   Sphere *sp;
00233   Parallelipiped *para;
00234  
00235   psh = &sh;
00236   
00237   cap = dynamic_cast<Capsule *>(psh);
00238   if(cap){
00239     return Distance(*cap,transfo);
00240   }
00241   sp = dynamic_cast<Sphere *>(psh);
00242   if(sp){
00243     return Distance(*sp,transfo);
00244   }
00245   para = dynamic_cast<Parallelipiped *>(psh);
00246   if(para){
00247     return Distance(*para,transfo);
00248   }
00249   
00250   return 0;
00251 }
00252 
00253 /*
00254 float Capsule::Distance(Sphere& sph, RigidTransfo& transfo){
00255 
00256         CVector3_t intersection, vec_dir, new_vec, point1, point2, centre;
00257         float norme_carre, prod_vec, multi, k=0, rayon_gelule, rayon_sphere;
00258 
00259 
00260         sph.GetParam(centre,rayon_sphere);
00261 
00262         v_copy(position,point1);
00263         v_add(point1,axis,point2);
00264         rayon_gelule=radius;     
00265 
00266         v_copy(axis,vec_dir); //axis=point2[j]-point1[j];
00267         
00268         
00269         //conversion de referentiel
00270         if (sph.GetNumshape() == 1){
00271           transfo.InverseTransform(centre,centre);
00272         }
00273         else{
00274           transfo.Transform(centre,centre);
00275         }
00276 
00277         norme_carre= pow(vec_dir[0],2)+pow(vec_dir[1],2)+pow(vec_dir[2],2);
00278         prod_vec=(centre[0]-point1[0])*vec_dir[0]+(centre[1]-point1[1])*vec_dir[1]+(centre[2]-point1[2])*vec_dir[2];
00279 
00280         multi=prod_vec/norme_carre;
00281 
00282         for (int l=0;l<3;l++){
00283                 intersection[l]=point1[l]+multi*vec_dir[l];
00284                 new_vec[l]=intersection[l]-point1[l];
00285                 if (new_vec[l]!=0. && vec_dir[l]!=0.)
00286                         k=new_vec[l]/vec_dir[l];
00287         }
00288 
00289 
00290         if (k<0.)
00291                 return (sqrt(pow(point1[0]-centre[0],2)+pow(point1[1]-centre[1],2)+pow(point1[2]-centre[2],2))-rayon_gelule-rayon_sphere);
00292         else{
00293                 if(k>1.)
00294                         return (sqrt(pow(point2[0]-centre[0],2)+pow(point2[1]-centre[1],2)+pow(point2[2]-centre[2],2))-rayon_gelule-rayon_sphere);
00295                 else
00296                         return (sqrt(pow(intersection[0]-centre[0],2)+pow(intersection[1]-centre[1],2)+pow(intersection[2]-centre[2],2))-rayon_gelule-rayon_sphere);
00297 
00298         }
00299 }
00300 
00301 */
00302 
00303 
00304 float Capsule::Distance(Sphere& sph, RigidTransfo& transfo){
00305 
00306   CVector3_t centre,centreR1,naxis;
00307   float f,f1,f2,srad;
00308 
00309   sph.GetPosition(centre);
00310   srad = sph.GetRadius();
00311   transfo.Transform(centre,centreR1);
00312   v_sub(centreR1,position,centreR1);
00313   f1 = v_normalize(axis,naxis);
00314   f2 = v_dot(centreR1,naxis);
00315   f =min(f1,max(0,f2)); 
00316   v_scale(naxis,f,naxis);
00317   v_sub(centreR1,naxis,centre);
00318   return v_length(centre)-radius-srad;
00319 }       
00320 
00321 
00322 
00323 
00324 float Capsule::Distance(Parallelipiped& par, RigidTransfo& transfo){
00325   RigidTransfo rt(transfo);
00326   rt.Invert();
00327   return par.Distance(*this,rt);
00328 }
00329 
00330 
00331 Sphere::Sphere(pTree config):Shape(){
00332   //  Capsule();
00333   unsigned int i;
00334   float f;
00335   CVector3_t v;
00336   Tree_List *subTrees = config->GetSubTrees();
00337   for(i=0;i<(*subTrees).size();i++){
00338     if((*subTrees)[i]->GetName().compare("Radius")==0){
00339       istringstream s((*subTrees)[i]->GetData());
00340       s >> f;
00341       SetRadius(f);
00342     }
00343     if((*subTrees)[i]->GetName().compare("Position")==0){
00344       istringstream s((*subTrees)[i]->GetData());
00345       s >> v[0]>>v[1]>>v[2];
00346       SetPosition(v);
00347     }
00348   }
00349 }
00350 
00351 
00352 
00353 void Sphere::Render(){
00354   glPushMatrix();
00355   glTranslatef(position[0],position[1],position[2]);
00356   if(highlight){
00357     glColor3f(0.8,0,0.8);
00358   }
00359   else{
00360     glColor3f(1.0,0,1.0);
00361   }
00362  glutSolidSphere(radius,15,15); 
00363   glPopMatrix();
00364   //  Unhighlight();
00365 }
00366 
00367 
00368 void Sphere::Stream(ostream& out) const{
00369   out<<"<Type> Sphere </Type>"<<endl;
00370   out<<"<Params>"<<endl;
00371   out<<"<Radius> "<<radius<<" </Radius>"<<endl;
00372   out<<"<Position> "<<position<<" </Position>"<<endl;
00373    out<<"</Params>"<<endl;
00374 }
00375 
00376 
00377 
00378 
00379 float Sphere::Distance(Shape& sh, RigidTransfo& transfo){
00380   
00381   Shape *psh;
00382   Capsule *cap;
00383   Sphere *sp;
00384   Parallelipiped *para;
00385  
00386   psh = &sh;
00387   
00388   cap = dynamic_cast<Capsule *>(psh);
00389   if(cap){
00390     return Distance(*cap, transfo);
00391   }
00392   sp = dynamic_cast<Sphere *>(psh);
00393   if(sp){
00394     return Distance(*sp, transfo);
00395   }
00396   para = dynamic_cast<Parallelipiped *>(psh);
00397   if(para){
00398     return Distance(*para, transfo);
00399   }
00400   
00401   return 0;
00402 }
00403 
00404 
00405 void Sphere::GetParam(CVector3_t center_sph, float& radius_sph){
00406   v_copy(position,center_sph);
00407   radius_sph=radius;
00408 }
00409 
00410 float Sphere::Distance(Sphere& sph, RigidTransfo& transfo){
00411   CVector3_t v1,v2;
00412   float r;
00413   sph.GetPosition(v1);
00414   r=sph.GetRadius();
00415 
00416   //conversion de referentiel
00417   transfo.Transform(v1,v2);               
00418   v_sub(position,v2,v1);
00419   return v_length(v1)-radius-r;
00420 }   
00421 
00422 float Sphere::Distance(Capsule& cap, RigidTransfo& transfo){
00423   RigidTransfo rt(transfo);
00424   rt.Invert();
00425   return cap.Distance(*this,rt);
00426 }
00427 
00428 float Sphere::Distance(Parallelipiped& para, RigidTransfo& transfo){
00429   RigidTransfo rt(transfo);
00430   rt.Invert(); 
00431   return para.Distance(*this,rt);
00432 }
00433 Parallelipiped::Parallelipiped():Shape(){
00434   v_clear(size);
00435 }
00436 
00437 Parallelipiped::Parallelipiped(pTree config):Shape(){
00438   CVector3_t v;
00439   Tree_List *subTrees = config->GetSubTrees();
00440   for(unsigned int i=0;i<(*subTrees).size();i++){
00441     if((*subTrees)[i]->GetName().compare("Size")==0){
00442       istringstream s((*subTrees)[i]->GetData());
00443       s >> v[0]>>v[1]>>v[2];
00444       SetSize(v);
00445     }
00446     if((*subTrees)[i]->GetName().compare("Position")==0){
00447       istringstream s((*subTrees)[i]->GetData());
00448       s >> v[0]>>v[1]>>v[2];
00449       SetPosition(v);
00450     }
00451   }
00452 
00453 }
00454 
00455 void Parallelipiped::Stream(ostream& out) const{
00456  out<<"<Type> Parallelipiped </Type>"<<endl;
00457   out<<"<Params>"<<endl;
00458   out<<"<Size> "<<size<<" </Size>"<<endl; 
00459   out<<"<Position> "<<position<<" </Position>"<<endl;
00460   out<<"</Params>"<<endl;
00461 }
00462 
00463 
00464 void Parallelipiped::Render(){
00465   Rotation rot;
00466   CMatrix4_t mat;
00467   rot.SetRotationParam(orientation);
00468   rot.RotToMatrix(mat);
00469   glPushMatrix();
00470 
00471  glTranslatef(position[0],position[1],position[2]);
00472  glMultMatrixf(mat);//new
00473  glTranslatef(0.5*size[0],0.5*size[1],0.5*size[2]);
00474  glScalef(size[0],size[1],size[2]);
00475  if(highlight){
00476    glColor3f(0.5,0.7,0.9);
00477  }
00478  else{
00479    glColor3f(0.8,0.9,1.0);
00480  }
00481  glutSolidCube(1.0);
00482  glColor3f(0,0,0);
00483  glScalef(1.01,1.01,1.01);
00484  glLineWidth(3.0);
00485  glutWireCube(1.0);
00486  glColor3f(1.0,0.8,0.7);
00487  glPopMatrix();
00488  // Unhighlight();
00489 }
00490 
00491 float Parallelipiped::Distance_point_face(const CVector3_t point, int plan, float longueur, float largeur, float hauteur){
00492 
00493         int i,j;
00494         float pos_plan[6];
00495 
00496         for (i=0;i<3;i++)
00497                 pos_plan[2*i]=0.0;
00498 
00499         pos_plan[1]=hauteur;
00500         pos_plan[3]=longueur;
00501         pos_plan[5]=largeur;
00502 
00503         j=plan/2;
00504 
00505         if (j==0){
00506                 if (point[0]<=longueur && point[0]>=0.0){
00507                         if (point[1]<=largeur && point[1]>=0.0)
00508                                 return (abs(point[2]-pos_plan[plan]));
00509                         else{
00510                                 if (point[1]<0.0)
00511                                         return (sqrt(pow(point[1],2)+pow(point[2]-pos_plan[plan],2)));
00512                                 else
00513                                         return (sqrt(pow(point[1]-largeur,2)+pow(point[2]-pos_plan[plan],2)));
00514                         }
00515                 }
00516                 else{
00517                         if (point[0]<0.0){
00518                                 if (point[1]>largeur)
00519                                         return (sqrt(pow(point[0],2)+pow(point[1]-largeur,2)+pow(point[2]-pos_plan[plan],2)));
00520                                 else{
00521                                         if (point[1]<0.0)
00522                                                 return (sqrt(pow(point[0],2)+pow(point[1],2)+pow(point[2]-pos_plan[plan],2)));
00523                                         else
00524                                                 return (sqrt(pow(point[0],2)+pow(point[2]-pos_plan[plan],2)));
00525                                 }
00526                         }
00527                         else{
00528                                 if (point[1]>largeur)
00529                                         return (sqrt(pow(point[0]-longueur,2)+pow(point[1]-largeur,2)+pow(point[2]-pos_plan[plan],2)));
00530                                 else{
00531                                         if (point[1]<0.0)
00532                                                 return (sqrt(pow(point[0]-longueur,2)+pow(point[1],2)+pow(point[2]-pos_plan[plan],2)));
00533                                         else
00534                                                 return (sqrt(pow(point[0]-longueur,2)+pow(point[2]-pos_plan[plan],2)));
00535                                 }
00536                         }
00537                 }
00538         }
00539         else{   
00540                 if (j==1){
00541                         if (point[1]<=largeur && point[1]>=0.0){
00542                                 if (point[2]<=hauteur && point[2]>=0.0)
00543                                         return (abs(point[0]-pos_plan[plan]));
00544                                 else{
00545                                         if (point[2]<0.0)
00546                                                 return (sqrt(pow(point[0]-pos_plan[plan],2)+pow(point[2],2)));
00547                                         else
00548                                                 return (sqrt(pow(point[0]-pos_plan[plan],2)+pow(point[2]-hauteur,2)));
00549                                 }
00550                         }
00551                         else{
00552                                 if (point[1]<0.0){
00553                                         if (point[2]>hauteur)
00554                                                 return (sqrt(pow(point[0]-pos_plan[plan],2)+pow(point[1],2)+pow(point[2]-hauteur,2)));
00555                                         else{
00556                                                 if (point[2]<0.0)
00557                                                         return (sqrt(pow(point[0]-pos_plan[plan],2)+pow(point[1],2)+pow(point[2],2)));
00558                                                 else
00559                                                         return (sqrt(pow(point[0]-pos_plan[plan],2)+pow(point[1],2)));
00560                                         }
00561                                 }
00562                                 else{
00563                                         if (point[2]>hauteur)
00564                                                 return (sqrt(pow(point[0]-pos_plan[plan],2)+pow(point[1]-largeur,2)+pow(point[2]-hauteur,2)));
00565                                         else{
00566                                                 if (point[2]<0.0)
00567                                                         return (sqrt(pow(point[0]-pos_plan[plan],2)+pow(point[1]-largeur,2)+pow(point[2],2)));
00568                                                 else
00569                                                         return (sqrt(pow(point[0]-pos_plan[plan],2)+pow(point[1]-largeur,2)));
00570                                         }
00571                                 }
00572                         }
00573                 }
00574                 else{
00575                         if (point[0]<=longueur && point[0]>=0.0){
00576                                 if (point[2]<=hauteur && point[2]>=0.0)
00577                                         return (abs(point[1]-pos_plan[plan]));
00578                                 else{
00579                                         if (point[2]<0.0)
00580                                                 return (sqrt(pow(point[1]-pos_plan[plan],2)+pow(point[2],2)));
00581                                         else
00582                                                 return (sqrt(pow(point[1]-pos_plan[plan],2)+pow(point[2]-hauteur,2)));
00583                                 }
00584                         }
00585                         else{
00586                                 if (point[0]<0.0){
00587                                         if (point[2]>hauteur)
00588                                                 return (sqrt(pow(point[0],2)+pow(point[1]-pos_plan[plan],2)+pow(point[2]-hauteur,2)));
00589                                         else{
00590                                                 if (point[2]<0.0)
00591                                                         return (sqrt(pow(point[0],2)+pow(point[1]-pos_plan[plan],2)+pow(point[2],2)));
00592                                                 else
00593                                                         return (sqrt(pow(point[0],2)+pow(point[1]-pos_plan[plan],2)));
00594                                         }
00595                                 }
00596                                 else{
00597                                         if (point[2]>hauteur)
00598                                                 return (sqrt(pow(point[0]-longueur,2)+pow(point[1]-pos_plan[plan],2)+pow(point[2]-hauteur,2)));
00599                                         else{
00600                                                 if (point[2]<0.0)
00601                                                         return (sqrt(pow(point[0]-longueur,2)+pow(point[1]-pos_plan[plan],2)+pow(point[2],2)));
00602                                                 else{
00603                                                         return (sqrt(pow(point[0]-longueur,2)+pow(point[1]-pos_plan[plan],2)));
00604                                                 }
00605                                         }
00606                                 }
00607                         }
00608                 }
00609         }
00610 }
00611 
00612 void Parallelipiped::Point_proche_plan(const CVector3_t point1, const CVector3_t point2, int plan, float longueur, float largeur, float hauteur, CVector3_t out){
00613         
00614         float dist1=0, dist2=0, pos_plan[6];
00615         int i,j;
00616 
00617         for (i=0;i<3;i++)
00618                 pos_plan[2*i]=0.0;
00619 
00620         pos_plan[1]=hauteur;
00621         pos_plan[3]=longueur;
00622         pos_plan[5]=largeur;
00623 
00624         j=(plan/2);
00625 
00626         if (j==0){
00627                 dist1=abs(point1[2]-pos_plan[plan]);
00628                 dist2=abs(point2[2]-pos_plan[plan]);
00629         }
00630         if (j==1){
00631                 dist1=abs(point1[0]-pos_plan[plan]);
00632                 dist2=abs(point2[0]-pos_plan[plan]);
00633         }
00634         if (j==2){
00635                 dist1=abs(point1[1]-pos_plan[plan]);
00636                 dist2=abs(point2[1]-pos_plan[plan]);
00637         }
00638 
00639         if (dist1<dist2)
00640                 for (i=0;i<3;i++)  
00641                         out[i]=point1[i];
00642         else{
00643                 for (i=0;i<3;i++)
00644                         out[i]=point2[i];
00645         }
00646 }
00647 
00648 int Parallelipiped::Inter_droite_plan (const CVector3_t point1, const CVector3_t point2, int plan, float dist, CVector3_t out){ 
00649      
00650     CVector3_t droite;
00651     float lambda=0;
00652 
00653     v_sub(point2,point1,droite);
00654     /*droite[0]=point2[0]-point1[0];
00655     droite[1]=point2[1]-point1[1];
00656     droite[2]=point2[2]-point1[2];
00657     cout<<"vec dir= "<<droite<<endl;*/
00658 
00659         switch(plan/2){
00660         case 0: if (abs(droite[2])<=0.01){
00661                    return 0;
00662                 }
00663                 out[2]=dist;
00664                 lambda=(out[2]-point1[2])/droite[2];
00665                 break;
00666         case 1: if (abs(droite[0])<=0.01){
00667                    return 0;
00668                 }
00669                 out[0]=dist;
00670                 lambda=(out[0]-point1[0])/droite[0];
00671                 break;
00672         case 2: if (abs(droite[1])<=0.01){
00673                    return 0;
00674                   
00675                 }
00676                 out[1]=dist;
00677                 lambda=(out[1]-point1[1])/droite[1];
00678                 break;
00679         }
00680 
00681         
00682         out[0]=point1[0]+lambda*droite[0];
00683         out[1]=point1[1]+lambda*droite[1];
00684         out[2]=point1[2]+lambda*droite[2];
00685 
00686 
00687         
00688         //cout << "lambda = " << lambda << endl;
00689         return 1;
00690 }
00691 
00692 
00693 
00694 
00695 
00696 
00697 
00698 float Parallelipiped::Distance(Sphere& sph, RigidTransfo& transfo){
00699           
00700       CVector3_t para, sphere ;
00701       float hauteur, longueur, largeur, rayon;    
00702       
00703       //cout<<"Paralllelipiped Sphere"<<endl;
00704       sph.GetParam(sphere,rayon);
00705       //for (int k=0;k<3;k++)
00706       // para[k]=position[k];
00707       
00708       //conversion de referentiel
00709    //    if(sph.GetNumshape() == 1){
00710 //      transfo.InverseTransform(sphere,sphere);
00711 //       }
00712 //       else{
00713         transfo.Transform(sphere,sphere);
00714         //      }
00715       //
00716         
00717       
00718       
00719       hauteur=size[0];
00720       longueur=size[1];
00721       largeur=size[2];
00722 
00723       para[0]=position[0]+largeur/2.;
00724       para[1]=position[1]+hauteur/2.;
00725       para[2]=position[2]+longueur/2.;
00726     
00727       //cout <<"Longueur = "<<longueur<< "  largeur = " <<largeur<<"  hauteur= "<<hauteur<<endl;
00728       //cout << "Position de la sphere"<<endl;
00729       //cout << sphere[0]<<" "<<sphere[1]<<" "<<sphere[2]<<endl;
00730       //cout << "rayon = " <<rayon<<endl;
00731       
00732       float dist[3];
00733       
00734       
00735       //Compute distances to planes
00736       
00737       if (sphere[0]>=para[0])
00738           dist[0]= abs(sphere[0]-para[0]-longueur/2.);             
00739       else
00740           dist[0]= abs(sphere[0]-para[0]+longueur/2.);
00741           
00742       if (sphere[1]>=para[1])
00743           dist[1]= abs(sphere[1]-para[1]-largeur/2.);             
00744       else
00745           dist[1]= abs(sphere[1]-para[1]+largeur/2.);
00746              
00747       if (sphere[2]>=para[2])
00748           dist[2]= abs(sphere[2]-para[2]-hauteur/2.);             
00749       else
00750           dist[2]= abs(sphere[2]-para[2]+hauteur/2.);       
00751       
00752       //cout<<"Distance aux plans "<<endl;
00753       //cout<<dist[0]<<" "<<dist[1]<<" "<<dist[2]<<endl;
00754 
00755        //Compute minimum distance
00756        
00757      if (sphere[0]<(para[0]-longueur/2.) || sphere[0]>(para[0]+longueur/2.)){
00758             if(sphere[1]<(para[1]-largeur/2.) || sphere[1]>(para[1]+largeur/2.)){
00759                  if (sphere[2]<(para[2]-hauteur/2.) || sphere[2]>(para[2]+hauteur/2.))
00760                        return (sqrt(pow(dist[0],2)+pow(dist[1],2)+pow(dist[2],2)) - rayon);
00761                  else
00762                        return (sqrt(pow(dist[0],2)+pow(dist[1],2)) - rayon);
00763             }
00764             else{
00765                  if (sphere[2]<(para[2]-hauteur/2.) || sphere[2]>(para[2]+hauteur/2.))
00766                        return (sqrt(pow(dist[0],2)+pow(dist[2],2)) - rayon); 
00767                  else
00768                        return (dist[0]-rayon);
00769             }
00770       }
00771       else{
00772             if(sphere[1]<(para[1]-largeur/2.) || sphere[1]>(para[1]+largeur/2.)){
00773                  if (sphere[2]<(para[2]-hauteur/2.) || sphere[2]>(para[2]+hauteur/2.)) 
00774                       return (sqrt(pow(dist[1],2)+pow(dist[2],2)) - rayon); 
00775                  else
00776                       return (dist[1]-rayon);
00777             }  
00778             else
00779                  return (dist[2]-rayon);
00780       }
00781 }
00782 
00783 float Parallelipiped::Distance(Shape& sh, RigidTransfo& transfo){
00784   
00785   Shape *psh;
00786   Capsule *cap;
00787   Sphere *sp;
00788  
00789   psh = &sh;
00790   
00791   cap = dynamic_cast<Capsule *>(psh);
00792   if(cap){
00793     return Distance(*cap, transfo);
00794   }
00795   sp = dynamic_cast<Sphere *>(psh);
00796   if(sp){
00797     return Distance(*sp, transfo);
00798   }
00799   
00800   return 100;
00801 }
00802 
00803 
00804 #ifdef FRANCIS_DIST
00805 float Parallelipiped::Distance(Capsule& cap, RigidTransfo& transfo){
00806    
00807         int faces[6], arretes[12], index[6], i,autorized=0;
00808         CVector3_t intersection, point,vec_dir_seg, vec_dir_edge, point_edge, point1, point2, axis;
00809         float distance[6], pos_plan[6], min=0.0, k1, k2, distance_edge[12], pos, longueur, largeur, hauteur, rayon;
00810         //struct timeval first,second;
00811         //static int k=0;
00812         
00813         //if (k==0){
00814         //gettimeofday(&first,NULL);
00815         //}
00816         //k++;
00817         //cout<<"Parallelipiped Capsule"<<endl;
00818         cap.GetParam(point1,axis,rayon);
00819         
00820 
00821 //      cout<<"refpos=  "<<position<<endl;
00822 //      cout<<"capspos=  "<<point1<<endl
00823         //conversion de referentiel
00824         
00825 //      if (cap.GetNumshape() == 1){
00826 //        transfo.InverseTransform(point1,point1);
00827 //        transfo.Rotation::InverseTransform(axis,axis);
00828 //      }
00829 //      else{
00830           transfo.Transform(point1,point1);
00831           transfo.Rotation::Transform(axis,axis);
00832           //    }
00833 
00834 //      v_copy(point1,tmp);
00835 //      point1[0]=tmp[2];
00836 //      point1[1]=tmp[0];
00837 //      point1[2]=tmp[1];
00838 //      v_copy(axis,tmp);
00839 //      axis[0]=tmp[2];
00840 //      axis[1]=tmp[0];
00841 //      axis[2]=tmp[1];
00842 //      longueur=size[2];
00843 //         largeur=size[0];
00844 //         hauteur=size[1];
00845         //
00846         
00847         
00848         //cout<<"capspos apres transfo=  "<<point1<<endl;
00849         //cout<<"axespos apres transfo=  "<<axis<<endl;
00850         
00851 
00852         v_sub(point1,position,point1);
00853         v_add(axis,point1,point2);
00854                 
00855         longueur=size[0];
00856         largeur=size[1];
00857         hauteur=size[2];
00858         
00859         //      cout<<longueur<<" "<<largeur<<" "<<hauteur<<endl;
00860         v_copy(axis,vec_dir_seg);
00861         
00862      
00863         for (i=0;i<6;i++){
00864                 faces[i]=1;
00865                 distance[i]=0.0;
00866         }
00867         for (i=0;i<12;i++){
00868                 arretes[i]=0;
00869                 distance_edge[i]=0.0;
00870         }
00871         for (i=0;i<3;i++)
00872                 pos_plan[2*i]=0.0;
00873 
00874         pos_plan[1]=longueur;
00875         pos_plan[3]=largeur;
00876         pos_plan[5]=hauteur;
00877          
00878         // Control if we can suppress sides
00879         //cout<<"Suppress  ";
00880         if (point1[0]>0.0 && point2[0]>0.0){
00881           faces[2]=0;//cout<<"faces[2]  "<<endl;
00882         }
00883         if (point1[1]>0.0 && point2[1]>0.0){
00884           faces[4]=0;//cout<<"faces[4]  ";
00885         }
00886         if (point1[2]>0.0 && point2[2]>0.0){
00887           faces[0]=0;//cout<<"faces[0]  ";
00888         }
00889         if (point1[0]<0.0 && point2[0]<0.0){
00890           faces[3]=0;//cout<<"faces[3]  ";
00891         }
00892         if (point1[1]<0.0 && point2[1]<0.0){
00893           faces[5]=0;//cout<<"faces[5]  ";
00894         }
00895         if (point1[2]<0.0 && point2[2]<0.0){
00896           faces[1]=0;//cout<<"faces[1]  ";
00897         }
00898         //      cout<<endl;
00899                 
00900         // Control if the intersection is in a plane
00901      
00902         for (i=0;i<6;i++){
00903                 autorized=0;
00904                 if (faces[i]==1){
00905                         switch(i){
00906                                 case 1: pos=hauteur;
00907                                                 break;
00908                                 case 3: pos=longueur;
00909                                                 break;
00910                                 case 5: pos=largeur;
00911                                                 break;
00912                                 default:pos=0.0;
00913                                                 break;
00914                         }
00915                         if (Inter_droite_plan ( point1, point2, i, pos, intersection)==0){
00916                           
00917                           //    cout << "Pas d'intersection sur face " <<i <<" !!!"<< endl;
00918                           //    cout << endl;
00919                                 autorized=1;
00920                         }
00921                         
00922 
00923                         
00924                         if (autorized!=1){
00925                           //cout<<"point1= "<<point1<<" point2= "<<point2<<" i= "<<i<<" pos= "<<pos<<endl;
00926                           //cout << "face " << i << endl;
00927                           //cout << "intersection = " << intersection[0] <<"      "<< intersection[1] <<"         "<< intersection[2] << endl;
00928                           //cout << endl;
00929                           if (intersection[(i/2)]>pos_plan[2*(i/2)+1]){
00930                                         arretes[2*i]=1;
00931                                         //cout<<"1"<<endl;
00932                                         if (i>3)
00933                                                 faces[0]=0;
00934                                         else
00935                                                 faces[2+2*(i/2)]=0;
00936                                 }
00937                           else{
00938                                 if (intersection[(i/2)]<0.){
00939                                                 arretes[(2*i)+1]=1;
00940                                                 //      cout<<"2"<<endl;
00941                                                 if (i>3)
00942                                                         faces[1]=0;
00943                                                 else
00944                                                         faces[2*(i/2)+3]=0;
00945                                         }
00946                                 else{
00947                                         if (i>3){
00948                                           //cout<<"3"<<endl;
00949                                                 if (intersection[0]>=0.0 && intersection[0]<= pos_plan[1])
00950                                                                 index[i]=1;
00951                                                 else{
00952                                                         if (intersection[0]<0.0){
00953                                                                 if (i==4)
00954                                                                         arretes[5]=1;
00955                                                                 else
00956                                                                         arretes[4]=1;
00957                                                         }
00958                                                         else{
00959                                                                 if (intersection[0]>pos_plan[1]){
00960                                                                         if (i==4)
00961                                                                                 arretes[7]=1;
00962                                                                         else
00963                                                                                 arretes[6]=1;
00964                                                                 }
00965                                                         }
00966                                                 }
00967                                         }
00968                                         else{
00969                                           //cout<<"4"<<endl;
00970                                           if (intersection[(i/2)+1]>=0.0 && intersection[(i/2)+1]<=pos_plan[2*(i/2)+3]){
00971                                                         index[i]=1;
00972                                                         
00973                                           }
00974                                           else{
00975                                                         if (intersection[(i/2)+1]<0.0){
00976                                                                 switch(i){
00977                                                                         case 0: arretes[9]=1;
00978                                                                                         break;
00979                                                                         case 1: arretes[8]=1;
00980                                                                                         break;
00981                                                                         case 2: arretes[1]=1;
00982                                                                                         break;
00983                                                                         case 3: arretes[0]=1;
00984                                                                                         break;
00985                                                                        }
00986                                                         }
00987                                                         else{
00988                                                                 if(intersection[(i/2)+1]>pos_plan[2*(i/2)+3]){
00989                                                                         switch(i){
00990                                                                                 case 0: arretes[11]=1;
00991                                                                                                 break;
00992                                                                                 case 1: arretes[10]=1;
00993                                                                                                 break;
00994                                                                                 case 2: arretes[3]=1;
00995                                                                                                 break;
00996                                                                                 case 3: arretes[2]=1;
00997                                                                                                 break;
00998                                                                         }       
00999                                                                 }
01000                                                         }
01001                                                 }
01002                                         }
01003                                 }
01004                         }
01005                         }
01006                 }
01007         }
01008 
01009 
01010 
01011         // Compute the distance from the segment to the face if it's the minimum distance
01012 
01013         for (i=0;i<6;i++){
01014                 if (index[i]==1){
01015                         Point_proche_plan( point1, point2, i, longueur, largeur, hauteur, point);
01016                         distance[i]=Distance_point_face(point,i, longueur, largeur, hauteur); 
01017                 }
01018         }
01019         
01020         // Find the minimum distance to a face
01021         
01022         min = FLT_MAX;
01023         for(i=0;i<6;i++){
01024                 if(distance[i]>0.){
01025                         if(distance[i]<min){
01026                                 min=distance[i];
01027                         }
01028                 }
01029         }
01030 
01031         if(min!=FLT_MAX){
01032           //if (k==1000){
01033           //k=0;
01034           //gettimeofday(&second,NULL);
01035           //cout<< second.tv_sec - first.tv_sec<<" sec  "<<second.tv_usec  - first.tv_usec<<" usec"<<endl;
01036           //}
01037 
01038           return (min-rayon);           
01039         }                       
01040 
01041         // Compute the minimal distance if there is no intersection on a face
01042 
01043         for (i=0;i<12;i++){
01044                 if (arretes[i]==1){
01045                         switch(i/4){
01046                                 case 0: vec_dir_edge[0]=0.0;
01047                                                 vec_dir_edge[1]=largeur;
01048                                                 vec_dir_edge[2]=0.0;
01049                                                 break;
01050 
01051                                 case 1: vec_dir_edge[0]=0.0;
01052                                                 vec_dir_edge[1]=0.0;
01053                                                 vec_dir_edge[2]=hauteur;
01054                                                 break;
01055 
01056                                 case 2: vec_dir_edge[0]=longueur;
01057                                                 vec_dir_edge[1]=0.0;
01058                                                 vec_dir_edge[2]=0.0;
01059                                                 break;
01060                         }
01061 
01062                         if (i==0 || i==2 || i==6 || i==7)
01063                                 point_edge[0]=longueur;
01064                         else
01065                                 point_edge[0]=0.0;
01066 
01067                         if (i==4 || i==6 || i==10 || i==11)
01068                                 point_edge[1]=largeur;
01069                         else
01070                                 point_edge[1]=0.0;
01071                         
01072                         if (i==2 || i==3 || i==8 || i==10)
01073                                 point_edge[2]=hauteur;
01074                         else
01075                                 point_edge[2]=0.0;
01076 
01077                         segments_nearest_points(point_edge, vec_dir_edge, point1, vec_dir_seg, &k1 , &k2, &distance_edge[i]);
01078                 }
01079         }
01080         
01081         // Take the minimum distance
01082 
01083         min = FLT_MAX;
01084         for(i=0;i<12;i++){
01085                 if(distance_edge[i]>0.){
01086                         if(distance_edge[i]<min){
01087                                 min=distance_edge[i];
01088                         }
01089                 }
01090         }
01091         //if (k==1000){
01092         //k=0;
01093         //gettimeofday(&second,NULL);
01094         //cout<< second.tv_sec - first.tv_sec<<" sec  "<<second.tv_usec  - first.tv_usec<<" usec"<<endl;
01095         //}
01096         return (min-rayon);             
01097                 
01098 }
01099 
01100 #else
01101 
01102 float Parallelipiped::Distance(Capsule& cap, RigidTransfo& transfo){
01103   CVector3_t upperLinkExtr, lowerLinkExtr,ax;
01104   CVector3_t objPos,s1,s2; 
01105   CVector3_t v1,v2,linkv,u,vertexPos,v_tmp,tmp3,pos;
01106   CMatrix4_t ref,tmpmat,tmpmat2;
01107   int i,j,k,dir1,dir2,pindex,min_i;
01108   float alldists[12]; //closest distance to each edge
01109   float allpoints[24]; // closest pair of points on each edge
01110   float min_dist,dist,rad;
01111   int s3[3];
01112   CVector3_t contact_vector;
01113   float point;
01114 
01115   rad=cap.GetRadius(); 
01116   cap.GetAxis(ax);
01117   cap.GetPosition(pos);
01118   transfo.Transform(pos,upperLinkExtr);  
01119   v_add(pos,ax,pos);
01120   transfo.Transform(pos,lowerLinkExtr);
01121  
01122   //setting the center of parallelipiped
01123   v_scale(size,0.5,objPos);
01124   v_add(position,objPos,objPos);
01125   m_identity(ref);
01126 
01127 //   transfo.RotToMatrix(tmpmat);
01128 //   m_transpose(tmpmat,ref);
01129 
01130   v_sub(lowerLinkExtr,objPos,v1);
01131   v_sub(upperLinkExtr,objPos,v2);
01132   //  cout<<v1<<" "<<v2<<endl;
01133   v_clear(Ss3);
01134   
01135   //checking when two parallel sides must be checked
01136   for(i=0;i<3;i++){
01137     s1[i] = v_dot(v1, &ref[i*4]);
01138     s2[i] = v_dot(v2, &ref[i*4]);
01139     if (s1[i]*s2[i]>=0){
01140       s3[i] = sign(s1[i]+s2[i]);
01141     }
01142   }
01143   //    cout << "s3: ";coutvec(s3);
01144   m_copy(ref,tmpmat);
01145   for(i=0;i<3;i++){
01146     if(s3[i]){
01147       v_sub(lowerLinkExtr,upperLinkExtr,linkv);
01148       v_scale(size,-0.5,u);
01149       u[i] *=-s3[i];
01150       v_add(objPos,u,vertexPos);
01151       //      cout<<"vPos "<<i<<": ";coutvec(vertexPos);
01152       v_scale(&(ref[i*4]),s3[i],&(tmpmat[i*4]));
01153       // cout<<"norm "<<i<<": ";coutvec((tmpmat+i*4));
01154       m_inverse(tmpmat,tmpmat2);
01155       if(v_dot(&(tmpmat[i*4]),linkv)<0){
01156         v_sub(lowerLinkExtr,vertexPos,v_tmp);
01157         point = 1;
01158       }
01159       else{
01160         v_sub(upperLinkExtr,vertexPos,v_tmp);
01161         point = 0;
01162       }
01163       //      cout<<"linkv ";coutvec(linkv);
01164       // cout <<"pt "<<*point<<endl;
01165         
01166       
01167 // #ifdef OLD_AV
01168 //       v_copy(linkv,(CVector3_t)&tmpmat[i*4]);
01169 //       m_inverse(tmpmat,tmpmat2);
01170 //       v_sub(upperLinkExtr,vertexPos,tmp2);
01171       // tmp3 should contain the intersection coordinates in
01172       // the referential defined by the edges of the surface 
01173       v_transform_normal(v_tmp,tmpmat2,tmp3); 
01174       // cout<<"tmp3 ";coutvec(tmp3);
01175       if(tmp3[(i+1)%3]>=0 && tmp3[(i+2)%3]>=0 // the link points to the rectangle
01176          && tmp3[(i+1)%3]<=size[(i+1)%3] && 
01177          tmp3[(i+2)%3]<=size[(i+2)%3]){
01178         if(tmp3[i]<0){
01179           dist = tmp3[i]-rad;
01180           return dist; // there is a collision
01181         }
01182         else{
01183           dist = tmp3[i];
01184           v_copy(&(tmpmat[i*4]),contact_vector);
01185           v_scale(contact_vector,dist,contact_vector);
01186           return dist-rad;
01187         }
01188       }
01189     }
01190   }
01191   for(i=0;i<12;i++){
01192     alldists[i]=FLT_MAX;
01193   }
01194   // the link does not point to a rectangle -> look for the edges
01195   v_scale(size,-0.5,u);
01196   for(i=0;i<3;i++){// each kind of edge
01197       dir1 = s3[(i+1)%3]; 
01198       dir2 = s3[(i+2)%3];
01199     for(j=0;j<2;j++){ 
01200       if(dir1 == 0 || dir1==2*j-1){ //edges of this face must be computed
01201         for(k=0;k<2;k++){
01202           if(dir2 == 0 || dir2==2*k-1){ //edges of this face must be computed
01203             v_copy(u,v_tmp);
01204             v_tmp[(i+1)%3]*=-(2*j-1);
01205             v_tmp[(i+2)%3]*=-(2*k-1);
01206             v_add(objPos,v_tmp,vertexPos);
01207             v_scale(&(ref[4*i]),size[i],v1); // edge with the right length
01208             pindex = 4*i+2*j+k;
01209             segments_nearest_points(vertexPos,v1,upperLinkExtr,linkv,&(allpoints[2*pindex]),&(allpoints[2*pindex+1]),&(alldists[pindex]));
01210         //     cout<<"i:"<<i<<" j:"<<j<<" k:"<<k<<"dist "<<alldists[pindex]<<endl;
01211 //          cout<<"v1:";coutvec(v1);
01212 //          cout<<"ref:";coutvec((&(ref[4*i])));
01213 //          cout<<"vP:";coutvec(vertexPos);
01214           }
01215         }
01216       }
01217     }
01218   }
01219   //looking for the min
01220   min_dist = alldists[0];
01221   min_i = 0;
01222   for(i=1;i<12;i++){
01223     if(alldists[i] < min_dist){
01224       min_dist = alldists[i];
01225       min_i = i;
01226     }
01227   }
01228   // returning the min distance
01229   dist = min_dist;
01230   //  cout<<"min "<<min_i<<" "<<dist<<endl;
01231   point = allpoints[2*min_i+1];
01232   v_scale(linkv,point,v1);
01233   v_add(upperLinkExtr,v1,v2); // nearest point on link
01234   k = min_i%2; //retrieving the right edge
01235   j = ((min_i-k)%4)/2;
01236   i = min_i/4; 
01237   //  cout<<"ijk: "<<i<<" "<<j<<" "<<k<<endl;
01238   v_copy(u,v_tmp);
01239   v_tmp[(i+1)%3]*=-(2*j-1);
01240   v_tmp[(i+2)%3]*=-(2*k-1); 
01241   v_add(objPos,v_tmp,vertexPos); // starting vertex of the edge
01242   v_scale(&(ref[3*1]),allpoints[2*min_i]*(size[min_i]),v1);
01243   v_add(vertexPos,v1,v1); // nearest point on solid
01244   v_sub(v2,v1,contact_vector);
01245   return dist-rad;
01246 }
01247 
01248 
01249 #endif
 All Data Structures Functions Variables

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