追踪、A*寻路算法

一、追踪算法

(一)基本追踪（坐标追踪）

实现原理：根据要追踪对象的坐标来修改追踪者的坐标，使两者的距离逐渐缩短。当追踪者的x、y坐标分别小于被追踪者的x、y坐标时，追踪者的x、y自增；反之，自减。
追踪者：predator
被追踪者： prey
对于追踪者来说：新位置 = 旧位置 + XY速度;

if(predator.x < prey.x)predator.x++ ;
else if(predator.x > prey.x)predator.x--;if(predator.y < prey.y)predator.y++;
else if(predator.y > prey.y)predator.y--;

(二)视线追踪

视线追踪方式，主要描述每一时刻追踪者都会沿着被追踪者之间的直线方向运动

通过追踪者predator和被追踪者prey的坐标可以求出指向被追踪者prey的方向单位向量dir。

//计算从追踪者到被追踪的方向单位向量
dir = (prey.x-predator.x,prey.y-predator.y).normalized();
//根据方向向量求出追踪者的旋转角度,数学公式得出来的是弧度，要转成角度
angle = Math.Atan2(dir.x,dir.y)*180/Math.PI;
//判断是逆时针还是顺时针旋转
angle = dir.x < 0? angle:-angle;
//调整追踪者当前的位置，xy速度都为speed
predator.x +=dir.x*speed;
predator.y +=dir.y*speed;

(三)拦截追踪

前面两种追踪都是追踪者跟在被追踪者后面，如果追踪者的速度比被追踪者慢，那么将永远追不上。比较合理的方法是在被追踪者前进的路径上的某个点进行拦截

拦截主要考虑的是拦截点的计算

拦截点不是被追踪者路径上离追踪者最近的点
拦截点的计算需要考虑到让追踪者和被追踪者同时到达某一点
为了让追踪者和被追踪者同时到达某一点，必须考虑他们之间的相对速度。对被追踪者的速度需要通过检测它在两帧之间的位移来计算，应为他的速度是实时变化的，所以拦截点也是实时变化的

//计算两帧之间被追踪者的速度Vprey
//计算追踪者与被追踪者之间的速度差（靠拢速度）Vr=Vprey-Vpredator
//计算靠拢距离Sr=Sprey-Spredator
//计算靠拢时间Tc=||Sr||/||Vr||
//计算拦截点St=Sprey+(Vprey)*Tc
//计算追踪者的位移//计算追踪者与拦截点的相对距离S=St-Ppredator//调整追踪者的速度Vpredator=S/Tc//计算追踪者的位移Ppredator+=Vpredator*fDeltaTime

二、A*寻路算法

概述

A*算法：A*（A-Star)算法是一种启发式搜索，一种求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。

代价值计算：F = G+H
F：从起点经过当前点到终点的代价值
G：从起点到当前点的实际代价
H：从当前点到终点的估计代价；
欧几里：两点间的直线距离
曼哈顿：x轴的距离+y轴的距离

步骤

开启列表：存放所有可能要走点
关闭列表：存放走过的点

把起点放入到开启列表中。
采用while循环。判断开启列表是否为空，不为空，进入循环；为空，则搜索失败。
从开启列表中取出F值最小的节点作为当前节点，并放入关闭列表中，从开启列表中移除。
遍历当前节点的周围8个节点，对每个节点
     （1）如果不能走（障碍物、在关闭列表中、越界），什么都不用做。
     （2）如果不在开启列表中，将该节点加入开启列表，计算FGH值，并将该节点作为当前节点。
     （3）如果在开启列表中，计算FG值，与原来的G值进行比较，如果小，更新FG值，将当前节点设为该节点的父节点。
     （4）查找结束点是否在开启列表中，如果在，则路径已找到，根据结束点反推得出路径。

实现

AStar.h

