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 #include <list.h> 00023 #include <string> 00024 00025 #include "vector.h" 00026 00027 class MSLTree; 00028 00029 class MSLNode { 00030 private: 00031 MSLVector state; 00032 MSLVector input; 00033 MSLNode* parent; 00034 list<MSLNode*> children; 00035 double time; 00036 double cost; 00037 int id; 00038 00039 public: 00041 MSLVector State() const {return state; }; 00042 00044 inline MSLVector Input() const {return input; }; 00045 00046 inline MSLNode* Parent() {return parent; }; 00047 inline list<MSLNode*> const Children() {return children; }; 00048 00050 inline double Time() const {return time; }; 00051 00053 inline double Cost() const {return cost; }; 00054 00056 inline void SetCost(const double &x) {cost = x; }; 00057 00059 inline void SetID(const int &i) {id = i; }; 00060 00062 inline int ID() const {return id; }; 00063 00064 MSLNode(); 00065 MSLNode(MSLNode *pn, const MSLVector &x, const MSLVector &u); 00066 MSLNode(MSLNode *pn, const MSLVector &x, const MSLVector &u, double t); 00067 ~MSLNode() { children.clear(); }; 00068 00069 inline void AddChild(MSLNode *cn) { children.push_back(cn); } 00070 00071 //friend istream& operator>> (istream& is, MSLNode& n); 00072 friend ostream& operator<< (ostream& os, const MSLNode& n); 00073 //friend istream& operator>> (istream& is, list<MSLNode*> & nl); 00074 friend ostream& operator<< (ostream& os, const list<MSLNode*> & nl); 00075 00076 friend class MSLTree; 00077 }; 00078 00079 00081 class MSLNodeLess { 00082 public: 00083 bool operator() (MSLNode* p, MSLNode* q) const { 00084 return p->Cost() < q->Cost(); 00085 } 00086 }; 00087 00088 00090 class MSLNodeGreater { 00091 public: 00092 bool operator() (MSLNode* p, MSLNode* q) const { 00093 return p->Cost() > q->Cost(); 00094 } 00095 }; 00096 00097 00098 class MSLTree { 00099 private: 00100 list<MSLNode*> nodes; 00101 MSLNode* root; 00102 int size; 00103 public: 00104 00105 MSLTree(); 00106 MSLTree(const MSLVector &x); // Argument is state of root node 00107 ~MSLTree(); 00108 00109 void MakeRoot(const MSLVector &x); 00110 MSLNode* Extend(MSLNode *parent, const MSLVector &x, const MSLVector &u); 00111 MSLNode* Extend(MSLNode *parent, const MSLVector &x, const MSLVector &u, 00112 double time); 00113 list<MSLNode*> PathToRoot(MSLNode *n); 00114 MSLNode* FindNode(int nid); 00115 inline list<MSLNode*> Nodes() const { return nodes; }; 00116 inline MSLNode* Root() {return root; }; 00117 inline int Size() {return size;} 00118 00119 void Clear(); 00120 00121 friend istream& operator>> (istream& is, MSLTree& n); 00122 friend ostream& operator<< (ostream& os, const MSLTree& n); 00123 }; 00124 00125 #endif 00126