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>