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

graph.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 "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 }
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.