A星寻路算法，C++版，Go语言版

2015-1-24 更新，发现错误，AStar.cpp文件的60行，原本的'>'改为'<'。。。。。

---

 

　　A星寻路算法真是我一生接触的第一个人工智能算法了。。。

　　A星寻路算法显然是用来寻路的，应用也很普遍，比如梦幻西游。。。算法的思路很简单，就是在bfs的基础上加了估值函数。

　　它的核心是 F(x) = G(x) + H(x) 和open、close列表：

　　G(x)表示从起点到X点的消耗（或者叫移动量什么的），H(X)表示X点到终点的消耗的估值，F(x)就是两者的和值。open列表记录了可能要走的区域，close列表记录了不会再考虑的区域。我们每次都选F值最小的区域搜索，就能搜到一条到终点的最短路径，其中估值H越接近准确值，需要搜索的节点就越少。

　　A星算法的步骤：

　　{

　　　　将起点区域添加到open列表中，该区域有最小的和值。

　　　　重复以下：

　　　　　　将open列表中最小F值的区域X移除，然后添加到close列表中。

　　　　　　对于与X相邻的每一块可通行且不在close列表中的区域T：

　　　　　　　　　　如果T不在open列表中：添加到open列表，把X设为T的前驱

　　　　　　　　　　如果T已经在open列表中：检查 F 是否更小。如果是，更新 F 和前驱

　　　　直到：

　　　　　　终点添加到了close列表。（已找到路径）

　　　　　　终点未添加到close列表且open列表已空。（未找到路径）

　　}

 

Golang实现如下：

（应该把OpenList和CloseList做成接口更好，赶时间先这样写了。。。）

  1 package astar

  2 

  3 import (

  4     "container/heap"

  5     "container/list"

  6     "math"

  7 )

  8 

  9 type Point struct {

 10     X int

 11     Y int

 12 }

 13 

 14 func (a Point) getManhattanDistance(b Point) int {

 15     return int(math.Abs(float64(a.X-b.X)) + math.Abs(float64(a.Y-b.Y)))

 16 }

 17 

 18 type Area struct {

 19     Point

 20     G int

 21     H int

 22 }

 23 

 24 type CloseList [][]bool

 25 

 26 func (CloseList) New(x, y int) CloseList {

 27     res := make([][]bool, x)

 28     for i := 0; i < x; i++ {

 29         res[i] = make([]bool, y)

 30     }

 31     return res

 32 }

 33 

 34 type OpenList []Area

 35 

 36 func (ls OpenList) Len() int {

 37     return len(ls)

 38 }

 39 

 40 func (ls OpenList) Less(a, b int) bool {

 41     return ls[a].G < ls[b].G

 42 }

 43 

 44 func (ls OpenList) Swap(a, b int) {

 45     ls[a], ls[b] = ls[b], ls[a]

 46 }

 47 

 48 func (ls *OpenList) Push(x interface{}) {

 49     *ls = append(*ls, x.(Area))

 50 }

 51 

 52 func (ls *OpenList) Pop() interface{} {

 53     lenth := (*ls).Len()

 54     res := (*ls)[lenth-1]

 55     *ls = (*ls)[0 : lenth-1]

 56     return res

 57 }

 58 

 59 func getPath(start, end Point, pathPre map[Point]Point) *list.List {

 60     pathList := list.New()

 61     pathCur := end

 62     for !(pathCur == start) {

 63         pathList.PushFront(pathCur)

 64         pathCur = pathPre[pathCur]

 65     }

 66     pathList.PushFront(start)

 67     return pathList

 68 }

 69 

 70 func Search(legal [][]int, start, end Point) (int, *list.List) {

 71     row := len(legal)

 72     column := len(legal[0])

 73 

 74     var closeList CloseList

 75     closeList = closeList.New(row, column)

 76 

 77     var openList OpenList

 78     openList = make([]Area, row*column)

 79 

 80     pathPre := map[Point]Point{}

 81 

 82     dir := [][]int{{0, 1}, {0, -1}, {1, 0}, {-1, 0}}

 83 

 84     openList[0] = Area{start, 0, 0}

 85 

 86     for len(openList) > 0 {

 87         cur := openList.Pop().(Area)

 88         closeList[cur.X][cur.Y] = true

 89         if cur.Point == end {

 90             return cur.G, getPath(start, end, pathPre)

 91         }

 92         for _, v := range dir {

 93             x := cur.X + v[0]

 94             y := cur.Y + v[1]

 95             g := cur.G + 1

 96             h := end.getManhattanDistance(cur.Point)

 97             if x >= 0 && x < row && y >= 0 && y < column && legal[x][y] != 0 && !closeList[x][y] {

 98                 inopen := false

 99                 tar := Area{Point{x, y}, g, h}

100                 for index := 0; index < openList.Len(); index++ {

101                     if openList[index].Point == tar.Point {

102                         inopen = true

103                         if openList[index].G > g {

104                             openList[index].G = g

105                             pathPre[tar.Point] = cur.Point

106                             break

107                         }

108                     }

109                 }

110                 if !inopen {

111                     heap.Push(&openList, tar)

112                     pathPre[tar.Point] = cur.Point

113                 }

114             }

115         }

116     }

117     return -1, nil

118 }

 1 package main

 2 

 3 import (

 4     "ex1/astar"

 5     "fmt"

 6 )

 7 

 8 func main() {

 9     legal := [][]int{

10         {0, 0, 0, 1, 1, 1, 1},

11         {0, 0, 1, 1, 0, 1, 0},

12         {1, 1, 1, 1, 1, 0, 0},

13         {1, 0, 0, 1, 0, 0, 0},

14         {1, 1, 1, 1, 1, 1, 1},

15     }

16     start := astar.Point{0, 6}

17     end := astar.Point{4, 6}

18     lenth, path := astar.Search(legal, start, end)

19     fmt.Println(lenth)

20 

21     for p := path.Front(); p != nil; p = p.Next() {

22         fmt.Println(p.Value)

23     }

24 }

 

 

 

