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 #ifndef MSL_GEOMPQP_H 00020 #define MSL_GEOMPQP_H 00021 00022 #include <LEDA/file.h> 00023 #include <LEDA/array.h> 00024 #include <LEDA/array2.h> 00025 #include <LEDA/list.h> 00026 #include <LEDA/matrix.h> 00027 #include <LEDA/point.h> 00028 #include <LEDA/polygon.h> 00029 #include <LEDA/stream.h> 00030 #include <LEDA/string.h> 00031 #include <LEDA/vector.h> 00032 #include <LEDA/graph.h> 00033 #include <LEDA/d3_point.h> 00034 00035 #include "geom.h" 00036 #include "3Dtriangle.h" 00037 00038 #include <PQP/PQP.h> 00039 00040 #include <LEDA/REDEFINE_NAMES.h> 00041 00042 // OK, this is bad. The maximum number should be dynamic, but PQP 00043 // really seems to want a static matrix. 00044 #define MAXBODIES 100 00045 00047 00048 class GeomPQP: public Geom { 00049 protected: 00050 PQP_REAL RR[3][3],RO[3][3]; 00051 PQP_REAL TR[3],TO[3]; 00052 public: 00053 list<Triangle> Obst; 00054 list<Triangle> Robot; 00055 PQP_Model Ro, Ob; 00056 GeomPQP(string path); 00057 virtual ~GeomPQP() {}; 00058 virtual void LoadEnvironment(string path); 00059 virtual void LoadRobot(string path); 00060 virtual bool CollisionFree(const vector &q){return 0;} 00061 virtual double DistanceComp(const vector &q){return 0;} 00062 virtual list<polygon> EnvironmentToLedaPolygons(); 00063 virtual list<polygon> RobotToLedaPolygons(const vector &q); 00064 }; 00065 00066 00068 00069 class GeomPQP2D: public GeomPQP { 00070 public: 00071 list<polygon> ObstLedaPolygons; 00072 list<polygon> BorderLedaPolygons; 00073 list<polygon> RobotLedaPolygons; 00074 GeomPQP2D(string path); 00075 virtual ~GeomPQP2D() {}; 00076 virtual void LoadEnvironment(string path); 00077 virtual void LoadRobot(string path); 00078 virtual list<polygon> EnvironmentToLedaPolygons(); 00079 }; 00080 00081 00083 00084 class GeomPQP2DPoint: public GeomPQP2D { 00085 public: 00086 GeomPQP2DPoint(string path); 00087 virtual ~GeomPQP2DPoint() {}; 00088 virtual bool CollisionFree(const vector &q); // Input is configuration 00089 virtual double DistanceComp(const vector &q); // Distance in world 00090 }; 00091 00092 00094 00095 class GeomPQP2DRigid: public GeomPQP2D { 00096 public: 00097 GeomPQP2DRigid(string path); 00098 virtual ~GeomPQP2DRigid() {}; 00099 virtual bool CollisionFree(const vector &q); // Input is configuration 00100 virtual double DistanceComp(const vector &q); // Distance in world 00101 virtual vector ConfigurationDifference(const vector &q1, 00102 const vector &q2); 00103 virtual list<polygon> RobotToLedaPolygons(const vector &q); 00104 void SetTransformation(const vector &q); // Input is configuration 00105 }; 00106 00107 00108 00110 00111 class GeomPQP2DRigidMulti: public GeomPQP2DRigid { 00112 private: 00113 array<list<Triangle> > Robot; 00114 array<PQP_Model> Ro; 00115 list<vector> CollisionPairs; // Index pairs to check for collision 00116 PQP_REAL TR[MAXBODIES][3]; // Robot translation 00117 PQP_REAL RR[MAXBODIES][3][3]; // Robot rotation 00118 public: 00119 bool SelfCollisionCheck; 00120 GeomPQP2DRigidMulti(string path); 00121 virtual ~GeomPQP2DRigidMulti() {}; 00122 virtual bool CollisionFree(const vector &q); // Input is configuration 00123 virtual double DistanceComp(const vector &q); // Distance in world 00124 virtual void LoadRobot(string path); // Load multiple robots 00125 void SetTransformation(const vector &q); // Input is configuration 00126 }; 00127 00128 00129 00131 00132 class GeomPQP3DRigid: public GeomPQP { 00133 public: 00134 GeomPQP3DRigid(string path); 00135 virtual ~GeomPQP3DRigid() {}; 00136 virtual bool CollisionFree(const vector &q); // Input is configuration 00137 virtual double DistanceComp(const vector &q); // Distance in world 00138 virtual vector ConfigurationDifference(const vector &q1, 00139 const vector &q2); 00140 void SetTransformation(const vector &q); // Input is configuration 00141 }; 00142 00143 00145 class GeomPQP3DRigidMulti: public GeomPQP3DRigid { 00146 private: 00147 array<list<Triangle> > Robot; 00148 array<PQP_Model> Ro; 00149 list<vector> CollisionPairs; // Index pairs to check for collision 00150 PQP_REAL TR[MAXBODIES][3]; // Robot translation 00151 PQP_REAL RR[MAXBODIES][3][3]; // Robot rotation 00152 public: 00153 bool SelfCollisionCheck; 00154 GeomPQP3DRigidMulti(string path); 00155 virtual ~GeomPQP3DRigidMulti() {}; 00156 virtual bool CollisionFree(const vector &q); // Input is configuration 00157 virtual double DistanceComp(const vector &q); // Distance in world 00158 virtual void LoadRobot(string path); // Load multiple robots 00159 void SetTransformation(const vector &q); // Input is configuration 00160 }; 00161 00162 #include <LEDA/UNDEFINE_NAMES.h> 00163 00164 #endif 00165 00166 00167 00168 00169 00170