Motion Strategy Library

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
tree.h
