使用RVO执行碰撞避免

基于DetourCrowd实现的碰撞避免在实践中的效果经常无法令人满意，特别是大规模聚集的时候这种完全基于斥力的碰撞避免会消耗很大的CPU时间，同时经常出现互相阻塞卡住的问题，我甚至遇到过两个Agent面对面移动互不相让导致的卡死问题。在相关的论文中也提到了这种完全基于斥力的碰撞避免系统可能出现的死锁现象：

detour的死锁

上图中就展示了死锁的现象，初始状态为图中的左侧，两个Agent正对着移动，在一个update之后计算出来两者应该选择一个避让速度，从而变化成为了上图中间，但是在下一次update的时候这两个agent发现当前速度下不会发生碰撞，因此又把速度调整为了刚开始的方向，就是上图右侧，此时又进入了初始状态，只有位移有一点点改变。

UE也发现了DetourCrowd在效率和效果上的不足之处，因此UE在UCharacterMovementComponent之上提供了另外的一种碰撞避免机制，相对速度障碍物算法（Reciprocal Velocity Obstacles，即RVO。在UE的官方文档在寻路系统中使用避障机制中有一个非常简单但是效果明显的对比测试，体现出了RVO在碰撞避免上的优越性。不过这里我们将不去讨论UE的RVO代码实现，因为他提供的实现比较简单，而且还会出现避障时被挤出寻路网格表面的情况。所以这里我们来详细的介绍一下有论文支撑的碰撞避免开源库RVO2,这个库实现了论文中提出的最优相对速度障碍法(Optimal Reciprocal Collision Avoidance简称ORCA)。这个ORCA算法能获取Optimal这个前缀自然是有实力的，因为他的内部实现会将多个Agent的碰撞避免进行协调，而不是DetourCrowd的基于斥力的各自独立避让。下面我们来结合ORCA的论文来深度的分析一下RVO2的实现，并讨论一下如何对其进行修改以避免冲出NavMesh的边界。

速度障碍物

了解ORCA前我们需要了解一个前置概念，什么叫速度障碍Velocity Obstacle。在二维平面中参与避障的Agent有两个基本的描述参数(P,R)，P代表Agent的位置，而R代表这个Agent的碰撞半径。假设平面中存在两个Agent, 以(Pa, Ra)和(Pb, Rb)进行描述:

初始的两个agent

此时B的速度为0，我们来考虑A应该选取什么速度来避免在后续的匀速移动中遇到B。由于坐标系的平移对于速度来说是没有任何影响的，所以我们可以把Pa的位置设置为坐标系的圆心，这样作图更美观一些，此时B的坐标变成了Pb-Pa，但是半径不变：

以pa作为圆心的坐标系

我们在上面的这张图中寻找两个可以使得A刚好与B相切的速度,相切时A的位置分别为C和D：

运动轨迹相切

AC向量和AD向量分别代表了相切时的A的速度方向。由于圆的切线性质可知，BC和BD的长度都等于Ra+Rb，BC与AC垂直，且BD与AD垂直。所以两个带半径的Agent的避障速度选择可以简化为一个质点与一个指定圆的避障速度选择，这个指定圆的半径为原始的两个Agent的半径之和。

简化为一个半径

此时可以看出如果A的移动速度 $V_{A}$ 处于由AC和AD两个涉嫌组成的锥形Cone内时，其移动轨迹最终一定会与B相交，这个会触发A与B相交的 $V_{A}$ 速度选择区域就叫做A相对于B的速度障碍Velocity Obstacle, 简称为 $V O_{A ∣ B}$ 。

速度障碍初始样例

上图中的橙黄色区域就是 $V O_{A ∣ B}$ ，对于任意一个不在 $V O_{A ∣ B}$ 内的点F，若A采取这个点F作为移动速度，则A一定不会与B相交。

前面讨论的都是B处于静止的情形，如果B有移动速度Vb，则此时的 $V O_{A ∣ B}$ 只需要将静止的 $V O_{A ∣ B}$ 加一个坐标偏移Vb即可,也就是从下图中的黄色区域VO1移动到了下图中的绿色区域VO2。

带目标移动速度的VO

在实际的场景模拟中所有Agent的速度都是动态变动中的，所以以某一个时刻的状态去计算一个永远不与其他Agent相碰撞的速度是没有意义，而且很多时候是不可行的。就以我们人类自身的移动来说，只会规划将来很短的一段时间内与周围的人群进行碰撞避免。所以实际的世界模拟设置中，计算的是Agent在时间t内会与周围的Agent相交的速度。此时我们需要对B的形状和位置都除以t，构造出新的 $V O_{A ∣ B}$ :

限定时间内的VO

其实可以看出新的 $V O_{A ∣ B}$ 与老的 $V O_{A ∣ B}$ 是同样的形状，但是需要剔除掉时间t内不会引发碰撞的速度，即上图中的紫色区域。这个带时间t限制的 $V O_{A ∣ B}$ 我们用符号 $V O_{A ∣ B}^{τ}$ 来表示，他的形状是一个圆角的锥形。根据定义可知 $V O_{A ∣ B}^{τ}$ 与 $V O_{B ∣ A}^{τ}$ 在平面上关于原点对称。

如果B有移动速度Vb，则 $V O_{A ∣ B}^{τ}$ 也需要加上这个移动速度Vb:

限定时间且带移动速度的VO

如果Vb的并不是一个固定值，在t时间内的取值范围为 $V_{B}$ ，则类似的需要将原始的 $V O_{A ∣ B}^{τ}$ 中的每个向量都加上 $V_{B}$ 中的每个向量构造出一个新的形状 $V O_{A ∣ B}^{τ} \oplus V_{B}$ , 这个两个形状的 $\oplus$ 操作就是闵可夫斯基和(Minkowski sum):

$A \oplus B = {a + b ∣ a \in A, b \in B}$

下图中左侧代表原始的 $V O_{A ∣ B}^{τ}$ ,在这个操作下变换出来的 $V O_{A ∣ B}^{τ} \oplus V_{B}$ 为下图中的右侧：

闵可夫斯基和

A选取任何一个不在 $V O_{A ∣ B}^{τ} \oplus V_{B}$ 中的速度都可以保证时间t内不会与B发生碰撞，这个速度的集合我们标记为碰撞避免速度Collision Avoiding Velocity，数学符号为 $C A_{A ∣ B}^{τ} (V_{B})$ , 其实就是 $V O_{A ∣ B}^{τ} \oplus V_{B}$ 的补集:

$C A_{A ∣ B}^{τ} (V_{B}) = {v ∣ v \in / V O_{A ∣ B}^{τ} \oplus V_{B}}$

最优碰撞避免

假如A的速度 $V_{A}$ 在 $C A_{A ∣ B}^{τ} (V_{B})$ 内且B的速度 $V_{B}$ 在 $C A_{B ∣ A}^{τ} (V_{A})$ 内，则可以保证时间t内A与B绝对不会发生碰撞,此时我们将 $V_{A}$ 与 $V_{B}$ 定义为相互碰撞避免速度(reciprocally collision-avoiding)。如果此时 $V_{A} = C A_{A ∣ B}^{τ} (V_{B})$ 且 $V_{B} = C A_{B ∣ A}^{τ} (V_{A})$ ，则将 $V_{A}$ 与 $V_{B}$ 之间的关系称为相互最大碰撞避免速度(reciprocally maximal)。

