Ticket #3078: BGLTest.cpp

#include <stdlib.h>
#include <time.h>

#include <iostream>
#include <deque>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/graph_utility.hpp>

/** 
* Singleton design pattern is used to
* to make an instance of a particular class unique.
*/
template< typename X > class Singleton
{
public:
        inline static X& getInstance()
        {
                static X x;

                return x;
        }

protected:
        Singleton( void ) { }
        ~Singleton( void ) { }
        Singleton( const Singleton& ) { }
        inline Singleton& operator = ( const Singleton& x )
        {
                return x;
        }
};


/***
 * @desc
 * PathRecorder class is a template for creating
 * path recorders which utilise visitor concepts
 * in BGL. I.e. as a search algorithm runs,
 * PathRecorder is called to fill in a "predecessor map"
 * which records how to get to a specific vertex and stores
 * edges with their targets as keys.
 ***/
template< class Graph, class Vertex, class Edge, class VertexIterator, class EdgeIterator > class PathRecorder
{
        typedef typename boost::property_map< typename Graph, boost::vertex_index_t >::type VertexId_PMap;
        typedef boost::iterator_property_map< typename std::vector< Vertex >::iterator, VertexId_PMap, typename Vertex, typename Vertex & > PredMap;
        typedef boost::iterator_property_map< typename std::vector< Edge >::iterator, VertexId_PMap, typename Edge, typename Edge & > PredEdgeMap;

public:
        /**
         * @desc
         * Update an entry inside a map
         **/
        void put( Vertex target, Vertex src )
        {
                boost::put( predMap, target, src );
        }

        /**
         * @desc
         * Update an entry inside a map
         **/
        void put( Vertex target, Edge e )
        {
                boost::put( predEdgeMap, target, e );
        }

        /**
         * @desc
         * Get predecessor (ancestor) of target
         **/
        Vertex getPredecessor( Vertex target ) const
        {
                return boost::get( predMap, target );
        }

        /**
         * @desc
         * Get Edge to target
         **/
        Edge getEdge( Vertex target ) const
        {
                return boost::get( predEdgeMap, target );
        }

        /**
         * @desc
         * Get edge from ancestor to sibling (if such edge exists)
         **/
        Edge PathRecorder :: getEdgeToSibling( Vertex ancestor ) const
        {
                VertexIterator i, end;

                for( boost::tie( i, end ) = boost::vertices( g ); i != end; ++i )
                {
                        Vertex pred = boost::get( predMap, *i );

                        if( pred == ancestor )
                        {
                                return boost::get( predEdgeMap, *i );
                        }
                }

                return Edge();
        }

        /***
         * @desc reset path recorder for
         * another search
         ***/
        virtual void reset( void )
        {
                edges.assign( edges.size(), Edge() );
                vertices.assign( vertices.size(), Vertex() );
        }
protected:
        PathRecorder( const Graph &graph ) : g( graph ),
                vertices( std::vector< Vertex >( boost::num_vertices( g ) ) ),
                edges( std::vector< Edge >( boost::num_vertices( g ) ) ),
                vertex_id( boost::get( boost::vertex_index, g ) ),
                predMap( vertices.begin(), vertex_id ),
                predEdgeMap( edges.begin(), vertex_id )
        {
        }

        const Graph g;

        ~PathRecorder( void ) { }
        PathRecorder( const PathRecorder & ) { }

        std::vector< Vertex > vertices;
        std::vector< Edge > edges;
        VertexId_PMap vertex_id;

        PredMap predMap;
        PredEdgeMap predEdgeMap;
};

/**
 * @desc
 * Terminator is a class which simply tells DFS
 * to stop once the target vertex is reached.
 **/
template< class Vertex, class Graph > struct Terminator
{
        Terminator( Vertex v ) : destination( v )
        {
        }

        bool operator()( Vertex v, const Graph &g )
        {
                return ( v == destination );
        }

private:
        const Vertex destination;
};

/*** 
 * Vertex Structure (bundled properties)
 * This is empty, but in original src code
 * this has some data inside.
 ***/
struct Clip_t
{
        Clip_t( void ) { }
};

/*** 
 * Edge Structure (bundled properties)
 * This is empty, but in original src code
 * this has some data inside.
 ***/
struct Transition_t
{
        Transition_t( void ) { }

        Transition_t( double d ) : distance( d )
        {
        }

        double distance;
};

/*** 
 * Type defines for our graph ( Called MotionGraph )
 ***/
typedef boost::adjacency_list< boost::listS, boost::vecS, boost::bidirectionalS, Clip_t, Transition_t > MotionGraph_t;
typedef boost::graph_traits< MotionGraph_t >::vertex_iterator ClipIterator;
typedef boost::graph_traits< MotionGraph_t >::vertex_descriptor Clip;
typedef boost::graph_traits< MotionGraph_t >::out_edge_iterator TransitionIterator;
typedef boost::graph_traits< MotionGraph_t >::edge_descriptor Transition;

/*** 
 * Singleton graph structure so we can access it anywhere we like
 * getBase is a way of passing the graph to BGL algorithm templates
 * if you pass this class directly templates will not compile due
 * to constructors and destructors being protected.
 ***/
class MotionGraph : public Singleton< MotionGraph >, public MotionGraph_t
{
        friend Singleton< MotionGraph >;
public:
        MotionGraph_t *getBase( void )
        {
                return static_cast< MotionGraph_t * >( &getInstance() );
        }
protected:
        ~MotionGraph() { }
        MotionGraph() { }
        MotionGraph( const MotionGraph & ) { }

};

