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