有了上述概念的定义之后，碰撞避免系统的更新目标就是对于每个寻路实体A，求解出其相对于系统内每个其他寻路实体B的碰撞避免速度 $C A_{A ∣ B}^{τ} (V_{B})$ 的交集C，选取C中的任意速度都可以保证A在时间t内不会出现碰撞。由于C是一个集合，任意的从集合中选取元素作为移动速度的话表现上就会出现一堆无头苍蝇在做布朗运动。考虑到碰撞避免系统中的每个寻路实体都是带着目标的移动，其在不考虑其他实体的情况下期望的最优速度 $V^{o pt}$ 就是朝向目标点的最大速度。所以我们从C中选取与 $V^{o pt}$ 方向偏差最小的速度作为A的最优碰撞避免速度(Optimal Reciprocal Collision Avoidance) $ORC A_{A ∣ B}^{τ}$ ，简称为ORCA，这个也是一个集合。在每个寻路实体都采用这个最优碰撞避免速度来作为更新后的移动速度时，碰撞避免系统的最优化目标就是尽可能的将 $ORC A_{A ∣ B}^{τ}$ 变大，这样使得后续移动时每个实体能选择的碰撞避免速度范围更大，能保证更多的对突发的环境变化的容错。所以此时碰撞避免系统计算出来的 $ORC A_{A ∣ B}^{τ}$ 和 $ORC A_{B ∣ A}^{τ}$ 需要满足下面的最优性质，循环套用计算出来的碰撞避免速度集合不会变化，且都是相互最大碰撞避免速度： $C A_{A ∣ B}^{τ} (ORC A_{B ∣ A}^{τ}) = ORC A_{A ∣ B}^{τ}$

$C A_{B ∣ A}^{τ} (ORC A_{A ∣ B}^{τ}) = ORC A_{B ∣ A}^{τ}$

同时需要与期望最优速度尽可能的接近，以及对于任意的半径 $r > 0$ , $ORC A$ 集合与以 $V_{o pt}$ 为圆心 $r$ 为半径的圆 $D (V^{o pt}, r)$ 的交集都会比其他的碰撞避免速度 $V$ 与 $D (V^{o pt}, r)$ 的交集大，即对于任意的属于A,B之间的碰撞避免速度集合 $(V_{A}, V_{B})$ $V_{A} \subseteq C A_{A ∣ B}^{τ} (V_{B}), V_{B} \subseteq C A_{B ∣ A}^{τ} (V_{A})$

都有下面的性质： $∣ ORC A_{A ∣ B}^{τ} \cap D (V_{A}^{o pt}, r) ∣ = ∣ ORC A_{B ∣ A}^{τ} \cap D (V_{B}^{o pt}, r) ∣ \geq min (∣ V_{A} \cap D (V_{A}^{o pt}, r) ∣, ∣ V_{B} \cap D (V_{B}^{o pt}, r) ∣)$

写这么多公式都不如用一张图进行解释：

orca

在上面的图中，我们先以相对位置和时间t构造出了 $V O_{A ∣ B}^{τ}$ ，此时A相对于B的最优速度差值的点坐标为 $V_{A}^{o pt} - V_{B}^{o pt}$ ，如果这个不在 $V O_{A ∣ B}^{τ}$ 中，则两者在时间t内不会发生碰撞。如果在 $V O_{A ∣ B}^{τ}$ 内，为了达到对最优期望速度扰动最小且避免碰撞，我们需要挑选在 $V O_{A ∣ B}^{τ}$ 边界上且与 $V_{A}^{o pt} - V_{B}^{o pt}$ 差值最小的点 $U$ ，记录速度差值 $u = U - (V_{A}^{o pt} - V_{B}^{o pt})$ ， $u$ 会与点 $U$ 在 $V O_{A ∣ B}^{τ}$ 边界上离开 $V O$ 的法线方向 $n$ 共线，这样才能符合差值最小的定义。此时只需要将 $V_{A}$ 调整为 $V_{A}^{o pt} + u$ 或者将 $V_{B}$ 调整为 $V_{B}^{o pt} - u$ 即可成功的实现碰撞避免。单独调整一个Agent的速度来实现碰撞避免会导致Agent的速度调整会很大，ORCA中将速度调整的任务平摊到了参与避障的两个Agent中，即将 $V_{A}$ 调整为 $V_{A}^{o pt} + u * 0.5$ 同时将 $V_{B}$ 调整为 $V_{B}^{o pt} - u * 0.5$ 。此时我们以点 $V_{A}^{o pt} + u * 0.5$ 和正方向 $u$ 构造一个半平面，这就是我们所求的 $ORC A_{A ∣ B}^{τ}$ ，这个半平面有下面的性质： $ORC A_{A ∣ B}^{τ} = {v ∣ (v - (v_{A}^{o pt} + \frac{1}{2} u)) . n \geq 0}$

类似的以点 $V_{B}^{o pt} - u * 0.5$ 和正方向 $- u$ 构造一个半平面为 $ORC A_{B ∣ A}^{τ}$ 。上面的示意图对应的是 $V_{A}^{o pt} - V_{B}^{o pt}$ 在 $V O_{A ∣ B}^{τ}$ ，其实对于不在 $V O_{A ∣ B}^{τ}$ 内部时上面的 $ORC A_{A ∣ B}^{τ}$ 内的速度依然会避免碰撞，因为我们将 $n$ 定义为了逃离 $V O_{A ∣ B}^{τ}$ 的方向， $V_{A}^{o pt} - V_{B}^{o pt}$ 不在 $V O_{A ∣ B}^{τ}$ 只会将这两条半平面分割线的距离增加 $2 u$ 而已，也就是各自拉远 $u$ 单位距离。

寻路场景里不仅有相互参与碰撞避免的寻路实体，而且还有一些障碍物，对于动态障碍物而言我们可以将其转化为一个速度不可变的Agent来处理，形状抽象为这个障碍物的外接圆，避障所需的速度改变完全由其他的agent来承担，也就是将上面公式里的0.5改成0.0，而避障的另外一方的0.5则改成1，这样就可以服用前面所述的各种流程了。对于静态障碍物，我们将障碍物的每条边 $O$ 单独拿出来处理，其闵可夫斯基和就是 $O \oplus - D (p_{A}, r_{A})$ ，这是一个胶囊体。使用这个胶囊体进行缩放也可以构造出一个时间t内会触发碰撞的 $V O_{A ∣ O}^{τ}$ 。此时以下图c的方式来构造 $ORC A_{A ∣ O}^{τ}$ ，即以这个鞍型的底边作为分割线，获取0所在的半平面，因为A如果速度为0则一定不会与 $O$ 相撞。

静态障碍物的orca

上面的计算只考虑了两个寻路实体之间的相互影响，如果场景里有多个参与寻路的实体，则需要综合考虑每两个实体之间的ORCA，此时在时间t内A的最优碰撞避免安全速度集合定义为 $ORC A_{A}^{τ}$ ，就是之前计算的多个半平面的交集，同时限定在当前实体的最大速度范围之内:

$ORC A_{A}^{τ} = D (0, v_{A}^{ma x}) \cap A \neq = B ⋂ ORC A_{A ∣ B}^{τ}$

多实体的orca

然后再选取 $ORC A_{A}^{τ}$ 中与期望速度差值最小的速度作为更新后的速度：

$v_{A}^{n e w} = v \in ORC A_{A}^{τ} arg min ∣∣ v - v_{A}^{o pt} ∣∣$

由于半平面是一个有凸Convex性质的形状，而根据凸形状相交之后形成的形状也是凸的这一性质，上面的公式其实就是在一个给定的凸区域内获取一个到指定点距离最小的点的凸优化问题，更详细点的说是在多个半平面限制下的求二次函数最小值的线性规划问题。如果 $ORC A_{A}^{τ}$ 区域非空，问题的求解是可行的，则总是可以获得一个最小值。但是有些情况下会出现半平面交为空,如下图中的配置，阴影方向代表这个半平面的正向，：

orca为空

出现这种情况时，碰撞避免系统也不能放弃治疗，仍然需要选出一个最优速度出来，不过此时最优的目标需要改一下，将速度 $v$ 与这些半平面的分割线之间的带符号距离 $d_{A ∣ B} (v)$ 的最大值最小化，当速度对应的点在半平面内时，这个值为负数，数值为点到这个分割线的距离，当速度对应的点在半平面外时，值的符号为正。此时新的优化目标为：

$v_{A}^{n e w} = v \in D (0, v_{A}^{ma x}) arg min B \neq = A max d_{A ∣ B} (v)$

$D (0, v_{A}^{ma x})$ 是一个圆，其也是一个凸区域，而对一个凸函数执行minmax转换后对应的函数依然是凸的，所以上面这个公式仍然可以规约到凸优化问题上。其几何意义就是将所有的半平面分割边都朝着其方向的反向移动距离 $v_{A}^{n e w}$ ，使得这些半平面的交集不为空，刚好包含0这一个元素。

RVO2 代码流程

RVO2提供了若干的样例测试来展示如何使用这个库，其中最简单的就是Circle程序，这个程序在一个特定大小的圆环上等间距的排列若干参与碰撞避免的Agent，然后让每个Agent都移动到圆环的对侧，下面就是其初始化代码：

/* Store the goals of the agents. */
std::vector<RVO::Vector2> goals;

void setupScenario(RVO::RVOSimulator *sim)
{
	/* Specify the global time step of the simulation. */
	sim->setTimeStep(0.25f);

	// 初始化一些agent的默认参数
	sim->setAgentDefaults(15.0f, 10, 10.0f, 10.0f, 1.5f, 2.0f);

	// 创建250个agent 均匀的分布在圆环上 同时设置其移动目标为对角线
	for (size_t i = 0; i < 250; ++i) {
		sim->addAgent(200.0f *
		              RVO::Vector2(std::cos(i * 2.0f * M_PI / 250.0f),
		                           std::sin(i * 2.0f * M_PI / 250.0f)));
		goals.push_back(-sim->getAgentPosition(i));
	}
}

上面的setAgentDefaults初始化了如下六个参数：

void RVOSimulator::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)
{
	if (defaultAgent_ == NULL) {
		defaultAgent_ = new Agent(this);
	}

	defaultAgent_->maxNeighbors_ = maxNeighbors;
	defaultAgent_->maxSpeed_ = maxSpeed;
	defaultAgent_->neighborDist_ = neighborDist;
	defaultAgent_->radius_ = radius;
	defaultAgent_->timeHorizon_ = timeHorizon;
	defaultAgent_->timeHorizonObst_ = timeHorizonObst;
	defaultAgent_->velocity_ = velocity;
}

RVO2与DetourCrowd都将Agent当作一个圆形来看待，因此每个Agent都有半径radius和最大速度maxSpeed这两个参数。RVO2执行碰撞避免时需要与DetourCrowd一样计算一个Agent周围一定半径的其他Agent和障碍物，这里的neighborDist用来获取周围邻居的圆半径，而maxNeighbors则是过度拥挤的情况下只保留最多这些数量的邻居Agent。timeHorizon代表如果两个Agent的预估碰撞时间小于这个值时才考虑这两者的碰撞避免，timeHorizonObst代表与某个静态障碍物的碰撞时间小于这个值时才考虑将这个障碍物与当前Agent之间的碰撞避免。

添加好了所有参与更新的Agent之后开始进入更新循环：

do {
	setPreferredVelocities(sim);
	sim->doStep();
}
while (!reachedGoal(sim));

这里的setPreferredVelocities就是将每个agent设置一下期望最优速度，设置完最优速度之后再调用doStep来执行一次内部更新：

void RVOSimulator::doStep()
{
	kdTree_->buildAgentTree();

#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
		agents_[i]->computeNeighbors();
		agents_[i]->computeNewVelocity();
	}

#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
		agents_[i]->update();
	}

	globalTime_ += timeStep_;
}

这里的doStep其实依赖了一个timeStep的参数，代表距离上次更新的时间间隔。在函数的开头会调用kdTree_->buildAgentTree()来构建当前所有Agent的KDTree，这个空间索引结构我们在之前已经介绍过了，因此此处不再介绍。然后遍历所有的agent，调用computeNeighbors来获取本次更新时需要考虑的周围的避障Agent和障碍物，并使用computeNewVelocity来计算最优避障速度。这里的computeNeighbors分为了两个部分，先计算周围的静态障碍物，然后再计算周围的其他agent:

void Agent::computeNeighbors()
{
	obstacleNeighbors_.clear();
	float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
	sim_->kdTree_->computeObstacleNeighbors(this, rangeSq);

	agentNeighbors_.clear();

	if (maxNeighbors_ > 0) {
		rangeSq = sqr(neighborDist_);
		sim_->kdTree_->computeAgentNeighbors(this, rangeSq);
	}
}

虽然这里computeObstacleNeighbors的this指针指向的是KDTree，但是其实他的实现是二叉空间分割BSP(Binary Space Paritioning)树。

BSP树是一个树形结构，其定义如下：

树中每个节点最多有两个子节点
每个节点里存储一条2D空间线段的信息
对于任意节点N，其左子树中的所有线段都处于节点N所存储的线段L的左边
对于任意节点N，其右子树中的所有线段都处于节点N所存储的线段L的右边

BSP算法的基本构思是找到对于特定集合的线段，寻找这个集合内的最优线段，使得集合内其他的线段在这条线段左边的数量与出现在右边的数量插值最小。然后递归的对左右两边的线段集合进行分割，并最后建立一个树形结构。

BSP树

上图就是BSP算法在一个样例多边形上的执行BSP树的构建过程:

