ORCA小结

前言

        我本来是没有计划去做关于ORCA(又被称为RVO2)的文章。因为关于分析RVO2的文章已经很多了，但是这些文章都没能让我完全懂RVO2。因此我不想去做什么总结和分析。毕竟我自己都没有搞懂的东西做什么分析和总结。好在RVO2相关的代码不需要我懂得全部的原理，因此项目还可以推进下去。直到现在朋友来问我关于RVO2相关的知识，并提出了一些在实践过程中的问题。果然我半桶水的水准完全不能帮助到我朋友，所以我打算好好思考一下RVO2相关的理论。

特别说明：本文大量充斥着我个人的想法，且这些想法我找不到资料去做验证。这一点在本文的源码分析中尤为常见。因此我并不能保证本文所述都是正确的。我恳请大家辩证的思考本文的内容，如果您找到了文章中的问题，麻烦您告知于我，我进行修改。

什么是ORCA

        ORCA是“Reciprocal n-body Collision Avoidance”的缩写，它来自于2011年的一篇文章。正如其名一样，它是讨论如何让n个单位互相避免的算法。它又被称为RVO2（Reciprocal Velocity Obstacles 2），下文都会用这个来称谓。文章全文都是英文，但是网上也能找到一些中文的翻译，我也个人尝试着翻译了一下:RVO2译文。我个人建议大家依据原文，多看几篇译文相互印证一下。

RVO2的原理

        要了解RVO2的原理，我们先要了解VO（Velocity Obstacles）这个概念。VO从直译就是速度阻碍的意思。如果你有看其他的文章，你就发现这些文章中都配了类似这样的图片。

图中是两个圆A和B，如果此时A，B都要进行移动，那么B要怎么移动才能保证不会碰撞到A。我们先假设A如果不运动的情况。那么我们可以将A和B看做质点，如果要想B在运动的时候不碰撞到A，则A和B的距离应该大于等于RA+RB(RA是圆A的半径，RB是圆B的半径)。那么我们以A点为圆心，RA+RB为半径画圆，如下图所示：

B衍生出来的两条虚线是经过点B且切于圆A的切线。分析到这里我们可以得出一个结论，只要A最后移动的距离不在图中红色阴影部分则为正确。但是这里就存在一个问题，即如果A不动的情况下，B要如何进行直线运动到达下图中黑色阴影的部分。

我们不难得出如果B要进行直线运动到达黑色阴影部分，那么它必然会先穿过红色阴影部分，即B在运动的时候穿过了A。这显然不符合我们的要求，因此B的移动范围应该是两个阴影之外的部分。为了方便，在分析中，我们一般以要移动的点（这里就是B点）作为原点进行计算。以此搭建的坐标系中，每一个点就表示B点在本次运动中所移动的距离（位移），在一些文章中会将这个坐标系称为位置坐标系。RVO2最终的计算其实是为了得到一个速度值。所以到了这步，大部分的文章就会说：“此时我们要将之前的位置坐标系转换为速度坐标系”。如何转换呢？我们知道位移除以时间等于速度，所以我们只要将我们得到的位置信息除以时间，则完成了位置坐标系到速度坐标系的转换。在速度坐标系下，每一个坐标点都代表一个速度（Velocity）值，而我们之前的计算得到的阴影区域也会被映射到这个速度坐标系上，这个被映射到速度坐标系上的阴影区域又被称为速度障碍（VO，Velocity Obstacle）。

        到这，我已经介绍了什么是VO，我也说明了RVO2计算最后都会在速度坐标系上进行。在其他的文章中，你会看到这样的数学符号：\(VO^{τ}_{B|A}\),读作：A在时间τ下引起的B的速度障碍。这是VO的符号。

        上面我们说的\(VO^{τ}_{B|A}\)，又可以被称为是B相对于A的所有相对速度的集合,即\(v_B-v_A \in VO^{τ}_{B|A}\)或\(v_A-v_B \in VO^{τ}_{A|B}\)。在之前讨论的时候，我们事先将A设定为不会运动的物体。那如果A本身会运动呢？显然当A不会运动的时候上述说法仍然成立，而当A会运动的时候，我们可以用两种方法去考虑。一种是我们直接把A的速度考虑到我们的速度中，即上述的\(v_B-v_A\)。则VO的范围还是和上图一致。如果我们不将A的速度考虑到我们的速度中，则图中阴影的部分就要根据A的速度来进行闵可夫斯基和（Minkowski sum）运算得到新的阴影，其表达式为\(VO^{τ}_{B|A} \oplus V_A\)。这里的大写速度A表示A速度的集合。这时候速度只要不要在这个闵可夫斯基和内就可以避免A和B互相碰撞了。

上图中整体\(VO^{τ}_{B|A}\)（红色阴影和黑色阴影）平移得到了图中紫色阴影和蓝色阴影的样子，而在平移过程中将\(VO^{τ}_{B|A}\)所有扫过的地方都合并起来则是\(VO^{τ}_{B|A} \oplus V_A\)。如下图中绿色阴影部分。

这里我仅展示了A物体只有一个速度的情况下，但是A物体的速度可能会是一个速度集合。如果你想要知道在速度集合下是什么情况，那么你可以去了解一下什么是闵可夫斯基和。如果你不能理解也没关系，实际游戏开发中我们可以不用速度集合，因为所有的物体我们只取一个速度值，且后面的计算中也可以不用到闵可夫斯基和。但是在源码理解中，这个知识点还是很重要的。

        网上关于RVO2的文章中，在我看到描述速度坐标系的阶段时，我对速度坐标系中表达的点位意义产生了疑惑。如果是按照前面的一种方案，速度坐标系中的每一个点应该是\(v_B-v_A\)，而后面一种做法则是每一个点就代表着B的速度。而我自己也是看了源码以后才明白，源码中计算的时候就是将速度坐标系的每一个点都当做是B的速度。

        通过上面的描述，如果我们要得到VO，那么我就要知道物体的形状，速度和位置。但是RVO2到这里并没有结束，它多求得了一个被允许速度半平面（The half-planes of permitted velocities）。什么是被允许速度半平面？从我们之前的理论上说，只要速度不在VO内，那么都应该是可选的速度。但是RVO2却认为在被允许速度半平面中的速度才是我们可选的速度。我个人理解是认为如果我们真的去求所谓的闵可夫斯基和，求解的难度会大很多，而求解一个半平面则会轻松不少。

        在介绍如何求得被允许速度半平面前，RVO2中有一个概念：首选速度（Optimization Velocity）。我们在进行计算的时候都是使用这个速度来进行，包括我们上面讨论的计算，甚至接下来我们要说明的被允许速度半平面。虽然叫做是首选速度，但是实际上就是物体当前的速度。对于首选速度的取值，在RVO2论文中有对其进行讨论，这里我们并不多进行讨论，我们只要知道结论：首选速度就是物体的当前速度。在方程中的表示为：\(v^{opt}\)。

        现在我们了解了什么是首选速度，那么我们就可以开始计算被允许速度半平面。上面我们提到如果\(v_B - v_A \in VO^{τ}_{B|A}\)则表示物体B和物体A会在τ时间内碰撞。那么当我们选择的首选速度也满足这个条件即\(v^{opt}_B - v^{opt}_A \in VO^{τ}_{B|A}\)时，我们做出如下操作。我们设定向量u,u是\(v^{opt}_B - v^{opt}_A\)到\(VO^{τ}_{B|A}\)边界长度最小的方向。所以如果我们要\(v^{opt}_B - v^{opt}_A \notin VO^{τ}_{B|A}\)，那么我们至少要加上一个u。

