A*寻路算法（更新cocos2dx版本）

(由于期间LOL战斗之夜以及一些事情，没来得及填坑A*算法orz，所以今天开始好好学习)
    参考了:http://blog.csdn.net/makecontral/article/details/38311943
     http://blog.csdn.net/zgwangbo/article/details/52078338
  两位博主都写得挺好的，容易理解。
 两篇的区别：①第一篇为理论基础与一些概念，通俗易懂，是博主翻译外国人的文章，翻译得很好，第二篇则是用简洁的语言与图形说明A*算法以及一些关于h值的取法，还有核心的java实现代码。
       ②第一篇所用的是8邻域扩展节点，第二篇则是4邻域。
       ③第一篇blog的有些数据没显示完全..感觉可能是从哪复制的，不过看到后面的步骤，也就了解了这个算法的实现过程。

正文:
   说到A*算法，就不得不说一下BFS(广度优先搜索)，BFS是A*算法的一种特例，在A*算法中，有这样一个函数式子，f=g+h，而BFS则是 h=0的情况，因为A*算法是一种寻路算法，可以理解把f理解为距离，而g为从起始点s到某一点m的距离，而h则为m到终点e的距离，那为什么要这么分呢？因为从要考虑到有障碍物，即不可通过的区域或者是给出一个预测值，达到启发式搜索的目的。
  (启发式搜索：启发式搜索就是在状态空间中的搜索对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无畏的搜索路径，提到了效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果)
常见的计算h的公式有:

