# A*寻路算法

273人阅读 评论(0)
 A*算法是静态环境下求最短路径的不二之选，由于是启发式搜索，比dijkstra、深搜广搜要快的多啦。
A*也算是我第一次接触的移动机器人算法，CSDN上的科普文章也不少，但我作为一个机械的小白，实现出来还是小有成就感滴。
今天抽空和大家分享一下源码，开发环境win7_64+opengl+vs2013~很遗憾没有补好注释，因为又要找工作又要搞课题。


Astar.h

#include<vector>
#include <algorithm>
#include"Heap.h"
using namespace std;

struct node
{
int row, col;
int f, g, h;
node* father;
bool free = true;
bool visited = false;
};

struct pose{
int col;
int row;
bool operator!=(const pose &p)const
{
if (this->col == p.col && this->row == p.row)
return false;
return true;
}
};

class Cmp
{
public:
bool operator()(node &n1, node &n2)
{
if (n1.f < n2.f)
return true;
return false;
}
};

void shortestPathByAstart(node grid[][20], pose start, pose goal);

bool existOpenlist(Heap<node, Cmp> &h, const node &n, int &index);

bool existCloselist(const vector<node> &v, const node &n);

inline int getH(const node &n, const pose &goal);

Astar.cpp

#include "Astart.h"

void shortestPathByAstart(node grid[][20], pose start, pose goal)
{
Cmp cmp;
Heap<node, Cmp>openlist(cmp);
vector<node>closelist;

if (goal.col == start.col && goal.row == start.row)
return;

grid[start.row][start.col].visited = 1;
grid[start.row][start.col].g = 0;
grid[start.row][start.col].h = getH(grid[start.row][start.col], goal);
grid[start.row][start.col].f = grid[start.row][start.col].h;
grid[start.row][start.col].father = NULL;
openlist.insert(grid[start.row][start.col]);

while (!openlist.empty())
{
// 找出开放列表中f值最小的点，并放在关闭列表
node temp = openlist.pop_head();
closelist.push_back(temp);
int x = temp.row, y = temp.col;
if (x == 3 && y == 7)
{
int dds = 1;
}
// 考察temp周围8个方向的节点
for (int k = 0; k < 8; k++)
{
int i = 0, j = 0;
switch (k)
{
case 0:i = -1, j = 0; break;
case 1:i = -1, j = 1; break;
case 2:i = 0, j = 1; break;
case 3:i = 1, j = 1; break;
case 4:i = 1, j = 0; break;
case 5:i = 1, j = -1; break;
case 6:i = 0, j = -1; break;
case 7:i = -1, j = -1; break;
}
if ((x + i) >= 0 && (x + i) < 20 && (y + j) >= 0 && (y + j) < 20 && grid[x + i][y + j].free &&  !existCloselist(closelist, grid[x + i][y + j]))
{
int index;
if (existOpenlist(openlist, grid[x + i][y + j],index))  //已经在开放列表, 比较g值
{
// 经过temp的g值
int dg = 10;
if (abs(i) == 1 && abs(j) == 1)
dg = 14;
int g_viatemp = grid[x][y].g + dg;
// 比较
if (g_viatemp < grid[x + i][y + j].g)
{
openlist[index].g = grid[x + i][y + j].g = g_viatemp;
openlist[index].h = grid[x + i][y + j].h = getH(grid[x + i][y + j], goal);
openlist[index].f = grid[x + i][y + j].f = grid[x + i][y + j].g + grid[x + i][y + j].h;
openlist[index].father = grid[x + i][y + j].father = &grid[x][y];

openlist.resetPriority(index);
}
}
else // 不在开放列表，加入到开放列表，并设置父节点、计算ghf值
{
int dg = 10;
if (abs(i) == 1 && abs(j) == 1)
dg = 14;

grid[x + i][y + j].visited = 1;
grid[x + i][y + j].g = grid[x][y].g + dg;
grid[x + i][y + j].h = getH(grid[x + i][y + j], goal);
grid[x + i][y + j].f = grid[x + i][y + j].g + grid[x + i][y + j].h;
grid[x + i][y + j].father = &grid[x][y];
openlist.insert(grid[x + i][y + j]);

if (x + i == goal.row && y + j == goal.col)  // 找到了目标
return;
}

}
}
}
}

bool existOpenlist(Heap<node,Cmp> &h, const node &n, int &index)
{
for (int i = 0; i < h.size(); i++)
{
if (h[i].col == n.col && h[i].row == n.row)
{
index = i;
return true;
}

}
return false;
}
bool existCloselist(const vector<node> &l, const node &n)
{
for (auto &e : l)
{
if (e.col == n.col&&e.row == n.row)
return true;
}
return false;
}

int getH(const node &n, const pose &goal)
{
int detx = abs(n.col - goal.col);
int dety = abs(n.row - goal.row);

return 10 * abs(detx - dety) + 14 * min(detx, dety);
}

main.cpp

#include <Windows.h>
#include <GL/glut.h>
#include "Astart.h"
using namespace std;

node grid[20][20];
pose cur,start,goal;

bool rightbutton = 1;

void init()
{
glClearColor(0, 0, 0, 0);
glMatrixMode(GL_PROJECTION);
gluOrtho2D(0, 400, 0, 400);

goal.col = 15;
goal.row = 4;
for (int i = 0; i < 20; i++)
for (int j = 0; j < 20; j++)
{
grid[i][j].row = i;
grid[i][j].col = j;
}
}

