ORCA避障算法图解

发表于 更新于 本文字数： 10k 阅读时长 ≈ 38 分钟

算法图解

ORCA避障算法，出自2011年的一篇论文《Optimal Reciprocal Collision Avoidance》。该算法的思想不算复杂，但实现上有很多细节需要注意。网上已有很多对该算法的讲解，但是大多都比较粗略，很多细节并未详解，缺少很多图解来帮助理解，因此本文着重通过图解的形式，辅以文字，来剖析该算法的思想，并对关键源码进行解释。

速度障碍（VO, Velocity Obstacle）

如图所示，假设存在A、B两个对象，他们以图中的速度（速度是向量，包含方向和模大小）运动。

两个即将碰撞的对象

为了清晰判断他们接下来是否会碰撞，我们进行三步转换：

  • 将坐标系转换成以A为原点的相对坐标
  • 将A的半径叠加到B上，A就可以抽象成一个点
  • 用A的速度向量减去B的速度向量，得到相对速度，如下图所示

由于目前的坐标系还处于距离空间，这里的速度只需要关注方向，不需要关注模大小。转换后，我们可以大致得出，A和B的相对速度确实可能在不久的将来使它们相撞。

转为相对距离和相对速度

我们定义一个时间窗口τ，用来表示τ时间内，A和B是否会发生碰撞。上图得到的相对坐标（圆心）和叠加圆半径都需要除以τ，从而转换到速度空间，下图所示的是τ=2时的图例。我从速度坐标原点引出这个圆的切线，可以围成图中所示的灰色截断圆锥，这个截断圆锥就是相对速度的“禁区”，也叫速度障碍VO，代表如果A和B的相对速度落在这个区域，那么在τ=2秒内，两者必然发生碰撞。

转换到速度空间

需要强调下将相对位置和叠加半径除以τ转换为速度空间这一步。这一步不理解，算法就无法真正掌握。
$$
\bm{V}_{relativePosition}^τ=\cfrac{\bm{P}_B-\bm{P}_A}{τ}
$$
$$
R_{combine}^τ=\cfrac{(R_A+R_B)}{τ}
$$
下图所示，是不同τ对应的速度障碍，τ取值不同，圆盘的位置也会不同，但是总是会与两侧的切线相切。τ作为分母，如果我们取很大的值，圆盘就会无限逼近原点；如果我们取很小的值，圆盘就会无限远离，如下图所示。

举个例子，如果你的游戏世界全是蜗牛，它们的速度很慢，时间窗口就会设置的大一些，比如设置10秒，VO圆盘就会非常接近原点，蜗牛极慢的速度，才有可能进入VO，从而提前修正碰撞。如果设置时间窗口很小，比如0.1秒，那这个圆盘会推向极远的位置，大多数时间下彼此尚有一定距离时，蜗牛本就极慢的速度是碰不到VO的，只有在两个蜗牛相距非常非常近，快贴在一起的时候，相对速度才会进入VO，此时才感知到彼此的存在，进行避让，但这个避让看起来倒不如说更像是碰到一起弹开了。

所以，理解完后如果一定要明确定义它们的具体含义：你从原点以$\bm{V}_{relatvePosition}^τ$的速度运动，τ秒时最终可以抵达$\bm{P}_{relative}$；你从$\bm{P}_{relative}$以$R_{combine}^τ$的模速度向任意方向运动，τ时间后，你总能抵达叠加圆的边界。

不同τ对应的VO

ORCA半平面

现在，我们根据A和B以及设定的τ获取了VO，如何进行碰撞避免呢？也就是说，如果A、B的相对速度进入了VO，如何以最小的代价进行修正呢？分两种情况，如下图：

速度修正情形A

如果$\bm{V}_{relative}$落在上图截断圆锥前方的黄色扇形区域，此时需要将他移出扇形区域，最近的修正方向就是从原点连接相对速度引出的射线，该射线交于圆环的点，就是我们希望修正后的相对速度，而这个修正量已在图中标注记作u。在$\bm{V}_A$上进行u的修正后就是修正后的$\bm{V}_A^{new}$。但实际上，我们发现在垂直于u且经过$\bm{V}_A+u$直线的右侧区域的整个半平面，都是合法的速度空间，也即，这个空间内的$\bm{V}_A^{new}$都是可以避免与B发生碰撞的可行域。

速度修正情形B

如果$\bm{V}_{relative}$落了左上角的灰色区域，此时将他移出VO的最小修正方向，应该是往切线方向进行投影，具体如上图所示。

速度修正情形C