关于边界问题 加上u后，因为u本身的定义，实际上我们得到值还是在VO的边界上的。这里我认为是RVO2并不把VO边界计算在VO内。实际上边界问题本身就是被模糊的问题，如果你真的要算也可以无非就是用一个很小的数延长u。

        现在物体A和物体B想要避免碰撞，那么他们就要进行速度值的修改，这个修改值之和至少为u。RVO2认为避障是一个相互的行为，那么他们应各自撑得一半的责任（权重），因此对于单个物体而言他最少的修改值应该为\(\frac{1}{2}u\)。我们现在就认定物体B只修改了\(\frac{1}{2}u\)，那么其当前速度就变成了\(v^{opt}_B + \frac{1}{2}u\)。我们再设一个向量n，其是u的单位向量。则速度半平面为\(\{v|(v-(v^{opt}_B + \frac{1}{2}u))\cdot n >= 0\}\)，即\(v^{opt}_B + \frac{1}{2}u\)到任何的速度v的方向，其与n的点乘值为正，此方向映射与n时方向与n一致。我们称这样所形成的半平面为被允许速度半平面表示为\(ORCA^{τ}_{B|A}\)。接下来我们对除了物体本身外的所有物体进行求半平面操作，然后我们取所有半平面的交集。最终速度的选取就是从这个交集中进行选择。

        即使我们划分了被允许速度半平面，我们仍然要面对无穷数量级（这里并不考虑为空的情况）的选择。那我们到底要怎么样选择我们的速度呢？在每一个时间点我们都会有一个预期速度，我们称其为\(v^{pre}\)，我们希望物体在这个时间点中尽量得靠近这个预期速度。那么最靠近这个预期速度的速度就是我们下一个阶段所要得到的速度即\(v^{next} = arg min(|v^{pre} - v|),v \in ORCA\)。

        以上就是RVO2中物体面对其他会移动的物体时进行的速度选择。那如果这个物体是一个不会移动的物体（静态物体）呢？实际上对于静态物体，RVO2在求解的想法是一样的，我们都要求得VO，然后在得到对应的被允许速度半平面。但是在一些小细节上的处理，还是存在一些区别。因为静态的物体是不会运动的，所以在半平面时移动物体承担所有的责任。在求ORCA的方法上，静态物体的ORCA和移动物体间的ORCA求解方法不一样。对于静态物体的ORCA，我们令其是静态物体VO内离移动物体首选速度最近点处与VO的切线，也就是说这个ORCA必然是和VO存在交点的。

        如果你看过RVO2的论文，你会在原文中找到这样的一句话：In case of obstacles, we choose\(v^{opt}_A = 0\)for all robots A.。在没有看到源码的时候，我认为这段话的中文翻译为：如果遇到障碍物，我们给所有的机器人A的首选速度设定为0。然而源码中的计算并非如此，唯有在移动物体已经碰撞到障碍物时才显得首选速度设定为0。所以在分析源码的时候，我感觉这里的翻译应该是：如果撞到了障碍物，我们给所有的机器人A的首选速度设定为0。但是我总觉得上面的翻译才是对的。除此之外，在碰撞的时，前面所说的取VO边界上的里首选速度最近点位也没有做到。当然我个人是认为论文中的论都是在说碰撞还未发生时，如何避免碰撞。而碰撞已经发生的情况下，这样的方法必然就失效。源码中的做法是强制将速度(0,0)包括在选择中，实际上就是扩大了速度选择范围。

        前面在说明的时候，我一直说物体要与其他所有物体进行计算并求解半平面。但是实际上，我们并不需要做到这样。显然对于一对离得十分远的物体而言，除非它们的速度非常大否则它们大概率是不会互相碰撞的。因此我们可以只选取物体周围的其他物体来做判断。而这个选取范围就要依靠游戏中具体的要求视情况而定。

源码分析

        RVO2源码(github源码下载)中使用了KDTree的方法来进行周围邻居的查询，这个并非是我们这个文章的主题，这里并不做过多的说明，大家只要知道这个是用来查找物体周围的一定范围的其他物体就好。虽然代码量不多，但是大部分的代码我这里也并不说明。我认为最关键的代码就是其Agent中的computeNewVelocity函数，即计算新速度函数。正是这个函数计算出了物体下一刻的速度。在开始分析之前，我强烈建议各位先下载一下源码，并看一部分。因为如果真的按照源码去分析，我觉得很难进行文章的描述，因此文中所贴示的代码与源码可能会有差别。

        在正式分析computeNewVelocity函数前，我先提几点。

        首先下文中，我们会用Agent来指代那些会移动的物体。

        其次，在源码中，障碍物被处理为点的集合。障碍物点的数据结构如下：

1
2
3
4
5
6
7
8
9
class Obstacle
{
    internal Obstacle next_; // 下一个节点
    internal Obstacle previous_; // 上一个节点
    internal RvoVector2 direction_; // 指向下一个节点
    internal RvoVector2 point_; // 此点的位置信息
    internal int id_; // 对应的id
    internal bool convex_; // 是否为凸点
}

点之间有着链接关系（next_和previous_形成链接关系），以此来形成障碍物的形状。与其说Agent在寻找周围的障碍物，不如说Agent在在寻找周围的障碍物点。所有的障碍物点都会被放置在障碍物KDTree（Agent们则是在Agent的KDtree中）中，以方便后面Agent查询。除此之外，源码还对这些点进行了凹凸点分析。在源码中他是这样这样判断的：

1
2
3
obstacle.convex_ = 
    (RVOMath.leftOf(vertices[(i == 0 ? vertices.Count - 1 : i - 1)], 
        vertices[i], vertices[(i == vertices.Count - 1 ? 0 : i + 1)]) >= 0.0f);

上面的代码取了三个点为：要判断的点（vertices[i]）、其前一个点位和其后一个点位得到两个方向做叉乘。如果叉乘的结果大于等于0，则是凸起。

1
2
3
4
internal static float leftOf(RvoVector2 a, RvoVector2 b, RvoVector2 c)
{
    return det(a - c, b - a);
}

显然这对于我们描述障碍物的点位顺序是有一定要求。如果不是按照某个特定的顺序（顺时针或者逆时针）进行描述障碍物点位，即使是一个三角形上，其上的所有点都会被认为是凹点。以Unity的X轴和Y轴形成的二维坐标系，我们可以从这段代码中推断出逆时针为正确的方向。

个人项目中关于障碍物的点的处理 因为障碍物是趮点去描述的，如果障碍物的点过于稀疏，则会导致错误的避障。当然过于稠密也不是什么好事。我个人项目中，障碍物的点位是按照关键点 + 最小距离设定的。关键点描述出障碍物的形状，最小距离负责在关键点中填充点位。最小距离设定由游戏中最小的单位来决定，我个人设置为最小单位大小的一半。obstacle1是Agent周围的障碍物点，而obstacle2是其下一个障碍物点位。
关于叉乘和点乘 在二维空间系下，叉乘可以反应出两个向量之间夹角的sin值，而点乘则可以反应出cos值。根据sin函数和cos函数的图像，我们可以知道sin可以用来区分出另外一个向量在左在右，而cos可以用来区分另外一个向量在上在下。要注意的是对于点乘而已向量运算的顺序并不需要注意，因为点乘的公式：x1x2 + y1y2。而叉乘就要注意运算顺序，运算顺序的不同会导致不同的结果。

        在求速度之前，我们要先得到一系列的允许速度半平面。虽然理论上说是半平面，但是代码中却使用Line（线）。源码如下：

1
2
3
4
5
public struct Line
{
    public RvoVector2 direction;
    public RvoVector2 point;
}

由上可知，Line是用一个点位和一个方向来表示。用direction定义Line有两个作用，一个是为了确认那些点是在Line上的点，另一个作用是来表示允许半平面的范围。在源码中，其大量地使用叉乘来作为区域判断。

        最后，关于时间，源码中对于障碍物（静态物体）和移动物体的时间计算上有进行区分：timeHorizon和timeHorizonObst。在其论文中也有说明：We can typically use a smaller value of τ with respect to obstacles than with respect to other robots，中文翻译：我们一般可以使用比考虑其他机器人更小的τ值考虑障碍物。但是它自己的模拟代码中对于这两个的时间设置的值都是一样的。

