Ticket #10896: astar_maze2.cpp

//          Copyright W.P. McNeill 2010.
//          Copyright Georg Gast 2014
//
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)


// This program uses the A-star search algorithm in the Boost Graph Library to
// solve a maze.  It is an example of how to apply Boost Graph Library
// algorithms to filtered graphs.
//
// This program generates a random maze and then tries to find the shortest
// path from the lower left-hand corner to the upper right-hand corner.  Mazes
// are represented by two-dimensional grids where a cell in the grid may
// contain a barrier.  You may move up, down, right, or left to any adjacent
// cell that does not contain a barrier.
//
// Once a maze solution has been attempted, the maze is printed.  If a
// solution was found it will be shown in the maze printout. 
// Note that not all mazes have solutions.
//
// The default maze size is 20x10, though different dimensions may be
// specified on the command line.

// Boost
#include <boost/array.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/astar_search.hpp>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <boost/lexical_cast.hpp>

// STL
#include <iostream>
#include <vector>
#include <cmath>
#include <ctime>

// ------------------------------------------------------------------------------
// Types and filters
// ------------------------------------------------------------------------------

#define DIMS 2

// Distance traveled in the maze
using distance = double;

// the graph types
using graph_t = boost::grid_graph < DIMS >;
using traits_t = boost::graph_traits < graph_t >;

using vertex_t = traits_t::vertex_descriptor;
using edge_t = traits_t::edge_descriptor;

using vertex_path = std::vector < vertex_t > ;

// the property types
using index_map_type_t  = boost::property_map<graph_t, boost::vertex_index_t>::const_type;
using barrier_map_t             = boost::vector_property_map < bool, index_map_type_t >;
using predessor_map_t   = boost::vector_property_map < vertex_t, index_map_type_t >;
using distance_map_t    = boost::vector_property_map < distance, index_map_type_t >;

// the filter
template <typename BarrierMap>
struct vertex_filter_no_barrier
{
        vertex_filter_no_barrier()      {}
        vertex_filter_no_barrier(const BarrierMap& b) : m_barrier(b) {}

        template <typename Vertex>
        bool operator()(const Vertex& v) const
        {
                return boost::get(m_barrier, v) == false;
        }

        BarrierMap m_barrier;
};
struct edge_filter_dummy
{
        edge_filter_dummy()     {}

        template <typename Edge>
        bool operator()(const Edge& e) const
        {
                return true;
        }
};

// the fitered grid
using filtered_graph_t = boost::filtered_graph < graph_t, edge_filter_dummy, vertex_filter_no_barrier<barrier_map_t>>;

// ------------------------------------------------------------------------------
// Helper
// ------------------------------------------------------------------------------
vertex_t start_vertex(const graph_t& /*graph*/)
{
        /*
        size_t size_x = graph.length(0);
        size_t size_y = graph.length(1);
        */
        vertex_t v = { 0, 0 };
        return v;
}
vertex_t goal_vertex(const graph_t& graph)
{
        size_t size_x = graph.length(0);
        size_t size_y = graph.length(1);

        vertex_t v = { size_x - 1, size_y - 1 };
        return v;
}

#define BARRIER "#"
void print_maze(const graph_t& graph, const barrier_map_t& barrier, const vertex_path& solution)
{
        size_t size_x = graph.length(0);
        size_t size_y = graph.length(1);

        // Header
        for (size_t i = 0; i < size_x + 2; i++)
                std::cout << BARRIER;
        std::cout << std::endl;

        vertex_t s = start_vertex(graph);
        vertex_t g = goal_vertex(graph);

        // body
        // x: left to right
        // y: bottom to top (0 bottom, growing upwards)
        for (size_t y = size_y - 1; y < size_y; --y) // underflow intended
        {
                for (size_t x = 0; x < size_x; ++x)
                {
                        // barrier on the left side
                        if (x == 0)
                        {
                                std::cout << BARRIER;
                        }

                        vertex_t v = { x, y };
                        bool is_solution_vtx  =
                                (std::find(solution.begin(), solution.end(), v) != solution.end());

                        if (get(barrier, v))
                        {
                                std::cout << "#";
                        }
                        else if (is_solution_vtx)
                        {
                                std::cout << ".";
                        }
                        else if (v == s)
                        {
                                std::cout << "S";
                        }
                        else if (v == g)
                        {
                                std::cout << "G";
                        }
                        else
                        {
                                std::cout << " ";
                        }

                        // barrier on the right side
                        if (x == size_x-1)
                        {
                                std::cout << BARRIER;
                        }

                }
                std::cout << std::endl;
        }

        // footer
        for (size_t i = 0; i < size_x + 2; i++)
                std::cout << BARRIER;
        std::cout << std::endl;

}

