应用A*解决最短路径问题

#include 
#include "astar.hpp"

struct MatrixNode
{
 bool MatrixNode::operator==(const MatrixNode& n) const { return x == n.x && y == n.y; }
 int f() const { return g() + h(); }
 int g() const { return dist; }
 int h() const { return left; }
 int g(int val) { return dist = val; }

 MatrixNode(int a, int b, int dst, int lef) : x(a), y(b), dist(dst), left(lef) {}
 int x, y, dist, left;
};

class Matrix
{
 friend struct MatrixNode;

 char const* data;
 const int width, height;
 int endx, endy;

 bool is_passable(int x, int y) const { return data[x + width * y] != '#'; }

 int distance(int x1, int y1, int x2, int y2) const
 {
  int x = std::abs(x1 - x2);
  int y = std::abs(y1 - y2);
  return 10 * std::abs(x - y) + 14 * std::min(x, y);
 }

public:
 typedef MatrixNode element_type;

 explicit Matrix(const char* dat, int len, int w)
  : data(dat), width(w), height(len / w)
 {
  int ix = std::find(data, data + width*height-1, 'E') - data;
  endx = ix % width, endy = ix / width;
 }

 template 
 void foreach_adjacents(const MatrixNode& a, Op op) const
 {
  static int offx[] = {-1, 0,1,-1, /*0,*/ 1,-1, 0, 1};
  static int offy[] = { 1, 1,1, 0, /*0,*/ 0,-1,-1,-1};
  for (size_t i = 0; i < sizeof(offx)/sizeof(int); ++i)
  {
   int x = a.x + offx[i], y = a.y + offy[i];
   if (x >= 0 && x < width && y >= 0 && y < height && is_passable(x, y))
    op(MatrixNode(x, y, a.g() + distance(a.x, a.y, x, y), distance(x, y, endx, endy)));
  }
 }

 MatrixNode start() const
 {
  int ix = std::find(data, data + width*height-1, 'S') - data;
  int x = ix % width, y = ix / width;
  return MatrixNode(x, y, 0, distance(x, y, endx, endy));
 }

 MatrixNode finish() const { return MatrixNode(endx, endy, -1, 0); }

 template  void result(const R& r, const R& rr) const;
};

#include 
#include 

template 
void Matrix::result(const R& r, const R& rr) const
{
 using namespace std;
 string m(data, data + width*height);
 for (typename R::const_iterator i = r.begin(), ie = r.end(); i != ie; ++i)
  m[i->x + width * i->y] = '*';
 for (typename R::const_iterator i = rr.begin(), ie = rr.end(); i != ie; ++i)
  m[i->x + width * i->y] = ' ';
 if (!r.empty())
  m[r.front().x + width * r.front().y] = 'S',
  m[r.back().x + width * r.back().y] = 'E';
 for (int i = 0; i < height; ++i)
  copy(m.begin() + width*i, m.begin() + width*(i+1), ostream_iterator(cout)),
  cout << endl;
}

//struct Print { template  void operator()(const Matrix& m, const R& r, const R& rr) const { m.result(r, rr); } };

int main()
{
# define MATRIX_LINE \
 "############################################################"
# define MATRIX_DATA MATRIX_LINE \
 "#..........................................................#" \
 "#.............................#............................#" \
 "#.............................#............................#" \
 "#.............................#............................#" \
 "#.......S.....................#............................#" \
 "#.............................#............................#" \
 "#.............................#............................#" \
 "#.............................#............................#" \
 "#.............................#............................#" \
 "#.............................#............................#" \
 "#.............................#............................#" \
 "#.............................#............................#" \
 "#######.#######################################............#" \
 "#....#........#............................................#" \
 "#....#........#............................................#" \
 "#....##########............................................#" \
 "#..........................................................#" \
 "#..........................................................#" \
 "#..........................................................#" \
 "#..........................................................#" \
 "#..........................................................#" \
 "#...............................##############.............#" \
 "#...............................#........E...#.............#" \
 "#...............................#............#.............#" \
 "#...............................#............#.............#" \
 "#...............................#............#.............#" \
 "#...............................###########..#.............#" \
 "#..........................................................#" \
 "#..........................................................#" \
 "############################################################"
 Matrix mat1(MATRIX_DATA, sizeof(MATRIX_DATA)-1, sizeof(MATRIX_LINE)-1);
# undef MATRIX_LINE
# undef MATRIX_DATA

# define MATRIX_LINE \
 "###################"
# define MATRIX_DATA MATRIX_LINE \
 "#.................#" \
 "#........#........#" \
 "#........#........#" \
 "#..S.....#.....E..#" \
 "#........#........#" \
 "#........#........#" \
 "#.................#" \
 "###################"
 Matrix mat2(MATRIX_DATA, sizeof(MATRIX_DATA)-1, sizeof(MATRIX_LINE)-1);
# undef MATRIX_LINE
# undef MATRIX_DATA

 Astar astar;
 astar.search(mat1, mat1.start(), mat1.finish());
 astar.search(mat2, mat2.start(), mat2.finish());
 return 0;
}