实际上，即使相对速度没有落在VO，也是会产生半平面的，如上图。图中我没有画具体的半平面，只画出了u和半平面内部法线n（这里n和u反向）。这里u虽然仍叫修正向量，但是相对速度目前不需要它来修正，但是半平面还是需要它来表示：$\bm{V}_A+u$。这个半平面的意义是保底，防止被其他半平面修正到违反该半平面的空间内。

当A周围存在多个运动对象时，A会与它们每个对象都形成一个半平面，如果区域存在公共交集，那么偏好速度就会修正到这个交集的边界上。如果不存在公共交集，那么会选择一个妥协速度，该速度与任何其他半平面的距离的最大值最小。

前面我一直称可行域是半平面, 实际上论文里定义的半平面与这还有些区别，ORCA中的R代表Reciprocal，即相互的。所以避障行为不可以是单方面的付出，应该是双方的责任。即A礼让$0.5u$，B礼让$-0.5u$（$VO_{A|B}$和$VO_{B|A}$是中心对称的）。因此最终的$ORCA_{A|B}$是垂直于n但经过$\bm{V}_A+0.5u$的半平面，所以前文图片中的半平面都应该沿着n的反方向后移半个u的距离，后文所有称呼半平面皆取自修正$0.5u$的ORCA半平面。

（注：并非说可行域只能是这个半平面，这个半平面只是所有可行域的一个子集，多个半平面围成的凸多边形在低维下可以进行快速求解）

算法理论介绍完了，我说的很简略，推荐看原论文的内容，里面还会讨论一般性问题，以及在求取半平面时，选择零速度、当前速度和偏好速度三个选择会有什么不一样，前文说的速度都是当前速度，这也是论文推荐的速度。下面着重分析代码。

代码图解

我们先说动态障碍物之间的碰撞，与静态障碍的碰撞放后面。

在开始之前需要再次解释一下速度，在之前算法图解部分，我一直用的$\bm{V}_A$和$\bm{V}_B$，它们表示的是A和B的当前速度，在整个ORCA算法里，其他对象的当前速度和当前位置都是可以观测的，但是每个对象内部还有一个偏好速度$\bm{V}_{pref}$，这个是不能被其他对象观测的。因此，在与其他对象进行ORCA半平面求解的时候用的是当前速度（处理静态障碍除外，后文叙），随后线性规划的时候，用的是偏好速度$\bm{V}_{pref}$，线性规划期间以及最后的输出都是优化速度$\bm{V}_{opt}$。在所有对象求出本帧各自的的优化速度后，再去批量更新，将优化速度设置为当前速度。

此外，本文不会介绍RVO2里的kdtree等空间划分算法，空间划分的方式多种多样除了kdtree，还有四叉/八叉树、格子法、BVH等等，这部分可以去搜索其他相关资料。

动态对象ORCA半平面

先强调一下代码里半平面的表示法，通过一个方向（单位）向量和边界上一个点来表示。这个方向向量不是论文里半平面的法向量n，而是半平面边界的方向，如果你有留意我前面的图，我在半平面边界上标注一个向量。从该方向向量看去，左侧是可行域，如下图，三个半平面围成一个三角形的交集区域。

1
2
3
4
5
6
7
8
// ORCA Line
public struct Line
{
    // 半平面边界方向，左侧是可行域
    public Vector2 direction;
    // 半平面经过的一点
    public Vector2 point;
}

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
41
42
43
44
// 时间窗口τ的倒数
float invTimeHorizon = 1.0f / timeHorizon_;
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
    Agent other = agentNeighbors_[i].Value;

    // 相对位置PB-PA
    Vector2 relativePosition = other.position_ - position_;

    // 相对速度VA-VB
    Vector2 relativeVelocity = velocity_ - other.velocity_;
    float distSq = RVOMath.absSq(relativePosition);

    // 叠加圆半径（闵可夫斯基和）
    float combinedRadius = radius_ + other.radius_;
    float combinedRadiusSq = RVOMath.sqr(combinedRadius);

    Line line;
    Vector2 u;
    // A和B暂未发生碰撞
    if (distSq > combinedRadiusSq)
    {
        // 从截断圆圆心指向相对速度的向量
        Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
        float wLengthSq = RVOMath.absSq(w);
        float dotProduct1 = w * relativePosition;

        if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
        {
            // 离VO最近的距离在截断圆圆弧上：投影到圆心上
        }
        else
        {
            // 离VO最近的距离在截断圆两条切线上：投影到这两条切线（腿）上
        }
    }
    else
    {
        // 当前A和B已经重叠了，需要快速分离
    }

    line.point = velocity_ + 0.5f * u;
    orcaLines_.Add(line);
}

未碰撞：投影到圆弧

1
2
3
4
5
6
7
8
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
    float wLength = RVOMath.sqrt(wLengthSq);
    Vector2 unitW = w / wLength;

    line.direction = new Vector2(unitW.y(), -unitW.x());
    u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}

