所谓避障算法，主要目的就是“避障”（废话），适用场合是并不复杂的agent自身周围的障碍规避，甚至对移动障碍有一定的预判。
所以在常见的游戏设计研发时，一般用于大型寻路算法（A*等）中，相对细微的近邻状态来做一些小距离的避障。
举个例子，你是饿了么骑手，你从你的当前位置，到达送餐地点的路线，主要由上层的寻路算法取得：
但是在路途中，你遇到车辆、建筑以及其他骑手的时候，你如何驾驶车车在他们之间穿梭，就是使用的避障算法。
需要注意的是，此时的穿梭并不会大距离的偏移出既定的导航路线，比如你再怎么闪避其他骑士，也不可能从人民大道跑到成华大道，一定还是大概在既定的路线上前进。
目前它的算法优化已经到了ORCA(RVO2)，但是在此之前，还是先从VO讲起，这样更容易理解算法逐渐优化演进的过程。
另外说明一点，避障算法使用的数学理论基本在高二之前我们就学习过，所以本身算法并没有太高深。
之所以搞得看起来这么复杂，我猜测是因为写成论文，需要严谨和学（zhuang）术（b）。
所以本文会以最白话的方式来叙述算法，不整那些有的没的（比如Minkowskisum一类的，我们就全部用几何图形来表示）。
VO其实本身并不能算是一种算法，他只是一种解决避障问题的思路，而后续的RVO以及ORCA其实都是基于这个设定。
它提出了一种叫做VelocityObstacle的东西，其实它也非常见文知意，其实它就相当于将一系列可能会发生碰撞的速度在速度域（几何空间内表示为以横纵速度为基的坐标系）中划分出来作为一个需要避免的区域，在选择下一帧的速度时，避免这个区域内的速度就好了。
它其实本身就像是处于速度域中的“障碍物”一样，所以VO这个名字其实起的真的很好233。
拥有VO这个概念以后，我们最容易想到的算法其实很简单，这也是我们一般称为“VO算法”的最基础的逻辑。
假设一个场景，A和B在空间中偶遇，因为VO算法本身是分布式的，也就是每个物体只考虑物体自身，这样做的好处和意义这里就不赘述了，所以，我们从A的角度来考虑这次偶遇。
我们假设A和B都有自己的社交安全距离，这个距离是一个圆形的范围，半径分别是RA和RB，那自然而然想到的算法就是：我们这一帧的相对移动要保证两个人的最近距离要大于等于RA+RB。
这是显而易见的，实现这个目标的方法也很简单，在A的位置放置一个质点，然后在B的位置放置一个半径为RA+RB的大圆，这个大圆以外的位置都是A结束时安全的相对位置，所以这一帧的相对移动只要位于圆外就可以。
但是又因为，虽然结束位置只要在圆外就可以，但是因为移动过程是连续的，所以我们移动的路径实际上是初始点到终点的一条直线，因此我们还应该保证我们不应该“穿过”这个大圆而到达大圆后方，即大圆相对于质点位置的后方的锥形区域也是不可达的。
我们画出几何图形其实相当显而易见：
如上图，灰色区域就是不可达区域，但是为了计算简单，同时有一定的预判效果，VO算法把整个三角区域全部视为“危险”区域。
需要联想到，此时所说的这一帧质点的相对位移，其实就是质点的单帧速度，而质点的单帧速度，其实就是A和B的相对速度，所以其实我们可以非常容易的把上图映射到速度域内，其坐标系原点即为质点的坐标。
可以看出来图形和上面那张坐标图的几何结构基本是一样的，只不过这已经映射到速度域了（红色区域在坐标域为危险区域，在速度域即为VO）。
理论上当A和B的相对速度选择红色三角以外的点，即可安全错开。
VO的思想非常简单，漏洞也是非常明显的，就是会发生抖动，且两个agent的情况就有可能发生，让我们看看原文的例子。
当A和B相向而行时，一开始A和B会错开，但当下一帧对方的速度发生变化时，他们又会把速度转回来，因为这个时候A会认为B就是向左上走的，所以我还是保持最佳的向下走也不会撞上，B也是这么想的，所以他们就转回来了。
实际上这可能也不会造成最终的碰撞，因为在第二帧的时候，它们的速度确实是错开的，所以由于预判了碰撞，最终还是会很危险的错开。
但这个过程中两个agent的反复横跳的过程还是不够优雅的，而原因也显而易见，VO算法中智能体总是默认其他智能体是稳定的恒速前进，这导致当对方的速度发生变化时，自己的行为就变得不符合预期。
所以在RVO中最大的改进，就是我们假设对方也会使用和我们相同的策略，而非保持匀速运动。
在基础的VO算法中，产生抖动的原因是A在第2帧选择新速度之后，发现B的速度也变化了很多，A就会认为改回最佳速度（即直接指向目的地的速度）似乎也不会碰撞了，因为B的新速度其实就是假设A保持最佳速度也能不碰撞的情况下改变的，所以A就会认为B允许他转回来，但同时B也是这么想的。
然而在RVO中，A把自己的速度只改变1/2，也就是说，我们假设A和B想要错开，总共需要错开10cm，VO算法中A和B都会各自错开10cm，而在RVO算法中A只错开一半，也就是5cm，同时A假设B会错开另外一半，B也是这么想的，因此两者不谋而合，第二帧的时候，两个人各自错开了一半，并且发现此时转回最佳速度依然是会碰撞的（因为每个人只转了一半嘛），因此有效避免了上述抖动的现象。
RVO虽然解决了VO算法出现的问题，并且在单对单的避障中几乎总是表现良好，但当智能体的数量增多时，还是会出现不符合预期的现象。
假设下面一种情况，A和B一左一右的与C相向而行，在RVO中会遇到什么情况呢？
很简单，A认为C会向右转，因此自己向左转了1/2，而B认为C会向左转，因此自己向右转了1/2，而C实际上两种做法都没有选择，因为在VO图中，这实际上是两个锥体摆在自己的面前，所以C选择非常努力的向左或者向右转向。
这个时候A、B、C三个人就都炸了，因为没有一个的预料是正确的，所以他们在第3帧时就会根据一个预料之外的对方速度修改自己的速度，从而形成抖动。
其实原因也很简单，在RVO中，所有的智能体都假设对方只考虑自己。
所以这次事故的原因就是，A认为C爱着自己，B也认为C爱着自己，但实际上C同时爱着对面两个人，就像是A找C约会，然后C把B带上一起了。
虽然这种情况下实际上也不会发生真正的碰撞，因为A和B终究会向左右挪开，但过程中的抖动也是不符合预期的。
后来ORCA就出现了，它切实的解决了上面这种抖动的问题（虽然我认为是碰巧解决了，因为ORCA相对于RVO的改进其实主要是效率原因）。
ORCA与RVO最大的区别，在于VO的形状差异，在以往的VO算法中，VO都是以无限高度的锥形出现的，二维中就是同源的两条射线的夹角，但在ORCA中，我们使用一个有向平面来分割空间。
因此求解最佳速度的计算也得到了优化，从一个由一堆尖角切出的空间变成了高考必考内容的线性规划问题。
而ORCA是如何得到这个平面的呢？我们看图。
图a是位置域，智能体A和B在这里偶遇了。
这时候我们从A的角度来看问题，把A的位置放到原点，那么VO算法中不可达区域在位置域中的位置就如同图b中大圆的部分及其后的锥形空间，即以pb-pa为圆心，半径为ra+rb的圆。
图c我们不用管它，其实就是为了方便我们理解的，不过我觉得其实想弄懂它完全没啥意义，而且对我们理解也没啥帮助，而且代码里其实完全没用上。
理解了这个之后，如果把这个VO解释成一个锥体，其实就是类似前面说的VO，但是ORCA中是把它解释成了一个二分平面，这个平面大概如下图所示：
就是红色箭头指着的这个平面，马赛克掉的地方不用去管它，那是智能体B的平面，我们现在是智能体A，所以看不到。
首先我解释一下图中的变量，vopt指的是两智能体的期望速度，一般就是指向目的地的速度，u是相对期望速度到上面计算出的VO区域外的最短向量，n是u指向的VO表面的位置的法线。
这个时候平面就确定了，就是位于v的期望速度+u/2的方向为n的半平面。
为啥这样确定咧？我相信聪明的小伙伴已经反应过来了，u是什么，u其实就是相对速度需要修改的值，相当于上面有一个例子里面的那10cm，所以vAopt+u/2其实就相当于A与B争端中的最佳修改后速度，到这里其实跟RVO是差不多的。
但不同的是，我们得到这个值后并不是用它作为最终答案的一部分，而是由它确定了这个半平面。
这个首先我们要说明为什么要确定一个半平面，其实之前说过了，使用半平面可以把问题转化为简单的线性规划问题，计算量断崖式下降。
那么我们为什么要选择这个半平面呢？原因其实很简单，这是包含当前这次冲突最佳解决方案的半平面，相信聪明的小伙伴可以理解这句话。
所以，当A在完成多个平面的切割后，每次平面切割剩下的那个可选的半空间，都一定包含了当此冲突最佳解决方案的速度，比如与B之间的速度一定包含在第一个半空间内，与C之间的最佳速度一定包含在第二个半空间内
这样可以保证，在这么些半空间的交集内，出现与每个对手解决方案的最优速度的可能性是最大的，即使最后最佳速度被切走了，也总能选到相对更好的速度。
个人认为这个结论还是挺容易理解的，所以此处就不赘述证明了，因为这违背了大白话讲算法的初衷。
好的那么空间切割完毕之后，如果最佳速度处于当前空间内，就选择最佳速度，否则，就选择距离最佳速度最近的切割后空间内速度，这个速度一般位于空间的边界顶点处，也就转化为了一个简单的线性规划问题。
这是论文中的图，注意图b中有一条曲线，这是因为论文中还有一个vmax，即智能体能达到的最大速度，这也是合理的，不过不属于算法核心，所以前面没讲。
当然需要注意的是，当对手非常多时，ORCA是非常容易把整个空间全部切光的，因为前面也说了，每次都切掉近乎一半的空间，所以很容易发生这种情况
这种情况的解决方案其实也很简单，就是增加了一个d变量，它表示速度到当前半平面的距离，d为正时表示当前速度在这个半平面以内，为负时表示被半平面切掉了，我们就是要找d的最大和时对应的速度，也就相当于是违规最小的速度。
从几何上看就直观很多，可以怎么理解呢，其实就是把半平面向外挪，让半平面少切一部分，使得这些个半平面刚好能切出一个点，同时对所有半平面的平移最少，就选这个点作为目标速度。
除此之外，论文中还讨论了关于静态障碍物的躲避，其实也是合理的，因为你在送餐的时候不能光躲会动的东西嘛，也要躲电线杆才行。
这个的逻辑其实很简单，就是因为障碍物本身没有速度是静止的，所以作为智能体要负责全部的躲避责任，而不是1/2了。
代码中比较复杂主要是因为障碍物的构成不是一个会移动的圆，而是一条条线段连成的多边形，所以计算上复杂了很多。
官方的代码做了很多为了效率上的让步，比如经常直接把平方值作为开根后的向量长度来用，或者直接使用magicnumber一类的操作，我们不要过分纠结，差不多就行~
本文主要解析的是官方github中的c#代码，为什么选择c#代码主要有两点，一是因为我对c++已经很久没有实际应用了，所以害怕因为自己的生疏对命令有误解，影响对代码的理解，二是因为我们主要目的是理解并复现这个算法，所以选择c#，因为可以减少很多不必要的代码，读起来也会觉得逻辑更清晰。
代码中主要围绕Simulator类在执行游戏循环，每一帧执行doStep函数，函数内容也很简单，就是先构建一大堆worker，然后重构kd树，kd树其实是有obstacle和agent两棵，但是obstacle的只需要构建一次，agent则需要每帧构建，因为agent会跑嘛，其实个人认为这里应该是个优化点，因为其实很多游戏中并不需要真实的获得离我最近的智能体有哪些，理论上因为有军团的存在，所以应该有比较方便的获取方法，目前还没细想。
构建完KDtree之后就是一个模块一个模块的执行worker的step和update逻辑，step是主要计算逻辑，update则是更新agent状态的逻辑，内容也很简单：
可以看到主要就是遍历所有的agent然后调用其中的函数，computeNeighbors主要就是树操作，从kd树中取出离自己最近的智能体，update就是更新自身状态，速度啊，位置啊一类的。
关键在于computeNewVelocity，我们上文所讲的ORCA的逻辑主要就在这个函数中。
不过这个函数相当长，我们分块来分析。
首先是computeNewVelocity的整体结构：
可以看到其实主要的逻辑都集中在生成静态障碍物的半平面和其他智能体形成的半平面两个循环中，外面的逻辑相对简单。
第一行就是把之前存储的半平面清空，重置本次计算的状态。
循环前对invTimeHorizon的计算则是为了减少循环中的计算数量，因为这其实是个常量值，放在循环中的话要多跑n遍。
最后就是用线性规划来求解最后的速度，这个是比较通用的内容，本文就不展开了，感兴趣自己去看，内容也很短，一个函数大概就50行左右的样子。
所以其实计算半平面的逻辑还是集中在了两个循环中，我们此处主要解析智能体的这个循环，静态障碍物的这个跟智能体的逻辑很类似，代码我也加注释了，感兴趣自己下下来看吧：
首先是做了一些变量的准备，大概代表的含义都如我的注释，其中me代表当前agent，you代表当前neighbor（下面简称A和B）。
line就是我们最后要算出来的半平面的那条切割线，这个类里面封装了它的位置和方向。
u则是上面算法中介绍的u，是一个向量，我们要用它来计算line的位置。
接下来是一个分支语句，可以看到它用距离的平方和半径和的平方去比较，因为距离和半径和本身都是正的，所以比较平方也可以比较出两者的大小。
所以这个分支相当于是判断A和B是否已经碰撞。
最后结束的两行，是在用上面算法中的公式来计算line的位置，并压入list。
好的我们先看如果两者已经接触的逻辑，因为比较简单容易理解：
对于w的理解我们可能得借助几何图形，我们还是看上面这张图：
我们先称这个图为雪人图，因为它很像一个倒挂的雪人（指大小两个圆），后面会用到好几次。
从雪人图里可以很容易的看出来，所谓invTimeStep*relativePosition其实就是此图中小圆的圆心位置，所以用相对速度减去它，得到的就是从小圆圆心指向当前相对速度的一条向量。
接下来的两行比较简单，就是计算了w的长度和w方向的单位向量。
接下来使用unitW来计算line的方向，使用的是常用的向量顺时针旋转90°的计算公式，这个公式可以很容易的用坐标系旋转的公式来推导，即cos90°=0和sin90°=1的性质。
下面一行是计算u，这里需要把公式拆解一下，把unitW乘进括号里，就会好理解很多。
combinedRadius*invTimeStep是小圆半径，乘以unitW可以理解为就是relativeVelocity到小圆圆心的连线与小圆圆周的交点，而wLength*unitW就是relativeVelocity自身，所以用前者减去后者，就是从relativeVelocity指到小圆圆周的向量，这不就是u嘛~
我们要求的就是这个嘛~
接下来再看尚未碰撞的情况，注释比较长，可以辅助理解：
这里的w、wLengthSq都比较简单，不用多解释
dotProduct1需要解释一下，这里参考雪人图改良版，如上图，relativePosition就是图中大圆的圆心，也就是从原点指向大圆圆心的向量，图中红色的这条，所以w与此向量相乘，得到的就是w投向图中红色向量的投影。
可以看到下面的分支语句就是用dotProduct1的正负来作为判断条件之一的，其实相对来说比较容易理解，向量点乘，两向量夹角为锐角时为正，钝角时为负，垂直时为0，所以此处第一个判断条件意思就是w和红色向量的夹角为钝角，那其实在速度域中就如下图：
图中的红线与上图的红色向量垂直，这条红线以下的点，都符合dotProduct1<0.0f，原因我在上面解释的很清楚了，因为这下面的点，都满足w与红色向量成钝角。
这个条件主要是为了第二个判断条件做第一部分筛选。
第二个判断条件需要做一点准备计算，基本的推理操作，这个不等式其实本身跟abs(dotProduct1)>combinedRadius*wLength是等价的，而abs(dotProduct1)可以理解为上上图红色向量到w投影的标量长度（长度只能为正）乘上wLength，不明白的自己去看向量点乘公式。
所以两边可以同时消掉wLength，这里的条件就变成了“红色向量到w投影的标量长度>combinedRadius”，那这个条件什么时候满足呢？说真的想破脑瓜也想不出个所以然，但是用几何来表现还是很清晰的。
我们可以先来看，什么时候左边等于右边，显而易见的：
当w与VO的两条腿（腿指的是那两条射线）垂直时，即紫色向量的方向时，红色向量在w的投影是与combinedRadius相等的，原因我觉得就不用解释了吧？不明白的请去复习向量点乘的几何意义。
这个时候可能就会有记忆力比较差的小朋友问了，关于大圆圆心与紫色向量对称的两条向量呢？红色向量在他们两个上面的投影也是combinedRadius啊？
虽然很不情愿，但是我还是得提醒一下，第一个条件，帮我们筛选掉了与红色向量夹角为锐角的向量，所以本来的四条，只剩下图中紫色的两条了。
而这两条的方向恰恰是什么呢？恰恰就是两条腿与大圆小圆的切点与圆心的连线，为什么这么说呢，因为切点与圆心的距离就是半径，不明白请回炉重造。
我们做这么多的意义是啥呢，可能聪明的朋友已经发现了，其实VO的整个区域是可以分成两部分的，一个部分的外边缘是小圆的圆周，另外一部分的外边缘则是两条腿的一部分。
而我们现在通过这两个条件筛选出来的，就是下图紫色向量之间的区域，这个区域的VO就是以小圆的圆周为外边缘的：
那么下面要做的事就显而易见了：
用w的方向来计算u，计算过程跟已经碰撞的情况是完全一样的，此处就不赘述了。
那么else的部分，半平面其实就是腿的方向。
首先通过勾股定理计算出原点到大圆切点的长度，这里称为腿长。
接下来的一个分支语句，稍微有一个小知识点，就是行列式的几何含义，就是有向体积，在二位空间中是有向面积。
所以可以通过计算relativePosition和w组成的行列式的值的正负，来判断relativeVelocity相对于向量relativePosition的位置，即此处的左腿右腿。
分开左腿和右腿后，接下来通过一个相对复杂一点的式子来计算腿的方向，我们只看一下左腿吧，右腿是完全类似的。
首先如上图中的三角形，我们称为“黄金三角形”，它的三边长度分别是combinedRadius、relativePositionLength以及leg。
我们先记住这个概念，然后来看公式。
line.direction=newVector2(relativePosition.x()*leg-relativePosition.y()*combinedRadius,relativePosition.x()*combinedRadius+relativePosition.y()*leg)/distSq;其中disSq是前面计算的relativePosition长度的平方，所以可以理解为relativePositionLength*relativePositionLength，这里我们可以先除一个relativePositionLength进来，整个式子就变成了：
line.direction=newVector2(relativePosition.x()*leg/relativePositionLength-relativePosition.y()*combinedRadius/relativePositionLength,relativePosition.x()*combinedRadius/relativePositionLength+relativePosition.y()*leg/relativePositionLength)/relativePositionLength;相信X大的小朋友已经发现了leg/relativePositionLength就是黄金三角型下面这个角的cos值，而combinedRadius/relativePositionLength就是sin了，所以此时，前面new出来的这个vector2就昭然若揭了，他就是把relativePositionLength沿原点逆时针旋转黄金三角型下方角度所得到的向量！
这个向量恰好与左腿重合，且长度与向量relativePosition相同。
注意了，刚才我们只除进来一个relativePositionLength，所以在vector2外面还有一个relativePositionLength要除，因此，我们得到了左腿的单位向量。
只能说，牛逼！
然后下面再用点乘，获得relativeVelocity在左腿上的投影长度dotProruct2，最后在用向量减法得出u。
完结撒花~*★,°*:.☆(￣▽￣)/$:*.°★*。
另外这是我的技术群：573228201，有问题可以直接来群里喷我，谢谢！