对于周围障碍物的分析

        在函数中开始是先计算物体周围障碍物形成的被允许速度半平面ORCA。并且对于这些ORCA做了一定的排除处理。条件是：如果障碍物的两个端点到已有的ORCA面的距离大于Agent的半径时便排除这条ORCA。这些计算都是在以当前的Agent为中心的速度坐标系中进行的，所以所有的数据都进行了转坐标系处理。源码部分节选如下：

1
2
3
4
5
6
7
8
9
10
11
12
invTimeHorizonObst = 1.0f / timeHorizonObst_;
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next_;
RvoVector2 relativePosition1 = obstacle1.point_ - position_;
RvoVector2 relativePosition2 = obstacle2.point_ - position_;
if(
    RVOMath.det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, 
        orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON 
&&
    RVOMath.det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, 
        orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON
){}

       代码中，obstacle1（障碍物点1）是Agent周围的障碍物点，而obstacle2（障碍物点2）是其下一个点位。因为上面的源码是只考虑二维的情况，所以这里代码中用叉乘（\(a \times b = |a|*|b|sin(\alpha)\)）的方法来计算出点到直线最短的距离（点垂直于直线的距离）。因为direction必然是单位向量，所以才可以直接用叉乘来计算。你可以在源码后面中看到其对direction进行了归一化。

1
2
3
float distSq1 = RVOMath.absSq(relativePosition1);
float distSq2 = RVOMath.absSq(relativePosition2);
float radiusSq = RVOMath.sqr(radius_);

这里distSq1是障碍物点1到Agent距离的平方（distSq2同理），radiusSq是Agent半径的平方。这里取平方是为了减少运算量。

1
2
3
RvoVector2 obstacleVector = obstacle2.point_ - obstacle1.point_;
float s = (-relativePosition1 * obstacleVector) / RVOMath.absSq(obstacleVector);
float distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);

这个共同构成了方向obstacleVector，我们这里可以称其为障碍物方向。而relativePosition1是从Agent到障碍物点1的方向（relativePosition2同理），这里取反则是障碍物点1到Agent的方向。源码中用了点乘并除以障碍物方向长度的平方来得到他们的对应关系s。因此s * obstacleVector就是-relativePosition1在obstacleVector投影的向量。distSqLine是由obstacle1和obstacle2所形成的直线到Agent的最小距离。

如果Agent碰撞到了障碍物（点或者线段）

        源码中先考虑Agent是否碰撞到物体。其分为了3种情况考虑：

  1. 如果s小于0，且Agent碰撞到了障碍物点1（distSq1 <= radiusSq），则如果障碍物点所在的点位是凸起，那就创建ORCA。

源码：

1
2
3
4
5
6
7
8
9
10
11
12
if (s < 0.0f && distSq1 <= radiusSq)
{
    /* Collision with left vertex. Ignore if non-convex. */
    if (obstacle1.convex_)
    {
        line.point = new RvoVector2(0.0f, 0.0f);
        line.direction = RVOMath.normalize(new RvoVector2(-relativePosition1.y(), relativePosition1.x()));
        orcaLines_.Add(line);
    }

    continue;
}

s小于0表示-relativePosition1和obstacleVector的夹角值超过了90度。注释中的意思是“和障碍物顶点1碰撞，如果是凹点则忽略”。

为什么是凹点就忽略呢？ 在RVO2原论文中，他们假设所有的代理形状都是凸多边形（或者圆形）We use a simplified robot model, where each robot is assumed to have a simple shape (circular or convex polygon) moving in a two-dimensional workspace. 译文：每个机器人都假定具有在二维工作空间中移动的简单形状（圆形或凸多边形）。。因此虽然对于障碍物的描述允许其存在凹陷部分，但是这些不考虑。在我找到其他的资料中（由AI寻找，来源不明）也有对应的描述：“当障碍物为凹多边形时，其速度障碍物区域可能不连续或形成非凸集合，导致半平面约束无法覆盖所有潜在碰撞方向。”

如果是凸点，我们就要做出其对应的允许速度半平面。此时他们已经碰撞了，源码中用原点作为Line的point，其direction是垂直于relativePosition1朝向relativePosition1的左边。示意图所下：

图中b0和b1分别指代obstacle1和obstacle2。因为其夹角值超过了90度，所以Agent只可能在绿色的半圆弧的方位上。如果你还记得前面的理论，那么此时我们应该令首选速度为0，找到由此障碍物形成的VO边界上离首选速度最近的一个点。这里很显然，源码并没有这么做。源码这样做法更像是，既然Agent已经碰撞到了obstacle1，那简单一点就是朝着relativePosition1相反方向（-relativePosition1）的移动必然满足我们的条件。实际上只要在-relativePosition1方向上的投影值大于0的移动方向都是满足条件的移动方向。转换到速度坐标系下，移动方向就是速度方向。而在下面的计算中，源码认为被允许速度应该在所有ORCA线方向的左边，为了满足下面的计算于是认定direction是垂直于relativePosition1朝向relativePosition1的左边。这样所有在-relativePosition1方向上的投影值大于0的速度都在其左边了。后面所有的计算也是如此。

  1. 如果s大于1，且Agent碰撞到了障碍物点2（distSq2 <= radiusSq），则如果障碍物点2是凸点，那就创建ORCA。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
if (s > 1.0f && distSq2 <= radiusSq)
{
    /*
     * Collision with right vertex. Ignore if non-convex or if
     * it will be taken care of by neighboring obstacle.
     */
    if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= 0.0f)
    {
        line.point = new RvoVector2(0.0f, 0.0f);
        line.direction = RVOMath.normalize
            (new RvoVector2(-relativePosition2.y(), relativePosition2.x()));
        orcaLines_.Add(line);
    }

    continue;
}

s大于1表示-relativePosition1和obstacleVector的夹角值小于90度。且-relativePosition1在obstacleVector上的投影超过了obstacleVector的长度。注释中的意思是“当Agent和障碍物点2碰撞时，如果其是凹点或者它会被邻近障碍物点考虑到时忽略这个”。如果其满足条件则和之前的处理一样，但是因为这次是碰撞到了障碍物点2，所以以relativePosition2去做半平面的划分。

  1. 如果s的值在0到1之间（不包括1），如果碰撞到了障碍物点1和障碍物点2形成的线段则添加对应的ORCA。
1
2
3
4
5
6
7
8
if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq)
{
    /* Collision with obstacle segment. */
    line.point = new RvoVector2(0.0f, 0.0f);
    line.direction = -obstacle1.direction_;
    orcaLines_.Add(line);
    continue;
}

s的值在0到1之间表示，Agent在两点之间。而ORCA的方向选择则是碰撞到的线段方向的反方向。

为什么一定是线段方向的反方向

以下的观点仅是我个人思考的结果，无任何的资料作为参考，请谨慎辨别。如果有误还请告知

按照我们之前的说法，我们碰撞到了这个线段，那么我应该用-relativePosition1 - s * obstacleVector这个来定ORCA。而单纯用线段的反方向明显存在问题。我个人一开始在思考的时候，感觉有些问题，但是这个算法既然已经经过很多项目的验证，那就说明这是没问题的。我个人认为，按我们之前说逆时针的方向描述障碍物点是正确的描述方向。对于一个按照逆时针方向描述物体点位的物体而言，方向的左边都是在物体内部。则其反方向的左边则是在物体外。关于这一点，大家可以做一个圆进行验证。

如果Agent没有碰撞到障碍物（点或者线段）

        当上面一连串判断完后还未得到ORCA，则证明Agent没有碰撞到障碍物（点或者线段）。接下来，我们就要计算因此障碍物线段得到的VO边界。在源码中，这边界被称为左，右腿。实际上就是两个向量。源码定义如下：

1
RvoVector2 leftLegDirection, rightLegDirection;

源码同样分了三种情况考虑：

  1. 当s小于0，且Agent到线段最近的距离小于Agent的半径。