void create_maze(const graph_t& graph, barrier_map_t& barrier)
{
        std::cout << "Creating Barriers ..." << std::endl;

        size_t size_x = graph.length(0);
        size_t size_y = graph.length(1);
        size_t total_space = size_x * size_y;

        size_t barriers_left = total_space / 4;
        boost::random::mt19937 gen;
        gen.seed(std::time(0));

        boost::random::uniform_int_distribution<> start_x(0, size_x - 1);
        boost::random::uniform_int_distribution<> start_y(0, size_y - 1);
        boost::random::uniform_int_distribution<> direction(0, 1);
        boost::random::uniform_int_distribution<> wall_length_x(1, size_x/4);
        boost::random::uniform_int_distribution<> wall_length_y(1, size_y/4);

        // start and goal vertex
        vertex_t s = start_vertex(graph);
        vertex_t g = goal_vertex(graph);

        while (barriers_left)
        {
                size_t sx = start_x(gen);
                size_t sy = start_y(gen);

                int dx, dy, wall;
                if (direction(gen) == 0)
                {
                        // x
                        dx = 1; dy = 0;
                        wall = wall_length_x(gen);
                }
                else
                {
                        // y
                        dx = 0; dy = 1;
                        wall = wall_length_y(gen);
                }

                // limit wall length to 20
                if (wall > 10) wall = 10;

                while (wall > 0 && barriers_left > 0)
                {
                        vertex_t v = { sx, sy };

                        // we dont put a barrier on start or goal
                        if (v == s || v == g)
                                break;

                        bool current_value = boost::get(barrier, v);
                        if (current_value == false)
                        {
                                boost::put(barrier, v, true);
                                barriers_left--;
                                wall--;
                        }
                        else
                        {
                                // stop this wall on this obstacle
                                break;
                        }
                        // next vertex
                        sx += dx; sy += dy;

                        // we dont go outside of our grid
                        if (sx >= size_x || sy >= size_y)
                                break;
                        if (barriers_left % 100 == 0)
                        {
                                std::cout << barriers_left << "...";
                        }
                }
        }
        std::cout << std::endl;
}
// ------------------------------------------------------------------------------
// AStar
// ------------------------------------------------------------------------------
// Visitor that terminates when we find the goal vertex
struct found_goal {}; // exception which is thrown when we find a solution
struct astar_goal_visitor :public boost::default_astar_visitor
{
        astar_goal_visitor(vertex_t goal) :m_goal(goal) {};

        void examine_vertex(vertex_t u, const filtered_graph_t&)
        {
                if (u == m_goal)
                        throw found_goal();
        }

private:
        vertex_t m_goal;
};

// This calculates the Euclidean distance between a vertex and a goal vertex.
class euclidean_heuristic :
        public boost::astar_heuristic<filtered_graph_t, double>
{
public:
        euclidean_heuristic(vertex_t goal) :m_goal(goal) {};

        double operator()(vertex_t v) {
                return sqrt(pow(double(m_goal[0] - v[0]), 2) + pow(double(m_goal[1] - v[1]), 2));
        }

private:
        vertex_t m_goal;
};

void solve(vertex_t s, vertex_t g, filtered_graph_t& fg, vertex_path& solution)
{
        // get the index map for the associated properties
        index_map_type_t index_map(get(boost::vertex_index, fg));

        // The predecessor map is a vertex-to-vertex mapping.
        predessor_map_t predecessor(num_vertices(fg), index_map);

        // The distance map is a vertex-to-distance mapping.
        distance_map_t distances(num_vertices(fg), index_map);

        // finally the weight map
        boost::static_property_map<distance> weight(1);

        euclidean_heuristic heuristic(g);
        astar_goal_visitor visitor(g);

        try {
                astar_search(fg, s, heuristic,
                        boost::weight_map(weight).
                        predecessor_map(predecessor).
                        distance_map(distances).
                        visitor(visitor));
        }
        catch (const found_goal&  /*fg*/) {
                // Walk backwards from the goal through the predecessor chain adding
                // vertices to the solution path.
                for (vertex_t u = g; u != s; u = predecessor[u])
                {
                        if (u != s && u != g)   solution.push_back(u);
                }
        }
}
// ------------------------------------------------------------------------------
// main
// ------------------------------------------------------------------------------
int main(int argc, char** argv)
{
        // The default maze size is 20x10.  A different size may be specified on
        // the command line.
        std::size_t x = 20;
        std::size_t y = 10;

        if (argc == 3) {
                x = boost::lexical_cast<std::size_t>(argv[1]);
                y = boost::lexical_cast<std::size_t>(argv[2]);
        }

        // Define a grid which doesnt wrap
        boost::array<std::size_t, DIMS> lengths = { x, y };
        boost::array<bool, DIMS> wrapped = { false, false };
        graph_t graph(lengths, wrapped);

        // Get the index map of the grid graph
        index_map_type_t index_map(get(boost::vertex_index, graph));

        // create the property for the vertexes: Barrier
        barrier_map_t barrier_map(num_vertices(graph), index_map);

        // create the walls
        create_maze(graph, barrier_map);

        // filter the graph
        filtered_graph_t fg(graph, edge_filter_dummy(), vertex_filter_no_barrier<barrier_map_t>(barrier_map));

        vertex_path solution;
        vertex_t s = start_vertex(graph);
        vertex_t g = goal_vertex(graph);
        solve(s,g,fg, solution);
        print_maze(graph, barrier_map, solution);

        if (solution.size())
                std::cout << "Maze solved" << std::endl;
        else
                std::cout << "Maze _NOT_ solved" << std::endl;
}