unity寻路算法

寻路算法

A*寻路算法

A*寻路就是用来计算玩家或者敌人的行进路径，通过它可以计算除避开阻挡的最短路径

基本原理

开始寻找起点周围相邻的格子中所有不被阻挡的区域，找到后选择距离终点最近的点行走，之后再重复之前的操作，但不会寻找已经找过的点，直至到达终点。

详细原理

寻路消耗公式：

​ f(寻路消耗) = g(离起点的距离) + h(离终点的距离，计算该距离最常用曼哈顿街区算法)

开启列表、关闭列表及格子对象的父对象

​ 开启列表和关闭列表相当于两个容器，开启列表中每一项包含该点，g(离起点的距离)，h(离终点的距离)和该点的父对象，关闭列表中每一项包含该点以及他的父对象。

​ 父对象便是该格子对象的父对象

​ 从a点开始到达蓝色方块，先将a点放入关闭列表中，再去寻找周围8个点（从左到右，从上到下，依次为a1~a8），将可行走区域的点放入开启列表，并计算其g(离起点的距离)的值(计算两点之间的直线距离)，a2,a4,a5,a7为1加上父对象的g值，其余的为根号2约为1.4加上父对象的g值，使用曼哈顿街区算法计算出每个点的h(离终点的距离)值，由此可以得出每个点的f(寻路消耗)值，将找出的最优解的点移到关闭列表中。

​ 之后从该点(可将其重命名)开始继续寻找周围的点，重复操作，选出最优值放入关闭列表中，每次选择最优解时要从开启列表中所有的点中选择（每次从新的点找周围的点时，如果周围的点已经再开启列表或者关闭列表中了，就不会再去管）。

​ 每次关闭列表中放点时，应该判断这个点是不是终点，如果是证明路径已经找完，如果不是，则继续找。

​ 经过操作到达终点后两个容器中内容如下(c点即b1点，b点即a5点，省略最后一步d点的相关计算)：

最终路径的确定

​ 从选中的最后一个点开始，一层一层向上寻找父对象(两个列表中都存储的有),一直找到某个点没有父对象时，该点便为起点，而这条路径便为最终确认的路径。ps：不能直接选择关闭列表中所存储的点来确认路径。

​ 以之前的放格图为例，假设终点左边一列除了最下面的格子其余全为阻挡，这时，到第三步时选择的则是c2点，之后在进行两次计算后，发现b2点位最优点，但这时c2点及之后一次计算的点已被放在关闭列表中，但很显然这两个点不是最终路径上要经过的点，所以这就是为什么不能直接选择关闭列表中的点当作最终路径的原因，这也是为什么每次计算不清空开启列表的原因。

判断死路

​ 当遇到某个点周围没有未计算的可行走的区域时，便可判断起点到终点并没有可以到达的路径，便为死路。（此为粗略解释，详细细节会在之后代码处解释）

代码实现

需求分析

代码

#include<iostream>
#include<vector>
#include<random>
using namespace std;

struct Vector2
{
	float x;
	float y;
	Vector2() : x(0), y(0) {}
	Vector2(float _x, float _y) : x(_x), y(_y) {}
};

//格子类
class AStarNode {
public:
	//坐标
	int x;
	int y;
	//寻路消耗
	float f;
	//离起点的距离
	float g;
	//离终点的距离
	float h;
	//格子类型(true为可行走区域，false为阻挡区域)
	bool isAllow;
	//父对象
	AStarNode* father;

	AStarNode(float _x, float _y, bool _isAllow) : x(_x), y(_y), isAllow(_isAllow), f(0), g(0), h(0), father(nullptr) {}
		
};

bool compare(const AStarNode* a, const AStarNode* b) {
	return a->f > b->f;
}

//管理类
class AStarMgr {
private:
	//地图宽高
	int mapW;
	int mapH;
	//地图相关的格子对象容器
	vector<vector<AStarNode*>> map;
	//开启列表
	vector<AStarNode*> openList;
	//关闭列表
	vector<AStarNode*> closeList;

	static AStarMgr* instance;

	AStarMgr() : mapW(0), mapH(0) {};

	AStarMgr(const AStarMgr&) = delete; // 禁止拷贝  

	AStarMgr& operator=(const AStarMgr&) = delete; // 禁止赋值
	
	/// <summary>
	/// 判断点是否在开启列表或者关闭列表中
	/// </summary>
	/// <param name="node"></param>
	/// <returns></returns>
	bool isInOpenListOrCloseList(AStarNode*& node){
		for (const AStarNode* n : openList) {
			if (node == n)
				return true;
		}
		for (const AStarNode* n : closeList) {
			if (node == n)
				return true;
		}
		return false;
	}

	/// <summary>
	/// 把临近的点放入开启列表中
	/// </summary>
	/// <param name="x"></param>
	/// <param name="y"></param>
	void FindNearLyNodeToOpenList(int x, int y, float g, AStarNode*& father, AStarNode*& end) {
		//边界判断
		if (x < 0 || x >= mapW ||
			y < 0 || y >= mapH)
			return;
		//取点
		AStarNode* node = map[y][x];
		//判断点是否时边界，是否时阻挡，是否再开启或者关闭列表中，如果不是，才放入开启列表
		if (node == nullptr ||
			!node->isAllow ||
			isInOpenListOrCloseList(node))
			return;
		//计算f值 f = g + h
		//记录父节点
		node->father = father;
		//计算g
		node->g = g + father->g;
		node->h = abs(end->x - node->x) + abs(end->y - node->y);
		node->f = node->g + node->h;

		openList.push_back(node);
	}

public:
	static AStarMgr* GetInstance() {
		if (instance == nullptr)
			instance = new AStarMgr();
		return instance;
	}

	//输出地图
	void printMap() {
		cout << "横向为x轴，纵向为y轴“.”为可行走区域，“*”为阻挡区域" << endl;
		for (int i = 0; i < mapH; ++i) {
			for (int j = 0; j < mapW; ++j) {
				cout << (map[i][j]->isAllow ? "." : "*");
				cout << " ";
			}
			cout << endl;
		}
	}