s小于0表示Agent更靠近obstacle1，但是Agent并没有碰撞到obstacle1，而是Agent到obstacle1线段最近的距离小于Agent的半径。因为其并没有碰撞到obstacle1，所以其并不算碰撞到obstacle1线段。但是因为线段最近的距离小于Agent的半径，所以我们下一时刻是有可能碰撞到obstacle1的。因此我们要对obstacle1做避障处理。源码中对于这个情况的边界处理如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
if (s < 0.0f && distSqLine <= radiusSq)
{
    /*
     * Obstacle viewed obliquely so that left vertex
     * defines velocity obstacle.
     */
    if (!obstacle1.convex_)
    {
        /* Ignore obstacle. */
        continue;
    }

    obstacle2 = obstacle1;

    float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
    leftLegDirection = new RvoVector2(relativePosition1.x() * leg1 - 
        relativePosition1.y() * radius_, relativePosition1.x() * radius_ + 
        relativePosition1.y() * leg1) / distSq1;
    rightLegDirection = new RvoVector2(relativePosition1.x() * leg1 + 
        relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + 
        relativePosition1.y() * leg1) / distSq1;
}

源码中这里仍然忽略了非凸点的情况。我们先暂时不管obstacle2 = obstacle1;。我们先理解左右"腿"的计算方法。根据此时的情况，我们可以绘制出如下的示意图：

PS：注意这是示意图，部分数值是我乱定的，虽然数值是乱定的但是图上的情况是符合上述条件的。

首先我们要明白在源码中Agent被视作为圆形。图中，紫色虚线圆形，其大小和Agent一致。根据前文的理论，我们为了在下一个时刻Agent不碰撞到obstacle1，则其在下一时刻的位置必然要离obstacle1至少是其半径的距离。即在下一时刻的位置Agent的位置不能在紫色虚线圆形内。图中，蓝色向量的长度就是leg1（distSq1是relativePosition1的长度平方，radiusSq是Agent半径的平方）。灰色虚线是Agent的半径，且灰色虚线垂直与两条"腿"。我们可以用相似三角形判定法来证明出两条腿的长度一致，且用勾股定理便可以证明其长度就是leg1。虽然我们知道了这些，但是我们又要如何求得这两条腿呢？我们可以使用向量旋转的知识来帮助我们求得（本人思考的时候参考的文章：求圆外一点做圆切线的切点坐标（算法））。我们可以认为“腿”是由relativePosition1旋转得来的。我们设relativePosition1于腿的夹角为θ，左腿旋转了θ，右腿则是-θ。根据旋转公式：

\[(x\cdot cosθ - y\cdot sinθ,x\cdot sinθ + y \cdot cosθ)\]

这个公式和上面的源码中的有些相似，但是还有些差距。那么从图中，我们可以得出以下结论：

\[cosθ = \frac{leg1}{\sqrt{distSq1}},sinθ = \frac{radius}{\sqrt{distSq1}}\]

即使我们将其代入到旋转公式中也和源码有些出入，即源码多除以了\(\sqrt{distSq1}\)。我们将relativePosition1进行旋转，实际得到的向量虽然和腿的方向一致，但是长度还是relativePosition1的查长度。但是这个并不重要，因为最终我们要得到的是腿的方向，而这个方向应该是单位向量。而relativePosition1的长度是就是\(\sqrt{distSq1}\)。所以最终源码多除以了\(\sqrt{distSq1}\)以获取到腿的方向。

  1. 当s大于1，且Agent到线段最近的距离小于Agent的半径。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
if (s > 1.0f && distSqLine <= radiusSq)
{
    /*
     * Obstacle viewed obliquely so that
     * right vertex defines velocity obstacle.
     */
    if (!obstacle2.convex_)
    {
        /* Ignore obstacle. */
        continue;
    }

    obstacle1 = obstacle2;

    float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
    leftLegDirection = new RvoVector2(relativePosition2.x() * leg2 - 
        relativePosition2.y() * radius_, relativePosition2.x() * radius_ + 
        relativePosition2.y() * leg2) / distSq2;
    rightLegDirection = new RvoVector2(relativePosition2.x() * leg2 + 
        relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + 
        relativePosition2.y() * leg2) / distSq2;
}

这个情况实际上和上一个情况差不多，有所区别的地方在于s大于1。这说明Agent更靠近obstacle2。所以后面的处理就围绕着obstacle2进行。

  1. 剩余的其他情况
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
if (obstacle1.convex_)
{
    float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
    leftLegDirection = new RvoVector2(relativePosition1.x() * leg1 - 
        relativePosition1.y() * radius_, relativePosition1.x() * radius_ + 
        relativePosition1.y() * leg1) / distSq1;
}
else
{
    /* Left vertex non-convex; left leg extends cut-off line. */
    leftLegDirection = -obstacle1.direction_;
}

if (obstacle2.convex_)
{
    float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
    rightLegDirection = new RvoVector2(relativePosition2.x() * leg2 + 
        relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + 
        relativePosition2.y() * leg2) / distSq2;
}
else
{
    /* Right vertex non-convex; right leg extends cut-off line. */
    rightLegDirection = obstacle1.direction_;
}

此时我们考虑整个线段的情况，源码中是左腿以obstacle1为标准，右腿以obstacle2为标准。关于这点我认为是因为正确的描述点位顺序是逆时针（以x向右为正方向）进行的，所以障碍物线段是以obstacle1为最左边，而obstacle2最右边。如果obstacle1和obstacle2是凸点。则根据之前的理论，我们只要考虑着两点做边界就是了。按照之前的理论，我们要对线段中所有的点做一个闵可夫斯基和。如下图所示：

图中，被紫色虚线包裹的范围就是我们求出来的闵可夫斯基和。源码对于它们为凹点的情况做了非常保守的处理，即尽可能的远离凹点区域。如果两边都是凹点，则左右腿在一条直线上。

        在上面的情况都处理完后，源码中还对左右腿做了额外的处理。源码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
Obstacle leftNeighbor = obstacle1.previous_;

bool isLeftLegForeign = false;
bool isRightLegForeign = false;

if (obstacle1.convex_ && RVOMath.det(leftLegDirection, -leftNeighbor.direction_) >= 0.0f)
{
    /* Left leg points into obstacle. */
    leftLegDirection = -leftNeighbor.direction_;
    isLeftLegForeign = true;
}

if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= 0.0f)
{
    /* Right leg points into obstacle. */
    rightLegDirection = obstacle2.direction_;
    isRightLegForeign = true;
}

源码中，对这段代码的注释：Legs can never point into neighboring edge when convex vertex, take cutoff-line of neighboring edge instead. If velocity projected on "foreign" leg, no constraint is added.。中文翻译：当为凸点时，腿不能指向邻边，而是以邻边的切线代替。如果速度投影在了“外”腿上，则不添加这个约束。。

        关于“外”腿的说法则是后面计算ORCA，所需要关心的，这里我们只要知道“外”腿和邻边是一个东西（这点，你看源码就可以看得出来）。这段代码主要是来描述：“当为凸点时，腿不能指向邻边，而是以邻边的切线代替”。这里多了“邻边”和“切线”，这两个概念。一开始我看不懂这段代码，也不懂这注释。而问题的根源就是我对于邻边（neighboring edge）这个概念的误解。从源码上看邻边指的是obstacle1到前一个点位的方向和obstacle2到下一个点位的方向。下图是一种可能出现的情况：

此图的情况是Agent先找到了B点，然后进行处理发现其并没有和自身碰撞，且C点更靠近自己。即无碰撞时，当s大于1，且Agent到线段最近的距离小于Agent的半径。。C，B点都是在矩形ABCD上的点位，其中CD平行于y轴且与y轴反向，CB平行于x轴且和x轴反向。按照这种情况来看，CB即为源码中的-leftNeighbor.direction_，CD则对应源码中的obstacle2.direction_，也就是CB和CD就是我们所说的邻边。我们对其进行左右边的判断，我们会发现CB是在leftLegDirection的右边，而CD则是在rightLegDirection的左边，也就是说上面源码中做的处理都没必要。此时我们想象一下如果要符合源码中对其进行的处理，这就意味着leftLegDirection和rightLegDirection所描述的VO范围会扩大。你会发现如果我们仅考虑C点，而不考虑C点的邻边。我们就无法保证我们选择的速度是否会在下一个时刻碰撞到C点的邻边。因此我认为这样处理后，我们就无需担心我们选中的速度会在某个时刻碰撞到C点的两个邻边。

        自此左右腿的处理都已经结束。在计算出左右腿后，我们还要额外获取一些信息。然后在速度坐标系中进行比较。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