在平面上初始有四条线段，初始待分割集合为[A, B,C,D]
步骤1：选择线段A作为分割线段，此线段将线段B切分为B1、B2, 线段C切分为C1、C2，线段D切分为D1、D2，所有在线段A左侧的线段集合[B1,C1,D1]分割进入左子树，所有在线段A右侧的线段集合[B2,C2,D2]进入右子树，形成步骤1中左边的树结构，然后递归处理右子节点[B2,C2,D2]
步骤2：对于[B2,C2,D2]集合，选择线段B2作为分割线段，这条线段将D2切分为D2,D3两个部分，然后在左边的线段集合为[C2, D3]，在右边的线段集合为[C2]，分别创建为当前节点的左右子节点,然后处理右子节点[D2]
步骤3：对于[D2]这个子节点,不可再分割，回溯到左边的兄弟节点[C2,D3]
步骤4：选取C2对[C2,D3]集合进行划分，将[D3]集合构造为当前节点的右子树
步骤5：由于当前[D3]集合无法再划分，因此回溯到[B1,C1,D1]节点进行递归处理
步骤6：选取B1对[B1,C1,D1]进行划分，[C1]设置为左子节点，[D1]设置为右子节点
步骤7：此时两个子节点的线段集合都只有一个元素，因此无法再分割，自此BSP树构建完成

在场景内线段数量为N时，构建流程的时间复杂度为N*N。在N比较大时，BSP树的创建时间会急速增大，导致无法在开放大场景里支持大量的空间障碍物。构建流程中最耗时的部分为寻找集合内的最优线段，使得集合内其他的线段在这条线段左边的数量与出现在右边的数量插值最小。这个设定导致必须遍历集合内所有的线段，计算这个线段与集合内其他线段的相交状态，在集合大小为N时，会进行 $(N - 1)^{2}$ 此相交状态比较。

在构建好BSP树之后，我们可以利用这棵树的结构来做范围内的阻挡边查询。对于给定的一个点C以及查询半径R，我们来执行下述过程来获取当前BSP树内所有与这个碰撞范围相交的线段集合S，设当前正在处理的BSP节点为N

计算当前点C与节点N的分割线段L所在直线的投影距离为D
如果D小于等于R
1. 计算点C到线段L的距离如果小于R，则把L加入到结果集合S
2. 如果D有子节点则对D所有的子节点递归的进行查询操作
如果D大于R ，则判断点C在线段L的哪一侧
1. 如果C在L的左侧则递归的处理节点N的右子节点
2. 如果C在L的右侧，则递归的处理节点N的左子节点

在递归下降处理完成之后，集合S里包含的线段就是所有与圆心为C半径为R的圆圈相交的线段。

通过BSP树查询得到的相交线段都会存储到数组obstacleNeighbors_中，数组中的元素按当前Agent到这条线段的距离排列的：

void Agent::insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq)
{
	const Obstacle *const nextObstacle = obstacle->nextObstacle_;

	const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_);

	if (distSq < rangeSq) {
		obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle));

		size_t i = obstacleNeighbors_.size() - 1;

		while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) {
			obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
			--i;
		}

		obstacleNeighbors_[i] = std::make_pair(distSq, obstacle);
	}
}

用来查询一定范围内的其他agent的函数computeAgentNeighbors没什么特殊的，就是平常的递归查询KDTree的实现，不过这里由于存储的邻居数量有限制，所以会优先存距离最近的那些：

void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq)
{
	if (this != agent) {
		const float distSq = absSq(position_ - agent->position_);

		if (distSq < rangeSq) {
			if (agentNeighbors_.size() < maxNeighbors_) {
				agentNeighbors_.push_back(std::make_pair(distSq, agent));
			}

			size_t i = agentNeighbors_.size() - 1;

			while (i != 0 && distSq < agentNeighbors_[i - 1].first) {
				agentNeighbors_[i] = agentNeighbors_[i - 1];
				--i;
			}

			agentNeighbors_[i] = std::make_pair(distSq, agent);

			if (agentNeighbors_.size() == maxNeighbors_) {
				rangeSq = agentNeighbors_.back().first;
			}
		}
	}
}

为了维持这个距离最近的限制，agentNeighbors_里存储的元素其实是按照离当前Agent的距离排列的，插入的时候走的是一个插入排序。

在构造了周围需要考虑碰撞避免的agent与静态障碍物之后，开始调用computeNewVelocity来计算最优速度。

RVO中的分割线

在了解了整体的基于RVO的碰撞避免最优化问题的定义之后，我们来研究一下RVO2是如何对这些最优化问题来进行求解的，对应的代码都在Agent::computeNewVelocity之中。这里先介绍一下RVO2中如何定义一个ORCA半平面的：

/**
	* \brief      Defines a directed line.
	*/
class RVO_EXPORT Line {
public:
	/**
		* \brief     A point on the directed line.
		*/
	Vector2 point;

	/**
		* \brief     The direction of the directed line.
		*/
	Vector2 direction;
};

他这里复用了一个直线的定义，作为这个ORCA半平面的分割线，direction的向量大小一定是1。由于一条分割线会产生两个半平面，所以RVO2这里规定，direction对应的方向向量逆时针旋转90度之后生成的向量朝向ORCA半平面内部，作为direction的法线方向，也就是说半平面在分割线方向的左侧，这就刚好与前面论文中的定义一样了。不过由于ORCA中一般首先计算出来的是最优避障速度的方向u，此时由u构造ORCA分割线方向需要顺时针旋转90，所以在后面的代码中会经常看到下面的变换:

Vector2 unitW; // 通过某种途径计算出来的最优避障速度方向
line.direction = Vector2(unitW.y(), -unitW.x()); // 顺时针旋转90 度 构造orca分割线的正向
// 假设w = (-1,1) 算出来的direction=(1,1) 刚好是顺时针旋转90度的方向

对应的逆时针旋转90度的代码如下：

line.direction = Vector2(-unitW.y(), unitW.x()); // 逆时针旋转90 度
// 假设w = (-1,1) 算出来的direction=(-1,-1) 刚好是逆时针旋转90度的方向

构造障碍物的ORCA半平面

computeNewVelocity代码的开始首先遍历之前计算好的静态障碍物线段，来创建每个静态障碍物线段对应的orca线：

orcaLines_.clear();

// 这里计算逆是为了后续老是执行除法
const float invTimeHorizonObst = 1.0f / timeHorizonObst_;

/* Create obstacle ORCA lines. */
for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) {

	const Obstacle *obstacle1 = obstacleNeighbors_[i].second;
	const Obstacle *obstacle2 = obstacle1->nextObstacle_;

	// 在当前agent坐标系下阻挡线段的左端点 我们记录为p1
	const Vector2 relativePosition1 = obstacle1->point_ - position_;
	// 在当前agent坐标系下阻挡线段的右端点 我们记录为p2
	const 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 (size_t j = 0; j < orcaLines_.size(); ++j) {
		// 计算当前线段两个端点到orcaline的距离都大于0 则当前agent会先与orcaline发生碰撞 然后再与当前obstacle发生碰撞 所以此时不需要把当前的obstacle创建对应的orcaline
		if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >=  -RVO_EPSILON) {
			alreadyCovered = true;
			break;
		}
	}

	if (alreadyCovered) {
		continue;
	}
	// 后续的创建障碍物orca的代码
}

上面有一个判断，如果当前的阻挡线段被现有的一条orca线挡住了，则这个阻挡线段不需要再考虑了。如果没有被其他orca线挡住，则考虑创建对应的orca。


