Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

geomPQP.C

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


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Anna Atramentov, Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.