	/// <summary>
	/// 初始化地图
	/// </summary>
	/// <param name="w"></param>
	/// <param name="h"></param>
	void InitMap(int w, int h) {

		mapW = w;
		mapH = h;

		map.resize(h, vector<AStarNode*>(w, nullptr));

		//创建随机数引擎
		default_random_engine e;
		//创建均匀分布
		uniform_int_distribution<int> u(0, 100);
		//使用当前时间为种子
		e.seed(time(0));

		for (int i = 0; i < h; ++i) {
			for (int j = 0; j < w; ++j) {
				AStarNode* node = new AStarNode(j, i, u(e) >= 20);
				map[i][j] = node;
			}
		}
	}

	/// <summary>
	/// 寻找方法
	/// </summary>
	/// <param name="startPos"></param>
	/// <param name="endPos"></param>
	/// <returns></returns>
	vector<AStarNode*> FindPath( Vector2 &startPos, Vector2 &endPos) {
		
		/*
			在实际的项目应用中，传入进来的坐标并非严格按照方位置和个数来计算，
			需要进行一定的换算，将传入进来的坐标转换为咱们所需要的坐标方式
		*/

		//判断两点是否在地图范围内
		if (startPos.x < 0 || startPos.x >= mapW ||
			startPos.y < 0 || startPos.y >= mapH ||
			endPos.x < 0 || endPos.x >= mapW ||
			endPos.y < 0 || endPos.y >= mapH) {
			cout << "输入的两个点不在地图范围内" << endl;
			return {};
		}
			
		//是否是阻挡区域
		AStarNode* start = map[(int)startPos.y][(int)startPos.x];
		AStarNode* end = map[(int)endPos.y][(int)endPos.x];
		if (!start->isAllow || !end->isAllow) {
			cout << "开始或者结束点时阻挡区域" << endl;
			return {};
		}

		openList.clear();
		closeList.clear();

		//初始化起点的值，防止上一次寻路影响这次的寻路
		start->father = nullptr;
		start->f = 0;
		start->g = 0;
		start->h = 0;
		closeList.push_back(start);

		//定义比较器
		auto myCompare = [](AStarNode* a, AStarNode* b) {
			return a->f < b->f;
		};

		//开始寻路
		while (true) {
			//从起点开始找周围的点并放入开启列表中
			//左上
			FindNearLyNodeToOpenList(start->x - 1, start->y - 1, 1.4f, start, end);
			//上
			FindNearLyNodeToOpenList(start->x, start->y - 1, 1.0f, start, end);
			//右上
			FindNearLyNodeToOpenList(start->x + 1, start->y - 1, 1.4f, start, end);
			//左
			FindNearLyNodeToOpenList(start->x - 1, start->y, 1.0f, start, end);
			//右
			FindNearLyNodeToOpenList(start->x + 1, start->y, 1.0f, start, end);
			//左下
			FindNearLyNodeToOpenList(start->x - 1, start->y + 1, 1.4f, start, end);
			//下
			FindNearLyNodeToOpenList(start->x, start->y + 1, 1.0f, start, end);
			//右下
			FindNearLyNodeToOpenList(start->x + 1, start->y + 1, 1.4f, start, end);

			//死路判断
			if (openList.empty()){
				cout << "死路，无法到达终点" << endl;
				return {};
			}

			//选出开启列表中寻路消耗最小的点
			stable_sort(openList.begin(), openList.end(), compare);

			//放入关闭列表中，让那后再从开启列表中移除
			closeList.push_back(openList.back());

			start = openList.back();

			openList.pop_back();

			//判断当前的点是否时终点
			if (start == end)
			{
				vector<AStarNode*> path;
				path.push_back(end);
				while (end->father != nullptr) {
					path.push_back(end->father);
					end = end->father;
				}
				return path;
			}
		}
	}
};
AStarMgr* AStarMgr::instance = nullptr;

int main() {
	AStarMgr* mgr = AStarMgr::GetInstance();
	int w, h, x = 0, y = 0;
	cout << "请输入需要构建的地图大小（第一个整数时宽，第二个整数为高）：" << endl;
	cin >> w >> h;
	mgr->InitMap(w, h);
	mgr->printMap();
	cout << "输入起点坐标" << endl;
	cin >> x >> y;
	Vector2 startPos(x, y);
	cout << "输入终点坐标" << endl;
	cin >> x >> y;
	Vector2 endPos(x, y);
	vector<AStarNode*> path = mgr->FindPath(startPos, endPos);
	if (!path.empty()) {
		cout << "最终路径为：" << endl;
		for (int i = 0; i < path.size(); ++i) {
			cout << "(" << path[i]->x << "," << path[i]->y << ")" << "\t";
		}
		cout << endl;
	}
	return 0;
}

死路判断，当openlist为空，即找不到未走过的路时便可得出无法到达终点。

算法优化

障碍邻点的预计算。

​ 判断周围格子是否有障碍还有计算格子的cost价值，如果在每次寻路的时候才去算就太浪费了，因为每次寻路的时候每个格子相邻的障碍是不会变的，价值cost也不变，我们大可以把这块计算抽出放到初始地图的时候计算。步骤如下:

​ 1.给节点添加邻节点数组nodeLinks，和邻节点价值组costLinks。(也可建一个linkNode对象，对象里面有节点和价值两属性，反正两数组是个映对关系),保证nodeLinks[i]的价值是costLinks[i]

​ 2.地图初始好的时候遍历每个格子，计算出其周围的所有非障碍的格子，并加入节点邻数组中存起。计算每个邻节点的价值，映对存入节点价值组。

​ 3.寻路的时候判断节点四周有哪些格子直接从当前计算的节点的nodeLinks属性取即可，不需要再计算。

预计算可节省很多性能，不过缺点是初始的时候很慢。

Array数组优化

​ Array的indexOf和shift方法是很吃性能的，很多人的A星在while里面有这两个东西，这是那几百毫秒慢下的原因。怀疑的同学可以自己写个1万次的大循环调用这两个方法看看。要判断节点是否在开户组里面，普通人的做法是用indexOf，其实这个方式可以换成以下处理

