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 <list.h> 00023 #include <vector.h> 00024 #include <PQP/PQP.h> 00025 00026 #include <stdlib.h> 00027 #include <stdio.h> 00028 #include <string.h> 00029 00030 #include "geom.h" 00031 #include "point.h" // This includes the MSLPolygon class 00032 #include "triangle.h" 00033 #include "util.h" 00034 00035 // OK, this is bad. The maximum number should be dynamic, but PQP 00036 // really seems to want a static matrix. 00037 #define MAXBODIES 100 00038 00040 00041 class GeomPQP: public Geom { 00042 protected: 00043 PQP_REAL RR[3][3],RO[3][3]; 00044 PQP_REAL TR[3],TO[3]; 00045 public: 00046 list<MSLTriangle> Obst; 00047 list<MSLTriangle> Robot; 00048 PQP_Model Ro, Ob; 00049 GeomPQP(string path); 00050 virtual ~GeomPQP() {}; 00051 virtual void LoadEnvironment(string path); 00052 virtual void LoadRobot(string path); 00053 virtual bool CollisionFree(const MSLVector &q){return 0;} 00054 virtual double DistanceComp(const MSLVector &q){return 0;} 00055 }; 00056 00058 00059 class GeomPQP2D: public GeomPQP { 00060 public: 00061 list<MSLPolygon> ObstPolygons; 00062 list<MSLPolygon> RobotPolygons; 00063 GeomPQP2D(string path); 00064 virtual ~GeomPQP2D() {}; 00065 virtual void LoadEnvironment(string path); 00066 virtual void LoadRobot(string path); 00067 }; 00068 00069 00071 00072 class GeomPQP2DPoint: public GeomPQP2D { 00073 public: 00074 GeomPQP2DPoint(string path); 00075 virtual ~GeomPQP2DPoint() {}; 00076 virtual bool CollisionFree(const MSLVector &q); // Input is configuration 00077 virtual double DistanceComp(const MSLVector &q); // Distance in world 00078 }; 00079 00080 00082 00083 class GeomPQP2DRigid: public GeomPQP2D { 00084 public: 00085 GeomPQP2DRigid(string path); 00086 virtual ~GeomPQP2DRigid() {}; 00087 virtual bool CollisionFree(const MSLVector &q); // Input is configuration 00088 virtual double DistanceComp(const MSLVector &q); // Distance in world 00089 virtual MSLVector ConfigurationDifference(const MSLVector &q1, 00090 const MSLVector &q2); 00091 void SetTransformation(const MSLVector &q); // Input is configuration 00092 }; 00093 00094 00096 class GeomPQP2DRigidMulti: public GeomPQP2DRigid { 00097 private: 00098 vector<list<MSLTriangle> > Robot; 00099 vector<PQP_Model> Ro; 00100 list<MSLVector> CollisionPairs; // Index pairs to check for collision 00101 PQP_REAL TR[MAXBODIES][3]; // Robot translation 00102 PQP_REAL RR[MAXBODIES][3][3]; // Robot rotation 00103 public: 00104 bool SelfCollisionCheck; 00105 GeomPQP2DRigidMulti(string path); 00106 virtual ~GeomPQP2DRigidMulti() {}; 00107 virtual bool CollisionFree(const MSLVector &q); // Input is configuration 00108 virtual double DistanceComp(const MSLVector &q); // Distance in world 00109 virtual void LoadRobot(string path); // Load multiple robots 00110 void SetTransformation(const MSLVector &q); // Input is configuration 00111 }; 00112 00113 00115 class GeomPQP3DRigid: public GeomPQP { 00116 public: 00117 GeomPQP3DRigid(string path); 00118 virtual ~GeomPQP3DRigid() {}; 00119 virtual bool CollisionFree(const MSLVector &q); // Input is configuration 00120 virtual double DistanceComp(const MSLVector &q); // Distance in world 00121 virtual MSLVector ConfigurationDifference(const MSLVector &q1, 00122 const MSLVector &q2); 00123 void SetTransformation(const MSLVector &q); // Input is configuration 00124 }; 00125 00126 00128 class GeomPQP3DRigidMulti: public GeomPQP3DRigid { 00129 private: 00130 vector<list<MSLTriangle> > Robot; 00131 vector<PQP_Model> Ro; 00132 list<MSLVector> CollisionPairs; // Index pairs to check for collision 00133 PQP_REAL TR[MAXBODIES][3]; // Robot translation 00134 PQP_REAL RR[MAXBODIES][3][3]; // Robot rotation 00135 public: 00136 bool SelfCollisionCheck; 00137 GeomPQP3DRigidMulti(string path); 00138 virtual ~GeomPQP3DRigidMulti() {}; 00139 virtual bool CollisionFree(const MSLVector &q); // Input is configuration 00140 virtual double DistanceComp(const MSLVector &q); // Distance in world 00141 virtual void LoadRobot(string path); // Load multiple robots 00142 void SetTransformation(const MSLVector &q); // Input is configuration 00143 }; 00144 00145 #endif 00146 00147 00148