00001
00002 #ifndef INCLUDED_PathPlanner_h_
00003 #define INCLUDED_PathPlanner_h_
00004
00005 #include <map>
00006 #include <iostream>
00007 #include <queue>
00008 #include <map>
00009 #include <bitset>
00010 #include "DualCoding.h"
00011
00012 namespace DualCoding {
00013
00014
00015
00016
00017 class PathPlanner {
00018 public:
00019 PathPlanner(coordinate_t xMax, coordinate_t xMin, coordinate_t yMax, coordinate_t yMin);
00020 virtual ~PathPlanner() {
00021 delete [] unreached;
00022 delete [] costMap;
00023 }
00024
00025
00026 struct direction {
00027 std::bitset<2> bitVal;
00028 direction (unsigned int i) : bitVal(i) {}
00029 operator unsigned int() const { return (unsigned int) bitVal.to_ulong(); }
00030 enum directions { N=0,W=1,S=2,E=3 };
00031 direction cw() const { direction cwd(bitVal.to_ulong()+1); return cwd; }
00032 direction ccw() const { direction ccwd(bitVal.to_ulong()-1); return ccwd; }
00033 };
00034 struct location {
00035 unsigned int pos;
00036 direction dir;
00037 location()
00038 : pos(0), dir(direction::N) {}
00039 location(unsigned int _pos, direction _dir)
00040 : pos(_pos), dir(_dir) {}
00041 location(const location& l)
00042 : pos(l.pos), dir(l.dir) {}
00043 bool operator==(const location& l) const { return pos==l.pos && dir==l.dir; }
00044 };
00045 struct state {
00046 location loc;
00047 unsigned int cost;
00048 std::pair<int,int> lms;
00049 state* dst;
00050 state(location _loc, int lm1, int lm2)
00051 : loc(_loc), cost(0), lms(lm1, lm2), dst(NULL) {}
00052 state(location _loc, std::pair<int,int> _lms)
00053 : loc(_loc), cost(0), lms(_lms), dst(NULL) {}
00054 state(const state& s)
00055 : loc(s.loc), cost(s.cost), lms(s.lms), dst(&(*s.dst)) {}
00056 state& operator=(const state& s)
00057 { if (this!=&s) { loc = s.loc; cost = s.cost; lms = s.lms; dst = s.dst; } return *this; }
00058 };
00059 struct edge {
00060 state *from, *to;
00061 unsigned int cost;
00062 edge(state& _from, state& _to, unsigned int _cost) : from(&_from), to(&_to), cost(_cost) {}
00063 edge(const edge& l) : from(&(*l.from)), to(&(*l.to)), cost(l.cost) {}
00064 edge& operator=(const edge& l)
00065 { if (this!=&l) { from = &(*l.from); to = &(*l.to); cost = l.cost; } return *this; }
00066 };
00067
00068
00069 protected:
00070 std::queue<state> allStates;
00071 std::vector<state*> *unreached;
00072
00073 struct lessCost { bool operator()(const edge& e1, const edge& e2) const { return e1.cost > e2.cost; }};
00074 std::priority_queue<edge,std::vector<edge>,lessCost> reached;
00075 static const int size=300;
00076 unsigned int numX, numY;
00077 float minX, minY, dX, dY;
00078 location start, goal;
00079 public:
00080 unsigned int maxDistance;
00081 enum Cost { transF, transB, transY, tranB, rotate, noLM, oneLM };
00082 std::map <Cost, unsigned int> costs;
00083 Point startPt, goalPt;
00084 AngTwoPi startOrientation, goalOrientation;
00085
00086 protected:
00087 unsigned int *costMap;
00088
00089 std::map<unsigned int, PointData> landmarks;
00090
00091 std::map<unsigned int, unsigned int> lmCosts;
00092 std::vector<std::pair<Point, unsigned int> > obstacles;
00093
00094 public:
00095 void findPath();
00096 void addLandmark(const ShapeRoot& lm) {
00097 landmarks.insert(std::make_pair<unsigned int,PointData>
00098 (lm->getId(),PointData(VRmixin::worldShS,Point(lm->getCentroid()))));
00099 }
00100 void addLandmarkAsObstacle(const ShapeRoot& lm, unsigned int cost) {
00101 addLandmark(lm);
00102 obstacles.push_back(std::pair<Point,unsigned int>(lm->getCentroid(),cost));
00103 }
00104
00105 protected:
00106 bool isLMVisible(location loc, const Point& lm);
00107 void initialize();
00108 void fillState(unsigned int low, unsigned int high);
00109 void findLinks(state& s);
00110 std::vector<std::pair<int,int> > findLMs(location loc);
00111 state* thereIs(unsigned int pos, direction dir, std::pair<int,int> lms);
00112 void computeLandmarkCosts();
00113 Point findWorldCoords(unsigned int pos);
00114 std::string toString(const PathPlanner::state &s);
00115
00116 private:
00117 PathPlanner& operator=(const PathPlanner&);
00118 PathPlanner(const PathPlanner&);
00119
00120 };
00121 std::ostream& operator<<(std::ostream& out, const PathPlanner::direction& d);
00122
00123 }
00124
00125 #endif