上方代码结合下图，条件判断里，第一个点积用来判断$\bm{V}_{relativePosition}^τ$与$\bm{w}$是否同朝向（点积大于0代表两个向量夹角小于90°），用以快速区分相对速度是否落在左上角蓝色区域。要判断是否确实应该投影到圆弧，我们还需要排除中间这两块紫色区域得到最终的黄色区域。RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq这块代码就是来做这件事的，为了更加详细，我这边推导下公式：

图中$θ$是$\bm{V}_{relativePosition}^τ$和$\bm{w}$的夹角（暂时把他们当成线段），$β$是$\bm{V}_{relativePosition}^τ$向切线作垂线形成的夹角。黄色区域的约束转换为满足$θ<β$。求具体的角度肯定不划算，我们可以直接拼出三角函数之间的不等式关系：
$$
θ<β
$$
在0~90°下，cos函数是单调递减的，有：
$$
cos(θ)>cos(β)
$$
带入图形中的边，用点积获得对另一个向量的平行分量：
$$
\cfrac{||\bm{w}\cdot{\bm{V}_{relativePosition}^τ}||}{||\bm{V}_{relativePosition}^τ||\cdot||\bm{w}||}>\cfrac{R_{combine}^τ}{||\bm{V}_{relativePosition}^τ||}
$$
化简得到：
$$
||\bm{w}\cdot{\bm{V}_{relativePosition}^τ}||>R_{combine}^τ||\bm{w}||
$$
是不是已经和代码很像了，代码用的距离坐标下的$\bm{P}_{relativePosition}$和$R_{combine}$，即：
$$
||\bm{w}\cdot{\bm{P}_{relativePosition}}||>R_{combine}||\bm{w}||
$$
其实两边除以τ就是我推导的公式了。我一开始看到这就误解了，为什么一个距离坐标系的向量要和一个速度坐标系的向量叉积，也因此我之前作图时，如果同时存在两个坐标系下的向量，我会特别说明。

作者可能考虑变量名不好命名了，比如$\bm{V}_{relativePosition}^τ$这个我就要写很长才能表示相对坐标在τ时间下的相对坐标速度，所以代码里都是临时乘以invTimeHorizon来表示这个概念。注意：这和A、B之间的相对速度$\bm{V}_{relative}$是两个概念！有的解析甚至都搞混了。

投影到圆弧

现在已经判断了在黄色区域，并且拿到了w向量，进一步得到该向量的单位向量unitW，combinedRadius * invTimeHorizon - wLength得到相对速度离圆环边界的距离，乘以单位向量，得到实际的修正向量u。

注意这里line.direction = new Vector2(unitW.y(), -unitW.x());，含义是将指unitW顺时针旋转90°，而unitW指向半平面可行侧，这样，direction左侧就是可行侧了，这和之前讨论的半平面direction保持一致。

未碰撞：投影到切线

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
float leg = RVOMath.sqrt(distSq - combinedRadiusSq);

// 投影到左切线（腿）
if (RVOMath.det(relativePosition, w) > 0.0f)
{
    line.direction = new Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
// 投影到右切线（腿）
else
{
    line.direction = -new Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}

float dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;

既然已经排除了黄色区域投影到圆弧的区域，其他区域全部都是投影到切线的。这里做了一个二维叉积/二维行列式来判断最近应该落在左右哪个切线上。叉积的作用是区分方向，$Det(a,b)>0$代表a沿着逆时针可以转到b的方向，或者说b在a的左侧。

既然修正向量垂直于切线，由于修正向量也垂直半平面，那么半平面一定和这个切线平行，具体来说与左切线同向，与右切线反向（所以代码里求右切线direction时前面加了负号）。所以问题转为，如何求切线。

投用到切线

如上图所示，我们发现可以把$\bm{V}_{relativePosition}^τ$这条向量向左旋转α就可以得到切线方向了，虽然我们没有具体的α值，但是我们有丰富的三角函数关系。具体来说需要应用一个二维旋转矩阵，这个矩阵可以通过极坐标加三角函数推到，这里不赘述。

$$
direction =
\left[
\begin{array}{cc}
\cos\theta & -\sin\theta \
\sin\theta & \cos\theta
\end{array}
\right]
\cdot{\bm{V}_{relativePosition}^τ}
$$
将边带入三角关系：
$$
\cos\theta=\cfrac{\text{leg}}{||\bm{V}_{relativePosition}^τ||}
$$
$$
\sin\theta=\cfrac{R_{combine}^τ}{||\bm{V}_{relativePosition}^τ||}
$$
直接展开矩阵，就可以得到代码写的那一长串了。注意代码最后除以的是$||\bm{V}_{relativePosition}^τ||^2$，等于是同时做了向量的标准化了。

拿到direction后，我们做相对速度和direction投影，拿到有向投影距离dotProduct2，将其向量化后减去相对速度就可以得到从相对速度指向切线的修正向量u。

已碰撞

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

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

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

line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeStep - wLength) * unitW;