const float distSq1 = absSq(relativePosition1);
const float distSq2 = absSq(relativePosition2);

const float radiusSq = sqr(radius_);

// 当前阻挡线段的方向向量
const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_;
// 这个s代表投影点与左侧端点的连线是在这个向量的多少倍 负值代表在投影点在左侧端点的左边 大于1代表投影点在右侧端点的右边
// 大于0小于1 代表投影点在这个线段上
const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector);
// distSqLine 代表agent当前位置到阻挡线段所在直线的距离
const float distSqLine = absSq(-relativePosition1 - s * obstacleVector);

Line line;

if (s < 0.0f && distSq1 <= radiusSq) {
	// 发生了碰撞 投影点在左侧端点的左边且左端点在agent的radius内
	/* Collision with left vertex. Ignore if non-convex. */
	if (obstacle1->isConvex_) {
		// 建立一条orcaline 起点为原点 最优速度方向为relativePosition1的反方向，direction方向为最优速度方向减去90度
		line.point = Vector2(0.0f, 0.0f);
		line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x()));
		orcaLines_.push_back(line);
	}

	continue;
}
else if (s > 1.0f && distSq2 <= radiusSq) {
	// 与上面类似 不过是右端点 还需要额外考虑这个相对位置方向会指向当前凸包的内部
	if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) {
		line.point = Vector2(0.0f, 0.0f);
		line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x()));
		orcaLines_.push_back(line);
	}

	continue;
}
else if (s >= 0.0f && s <= 1.0f && distSqLine <= radiusSq) {
	// 与阻挡线段相交且两个端点都不在圆内 直接以这个阻挡线段的方向的负方向作为orca线
	// 这里之所以乘以-1 是因为这个阻挡边的法线方向永远是朝向阻挡凸包内的 我们为了远离凸包 需要将法线旋转180度 因此需要乘以-1
	line.point = Vector2(0.0f, 0.0f);
	line.direction = -obstacle1->unitDir_;
	orcaLines_.push_back(line);
	continue;
}

上面代码的第一个分支对应下图中的a，左端点在圆内。第二个分支对应下图中的b，右端点在圆内。第三个分支对应下图中的c，没有端点在圆内。每个子图中的浅绿色线段代表阻挡线段所在的直线，浅黄色线段代表阻挡边向量，紫色线段代表当前agent位置出发的与阻挡线段垂直的向量，红色线段代表最后添加的orca半平面分界向量，注意其箭头方向。

与线段相交的情形

上面这些代码处理的是阻挡线段与agent相交的情况，接下来处理阻挡线段所在直线是否与agent相交：

Vector2 leftLegDirection, rightLegDirection;

if (s < 0.0f && distSqLine <= radiusSq) {
	/*
		* Obstacle viewed obliquely so that left vertex
		* defines velocity obstacle.
		*/
	// 投影点在左端点的左侧
	if (!obstacle1->isConvex_) {
		/* Ignore obstacle. */
		continue;
	}

	obstacle2 = obstacle1; // 只考虑单点阻挡

	const float leg1 = std::sqrt(distSq1 - radiusSq);
	leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
	rightLegDirection = 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->isConvex_) {
		/* Ignore obstacle. */
		continue;
	}

	obstacle1 = obstacle2; // 只考虑单点阻挡

	const float leg2 = std::sqrt(distSq2 - radiusSq);
	leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
	rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
}
else {
	// 投影点在左右两个端点中间 且投影距离大于圆的半径
	/* Usual situation. */
	if (obstacle1->isConvex_) {
		const float leg1 = std::sqrt(distSq1 - radiusSq);
		leftLegDirection = 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->unitDir_;
	}

	if (obstacle2->isConvex_) {
		const float leg2 = std::sqrt(distSq2 - radiusSq);
		rightLegDirection = 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->unitDir_;
	}
}

上面的代码里有一些求点到圆形的切线方向这样的操作，都是一些数学计算，不怎么好理解，这里使用图例来说明。

点与圆的切点

在上面的图中，红色线段对应的长度的平方为distSq1简称为S，relativePosition1对应的点为P，其半径为radius_简称为r，从O点到这个圆的左边切线与圆的交点为L,则线段LO的长度为leg1简称为leg。假设向量OP与x轴的夹角为m,对应上图中的红色圆弧，而OP与OL之间的夹角为n，对应上图中的紫色圆弧。在这些变量的定义下，我们可以通过下面的方式来计算L的方向： $s in (m) s in (n) cos (m + n) s in (m + n) = P . y / S, cos (m) = P . x / (S) = r / S, cos (n) = l e g / (S) = cos (m) * cos (n) - s in (m) * s in (n) = P . x * l e g / S - P . y * r / S = (P . x * l e g - P . y * r) / S = s in (m) * cos (n) + s in (n) * cos (m) = P . y * l e g / S + P . x * r / S = (P . x * r + P . y * l e g) / S$ 所以leftLegDirection计算的就是左切线的单位向量。对应的右切线的单位向量计算方式如下： $cos (m - n) s in (m - n) = cos (m) * cos (n) + s in (m) * s in (n) = P . x * l e g / S + P . y * r / S = (P . x * l e g + P . y * r) / S = s in (m) * cos (n) - s in (n) * cos (m) = P . y * l e g / S - P . x * r / S = (- P . x * r + P . y * l e g) / S$ 可以看出这个结果与上面的rightLegDirection是完全对的上的。

在搞清楚上面的leftLegDirection和rightLegDirection的计算公式之后，对应的分支判断也就非常清楚了。上面代码的第一个分支对应下图中的a，直线相交且最近点为左端点，此时以这个左端点为圆心画一个半径为radius_的浅色的圆，agent位置与这个圆的切线夹角区域所代表的速度在将来的某一个时候会引发碰撞，所以以这两个切线作为VO区域的左右两条腿。第二个分支对应下图中的b，直线相交且最近点为右端点。第三个分支对应下图中的c，最近点为agent位置到这个直线的投影点。每个子图中的浅绿色线段代表阻挡线段所在的直线，浅黄色线段代表阻挡边向量，紫色线段代表当前agent位置出发的与阻挡线段垂直的向量，蓝色向量代表左边的腿，红色向量代表右边的腿，注意其箭头方向。

判断阻挡直线是否与agent相交

在构造完成VO区域的两条腿之后，我们需要做一下凸包修正：

// 避免两条腿边会指向回当前凸包阻挡体
const Obstacle *const leftNeighbor = obstacle1->prevObstacle_;

bool isLeftLegForeign = false; // 左腿边是否会指向当前凸包内部
bool isRightLegForeign = false; // 右腿边是否会指向回凸包内部
// 如果指向回了凸包内部 则代表不需要考虑 因为在其他边的处理中已经考虑过了

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

if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) {
	/* Right leg points into obstacle. */
	rightLegDirection = obstacle2->unitDir_;
	isRightLegForeign = true;
}

计算isLeftLegForeign的图形如下,左边的子图代表leftLegDirection会指向原来的凸包内部，棕色的边代表前一条边，此时需要修正方向为前一条边的单位方向乘以-1，即新添加的从原点位置延申出来的棕色腿,右子图则不需要：

计算isLeftLegForeign