1、曼哈顿距离：横向距离+纵向的距离；
2、欧式距离：两点间的直线距离sqrt((x1-x2)^2 + (y1-y2)^2)

  图中的F、G、H的关系就如同上面所说的f=g+h，当我看到这里的时候(第一篇blog)，我是这样想的，用8邻域遍历周边格子，然后把它们都存放起来(删除起始节点），然后选出最小的F值，进行8邻域扩展，然后再把节点都存放起来(若是周边的节点是不可达节点，即是图中的蓝色节点，则不存放，且每次扩展则把扩展孩子节点的父 节点删除)重复操作，则应该大概也许能找到最优的路径。。。

————————————————–第五次更新，更新代码部分(2018.7.4)——————————————————

  （此次更新的第二段代码为完善后的寻路算法，附加推算的过程图，之前没看完文章，按照自己的想法瞎写了很久没找出最短路径，阅读完之后，发现parent指针的重要性，h的计算是用两点之间的距离公式计算，取消了std::priority_queue(优先队列)的使用，使用std::list运用列表，对应的写一些取值和赋值操作，因为后面更新g值的时候要更新f值，更新的时候就要更新队列，但是std优先队列无法自由的取出值，对值进行更新，因为没想到怎么写一个好的方法去更新，只想到了把所有值取出来然后放进去的蠢方法，就放弃使用优先队列，转向选择了list容器）
  在看开头介绍的两篇博客(A*)的时候，对更新g值不太理解，打算自己先写写代码实现以下，寻路的目的是达到了(应该达到了吧)，但是有些节点应该进入队列的却没进，先贴代码，以及结果吧。
代码的思路是按我上面的想法写的，应该算是求曼哈顿距离来寻路，有个小问题就是，我存放路径和显示拜访过路径的地方有问题，可以从下面的结果图看出从起点(4)到终点(5)的过程中，几乎都是1或者说都是1。
  之前的代码未找出最优路径代码(使用std::priority_queue，无法对值更新).

算法的大概步骤：
  1、刚开始，通过开始的节点，通过BFS扩展节点，然后把节点存入列表或者最小堆中。
  2、取出列表中f=g+h值最小的节点，然后将该点从列表或最小堆中删除，然后通过该节点扩展节点。
  3、在扩展的过程中，若扩展节点扩展出来的点已经在列表或最小堆中，判断列表中的g值与扩展出来的节点g值大小，若列表中g值大于该点的g值，则更新列表中节点的g值与f值，并将该节点的parent指针指向扩展节点。
  4、重复2-3步骤操作，直到找到end点或者列表为空的时候，中断程序。

  上面的图是以前以前写的操作台结果，这次更新的是cocos2dx的图像版本，黑点为起始点，红点为终点，运行的时候绿点会慢慢走向红点。

  代码部分：

//HelloWorldScene.h

#ifndef __HELLOWORLD_SCENE_H__
#define __HELLOWORLD_SCENE_H__

#include "cocos2d.h"
#include"proj.win32\route.h"

class HelloWorld : public cocos2d::Scene
{
public:
    static cocos2d::Scene* createScene();

    virtual bool init();
    void update(float dt);
    // a selector callback
    // implement the "static create()" method manually
    CREATE_FUNC(HelloWorld);
public:
    std::stack route;

};

//HelloWorldScene.cpp

#include "HelloWorldScene.h"

USING_NS_CC;

Scene* HelloWorld::createScene()
{
    return HelloWorld::create();
}

// on "init" you need to initialize your instance
bool HelloWorld::init()
{
        int _map[MAP_W][MAP_H] = {
            { 0,4,3,0,0,0 },
            { 0,0,0,0,0,0 },
            { 0,3,3,3,0,3 },
            { 0,0,0,3,0,3 },
            { 3,0,0,0,0,0 },
            { 0,0,3,0,3,0 },
            { 0,0,0,0,0,0 },
            { 0,3,3,5,0,3 },
        };
        std::map<int, Color4B> mode;
        mode[0] = Color4B::WHITE;
        mode[3] = Color4B::BLUE;
        mode[4] = Color4B::BLACK;
        mode[5] = Color4B::RED;
        for (int i = 0; i < MAP_W; i++) {
            for (int j = 0; j < MAP_H; j++) {
                int tmp = _map[i][j];
                LayerColor *layer_block = nullptr;
                layer_block = LayerColor::create(mode[tmp], 128, 128);
                this->addChild(layer_block);
                layer_block->setPosition(i*128, j*128);
            }
        }
        route=Astar(MyVec2(0, 1), MyVec2(7, 3), _map);
        schedule(schedule_selector(HelloWorld::update), 1.0f, kRepeatForever, 0);
    return true;
}

void HelloWorld::update(float dt)
{
    if (!route.empty()) {
        auto tmp = route.top();
        route.pop();
        auto layer_block = LayerColor::create(Color4B::GREEN, 128, 128);
        this->addChild(layer_block);
        layer_block->setPosition(tmp.x * 128, tmp.y * 128);
    }
}

//重载Vec2为A*算法算权值做准备
//route.h
#pragma once
#include
#include
#define Wall 3
#define Visited 2
#define Path 1
#define Space 0
#define Start 4
#define End 5
#define MAP_H 6
#define MAP_W 8

class MyVec2:public cocos2d::Vec2 {
public:
    MyVec2(int xx, int yy) :Vec2(xx, yy) {
        parent = nullptr;
        f = 0, g = 0, h = 0;
    };
    MyVec2(const Vec2 ©) :Vec2(copy) {
        parent = nullptr;
        f = 0, g = 0, h = 0;
    };
    MyVec2(const MyVec2 ©) {
        this->x = copy.x;
        this->y = copy.y;
        parent = copy.parent;
        f = copy.f;
        g = copy.g;
        h = copy.h;
    }
    friend bool operator <(const MyVec2 &p1, const MyVec2 &p2)
    {
        return p1.f > p2.f;
    }
    ~MyVec2() {
        delete parent;
        parent = nullptr;
    }
public:
    MyVec2 *parent;
    double f, g, h;

};

MyVec2* findPoint(std::listlist, int x, int y);
bool clearPoint(std::list&list, MyVec2* target);
MyVec2* getMin(std::listm_list);
std::stack Astar(MyVec2 start, MyVec2 end, int (&map)[MAP_W][MAP_H]);

//route.cpp
//实现A*算法
#include"route.h"

const MyVec2 connects[] = { MyVec2(0,1),MyVec2(1,0),MyVec2(0,-1),MyVec2(-1,0) };

MyVec2* findPoint(std::listlist, int x, int y)
{
    MyVec2 *temp;
    for (auto i : list)
    {
        if (i->x == x&&i->y == y)
        {
            temp = i;
            return temp;
        }
    }
    return nullptr;
}

bool clearPoint(std::list&list, MyVec2* target)
{
    for (auto i : list)
    {
        if (i->x == target->x&&i->y == target->y)
        {
            list.remove(i);
            return true;
        }
    }
    return false;
}

MyVec2* getMin(std::listm_list)
{
    if (!m_list.empty())
    {
        auto res = m_list.front();
        for (auto i : m_list)
        {
            if (res->f > i->f)
                res = i;
        }
        return res;
    }
}

std::stack Astar(MyVec2 start, MyVec2 end, int (&map)[MAP_W][MAP_H])
{
    std::stack route;
    std::listopen_queue;
    bool isarrvie = false;
    MyVec2 *last = &start;
    MyVec2 *m_start = &start;
    open_queue.push_back(m_start);
    while (!open_queue.empty() && isarrvie == false)
    {
        MyVec2* curr = getMin(open_queue);
        clearPoint(open_queue, curr);
        bool is = clearPoint(open_queue, curr);
        if (map[(int)curr->x][(int)curr->y] != Start && map[(int)curr->x][(int)curr->y] == Space)
        {
            map[(int)curr->x][(int)curr->y] = Visited;
        }
        for (auto dir : connects)
        {
            MyVec2 *temp = new MyVec2(curr->x + dir.x, curr->y + dir.y);
            if (temp->x >= 0 && temp->x < MAP_W&&temp->y < MAP_H&&temp->y >= 0)
            {

                if (map[(int)temp->x][(int)temp->y] == Space)
                {
                    temp->parent = curr;
                    temp->g = curr->g + 1;
                    temp->h = sqrt(pow(abs(temp->x - end.x), 2) + pow(abs(temp->y - end.y), 2));
                    temp->f = temp->g + temp->h;
                    open_queue.push_back(temp);
                }
                else if (map[(int)temp->x][(int)temp->y] == End)
                {
                    isarrvie = true;
                    last = curr;
                    break;
                }
                else if (findPoint(open_queue, temp->x, temp->y)!=nullptr)
                {
                    MyVec2* Temp = findPoint(open_queue, temp->x, temp->y);
                    if (Temp->g > curr->g + 1)
                    {
                        Temp->parent = curr;
                        Temp->g = curr->g + 1;
                        Temp->f = Temp->g + Temp->h;
                    }
                }
            }
        }
    }
    while (last->parent != NULL)
    {
        map[(int)last->x][(int)last->y] = Path;
        route.push(MyVec2(last->x, last->y));
        last = last->parent;
    }
    return route;
}