已碰撞或者说重叠，那就没啥好说的了，赶紧分离是第一要务。时间窗口设置成DeltaTime，意图是期望一帧内就分离，从而产生一个极远的半平面以期望选择一个很大的速度（但是实际会被agent的MaxSpeed约束的:）；速度的朝向是背向圆心的方向。

如下图所示，由于DeltaTime往往比较小，例如60fps游戏是0.01666秒，速度半径$R_{compose}^τ$和$\bm{V}_{relativePosition}^τ$都被放大很多，$\bm{V}_{relative}$要想离开这个速度半径就需要一个很大的修正量u，半平面也因此离原点很远。由于实体自身的最大速度往往无法触及这个区域，由此会使得程序走入代码里所谓3d线性规划的函数体内进行松弛，这是后话。

已经碰撞

最后，我们得到A与所有动态对象之间形成的所有ORCA半平面，是时候进行线性规划求解了。

1
2
line.point = velocity_ + 0.5f * u;
orcaLines_.Add(line);

线性规划

用的算法不是单纯形法，而是增量式线性规划算法（Incremental Linear Programming）又称作逐次约束添加法，源自《Computational Geometry: Algorithms and Applications》，专门针对低维凸多边形设计的轻量级算法，期望复杂度是$O(n)$。相较于此，单纯形法是通用线性规划，可解任意维，但是复杂度是$O(n^3)$。逻辑很符合直觉，不需要提前学习，直接看代码就行了。

linearProgram2

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
private int linearProgram2(IList<Line> lines, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 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. */
            Vector2 tempResult = result;
            if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
            {
                result = tempResult;

                return i;
            }
        }
    }

    return lines.Count;
}

参数解释：lines是传入的半平面集合；radius是最大速度；optVelocity区分是否是由linearProgram3d调用，如果是，内部会抛弃对象的偏向速度，而会指定一个“最优速度”用来最小化所有半平面的“惩罚”，这个目前不需要懂，后面会解释。

先进行了速度修正，避免超出最大速度，随后就是遍历所有半平面，当处理第i个半平面时，如果当前速度在半平面的可行侧，无须处理，否则，需要通过linearProgram1在第i条半平面线上尝试找到“最优速度”（有公共交集），否则从i开始到最后一个半平面，都交给linearProgram3d来处理（无公共交集）。

linearProgram1

半平面与最大速度圆相交

1
2
3
4
5
6
7
8
9
10
11
12
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;

这段其实是二次函数和圆交点方程求解，首先半平面边界的直线方程：
$$
Line_i(x)=\bm{p}_i+x\bm{d}_i
$$
$\bm{p}_i$是半平面结构体里的point，$\bm{d}_i$是里面的direction，由于需要将半平面限制在最大速度为半径的圆内，有：
$$
||\bm{p}_i+t\bm{d}_i||^2\le{R_{maxSpeed}^2}
$$
展开：
$$
||\bm{d}_i||^2t^2+2t(\bm{p}_i\cdot{\bm{d}_i})+||\bm{p}_i||^2\le{R_{maxSpeed}^2}
$$
由于$\bm{d}_i$是单位向量，有：
$$
t^2+2t(\bm{p}_i\cdot{\bm{d}_i})+||\bm{p}_i||^2-{R_{maxSpeed}^2}\le0
$$
二次函数判别式：
$$
\Delta=b^2-4ac=4||\bm{p}_i\cdot{\bm{d}_i}||^2+4{R_{maxSpeed}^2}-4||\bm{p}_i||^2
$$
解是：
$$
x=\frac{-b\pm{\sqrt{\Delta}}}{2a}=\cfrac{-2\bm{p}_i\cdot{\bm{d}_i}\pm{\sqrt{4||\bm{p}_i\cdot{\bm{d}_i}||^2+4{R_{maxSpeed}^2}-4||\bm{p}_i||^2}}}{2}
$$
简化：
$$
x=-\bm{p}_i\cdot{\bm{d}_i}\pm{\sqrt{||\bm{p}_i\cdot{\bm{d}_i}||^2+{R_{maxSpeed}^2}-||\bm{p}_i||^2}}
$$
代码里float discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point);的变量虽然叫discriminant（判别式），但是缺少系数4，只是判断正负的时候不需要，后面实际用到的时候又能直接约掉，所以直接去掉了系数4。判别式小于0代表无解，等于0代表1个解，大于0代表两个解。最后的tLeft和tRight就是二次函数的两个解，如下图所示。

