Motion Strategy Library

00001 //----------------------------------------------------------------------
00002 //               The Motion Strategy Library (MSL)
00003 //----------------------------------------------------------------------
00004 //
00005 // Copyright (c) 1998-2000 Iowa State University and Steve LaValle.  
00006 // All Rights Reserved.
00007 // 
00008 // Permission to use, copy, and distribute this software and its 
00009 // documentation is hereby granted free of charge, provided that 
00010 // (1) it is not a component of a commercial product, and 
00011 // (2) this notice appears in all copies of the software and
00012 //     related documentation. 
00013 // 
00014 // Iowa State University and the author make no representations
00015 // about the suitability or fitness of this software for any purpose.  
00016 // It is provided "as is" without express or implied warranty.
00017 //----------------------------------------------------------------------
00018 
00019 #include <fstream.h>
00020 #include <math.h>
00021 
00022 #include "geomPQP.h"
00023 #include "defs.h"
00024 
00025 
00026 // Define some useless stream defaults
00027 istream& operator>> (istream& is, PQP_Model& p) { 
00028   // Do nothing -- no reader available
00029   return is; 
00030 }
00031 
00032 ostream& operator<< (ostream& os, const PQP_Model& p) { 
00033   os << "PQP_Model ";
00034   return os; 
00035 }
00036 
00037 
00038 
00039 
00040 // *********************************************************************
00041 // *********************************************************************
00042 // CLASS:     GeomPQP
00043 // 
00044 // *********************************************************************
00045 // *********************************************************************
00046 
00047 
00048 GeomPQP::GeomPQP(string path = ""):Geom(path) {
00049 
00050   // Define empty transformation for obstacles
00051   TO[0]=TO[1]=TO[2]=0.0;
00052   RO[0][0]=RO[1][1]=RO[2][2]=1.0;
00053   RO[0][1]=RO[0][2]=RO[1][0]= RO[1][2]= RO[2][0]= RO[2][1]=0.0;
00054 
00055   NumBodies = 1; // Default
00056 }
00057 
00058 
00059 
00060 
00061 void GeomPQP::LoadEnvironment(string path){
00062 
00063   READ_OPTIONAL_PARAMETER(Obst);
00064 
00065   int i=0;
00066   list<MSLTriangle>::iterator t;
00067 
00068   Ob.BeginModel();
00069   PQP_REAL p1[3],p2[3],p3[3];
00070   forall(t,Obst) {
00071     p1[0] = (PQP_REAL) t->p1.xcoord();
00072     p1[1] = (PQP_REAL) t->p1.ycoord();
00073     p1[2] = (PQP_REAL) t->p1.zcoord();
00074     p2[0] = (PQP_REAL) t->p2.xcoord();
00075     p2[1] = (PQP_REAL) t->p2.ycoord();
00076     p2[2] = (PQP_REAL) t->p2.zcoord();
00077     p3[0] = (PQP_REAL) t->p3.xcoord();
00078     p3[1] = (PQP_REAL) t->p3.ycoord();
00079     p3[2] = (PQP_REAL) t->p3.zcoord();
00080     Ob.AddTri(p1,p2,p3,i);
00081     i++;
00082   }
00083   Ob.EndModel();
00084 }
00085 
00086 
00087 
00088 void GeomPQP::LoadRobot(string path){
00089 
00090   READ_OPTIONAL_PARAMETER(Robot);
00091 
00092   int i=0;
00093   list<MSLTriangle>::iterator t;
00094 
00095   Ro.BeginModel();
00096   PQP_REAL p1[3],p2[3],p3[3];
00097   forall(t,Robot){
00098     p1[0] = (PQP_REAL) t->p1.xcoord();
00099     p1[1] = (PQP_REAL) t->p1.ycoord();
00100     p1[2] = (PQP_REAL) t->p1.zcoord();
00101     p2[0] = (PQP_REAL) t->p2.xcoord();
00102     p2[1] = (PQP_REAL) t->p2.ycoord();
00103     p2[2] = (PQP_REAL) t->p2.zcoord();
00104     p3[0] = (PQP_REAL) t->p3.xcoord();
00105     p3[1] = (PQP_REAL) t->p3.ycoord();
00106     p3[2] = (PQP_REAL) t->p3.zcoord();
00107     Ro.AddTri(p1,p2,p3,i);
00108     i++;
00109   }
00110   Ro.EndModel();
00111 }
00112 
00113 
00114 
00115 
00116 // *********************************************************************
00117 // *********************************************************************
00118 // CLASS:     GeomPQP2D
00119 // 
00120 // *********************************************************************
00121 // *********************************************************************
00122 
00123 GeomPQP2D::GeomPQP2D(string path = ""):GeomPQP(path) {
00124 
00125   LoadEnvironment(FilePath);
00126 }
00127 
00128 
00129 void GeomPQP2D::LoadEnvironment(string path) 
00130 {
00131   std::ifstream fin;
00132 
00133   Obst.clear();
00134   fin.open((FilePath+"Obst").c_str());
00135   if (fin) {
00136     fin >> ObstPolygons;
00137     // Make triangles to store in Obst
00138     Obst = PolygonsToTriangles(ObstPolygons,3.0);
00139   }
00140   fin.close();
00141 
00142   int i=0;
00143   list<MSLTriangle>::iterator t;
00144 
00145   Ob.BeginModel();
00146   PQP_REAL p1[3],p2[3],p3[3];
00147   forall(t,Obst) {
00148     p1[0] = (PQP_REAL) t->p1.xcoord();
00149     p1[1] = (PQP_REAL) t->p1.ycoord();
00150     p1[2] = (PQP_REAL) t->p1.zcoord();
00151     p2[0] = (PQP_REAL) t->p2.xcoord();
00152     p2[1] = (PQP_REAL) t->p2.ycoord();
00153     p2[2] = (PQP_REAL) t->p2.zcoord();
00154     p3[0] = (PQP_REAL) t->p3.xcoord();
00155     p3[1] = (PQP_REAL) t->p3.ycoord();
00156     p3[2] = (PQP_REAL) t->p3.zcoord();
00157     Ob.AddTri(p1,p2,p3,i);
00158     i++;
00159   }
00160   Ob.EndModel();
00161 
00162   //cout << "Obstacle region:\n" << ObstPolygons << "\n";
00163   //cout << "Number of polygons: " << ObstPolygons.size() << "\n";
00164 }
00165 
00166 
00167 
00168 void GeomPQP2D::LoadRobot(string path){
00169   std::ifstream fin;
00170 
00171   Robot.clear();
00172   fin.open((FilePath+"Robot").c_str());
00173   if (fin) {
00174     fin >> RobotPolygons;
00175     // Make triangles to store in Robot
00176     Robot = PolygonsToTriangles(RobotPolygons,3.0);
00177   }
00178   fin.close();
00179 
00180   int i=0;
00181   list<MSLTriangle>::iterator t;
00182 
00183   Ro.BeginModel();
00184   PQP_REAL p1[3],p2[3],p3[3];
00185   forall(t,Robot){
00186     p1[0] = (PQP_REAL) t->p1.xcoord();
00187     p1[1] = (PQP_REAL) t->p1.ycoord();
00188     p1[2] = (PQP_REAL) t->p1.zcoord();
00189     p2[0] = (PQP_REAL) t->p2.xcoord();
00190     p2[1] = (PQP_REAL) t->p2.ycoord();
00191     p2[2] = (PQP_REAL) t->p2.zcoord();
00192     p3[0] = (PQP_REAL) t->p3.xcoord();
00193     p3[1] = (PQP_REAL) t->p3.ycoord();
00194     p3[2] = (PQP_REAL) t->p3.zcoord();
00195     Ro.AddTri(p1,p2,p3,i);
00196     i++;
00197   }
00198   Ro.EndModel();
00199 }
00200 
00201 
00202 
00203 // *********************************************************************
00204 // *********************************************************************
00205 // CLASS:     GeomPQP2DPoint
00206 // 
00207 // *********************************************************************
00208 // *********************************************************************
00209 
00210 GeomPQP2DPoint::GeomPQP2DPoint(string path = ""):GeomPQP2D(path) {
00211 
00212   MaxDeviates = MSLVector(1.0,1.0);  // Translation only
00213 
00214   TR[2]=0.0;
00215   RR[0][0]=RR[1][1]=RR[2][2]=1.0;
00216   RR[0][1]=RR[0][2]=RR[1][0]= RR[1][2]= RR[2][0]= RR[2][1]=0.0;
00217 
00218 }
00219 
00220 
00221 
00222 bool GeomPQP2DPoint::CollisionFree(const MSLVector &q){
00223 
00224   TR[0]=(PQP_REAL)q[0];
00225   TR[1]=(PQP_REAL)q[1];
00226 
00227   PQP_CollideResult cres;  
00228   PQP_Collide(&cres,RR,TR,&Ro,RO,TO,&Ob,PQP_FIRST_CONTACT);
00229 
00230   return (cres.NumPairs() == 0);
00231 }
00232 
00233 
00234 
00235 double GeomPQP2DPoint::DistanceComp(const MSLVector &q){
00236 
00237   TR[0]=(PQP_REAL)q[0];
00238   TR[1]=(PQP_REAL)q[1];
00239 
00240   PQP_DistanceResult dres;  
00241   PQP_Distance(&dres,RR,TR,&Ro,RO,TO,&Ob,0.0,0.0);
00242 
00243   return dres.Distance();
00244 }
00245 
00246 
00247 
00248 // *********************************************************************
00249 // *********************************************************************
00250 // CLASS:     GeomPQP2DRigid
00251 // 
00252 // *********************************************************************
00253 // *********************************************************************
00254 
00255 GeomPQP2DRigid::GeomPQP2DRigid(string path = ""):GeomPQP2D(path) {
00256 
00257   // Compute the maximum deviates -- 2D with rotation
00258   double dmax = 0.0;
00259   double mag;
00260   list<MSLTriangle>::iterator tr;
00261 
00262   LoadRobot(path);
00263 
00264   forall(tr,Robot) {
00265     // Find maximum effective radius; ignore z distance here
00266     mag = sqrt(sqr(tr->p1.xcoord())+sqr(tr->p1.ycoord()));
00267     if (mag > dmax) 
00268       dmax = mag;
00269     mag = sqrt(sqr(tr->p2.xcoord())+sqr(tr->p2.ycoord()));
00270     if (mag > dmax) 
00271       dmax = mag;
00272   }
00273 
00274   MaxDeviates = MSLVector(1.0,1.0,dmax);
00275 
00276 }
00277 
00278 
00279 void GeomPQP2DRigid::SetTransformation(const MSLVector &q){
00280 
00281   // Set translation
00282   TR[0] = (PQP_REAL)q[0];
00283   TR[1] = (PQP_REAL)q[1];
00284   TR[2] = 0.0;
00285 
00286   // Set yaw rotation
00287   RR[0][0] = (PQP_REAL)(cos(q[2]));
00288   RR[0][1] = (PQP_REAL)(-sin(q[2]));
00289   RR[0][2] = 0.0;
00290   RR[1][0] = (PQP_REAL)(sin(q[2]));
00291   RR[1][1] = (PQP_REAL)(cos(q[2]));
00292   RR[1][2] = 0.0;
00293   RR[2][0] = 0.0;
00294   RR[2][1] = 0.0;
00295   RR[2][2] = 1.0;
00296  
00297 }
00298 
00299 
00300 bool GeomPQP2DRigid::CollisionFree(const MSLVector &q){
00301 
00302   SetTransformation(q);
00303 
00304   PQP_CollideResult cres;  
00305   PQP_Collide(&cres,RR,TR,&Ro,RO,TO,&Ob,PQP_FIRST_CONTACT);
00306 
00307   return (cres.NumPairs() == 0);
00308 }
00309 
00310 
00311 double GeomPQP2DRigid::DistanceComp(const MSLVector &q){
00312 
00313   SetTransformation(q);
00314 
00315   PQP_DistanceResult dres;  
00316   PQP_Distance(&dres,RR,TR,&Ro,RO,TO,&Ob,0.0,0.0);
00317 
00318   return dres.Distance();
00319 }
00320 
00321 
00322 
00323 
00324 MSLVector GeomPQP2DRigid::ConfigurationDifference(const MSLVector &q1, 
00325                                                     const MSLVector &q2)
00326 {
00327   MSLVector dq(3);
00328 
00329   dq[0] = q2[0] - q1[0];
00330   dq[1] = q2[1] - q1[1];
00331 
00332   if (fabs(q1[2]-q2[2]) < PI)
00333     dq[2] = q2[2] - q1[2];
00334   else {
00335     if (q1[2] < q2[2])
00336       dq[2] = -(2.0*PI - fabs(q1[2]-q2[2]));
00337     else
00338       dq[2] = (2.0*PI - fabs(q1[2]-q2[2]));
00339   }
00340 
00341   return dq;
00342 }
00343 
00344 
00345 
00346 // *********************************************************************
00347 // *********************************************************************
00348 // CLASS:     GeomPQP2DRigidMulti
00349 // 
00350 // *********************************************************************
00351 // *********************************************************************
00352 
00353 GeomPQP2DRigidMulti::GeomPQP2DRigidMulti(string path = ""):GeomPQP2DRigid(path) {
00354   
00355   // FilePath is set in the base class
00356   LoadEnvironment(FilePath);
00357   LoadRobot(FilePath);
00358   
00359   READ_OPTIONAL_PARAMETER(CollisionPairs);
00360 }
00361 
00362 
00363 
00364 void GeomPQP2DRigidMulti::LoadRobot(string path) {
00365   int i,j;
00366   string fname;
00367   list<MSLPolygon> pl;
00368 
00369   char* s = new char[50];
00370 
00371   // First check how many robot parts there are Robot0, Robot1, ...
00372   i = 0;
00373   sprintf(s,"%sRobot%d",FilePath.c_str(),i);
00374   while (is_file(s)) {
00375     i++;
00376     sprintf(s,"%sRobot%d",FilePath.c_str(),i);
00377   }
00378 
00379   NumBodies = i;
00380   
00381   if (NumBodies == 0)
00382     cout << "ERROR: No robot files at " << FilePath << "\n";
00383   
00384   Robot = vector<list<MSLTriangle> >(NumBodies);
00385   Ro = vector<PQP_Model>(NumBodies);
00386   
00387   for (i = 0; i < NumBodies; i++) {
00388     
00389     sprintf(s,"%sRobot%d",FilePath.c_str(),i);
00390     std::ifstream fin(s);
00391     pl.clear();
00392     fin >> pl;
00393 
00394     Robot[i] = PolygonsToTriangles(pl,5.0);
00395     j=0;
00396     list<MSLTriangle>::iterator t;
00397     
00398     Ro[i].BeginModel();
00399     PQP_REAL p1[3],p2[3],p3[3];
00400     forall(t,Robot[i]){
00401       p1[0] = (PQP_REAL) t->p1.xcoord();
00402       p1[1] = (PQP_REAL) t->p1.ycoord();
00403       p1[2] = (PQP_REAL) t->p1.zcoord();
00404       p2[0] = (PQP_REAL) t->p2.xcoord();
00405       p2[1] = (PQP_REAL) t->p2.ycoord();
00406       p2[2] = (PQP_REAL) t->p2.zcoord();
00407       p3[0] = (PQP_REAL) t->p3.xcoord();
00408       p3[1] = (PQP_REAL) t->p3.ycoord();
00409       p3[2] = (PQP_REAL) t->p3.zcoord();
00410       Ro[i].AddTri(p1,p2,p3,j);
00411       j++;
00412     }
00413     Ro[i].EndModel();
00414   }
00415 }
00416 
00417 
00418 bool GeomPQP2DRigidMulti::CollisionFree(const MSLVector &q){
00419   int i,j;
00420   list<MSLVector>::iterator v;
00421 
00422   PQP_CollideResult cres;  
00423   SetTransformation(q);
00424 
00425   // Check for collisions with obstacles
00426   for (i = 0; i < NumBodies; i++) {    
00427     PQP_Collide(&cres,RR[i],TR[i],&Ro[i],RO,TO,&Ob,PQP_FIRST_CONTACT);    
00428     if (cres.NumPairs() >= 1)
00429       return false;
00430   }
00431   
00432   // Check for pairwise collisions
00433   forall(v,CollisionPairs) {
00434     i = (int) v->operator[](0); 
00435     j = (int) v->operator[](1);
00436     PQP_Collide(&cres,RR[i],TR[i],&Ro[i],RR[j],TR[j],&Ro[j],PQP_FIRST_CONTACT);    
00437     if (cres.NumPairs() >= 1)
00438       return false;
00439   }
00440 
00441   return true;
00442 }
00443 
00444 
00445 double GeomPQP2DRigidMulti::DistanceComp(const MSLVector &q){
00446   int i,j;
00447   list<MSLVector>::iterator v;
00448   double dist = INFINITY;
00449 
00450   PQP_DistanceResult dres;  
00451   SetTransformation(q);
00452 
00453   // Check for collisions with obstacles
00454   for (i = 0; i < NumBodies; i++) {    
00455     PQP_Distance(&dres,RR[i],TR[i],&Ro[i],RO,TO,&Ob,0.0,0.0);    
00456     if (dres.Distance() < dist)
00457       dist = dres.Distance();
00458   }
00459   
00460   // Check for pairwise collisions
00461   forall(v,CollisionPairs) {
00462     i = (int) v->operator[](0); 
00463     j = (int) v->operator[](1);
00464     PQP_Distance(&dres,RR[i],TR[i],&Ro[i],RR[j],TR[j],&Ro[j],0.0,0.0);    
00465     if (dres.Distance() < dist)
00466       dist = dres.Distance();
00467   }
00468 
00469   return dist;
00470 }
00471 
00472 
00473 void GeomPQP2DRigidMulti::SetTransformation(const MSLVector &q){
00474 
00475   int i;
00476   MSLVector qi(3);
00477 
00478   for (i = 0; i < NumBodies; i++) {
00479     // Get the configuration
00480     qi[0] = q[i*3]; qi[1] = q[i*3+1]; 
00481     qi[2] = q[i*3+2]; 
00482     
00483     // Set translation
00484     TR[i][0]=(PQP_REAL)qi[0];
00485     TR[i][1]=(PQP_REAL)qi[1];
00486     TR[i][2]=0.0;
00487     
00488     // Set yaw rotation
00489     RR[i][0][0] = (PQP_REAL)(cos(qi[2]));
00490     RR[i][0][1] = (PQP_REAL)(-sin(qi[2]));
00491     RR[i][0][2] = 0.0;
00492     RR[i][1][0] = (PQP_REAL)(sin(qi[2]));
00493     RR[i][1][1] = (PQP_REAL)(cos(qi[2]));
00494     RR[i][1][2] = 0.0;
00495     RR[i][2][0] = 0.0;
00496     RR[i][2][1] = 0.0;
00497     RR[i][2][2] = 1.0;    
00498   }
00499 }
00500 
00501 
00502 
00503 // *********************************************************************
00504 // *********************************************************************
00505 // CLASS:     GeomPQP3DRigid
00506 // 
00507 // *********************************************************************
00508 // *********************************************************************
00509 
00510 GeomPQP3DRigid::GeomPQP3DRigid(string path = ""):GeomPQP(path) {
00511 
00512   LoadEnvironment(FilePath);
00513   LoadRobot(FilePath);
00514 
00515   // Compute the maximum deviates -- 2D with rotation
00516   MaxDeviates = MSLVector(6);
00517   MaxDeviates[0] = 1.0;
00518   MaxDeviates[1] = 1.0;
00519   MaxDeviates[2] = 1.0;
00520   double dmax1 = 0.0;
00521   double dmax2 = 0.0;
00522   double dmax3 = 0.0;
00523   double mag;
00524 
00525   list<MSLTriangle>::iterator tr;
00526 
00527   forall(tr,Robot) {
00528     // Check roll deviations
00529     mag = sqrt(sqr(tr->p1.ycoord())+sqr(tr->p1.zcoord()));
00530     if (mag > dmax1) 
00531       dmax1 = mag;
00532     mag = sqrt(sqr(tr->p2.ycoord())+sqr(tr->p2.zcoord()));
00533     if (mag > dmax1) 
00534       dmax1 = mag;
00535     mag = sqrt(sqr(tr->p3.ycoord())+sqr(tr->p3.zcoord()));
00536     if (mag > dmax1) 
00537       dmax1 = mag;
00538     // Check pitch deviations
00539     mag = sqrt(sqr(tr->p1.xcoord())+sqr(tr->p1.zcoord()));
00540     if (mag > dmax2) 
00541       dmax2 = mag;
00542     mag = sqrt(sqr(tr->p2.xcoord())+sqr(tr->p2.zcoord()));
00543     if (mag > dmax2) 
00544       dmax2 = mag;
00545     mag = sqrt(sqr(tr->p3.xcoord())+sqr(tr->p3.zcoord()));
00546     if (mag > dmax2) 
00547       dmax2 = mag;
00548     // Check yaw deviations
00549     mag = sqrt(sqr(tr->p1.xcoord())+sqr(tr->p1.ycoord()));
00550     if (mag > dmax3) 
00551       dmax3 = mag;
00552     mag = sqrt(sqr(tr->p2.xcoord())+sqr(tr->p2.ycoord()));
00553     if (mag > dmax3) 
00554       dmax3 = mag;
00555     mag = sqrt(sqr(tr->p3.xcoord())+sqr(tr->p3.ycoord()));
00556     if (mag > dmax3) 
00557       dmax3 = mag;
00558   }
00559 
00560   MaxDeviates[3] = dmax1;
00561   MaxDeviates[4] = dmax2;
00562   MaxDeviates[5] = dmax3;
00563 
00564   //cout << "MD: " << MaxDeviates << "\n";
00565 }
00566 
00567 
00568 bool GeomPQP3DRigid::CollisionFree(const MSLVector &q){
00569 
00570   SetTransformation(q);
00571 
00572 
00573   PQP_CollideResult cres;  
00574   PQP_Collide(&cres,RR,TR,&Ro,RO,TO,&Ob,PQP_FIRST_CONTACT);
00575 
00576   return (cres.NumPairs() == 0);
00577 }
00578 
00579 
00580 
00581 
00582 double GeomPQP3DRigid::DistanceComp(const MSLVector &q){
00583 
00584   SetTransformation(q);
00585 
00586   PQP_DistanceResult dres;  
00587   PQP_Distance(&dres,RR,TR,&Ro,RO,TO,&Ob,0.0,0.0);
00588 
00589   return dres.Distance();
00590 }
00591 
00592 
00593 
00594 
00595 MSLVector GeomPQP3DRigid::ConfigurationDifference(const MSLVector &q1, 
00596                                                 const MSLVector &q2)
00597 {
00598   MSLVector dq(6);
00599 
00600   dq[0] = q2[0] - q1[0];
00601   dq[1] = q2[1] - q1[1];
00602   dq[1] = q2[1] - q1[1];
00603 
00604   if (fabs(q1[3]-q2[3]) < PI)
00605     dq[3] = q2[3] - q1[3];
00606   else {
00607     if (q1[3] < q2[3])
00608       dq[3] = -(2.0*PI - fabs(q1[3]-q2[3]));
00609     else
00610       dq[3] = (2.0*PI - fabs(q1[3]-q2[3]));
00611   }
00612 
00613   if (fabs(q1[4]-q2[4]) < PI)
00614     dq[4] = q2[4] - q1[4];
00615   else {
00616     if (q1[4] < q2[4])
00617       dq[4] = -(2.0*PI - fabs(q1[4]-q2[4]));
00618     else
00619       dq[4] = (2.0*PI - fabs(q1[4]-q2[4]));
00620   }
00621 
00622   if (fabs(q1[5]-q2[5]) < PI)
00623     dq[5] = q2[5] - q1[5];
00624   else {
00625     if (q1[5] < q2[5])
00626       dq[5] = -(2.0*PI - fabs(q1[5]-q2[5]));
00627     else
00628       dq[5] = (2.0*PI - fabs(q1[5]-q2[5]));
00629   }
00630 
00631   return dq;
00632 }
00633 
00634 
00635 
00636 void GeomPQP3DRigid::SetTransformation(const MSLVector &q){
00637 
00638   // Set translation
00639   TR[0]=(PQP_REAL)q[0];
00640   TR[1]=(PQP_REAL)q[1];
00641   TR[2]=(PQP_REAL)q[2];
00642 
00643   // Set rotation
00644   RR[0][0]=(PQP_REAL)(cos(q[5])*cos(q[4]));
00645   RR[0][1]=(PQP_REAL)(cos(q[5])*sin(q[4])*sin(q[3])-sin(q[5])*cos(q[3]));
00646   RR[0][2]=(PQP_REAL)(cos(q[5])*sin(q[4])*cos(q[3])+sin(q[5])*sin(q[3]));
00647   RR[1][0]=(PQP_REAL)(sin(q[5])*cos(q[4]));
00648   RR[1][1]=(PQP_REAL)(sin(q[5])*sin(q[4])*sin(q[3])+cos(q[5])*cos(q[3]));
00649   RR[1][2]=(PQP_REAL)(sin(q[5])*sin(q[4])*cos(q[3])-cos(q[5])*sin(q[3]));
00650   RR[2][0]=(PQP_REAL)((-1)*sin(q[4]));
00651   RR[2][1]=(PQP_REAL)(cos(q[4])*sin(q[3]));
00652   RR[2][2]=(PQP_REAL)(cos(q[4])*cos(q[3]));
00653 
00654 }
00655 
00656 
00657 
00658 
00659 
00660 // *********************************************************************
00661 // *********************************************************************
00662 // CLASS:     GeomPQP3DRigidMulti
00663 // 
00664 // *********************************************************************
00665 // *********************************************************************
00666 
00667 GeomPQP3DRigidMulti::GeomPQP3DRigidMulti(string path = ""):GeomPQP3DRigid(path) {
00668 
00669   LoadEnvironment(FilePath);
00670   LoadRobot(FilePath);
00671 
00672   READ_OPTIONAL_PARAMETER(CollisionPairs);
00673 }
00674 
00675 
00676 
00677 void GeomPQP3DRigidMulti::LoadRobot(string path){
00678   int i,j;
00679   string fname;
00680 
00681   char* s = new char[50];
00682 
00683   // First check how many robot parts there are Robot0, Robot1, ...
00684   i = 0;
00685   sprintf(s,"%sRobot%d",FilePath.c_str(),i);
00686   while (is_file(s)) {
00687     i++;
00688     sprintf(s,"%sRobot%d",FilePath.c_str(),i);
00689   }
00690 
00691   NumBodies = i;
00692   
00693   if (NumBodies == 0)
00694     cout << "ERROR: No robot files at " << FilePath << "\n";
00695   
00696   Robot = vector<list<MSLTriangle> >(NumBodies);
00697   Ro = vector<PQP_Model>(NumBodies);
00698   
00699   for (i = 0; i < NumBodies; i++) {
00700     
00701     sprintf(s,"%sRobot%d",FilePath.c_str(),i);
00702     std::ifstream fin(s);
00703     fin >> Robot[i];
00704     
00705     j=0;
00706     list<MSLTriangle>::iterator t;
00707     
00708     Ro[i].BeginModel();
00709     PQP_REAL p1[3],p2[3],p3[3];
00710     forall(t,Robot[i]){
00711       p1[0] = (PQP_REAL) t->p1.xcoord();
00712       p1[1] = (PQP_REAL) t->p1.ycoord();
00713       p1[2] = (PQP_REAL) t->p1.zcoord();
00714       p2[0] = (PQP_REAL) t->p2.xcoord();
00715       p2[1] = (PQP_REAL) t->p2.ycoord();
00716       p2[2] = (PQP_REAL) t->p2.zcoord();
00717       p3[0] = (PQP_REAL) t->p3.xcoord();
00718       p3[1] = (PQP_REAL) t->p3.ycoord();
00719       p3[2] = (PQP_REAL) t->p3.zcoord();
00720       Ro[i].AddTri(p1,p2,p3,j);
00721       j++;
00722     }
00723     Ro[i].EndModel();
00724   }
00725 }
00726 
00727 
00728 bool GeomPQP3DRigidMulti::CollisionFree(const MSLVector &q){
00729   int i,j;
00730   list<MSLVector>::iterator v;
00731 
00732   PQP_CollideResult cres;  
00733   SetTransformation(q);
00734 
00735   // Check for collisions with obstacles
00736   for (i = 0; i < NumBodies; i++) {    
00737     PQP_Collide(&cres,RR[i],TR[i],&Ro[i],RO,TO,&Ob,PQP_FIRST_CONTACT);    
00738     if (cres.NumPairs() >= 1)
00739       return false;
00740   }
00741   
00742   // Check for pairwise collisions
00743   forall(v,CollisionPairs) {
00744     i = (int) v->operator[](0); 
00745     j = (int) v->operator[](1);
00746     PQP_Collide(&cres,RR[i],TR[i],&Ro[i],RR[j],TR[j],&Ro[j],PQP_FIRST_CONTACT);    
00747     if (cres.NumPairs() >= 1)
00748       return false;
00749   }
00750 
00751   return true;
00752 }
00753 
00754 
00755 double GeomPQP3DRigidMulti::DistanceComp(const MSLVector &q){
00756   int i,j;
00757   list<MSLVector>::iterator v;
00758   double dist = INFINITY;
00759 
00760   PQP_DistanceResult dres;  
00761   SetTransformation(q);
00762 
00763   // Check for collisions with obstacles
00764   for (i = 0; i < NumBodies; i++) {    
00765     PQP_Distance(&dres,RR[i],TR[i],&Ro[i],RO,TO,&Ob,0.0,0.0);    
00766     if (dres.Distance() < dist)
00767       dist = dres.Distance();
00768   }
00769   
00770   // Check for pairwise collisions
00771   forall(v,CollisionPairs) {
00772     i = (int) v->operator[](0); 
00773     j = (int) v->operator[](1);
00774     PQP_Distance(&dres,RR[i],TR[i],&Ro[i],RR[j],TR[j],&Ro[j],0.0,0.0);    
00775     if (dres.Distance() < dist)
00776       dist = dres.Distance();
00777   }
00778 
00779   return dist;
00780 }
00781 
00782 
00783 void GeomPQP3DRigidMulti::SetTransformation(const MSLVector &q){
00784 
00785   int i;
00786   MSLVector qi(6);
00787 
00788   for (i = 0; i < NumBodies; i++) {
00789     // Get the configuration
00790     qi[0] = q[i*6]; qi[1] = q[i*6+1]; qi[2] = q[i*6+2]; 
00791     qi[3] = q[i*6+3]; qi[4] = q[i*6+4]; qi[5] = q[i*6+5]; 
00792     
00793     // Set translation
00794     TR[i][0]=(PQP_REAL)qi[0];
00795     TR[i][1]=(PQP_REAL)qi[1];
00796     TR[i][2]=(PQP_REAL)qi[2];
00797     
00798     // Set rotation
00799     RR[i][0][0]=(PQP_REAL)(cos(qi[5])*cos(qi[4]));
00800     RR[i][0][1]=(PQP_REAL)(cos(qi[5])*sin(qi[4])*sin(qi[3])-sin(qi[5])*cos(qi[3]));
00801     RR[i][0][2]=(PQP_REAL)(cos(qi[5])*sin(qi[4])*cos(qi[3])+sin(qi[5])*sin(qi[3]));
00802     RR[i][1][0]=(PQP_REAL)(sin(qi[5])*cos(qi[4]));
00803     RR[i][1][1]=(PQP_REAL)(sin(qi[5])*sin(qi[4])*sin(qi[3])+cos(qi[5])*cos(qi[3]));
00804     RR[i][1][2]=(PQP_REAL)(sin(qi[5])*sin(qi[4])*cos(qi[3])-cos(qi[5])*sin(qi[3]));
00805     RR[i][2][0]=(PQP_REAL)((-1)*sin(qi[4]));
00806     RR[i][2][1]=(PQP_REAL)(cos(qi[4])*sin(qi[3]));
00807     RR[i][2][2]=(PQP_REAL)(cos(qi[4])*cos(qi[3]));
00808   }
00809 }
00810
geomPQP.C