计算isRightLegForeign的图形如下，左边的子图代表rightLegDirection会指向原来的凸包内部，棕色的边代表后一条边，此时需要修正方向为前一条边的单位方向，即新添加的从原点位置延申出来的棕色腿,右子图则不需要：

计算isRightLegForeign

修正完了之后，再来计算当前速度是否在时间t内会触发与这个边的碰撞：

// 底面胶囊体的左右两个中心点
const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_);
const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_);
// cutoffvec底面胶囊体中心的连线 会与VO的底面边平行
const Vector2 cutoffVec = rightCutoff - leftCutoff;

/* Project current velocity on velocity obstacle. */

/* Check if current velocity is projected on cutoff circles. */
// t代表当前速度减去底面胶囊左中心之后的向量投影到底面向量的带符号长度
const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec));
// tleft代表当前速度在左腿上的投影向量
const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection);
// tright代表当前速度在右腿上的投影向量
const 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. */
	const Vector2 unitW = normalize(velocity_ - leftCutoff);
	// 选择逃离左圆心的速度 然后初始点定位与leftcutoff的距离为当前圆的半径 这样就保证继续以运行下去在t时间内都不会与左圆心碰撞
	line.direction = Vector2(unitW.y(), -unitW.x());
	line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW;
	orcaLines_.push_back(line);
	continue;
}
else if (t > 1.0f && tRight < 0.0f) {
	/* Project on right cut-off circle. */
	const Vector2 unitW = normalize(velocity_ - rightCutoff);
	// 选择逃离右圆心的速度 然后初始点定位与leftcutoff的距离为当前圆的半径 这样就保证继续以运行下去在t时间内都不会与右圆心碰撞
	line.direction = Vector2(unitW.y(), -unitW.x());
	line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW;
	orcaLines_.push_back(line);
	continue;
}

这里的两个分支讨论基本是一样的，我们只需要解释第一个分支的代码，此时agent的速度对应的图形如下：

left safe

由线段的左端点构造出两条分割线。这里的长的紫色线段与阻挡线段垂直，对应的左侧半平面满足t<0.0f。同时绿色线段与VO的左腿垂直，对应的右侧半平面满足tLeft <0.0f，两条新添加的直线构造的半平面交（以棕色填充区域来表示）内的速度就能满足t < 0.0f && tLeft < 0.0f。如果agent的速度va处于这个区域，则远离阻挡边最快的方向就是左端点到速度点的连线方向。所以此时添加的orca线会以逃离leftCutoff的方向作为最优速度来构造，同时为了为了让orca线对应的区域最大化，选取了这个方向与左边端点对应的半圆弧的交点作为经过的点。

最后讨论的是一般情况下，当前速度会在时间t内的某个时刻与阻挡边相交:

/*
	* Project on left leg, right leg, or cut-off line, whichever is closest
	* to velocity.
	*/
const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec)));
const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection)));
const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection)));
// 获取当前速度到这三条线段或者射线的最短距离
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
	/* Project on cut-off line. */
	line.direction = -obstacle1->unitDir_; // 这里取反方向因为要保证法线方向朝orca区域的内部 远离vo
	line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
	orcaLines_.push_back(line);
	continue;
}
else if (distSqLeft <= distSqRight) {
	/* Project on left leg. */
	if (isLeftLegForeign) {
		continue;
	}
	// 法线方向永远是当前direction 加90度 以保证朝外
	line.direction = leftLegDirection;
	line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
	orcaLines_.push_back(line);
	continue;
}
else {
	/* Project on right leg. */
	if (isRightLegForeign) {
		continue;
	}
	// 右边界需要乘以负一 以保证其法线方向会远离当前vo
	line.direction = -rightLegDirection;
	line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
	orcaLines_.push_back(line);
	continue;
}

上面的代码会计算当前速度与VO的哪一条边距离最近，下图中三条浅绿色的线段就是当前速度到三条边的垂直距离，选择距离最近的那一条边的方向同时会以radius_ * invTimeHorizonObst为偏移距离来构造orca分割线，这样就保证时间t内当前agent不会与当前障碍物相撞:

选取最短距离的边来构造orca

构造Agent之间的ORCA半平面

在消化了如何从静态障碍物构造ORCA半平面之后，对于后续的从agent构造ORCA半平面的流程就更容易理解了。这里也是分类讨论，首先判断当前两个Agent现在是否已经发生碰撞，根据是否已经撞上了走不同的分支逻辑：

const Agent *const other = agentNeighbors_[i].second;

const Vector2 relativePosition = other->position_ - position_;
const Vector2 relativeVelocity = velocity_ - other->velocity_;
const float distSq = absSq(relativePosition); // 相对速度的平方
const float combinedRadius = radius_ + other->radius_;
const float combinedRadiusSq = sqr(combinedRadius);

Line line;
Vector2 u;

if (distSq > combinedRadiusSq) 
{
	// 这部分代码处理当前没有碰撞的情况
}
else
{
	/* Collision. Project on cut-off circle of time timeStep. */
	// 已经撞上了 则 选取当前速度与相对圆心的连线向量 
	const float invTimeStep = 1.0f / sim_->timeStep_;

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

	const float wLength = abs(w);
	const Vector2 unitW = w / wLength;
	// 照这个方向执行移动可以在sim_->timestep之后脱离接触
	line.direction = Vector2(unitW.y(), -unitW.x());
	u = (combinedRadius * invTimeStep - wLength) * unitW;

}

当发现相撞之后，RVO2力图在时间t内将两者脱离接触，此时最快的脱离速度方向就是两点连线的逆方向，也就是上文中的w，其单位向量为unitW，由这个逃逸速度方向构造的orca分割线正向就是顺时针旋转90度之后的Vector2(unitW.y(), -unitW.x())。其对应的示意图如下：

agent出现碰撞

上图中灰色的圆代表平移坐标系后的相对圆，此时原点在圆内，所以目前已经相交。而橙色小圆则代表这个相对圆除以t之后形成的圆。根据当前的相对速度relativeVelocity是否在这个小圆内可以画出两种不同的结果。途中的浅绿色线段代表两种不同情况的relativeVelocity，此时两个对应的黄色端点就是最后算出来的u，而紫色线段就代表unitW对应的方向，经过黄色端点并与紫色线段垂直的就是两种情况下的orca分割线，可以看出都是unitW顺时针旋转90度之后形成的。

在没有处于碰撞的情况下，就是经典的锥形区域的ORCA分析，根据与VO的最优逃逸速度方向计算方式可以分为三种情况：

/* No collision. */
// 小圆圆心invTimeHorizon * relativePosition
// w为小圆圆心指向当前速度位置的向量
const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
const float wLengthSq = absSq(w);
// w向量在relativePosition向量上的投影
const float dotProduct1 = w * relativePosition;