C++实现如下：

　　

  1 /***************************

  2 *    A星寻路算法

  3 *

  4 *    Anti-Magic

  5 *    2014/5/26

  6 ****************************/

  7 

  8 #ifndef __ASTAR_H__

  9 #define __ASTAR_H__

 10 

 11 #include <map>

 12 #include <set>

 13 #include <vector>

 14 #include <list>

 15 

 16 //属性宏

 17 #define SYNTHESIZE(varType, varName, funName) \

 18 protected: \

 19     varType varName; \

 20 public: \

 21 virtual varType get##funName(void) const { \

 22     return varName; \

 23 } \

 24 virtual void set##funName(varType var) { \

 25     varName = var; \

 26 }

 27 

 28 /***************************

 29 *    Point类

 30 *    表示一个二维坐标

 31 ****************************/

 32 class Point

 33 {

 34     SYNTHESIZE(int, _x, X);

 35     SYNTHESIZE(int, _y, Y);

 36 public:

 37     Point();

 38     Point(int x, int y);

 39     //获得曼哈顿距离

 40     int getManhattanDistance(const Point& t) const;

 41     //获得欧几里得距离

 42     double getEuclidDistance(const Point& t) const;

 43     bool operator== (const Point& t) const;

 44     bool operator< (const Point& t) const;

 45 };

 46 

 47 /***************************

 48 *    Local类

 49 *    在A星寻路算法中的单位区域

 50 ****************************/

 51 class Local : public Point

 52 {

 53     SYNTHESIZE(int, _g, G);

 54     SYNTHESIZE(int, _h, H);

 55 public:

 56     Local();

 57     Local(int x, int y, int g, int h);

 58     Local(const Point& t, int g, int h);

 59     Local(const Local& t);

 60     int getF() const;

 61     bool operator< (const Local& t) const;

 62 };

 63 

 64 /***************************

 65 *    SuperCloseList类 抽象类

 66 *    Close列表的基类

 67 *    因为实现Close列表在不同场合有不同的实现

 68 *    需要继承此类

 69 ****************************/

 70 class SuperCloseList

 71 {

 72 public:

 73     //插入close列表

 74     virtual void insert(const Local& p) = 0;

 75     //查询close列表中是否已存在此区域

 76     virtual bool find(const Local& p) const = 0;

 77 };

 78 

 79 /***************************

 80 *    CloseList类

 81 *    Close列表的哈希实现

 82 ****************************/

 83 class CloseList : public SuperCloseList

 84 {

 85 public:

 86     CloseList();

 87     CloseList(int x, int y);

 88     ~CloseList();

 89     void insert(const Local& p) override;

 90     bool find(const Local& p) const override;

 91 private:

 92     bool** _close;

 93     int size_x, size_y;

 94     //申请数组内存

 95     void newClose(int x, int y);

 96 };

 97 

 98 /***************************

 99 *    A星算法类

100 *    实现A星算法的逻辑

101 ****************************/

