/* * A-Star.cpp Copyright (C) 2013 Paolo Medici * * This library is free software; you can redistribute it and/or modify it under the terms of the * GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of * the License, or (at your option) any later version. * * This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public * License for more details. * * You should have received a copy of the GNU Lesser General Public License along with this library; if not, * write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include <vector> #include <map> #include <cmath> #include <limits> #include <iostream> #include <cassert> //////////////// #define NUM_OF_DIRECTIONS 8 /// DIRECTIONS static const int directions_x[NUM_OF_DIRECTIONS] = {0,1,1,1,0,-1,-1,-1}; static const int directions_y[NUM_OF_DIRECTIONS] = {-1,-1,0,1,1,1,0,-1}; // "Euristic" float H(const std::pair<int, int> & a, const std::pair<int,int> & b) { return std::sqrt( std::pow(a.first - b.first,2) + std::pow(a.second - b.second,2) ); } bool find_path(std::vector< std::pair<int, int> > & out, const bool *wall, unsigned int width, unsigned int height, std::pair<int,int> start, std::pair<int,int> goal) { assert (width>0 && height>0 && width<65536 && height<65536); // bounds assert (wall != 0 ); // invalid parameters float *g = new float[width * height]; // travel cost map int *path = new int[width * height]; // travelling directions map std::multimap<float, std::pair<int,int> > openset; // the open set float delta[NUM_OF_DIRECTIONS]; // precomputed traversing cost // precompute trasversing cost for(int i =0;i<NUM_OF_DIRECTIONS;++i) delta[i] = std::sqrt( directions_x[i]*directions_x[i] + directions_y[i]*directions_y[i] ); // initialize path and g for(int i =0;i<width * height;++i) { // g[i] = wall[i] ? 0.0f : std::numeric_limits<float>::max() // could be used as alternative g[i] = std::numeric_limits<float>::max(); // path[i] = -1; } // set the starting point g[ start.first + start.second * width] = 0.0; float f = H(start, goal); openset.insert (std::pair<float, std::pair<int,int> >(f, start)); // iterate until openset is empty while(!openset.empty()) { // get the element with lower f using multimap std::multimap<float, std::pair<int,int> >::iterator it=openset.begin(); float f = it->first; std::pair<int,int> current = it->second; openset.erase(it); // is it the goal? if(current == goal) { // reconstruct path std::pair<int, int> cur = goal; out.push_back(cur); while(start != cur) { int d = path[cur.first + cur.second * width]; if(d == -1) { // INTERNAL ERROR delete [] g; delete [] path; return false; } cur.first = d % width; cur.second = d / width; out.push_back( cur ); } delete [] g; delete [] path; return true; } // search on neighbours for(int i=0;i<NUM_OF_DIRECTIONS;++i) { std::pair<int, int> n(current.first + directions_x[i], current.second + directions_y[i]); // evaluate point inside map and not walled if(n.first>=0 && n.second>=0 && n.first<width && n.second<height && (!wall[ n.first + n.second * width ]) ) { // update travel cost (g) float t_g = g[current.first + current.second * width] + delta[i]; if(t_g < g[n.first + n.second * width]) { // compute expected cost to goal (f) float t_f = t_g + H(n, goal); g[n.first + n.second * width] = t_g; // update travel cost path[n.first + n.second * width] = current.first + current.second * width; // store coming path openset.insert (std::pair<float, std::pair<int,int> >(t_f, n)); // put this path on openset } } } } delete [] g; delete [] path; return false; } //////////////// TEST APP ///////////////////// int main() { std::vector< std::pair<int, int> > path; // an example of wall-map bool wall[8*8]={0,0,0,0,0,0,0,0, 0,0,1,1,1,1,0,0, 0,0,1,0,0,1,1,1, 0,0,1,0,0,0,0,0, 0,0,1,0,1,1,0,0, 0,0,1,0,1,0,0,0, 0,0,0,0,1,0,0,0, 0,0,0,0,1,0,0,0}; // execute A-star std::pair<int,int> start(1,1); std::pair<int,int> goal(5,6); find_path(path, wall, 8,8, start, goal ); // print path for(std::vector< std::pair<int, int> >::const_reverse_iterator i = path.rbegin(); i!= path.rend(); ++i) { std::cout << i->first << ' ' << i->second << '\n'; } // draw map char draw[8*8]; for(int i = 0;i<8*8;++i) { draw[i] = wall[i] ? '#' : '.'; } for(std::vector< std::pair<int, int> >::const_reverse_iterator i = path.rbegin(); i!= path.rend(); ++i) { draw[i->first + i->second * 8 ] = '*'; } draw[start.first + start.second * 8 ] = 'S'; draw[goal.first + goal.second * 8 ] = 'G'; for(int i = 0;i<8*8;++i) { std::cout << draw[i]; if((i&0x07)==0x07) std::cout << std::endl; } }