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 "graph.h" 00021 00022 // ********************************************************************* 00023 // ********************************************************************* 00024 // CLASS: MSLVertex class 00025 // 00026 // ********************************************************************* 00027 // ********************************************************************* 00028 00029 ostream& operator<<(ostream& out, const MSLVertex& n) 00030 { 00031 out << n.id << " " << n.state; 00032 out << "\n"; 00033 00034 return out; 00035 } 00036 00037 00038 ostream& operator<<(ostream& out, const list<MSLVertex*>& L) 00039 { 00040 list<MSLVertex*>::iterator x; 00041 list<MSLVertex*> vl; 00042 vl = L; 00043 for (x = vl.begin(); x != vl.end(); x++) 00044 out << " " << **x; 00045 return out; 00046 } 00047 00048 00049 istream& operator>>(istream& in, list<MSLVertex*>& L) 00050 { 00051 L.clear(); 00052 // MSLVertex *x; 00053 for(;;) 00054 { 00055 char c; 00056 while (in.get(c) && isspace(c)); 00057 if (!in) break; 00058 in.putback(c); 00059 // x = new MSLVertex(); in >> x; L.push_back(x); 00060 } 00061 return in; 00062 } 00063 00064 00065 MSLVertex::MSLVertex() { 00066 } 00067 00068 00069 MSLVertex::MSLVertex(const MSLVector &x) { 00070 state = x; 00071 cost = 0.0; 00072 } 00073 00074 00075 MSLVertex::~MSLVertex() { 00076 edges.clear(); 00077 } 00078 00079 00080 // ********************************************************************* 00081 // ********************************************************************* 00082 // CLASS: MSLEdge class 00083 // 00084 // ********************************************************************* 00085 // ********************************************************************* 00086 00087 ostream& operator<<(ostream& out, const MSLEdge& e) 00088 { 00089 out << e.source->ID() << " " << e.target->ID(); 00090 out << "\n"; 00091 00092 return out; 00093 } 00094 00095 00096 ostream& operator<<(ostream& out, const list<MSLEdge*>& L) 00097 { 00098 list<MSLEdge*>::iterator x; 00099 list<MSLEdge*> vl; 00100 vl = L; 00101 for (x = vl.begin(); x != vl.end(); x++) 00102 out << " " << **x; 00103 return out; 00104 } 00105 00106 00107 00108 MSLEdge::MSLEdge() { 00109 } 00110 00111 00112 MSLEdge::MSLEdge(MSLVertex* v1, MSLVertex* v2) { 00113 // Make sure the node edge lists point correctly 00114 v1->edges.push_back(this); 00115 v2->edges.push_back(this); 00116 00117 source = v1; target = v2; 00118 time = 1.0; cost = 1.0; 00119 } 00120 00121 00122 MSLEdge::MSLEdge(MSLVertex* v1, MSLVertex* v2, const MSLVector &u, 00123 double t) { 00124 v1->edges.push_back(this); 00125 v2->edges.push_back(this); 00126 00127 source = v1; target = v2; 00128 input = u; time = t; cost = 1.0; 00129 } 00130 00131 00132 00133 MSLEdge::MSLEdge(MSLVertex* v1, MSLVertex* v2, double t) { 00134 v1->edges.push_back(this); 00135 v2->edges.push_back(this); 00136 00137 source = v1; target = v2; 00138 time = t; cost = 1.0; 00139 } 00140 00141 00142 // ********************************************************************* 00143 // ********************************************************************* 00144 // CLASS: MSLGraph class 00145 // 00146 // ********************************************************************* 00147 // ********************************************************************* 00148 00149 ostream& operator<< (ostream& os, const MSLGraph& G) { 00150 os << G.numvertices << " " << G.numedges << "\n"; 00151 list<MSLVertex*>::iterator x; 00152 list<MSLVertex*> vl; 00153 vl = G.vertices; 00154 for (x = vl.begin(); x != vl.end(); x++) 00155 os << " " << **x; 00156 00157 os << "\n"; 00158 list<MSLEdge*>::iterator y; 00159 list<MSLEdge*> el; 00160 el = G.edges; 00161 for (y = el.begin(); y != el.end(); y++) 00162 os << " " << **y; 00163 os << "\n\n"; 00164 00165 return os; 00166 } 00167 00168 00169 istream& operator>> (istream& is, MSLGraph & G) { 00170 int i,nid1,nid2,nume,numv; 00171 MSLVector x,u; 00172 MSLVertex *v1,*v2; 00173 00174 G.Clear(); 00175 00176 is >> numv >> nume; 00177 cout << "Loading a graph that has " << numv 00178 << " vertices and " << nume << " edges\n"; 00179 00180 // Handle vertices 00181 for (i = 0; i < numv; i++) { 00182 is >> nid1 >> x; 00183 v1 = G.AddVertex(x); 00184 v1->SetID(nid1); 00185 } 00186 00187 // Handle edges 00188 for (i = 0; i < nume; i++) { 00189 is >> nid1 >> nid2; 00190 v1 = G.FindVertex(nid1); 00191 v2 = G.FindVertex(nid2); 00192 if (v1 && v2) 00193 G.AddEdge(v1,v2); 00194 else 00195 cout << "ERROR: Read edge does not connect to vertices\n"; 00196 } 00197 00198 return is; 00199 } 00200 00201 00202 MSLGraph::MSLGraph() { 00203 numvertices = 0; 00204 numedges = 0; 00205 } 00206 00207 00208 00209 MSLGraph::~MSLGraph() { 00210 Clear(); 00211 } 00212 00213 00214 MSLVertex* MSLGraph::AddVertex(const MSLVector &x) { 00215 MSLVertex *nv; 00216 00217 nv = new MSLVertex(x); 00218 nv->id = numvertices; 00219 vertices.push_back(nv); 00220 numvertices++; 00221 00222 return nv; 00223 } 00224 00225 00226 MSLEdge* MSLGraph::AddEdge(MSLVertex* v1, MSLVertex* v2) { 00227 MSLVector u; 00228 return AddEdge(v1,v2,u,1.0); 00229 } 00230 00231 00232 MSLEdge* MSLGraph::AddEdge(MSLVertex* v1, MSLVertex* v2, double time) { 00233 MSLVector u; 00234 return AddEdge(v1,v2,u,time); 00235 } 00236 00237 00238 MSLEdge* MSLGraph::AddEdge(MSLVertex* v1, MSLVertex* v2, 00239 const MSLVector &u, double time) { 00240 00241 MSLEdge* e; 00242 e = new MSLEdge(v1,v2,u,time); 00243 00244 edges.push_back(e); 00245 numedges++; 00246 00247 return e; 00248 } 00249 00250 00251 bool MSLGraph::IsEdge(MSLVertex* v1, MSLVertex* v2) { 00252 00253 list<MSLEdge*> elist; 00254 elist = v1->Edges(); 00255 00256 list<MSLEdge*>::iterator e; 00257 for (e = elist.begin(); e != elist.end(); e++) { 00258 if ((*e)->Target() == v2) 00259 return true; 00260 } 00261 00262 return false; 00263 } 00264 00265 00266 00267 MSLVertex* MSLGraph::FindVertex(int nid) { 00268 list<MSLVertex*>::iterator vi; 00269 00270 for(vi = vertices.begin(); vi != vertices.end(); vi++) { 00271 if ((*vi)->id == nid) 00272 return *vi; 00273 } 00274 00275 return NULL; // Indicates failure 00276 } 00277 00278 00279 00280 void MSLGraph::Clear() { 00281 list<MSLVertex*>::iterator v; 00282 for (v = vertices.begin(); v != vertices.end(); v++) 00283 delete *v; 00284 vertices.clear(); 00285 00286 list<MSLEdge*>::iterator e; 00287 for (e = edges.begin(); e != edges.end(); e++) 00288 delete *e; 00289 edges.clear(); 00290 }