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 }