/* Compute cut-off centers. */
/*中文翻译：计算截止中心。*/ 
RvoVector2 leftCutOff = invTimeHorizonObst * (obstacle1.point_ - position_);
RvoVector2 rightCutOff = invTimeHorizonObst * (obstacle2.point_ - position_);
RvoVector2 cutOffVector = rightCutOff - leftCutOff;

/* Project current velocity on velocity obstacle. */
/*中文翻译：投影当前速度在VO上*/

/* Check if current velocity is projected on cutoff circles. */
/*中文翻译：检测当前速度是否被投影在截止环上*/
float t = obstacle1 == obstacle2 ? 
    0.5f : ((velocity_ - leftCutOff) * cutOffVector) / RVOMath.absSq(cutOffVector);
float tLeft = (velocity_ - leftCutOff) * leftLegDirection;
float tRight = (velocity_ - rightCutOff) * rightLegDirection;

invTimeHorizonObst是障碍物时间的倒数（invTimeHorizonObst = 1.0f / timeHorizonObst_;）。通过乘以时间倒数的方式，所有的信息就从位置坐标系到速度坐标系中(再次声明，本文所有的图都会在位置坐标系下，所有的速度都默认视为乘上时间后得到的值)。从此刻开始，所有的分析都在速度坐标系中。leftCutOff和rightCutOff实际上就是之前计算出来的relativePosition转换到速度坐标系的结果。但是我们要注意的是，在之前的判断中，对于单点的避免（即带有distSqLine <= radiusSq的前两个判断），obstacle1和obstacle2数值相等，此时cutOffVector为0。在考虑两边节点的时候，cutOffVector即为(obstacle2.point_ - obstacle1.point_) * invTimeHorizonObst。t值是一个重要的信息，它关乎后面的判断和计算。从代码中可以看得出来，t值是velocity_ - leftCutOff在cutOffVector投影值和cutOffVector的比值。如其上面注释所描述:检测当前速度是否被投影在截止环上。在obstacle1 == obstacle2时，t等于0.5，但是这并不是默认其在截止环上，实际上根据源码来看t在[0,1]之间任取一个实数都可以。tLeft

        在进行接下来的源码分析之前，我们再回顾一下之前的知识。我们要找到由此障碍物形成的VO边界上离首选速度最近的一个点，而ORCA就是这个点在VO上的切线。本来这里还应该补一句“我们令首选速度为0”。但是我们代码中可以看得出来，源码并没有将首选速度设定为0，反而是用它继续进行计算。现在我们再回来看源码。为了求得VO边界上离首选速度最近的点，源码又分出了两种情况来：

  1. 最近的点在VO的圆弧部分

判断的源码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f))
{
    /* Project on left cut-off circle. */
    RvoVector2 unitW = RVOMath.normalize(velocity_ - leftCutOff);

    line.direction = new RvoVector2(unitW.y(), -unitW.x());
    line.point = leftCutOff + radius_ * invTimeHorizonObst * unitW;
    orcaLines_.Add(line);

    continue;
}
else if (t > 1.0f && tRight < 0.0f)
{
    /* Project on right cut-off circle. */
    RvoVector2 unitW = RVOMath.normalize(velocity_ - rightCutOff);

    line.direction = new RvoVector2(unitW.y(), -unitW.x());
    line.point = rightCutOff + radius_ * invTimeHorizonObst * unitW;
    orcaLines_.Add(line);

    continue;
}

上面代码所展示的情况如下图所示：

图中紫色斜线部分是对应VO，黄色透明部分则是符合源码中第一个if的情况，蓝色透明部分则是对应第二个if的情况。黄色透明部分和蓝色透明部分都有一个相同的情况：他们都带有一段圆弧，且都在一个圆内。那么此时VO边界上离速度最近的点位必然在这些圆弧上，则以离此圆弧最近的圆心到速度的方向（这里设为unitW）和圆弧相交的交点就是我们要求的点位。而其切线就是垂直与unitW的直线。因为我们要求所有被允许的速度应该在ORCA线的右边，于是line.direction = new RvoVector2(unitW.y(), -unitW.x());。

  1. 最近的点在VO的左腿、右腿或切线上

源码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
float distSqCutoff = 
    (t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? 
    float.PositiveInfinity : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));

float distSqLeft = tLeft < 0.0f ? 
    float.PositiveInfinity : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));

float distSqRight = tRight < 0.0f ? 
    float.PositiveInfinity : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection));

if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
    /* Project on cut-off line. */
    line.direction = -obstacle1.direction_;
    line.point = leftCutOff + 
        radius_ * invTimeHorizonObst * new RvoVector2(-line.direction.y(), line.direction.x());
    orcaLines_.Add(line);

    continue;
}

if (distSqLeft <= distSqRight)
{
    /* Project on left leg. */
    if (isLeftLegForeign)
    {
        continue;
    }

    line.direction = leftLegDirection;
    line.point = leftCutOff + 
        radius_ * invTimeHorizonObst * new RvoVector2(-line.direction.y(), line.direction.x());
    orcaLines_.Add(line);

    continue;
}

/* Project on right leg. */
if (isRightLegForeign)
{
    continue;
}

line.direction = -rightLegDirection;
line.point = rightCutOff + 
    radius_ * invTimeHorizonObst * new RvoVector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);

distSqCutoff、distSqLeft和distSqRight分别表示首选速度到切线的距离平方，首选速度到左腿的距离平方和首选速度到右腿的距离平方。这里取距离的平方是为了简化计算。这里对于距离的计算也有限制。如果不存在这个距离，则直接设定为正无穷大。如同源码中在单节点的情况下是不存在所谓的切线的，于是首选速度到切线的距离平方被设定为无穷大。除此之外被设定为正无穷大，我个人认为是因为此时速度在其上的投影向量和对应的向量的方向相反。这也说明此时速度的投影并不在对应的边界上。其实关于这个正无穷大的设定，我是有几个想法，但是我认为这个想法更合理一些。因为从切线正无穷大的另一个设定't < 0.0f || t > 1.0f '，源码还是有在判断是否一定要在边界上。计算距离方法就是先求出速度在其对应向量上的投影向量，然后让速度减去这个投影向量便得到了，速度垂直于对应向量的垂直向量。最后计算这个垂直向量的距离。这里仍然忽略了当边界是由凹点形成的情况。

对于周围其他Agent的分析

        逻辑处理完静态物体后，便是处理其他Agent。其与静态物体一样也被分成了两种情况，已经碰撞了和没有碰撞到。下面是在判断前的数据处理的源码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
// 获取周围的其他Agent
Agent other = agentNeighbors_[i].Value;

// 相对位置
RvoVector2 relativePosition = other.position_ - position_;
// 相对速度
RvoVector2 relativeVelocity = velocity_ - other.velocity_;
// 相对的距离平方
float distSq = RVOMath.absSq(relativePosition);
// 两个Agent的半径和
float combinedRadius = radius_ + other.radius_;
// 两个Agent的半径和平方
float combinedRadiusSq = RVOMath.sqr(combinedRadius);

Line line;
RvoVector2 u;

在后面的文章中，其他Agent统一称为Other。下文源码中出现的invTimeHorizon表示Agent间计算时间的倒数，即\(\frac{1}{τ}\)。

没碰撞到其他Agent

        当distSq > combinedRadiusSq时，此刻Agent和Other并没有发生碰撞。接下来获取在VO边界上离速度差最近的点在VO的哪一个部分。然后求速度差到这个点位的向量u。因为是Agent间的碰撞，所以责任要一半。代入我们之前在理论中提过的公式：