#pragma once
#include <vector>
#include <list>const int kCost1 = 10; //直移一格消耗
const int kCost2 = 14; //斜移一格消耗struct Point
{int x, y; //点坐标，这里为了方便按照C++的数组来计算，x代表横排，y代表竖列int F, G, H; //F=G+HPoint *parent; //parent的坐标，这里没有用指针，从而简化代码Point(int x, int y) :x(x), y(y), F(0), G(0), H(0), parent(NULL)  //变量初始化{}
};//A*算法
class AStar
{
public://初始化A*算法使用的地图void InitAstar(std::vector<std::vector<int>> &_maze);//获取路径：1.开始点 2.结束点 3.斜边移动是否考虑四周有障碍物  isIgnoreCorner:比如上右是障碍物，右斜上能否走？std::list<Point *> GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner);private://查找路径的方法：A*算法的核心逻辑Point *findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner);//获取周围八个点，如果能走或者不在关闭列表中，则放入vector并返回std::vector<Point *> getSurroundPoints(const Point *point, bool isIgnoreCorner) const;//判断某点是否可以用于下一步判断（是否可走）bool isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const; //判断开启/关闭列表中是否包含某点Point *isInList(const std::list<Point *> &list, const Point *point) const;//从开启列表中返回F值最小的节点Point *getLeastFpoint(); //计算FGH值int calcG(Point *temp_start, Point *point);int calcH(Point *point, Point *end);int calcF(Point *point);private:std::vector<std::vector<int>> maze;//地图（maze：迷宫）std::list<Point *> openList;  //开启列表std::list<Point *> closeList; //关闭列表};

AStart.cpp

#include "AStar.h"
#include <math.h>AStar::AStar()
{
}AStar::~AStar()
{
}void AStar::InitAstar(std::vector<std::vector<int>> &_maze)
{maze = _maze;
}int AStar::calcG(Point *temp_start, Point *point)
{//如果是斜边，extraG为14，否则为10int extraG = (abs(point->x - temp_start->x) + abs(point->y - temp_start->y)) == 1 ? kCost1 : kCost2;int parentG = point->parent == NULL ? 0 : point->parent->G; //如果是初始节点，则其父节点是空return parentG + extraG;  //当前点的G值 = 父节点的G值 + 额外的G值
}int AStar::calcH(Point *point, Point *end)
{//欧几里距离：两点之间的直线距离 //return sqrt((double)(end->x - point->x)*(double)(end->x - point->x) + (double)(end->y - point->y)*(double)(end->y - point->y))*kCost1;//曼哈顿距离：水平距离+垂直距离return (abs(end->x - point->x) + abs(end->y - point->y)) * kCost1;
}int AStar::calcF(Point *point)
{//F = G + Hreturn point->G + point->H;
}Point *AStar::getLeastFpoint()
{//将第一个点作为F值最小的点，进行比较，最终拿到F值最小的点//泛型for(元素类型 变量名:容器名)//容器：数组或者具有begin和end的容器//不允许遍历的过程中添加或删除元素//但可以删除或添加后，break;if (!openList.empty()){auto resPoint = openList.front();for (auto &point : openList)if (point->F<resPoint->F)resPoint = point;return resPoint;}return NULL;
}Point *AStar::findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)
{//把起始点添加到开启列表openList.push_back(new Point(startPoint.x, startPoint.y)); //置入起点,拷贝开辟一个节点，内外隔离while (!openList.empty()){auto curPoint = getLeastFpoint(); //找到F值最小的点openList.remove(curPoint); //从开启列表中删除closeList.push_back(curPoint); //放到关闭列表//1,找到当前周围八个格中可以通过的格子（1.可走且没有在关闭列表）auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);for (auto &target : surroundPoints){//2,对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算F G Hif (!isInList(openList, target)){target->parent = curPoint;target->G = calcG(curPoint, target);target->H = calcH(target, &endPoint);target->F = calcF(target);openList.push_back(target);}//3，对某一个格子，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和Felse{int tempG = calcG(curPoint, target);if (tempG<target->G){target->parent = curPoint;target->G = tempG;target->F = calcF(target);}}//查找结束点是否已经在开启列表中,如果在，这时候路径被找到。Point *resPoint = isInList(openList, &endPoint);//如果返回不为空，表示找到终点，则通过终点的parent一直反推得出路径if (resPoint)return resPoint; //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝}}return NULL;//找不到一条路径
}std::list<Point *> AStar::GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)
{Point *result = findPath(startPoint, endPoint, isIgnoreCorner);std::list<Point *> path;//返回路径，如果没找到路径，返回空链表while (result){path.push_front(result);result = result->parent;//通过parent回溯}// 清空临时开闭列表，防止重复执行GetPath导致结果异常openList.clear();closeList.clear();return path;
}Point *AStar::isInList(const std::list<Point *> &list, const Point *point) const
{//判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标for (auto p : list)if (p->x == point->x&&p->y == point->y)return p;return NULL;
}bool AStar::isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const
{if (target->x<0 || target->x>maze.size() - 1|| target->y<0 || target->y>maze[0].size() - 1|| maze[target->x][target->y] == 1|| target->x == point->x&&target->y == point->y|| isInList(closeList, target)) //如果点与当前节点重合、超出地图、是障碍物、或者在关闭列表中，返回falsereturn false;else{if (abs(point->x - target->x) + abs(point->y - target->y) == 1) //非斜角可以return true;else{//斜对角要判断是否绊住if (maze[point->x][target->y] == 0 && maze[target->x][point->y] == 0)return true;elsereturn isIgnoreCorner;}}
}std::vector<Point *> AStar::getSurroundPoints(const Point *point, bool isIgnoreCorner) const
{std::vector<Point *> surroundPoints;for (int x = point->x - 1; x <= point->x + 1; x++)for (int y = point->y - 1; y <= point->y + 1; y++)if (isCanreach(point, new Point(x, y), isIgnoreCorner))//判断是否能走，能走则加入到vector中返回surroundPoints.push_back(new Point(x, y));return surroundPoints;}