A*寻路算法

 A*算法是静态环境下求最短路径的不二之选，由于是启发式搜索，比dijkstra、深搜广搜要快的多啦。
 A*也算是我第一次接触的移动机器人算法，优快云上的科普文章也不少，但我作为一个机械的小白，实现出来还是小有成就感滴。
 今天抽空和大家分享一下源码，开发环境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();
}