Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

tree.C

Go to the documentation of this file.
00001 //----------------------------------------------------------------------
00002 //               The Motion Strategy Library (MSL)
00003 //----------------------------------------------------------------------
00004 //
00005 // Copyright (c) 1998-2000 Iowa State University and Steve 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 // Iowa State University 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 
00020 #include "tree.h"
00021 
00022 // *********************************************************************
00023 // *********************************************************************
00024 // CLASS:     MSLNode class
00025 // 
00026 // *********************************************************************
00027 // *********************************************************************
00028 
00029 ostream& operator<<(ostream& out, const MSLNode& n)
00030 {
00031   out << n.id;
00032   if (n.parent) 
00033     out << " " << n.parent->id;
00034   else
00035     out << " -1";
00036   out << " " << n.state << " " << n.input;
00037   out << "\n";
00038 
00039   return out;
00040 }
00041 
00042 
00043 ostream& operator<<(ostream& out, const list<MSLNode*>& L)
00044 {
00045   list<MSLNode*>::iterator x; 
00046   list<MSLNode*> vl;
00047   vl = L;
00048   for (x = vl.begin(); x != vl.end(); x++) 
00049     out << " " << **x;
00050   return out;
00051 }
00052 
00053 
00054 
00055 
00056 MSLNode::MSLNode() { 
00057 }
00058 
00059 
00060 MSLNode::MSLNode(MSLNode *pn, const MSLVector &x, const MSLVector &u) {
00061   state = x;
00062   input = u;
00063   parent = pn;
00064   time = 1.0; // Make up a default
00065   cost = 0.0;
00066 }
00067 
00068 
00069 MSLNode::MSLNode(MSLNode *pn, const MSLVector &x, const MSLVector &u, double t) {
00070   state = x;
00071   input = u;
00072   parent = pn;
00073   time = t;
00074   cost = 0.0;
00075 }
00076 
00077 
00078 
00079 // *********************************************************************
00080 // *********************************************************************
00081 // CLASS:     MSLTree class
00082 // 
00083 // *********************************************************************
00084 // *********************************************************************
00085 
00086 
00087 ostream& operator<< (ostream& os, const MSLTree& T) {
00088   list<MSLNode*>::iterator x;
00089   list<MSLNode*> vl;
00090   vl = T.nodes;
00091   os << T.size << "\n";
00092   for (x = vl.begin(); x != vl.end(); x++) 
00093     os << **x;
00094   return os;
00095 }
00096 
00097 
00098 
00099 istream& operator>> (istream& is, MSLTree & T) {
00100   int i,nid,pid,tsize;
00101   MSLVector x,u;
00102   MSLNode *pnode,*n; // Parent node
00103 
00104   T.Clear();
00105 
00106   is >> tsize;
00107   cout << "Loading a tree that has " << tsize << " nodes\n";
00108   for (i = 0; i < tsize; i++) {
00109     is >> nid >> pid >> x >> u;
00110     pnode = T.FindNode(pid);
00111     if (pnode) {
00112       n = T.Extend(pnode,x,u);
00113       n->SetID(nid);
00114     }
00115     else
00116       T.MakeRoot(x);
00117   }
00118 
00119   return is; 
00120 }
00121 
00122 
00123 MSLTree::MSLTree() { 
00124   root = NULL;
00125   size = 0;
00126 }
00127 
00128 
00129 MSLTree::MSLTree(const MSLVector &x) { 
00130   MSLVector u;
00131 
00132   root = new MSLNode(NULL,x,u,0.0);
00133   root->id = 0;
00134   nodes.push_back(root);
00135   size = 1;
00136 }
00137 
00138 
00139 MSLTree::~MSLTree() {
00140   Clear();
00141 }
00142 
00143 
00144 void MSLTree::MakeRoot(const MSLVector &x) {
00145   MSLVector u;
00146   
00147   if (!root) {
00148     root = new MSLNode(NULL,x,u,0.0);
00149     root->id = 0;
00150     nodes.push_back(root);
00151   }
00152   else
00153     cout << "Root already made.  MakeRoot has no effect.\n";
00154   size = 1;
00155 }
00156 
00157 
00158 MSLNode* MSLTree::Extend(MSLNode *parent, const MSLVector &x, const MSLVector &u) {
00159   MSLNode *nn;
00160 
00161   nn = new MSLNode(parent, x, u);
00162   nn->id = size;
00163   nodes.push_back(nn);
00164   size++;
00165 
00166   return nn;
00167 }
00168 
00169 
00170 
00171 MSLNode* MSLTree::Extend(MSLNode *parent, const MSLVector &x, 
00172                          const MSLVector &u,
00173                          double time) {
00174   MSLNode *nn;
00175 
00176   nn = new MSLNode(parent, x, u, time);
00177   nn->id = size;
00178   nodes.push_back(nn);
00179   size++;
00180 
00181   return nn;
00182 }
00183 
00184 
00185 
00186 MSLNode* MSLTree::FindNode(int nid) {
00187   list<MSLNode*>::iterator ni;
00188 
00189   for(ni = nodes.begin(); ni != nodes.end(); ni++) {
00190     if ((*ni)->id == nid)
00191       return *ni;
00192   }
00193 
00194   return NULL; // Indicates failure
00195 }
00196 
00197 
00198 
00199 list<MSLNode*> MSLTree::PathToRoot(MSLNode *n) {
00200   list<MSLNode*> nl;
00201   MSLNode *ni;
00202   
00203   ni = n;
00204   while (ni != root) {
00205     nl.push_back(ni);
00206     ni = ni->Parent();
00207   }
00208 
00209   nl.push_back(root);
00210  
00211   return nl;
00212 }
00213 
00214 
00215 void MSLTree::Clear() {
00216   list<MSLNode*>::iterator n; 
00217   for (n = nodes.begin(); n != nodes.end(); n++)
00218     delete *n;
00219   nodes.clear();
00220   root = NULL;
00221 }
Motion Strategy Library


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Anna Atramentov, Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.