1 | #include <stdlib.h>
|
---|
2 | #include <time.h>
|
---|
3 |
|
---|
4 | #include <iostream>
|
---|
5 | #include <deque>
|
---|
6 |
|
---|
7 | #include <boost/graph/adjacency_list.hpp>
|
---|
8 | #include <boost/graph/visitors.hpp>
|
---|
9 | #include <boost/graph/graph_utility.hpp>
|
---|
10 |
|
---|
11 | // undefine this to test without bundled properties
|
---|
12 | #define __USE_BUNDLED__
|
---|
13 |
|
---|
14 | /**
|
---|
15 | * Singleton design pattern is used to
|
---|
16 | * to make an instance of a particular class unique.
|
---|
17 | */
|
---|
18 | template< typename X > class Singleton
|
---|
19 | {
|
---|
20 | public:
|
---|
21 | inline static X& getInstance()
|
---|
22 | {
|
---|
23 | static X x;
|
---|
24 |
|
---|
25 | return x;
|
---|
26 | }
|
---|
27 |
|
---|
28 | protected:
|
---|
29 | Singleton( void ) { }
|
---|
30 | ~Singleton( void ) { }
|
---|
31 | Singleton( const Singleton& ) { }
|
---|
32 | inline Singleton& operator = ( const Singleton& x )
|
---|
33 | {
|
---|
34 | return x;
|
---|
35 | }
|
---|
36 | };
|
---|
37 |
|
---|
38 |
|
---|
39 | /***
|
---|
40 | * @desc
|
---|
41 | * PathRecorder class is a template for creating
|
---|
42 | * path recorders which utilise visitor concepts
|
---|
43 | * in BGL. I.e. as a search algorithm runs,
|
---|
44 | * PathRecorder is called to fill in a "predecessor map"
|
---|
45 | * which records how to get to a specific vertex and stores
|
---|
46 | * edges with their targets as keys.
|
---|
47 | ***/
|
---|
48 | template< class Graph, class Vertex, class Edge, class VertexIterator, class EdgeIterator > class PathRecorder
|
---|
49 | {
|
---|
50 | typedef typename boost::property_map< typename Graph, boost::vertex_index_t >::type VertexId_PMap;
|
---|
51 | typedef boost::iterator_property_map< typename std::vector< Vertex >::iterator, VertexId_PMap, typename Vertex, typename Vertex & > PredMap;
|
---|
52 | typedef boost::iterator_property_map< typename std::vector< Edge >::iterator, VertexId_PMap, typename Edge, typename Edge & > PredEdgeMap;
|
---|
53 |
|
---|
54 | public:
|
---|
55 | /**
|
---|
56 | * @desc
|
---|
57 | * Update an entry inside a map
|
---|
58 | **/
|
---|
59 | void put( Vertex target, Vertex src )
|
---|
60 | {
|
---|
61 | boost::put( predMap, target, src );
|
---|
62 | }
|
---|
63 |
|
---|
64 | /**
|
---|
65 | * @desc
|
---|
66 | * Update an entry inside a map
|
---|
67 | **/
|
---|
68 | void put( Vertex target, Edge e )
|
---|
69 | {
|
---|
70 | boost::put( predEdgeMap, target, e );
|
---|
71 | }
|
---|
72 |
|
---|
73 | /**
|
---|
74 | * @desc
|
---|
75 | * Get predecessor (ancestor) of target
|
---|
76 | **/
|
---|
77 | Vertex getPredecessor( Vertex target ) const
|
---|
78 | {
|
---|
79 | return boost::get( predMap, target );
|
---|
80 | }
|
---|
81 |
|
---|
82 | /**
|
---|
83 | * @desc
|
---|
84 | * Get Edge to target
|
---|
85 | **/
|
---|
86 | Edge getEdge( Vertex target ) const
|
---|
87 | {
|
---|
88 | return boost::get( predEdgeMap, target );
|
---|
89 | }
|
---|
90 |
|
---|
91 | /**
|
---|
92 | * @desc
|
---|
93 | * Get edge from ancestor to sibling (if such edge exists)
|
---|
94 | **/
|
---|
95 | Edge PathRecorder :: getEdgeToSibling( Vertex ancestor ) const
|
---|
96 | {
|
---|
97 | VertexIterator i, end;
|
---|
98 |
|
---|
99 | for( boost::tie( i, end ) = boost::vertices( g ); i != end; ++i )
|
---|
100 | {
|
---|
101 | Vertex pred = boost::get( predMap, *i );
|
---|
102 |
|
---|
103 | if( pred == ancestor )
|
---|
104 | {
|
---|
105 | return boost::get( predEdgeMap, *i );
|
---|
106 | }
|
---|
107 | }
|
---|
108 |
|
---|
109 | return Edge();
|
---|
110 | }
|
---|
111 |
|
---|
112 | /***
|
---|
113 | * @desc reset path recorder for
|
---|
114 | * another search
|
---|
115 | ***/
|
---|
116 | virtual void reset( void )
|
---|
117 | {
|
---|
118 | edges.assign( edges.size(), Edge() );
|
---|
119 | vertices.assign( vertices.size(), Vertex() );
|
---|
120 | }
|
---|
121 | protected:
|
---|
122 | PathRecorder( const Graph &graph ) : g( graph ),
|
---|
123 | vertices( std::vector< Vertex >( boost::num_vertices( g ) ) ),
|
---|
124 | edges( std::vector< Edge >( boost::num_vertices( g ) ) ),
|
---|
125 | vertex_id( boost::get( boost::vertex_index, g ) ),
|
---|
126 | predMap( vertices.begin(), vertex_id ),
|
---|
127 | predEdgeMap( edges.begin(), vertex_id )
|
---|
128 | {
|
---|
129 | }
|
---|
130 |
|
---|
131 | const Graph g;
|
---|
132 |
|
---|
133 | ~PathRecorder( void ) { }
|
---|
134 | PathRecorder( const PathRecorder & ) { }
|
---|
135 |
|
---|
136 | std::vector< Vertex > vertices;
|
---|
137 | std::vector< Edge > edges;
|
---|
138 | VertexId_PMap vertex_id;
|
---|
139 |
|
---|
140 | PredMap predMap;
|
---|
141 | PredEdgeMap predEdgeMap;
|
---|
142 | };
|
---|
143 |
|
---|
144 | /**
|
---|
145 | * @desc
|
---|
146 | * Terminator is a class which simply tells DFS
|
---|
147 | * to stop once the target vertex is reached.
|
---|
148 | **/
|
---|
149 | template< class Vertex, class Graph > struct Terminator
|
---|
150 | {
|
---|
151 | Terminator( Vertex v ) : destination( v )
|
---|
152 | {
|
---|
153 | }
|
---|
154 |
|
---|
155 | bool operator()( Vertex v, const Graph &g )
|
---|
156 | {
|
---|
157 | return ( v == destination );
|
---|
158 | }
|
---|
159 |
|
---|
160 | private:
|
---|
161 | const Vertex destination;
|
---|
162 | };
|
---|
163 |
|
---|
164 | #ifdef __USE_BUNDLED__
|
---|
165 | /***
|
---|
166 | * Vertex Structure (bundled properties)
|
---|
167 | * This is empty, but in original src code
|
---|
168 | * this has some data inside.
|
---|
169 | ***/
|
---|
170 | struct Clip_t
|
---|
171 | {
|
---|
172 | Clip_t( void ) { }
|
---|
173 | };
|
---|
174 |
|
---|
175 | /***
|
---|
176 | * Edge Structure (bundled properties)
|
---|
177 | * This is empty, but in original src code
|
---|
178 | * this has some data inside.
|
---|
179 | ***/
|
---|
180 | struct Transition_t
|
---|
181 | {
|
---|
182 | Transition_t( void ) { }
|
---|
183 |
|
---|
184 | Transition_t( double d ) : distance( d )
|
---|
185 | {
|
---|
186 | }
|
---|
187 |
|
---|
188 | double distance;
|
---|
189 | };
|
---|
190 | #endif
|
---|
191 |
|
---|
192 | /***
|
---|
193 | * Type defines for our graph ( Called MotionGraph )
|
---|
194 | ***/
|
---|
195 | #ifdef __USE_BUNDLED__
|
---|
196 | typedef boost::adjacency_list< boost::listS, boost::vecS, boost::bidirectionalS, Clip_t, Transition_t > MotionGraph_t;
|
---|
197 | #else
|
---|
198 | typedef boost::adjacency_list< boost::listS, boost::vecS, boost::bidirectionalS > MotionGraph_t;
|
---|
199 | #endif
|
---|
200 |
|
---|
201 | typedef boost::graph_traits< MotionGraph_t >::vertex_iterator ClipIterator;
|
---|
202 | typedef boost::graph_traits< MotionGraph_t >::vertex_descriptor Clip;
|
---|
203 | typedef boost::graph_traits< MotionGraph_t >::out_edge_iterator TransitionIterator;
|
---|
204 | typedef boost::graph_traits< MotionGraph_t >::edge_descriptor Transition;
|
---|
205 |
|
---|
206 | /***
|
---|
207 | * Singleton graph structure so we can access it anywhere we like
|
---|
208 | * getBase is a way of passing the graph to BGL algorithm templates
|
---|
209 | * if you pass this class directly templates will not compile due
|
---|
210 | * to constructors and destructors being protected.
|
---|
211 | ***/
|
---|
212 | class MotionGraph : public Singleton< MotionGraph >, public MotionGraph_t
|
---|
213 | {
|
---|
214 | friend Singleton< MotionGraph >;
|
---|
215 | public:
|
---|
216 | MotionGraph_t *getBase( void )
|
---|
217 | {
|
---|
218 | return static_cast< MotionGraph_t * >( &getInstance() );
|
---|
219 | }
|
---|
220 | protected:
|
---|
221 | ~MotionGraph() { }
|
---|
222 | MotionGraph() { }
|
---|
223 | MotionGraph( const MotionGraph & ) { }
|
---|
224 |
|
---|
225 | };
|
---|
226 |
|
---|
227 | /**
|
---|
228 | @desc
|
---|
229 | ForwardEdge is a visitor which tries to decide whether
|
---|
230 | to replace a newly discovered edge with the edge inside
|
---|
231 | a DFS tree (predecessor map).
|
---|
232 | Forward Edge is an edge (U,V) and both U, V have already
|
---|
233 | been discovered by DFS.
|
---|
234 | **/
|
---|
235 | struct TestForwardEdge : public boost::base_visitor< TestForwardEdge >
|
---|
236 | {
|
---|
237 | typedef boost::on_forward_or_cross_edge event_filter;
|
---|
238 |
|
---|
239 | template < class Edge, class Graph > void operator()( Edge e, const Graph &g );
|
---|
240 | };
|
---|
241 |
|
---|
242 | /**
|
---|
243 | @desc
|
---|
244 | PredRecorder adds a tree edge to predecessor map.
|
---|
245 | Tree edge is the first edge for pair of vertices
|
---|
246 | (U, V) that was ever discovered.
|
---|
247 | **/
|
---|
248 | struct TestPredRecorder : public boost::base_visitor< TestPredRecorder >
|
---|
249 | {
|
---|
250 | typedef boost::on_tree_edge event_filter;
|
---|
251 |
|
---|
252 | template < class Edge, class Graph > void operator()( Edge e, const Graph& g );
|
---|
253 | };
|
---|
254 |
|
---|
255 | /***
|
---|
256 | * Quick test implementation of PathRecorder interface.
|
---|
257 | ***/
|
---|
258 | class TestPathRecorder : public PathRecorder< MotionGraph_t, Clip, Transition, ClipIterator, TransitionIterator >, public Singleton< TestPathRecorder >
|
---|
259 | {
|
---|
260 | friend class Singleton< TestPathRecorder >;
|
---|
261 | public:
|
---|
262 | template< typename Container > void search( Container &agenda, Clip start, Clip end )
|
---|
263 | {
|
---|
264 | std::vector< unsigned short > c( boost::num_vertices( g ) );
|
---|
265 |
|
---|
266 | reset();
|
---|
267 |
|
---|
268 | typedef boost::property_map< MotionGraph_t, boost::vertex_index_t >::type VertexId_PMap;
|
---|
269 | VertexId_PMap vertex_id = boost::get( boost::vertex_index, g );
|
---|
270 | typedef boost::iterator_property_map< std::vector< unsigned short >::iterator, VertexId_PMap, unsigned short, unsigned short & > ColourMap;
|
---|
271 |
|
---|
272 | boost::depth_first_visit(
|
---|
273 | g,
|
---|
274 | start,
|
---|
275 | boost::make_dfs_visitor( boost::make_list( TestPredRecorder(), TestForwardEdge() ) ),
|
---|
276 | ColourMap( c.begin(), vertex_id ),
|
---|
277 | Terminator< Clip, MotionGraph_t >( end )
|
---|
278 | );
|
---|
279 |
|
---|
280 | getPath( agenda, start, end );
|
---|
281 | }
|
---|
282 |
|
---|
283 | template< typename Container > void getPath( Container &agenda, Clip start, Clip end ) const
|
---|
284 | {
|
---|
285 | Transition cur = boost::get( predEdgeMap, end );
|
---|
286 |
|
---|
287 | while( cur.m_eproperty )
|
---|
288 | {
|
---|
289 | agenda.push_front( boost::target( cur, g ) );
|
---|
290 | agenda.push_front( boost::source( cur, g ) );
|
---|
291 |
|
---|
292 | cur = boost::get( predEdgeMap, boost::source( cur, g ) );
|
---|
293 | }
|
---|
294 |
|
---|
295 | if( agenda.empty() || agenda.front() != start )
|
---|
296 | {
|
---|
297 | agenda.clear();
|
---|
298 | }
|
---|
299 | }
|
---|
300 |
|
---|
301 | protected:
|
---|
302 | TestPathRecorder( void ) : PathRecorder( *MotionGraph::getInstance().getBase() ) { }
|
---|
303 | ~TestPathRecorder( void ) { }
|
---|
304 | TestPathRecorder( const TestPathRecorder &mpr ) : PathRecorder( mpr.g ) { }
|
---|
305 | };
|
---|
306 |
|
---|
307 | /**** Definitions of methods for visitor classes declared before TestPathRecorder ****/
|
---|
308 |
|
---|
309 | template < class Edge, class Graph > void TestPredRecorder :: operator()( Edge e, const Graph &g )
|
---|
310 | {
|
---|
311 | // simplified method, simply overwrite
|
---|
312 | TestPathRecorder::getInstance().put( boost::target( e, g ), boost::source( e, g ) );
|
---|
313 | TestPathRecorder::getInstance().put( boost::target( e, g ), e );
|
---|
314 | }
|
---|
315 |
|
---|
316 | template < class Edge, class Graph > void TestForwardEdge :: operator()( Edge e, const Graph &g )
|
---|
317 | {
|
---|
318 | // simplified method, simply overwrite
|
---|
319 | TestPathRecorder::getInstance().put( boost::target( e, g ), boost::source( e, g ) );
|
---|
320 | TestPathRecorder::getInstance().put( boost::target( e, g ), e );
|
---|
321 | }
|
---|
322 |
|
---|
323 | /******************************/
|
---|
324 |
|
---|
325 | /*** Roll N-sided die ***/
|
---|
326 | int roll( int sides )
|
---|
327 | {
|
---|
328 | return rand() % sides;
|
---|
329 | }
|
---|
330 |
|
---|
331 | /*** Roll N-sided die so that result is different to prev ***/
|
---|
332 | int rollUnique( int sides, int prev )
|
---|
333 | {
|
---|
334 | int r = roll( sides );
|
---|
335 |
|
---|
336 | while( r == prev )
|
---|
337 | {
|
---|
338 | r = roll( sides );
|
---|
339 | }
|
---|
340 |
|
---|
341 | return r;
|
---|
342 | }
|
---|
343 |
|
---|
344 | /*** Choose a random src and target and attempt to find a path ***/
|
---|
345 | void randomTest( void )
|
---|
346 | {
|
---|
347 | std::deque< Clip > mg;
|
---|
348 |
|
---|
349 | int start = rand() % boost::num_vertices( MotionGraph::getInstance() );
|
---|
350 | int end = rand() % boost::num_vertices( MotionGraph::getInstance() );
|
---|
351 |
|
---|
352 | TestPathRecorder::getInstance().search( mg, start, end );
|
---|
353 |
|
---|
354 | if( mg.empty() )
|
---|
355 | {
|
---|
356 | std::cout << "Destination unreachable" << std::endl;
|
---|
357 | return;
|
---|
358 | }
|
---|
359 |
|
---|
360 | std::cout << "Path from " << start << " to " << end << ":" << std::endl;
|
---|
361 |
|
---|
362 | for( int i = 0; i < mg.size(); ++i )
|
---|
363 | {
|
---|
364 | std::cout << mg.at( i ) << ( i != mg.size() - 1 ? " -> " : "\n" );
|
---|
365 | }
|
---|
366 | }
|
---|
367 |
|
---|
368 | int main( void )
|
---|
369 | {
|
---|
370 | srand( time( NULL ) );
|
---|
371 |
|
---|
372 | // randomly choose number of vertices
|
---|
373 | int vertices = roll( 500 );
|
---|
374 |
|
---|
375 | // add vertices
|
---|
376 | for( int i = 0; i < vertices; ++i )
|
---|
377 | {
|
---|
378 | #ifdef __USE_BUNDLED__
|
---|
379 | boost::add_vertex( Clip_t(), MotionGraph::getInstance() );
|
---|
380 | #else
|
---|
381 | boost::add_vertex( MotionGraph::getInstance() );
|
---|
382 | #endif
|
---|
383 | }
|
---|
384 |
|
---|
385 | // for each vertex V choose a random number of edges and connect them to random vertices (excluding the V itself)
|
---|
386 | for( int i = 0; i < vertices; ++i )
|
---|
387 | {
|
---|
388 | int edges = roll( vertices - 1 );
|
---|
389 |
|
---|
390 | for( int j = 0; j < edges; j++ )
|
---|
391 | {
|
---|
392 | int target = rollUnique( vertices, i );
|
---|
393 |
|
---|
394 | #ifdef __USE_BUNDLED__
|
---|
395 | boost::add_edge( i, target, Transition_t(), MotionGraph::getInstance() );
|
---|
396 | #else
|
---|
397 | boost::add_edge( i, target, MotionGraph::getInstance() );
|
---|
398 | #endif
|
---|
399 | }
|
---|
400 | }
|
---|
401 |
|
---|
402 | // run a random path finding test for half the number of vertices
|
---|
403 | for( int i = 0; i < vertices / 2; i++ )
|
---|
404 | {
|
---|
405 | randomTest();
|
---|
406 | }
|
---|
407 |
|
---|
408 | return 0;
|
---|
409 | }
|
---|
410 |
|
---|