void display()
{
glClear(GL_COLOR_BUFFER_BIT);
glColor3f(1.0f, 1.0f, 1.0f);//设置当前的绘图颜色为红色
glRectf(start.col * 20 + 1, 399 - start.row * 20, start.col * 20 + 19, 381 - start.row * 20);//绘制一个矩形
glColor3f(0.7f, 0.7f, 0.7f);//设置当前的绘图颜色为红色
glRectf(goal.col * 20 + 1, 399 - goal.row * 20, goal.col * 20 + 19, 381 - goal.row * 20);//绘制一个矩形
int p[2] = { 0, 0 };
glBegin(GL_LINES);
for (int i = 0; i <= 400; i += 20)
{
p[0] = i;p[1] = 0;
glVertex2iv(p);
p[0] = i;p[1] = 400;
glVertex2iv(p);
p[0] = 0;p[1] = i;
glVertex2iv(p);
p[0] = 400;p[1] = i;
glVertex2iv(p);
}
glEnd();
glFlush();
}

void mouseFunc(int button, int state, int x, int y)
{
if (state == GLUT_DOWN)
{
if (button == GLUT_LEFT_BUTTON)
{
cur.col = x / 20;
cur.row = y / 20;
if (cur.col >= 20) cur.col = 19;
if (cur.row >= 20) cur.row = 19;

if (grid[cur.row][cur.col].free && cur != start && cur != goal)
{
glColor3f(1.0f, 0.0f, 0.0f);//设置当前的绘图颜色为红色
glRectf(cur.col * 20 + 1, 399 - cur.row * 20, cur.col * 20 + 19, 381 - cur.row * 20);//绘制一个矩形
grid[cur.row][cur.col].free = !grid[cur.row][cur.col].free;
}
else if (!grid[cur.row][cur.col].free && cur != start&&cur != goal)
{
glColor3f(0.0f, 0.0f, 0.0f);
glRectf(cur.col * 20 + 1, 399 - cur.row * 20, cur.col * 20 + 19, 381 - cur.row * 20);//绘制一个矩形
grid[cur.row][cur.col].free = !grid[cur.row][cur.col].free;
}
glFlush();
}
else if (button == GLUT_RIGHT_BUTTON)
{
if (grid[y / 20][x / 20].free)
{
glColor3f(0.0f, 0.0f, 0.0f);
if (rightbutton)
{
glRectf(start.col * 20 + 1, 399 - start.row * 20, start.col * 20 + 19, 381 - start.row * 20);
start.col = x / 20;
start.row = y / 20;
glColor3f(1.0f, 1.0f, 1.0f);
glRectf(start.col * 20 + 1, 399 - start.row * 20, start.col * 20 + 19, 381 - start.row * 20);
}
else
{
glRectf(goal.col * 20 + 1, 399 - goal.row * 20, goal.col * 20 + 19, 381 - goal.row * 20);
goal.col = x / 20;
goal.row = y / 20;
glColor3f(0.7f, 0.7f, 0.7f);
glRectf(goal.col * 20 + 1, 399 - goal.row * 20, goal.col * 20 + 19, 381 - goal.row * 20);
}
rightbutton = !rightbutton;
glFlush();
}

}
else if (button == GLUT_MIDDLE_BUTTON)
{
for (int i = 0; i < 20;i++)
for (int j = 0; j < 20; j++)
{
grid[i][j].father = NULL;
grid[i][j].visited = 0;
}

shortestPathByAstart(grid, start, goal);

glColor3f(0.3f, 0.3f, 0.3f);//设置当前的绘图颜色为绿色
for (int i = 0; i < 20; i++)
for (int j = 0; j < 20; j++)
{
if (grid[i][j].visited && !(i == start.row&&j == start.col) && !(i == goal.row&&j == goal.col))
glRectf(grid[i][j].col * 20 + 2, 398 - grid[i][j].row * 20, grid[i][j].col * 20 + 18, 382 - grid[i][j].row * 20);
}

glColor3f(0.0f, 1.0f, 0.0f);//设置当前的绘图颜色为绿色
node *p = grid[goal.row][goal.col].father;
if (p == NULL)
return;
while (p->father != NULL)
{
glRectf(p->col * 20 + 1, 399 - p->row * 20, p->col * 20 + 19, 381 - p->row * 20);//绘制一个矩形
p = p->father;
}

glFlush();
}

glColor3f(0.0f, 0.0f, 0.0f);//把遍历过的、没有障碍的、不是起终点的，变黑
for (int i = 0; i < 20; i++)
for (int j = 0; j < 20; j++)
{
if (grid[i][j].visited && grid[i][j].free && !(i == start.row&&j == start.col) && !(i == goal.row&&j == goal.col))
glRectf(grid[i][j].col * 20 + 1, 399 - grid[i][j].row * 20, grid[i][j].col * 20 + 19, 381 - grid[i][j].row * 20);
}
}

}

int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nShowCmd)
{
glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB);
glutInitWindowSize(400, 400);
glutCreateWindow("A*：左键-障碍 右键-起止 中键-寻路");
glutMouseFunc(&mouseFunc);
init();
glutDisplayFunc(display);
glutMainLoop();
}
0
0

个人资料
• 访问：1324次
• 积分：34
• 等级：
• 排名：千里之外
• 原创：2篇
• 转载：0篇
• 译文：0篇
• 评论：0条
文章分类
文章存档
阅读排行