半平面与最大速度圆相交

半平面区间切割

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
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;
    }
}

这段是通过应用前lineNo-1条半平面来切割当前的第lineNo条半平面，最终的区间才是这lineNo条半平面在第lineNo条半平面边界上的最短公共交集。代码里的分子numerator和分母denominator是怎么来的？我们来推导下，之前已经定义了直线方程：$Line_i(x)=\bm{p}_i+x\bm{d}_i$，并且有半平面约束：$Det(\bm{d}_i, \bm{v}-\bm{p}_i)>0$，带入当前的第lineNo个半平面和之前的某条半平面，记作第i条：
$$
Det(\bm{d}_i, \bm{p}_{lineNo}+t\bm{d}_{lineNo}-\bm{p}_i)>0
$$
叉积展开：
$$
Det(\bm{d}_i,\bm{p}_{lineNo}-\bm{p}_i)+tDet(\bm{d}_i,\bm{d}_{lineNo})>0
$$
调整下其中后面那个叉积的顺序：
$$
Det(\bm{d}_i,\bm{p}_{lineNo}-\bm{p}_i)-tDet(\bm{d}_{lineNo},\bm{d}_i)>0
$$
如果$Det(\bm{d}_{lineNo},\bm{d}_i)$大于0，有：
$$
t<\cfrac{Det(\bm{d}_i,\bm{p}_{lineNo}-\bm{p}_i)}{Det(\bm{d}_{lineNo},\bm{d}_i)}
$$
如果$Det(\bm{d}_{lineNo},\bm{d}_i)$小于0，有：
$$
t>\cfrac{Det(\bm{d}_i,\bm{p}_{lineNo}-\bm{p}_i)}{Det(\bm{d}_{lineNo},\bm{d}_i)}
$$
等于0不会存在，因为代码在前面已经排除了。等于0代表两个向量是平行的，numerator是第i条半平面的约束，而第lineNo不满足该约束，当然直接return false了。此时当前的第lineNo条半平面和第i条半平面只存在一种情形：首先必须在第i条半平面右边，且方向与第i条相反，两者永无交集，中间是真空带。为什么不能是方向相同？因为如果相同，第lineNo条半平面是完全被第i条半平面包含的，在linearProgram2的循环Det就直接continue了，不会进入linearProgram1。

在存在公共交集的情况下。经过前lineNo-1次半平面的切割，可选区间已经从最初与最大速度圆交点的两个区间缩小到了与所有半平面可行域交点形成的公共交集区间，此时我们已经拿到了在第lineNo条半平面边界上的合法区间，这时候可以进行optVelocity的修正了！

半平面切割

上图中，lineNo半平面被i半平面切割，tLeft从左上侧原先的与最大速度交点，收缩到右侧的新tLeft，最终得到的区间是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;
    }
}

这里directionOpt是linearProgram3调用时才会设置为true的参数，表示只取离optVelocity方向最接近的端点，不取中间值，因为这样才能最大化靠近optVelocity作为法线指向的半平面，但是现在不需要懂这些，先不管他。我们看else里的，其实就是将optVelocity投影到半平面边界线上，如果处于橙色线段区域，那就采用垂点作为新的速度。如果超出两侧tLeft或tRight，那就直接取两侧的值带入求得新速度，相当于clamp一下。

linearProgram3

如果在前面进行线性规划的时候，检测到某条半平面i出现：1、半平面i与最大速度圆没有交集；2、与前面的i-1条半平面无法围成公共交集。此时需要进入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
private void linearProgram3(IList<Line> lines, int numObstLines, int beginLine, float radius, ref Vector2 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
                {
                    // 交点公式，之前已经推导过类似的了pi+dixt = pj+djxt叉积消元轻松推导
                    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);
            }

            Vector2 tempResult = result;
            if (linearProgram2(projLines, radius, new Vector2(-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);
        }
    }
}

先解释下参数，lines是ORCA半平面列表；numObstLines是静态障碍物的半平面个数，静态障碍物的半平面是必定存在公共交集的（例如原点），因此绝对不能对他们进行松弛，因此需要这个变量就是偏移量，从静态障碍半平面后面开始处理；beginLine是遇到首个无法形成公共交集的半平面；radius依旧是最大速度。

首先从那个无法形成交集的半平面开始循环，假如是第k个半平面，如果当前求得的速度满足其半平面要求，那么皆大欢喜；否则进入里面开始处理。首先添加所有的静态半平面约束，然后依次处理前k-1个由动态对象形成的半平面，每对半平面(j,k)形成一个新的松弛半平面$projLine_{j|k}$（这里是我取的形象名字，我觉得叫投影有点反直觉），这个半平面的要求是：在这个新的半平面边界移动时，j和k两个半平面约束“违反程度”相等。