/**
@desc
ForwardEdge is a visitor which tries to decide whether
to replace a newly discovered edge with the edge inside
a DFS tree (predecessor map).
Forward Edge is an edge (U,V) and both U, V have already
been discovered by DFS.
**/
struct TestForwardEdge : public boost::base_visitor< TestForwardEdge >
{
        typedef boost::on_forward_or_cross_edge event_filter;

        template < class Edge, class Graph > void operator()( Edge e, const Graph &g );
};

/**
@desc
PredRecorder adds a tree edge to predecessor map.
Tree edge is the first edge for pair of vertices
(U, V) that was ever discovered.
**/
struct TestPredRecorder : public boost::base_visitor< TestPredRecorder >
{
        typedef boost::on_tree_edge event_filter;

        template < class Edge, class Graph > void operator()( Edge e, const Graph& g );
};

/*** 
 * Quick test implementation of PathRecorder interface.
 ***/
class TestPathRecorder : public PathRecorder< MotionGraph_t, Clip, Transition, ClipIterator, TransitionIterator >, public Singleton< TestPathRecorder >
{
        friend class Singleton< TestPathRecorder >;
public:
        template< typename Container > void search( Container &agenda, Clip start, Clip end )
        {
                std::vector< unsigned short > c( boost::num_vertices( g ) );

                reset();

                typedef boost::property_map< MotionGraph_t, boost::vertex_index_t >::type VertexId_PMap;
                VertexId_PMap vertex_id = boost::get( boost::vertex_index, g );
                typedef boost::iterator_property_map< std::vector< unsigned short >::iterator, VertexId_PMap, unsigned short, unsigned short & > ColourMap;

                boost::depth_first_visit(
                        g,
                        start,
                        boost::make_dfs_visitor( boost::make_list( TestPredRecorder(), TestForwardEdge() ) ),
                        ColourMap( c.begin(), vertex_id ),
                        Terminator< Clip, MotionGraph_t >( end )
                        );

                getPath( agenda, start, end );
        }

        template< typename Container > void getPath( Container &agenda, Clip start, Clip end ) const
        {
                Transition cur = boost::get( predEdgeMap, end );

                while( cur.m_eproperty )
                {
                        agenda.push_front( boost::target( cur, g ) );
                        agenda.push_front( boost::source( cur, g ) );

                        cur = boost::get( predEdgeMap, boost::source( cur, g ) );
                }

                if( agenda.empty() || agenda.front() != start )
                {
                        agenda.clear();
                }
        }

protected:
        TestPathRecorder( void ) : PathRecorder( *MotionGraph::getInstance().getBase() ) { }
        ~TestPathRecorder( void ) { }
        TestPathRecorder( const TestPathRecorder &mpr ) : PathRecorder( mpr.g ) { }
};

/**** Definitions of methods for visitor classes declared before TestPathRecorder ****/

template < class Edge, class Graph > void TestPredRecorder :: operator()( Edge e, const Graph &g )
{
        // simplified method, simply overwrite
        TestPathRecorder::getInstance().put( boost::target( e, g ), boost::source( e, g ) );
        TestPathRecorder::getInstance().put( boost::target( e, g ), e );
}

template < class Edge, class Graph > void TestForwardEdge :: operator()( Edge e, const Graph &g )
{
        // simplified method, simply overwrite
        TestPathRecorder::getInstance().put( boost::target( e, g ), boost::source( e, g ) );
        TestPathRecorder::getInstance().put( boost::target( e, g ), e );
}

/******************************/

/*** Roll N-sided die ***/
int roll( int sides )
{
        return rand() % sides;
}

/*** Roll N-sided die so that result is different to prev ***/
int rollUnique( int sides, int prev )
{
        int r = roll( sides );

        while( r == prev )
        {
                r = roll( sides );
        }

        return r;
}

/*** Choose a random src and target and attempt to find a path ***/
void randomTest( void )
{
        std::deque< Clip > mg;

        int start = rand() % boost::num_vertices( MotionGraph::getInstance() );
        int end = rand() % boost::num_vertices( MotionGraph::getInstance() );

        TestPathRecorder::getInstance().search( mg, start, end );

        if( mg.empty() )
        {
                std::cout << "Destination unreachable" << std::endl;
                return;
        }

        std::cout << "Path from " << start << " to " << end << ":" << std::endl;

        for( int i = 0; i < mg.size(); ++i )
        {
                std::cout << mg.at( i ) << ( i != mg.size() - 1 ? " -> " : "\n" );
        }
}

int main( void )
{
        srand( time( NULL ) );

        // randomly choose number of vertices
        int vertices = roll( 500 );

        // add vertices
        for( int i = 0; i < vertices; ++i )
        {
                boost::add_vertex( Clip_t(), MotionGraph::getInstance() );
        }

        // for each vertex V choose a random number of edges and connect them to random vertices (excluding the V itself)
        for( int i = 0; i < vertices; ++i )
        {
                int edges = roll( vertices - 1 );

                for( int j = 0; j < edges; j++ )
                {
                        int target = rollUnique( vertices, i );

                        boost::add_edge( i, target, Transition_t(), MotionGraph::getInstance() );
                }
        }

        // run a random path finding test for half the number of vertices
        for( int i = 0; i < vertices / 2; i++ )
        {
                randomTest();
        }

        return 0;
}