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_FDP_H 00020 #define MSL_FDP_H 00021 00022 #include <queue> 00023 #ifdef WIN32 00024 using namespace std; 00025 #endif 00026 //#include <vector.h> 00027 00028 #include "marray.h" 00029 #include "planner.h" 00030 #include "tree.h" 00031 #include "vector.h" 00032 #include "defs.h" 00033 #include "mslio.h" 00034 00035 #define UNVISITED 0 00036 #define VISITED 1 // Visited by the first tree, G 00037 #define COLLISION 2 00038 #define VISITED2 3 // Visited by the second tree, G2 00039 #define GOAL 4 // Lies in goal region (not used in base class) 00040 00041 00062 00065 00066 class FDP: public IncrementalPlanner { 00067 protected: 00068 00070 priority_queue<MSLNode*,vector<MSLNode*>,MSLNodeGreater> Q; 00071 00073 MultiArray<int> *Grid; 00074 00076 vector<int> GridDimensions; 00077 00079 int GridDefaultResolution; 00080 00082 MSLVector Quantization; 00083 00084 virtual double SearchCost(double initcost, 00085 MSLNode* &n, 00086 MSLNode* &nn); 00087 00088 virtual vector<int> StateToIndices(const MSLVector &x); 00089 00090 virtual MSLVector IndicesToState(const vector<int> &indices); 00091 00092 public: 00093 00095 FDP(Problem *problem); 00096 00098 ~FDP() {}; 00099 00101 int SatisfiedCount; 00102 00104 virtual void Reset(); 00105 00107 virtual bool Plan(); 00108 00109 }; 00110 00111 00112 00114 class FDPStar: public FDP { 00115 protected: 00116 virtual double SearchCost(double initcost, 00117 MSLNode* &n, 00118 MSLNode* &nn); 00119 public: 00120 FDPStar(Problem *p); 00121 }; 00122 00123 00125 class FDPBestFirst: public FDP { 00126 protected: 00127 virtual double SearchCost(double initcost, 00128 MSLNode* &n, 00129 MSLNode* &nn); 00130 public: 00131 FDPBestFirst(Problem *p); 00132 }; 00133 00134 00136 class FDPBi: public FDP { 00137 protected: 00138 00140 priority_queue<MSLNode*,vector<MSLNode*>,MSLNodeGreater> Q2; 00141 00143 void RecoverSolution(MSLNode* &n1, MSLNode* &n2); 00144 00145 public: 00146 00148 FDPBi(Problem *problem); 00149 00151 ~FDPBi() {}; 00152 00154 virtual void Reset(); 00155 00157 virtual bool Plan(); 00158 }; 00159 00160 00161 #endif 00162 00163