LP3松弛操作

上图左子图是按照LP1依次处理i和j半平面，最初传入的速度是$\bm{V}_{pref}$。半平面i未处理该速度，因为该速度已经在其半平面内；轮到半平面j了，半平面j发现此时的速度不属于其可行侧，对$\bm{V}_{pref}$执行了投影，发现投影点已经超出交集区间，因此clamp到$\bm{V}_{ORCA|j}$，这是半平面j与最大速度圆的交点；随后半平面k开始处理，发现当前速度$\bm{V}_{ORCA|j}$并不在其可行侧，随后让前面k-1条（也就是i和j）半平面进行切割，发现切割后的区间起点和终点已经不符合要求，直接返回false，随后算法转入linearProgram3进行处理。

上图右子图是linearProgram3里对i和j半平面进行松弛的图解。新的半平面的point是两个半平面的交点，而方向则是RVOMath.normalize(lines[j].direction - lines[i].，它代表角平分线，这个平分线穿过两个平面的可行侧交集和两个平面的不可行测交集。这也就意味着，在这条新的半平面边界上移动，如果违反了某个半平面，那么也同时违法另一个半平面，且违反成都相等，不会偏袒任何一方。且仔细观察，新的半平面和违约的半平面k所形成的并集包含了更多区域，这相当于松弛了原先的k-1条半平面。

最后调用linearProgram2，将从之前构造的k-1条松弛半平面的边界上找到新的速度。注意，此时传入的optVelocity = new Vector2(-lines[i].direction.y(), lines[i].direction.x())，也即半平面k的可行侧法向量。之前的k-1条松弛半平面包含了原先的k-1条半平面的信息，而这个optVelocity则包含了第k条半平面的信息，最终返回的resultVelocity就是权衡各方利益得到的最妥协速度。论文里用公式表达：
$$
\mathbf{v}_A^{\mathrm{new}} = \arg\min_{\mathbf{v} \in D(\mathbf{0}, v_A^{\max})} \max_{B \neq A} d_{A|B}(\mathbf{v})
$$
也即，找到的速度，满足与所有半平面“违约”度最大的那个值最小。代码里distance = RVOMath.det(lines[i].direction, lines[i].point - result);就是用来记录当前的最大违约值。k后面遇到不在可行侧的半平面，会计算违约距离，如果小于当前维护的distance，那就不需要处理。

此外，代码里会线判断前k-1条半平面是否存在某个半平面i与当前处理的半平面k平行，会有两种情况：

  • 同向：第k条半平面必然在半平面i左侧，被其完全包含，因此这条半平面无须产生任何松弛
  • 反向：第k条半平面必然在半平面i右侧，两者形成“真空带”，松弛的半平面位置取自两者中点，方向则跟半平面i保持一致

你可能会有疑问：为什么保证在linearProgram3里面调用linearProgram2就一定保证，这些松弛半平面一定有公共交集？作者在代码里也说，除非浮点数误差，不然肯定有交集，为什么呢？我们来思考下。

证明1

上方gif提供一个形象的直觉，中间有一个圆，圆心代表当前求得的$\bm{V}_{opt}$，圆心到前k-1条半平面的垂直交点（也即距离）被约束在圆内，或者说圆的半径就是当前的distance。因此当执行到第k条半平面，发现其与当前求得的速度距离大于distance时，k的半平面一定不会与这个圆相交或相切，而是有一定距离，从动图可以看出，不论怎么调整内部前k-1条任意半平面，其交集总是会包含至少圆心这个点所代表的，当前的$\bm{V}_{opt}$，因此前k-1条松弛变量必然包含公共交集。

证明2

用另一种方式证明，上图中$\bm{a}$与$\bm{b}$的角平分线是$\bm{c}$，由于b并不与圆相切，两者夹角会比$\bm{a}$与圆另一侧切线$\bm{a’}$的夹角要大。而圆从一点引出的两条切线的角平分线过原点，所以$\bm{a}$与$\bm{b}$角平分线所形成的上侧平分角必然包含原点，从而证明，最终不论多少条松弛半平面，它们必然会有公共交集。

linearProgram1里的directionOpt

现在可以完全解释这个变量了。

  • 当该变量为true时，优化目标是最大化$dot(\bm{V},\bm{V_{opt}})$，目的是找到最符合目标方向的合法点，存在于松弛半平面的公共交集的顶点处。
  • 当该变量为false时，优化目标是最小化$|\bm{V}-\bm{V_{opt}}|$，目的是找到最接近目标速度的合法点，是$\bm{V}_{opt}$在边界上clamp的投影点。

静态避障

障碍的表示

障碍物包含一组顶点，顶点逆时针连接组成边，如下图所示。图中包含代码里定义的direction、point、previous、next等等。逆时连接各个顶点，所以direction的左侧是不可通行区域。

障碍物的表示

论文里说过，障碍物可以抽象成一组边，针对其中的每条边进行处理，我们可以沿用前面的技巧。将A的半径放到障碍边两端的顶点上，A就可以退化成一个点，随后引出切线分别切向两个端点的圆，这样我们旧就组成了在$τ=1$时针对静态障碍边的VO区域，如下图所示。τ取不同的值的效果之前已经讨论了。

障碍物的VO表示

由于和动态对象间的VO一样，静态障碍边的VO也是凸集，所以他的补集（可行域）也是凹集，无法进行简单线性规划，所以这里也和之前一样定义VO切线来划分可行侧，切线切的点就是离相对速度最近的点，由于静态障碍没有速度，所以相对速度就是A的速度，而A的速度，在我们求取静态障碍形成的VO时，要设为零，这样可以保证所得的半平面可行侧必然包含原点，因此所有静态障碍所形成的半平面必然包含交集，针对他们的线性规划一定有解。

此外，针对静态障碍，我们选择时间窗口τ的时候可以设置的更小一点，原因之前也说过了，相较于动态对象，我们不需要大老远就对静态障碍物进行避让，而是在足够接近的时候才进行避让。另一方面，针对静态障碍物形成的半平面，我们应该严格对待，确保不会撞上静态障碍物，这点我们之前讨论过，在进行linearProgram3的时候，我们不会对静态障碍物形成的ORCA半平面进行松弛。

ORCA算法只是进行避障，不会让对象绕过障碍移动，绕行的逻辑是全局路径规划算法比如A*这类的职责。因此，如果你的物体目的地在障碍物后面，而物体和目的地的连线正好垂直于障碍物的边，此时它的$\bm{V}_{pref}$也是垂直于这个障碍边，那么物体的表现是逐渐减速然后停在障碍物前面一动不动，由于偏好速度垂直，切线方向无任何分量来提供它滑动。

下面的三百多行代码全是为了求静态障碍的半平面，我们一个个分析：

跳过重复检测

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
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next_;

Vector2 relativePosition1 = obstacle1.point_ - position_;
Vector2 relativePosition2 = obstacle2.point_ - position_;

/*
* Check if velocity obstacle of obstacle is already taken care
* of by previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;

for (int j = 0; j < orcaLines_.Count; ++j)
{
    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)
    {
        alreadyCovered = true;

        break;
    }
}

if (alreadyCovered)
{
    continue;
}

重复半平面

如上图所示，就是这段代码的全部动机。遍历已经生成的ORCA半平面，看看是不是有针对当前边已经求出的半平面，重复则跳过。if里的det就是求垂直分量，由于方向是单位向量，所有就是求垂直距离，如果两侧的垂直距离都大于半径，那就代表已经有半平面全部包含了个边，因此不用计算。很有用，比如一个矩形，你计算了近的这个线段，那它那个反向的对边（如果被选入obstacleNeighbor_）就会在这里被排除掉。

已经碰撞

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
/* Not yet covered. Check for collisions. */
float distSq1 = RVOMath.absSq(relativePosition1);
float distSq2 = RVOMath.absSq(relativePosition2);

float radiusSq = RVOMath.sqr(radius_);

Vector2 obstacleVector = obstacle2.point_ - obstacle1.point_;
float s = (-relativePosition1 * obstacleVector) / RVOMath.absSq(obstacleVector);
float distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);