\[ORCA^{τ}_{Agent|Other} = \{v|(v-(v^{opt}_{Agent} + \frac{1}{2}u))\cdot n >= 0\}\]

1
2
3
4
5
6
/* No collision. */
RvoVector2 w = relativeVelocity - invTimeHorizon * relativePosition;

/* Vector from cutoff center to relative velocity. */
float wLengthSq = RVOMath.absSq(w);
float dotProduct1 = w * relativePosition;

源码这里invTimeHorizon * relativePosition;是将位置坐标系的信息转换到速度坐标系中。w是相对速度减去其的值。wLengthSq是w距离的平方，dotProduct1展示出w和相对位置的投影关系。之后源码进行以下的判断：

1
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)

这段代码是判断最近的点是否在VO的圆弧边界上。下面是我绘制的示意图：

因为我们要以Agent为原点，所以invTimeHorizon * relativePosition即为Other在以Agent为原点时Other在速度坐标系上的点位。因此w就是Other到相对速度到的向量。图中从A点到Other的向量就是relativePosition。此时dotProduct1 < 0.0f，所以relativeVelocity只能在蓝色的部分。而条件的另外一个RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq是用来判断relativeVelocity在w上的投影长度是否大于combinedRadiusSq。单纯看这段代码可能很难看得出来，但是我们只要变换一下公式就可以得出了：

\[\begin{aligned} dotProduct1 &= w \cdot \vec{relativePosition} \\&= \sqrt {wLengthSq} * |\vec{relativePosition}| * \cos \end{aligned}\]

我们知道relativePosition的长度，乘上其与w的夹角cos值便是relativeVelocity在w上的投影长度。源代码对其进行了平分处理，并让combinedRadiusSq多乘以wLengthSq来达到判断relativeVelocity在w上的投影长度是否大于combinedRadiusSq的目的。那么为何是只要大于combinedRadiusSq即为在VO边界的圆弧上呢？上图中紫色区域的部分是VO，其中A切与VO边界的C和D点。若此时relativeVelocity在Other到C（或者是）的向量上，则relativeVelocity在w上的投影长度为combinedRadiusSq。我们又知道此时dotProduct1 < 0.0f，所以relativePosition与w在(90°,180°]范围中。又因为进行了平分计算，所以我们可以得出RVOMath.sqr(dotProduct1) / wLengthSq随着角度变大会越来越大。从图中来看大于combinedRadiusSq的角度正好在VO边界的弧度上。图中我多做了两个I和J的投影效果也符合我们的结论。现在我们知道边界上的点位就可以来求得ORCA了，源码如下：

1
2
3
4
5
6
7
/* Project on cut-off circle. */
float wLength = RVOMath.sqrt(wLengthSq);
RvoVector2 unitW = w / wLength;

line.direction = new RvoVector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
line.point = velocity_ + 0.5f * u;

如果此时最近点位并不在圆弧上，那么就是在VO的两个腿上。源码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/* Project on legs. */
float leg = RVOMath.sqrt(distSq - combinedRadiusSq);

if (RVOMath.det(relativePosition, w) > 0.0f)
{
    /* Project on left leg. */
    line.direction = 
        new RvoVector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, 
        relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
else
{
    /* Project on right leg. */
    line.direction = 
        -new RvoVector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius,
         -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}

float dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
line.point = velocity_ + 0.5f * u;

这段代码是先用叉乘求得应该在哪条腿上，然后用前文提到的方法求出腿的方向。最后求得u。

碰撞到其他Agent

源码如下：

1
2
3
4
5
6
7
8
9
10
11
12
/* Collision. Project on cut-off circle of time timeStep. */
float invTimeStep = 1.0f / Simulator.Instance.timeStep_;

/* Vector from cutoff center to relative velocity. */
RvoVector2 w = relativeVelocity - invTimeStep * relativePosition;

float wLength = RVOMath.abs(w);
RvoVector2 unitW = w / wLength;

line.direction = new RvoVector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeStep - wLength) * unitW;
line.point = velocity_ + 0.5f * u;

当Agent和Other碰撞的时候，源码中直接认为VO为在Other位置上以combinedRadius为半径的圆形。然后用这个特别的VO按照前文所述的方法求得u并最终算出对应的ORCA。

分析ORCA求得最终的速度

        当Agent遍历完所有的静态障碍物和其他的Agent后，我们获得了一系列的ORCA。那么接下来的操作就是我们从这些ORCA求得最靠近预期速度的速度。源码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);

private int linearProgram2(IList<Line> lines, float radius, RvoVector2 optVelocity, bool directionOpt, ref RvoVector2 result)
{
    if (directionOpt)
    {
        /*
         * Optimize direction. Note that the optimization velocity is of
         * unit length in this case.
         */
        result = optVelocity * radius;
    }
    else if (RVOMath.absSq(optVelocity) > RVOMath.sqr(radius))
    {
        /* Optimize closest point and outside circle. */
        result = RVOMath.normalize(optVelocity) * radius;
    }
    else
    {
        /* Optimize closest point and inside circle. */
        result = optVelocity;
    }

    for (int i = 0; i < lines.Count; ++i)
    {
        if (RVOMath.det(lines[i].direction, lines[i].point - result) > 0.0f)
        {
            /* Result does not satisfy constraint i. Compute new optimal result. */
            RvoVector2 tempResult = result;
            if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
            {
                result = tempResult;

                return i;
            }
        }
    }

    return lines.Count;
}

源码中通过linearProgram2函数来确定给定的速度是否满足所有的ORCA的限制。首先我们先尝试预期速度是否满足条件。从源码上看linearProgram2在判断速度是否满足所有的ORCA的限制前，先做了一些限制。这里我们先不讨论。我们先看其是如何进行判断。关键的代码在于if (RVOMath.det(lines[i].direction, lines[i].point - result) > 0.0f)。这就是我们之前所说的，所有的速度都应该在ORCA线方向的左边。如果不满足其中一个ORCA则进入linearProgram1函数。（之后关于linearProgram1函数和linearProgram3函数的分析，如果你觉得我讲述的不够明白可以看基于 DOTS 的 ORCA(RVO2) 群体避障算法原理及实现这篇文章，我也是从中理解的。特别是linearProgram3函数的分析。）linearProgram1函数源码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
private bool linearProgram1(IList<Line> lines, int lineNo, float radius, RvoVector2 optVelocity, bool directionOpt, ref RvoVector2 result)
{
    float dotProduct = lines[lineNo].point * lines[lineNo].direction;
    float discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point);

    if (discriminant < 0.0f)
    {
        /* Max speed circle fully invalidates line lineNo. */
        return false;
    }

    float sqrtDiscriminant = RVOMath.sqrt(discriminant);
    float tLeft = -dotProduct - sqrtDiscriminant;
    float tRight = -dotProduct + sqrtDiscriminant;

    for (int i = 0; i < lineNo; ++i)
    {
        float denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
        float numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point);

        if (RVOMath.fabs(denominator) <= RVOMath.RVO_EPSILON)
        {
            /* Lines lineNo and i are (almost) parallel. */
            if (numerator < 0.0f)
            {
                return false;
            }

            continue;
        }

        float t = numerator / denominator;

        if (denominator >= 0.0f)
        {
            /* Line i bounds line lineNo on the right. */
            tRight = Math.Min(tRight, t);
        }
        else
        {
            /* Line i bounds line lineNo on the left. */
            tLeft = Math.Max(tLeft, t);
        }

        if (tLeft > tRight)
        {
            return false;
        }
    }

    if (directionOpt)
    {
        /* Optimize direction. */
        if (optVelocity * lines[lineNo].direction > 0.0f)
        {
            /* Take right extreme. */
            result = lines[lineNo].point + tRight * lines[lineNo].direction;
        }
        else
        {
            /* Take left extreme. */
            result = lines[lineNo].point + tLeft * lines[lineNo].direction;
        }
    }
    else
    {
        /* Optimize closest point. */
        float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point);

        if (t < tLeft)
        {
            result = lines[lineNo].point + tLeft * lines[lineNo].direction;
        }
        else if (t > tRight)
        {
            result = lines[lineNo].point + tRight * lines[lineNo].direction;
        }
        else
        {
            result = lines[lineNo].point + t * lines[lineNo].direction;
        }
    }

    return true;
}