102 class AStar

103 {

104 public:

105     AStar(std::vector<std::vector<bool> >& legal, Point& start, Point& end);

106     //获得距离

107     int getDistance() const;

108     //获得路径

109     std::list<Point> getPath() const;

110 private:

111     std::vector<std::vector<bool> > _legal;

112     std::multiset<Local> open;

113     CloseList close;

114     Point _start;

115     Point _end;

116     std::map<Point, Point> _path_pre;

117     std::list<Point> _path_list;

118     int _distance;

119     //路径规划，算法的核心

120     void pathPlanning();

121     //检查区域

122     void checkLocal(Local& local_tar, Local& local_cur);

123     //生成路径

124     void initPath();

125 };

126 

127 #endif

  1 #include "AStar.h"

  2 

  3 Point::Point()

  4 {

  5     _x = _y = 0;

  6 }

  7 Point::Point(int x, int y)

  8 {

  9     _x = x;

 10     _y = y;

 11 }

 12 int Point::getManhattanDistance(const Point& t) const

 13 {

 14     return std::abs(_x - t._x) + std::abs(_y - t._y);

 15 }

 16 double Point::getEuclidDistance(const Point& t) const

 17 {

 18     return std::sqrt((_x - t._x) * (_x - t._x) + (_y - t._y) * (_y - t._y));

 19 }

 20 bool Point::operator== (const Point& t) const

 21 {

 22     return _x == t._x && _y == t._y;

 23 }

 24 bool Point::operator< (const Point& t) const

 25 {

 26     if (_x == t._x)

 27     {

 28         return _y < t._y;

 29     }

 30     return _x < t._x;

 31 }

 32 

 33 Local::Local() : Point()

 34 {

 35     _g = _h = 0;

 36 }

 37 Local::Local(int x, int y, int g, int h) : Point(x, y)

 38 {

 39     _g = g;

 40     _h = h;

 41 }

 42 Local::Local(const Point& t, int g, int h) : Point(t)

 43 {

 44     _g = g;

 45     _h = h;

 46 }

 47 Local::Local(const Local& t)

 48 {

 49     _x = t._x;

 50     _y = t._y;

 51     _g = t._g;

 52     _h = t._h;

 53 }

 54 int Local::getF() const

 55 {

 56     return _g + _h;

 57 }

 58 bool Local::operator< (const Local& t) const

 59 {

 60     return getF() < t.getF();

 61 }

 62 

 63 CloseList::CloseList()

 64 {

 65     newClose(100, 100);

 66 }

 67 CloseList::CloseList(int x, int y)

 68 {

 69     newClose(x, y);

 70 }

 71 CloseList::~CloseList()

 72 {

 73     for (int i = 0; i < size_y; i++)

 74     {

 75         delete[] _close[i];

 76     }

 77     delete[] _close;

 78 }

 79 void CloseList::insert(const Local& p)

 80 {

 81     _close[p.getX()][p.getY()] = true;

 82 }

 83 bool CloseList::find(const Local& p) const

 84 {

 85     return _close[p.getX()][p.getY()] == true;

 86 }

 87 void CloseList::newClose(int x, int y)

 88 {

 89     size_x = x;

 90     size_y = y;

 91     _close = new bool*[x];

 92     for (int i = 0; i < x; i++)

 93     {

 94         _close[i] = new bool[y];

 95         memset(_close[i], false, sizeof(_close[i]));

 96     }

 97 }

 98 

 99 AStar::AStar(std::vector<std::vector<bool> >& legal, Point& start, Point& end)