Line line;

if (s < 0.0f && distSq1 <= radiusSq)
{
    /* Collision with left vertex. Ignore if non-convex. */
    if (obstacle1.convex_)
    {
        line.point = new Vector2(0.0f, 0.0f);
        line.direction = RVOMath.normalize(new Vector2(-relativePosition1.y(), relativePosition1.x()));
        orcaLines_.Add(line);
    }

    continue;
}
else 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 Vector2(0.0f, 0.0f);
        line.direction = RVOMath.normalize(new Vector2(-relativePosition2.y(), relativePosition2.x()));
        orcaLines_.Add(line);
    }

    continue;
}
else if (s >= 0.0f && s <= 1.0f && distSqLine <= radiusSq)
{
    /* Collision with obstacle segment. */
    line.point = new Vector2(0.0f, 0.0f);
    line.direction = -obstacle1.direction_;
    orcaLines_.Add(line);

    continue;
}

如下图所示，左中右三图分别对应这三个条件分支。其他的很好理解不多解释，说下line的point。point取原点，为什么不像之前动态避障那样取往外推的修正向量u？因为那样会导致可行侧丢失原点，我们求静态避障ORCA半平面的前提就是必须是包含原点的，但是像这样直接重叠的情况本身就是异常的，所以这里为了满足这个条件，所以需要将point设置回原点。