我们分段来看这段代码。首先是下面这一段：

1
2
3
4
5
6
7
8
float dotProduct = lines[lineNo].point * lines[lineNo].direction;
float discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point);

if (discriminant < 0.0f)
{
    /* Max speed circle fully invalidates line lineNo. */
    return false;
}

初看这段代码，有些让人摸不着头脑。但是我们进行一些变换就可以找到一些头绪。上面的point原本代指位置，这里代指从原点到point位置的向量。则上面这段代码，我们可以用下面的公式表示：

\[\begin{aligned} dotProduct &= \vec{point} \cdot \vec{direction} \\&= |\vec{point}|* |\vec{direction}| * \cos \end{aligned}\]

\[\begin{aligned} discriminant &= dotProduct^2 + radius^2 - (|\vec{point}|)^2 \\&= (|\vec{point}|)^2 *(|\vec{direction}|)^2 * \cos^2 + radius^2 - (|\vec{point}|)^2 \\&= (|\vec{point}|)^2 *(|\vec{direction}|)^2 * \cos^2 + radius^2 - (|\vec{point}|)^2 \\&= (|\vec{point}|)^2 *\{(|\vec{direction}|)^2 * \cos^2 - 1 \} + radius^2 \end{aligned}\]

从前文中来看，direction一定是一个单位向量。如果大家忘记了，可以看一下源码，虽然源码中没有强制direction一定是一个单位向量，但是整个过程中的验算其实都保证了这一点。所以最后的公式变为：

\[\begin{aligned} discriminant &= (|\vec{point}|)^2 *(\cos^2 - 1 ) + radius^2 \\&= -(|\vec{point}|)^2 * \sin^2 + radius^2 \end{aligned}\]

经过这样的变换后，我们便可发现，这段代码的意义是比较从原点到这条线的最短距离是否大于最大速度（此时的radius就是最大速度）。此时我们的首选速度已经不满足这条线的限制条件了。如果我们离它的最短距离超过最大速度，那么我们就不可能找到一个在最大速度内的速度满足它的限制条件。如果此线段满足条件就执行下面这段代码：

1
2
3
float sqrtDiscriminant = RVOMath.sqrt(discriminant);
float tLeft = -dotProduct - sqrtDiscriminant;
float tRight = -dotProduct + sqrtDiscriminant;

这里的说明，我们结合下图进行分析。

图中点p是lines[lineNo].point，红色的向量表示lines[lineNo].direction。点D是点A垂直与lines[lineNo]的交点。点F和点G是以A为圆心的圆（半径为radius）和lines[lineNo]的交点。A点在原点。因为dotProduct = lines[lineNo].point * lines[lineNo].direction;，且lines[lineNo].direction是单位向量。所以根据点积的公式dotProduct的大小对应着图中线段PD的长度。

\[PD = |dotProduct|\]

discriminant在我们之前的公式化简中其等于

\[discriminant = radius^2 -(|\vec{point}|)^2 * \sin^2\]

图中线段AP的长度等于lines[lineNo].point的长度，所以lines[lineNo].point乘上与直线方向夹角的sin值的长度，即为图中线段AD的长度。故

\[AD^2 = (|\vec{point}|)^2 * \sin^2\]

我们知道AD垂直于直线Line。那么三角形ADG为直角三角形，又因为G是圆上的交点，所以\(AG=radius\)根据勾股定理则有以下公式:

\[\begin{aligned} DG^2 &= AG^2 - AD^2 \\&= radius^2 -(|\vec{point}|)^2 * \sin^2 \\&=discriminant \end{aligned}\]

所以sqrtDiscriminant的大小就是图中DG的长度。通过相似三角形判定法，我们很容易就可以得出DG和DF长度相等。现在我们结合图片进行分析。从图片来看此时lines[lineNo].point和lines[lineNo].direction的点积为负值。所以：

\[PD = -dotProduct\]

我们再结合前面的公式可得到下面两个公式：

\[\begin{aligned} & tLeft = PD - GD \\&tRight = PD + DF \end{aligned}\]

我们再结合一下上面的图像，你会发现tLeft绝对值等于线段PG的大小，tRight的大小等于线段PF的大小。且向量PG刚好在向量AP的左边，向量PF在向量AP的右边。它们都在直线line上，向量PG与line方向相反，向量PF则相同。图中我还多绘制了p点的其他情况，p1，p2。大家也可以自行验证。现在我们先记下这一点，继续我们的分析。

1
2
3
4
5
6
7
8
9
10
11
12
13
float denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
float numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point);

if (RVOMath.fabs(denominator) <= RVOMath.RVO_EPSILON)
{
    /* Lines lineNo and i are (almost) parallel. */
    if (numerator < 0.0f)
    {
        return false;
    }

    continue;
}

denominator是lines[i].direction和lines[lineNo].direction的叉积，numerator则是lines[i].direction和当前ORCA点位到lines[lineNo].point的叉积。如果denominator的绝对值小于给定的极小值RVOMath.RVO_EPSILON，则表明这两个方向的夹角度数要么接近0°，要么接近180°。则这两个ORCA接近平行。此时如果numerator < 0.0f，则表明lines[lineNo]在lines[i]朝向的右边。现在我们假设他们就是平行的，对这个情况进行分类讨论：

  1. lines[lineNo]和lines[i]朝向一致。如下图所示

那么lines[lineNo]包含了lines[i]的范围，既然lines[i]是合理的，那么lines[lineNo]也应该是合理的，不可能满足进入此函数的条件。

  1. lines[lineNo]和lines[i]朝向相反。如下图所示

虽然此时满足了进入函数的条件，但是lines[lineNo]和lines[i]的范围也不存在交集。则表示不存在一个速度其既在lines[lineNo]所描述的范围中，也在lines[i]所描述的范围中。

        综上所述，如果出现了这样的情况只能是无法算出速度的情况，故就直接返回false。如果此时lines[lineNo]在lines[i]朝向的左边，lines[lineNo]和lines[i]的范围都可能存在交集。但是在接下来的运算中，存在float t = numerator / denominator;，当在几乎平行的情况下计算会存在问题。且既然知道这条线和lines[lineNo]存在交集，那我们也可以不继续进行验算。

1
float t = numerator / denominator;

在分析这代码前，我们先看下图:

如果不是平行，那么这两个ORCA必然会存在一个交点，正如上图的O点。向量OC和向量OB分别代表单位向量lines[i].direction和lines[lineNo].direction。点\(p_i\)和点\(p_{lineNo}\)分别表示lines[i].point和lines[lineNo].point。线段BD垂直lines[i]于D点，线段\(p_{lineNo}E\)垂直lines[i]于E点。如果不考虑正负，只单纯考虑大小。我们可以得到以下的关系：

\[ BD = |denominator| \\ p_{lineNo}E = |numerator| \]

结合图像，因为两个垂直线，结合相似三角形判定方法，我们得出下面的关系：

\[\triangle DOB \simeq \triangle Op_ip_{lineNo}\]

因为三角形相似，我们又可以得到下面的关系:

\[ \frac{p_{lineNo}O}{BO} = \frac{p_{lineNo}E}{BD} \]

因为BO代表的是单位向量lines[lineNo].direction，所以BO大小为1则关系式变为：

\[ p_{lineNo}O = \frac{p_{lineNo}E}{BD} \]

所以代码中t表示lines[lineNo].point到交点的大小。接下来这段代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
if (denominator >= 0.0f)
{
    /* Line i bounds line lineNo on the right. */
    tRight = Math.Min(tRight, t);
}
else
{
    /* Line i bounds line lineNo on the left. */
    tLeft = Math.Max(tLeft, t);
}

