关闭

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
查看评论

a*自动寻路算法详解

这篇博文是在其他博客基础上加工的,主要原因是感觉原博客举得例子不太好,很多细节感觉没有描述。 A*算法主要是在父节点更新那个地方很容易误解,但是父节点的更新又是A*算法的核心,因为遍历到目标节点之后就是根据父节点回溯返回找到的路径的。 开始: 一只探路猫   让我们想象一下,有一款游戏,...
  • jialeheyeshu
  • jialeheyeshu
  • 2016-11-09 20:47
  • 1101

A*寻路算法浅析

A*算法:A*(A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法。估价值与实际值越接近,估价函数取得就越好。(像这种专业的概念的解释,我们还是交给度娘来做吧)
  • yiyikela
  • yiyikela
  • 2015-05-28 21:59
  • 4776

Unity3D实现A*寻路算法

目录(?)[+] A算法复习实现NodePriorityQueueGridManagerAStarTestCode ClassScene setupTesting总结 这篇文章翻译自Unity 4.x Game AI Programming这本书第七章 在本...
  • IT_faquir
  • IT_faquir
  • 2015-05-05 15:55
  • 2584

A*寻路算法讲解+源码DEMO演示

本文源码下载地址:http://download.csdn.net/detail/sun2043430/5907609(第一版) http://download.csdn.net/detail/sun2043430/5909315(第二版) ...
  • qq_35653247
  • qq_35653247
  • 2016-08-27 14:35
  • 3061

Unity A*寻路三个简单实用的算法

1.怎么判断直线通往目标的路径上有障碍物? 这种情况下,DDA算法比A*更快,如果没有障碍物,那么直接前进即可,不用A*算法,对于coc这种百人寻路是可以节省不少性能的。 代码如下: public bool CheckCanGoForward(Vector3 startPos, Vector3...
  • yxriyin
  • yxriyin
  • 2015-10-13 11:42
  • 3624

A*寻路算法的实现

原理:http://www.cppblog.com/christanxw/archive/2006/04/07/5126.html 算法理论请到原理这个传送门,代码中的注释,已经比较详细,所以我不会讲太多的原理,该文章本身就是以A*的思路,对算法进行一次速度上的优化,用一些更效率的方式来代替算法原...
  • Gnorth
  • Gnorth
  • 2013-06-21 20:01
  • 4105

A*寻路算法的C#实现

详细讲述了A*寻路算法,有下列地方值得学习1. 不要用AStarPathNode来构造PathFinder的Matrix,Matrix仅仅是byte[,],需要对Matrix中某个元素进行处理时再构造相应的AStarPathNode2. 对每个Matrix中的元素取值不仅仅0和1,而是代表其权重,0...
  • kenkao
  • kenkao
  • 2010-04-13 12:34
  • 6007

Unity3d 中的 A*寻路

在本章中,我们将在Unity3D环境中使用C#实现A*算法.尽管有很多其他算法,像Dijkstra算法,但A*算法以其简单性和有效性而广泛的应用于游戏和交互式应用中.我们之前在第一章AI介绍中短暂的涉及到了该算法.不过现在我们从实现的角度来再次复习该算法.
  • aisajiajiao
  • aisajiajiao
  • 2013-12-27 23:54
  • 13405

A*寻路算法入门(一)

大熊猫猪·侯佩原创或翻译作品.欢迎转载,转载请注明出处. 如果觉得写的不好请告诉我,如果觉得不错请多多支持点赞.谢谢! hopy ;) 免责申明:本博客提供的所有翻译文章原稿均来自互联网,仅供学习交流之用,请勿进行商业用途。同时,转载时不要移除本申明。如产生任何纠纷,均与本博客所有...
  • mydo
  • mydo
  • 2015-11-21 20:45
  • 2550

一篇写的比较简单的A*寻路算法(转)

http://www.raywenderlich.com/zh-hans/21503/a%E6%98%9F%E5%AF%BB%E8%B7%AF%E7%AE%97%E6%B3%95%E4%BB%8B%E7%BB%8D 这篇文章还可以在这里找到 英语 If you're ne...
  • u012995513
  • u012995513
  • 2014-02-17 20:08
  • 1537
    个人资料
    • 访问:1324次
    • 积分:34
    • 等级:
    • 排名:千里之外
    • 原创:2篇
    • 转载:0篇
    • 译文:0篇
    • 评论:0条
    文章分类
    文章存档
    阅读排行