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 #include <vector.h> 00024 00025 #include "marray.h" 00026 #include "planner.h" 00027 #include "tree.h" 00028 #include "vector.h" 00029 #include "util.h" 00030 00031 #define UNVISITED 0 00032 #define VISITED 1 // Visited by the first tree, G 00033 #define COLLISION 2 00034 #define VISITED2 3 // Visited by the second tree, G2 00035 #define GOAL 4 // Lies in goal region (not used in base class) 00036 00037 00059 00060 00061 00062 class FDP: public IncrementalPlanner { 00063 protected: 00064 00066 priority_queue<MSLNode*,vector<MSLNode*>,MSLNodeGreater> Q; 00067 00069 MultiArray<int> *Grid; 00070 00072 vector<int> GridDimensions; 00073 00075 int GridDefaultResolution; 00076 00078 MSLVector Quantization; 00079 00080 virtual double SearchCost(double initcost, 00081 MSLNode* &n, 00082 MSLNode* &nn); 00083 00084 virtual vector<int> StateToIndices(const MSLVector &x); 00085 00086 virtual MSLVector IndicesToState(const vector<int> &indices); 00087 00088 public: 00089 00091 FDP(Problem *problem); 00092 00094 ~FDP() {}; 00095 00097 int SatisfiedCount; 00098 00100 virtual void Reset(); 00101 00103 virtual bool Plan(); 00104 00105 }; 00106 00107 00108 00110 class FDPStar: public FDP { 00111 protected: 00112 virtual double SearchCost(double initcost, 00113 MSLNode* &n, 00114 MSLNode* &nn); 00115 public: 00116 FDPStar(Problem *p); 00117 }; 00118 00119 00121 class FDPBestFirst: public FDP { 00122 protected: 00123 virtual double SearchCost(double initcost, 00124 MSLNode* &n, 00125 MSLNode* &nn); 00126 public: 00127 FDPBestFirst(Problem *p); 00128 }; 00129 00130 00132 class FDPBi: public FDP { 00133 protected: 00134 00136 priority_queue<MSLNode*,vector<MSLNode*>,MSLNodeGreater> Q2; 00137 00139 void RecoverSolution(MSLNode* &n1, MSLNode* &n2); 00140 00141 public: 00142 00144 FDPBi(Problem *problem); 00145 00147 ~FDPBi() {}; 00148 00150 virtual void Reset(); 00151 00153 virtual bool Plan(); 00154 }; 00155 00156 00157 #endif 00158 00159