100 {

101     _legal = legal;

102     _start = start;

103     _end = end;

104     pathPlanning();

105 }

106 int AStar::getDistance() const

107 {

108     return _distance;

109 }

110 std::list<Point> AStar::getPath()  const

111 {

112     return _path_list;

113 }

114 void AStar::pathPlanning()

115 {

116     open.insert(Local(_start, 0, 0));

117     int dir[][2] = {

118         { 0, 1 }, { 0, -1 },

119         { 1, 0 }, { -1, 0 }

120     };

121     while (!open.empty())

122     {

123         Local cur = *open.begin();

124         open.erase(open.begin());

125         close.insert(cur);

126         if (cur == _end)

127         {

128             initPath();

129             _distance = cur.getG();

130             return;

131         }

132         for (int i = 0; i < 4; i++)

133         {

134             int x = cur.getX() + dir[i][0];

135             int y = cur.getY() + dir[i][1];

136             int g = cur.getG() + 1;

137             int h = _end.getManhattanDistance(Point(x, y));

138             Local point_choose(x, y, g, h);

139             checkLocal(point_choose, cur);

140         }

141     }

142     _distance = -1;

143 }

144 void AStar::checkLocal(Local& local_tar, Local& local_cur)

145 {

146     int x = local_tar.getX();

147     int y = local_tar.getY();

148     int g = local_tar.getG();

149     int h = local_tar.getH();

150     if (x < _legal.size() && x >= 0 && y < _legal[0].size() && y >= 0 &&

151         _legal[x][y] && !close.find(local_tar))

152     {

153         bool inopen = false;

154         for (std::multiset<Local>::iterator it = open.begin(); it != open.end(); it++)

155         {

156             if (it->getX() == x && it->getY() == y)

157             {

158                 inopen = true;

159                 if (it->getG() > g)

160                 {

161                     open.erase(it);

162                     open.insert(local_tar);

163                     _path_pre[local_tar] = local_cur;

164                     break;

165                 }

166             }

167         }

168         if (!inopen)

169         {

170             open.insert(local_tar);

171             _path_pre[local_tar] = local_cur;

172         }

173     }

174 }

175 void AStar::initPath()

176 {

177     Point path_res = _end;

178     while (!(path_res == _start))

179     {

180         _path_list.push_front(path_res);

181         path_res = _path_pre[path_res];

182     }

183     _path_list.push_front(_start);

184 }

 

测试代码：

 1 #define _CRT_SECURE_NO_DEPRECATE

 2 

 3 #include "AStar.h"

 4 #include <stdio.h>

 5 

 6 int main()

 7 {

 8     std::vector<std::vector<bool> > legal {

 9         std::vector<bool> {0, 0, 0, 1, 1, 1, 1},

10         std::vector<bool> {1, 1, 1, 1, 0, 1, 0},

11         std::vector<bool> {1, 1, 1, 1, 1, 0, 0},

12         std::vector<bool> {1, 0, 0, 0, 0, 0, 0},

13         std::vector<bool> {1, 1, 1, 1, 1, 1, 1},

14     };

15     Point start(1, 5);

16     Point end(4, 5);

17     AStar* astar = new AStar(legal, start, end);

18     printf("%d\n", astar->getDistance());

19     std::list<Point> path = astar->getPath();

20     for (auto path_cur : path)

21     {

22         printf(".....%d %d\n", path_cur.getX(), path_cur.getY());

23     }

24     delete astar;

25     system("pause");

26     return 0;

27 }