// 以上条件都满足，也就是说在小圆弧上计算u
if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) {
	/* Project on cut-off circle. */
	const float wLength = std::sqrt(wLengthSq);
	const Vector2 unitW = w / wLength;
	// 直接以这个向量作为法向 此时direction需要顺时针旋转90度
	line.direction = Vector2(unitW.y(), -unitW.x());
	u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else {
	/* Project on legs. */
	const float leg = std::sqrt(distSq - combinedRadiusSq);

	if (det(relativePosition, w) > 0.0f) {
		/* Project on left leg. */
		// 更靠近左边的腿 计算方向为左腿与大圆的切线方向
		line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
	}
	else {
		/* Project on right leg. */
		// 注意右腿需要乘以-1
		line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
	}
	// dotProduct2是相对速度点到腿上的投影长度
	const float dotProduct2 = relativeVelocity * line.direction;
	// 相对速度点到腿上的投影向量，减去相对速度向量，就得到了u 
	u = dotProduct2 * line.direction - relativeVelocity;
}

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

这里比较难理解的就是刚开始的判断条件如何定位到当前速度点离VO边界上最近的点是在VO的圆弧区域上。

落在圆弧区域上

设小圆圆心为A，大圆圆心为B，则 OB=t*OA。当前相对速度relativeVelocity为原点出发的蓝色线段OV，则w就是图中小圆圆心引出的橙色线段AV。假设w与AO的夹角为m，则dotProduct1 = w*OB = |OB| * |w| * cos(180-n)。所以dotProduct1 <0.0时n<90, 此时V处于由A出发的与OB垂直的分割线（图中经过A的蓝色线段）引发的下方半平面中。

取两条腿与小圆的任意一个切点为C，此时AC即为上图中A出发的绿色线段。设AC与AO之间的夹角为m，则cos(m) = |AC|/|AO|= combinedRadius/|BO|, 即combinedRadius = |BO| * cos(m),所以sqr(dotProduct1) > combinedRadiusSq * wLengthSq可以转化为: $∣ OB ∣^{2} * ∣ w ∣^{2} * cos (n)^{2} cos (n)^{2} > ∣ OB ∣^{2} * cos (m)^{2} * ∣ w ∣^{2} > cos (m)^{2}$

由于dotProduct1 <0.0限定了n是一个锐角，同时由于m也是一个锐角，所以可以推断出m > n，所以此时V处于两个切点构造的两条AC形成的锥形区域, 即下图中的阴影区域内：

落在圆弧上2

处于这个区域内的V有两种情况，分别是在小圆中或者不在小圆中。如果在小圆中，代表此速度下会发生碰撞，此时能够避免碰撞同时与当前速度差值最小的速度为w延伸到小圆的圆周上交点。如果不在小圆中，代表此速度下不会发生碰撞，此时能够触发碰撞且与当前速度差值最小的速度为w回缩到小圆的圆周上的交点。两种情况下的最优速度方向都是w，其对应的ORCA线方向需要顺时针旋转90度，即 Vector2(unitW.y(), -unitW.x())。此时的VC向量就是u，其带符号长度（正值代表w向量延申，负值代表w向量收缩）就是小圆的半径减去w的长度，也就是combinedRadius - invTimeHorizon - wLength。

剩下的两个分支讨论就是V到两个腿的距离会比V到小圆的外侧圆弧距离更短的情况，内部分为了离左腿更短还是离右腿更短，此时line.direction的赋值都使用了之前介绍的点到圆的切线方向计算公式，注意如果是右边的切线对应的ORCA分割线需要反向。在三种分支都讨论完成之后，执行line.point = velocity_ + 0.5f * u来设定半平面分割线应该经过的点，这里的0.5代表就是agent之间各自避让一半。

对避障速度区域进行求解

上面计算好了所有的ORCA线之后，RVO开始对这些ORCA线定义的半平面求交，并获取一个可行避障速度。这部分的代码就这一行：

size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);

这个函数的定义如下：

size_t linearProgram2(const std::vector<Line> &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
{
	if (directionOpt) {
		// 此时要求optVelocity的长度为1
		result = optVelocity * radius;
	}
	else if (absSq(optVelocity) > sqr(radius)) {
		// 对于超过最大速度的时候 拉回最大速度
		result = normalize(optVelocity) * radius;
	}
	else {
		// 否则不对最大速度进行修改
		result = optVelocity;
	}

	for (size_t i = 0; i < lines.size(); ++i) {
		if (det(lines[i].direction, lines[i].point - result) > 0.0f) {
			/* 当前result处于lines[i]对应的半平面之外 会导致与lines[i]对应的VO发生碰撞 需要对result进行修正 */
			const Vector2 tempResult = result;

			if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
				result = tempResult;
				return i;
			}
		}
	}

	return lines.size();
}

这里的radius参数对应的并不是agent的碰撞半径，而是最大可能速度，optVelocity代表当前不考虑阻挡情况下的最优速度，directionOpt这个代表传入的速度已经被单位化了需要恢复到最大速度，result参数代表最后算出来的最优速度。而函数的返回值代表内部迭代过程执行到第几条ORCA线的时候发现半平面交集为空，如果返回值等于分割线数组的大小，代表相交区域不为空。内部其实就是一个非常简单的循环，以此遍历所有的分割平面，判断当前result是否在这个分割平面内，如果不在则需要对result进行修正。这里的det(A,B)>0就代表从A逆时针旋转到B所需要的夹角要大于180度，也就是说B点不在A向量定义的经过原点的半平面内。

这里我们再提供一个凸优化中的断言：一个凸函数在凸区域的极值点一定在这个凸区域的边界上。所以为了求Agent在这些ORCA线组成的半平面交的与最优速度相差最小的最优速度，只需要处理这些半平面交的边界线上的所有点即可。所以如果result满足了前面i-1条线所定义的半平面交但是不满足lines[i]定义的半平面交，则我们只需要处理lines[i]与前i-1条线所形成的凸区域的相交线段并获取这条线段上最优速度即可。这里的速度修正调用的linearProgram1函数定义如下：

bool linearProgram1(const std::vector<Line> &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)

参数中的radius还是最开始传入的最大速度，接下来我们将这个函数体按步骤进行解析：

const float dotProduct = lines[lineNo].point * lines[lineNo].direction;
const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point);

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

dotProduct计算的是这段代码的意思lines[lineNo].point这个向量投影到这条线的长度，absSq(lines[lineNo].point) - sqr(dotProduct)则计算的是原点到这条ORCA线的距离h的平方。如果discriminant小于零代表，原点到这条ORCA的距离h大于了最大速度radius，则说明在agent在速度长度限制下的任意速度都不会与当前ORCA线的半平面产生交集。因此Agent会与当前orca线所代表的物体相撞，无法找到一个可行避障速度，此时对应下面的图例：

最大速度不在有效区域内

上图中的P点就是lines[lineNo].point,H点是原点O到这个线的最短距离点，所以OH与HP垂直, HP等于dotProduct，sqr(radius) - OH*OH = sqr(radius) - (OP*OP - HP*HP) = discriminant。

接下来分析radius半径大小的圆与当前分割线有交点的时候，此时的图例如下：

最大速度在有效区域内

这里设最大速度半径圆形与分割线的交点分别为L,R, OL在OH左侧，而OR在OH右侧，代码中首先求出这两个交点的位置：

const float sqrtDiscriminant = std::sqrt(discriminant);
float tLeft = -dotProduct - sqrtDiscriminant;
float tRight = -dotProduct + sqrtDiscriminant;