1.给节点添加isOpen:Boolean属性

2.每次push节点到打开数组的里面设节点的isOpen为true

3.节点移出打开列表时，将节点的isOpen属性设为false

4.判断节点是否在开启列表时只需要判断isOpen即可

判断关闭同理，另外还有会用上indexOf的地方是二叉堆里面，干掉方式略。

​ 将关闭组添加到通过父节点添加到返回路径时，很多人会用shift，把节点一个个添到path数组里面，这也是个慢的因素。可以换成push方式，push不会改变索引，所以效率很高。将关闭节点组数据通过push加入路径后，再调用reverse把数组倒置，结果跟用shift一样，不过用时可大不一样。

打开关闭标记，换掉不用重置

​ 用isOpen来替代indexOf大大提升了一步性能，可是每次寻路完之后会有个问题，那就是要把节点重置，就是把所有打开和关闭列表里面的节点的isOpen,isClose设false。呵呵，虽然这个重置吃的性能相当小，不过本着追求精神，我又找出了更变态的方式。步骤如下

1.每次A星计算设一唯一自增标记markIndex,每计算一次寻路，这个整型属性+1

2.把节点里面的isOpen和isClose布尔属性改成openMark,closeMark整型属性。

3.加入列表时把openmark设为本次计算的自动标记markIndex，移出列表设为-1（或随便一个不等于markIndex的数)

4.判断是否在打开列表中只需判断if(openMark==markIndex)即可

5.某次寻路计算完之后markIndex+1

​ 因为自增标记每次计算都不一样，所以节点的openMark不需要重置，下次也能继续用。这个方式带来的性能提升很微小不多，不追求那几十毫秒的大可无视。

减少while里面get/set/function

​ get/set的性能其实很高，不过在项目中，大量的格子一经过while方法就是数万次的运算，虽然每次性能相差很少，但量一大起来效率就明显了。不信的人可以写个大循环在里面用getter和public属性的方式对比一下，性能相差三四倍。如果节点的f,g,h,x,y这些属性用了接口get/set，那个运算毫秒相差就很明显了。function 是代码设计不可省的东西，这一步优化只适合在A星的while中用，正常项目中可省不得。

位运算，再提一提微小的性能

​ 在二叉堆中的除以2，num/2==num0.5==num >> 1，这三个种计算方式最快的明显是第三种，虽然可读性差些，不过效率还是有点提升滴。另外num2 == num<<1;也可提升些性能

优化前5条原文章：http://www.cnblogs.com/pelephone/archive/2012/09/27/2704902.html

使用二叉堆

​ 在寻路过程中，因为每次都要从开启列表中取出消耗最小的点来进行下一步操作，一般情况下为了方便快速取到需要的点，所以每次都会进行排序，在每次放入新点时，都会与列表中的各个点的消耗值进行比较，一变把其放在合适的位置，但是随着地图增大，每次插入新的点时，所消耗的时间会随着增长，所以为此而引入二叉堆，即将开启列表中的元素以堆的形式存放，根节点方消耗最小的点。

​ 使用二叉堆存放时，一般可以使用以为数组进行储存(节点从下标为1开始而非从0开始)，只需要节点上移和下移操作保证每个节点的f值大于等于两个子节点的f值，小于等于其父节点的f值就可以了，并不需要保证这个一维数组严格排序，这时每次进行插入操作时所需要的比较次数就会减少，而且其与正常存储之间的差距会随着实际地图大小（地图节点的多少）的增大，而随几何倍的增长，但是若是时间地图很小，那么使用二叉堆和不适用二叉堆的差距就不会很大，所以一般地图不是很大的时候，并不会去使用二叉堆去优化。

向堆中添加元素

void addPointReszie(){
    int last = openList.size() - 1;
    
    while(last > 1){
        int half = last >> 1;
        if(openList[last].f >= openList[half].f)
            break;
        AStarNode* temp = openList[Last];
        openList[last] = openList[half];
        openList[half] = temp;
        last >>= 1;
    }
}

从队中删除元素

void removePointResize(){
    int last = openList.size - 1;
    openList[1] = openList[last];
    openlist.pop_back();
    last = openList.size - 1;
    int head = 1;
    while((head << 1) + 1 <= last){
        int child1 = head << 1, child2 = child1 + 1;
        int childMin = openList[child1].f < openList[child2].f ? child1 : child2;
        if(openList[head].f <= openList[childMin].f)
            break;
        AStarNode* temp = openList[head];
        openList[head] = openList[childMin];
        openList[childMin] = temp;
        head = childMin;
    }
}

对开启列表的元素重排

参数i为所需要重排的元素所在数组下标位置

void reSetPointResize(int i){
    int last = i;
    while(last > 1){
        int half = last >> 1;
        if(openList[last].f >= openList[half].f)
            break;
        AStarNode* temp = openList[Last];
        openList[last] = openList[half];
        openList[half] = temp;
        last >>= 1;
    }
}

NavMesh导航系统