if (tLeft > tRight)
{
    return false;
}

通过denominator来判断lines[i]和lines[lineNo]的位置关系进而更新lines[lineNo]的左右边界值。如果出现左界值大于右界值则表明无法得到一个有效的交集。在循环结束后，我们可以得到一个左右的边界值。接下来就是计算新的速度：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
if (directionOpt)
{
    /* Optimize direction. */
    if (optVelocity * lines[lineNo].direction > 0.0f)
    {
        /* Take right extreme. */
        result = lines[lineNo].point + tRight * lines[lineNo].direction;
    }
    else
    {
        /* Take left extreme. */
        result = lines[lineNo].point + tLeft * lines[lineNo].direction;
    }
}
else
{
    /* Optimize closest point. */
    float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point);

    if (t < tLeft)
    {
        result = lines[lineNo].point + tLeft * lines[lineNo].direction;
    }
    else if (t > tRight)
    {
        result = lines[lineNo].point + tRight * lines[lineNo].direction;
    }
    else
    {
        result = lines[lineNo].point + t * lines[lineNo].direction;
    }
}

这段代码将新速度设为在一定在lines[lineNo]上。主要的思想就是计算之前的速度在lines[lineNo]投影值。如果是directionOpt == true，那么根据速度和线段方向的投影值的正负关系选择交集的极左值和极右值。如果是directionOpt == false，那么根据速度和lines[lineNo].point的差值在lines[lineNo]上的投影值。并且选取最靠近速度的一个。

        如果经过了linearProgram1函数之后，我们仍然可以得到一个满足所有ORCA的速度，那么我们就无需进入linearProgram3函数。但是实际上，我们仍然有可能进入linearProgram3函数。

1
2
3
4
5
6
int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);

if (lineFail < orcaLines_.Count)
{
    linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
}

如果linearProgram2在计算途中离开，即其没有循环遍历完所有的ORCA，则进入linearProgram3函数，并且将失败的ORCA的下标传入。对于linearProgram3函数，网上对其的分析十分的少见。而且部分的分析我感觉是有些的问题。实际上，我个人能理解linearProgram3函数是如何做的，但是我不知道它这样做的理由。所以下面的分析就只是尽量说明我个人的理解。源码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
private void linearProgram3(IList<Line> lines, int numObstLines, int beginLine, float radius, ref RvoVector2 result)
{
    float distance = 0.0f;

    for (int i = beginLine; i < lines.Count; ++i)
    {
        if (RVOMath.det(lines[i].direction, lines[i].point - result) > distance)
        {
            /* Result does not satisfy constraint of line i. */
            IList<Line> projLines = new List<Line>();
            for (int ii = 0; ii < numObstLines; ++ii)
            {
                projLines.Add(lines[ii]);
            }

            for (int j = numObstLines; j < i; ++j)
            {
                Line line;

                float determinant = RVOMath.det(lines[i].direction, lines[j].direction);

                if (RVOMath.fabs(determinant) <= RVOMath.RVO_EPSILON)
                {
                    /* Line i and line j are parallel. */
                    if (lines[i].direction * lines[j].direction > 0.0f)
                    {
                        /* Line i and line j point in the same direction. */
                        continue;
                    }
                    else
                    {
                        /* Line i and line j point in opposite direction. */
                        line.point = 0.5f * (lines[i].point + lines[j].point);
                    }
                }
                else
                {
                    line.point = lines[i].point + 
                        (RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / 
                            determinant) * lines[i].direction;
                }

                line.direction = RVOMath.normalize(lines[j].direction - lines[i].direction);
                projLines.Add(line);
            }

            RvoVector2 tempResult = result;
            if (linearProgram2(projLines, radius, new RvoVector2(-lines[i].direction.y(), lines[i].direction.x()), true, ref result) < projLines.Count)
            {
                /*
                 * This should in principle not happen. The result is by
                 * definition already in the feasible region of this
                 * linear program. If it fails, it is due to small
                 * floating point error, and the current result is kept.
                 */
                result = tempResult;
            }

            distance = RVOMath.det(lines[i].direction, lines[i].point - result);
        }
    }
}

关于distance，首先是有关其的判断代码RVOMath.det(lines[i].direction, lines[i].point - result) > distance。当初始distance为0的时候，那这个就是在linearProgram2函数判断速度是否满足ORCA限定条件的判断。其次在每次计算的最后用新得到的line对distance进行了更新。所以distance的作用应该是从失败的下标开始，用来判断现在的速度不满足那些线的限定条件。然后这些线要进行改造并计算一个新的速度，最终得到一个新的distance。如果接下来的线的叉积计算比distance小，那么现在的速度必然满足这条线的条件，则无需进行之后的计算。

        综上所述，distance的作用有两个，一个是找到现在的速度满足不了的线段，另一个是减少不必要的计算。

1
2
3
4
5
IList<Line> projLines = new List<Line>();
for (int ii = 0; ii < numObstLines; ++ii)
{
    projLines.Add(lines[ii]);
}

linearProgram3函数中这一段是将障碍物的ORCA先加入到projLines中。之后的代码中从非障碍物的ORCA开始一个个改造为新的line并加入projLines中。而改造line就是这个函数的核心逻辑。接下来源码中使用了和linearProgram1函数一样的方法来判断两条线是否平行。不一样的地方在于，这里使用了点积的方式来判断两条线是否同向。如果同向，则忽视。因为之后存在两个方向互相相减，它们又都是单位向量。从计算的角度来说，这样会在求归一化时导致非法运算。如果方向相反，则点位选取两条线的点位的中心位置。

1
2
3
line.point = lines[i].point + 
                (RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / 
                    determinant) * lines[i].direction;

而这段代码就是和linearProgram1函数一样求出交点的位置。最后是决定新line的方向。

1
line.direction = RVOMath.normalize(lines[j].direction - lines[i].direction);

对于非平行的线段而言，这个方向是取了它们的角平分线。最后使用垂直与lines[i].direction的方向进行linearProgram2。

关于linearProgram3分析中，个人的疑惑

        我并非在网上没有找到关于linearProgram3函数的讲解。在我理解RVO2时找到的两篇参考文章：基于 DOTS 的 ORCA(RVO2) 群体避障算法原理及实现和rvo动态避障算法源码分析，都有对其的讲解和说明。特别是rvo动态避障算法源码分析的讲解更为详细。但是我个人不能理解他们的说法，实际上我感觉他们的说描述的图像和实际源码中的情况有些差别。所以我并没有复述他们的说法，只说明了一些我自己能理解的地方。下面是我的一点想法。

上面这张图满足触发linearProgram3函数的条件。我们以蓝色这条作为lines[i]，便得到以下的图像。

其中向量a和b是求得角平分线的向量，是新的line。它们的交集就是图中蓝色阴影区域的面积，而红色阴影区域是之前的黄色向量和红色向量相交的区域。在我画完图后，我感觉这linearProgram3就是扩大了速度选择的区域，并选择一个尽量靠近lines[i]允许区域的速度。

参考文章

闲言碎语

        本来这篇文章的篇幅应该更长一些的。因为我一开始是打算加入Unity上的实操、实际中遇到的问题与其解决方法，来和现在网络上的文章进行区分。然后引申出如何与全局寻路算法结合等等。但是这文章篇幅太长了，算上我翻译RVO2英文论文的时间。关于RVO2相关的文章梳理已经花费了我3个月的时间去写了。在这3个月的时间内，我苦苦挣扎于相关原理的实现，简直是遭受了英语和数学的双重折磨。又因为我找不到部分资料，很多东西都只能靠我自己去猜。我也实在难保证我自己的理解是否正确，而且我觉得我自己写出来的东西和网络上的并无差别。但是我已经写了这么多了，无论我理解的是否正确都是我自己个人理解，无论是否和网络上的文章有所差别其中也包含了我自己的思考。这文章也应该到了要完篇的时候，因此就到这吧。我实在是累的不行了。