此时|HR| = |HL| = sqrt(sqr(radius) - OH*OH) 也就是sqrtDiscriminant，此时向量PL等于-HP - HR,长度等于HP + HR，所以此时tLeft代表的是向量PL在这条线的正方向的带符号长度，对应的tRight就代表了向量PR在这条线的正方向的带符号长度。对应的任意t，如果满足了tLeft<=t<=tRight，则代表OP + t * line.direction所代表的点T一定在线段LR内，此时OT代表的速度刚好可以保证当前Agent与这条ORCA线对应的VO相切。

接下来要考虑前面的lineNo-1条分割线组成的凸区域S与lines[lineNo]的相交线段，这个线段一定在LR之内，因为在直线lines[lineNo]上只有LR线段上的点才能保证不碰撞，而S区域内的任意点都不会触发碰撞。求相交线段的流程其实就是遍历之前的所有直线与当前lines[lineNo]的相交点，然后更新LR线段：

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

	if (std::fabs(denominator) <= RVO_EPSILON) {
		/* 说明是平行线 */
		if (numerator < 0.0f) {
			// 如果平行线方向完全相反 同时由于当前result 不满足lines[lineNo], 则说明交集区域肯定为空
			return false;
		}
		else {
			continue;
		}
	}

	const float t = numerator / denominator;

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

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

设遍历过程中lines[i].point为Q，同时两条直线的交点为T，此时令向量PT = t * lines[lineNo].direction,则向量TQ = OT - OQ = OP + PT - OQ 与lines[i].direction共线，即det(TQ, lines[i].direction) = 0, 由于向量的叉积也是满足加法的，所以det(QP + PT , lines[i].direction) = 0进一步可以拆解为det(PT, lines[i].direction) = det(PQ, lines[i].direction) = numerator。又因为det(PT, lines[i].direction) = t* det(lines[lineNo].direction, lines[i].direction) = t * denominator，所以最后可以得到t = numerator / denominator。

我们上一步求出了T点的坐标，但是我们需要的是获取经过T点且与lines[lineNo].direction共线的满足lines[i]半平面需求的射线，获取了这条射线之后在与之前求出来的LR线段求交，用求交后的结果线段来更新LR，这样就可以保证LR线段是[0,lineNo]这些分割线所定义的凸区域的一条边界。用T去更新LR需要明确这个射线方向上t是逐渐增大的还是逐渐减小的。当t是逐渐增大时则T可以用来替换L，而t是逐渐减小时则T可以用来替换R。

边界线段更新

在上面的图中，我们提供了两个用来测试的直线：

对于紫色的长直线L1，其交点为T1，由于两条分割线方向的叉积det(L,L1)为正，此时的紫色短射线代表所求的射线，t需要逐渐减小，所以需要去替换端点R。
对于黄色的长直线L2，其交点为T2，由于两条分割线方向的叉积det(L,L2)为负, 此时的黄色短射线代表所求的射线，t需要逐渐减增大，所以需要去替换端点L。

如果迭代过程中发现L >R，则说明当前[0,lineNo]这些分割线所定义的凸区域为空，此时返回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 {
	const 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;

如果开启了directionOpt选项，则代表尽可能的选择与最优速度夹角最小的点
如果关闭了directionOpt选项，则代表尽可能的选择与最优速度偏差最小的点，为此这里需要首选计算出最优速度在这条直线上的投影点Q对应的向量PQ在当前分割线方向上的带符号长度t，根据t的取值来选择偏差最小的点

在无可行解情况下的处理

在VO比较稠密的情况下之前的linearProgram2可能没有可行解，所以此时要按照之前论文里介绍的方法来获取一个比较安全的速度。

size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);

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

这个调整方法linearProgram3就是将所有的分割线都朝外侧拉远一定距离h，直到交集区域出现了一个点v，此时的v就是所求的一个比较安全的速度，这个往外侧拉远同样距离的调整可以看作允许Agent之间发生一定程度的碰撞。不过对于静态障碍物来说任何碰撞都是不允许的，所以这个函数最终的实现如下：


void linearProgram3(const std::vector<Line> &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result)
{
	float distance = 0.0f;

	for (size_t i = beginLine; i < lines.size(); ++i) {
		if (det(lines[i].direction, lines[i].point - result) > distance) {
			// 如果当前速度点到这条orca线的距离大于了之前调整的distance 则说明我们需要继续增大distance

			// 所有的静态障碍物构造出来的orca线都不能调整 直接全复制
			std::vector<Line> projLines(lines.begin(), lines.begin() + static_cast<ptrdiff_t>(numObstLines));
			// 遍历所有的非障碍物引发的orca线
			for (size_t j = numObstLines; j < i; ++j) {
				Line line;

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

				if (std::fabs(determinant) <= RVO_EPSILON) {
					// 两条直线平行
					if (lines[i].direction * lines[j].direction > 0.0f) {
						// 相同方向则不管
						continue;
					}
					else {
						// 不同方向 则将line向外调整 处于当前两条线的中间
						line.point = 0.5f * (lines[i].point + lines[j].point);
					}
				}
				else {
					// 这里将这条线的point调整为路过两者的交点
					line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
				}
				line.direction = normalize(lines[j].direction - lines[i].direction);
				projLines.push_back(line);
			}

			const Vector2 tempResult = result;
			// 以当前line[i]的法向作为最优速度方向 开启方向优化来计算调整后的最优速度
			if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) {
				// 这个分支应该不可能进入 因为总是可以通过外移一段距离来获取非空交集
				// 作者解释说 可能出现了浮点误差
				result = tempResult;
			}
			// 获取了调整后的最优速度之后，计算这个速度到原来的分割线的距离
			distance = det(lines[i].direction, lines[i].point - result);
		}
	}
}

上面代码里最难以弄懂的就是line.direction = normalize(lines[j].direction - lines[i].direction)的更新操作，对于平行线但是逆向的情况下，这行代码执行与不执行没啥区别，此时新的line是原来两条老的line的平分线，任何在新line上的点到原来两条平行线的距离都相等。对于非平行情况下就比较费解了，这里我们用图例来展示一下这行代码的意图：

orca构造角平分线

这里我们用直线J代表lines[j]，直线I代表lines[i]、以两直线相交点为圆心做一个半径为1的圆形，lines[j].direction - lines[i].direction就是上图中的线段n，保持这个n的方向并平移到这个圆心得到了直线m，此时我们可以证明直线m就是直线J与直线-I之间的角平分线。在角平分线上的任意一点到直线I的距离都等于到直线J的距离。上图中的虚线I和虚线J就是原来的实线I与实线J向外扩展距离h之后形成的新分割线，这两条虚线的交点到原来的两条实线的交点都是h。同时这条角平分线对应的半平面内部（不在边界上）的点到直线J的距离一定会大于到直线I的距离。由于在凸区域上凸函数的极值点一定在这个区域的边界上，所以最终算出来的最优速度到任意直线I的距离一定不大于到直线J的距离，同时存在一条直线I使得最优速度点到直线I与直线J的距离相等。因此此时计算出来的distance就是在前J条线的限制下需要让交集区域不为空的最小外延距离。

深入理解游戏服务端:从网络通信到无缝迁移