00001 //---------------------------------------------------------------------- 00002 // The Motion Strategy Library (MSL) 00003 //---------------------------------------------------------------------- 00004 // 00005 // Copyright (c) University of Illinois and Steven M. 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 // The University of Illinois 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 true;} 00054 virtual double DistanceComp(const MSLVector &q){return 10000.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 GeomPQP2DRigid: public GeomPQP2D { 00073 public: 00074 GeomPQP2DRigid(string path); 00075 virtual ~GeomPQP2DRigid() {}; 00076 virtual bool CollisionFree(const MSLVector &q); // Input is configuration 00077 virtual double DistanceComp(const MSLVector &q); // Distance in world 00078 virtual MSLVector ConfigurationDifference(const MSLVector &q1, 00079 const MSLVector &q2); 00080 void SetTransformation(const MSLVector &q); // Input is configuration 00081 }; 00082 00083 00085 class GeomPQP2DRigidMulti: public GeomPQP2DRigid { 00086 private: 00087 vector<list<MSLTriangle> > Robot; 00088 vector<PQP_Model> Ro; 00089 list<MSLVector> CollisionPairs; // Index pairs to check for collision 00090 PQP_REAL TR[MAXBODIES][3]; // Robot translation 00091 PQP_REAL RR[MAXBODIES][3][3]; // Robot rotation 00092 public: 00093 bool SelfCollisionCheck; 00094 GeomPQP2DRigidMulti(string path); 00095 virtual ~GeomPQP2DRigidMulti() {}; 00096 virtual bool CollisionFree(const MSLVector &q); // Input is configuration 00097 virtual double DistanceComp(const MSLVector &q); // Distance in world 00098 virtual void LoadRobot(string path); // Load multiple robots 00099 void SetTransformation(const MSLVector &q); // Input is configuration 00100 }; 00101 00102 00104 class GeomPQP3DRigid: public GeomPQP { 00105 public: 00106 GeomPQP3DRigid(string path); 00107 virtual ~GeomPQP3DRigid() {}; 00108 virtual bool CollisionFree(const MSLVector &q); // Input is configuration 00109 virtual double DistanceComp(const MSLVector &q); // Distance in world 00110 virtual MSLVector ConfigurationDifference(const MSLVector &q1, 00111 const MSLVector &q2); 00112 void SetTransformation(const MSLVector &q); // Input is configuration 00113 }; 00114 00115 00117 class GeomPQP3DRigidMulti: public GeomPQP3DRigid { 00118 private: 00119 vector<list<MSLTriangle> > Robot; 00120 vector<PQP_Model> Ro; 00121 list<MSLVector> CollisionPairs; // Index pairs to check for collision 00122 PQP_REAL TR[MAXBODIES][3]; // Robot translation 00123 PQP_REAL RR[MAXBODIES][3][3]; // Robot rotation 00124 public: 00125 bool SelfCollisionCheck; 00126 GeomPQP3DRigidMulti(string path); 00127 virtual ~GeomPQP3DRigidMulti() {}; 00128 virtual bool CollisionFree(const MSLVector &q); // Input is configuration 00129 virtual double DistanceComp(const MSLVector &q); // Distance in world 00130 virtual void LoadRobot(string path); // Load multiple robots 00131 void SetTransformation(const MSLVector &q); // Input is configuration 00132 }; 00133 00134 #endif 00135 00136 00137