Recast Navigation源码分析：(https://zhuanlan.zhihu.com/p/484520809)

​ NavMesh所依据的库是Recast Navigation，NavMesh来进行计算，生成寻路所需要的信息，最后由DetailMesh执行寻路操作。

运行基本逻辑图

​ NavMesh是根据碰撞器来进行输入，由游戏引擎将场景里的碰撞器集聚成一个Mesh，输出给Recast，而Recast是将以三角形集合形式表示的空间场景转化为可供使用的导航数据(navmesh)，detour部分是根据recast生成的navmesh，为指定源点和终点生成直线段路径。

Meadow mapping

​ 在看Recast Navigation前，可以先了解一下最早的凸多边形化算法Meadow mapping，最早是由Ronald C. Arkin 在1986提出，提出了使用凸多边形构建可行走区域。并使用A*在其中寻路。从可行走区域，挖去遮挡物所占区域，输出由凸多边形组成的2D Footprint。

整体需要实现的操作

根据以上Roadmap，实现对应gongneng。

如何划分凸多边形区域

​ 如果一个区域已经是凸多边形，则直接结束算法，若不是，则选择多边形内部一个角度大于180度的点，尝试和多变内部另一个点连接起来，把这个角给干掉，此时得到两个新的区域，然后不断重复进行这个操作，直至多少有多边形凸多边形化。

如何判断是否为凸多边形

​ 检查多边形内部是否由大于180度的角，没有便是凸多边形，此时问题便变为如何判断角度大于180度。

​ 假设，我们所要计算的凸多边形给的这个节点的书讯是以你是正的顺序给的

​ 例如图右边的例子，若x+在向量x-x的右边时，则x这个角大于180度，若是在左边则这个角度小于180度。

​ 判断是否在左边可以使用叉乘计算判断：右手定则。如果叉乘结果是正的，它就是在左边，为负就是在右边。

bool left(Vector2& x, Vector2& y, Vector2& z){
    //Vecto2是一个结构体，里面包含x,y两个float类型，代表坐标
    //为了简便向量也用Vector2表示
    Vector2 xy((y.x - x.x), (y.y - x.y));
    Vector2 yz((z.x - y.x), (z.y - x.y));
    return (xy.x * yz.y - xy.y * yz.y) > 0;
}

​ 遍历多边形所有节点，如果有concave节点(角度大于180度)就返回其坐标，否则就返回-1(为convex)。

int FindConcaveVertex(vector<Vector2*> &verts, vector<int> &indices){
    int n = indices.size()
   	for(int i = 0; i < n; ++i){
        int iaPrev = (i - 1 >= 0) ? i - 1 : n - 1;
        int iaNext = (i + 1 < n) ? i + 1 : 0;
        if(!leftOn(verts[indices[iaPrev]], verts[indices[i], verts[indices[iaNext]]))
            return i;
    }
    return -1;
}

verts用于存储多边形的各个点，indices用于存储多边形点在verts里的位置，即索引

将返回的顶点与多边形中其它“可见”的顶点相连

​ 以此图中a点为例，其中ac，ad，ae这三条线在多边形内部，所以c，d，e为“可见”的顶点，因为ag在多边形外面，af跨越了多边形，所以g，f点不是“可见”顶点。只有当这条线不和非相邻的边交叉并且在多边形内部才是符合条件的。

检测不和非临边相交

​ 可以继续使用点在线段哪侧来判断，若两条线段ab,cd其中a点和b点分别在cd线段两侧，而且c点和d点同时也分别在ab线段两侧，此时可以说这两条线段是相交的。

bool intersect(Vector2 &a, Vector2 &b, Vector2 &c, Vector2 &d){
    bool cdCross = ((left(a, b, c) && !left(a, b, d)) ||
                    (!left(a, b, c) && left(a, b, d)));
 	bool abCross = ((left(c, d, a) && !left(c, d, b)) ||
					(!left(c, d, a) && left(c, d, b)));
    return (cdCross && abCross);
}

​ 改进：如果出现共线情况下的解决方法

​ 假设a，b，c共线，那么当且仅当c在a，b当中时，为共线。

改进后代码在判断是否点是否在线段两侧前，向判断其中三点是否共线。

bool intersect(Vector2 &a, Vector2 &b, Vector2 &c, Vector2 &d){
    
    if(collinear(a, b, c))
        return between(a, b, c);
    if(collinear(a, b, d))
        return between(a, b, d);
    if(collinear(c, d, a))
        return between(c, d, a);
    if(collinear(c, d, b))
        return between(c, d, b);
    
    bool cdCross = ((left(a, b, c) && !left(a, b, d)) ||
                    (!left(a, b, c) && left(a, b, d)));
 	bool abCross = ((left(c, d, a) && !left(c, d, b)) ||
					(!left(c, d, a) && left(c, d, b)));
    return (cdCross && abCross);
}

​ 其中collinear用于判断三点是否共线，可以用向量cb和ba的叉乘是否很小接近0来判断三点是否共线，between可以直接使用左边来进行计算。

bool diagonalie(vector<Vector2*> &verts, vector<int> &indices, int ia, int ib){
    n = indices.size();
    for(int i = 0: i < n; ++i){
        if(indices[i] == indices[ia] or indices[i] == indices[ib])
            continue;
        int nextI = (i + 1) % n;
        if(indices[nextI] = indices[ia] or indices[nextI] == indices[ib])
            continue;
        if(intersect(verts[indices[ia]], verts[indices[ib]], verts[indices[i]], verts[indices[nextI]]))
            return false;
    }
    return true;
}

​ 以上代码为判断多边形中的两点连成的线段是否与其非相邻线段相交。

检测线段是否在多边形内部

​ 可以将其转换为检测对角线是否在多边形内部

bool inCone(vector<Vector2*> &verts, vector<int> &indices, int ia, int ib){
    n = indices.size();
    iaPrev = (ia - 1 >= 0) ? ia - 1 : n - 1;
    iaNext = (ia + 1 >= 0) ? ia + 1 : 0;
    
    ia = indices[id];
    ib = indices[ib];
    iaPrev = indices[iaPrev];
    iaNext = indices[iaNext];
    
    if(leftOn(verts[iaPrev], verts[ia], verts[iaNext]))
        return (left(verts[ia], verts[ib], verts[iaPrev]) &&
               left(verts[ib], verts[ia], verts[iaNext]));
    return !(leftOn(verts[ia], verts[ib], verts[iaNext]) &&
            leftOn(verts[ib], verts[ia], verts[iaPrev]));
}

diagonal的实现

bool diagonal(vector<Vector2*> &verts, vector<int> &indices, int ia, int ib){
    return (inCone(verts, indices, ia, ib) && 
           inCone(verts, indices, ib, ia) &&
           diagonalie(verts, indices, ia, ib));
}

​ 在开销方面因为diagonalie中判断与非临边是否相交需要遍历多边形多有的边，所以

一般会选择a和b中选择，这样若是该边不在多边形内部就不用去计算相交问题，虽然此处代码写的是a的类型，但在Recast Navigation中并不需要堆再进行第二个的inCone的计算。

​ 最后convexify的实现就是再使用二分法来实现

输入：可以站立的区域，阻挡物位置
初始化：将阻挡物和边界合并，放入行走区域中

如果 区域是凸(convex)的
    结束
不然 找到一个凹(concave, >180)的角，将其连接到可见顶点。对新的两块区域继续递归调用当前算法。

合并洞

​ 当处理带hole的多边形时，可将hole上一点连接到多边形上一个可见的点，即可将空洞并入到多边形上，如下图所示：

​ 进行缝合之后，便可以得到一个简单的多边形，之后便可进行convexify操作。

​ 在Recast Navigation中，rcBuildPolyMesh使用的时耳切法(Ear Clipping)的算法去处理，死路与之相似。

​ 耳切法使用更加聪明的方式，时每个切出来的区域都是单独的三角形，避免了Meadow Mapping递归调用的额外开销，不过在之后recast会将这些三角形重新组装成凸多边形，避免有太多的三角形导致之后寻路的开销过大。

体素化

为什么要体素化

​ 在很多的材料中，体素化皆在处理Polygon Soup(多边形大乱炖)问题。在面对若干Mesh时，体素化能够将多个Mesh同一成一种表达。体素化一般在Recast Navigation Build的过程中占据耗时量的80%~90%，所以怎么去优化体素化是一个关键问题。

​ 体素化对于一些有场景破坏的场景有着更好的性能，而且在Navmesh构建过程中体素化也是一个非常重要的步骤。

体素化的实现

​ 切割三角面，算法思路

​ 1）遍历所有碰撞每个三角面

​ 2）沿水平面垂直(y轴)切分（以Cell Size切分）

​ 3）沿水平面水平(x轴)切分（以Cell Size切分）

​ 4）将切分下来的多边形体素化（水品面切分后能保证在一个Cell里，这时根据多边形z值最大最小值，覆盖所有经过的Voxel）。

static void dividePoly(
	const float* in, int nin, 输入多边形和节点个数
	float* out1, int* nout1, 输出多边形1和节点个数
	float* out2, int* nout2, 输出多边形2和节点个数
	float x, int axis
)	x为切的具体坐标
	axis为在第几个维度（x轴为0，y轴为1，z轴为2）去切分多边形

​ dividePoly核心思路：逆时针遍历节点

​ 1）根据节点在axis左右决定加入哪个多边形

​ 2）根据之前节点在axis左右，决定是否要添加新增节点（由于edge被线切分导致）

以下图为例来说明

第一步
1）发现xi处在切分线左边，加入out1多边形
2）发现xi-1处在切分先右边，xi和xi-1不在同侧。往两个多边形都插入新增节点（根据切分线位置插值）。

第二步
1）发现xi处在切分先左边，加入out1多边形
2）发现xi-1处在切分先右边，xi和xi-1在同侧，此时，什么都不做。

​ 之后依次循环遍历每个节点，根据节点所在分割线位置，来将点分在不同的子多边形。

​ 遍历完成之后，便可将原来的多边形分为以上图片所示的两个小多边形。

static void dividePoly(const float* in, int nin, 
						float* out1, int* nout1, 
						float* out2, int* nout2, 
						float x, int axis){
    float d[12];
    for(int i = 0; i < nin; ++i)
        d[i] = x - in[i * 3 + axis];
    
    int m = 0, n = 0;
    for(int i = 0, j = nin - 1; i < nin; j = i, ++i){
        bool ina = d[j] >= 0;
        bool inb = d[i] >= 0;
        if(ina != inb){
            float s = d[j] / (d[j] - d[i]);
            out1[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s;
            out1[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s;
            out1[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s;
        	//将分割线与线段相交的点依次添加到两个凸多边形out1和out2中
            rcVcopy(out2 + n*3, out1 + m*3);
            ++m;
            ++n;
            //判断端点i在分割线的下边或者左边，则将i放入out1中，否则加入out2中
            if(d[i] > 0){
            	rcVcopy(out1 + m*3, in + i*3);
                ++m;
            }else if(d[i] < 0){
                rcVcopy(out2 + m*3, in + i*3);
                ++n;
            }
        }else{
            if(d[i] >= 0){
                rcVcopy(out1 + m*3, in + i*3);
                ++m;
                //若点是在分割线的下边或者左边，则不需要往多边形out2中添加
                if(d[i] != 0)
                    continue;
            }
            rcVcopy(out2 + n*3, in + i*3);
            ++n;
        }
    }
    *nout1 = m;
    *nout2 = n;
}

​ 由dividePoly切分出来的多边形水平上处在一个Cell当中，我们可以确定水平上(x,y)的坐标，将其树枝上z轴接触的区域全部标记为“占有”。例如下图：

​ 该多边形接触到了三个体素（可以根据z轴的值去判断），所以将这三个体素去不标记为“占有”。

​ 为什么说不使用跟简单的光栅化流水线+插值的方式，因为在使用该方法时会导致体素不连续，这对于台阶等这类斜面的徐璐是致命的。

体素化的表达

Recast Idea：使用Run-Length encoding压缩体素空间

下面是一个Column下的表达（实际的体素化空间由水品面上2D列阵的Column表示）

三种存储各有优劣：

​ DenseArray的表达方式虽然消耗的内存较大，但是我们在场景中新增一个体素或者碰撞体的时候，只需要花费O(1)的时间就可完成，而且对于要之多一个爬墙的多做来说，也会跟容易的找到那些是可行走的区域。

​ SolidHeightField的表达（以链表的形式）会对与一大片连接在一起的被“占用”的体素进行一个合并存储，这样对于内存的消耗会有一个很大的节省，在对于一个动态建筑（包含建造玩法），去将新的体素化时标胶好做（相对于第三种存储的方式）。但是当遇到需要查询时，则会消耗比DenseArray存储的查询更多的时间（根据坐标去查询是否被占有，需要遍历整个链表）。

​ CompactHeightField的表达，因为存储的为可以走区域，所以对于查询可行走区域时有着很好优势，但是对于增删改确实很难进行。

Recast Navigation

​ Recast Navigation分为recast和detour两个部分，由recast输出导航数据，由detour接受经处理生成可供寻路的路径。

recast

recast的输入

cellSize————x、z方向上的体素精度

walkableSlopeAngle————agent的可行走最大坡度

walkableHeight————agent的可行走的最小高度

walkableClimb————agent的可攀爬高度

walkableRadius————agent的行走半径

float* bmin和float* bmax————场景的AABB包围盒

int* tris数组————场景的三角形序列

ntris————场景的三角形个数

float* verts————场景三角形各个顶点的坐标

nverts————场景三角形的顶点总数

​ 若一个场景由3个三角形和5个顶点组成，则5个顶点的坐标由float* verts[3 * nverts]存储，分别表示nverts(5)个顶的x，y，z坐标，3个三角形则由float* tris[3 * ntris]存储，分别表示ntris(3)个三角形的3*ntris个顶点在verts数组中的下标。在这个例子中verts数组中的内容是[x1,y1,z1,x2,y2,z2,x3,y3,z3,x4,y4,z4,x5,y5,z5]，而tris[3 * ntris]数组的内容是[0,1,2,1,2,4,2,3,4]。

标记可行走的三角形

​ 如上图，三角形的坡度较为Φ，我们利用向量V2V1和向量V2V3的叉乘得到三角形的法向量，对法向量归一化成单位法向量，那么此时单位向量的Y轴坐标便是坡度较的余弦值，法向量的Y坐标大于最大可行走坡度较的余弦值时说明该三角形时可行走的。

void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAnle,
                            const float* verts, int nv,
                            const int* tris, int nt,
                            unsigned char* areas){
    rcIgnoreUnised(ctx);
    rcIgnoreUnused(nv);
    
    const float walkableThr = cosf(waklableSlopAngle/180.0f * RC_PI);
    
    float norm[3];
    for(int i = 0; i < nt; ++i){
        const int* tri = &tris[i*3];
        calcTrNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
        if(norm[1] > walkableThr)
            areas[i] = RC_WALKABLE_AREA;
    }
}

构建高度场HeightField

​ 该表达方式已在前面介绍锅，此处不在介绍。

​ 高度场对象结构体和实心柱结构体即体素：

//实心高度场对象
struct rcHeightField{
    rcHeightFileld();
    ~rcHeightFileld();
    
    int width; //高度场的宽度（沿x轴，以单元为单位）
    int height; //高度场的高度（沿z轴，以单元为单位）
    float bmin[3]; //世界空间中的最小边界。[(x, y, z)]
    float bmax[3]; //世界空间中的最大边界。[(x, y, z)]
    float cs; //每个单元格的大小。（在xz平面）
    float ch; //每个单元格的高度。（沿y轴的最小增量）
    rcSpan** spans; //实心柱的链表结构（大小时width*height）
    rcSpanPool* pools; //链表的对象池
    rcSpan* freelist; //下一个free span
    
private:
    //禁止外部调用拷贝构造和赋值操作
    rcHeightField(const rcHightField&);
    rcHeightField& operator=(const rcHeightField&);
    
}

//实心柱对象
struct rcSpan{
    unsigned int smin : RC_SPAN_HEIGHT_BITS; //span的下限。[Limit: < #smax]
    unsigned int smax : RC_SPAN_HEIGHT_BITS; //span的上限。[Limit : <= #RC_SPAN_MAX_HEIGHT]
    unsigned int area : 6; // 当前三角面的可行走标记
    rcSpan* next;
}

​ 对于一个体素插入到链表数组里的哪个链表中，是由x + z * width的值来决定的，这个值代表所插入链表在数组中的下标。比如x=1，z=1出体素就查到下标为3的链表中。如果待插入体素的y坐标范围与链表中已有体素的y坐标范围由重合，则需要做体素合并。

//高度场构建的核心代码
//hf为当前高度场，cur为当前span，s为新的spans
//插入并合并spans
while(cur){
    if(cur->smin > s->smax){
        // 当前的span比新的span更远
        break;
    }else if(cur->smax < s->smin){
        //当前的span在新的span的之前
        prev = cur;
        cur = cur->next;
    }else{
        //合并spans
        if(cur->smin < s->smin)
            s->min = cur->smin;
        if(cur->smax > s->smax)
            s->smax = cur->smax;
        
        if(rcABs((int)s->smax - (int)cur->smax) <= flagMergeThr)
            s->area = rcMax(s->area, cur->area);
        
        rcSpan* next = cur->next;
        freeSpan(hf, cur);
        if(prev)
            prev->next = next;
        else
            hf.spans[idx] = next;
        cur = next;
    }
}
//插入新的spans
if(prev){
    s->next = prev->next;
    prev->next = s;
}else{
    s->next = hf.spans[idx];
    hf.spans[idx] = s;
}

高度场的可行走标记修正

定义：

walkableHeight ———— actor行走所需要的最小垂直高度。

walkableClimb ———— actor所能攀爬的最大垂直高度。

第一种情况，如下图：

如果体素A是可行走的，并且height < walkahleClimb，则体素B也是可行走的。

第二种情况，如下图：

定义邻居体素可达的条件为：min(top, ntop) - max(bot, nbot) > walkableHeight。则对于某个体素，其所有可达的邻居体素中：

1.如果存在(bot - nbot > walkableClimb)，则将该体素修正为不可行走。

2.如果max(nbot) - min(nbot) > walkableClimb，则将该体素修正为不可行走。

第三种情况：当两个上下排列的体素之间的间隔height < walkableHeight， 则体素A需要修正为不可走。

构建紧凑高度场CompactHeightfield(反体素化)

遍历之前的高度场数据，将体素信息转为反体素，反体素的y=体素的上沿y坐标，反体素的h=(链表下一个体素的下沿y坐标或者最大y坐标-该体素的上沿y坐标)，不可行走的体素不用转换为反体素，

某个地点(x、z坐标)处的；体素被访问时，首先计算值(x + z * width)，用这个值去元信息数组中访问rcCompactCell数据。元信息数据中的index代表该处位置的反体素在反体素数组中的开始下标，count字段表示该地点(x、z坐标)有几层反体素

struct rcCompactCell{
  	unsigned int index : 24;
    unsigned int count : 8;
};

计算反体素的连通性：修正那一小段的第二种情况的图为例，若这两个反体素想要联通，需要满足两个条件。

1.2个反体素的y坐标差值要小于等于agent的攀爬高度，即(bot - nbot) <= walkahleClimb。

2.2个反体素的重叠部分的h要大于等于可行走高度，即(ntop - bot) >= walkableHeight。

某个反体素与前后左右4个邻居反体素的连通信息存储在反体素结构的con字段，每个方向占6个bit，相应bit值表征连通邻居反体素的layer层。

struct rcCompactSpan{
 	unsigned short y;
    unsigned short reg;
    unsigned int con : 24;
    unsigned int h : 8;
};

比如con字段的二进制值为000001 000010 000000 000100时，意义如下：

1. 左方向，该体素与layer为1的体素连通

2. 上方向，该体素与layer为2的体素连通

3. 右方向，该体素无连通体素

4. 下方向，该体素与layer为4的体素连通

裁剪可行走区域

我们采用dist数组来保存每个反素体与可行走区域边缘的最近距离。

对于上图的中间那个体素：

1.从左到右、由下及上扫描反体素时，绿色的那4个邻居体素已先被扫描到。

2.从右到左、由上及下扫描反体素时，蓝色的那4个邻居体素已先被扫描到。

因此，我们可以通过上述两次对所有反体素的扫描可以得到每个反体素与可行走区域边缘的最近距离。

对于dist值小于agent直径的反体素，将其标记为不可行走。

标记体素掩码值

​ 通过部署一些多边形柱子，然后遍历所有反体素，对于在多边形柱子内的反体素，将其areaId标记为相应值。areaId表示该体素是否可行走，是否是山地、草地之类。

​ 后续的区域划分会确保1个区域不会包含两种areaId，detour寻路也支持对于不同的areaId定义不同的单位路径损耗cost。

const ConvexVolume* vols = m_geom->getConvexVolumes();
for(int i = 0; i < m_geom->getConvexVolumeCount(); ++i)
    rcMarkConvexPolyArea(m_ctx, vols[i], vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigened char)vols[i].area, *m_vhf);

区域划分算法

1）分水岭(watershed)：racast默认算法，效果好，速度慢。

2）Monotone：速度快，但是生成的Region可能会又细又长，效果一般。

3）layers：类同monotone，只是区域在生成过程中不会有叠层(不会跨相同x、z坐标的多个y坐标体素)。

分水岭算法

​ 首先也是构建距离场dist数组(反体素到边界的距离)，这个过程与前面裁剪可行走区域过程中构建距离场非常类似。区别仅在于对非边界体素的判定：前者是前后左右4个方向都存在连通邻居并且体素掩码相同；后者只需要前后左右4个方向都存在连通邻居。

​ 距离场构建完后，需要再对dist数组进行均值降噪处理。体素到边界的距离修正为 (自身到边界距离 + 8个邻居到边界距离) /9，如果有邻居不连通，则用自身到边界的距离补位。

1.首先根据前面构建的距离场将体素划分到不同的批次，每隔2个距离场1个批次(看代码发现如果dist数组的最大值d是偶数，则第一个批次会包含d-2、d-1、d 三个距离的体素)。

2.从距离场最大的批次开始填充水位，新水位填充后立刻进行深度优先泛洪邻居体素。

3.新批次的体素，进行广度优先泛洪邻居体素，尚未被泛洪到的体素进行新水位填充。

如上图所示，最终体素被划分成了A、B、C三个区域。

• Monotone算法

​ 称某一行的连续体素成为一个sweepSpan，该算法从左到右逐行扫描体素，每扫描完一行体素，针对该行体素形成的一系列sweepSpan进行判断：如果某个sweepSpan A在-y方向上的邻居sweepSpan只有1个并且该邻居在+y方向上的邻居也只有1个(必然是sweepSpan A)，那么合并这两个sweepSpan。

区域裁剪

做区域裁剪前，需要找出每个区域的邻接区域。寻找邻接区域的流程如下图所示：

比如下面这个图中：

1.区域7的邻接区域为区域6、3、8、9、5

2.区域5的邻接区域为区域4、6、7、9

找出邻接区域后，再做如下处理来完成区域裁剪。

1.针对每个区域，采用深度优先遍历其所有邻接区域，如果最终包含的体素数目小于minRegionArea，则将遍历到的所有区域裁剪掉。我理解这个操作是为了减少比较小的孤立区域。

2.对体素数量过少的区域A进行合并，合并到最小的邻接区域B中。合并过程中，需要将A的邻接区域合并到B的邻接区域中，针对所有区域的邻接区域，需要将其中的A区域需要替换为B区域。

3.经过区域裁剪和合并后，region会变少，需要对区域的regionID重新remap赋值，以此来降低regionID的最大值。

生成轮廓线

与寻找邻接区域类似，都是沿着区域边界顺时针行走。行走过程中取轮廓点的规则为:

1. 体素左方是边界，轮廓点取其上方体素。

2. 体素上方是边界，轮廓点取其右上方体素。

3. 体素右方是边界，轮廓点取其右方体素。

4. 体素下方是边界，轮廓点取其自身。

这样做的目的是，使得各个区域的轮廓线多边形的边互相重合。最终效果如下图所示：

轮廓线简化：简化的目的是使用尽可能少的直线段来逼近带毛刺的边界。整个简化过程如下：

1. 左下角和右上角顶点作为初始轮廓。

2. 对于轮廓线段，遍历线段中间的其它顶点，找到偏离线段最远的顶点，如果偏离距离大于指定值，则将该顶点加入轮廓。

3. 一直迭代，直到所有顶点与轮廓的距离在指定值内。

检查轮廓线的空洞

​ 如果顶点的存储顺序为V1,V2,V3,V4,V5.如果按存储顺序去点的话们应该是V1V2、V2V3、V3V4、V4V5和V5V1.但Recast源码计算多边形面积，确是与此相反。因此空洞轮廓线的顶点存储虽然是逆时针，但其面积反而为负数。下图是计算多边形面积的函数。

static int calcAreaOfPolygon2D(const int* verts, const int nverts){
    int area = 0;
    //取顶点顺序与顶点的存储顺序刚好相反
    for(int i = 0, j = nverts - 1; ji < nverts; j = i++){
        const int* vi = &verts[i*4];
        const int* vj = &verts[j*4];
        area +=vi[0] * vj[2] - vj[0] * vi[2];
    }
    return (area + 1) / 2;
}

下面是利用多边形面积检测空洞的相关代码。

int nholes = 0;
for(int i = 0; i < cset.nconts; ++i){
    rcContour& cont = cset.conts[i];
    
    winding[i] = calcAreaOfPolygon2D(cont.verts, vont.nverts) < 0 ? -1 : 1;
    if(winding[i] < 0)
        ++nboles;
}

合并空洞

分为四步：

1）找到空洞的左下方顶点B4

2）将轮廓线所有顶点与B4相连，如果连线与轮廓线、空洞都不相交，则连线构成1条对角线。

3）选择其中长度最短的1条对角线，将空洞合并到轮廓线中。

如果包含多个空洞的话，将空洞按左下方顶点排序，依次迭代将外围轮廓与空洞进行合并。

轮廓线的三角剖分(耳切法)

定义：
1）顶点是一个凸点

2）左右顶点相连的对角线与其它边不相交

在上图，V1、V4、V4、V6是耳尖

将对角线最短的耳尖V1进行切割，切割后需要对左右相邻的顶点是否为耳尖重新判断，

切割后耳尖为V2、V4、V5、V6、V7。经过多次迭代后，行程多个三角形。

凸多边形的合并

轮廓线经过三角剖分后形成了一系列凸多边形(三角形是最简单的凸多边形)。为了提升detour寻路的效率，我们需要凸多边形进行合并。

2个凸多边形必须满足下面两个条件才可以合并：

1）必须要有公共边

2）合并后，公共边的2个顶点是否能维持凸多边形。

detour寻路

​ 通过前面的体素化、构建高度场、区域划分、轮廓线生成、三角剖分、凸多边形合并，我们将场景构建成了一系列可用于寻路的凸多边形。

Detour寻路算法步骤分为：
1）寻找离起点A和终点B距离最近的凸多边形

2）通过A*寻路算法找出点A到点B所经过的凸多边形序列

3）通过漏斗算法确认出最终路径

如何寻找最近的凸多边形

​ 为了提升查找效率，我们利用所有凸多边形构建一颗BVH树，查找时，将点扩张为小正方体，判断是否相交或者是否是叶子节点，如果是，则对于相交的叶子节点，取出其中的凸多边形。继续迭代下一个节点。如果为否，则为不相交的非叶子节点，通过继续遍历索引找到右子树。

这颗BVH树的特点有：
1）根节点的包围盒包含左右子树的包围盒。

2）叶子节点才存储凸多边形数据。

3）划分左右子树的时候，选择最能均匀分割多边形的坐标轴。

构建BVH树的代码如下：

static void subdivide(BVItem* items, int nitems, int imin, int imax, 					int& curNode, dtBVNode* nodes){
    int inum = imax - imin;
    int icur = curNode;
    
    dtBVNode& node = nodes[curNode++];
    
    if(inum == 1){
        node.bmin[0] = items[imin].bmin[0];
        node.bmin[1] = items[imin].bmin[1];
        node.bmin[2] = items[imin].bmin[2];
        
        node.bmax[0] = items[imin].bmax[0];
        node.bmax[0] = items[imin].bmax[0];
        node.bmax[0] = items[imin].bmax[0];
        
        node.i = items[imin].i;
    }else{
        calcExtends(items, nitems, imin, imax, node.bmin. node.bmax);
        int axis = longestAxis(node.bmax[0] - node.bmin[0],
        					  node.bmax[1] - node.bmin[0],
                              node.bmax[2] - node.bmin[2]);
        if(axis == 0){
            qsort(times+imin, inum, sizeof(BVItem), copmpareItemX);
        }else if(axis == 1){
            qsort(times+imin, inum, sizeof(BVItem), copmpareItemY);
        }else{
            qsort(times+imin, inum, sizeof(BVItem), copmpareItemZ);
        }
        int isplit = imin + inum / 2;
        subdivide(items, nitems, imin, isplit, curNode, nodes);
        subdivide(items, nitems, isplit, imax, curNode,nodes);
        int iescape = curNode - icur;
        
        node.i = -iescape;
    }
}

A星算法确定路径的凸多边形序列

​ A星算法的流程在前面介绍过，在这只介绍区别。

多边形的F值热河确定：

​ 多边形的G值 = parent凸多边形的G值 + 代表parent凸多边形的顶点到parent与该多边形公共边中点的欧几里得距离。这里选择顶点代表凸多边形的规则为:parent凸多边形与其本身公共边的中点。

​ 多边形的H值 = parent与该多边形公共边中点到终点的欧几里得距离。

​ 算法迭代过程中，顶点所用的数据结构如下所示，cost代表起点到此所用的开销，total表示F值，pidx代表parent凸多边形，flags代表该点当前是在openList还是closeList中，id代表其所属的凸多边形。

struct dtNode{
    float pos[3];
    float cost;
    float total;
    unsigned int pidx : DT_NODE_PARENT_BITS;
    unsigned int state : DT_NODE_STATE_BITS;
    unsigned int flags : 3;
    ddtPolyRef id;
};

漏斗算法平滑路径

整个算法过程可以用下面这个图来描述

​ 起点A不仅作为漏斗的初始顶点，也作为漏斗的初始两个端口，此后两个端口不停地向公共边的两个端点移动。

漏斗左右端点继续移动，需要满足下面2个条件

1.移动端点后的边是朝向漏斗收缩的方向。

2.移动端点后的边没有跨过另外1条边。

​ 如果移动端点后的边是朝向漏斗收缩的方向，但会跨过另外1条边。— 此时将另外1个端点加入路径，并将其更新为新漏斗的顶点。