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_TREE_H 00020 #define MSL_TREE_H 00021 00022 #ifdef WIN32 00023 #include <list> 00024 #include <string> 00025 using namespace std; 00026 #else 00027 #include <list.h> 00028 #include <string> 00029 #endif 00030 00031 00032 #include "vector.h" 00033 #include "mslio.h" 00034 00035 class MSLTree; 00036 00037 class MSLNode { 00038 private: 00039 MSLVector state; 00040 MSLVector input; 00041 MSLNode* parent; 00042 list<MSLNode*> children; 00043 double time; 00044 double cost; 00045 int id; 00046 00047 void* info; 00048 00049 public: 00051 MSLVector State() const {return state; }; 00052 00054 inline MSLVector Input() const {return input; }; 00055 00056 inline MSLNode* Parent() {return parent; }; 00057 inline list<MSLNode*> const Children() {return children; }; 00058 00060 inline double Time() const {return time; }; 00061 00063 inline double Cost() const {return cost; }; 00064 00066 inline void SetCost(const double &x) {cost = x; }; 00067 00069 inline void SetID(const int &i) {id = i; }; 00070 00072 inline int ID() const {return id; }; 00073 00075 void* GetInfo() {return info; }; 00076 00078 void SetInfo(void* in) {info = in; }; 00079 00080 00081 MSLNode(); 00082 MSLNode(void* pninfo); 00083 MSLNode(MSLNode* pn, const MSLVector &x, const MSLVector &u); 00084 MSLNode(MSLNode* pn, const MSLVector &x, const MSLVector &u, double t); 00085 MSLNode(MSLNode* pn, const MSLVector &x, const MSLVector &u, double t, void* pninfo); 00086 ~MSLNode() { children.clear(); }; 00087 00088 inline void AddChild(MSLNode *cn) { children.push_back(cn); } 00089 00090 //friend istream& operator>> (istream& is, MSLNode& n); 00091 friend ostream& operator<< (ostream& os, const MSLNode& n); 00092 //friend istream& operator>> (istream& is, list<MSLNode*> & nl); 00093 friend ostream& operator<< (ostream& os, const list<MSLNode*> & nl); 00094 00095 friend class MSLTree; 00096 }; 00097 00098 00100 class MSLNodeLess { 00101 public: 00102 bool operator() (MSLNode* p, MSLNode* q) const { 00103 return p->Cost() < q->Cost(); 00104 } 00105 }; 00106 00107 00109 class MSLNodeGreater { 00110 public: 00111 bool operator() (MSLNode* p, MSLNode* q) const { 00112 return p->Cost() > q->Cost(); 00113 } 00114 }; 00115 00116 00117 class MSLTree { 00118 private: 00119 list<MSLNode*> nodes; 00120 MSLNode* root; 00121 int size; 00122 public: 00123 00124 MSLTree(); 00125 MSLTree(const MSLVector &x); // Argument is state of root node 00126 MSLTree(const MSLVector &x, void* nodeinfo); 00127 ~MSLTree(); 00128 00129 void MakeRoot(const MSLVector &x); 00130 MSLNode* Extend(MSLNode *parent, const MSLVector &x, const MSLVector &u); 00131 MSLNode* Extend(MSLNode *parent, const MSLVector &x, const MSLVector &u, 00132 double time); 00133 MSLNode* Extend(MSLNode *parent, const MSLVector &x, const MSLVector &u, 00134 double time, void* pninfo); 00135 00136 list<MSLNode*> PathToRoot(MSLNode *n); 00137 MSLNode* FindNode(int nid); 00138 inline list<MSLNode*> Nodes() const { return nodes; }; 00139 inline MSLNode* Root() {return root; }; 00140 inline int Size() {return size;} 00141 00142 void Clear(); 00143 00144 friend istream& operator>> (istream& is, MSLTree& n); 00145 friend ostream& operator<< (ostream& os, const MSLTree& n); 00146 }; 00147 00148 #endif 00149