已经碰撞的三种情况

未碰撞的三种情况

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
/*
* No collision. Compute legs. When obliquely viewed, both legs
* can come from a single vertex. Legs extend cut-off line when
* non-convex vertex.
*/

Vector2 leftLegDirection, rightLegDirection;

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 Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_,
        relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
    rightLegDirection = new Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_,
        -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
}
else 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 Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_,
        relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
    rightLegDirection = new Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_,
        -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
}
else
{
    /* Usual situation. */
    if (obstacle1.convex_)
    {
        float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
        leftLegDirection = new Vector2(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 Vector2(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_;
    }
}

如下图所示，左中右三图分别对应这三个条件分支。

  • 足够倾斜的情况下，如果在左侧且垂直距离小于半径，那针对右侧圆的切线就不足以描述VO，需要针对左侧圆作新的右切线，这里用到旋转公式同前。
  • 足够倾斜的情况下，如果在右侧且…
  • 如果在中间且垂直距离大于半径。需要进一步判断两侧端点的凹凸性。如果是凸的，那正常用旋转公式求切线；否则：
    • 左侧凹顶点：切线退化成沿着cut-off line的左延长线（-obstacle1.direction_）
    • 右侧凹顶点：切线退化成沿着cut-off line的右延长线（obstacle1.direction_）

对于倾斜到两侧某一端点，这个端点如果是凹的，直接continue，凹顶点本身不能作为一个独立VO顶点来形成锥区域。

至于非倾斜情况下，凹顶点那侧的切线设置为direction_，这里已经失去切线的含义，而是当前VO边界退化到cut-off line对应方向延长线。它附近的VO边界不会由该顶点的“圆帽/切线”来决定。邻边的VO会把那部分边界“遮掉”。但是至少cut-off line这条保底仍然是有效约束，需要保留。

未碰撞的三种情况

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
/*
* 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.
*/

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;
}

  • 左侧凸顶点：左侧切线会被左侧凹顶点与其previous顶点的边切断，左切线换成左侧凹顶点的previous顶点的-direction（也即，direction从左侧凹顶点的previous指向左侧凹顶点）

  • 右侧凸顶点：右侧切线会被右侧凹顶点与其next顶点的边切断，右切线换成右侧凹顶点自己的direction（也即，direction从右侧凹顶点指向右侧凹顶点的next顶点）

这里的注释也说了，如果算出来的左右切线指进了左右侧临边的内部，那切线就不再是合法VO，那就用那个临边当切线。

切线切入临边，进行修正

寻找最近投影点

这里开始，求静态障碍ORCA半平面开始引入velocity_（物体当前速度），这与论文的说法是违背的。论文里说的是参与计算静态ORCA计算时，速度要设为0，论文截图如下：

论文截图

但是源码里却融入了当前速度来求得最近切线点，这样的话，静态障碍半平面必然存在交集的证明不就失效了吗？我尝试将后文计算静态ORCA的velocity_都改为0，发现运行起来也没问题，那就不管了，既然是源码解读，那就按照源码来，后面的作图也是，不再要求$\bm{V}_A$是零向量了。

投影点不外乎5段边界：左切线（左腿）：左弧、中间cut-off line（截断线）、右弧、右切线（右腿），下面依次分析。

投影到左右弧

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
/* Compute cut-off centers. */
Vector2 leftCutOff = invTimeHorizonObst * (obstacle1.point_ - position_);
Vector2 rightCutOff = invTimeHorizonObst * (obstacle2.point_ - position_);
Vector2 cutOffVector = rightCutOff - leftCutOff;

/* Project current velocity on velocity obstacle. */ // ？？？

/* 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;

if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f))
{
    /* Project on left cut-off circle. */
    Vector2 unitW = RVOMath.normalize(velocity_ - leftCutOff);

    line.direction = new Vector2(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. */
    Vector2 unitW = RVOMath.normalize(velocity_ - rightCutOff);

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

    continue;
}

图片如下，黄色圆弧对应的扇形区域就是这段代码处理的区间。还有倾斜到一侧的情况，但原理一样。

投影到圆弧

投影到左右切线和截断线

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
/*
* Project on left leg, right leg, or cut-off line, whichever is
* closest to velocity.
*/
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 Vector2(-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 Vector2(-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 Vector2(-line.direction.y(), line.direction.x());
orcaLines_.Add(line);

下图左中右分别是投影到左切线、截断线和右切线的情况，已经没有什么花样了，不需要解释。

投影到圆弧

参考资料