# ORCA-Test
**Repository Path**: wanfengqu/orca-test
## Basic Information
- **Project Name**: ORCA-Test
- **Description**: No description available
- **Primary Language**: C#
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No
## Statistics
- **Stars**: 0
- **Forks**: 10
- **Created**: 2024-03-06
- **Last Updated**: 2024-03-06
## Categories & Tags
**Categories**: Uncategorized
**Tags**: None
## README
# ORCA-Test
#### 介绍
{**以下是 Gitee 平台说明，您可以替换此简介**
Gitee 是 OSCHINA 推出的基于 Git 的代码托管平台（同时支持 SVN）。专为开发者提供稳定、高效、安全的云端软件开发协作平台
无论是个人、团队、或是企业，都能够用 Gitee 实现代码托管、项目管理、协作开发。企业项目请看 [https://gitee.com/enterprises](https://gitee.com/enterprises)}
#### 软件架构
软件架构说明
https://mp.weixin.qq.com/s/ZxAyBlSgjiQKVjQHHgx1ew
#### 安装教程
1. xxxx
2. xxxx
3. xxxx
#### 使用说明
1. xxxx
2. xxxx
3. xxxx
#### 参与贡献
1. Fork 本仓库
2. 新建 Feat_xxx 分支
3. 提交代码
4. 新建 Pull Request
#### 特技
1. 使用 Readme\_XXX.md 来支持不同的语言，例如 Readme\_en.md, Readme\_zh.md
2. Gitee 官方博客 [blog.gitee.com](https://blog.gitee.com)
3. 你可以 [https://gitee.com/explore](https://gitee.com/explore) 这个地址来了解 Gitee 上的优秀开源项目
4. [GVP](https://gitee.com/gvp) 全称是 Gitee 最有价值开源项目，是综合评定出的优秀开源项目
5. Gitee 官方提供的使用手册 [https://gitee.com/help](https://gitee.com/help)
6. Gitee 封面人物是一档用来展示 Gitee 会员风采的栏目 [https://gitee.com/gitee-stars/](https://gitee.com/gitee-stars/)
Unity 的 DOTS 被广泛应用于高性能的游戏项目开发中，比如需要大量处理实体、物理计算、角色 AI 等的游戏。现在使用 DOTS 开发的游戏，很多都会有控制大批量角色的需求。如何控制大批量角色移动和避障是角色 AI 的需求之一。
本文利用 DOTS 高性能这一特性，详细分析 ORCA（Optimal Reciprocal Collision Avoidance）算法的原理，并使用 DOTS 进行算法实现。
ORCA 避障算法是一个经典的算法，源自 2011 年的文章《Reciprocal n-body Collision Avoidance》[1]，目前已被广泛应用于许多领域，有不同计算机语言的实现版本。
虽然网上已经有许多针对 ORCA 的算法原理讲解，但是要么仅仅单纯的翻译原文，要么讲解得比较粗略，很少有能细致讲解清楚的，下面详细介绍一下算法原理和代码实现。
1. 算法原理
1.1. VO（Velocity Obstacle）
假设有 A、B 两个运动中的圆形物体，如下图所示。
图片
为了判断下一时间点能不能相撞，可以简化一下上面的模型。将圆 A 简化成一个质点 p
A
，那么 B 就要相应地变成半径为 r
A
+r
B
的圆。运行的下一时刻，只要点 p
A
不出现在下图中阴影区域，圆 A 和圆 B 就不会碰撞。
图片
为了后续计算简便，将上面的坐标系转换为以点 p
A
为参照的坐标系，也就是将 p
A
当作坐标系原点进行计算。
图片
计算避障的核心是控制物体的速度，因此坐标系转需要从上面的位置坐标系转换为速度坐标系。也就是说，X 轴表示 X 方向的速度，即 X 方向上单位时间移动的距离，Y 轴同理。因此只需要位置坐标系的所有坐标都除以单位时间τ，就可以得到速度坐标系坐标了。
图片
上图中，因为所有坐标都除以了 τ，因此阴影区域的形状也发生了改变，原来以 p
A
+p
B
为圆心、 r
A
+r
B
为半径的大圆，变成了以 (p
A
+p
B
)/τ 为圆心、 (r
A
+r
B
)/τ 为半径的小圆。根据三角形相似原理，可以很容易推导出：
• 大圆和小圆仅有一个交点；
• 大圆和小圆都内切于阴影区域。
在速度坐标系下，每一个坐标点都代表一个速度（Velocity）值，因为这个坐标系是以圆 A 为参考建立的相对速度坐标系，因此每一个坐标点实际代表的是圆 A 的相对速度，记作 v
A
−v
B
。坐标图中，这片阴影区域叫做速度障碍（VO，Velocity Obstacle），记作 VO
A∣B
τ
。
VO
A∣B
τ
是以圆 A 为参照建立坐标系求得的，那么以圆 B 为参照建立坐标系求得的 VO，我们记作 VO
B∣A
τ
。 VO
A∣B
τ
和 VO
B∣A
τ
相对于速度坐标系原点对称。
1.2. 避障速度集合
因为 A 的相对速度坐标系下，每一个坐标点都是圆 A 的相对速度，也就是 v
A
−v
B
。因此，如果 v
B
取值范围不同，那么为了避免圆 A 和圆 B 发生碰撞，因此 v
A
也需要有对应的速度取值范围。这个问题就演变成以下数学问题：
设圆 B 的速度取值范围为集合 V
B
，如果 v
B
∈V
B
，为了避免圆 A 和圆 B 发生碰撞，求圆 A 的速度取值范围集合 V
A
。
图片
如上图所示，设速度 v
B
的取值范围为集合 V
B
，也就是蓝色填充区域，那么 V
B
的闵可夫斯基和表示为 VO
A∣B
τ
⊕V
B
。这里的闵可夫斯基和可以形象地理解为以 VO
A∣B
τ
为笔刷，以平移 ∣v
A
−v
B
∣ 个单位距离的 V
B
为笔刷路径（如蓝色虚线框所示），涂抹出来的区域集合（灰色区域）。
因此我们可以得出结论，只要圆 A 的速度 v
A
不在 VO
A∣B
τ
⊕V
B
范围内，两个圆就不会碰撞。也就是说，如果 v
A
∈
/
VO
A∣B
τ
⊕V
B
，则两个圆就不会碰撞。
在实际工程中，程序执行的每个时刻，我们总是希望赋予圆（也就是机器人）一个期望移动速度 v
opt
，让圆尽可能按照这个速度移动。这个期望移动速度可能会导致圆之间互相碰撞，也就是说，有可能出现 v
A
opt
−v
B
opt
∈VO
A∣B
τ
。
通常机器人的期望移动速度 v
opt
指向它的目的地。
但是没有关系，我们可以根据期望移动速度，求出一个闵可夫斯基和。令实际的移动速度 v
new
在这个闵可斯基和的范围之外，就可以保证圆在接近期望移动速度的情况下移动，且不会出现互相碰撞。
图片
如图所示，可以看出 v
A
opt
−v
B
opt
∈VO
A∣B
τ
，如果圆 A 以期望移动速度 v
A
opt
移动，那么将会与圆 B 碰撞。为了避免碰撞，首先求出坐标点 v
A
opt
−v
B
opt
到 VO
A∣B
τ
边缘最近的向量
u
，向量
u
的方向和切平面的法线
n
一致。然后，在离 v
A
opt
距离 ∣
u
∣∗w 的地方，沿着圆切线方向求闵可夫斯基和，就可以得到圆 A 的实际移动速度 v
new
允许的范围，记作 ORCA
A∣B
τ
。
上面的求解里，还有一个 w 没有解释，这是一个权重值。ORCA 算法认为，避障是双方的责任，也就是当圆 A 和圆 B 相遇时，互相都要避让对方。这个权重值就是避让的权重，这里可以默认其为 0.5。
当多个 ORCA 区域重叠时，取其交集，也就是实际移动速度最终允许的范围，如下图所示为圆 A 的实际移动速度取值范围，也就是最终所求的避障速度集合。
图片
当然也会出现没有交集的情况，这种情况的处理办法会在后续讲解代码时具体说明。
1. 代码实现
如果需要完整源码，请参看代码仓库 https://gitee.com/zd304/orca-test。
2.1. DOTS 代码结构
本小节通过分析 ECS 的结构，概括性地了解整体的代码结构。
2.1.1. Component
为每个圆生成如下避障代理组件，因此后文中的圆和 Agent 是同一个意思。
public struct Agent : IComponentData
{
///
/// 代理ID，全局唯一
///
public int id;
///
/// 当前位置
///
public float2 position;
///
/// 期望速度，也就是v^opt
///
public float2 prefVelocity;
///
/// 当前速度
///
public float2 velocity;
///
/// 圆的半径
///
public float radius;
///
/// 避障责任权重
///
public float weight;
///
/// 最大速率
///
public float maxSpeed;
///
/// KDTree查找的最大相邻Agent数量
///
public int maxNeighbors;
///
/// KDTree查找相邻Agent时候的最大检测距离
///
public float neighborDist;
///
/// 提前避障的时间（可以大于帧间隔deltaTime，表示要提前避障）
///
public float timeHorizon;
///
/// 移动的目标位置，用来计算期望速度prefVelocity
///
public float2 targetPos;
}
每个字段注释已经写清楚用途，不再赘述。其中需要用户配置的数据主要是以下几个。
public class AgentAuthoringAuthoring : MonoBehaviour
{
[Header("避障半径")]
public float radius;
[Header("避障责任权重")]
public float weight;
[Header("KDTree查找的最大相邻Agent数量")]
public int maxNeighbors;
[Header("KDTree查找相邻Agent时候的最大检测距离")]
public float neighborDist;
[Header("提前避障的时间")]
public float timeHorizon;
[Header("预期移动速率")]
public float speed;
private class AgentAuthoringBaker : Baker
{
......
}
}
2.1.2. System
Entities 里为每个圆组织好以上组件数据，就可以到 System 里去访问并使用这些数据进行位移计算了。
处理 Agent 组件移动的 System 命名为 SimulatorSystem，其主要通过以下步骤完成 Agent 对应实体的移动：
1. 构建 KDTree；
2. 计算预速度；
3. 从 KDTree 里查找附近的圆；
4. 计算新的移动速度；
5. 根据新的移动速度，更新位置。
public partial struct SimulatorSystem : ISystem
{
......
[BurstCompile]
public partial struct MoveJob : IJobEntity
{
public KDTree kdTree;
public float deltaTime;
///
/// 遍历每一个Agent，并更新位置
///
private void Execute(Entity entity, [EntityIndexInQuery] [SuppressMessage("ReSharper", "UnusedParameter.Local")] int entityInQueryIndex,
ref Agent agent, ref LocalTransform transform)
{
// 2. 计算预期速度
agent.prefVelocity = math.normalizesafe(agent.targetPos - agent.position) \* agent.maxSpeed;
float prefDistSq = math.lengthsq(agent.targetPos - agent.position);
if (prefDistSq < RVOMath.sqr(agent.radius))
{
agent.prefVelocity = float2.zero;
}
float2 newVelocity;
NativeList neighbors = new NativeList(agent.maxNeighbors, Allocator.Temp);
neighbors.Clear();
// 3. 从KDTree里查找附近的圆
ComputeNeighbors(ref agent, ref neighbors);
// 4. 计算新的移动速度
ComputeNewVelocity(ref agent, in neighbors, out newVelocity);
// 5. 根据新的移动速度，更新位置
Update(ref agent, newVelocity);
transform.Position = new float3(agent.position.x, 0.0f, agent.position.y);
neighbors.Dispose();
}
}
......
[BurstCompile]
public void OnUpdate(ref SystemState state)
{
NativeArray agents = agentQuery.ToComponentDataArray(Allocator.TempJob);
KDTree kdTree = new KDTree();
// 1. 构建KDTree
kdTree.BuildAgentTree(ref agents);
state.Dependency = new MoveJob()
{
kdTree = kdTree,
deltaTime = SystemAPI.Time.DeltaTime,
}.ScheduleParallel(state.Dependency);
state.Dependency = agents.Dispose(state.Dependency);
}
}
通过以上代码注释，可以看到实现 Agent 对应实体的移动的主要步骤，下面详细介绍每一步实现。
2.2. KDTree
如果熟悉 KDTree 算法的话，可以跳过本节内容。
根据前文分析 ORCA 算法原理的时候知道，为了计算出合适的避障速度集合，需要计算每一个圆和其他所有圆的 ORCA 半平面，然后求这些半平面的交集。这样的计算量是巨大的，每增加一个圆的实体，其计算量都会呈几何倍数增长，这样的算法不是高效的算法。
为了提高计算的效率，我们仅仅需要关注那些离得比较近的实体间是否有碰撞。对于那些离得比较远的实体，由于其距离已经远大于避障检测的步长，因此他们是肯定不会碰撞的，这种情况下再去计算他们的 ORCA 半平面，将会是很大的计算浪费。
为了找到距离较近的圆进行避障检测，我们就需要对所有圆所处的位置空间进行空间分割。常用的空间分割方法有 KDTree、四叉树（八叉树）等。通常对均匀分布在场景中的物体做空间分割，会用四叉树（八叉树）来实现，比如《Minecraft》里面的地形盒子，一个格子最多一个地形块，其分布就很均匀，就适合用八叉树来进行空间分割。但是 ORCA 算法面对的对象是不停移动的物体，他们在空间中是不均匀分布的，因此非常适合用 KDTree 来进行空间分割。
KDTree 的本质是一棵二叉树，KDTree 根据物体的位置关系，将物体所处的空间不断二分，如下图所示。
图片
最终将细分的空间中的物体组成一棵二叉树，如下图所示就是组成的 KDTree，通过这棵二叉树很容易经过最多 3 次查询，就查询到目标物体。
图片
2.2.1. KDTree 数据结构
KDTree 里面的主要数据结构如下所示。
public struct KDTree
{
///
/// KDTree的节点
///
private struct AgentTreeNode
{
///
/// Agent的开始索引
///
internal int begin;
///
/// Agent的结束索引
///
internal int end;
///
/// 左叶子节点的AgentTreeNode索引
///
internal int left;
///
/// 右叶子节点的AgentTreeNode索引
///
internal int right;
#region KDTree节点范围
internal float maxX;
internal float maxY;
internal float minX;
internal float minY;
#endregion
}
......
///
/// 本帧的所有代理
///
[ReadOnly] public NativeArray agents;
///
/// KDTree的所有节点
///
[ReadOnly] private NativeArray agentTree;
......
}
根据代码中的 AgentTreeNode 结构可以知道，KDTree 的每一个节点主要保存的是当前节点里所有 Agent 的索引范围，而不是保存的 Agent 的引用。真正的 Agent 引用其实是保存在 agents 这个数组里。
2.2.2. 构建 KDTree
构建 KDTree 的过程非常简单，主要分两个步骤：
• 递归生成 KDTree 节点；
• 找到每一个节点的中间值，执行快速排序算法。
递归生成 KDTree 节点的逻辑很简单，就是通过层次遍历的方式，遍历生成每一层子节点。
生成了当前节点后，需要判断当前节点的 AABB 包围盒的长和宽的关系。
• 如果包围盒的长大于宽，那么以竖直方向边上的中点 Y 值作为参考值：
• 将小于中点值的 Agent 移动到数组左边，归为左子节点；
• 将大于中点值的 Agent 移动到数组右边，归为右子节点。
• 如果包围盒的宽大于长，那么以水平方向边上的中点 X 值作为参考值：
• 将小于中点值的 Agent 移动到数组左边，归为左子节点；
• 将大于中点值的 Agent 移动到数组右边，归为右子节点。
以上“找中点 → 移动数组元素”的过程，其实就是快速排序算法的过程。
图片
2.2.3. 查找相邻 Agent
查找指定 Agent 的相邻 Agent，需要给查找函数传入查找范围的引用 ref float rangeSq。之所以要传入引用是因为查找函数会动态调整这个值。
查找函数有主要步骤为：
• 递归遍历 KDTree 的节点；
• 叶子节点的所有 Agent 插入返回列表。
KDTree 的递归遍历，受限于检测距离 rangeSq，并非遍历全部节点。也就是说，只有离当前 Agent 距离小于等于 rangeSq 的节点才会被遍历到。
图片
如上图所示，因为 distSqLeft < rangeSq < distSqRight，所以只遍历左子节点。
如果当前 Agent 的位置在节点包围盒内，则当前 Agent 到包围盒的距离视为 0。
当遍历到叶子节点的时候，会将在检测距离范围内的叶子节点的所有 Agent 插入返回列表。插入过程中会遇到以下情况，分别按照以下描述的方法处理：
• 如果返回列表的元素数量小于当前 Agent 的最大相邻数量，则直接插入。
• 如果返回列表的元素数量大于等于当前 Agent 的最大相邻数量，说明不需要返回那么多相邻 Agent，则替换距离最远的一个 Agent，并且缩小检测范围 rangeSq。
最后，按照距离由近到远对返回列表进行排序。
完整的查找流程如下图所示。
图片
2.3. ORCA 避障代码
ORCA 算法的避障代码就是函数 SimulatorSystem.MoveJob.ComputeNewVelocity，下面是主要代码。
private void ComputeNewVelocity(ref Agent agent, in NativeList neighbors, out float2 newVelocity)
{
NativeList orcaLines = new NativeList(Allocator.Temp);
// 提前避障时间的倒数
float invTimeHorizon = 1.0f / agent.timeHorizon;
for (int i = 0; i < neighbors.Length; ++i)
{
Agent other = neighbors[i].agent;
// 相对位置：pB - pA
float2 relativePosition = other.position - agent.position;
// 相对速度：vA - vB = (pA - pB) / τ
float2 relativeVelocity = agent.velocity - other.velocity;
// 两个Agent之间的距离的平方
float distSq = math.lengthsq(relativePosition);
// 大圆的半径：rA + rB
float combinedRadius = agent.radius + other.radius;
// 两个Agent半径和的平方
float combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line = new Line();
float2 u;
// 两个Agent之间的距离大于两者半径的和，说明还没有碰撞
if (distSq > combinedRadiusSq) {...}
// 已经碰撞
else {...}
line.point = agent.velocity + GetAgentWeight(agent, other.weight) \* u;
orcaLines.Add(line);
}
// 通过线性规划找到ORCA半平面的交集
int lineFail = LinearProgram2(orcaLines, agent.maxSpeed, agent.prefVelocity, false, out newVelocity);
if (lineFail < orcaLines.Length)
{
// 找不交集的情况下，使用3D线性规划计算
LinearProgram3(in orcaLines, agent.weight, 0, lineFail, agent.maxSpeed, ref newVelocity);
}
orcaLines.Dispose();
}
代码第 6 行的 agent.timeHorizon 是提前避障的时间，一般大于等于帧间隔 deltaTime（τ）。之所以有这样一个值，是为了让 Agent 在一定距离外就可以避让其他 Agent，不至于快要撞上了才避让。提前避让的好处是能让整体避让拥堵程度降低；坏处是当前 Agent 容易受到其他 Agent 影响主动避让，导致位置稳定得比较慢。因此，提前避让的时间是一个经验值，设置得好的话，可以很好减轻拥堵程度。
代码第 8 行到第 33 行的 for 循环，计算了每个 Agent 和它的所有相邻 Agent 的 ORCA 半平面，并存放到 orcaLines 列表里，供后续线性规划使用。
代码第 12 行到第 21 行准备了一些后续计算需要用到的数据，如下图所示。
图片
代码第 27 行判断是否 distSq > combinedRadiusSq，也就是判断原点 O 是否在大圆内部，也就是圆 A 和圆 B 是否已经碰撞。这两个条件分支将在 2.3.1.小节和 2.3.2.小节分别分析。
2.3.1. 两个 Agent 已经碰撞
如果两个 Agent 已经碰撞的话，将会执行到下面代码里。
// 已经碰撞
else
{
float invTimestep = 1.0f / deltaTime;
// 小圆圆心到当前速度的向量
float2 w = relativeVelocity - invTimestep \* relativePosition;
float wLength = math.length(w);
float2 unitW = w / wLength;
line.direction = new float2(unitW.y, -unitW.x);
// u指向小圆圆周
u = (combinedRadius \* invTimestep - wLength) \* unitW;
}
这段代码可以用下图的几何模型来表示。
图片
从图上可以看出来，relativeVelocity 就是当前的相对速度，即 v
A
opt
−v
B
opt
。combinedRadius * invTimestep 用数学表达式表达就是 (r
A
+r
B
)/τ ，也就是小圆的半径。向量
w
就是小圆圆心指向 relativeVelocity 的向量，向量
u
就是 relativeVelocity 指向小圆圆周的向量。
求出了
u
，那么 ORCA 半平面就可以确定了，也就是垂直于
u
。
两个已经碰撞的 Agent 的 ORCA 半平面也就求解出来了。
2.3.2. 两个 Agent 未碰撞
如果两个 Agent 未碰撞的话，将会执行到下面代码里。
// 两个Agent之间的距离大于两者半径的和，说明还没有碰撞
if (distSq > combinedRadiusSq)
{
// 小圆圆心invTimeHorizon \* relativePosition
// u为小圆圆心指向当前速度位置的向量
float2 w = relativeVelocity - invTimeHorizon \* relativePosition;
float wLengthSq = math.lengthsq(w);
// w向量在relativePosition向量上的投影
float dotProduct1 = math.dot(w, relativePosition);
// dotProduct1小于0，说明w向量和relativePosition向量的夹角为钝角
// dotProduct1 = wLength \* length(relativePosition) \* cos(夹角)
// RVOMath.sqr(dotProduct1) > combinedRadiusSq \* wLengthSq可简化为：
// length(relativePosition) \* cos(夹角) > combinedRadius
// 以上条件都满足，也就是说在小圆弧上计算u
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq \* wLengthSq) {...}
// 否则，在两条腿上计算u
else {...}
}
这段代码可以用下图的几何模型来表示。
图片
从图上可以看出来，relativeVelocity 就是当前的相对速度，即 v
A
opt
−v
B
opt
。relativePosition 也就是大圆的圆心，这里要看作原点到大圆圆心的向量，记作
p
。向量
w
就是小圆圆心指向 relativeVelocity 的向量。根据上图可知，dotProduct1 其实就是向量
w
在向量
p
上的投影。
如上图所示，此时的 dotProduct1 为负数，那么后续计算出来
u
就会指向小圆圆弧的黄色和绿色部分。
但是，
u
其实不应该指向黄色部分，也就是说，当 v
A
opt
−v
B
opt
出现在黄色线框的区域里面时，
u
应该指向 VO 的一条直线边。后续称这条直线边为 VO 的“腿（leg）”。
因此，除了判断 dotProduct1 是否小于 0，还需要判断是否 RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq 才能决定
u
是否指向小圆的弧。
RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq 这个不等式表达的含义比较隐晦，我们需要对不等式做一下变换。
不等式用数学表达式书写为 dotProduct1
2
>combinedRadius
2
∗wLength
2
；
设向量
w
和向量
p
的夹角为 θ， 根据点乘公式得 dotProduct1=wLength∗∣
p
∣∗cos(θ) ；
将上述等式带入不等式得
wLength
2
(wLength∗∣
p
∣∗cos(θ))
2
>combinedRadius
2
；
简化后得 (∣
p
∣∗cos(θ))
2
>combinedRadius
2
；
再简化得 ∣
p
∣∗cos(θ)>combinedRadius 。
图片
根据上图的几何模型，可以继续简化上面的不等式： cos(θ)>
∣
p
∣
combinedRadius
。根据三角函数公式，最终得到 cos(θ)>cos(φ) 。
因此，综上所述，如果 dotProduct1 < 0.0f 并且 cos(θ)>cos(φ) ，则
u
需要指向小圆的弧；否则
u
指向离得最近的 VO“腿”上。
2.3.2.1. 在小圆上计算 u
如果满足条件 dotProduct1 < 0.0f 并且 cos(θ)>cos(φ) ，则在小圆上计算
u
，代码如下。
// 以上条件都满足，也就是说在小圆弧上计算u
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq \* wLengthSq)
{
float wLength = math.sqrt(wLengthSq);
float2 unitW = w / wLength;
line.direction = new float2(unitW.y, -unitW.x);
u = (combinedRadius \* invTimeHorizon - wLength) \* unitW;
}
计算过程和 2.3.1.小节一样，不再赘述。
2.3.2.2. 在两条腿上计算 u
如果以上条件不满足，则在两条腿上计算
u
，代码如下。
// 否则，在两条腿上计算u
else
{
// 根据勾股定理，求得腿长
float leg = math.sqrt(distSq - combinedRadiusSq);
// det利用叉积（sin值）判断在哪条腿上
if (RVOMath.det(relativePosition, w) > 0.0f)
{
/\* 向量（pB - pA）到左腿的投影向量 \*/
// 计算leg和大圆的切点，这个切点到原点的方向即为ORCA line的方向
line.direction = new float2(relativePosition.x \* leg - relativePosition.y \* combinedRadius, relativePosition.x \* combinedRadius + relativePosition.y \* leg) / distSq;
}
else
{
/\* 向量（pB - pA）到右腿的投影向量 \*/
// 计算leg和大圆的切点，这个切点到原点的方向的逆方向即为ORCA line的方向
line.direction = -new float2(relativePosition.x \* leg + relativePosition.y \* combinedRadius, -relativePosition.x \* combinedRadius + relativePosition.y \* leg) / distSq;
}
// dotProduct2是相对速度点到腿上的投影长度
float dotProduct2 = math.dot(relativeVelocity, line.direction);
// 相对速度点到腿上的投影向量，减去相对速度向量，就得到了u
u = dotProduct2 \* line.direction - relativeVelocity;
}
代码中的 distSq 是原点到大圆圆心的距离 ∣
p
∣ 的平方。代码中的 combinedRadiusSq 是 combinedRadius 的平方，也就是大圆半径的平方。根据勾股定理，leg 就等于腿的长度。
这段代码可以用下图的几何模型来表示。
图片
代码第 8 行的判断语句里面的 RVOMath.det(relativePosition, w)，也就是上图中的 det(
p
,
w
) ，代表的是向量
w
到向量
p
的投影的另一条直角边的长度。其长度大于 0，则更靠近左腿；其长度小于 0，则更靠近右腿。例如上图就是更加靠近左腿。
以左腿为例，代码第 12 行计算出了 ORCA 半平面直线的 direction。因为左腿也是一条直线，所以其实 ORCA 半平面的 direction 和左腿是平行的，因此如果能算出左腿上某一点的坐标 p
1
，也就能知道 ORCA 半平面的 direction。
设点 relativePosition 为 p
r
，设长度 combinedRadius 为 r
1
。
根据上图，假设已知向量
p
与左腿的夹角为 θ， p
1
是点 p
r
旋转 θ 后与左腿的交点。那么我们可以得以下等式。
p
1
=p
r
∗(
cos(θ)
sin(θ)
−sin(θ)
cos(θ)
)
所以：
p
1
=(p
r
.x∗cos(θ)−p
r
.x∗sin(θ),p
r
.y∗sin(θ)+p
r
.y∗cos(θ))
又因为 cos(θ)=
∣
p
∣
leg
， sin(θ)=
∣
p
∣
r
1
。
所以：
p
1
=(p
r
.x∗
∣
p
∣
leg
−p
r
.x∗
∣
p
∣
r
1
,p
r
.y∗
∣
p
∣
r
1
+p
r
.y∗
∣
p
∣
leg
)
提取公因式得到：
p
1
=
∣
p
∣
(p
r
.x∗leg−p
r
.x∗r
1
,p
r
.y∗r
1
+p
r
.y∗leg)
因为 p
1
的长度为 ∣
p
∣ ，因此 p
1
的单位向量 p
1n
为：
p
1n
=
∣
p
∣
2
(p
r
.x∗leg−p
r
.x∗r
1
,p
r
.y∗r
1
+p
r
.y∗leg)
因此，ORCA 半平面的方向就是 p
1
的单位向量 p
1n
，代码在上一段代码的第 12 行。
/\* 向量（pB - pA）到左腿的投影向量 \*/
// 计算leg和大圆的切点，这个切点到原点的方向即为ORCA line的方向
line.direction = new float2(relativePosition.x \* leg - relativePosition.y \* combinedRadius, relativePosition.x \* combinedRadius + relativePosition.y \* leg) / distSq;
右腿的计算同理，不再赘述。
有了 line.direction，就可以求
u
了，代码如下。
// dotProduct2是相对速度点到腿上的投影长度
float dotProduct2 = math.dot(relativeVelocity, line.direction);
// 相对速度点到腿上的投影向量，减去相对速度向量，就得到了u
u = dotProduct2 \* line.direction - relativeVelocity;
这段代码可以用下图的几何模型来表示。
图片
设 relativeVelocity 在左腿上的投影为
OU
，那么
OU
的单位向量也就是 line.direction，则 dotProduct2 就是投影长度。所以点 U 的坐标等于 line.direction*dotProduct2。因此，
u
=U−relativeVelocity 。
2.3.3. 计算 line.point
line.point 就是当前 Agent 的速度沿着
u
方向，偏移
u
×w 单元距离的点。跟算法描述的一致，不再过多解释，具体看如下代码。
line.point = agent.velocity + GetAgentWeight(agent, other.weight) \* u;
orcaLines.Add(line);
代码第 2 行，将计算完成的 line 加入到 orcaLines 列表，以供接下来的线性规划计算使用。
2.3.4. 线性规划
在 MoveJob 中，有三个线性规划的函数，分别叫做 LinearProgram1、LinearProgram2、LinearProgram3。
2.3.4.1. LinearProgram2
首先看一下 LinearProgram2 函数。这个函数比较简单，就是遍历每一条 line 去执行 LinearProgram1 函数，求出当前 Agent 的实际移动速度。如果求解失败，则返回失败的 line 的索引。
///
/// 通过调用LinearProgram1线性规划，计算出Agent实际的移动速度
///
/// 所有ORCA
/// 当前Agent的半径
/// 当前Agent的期望移动速度
/// 这个值通常是false，只有被LinearProgram3调用时候才为true
/// Agent实际的移动速度。如果某一条line在执行LinearProgram1的时候失败，则返回上次计算成功的速度点
/// 如果计算成功，返回lines.Length；如果每一条line在执行LinearProgram1的时候失败，则返回失败的索引
private int LinearProgram2(in NativeList lines, float radius, float2 optVelocity, bool directionOpt, out float2 result)
{
// 在LinearProgram3里面，如果要直接使用预期速度的话
if (directionOpt)
{
// 直接赋值预期速度
result = optVelocity \* radius;
}
// 如果预期速度超过最大速度
else if (math.lengthsq(optVelocity) > RVOMath.sqr(radius))
{
// 限制预期速度大小不能超过最大速度
result = math.normalizesafe(optVelocity) \* radius;
}
else
{
result = optVelocity;
}
for (int i = 0; i < lines.Length; ++i)
{
Line line = lines[i];
// 当前Vopt在line的右边
if (RVOMath.det(line.direction, line.point - result) > 0.0f)
{
float2 tempResult = result;
// 调用LinearProgram1计算实际移动速度
if (!LinearProgram1(in lines, i, radius, optVelocity, directionOpt, ref result))
{
result = tempResult;
return i;
}
}
}
return lines.Length;
}
2.3.4.2. LinearProgram1
LinearProgram1 函数用来计算在指定 line 上当前 Agent 的实际移动速度，这个实际移动速度大小不能超过 Agent 的最大移动速率，同时要尽量贴合预期移动速度。
LinearProgram1 函数核心思想如下图所示。
图片
如上图所示，传入 LinearProgram1 函数的预期移动速度 v
opt
有可能在 ORCA 半平面以外，为了避免和其他 Agent 碰撞，需要以 Agent.maxSpeed 为半径，围绕原点旋转速度方向，直到速度和 ORCA 半平面有交点为止。得到的新的速度，即为相对当前 line 的实际速度 v
new
。
代码前三句判断了以 Agent.maxSpeed 为半径，围绕原点旋转速度方向，是否能与 ORCA 半平面相交，如果 Agent.maxSpeed 太短，则可能没有交点，此时 LinearProgram1 函数计算失败。
// 设速度空间原点O到line的垂足为B，则dot(point, direction)算出的投影长度就是point到B的长度。
float dotProduct = math.dot(lines[lineNo].point, lines[lineNo].direction);
// 如果discriminant小于0，则说明radius小于向量OB的长度，则说明在lineNo这条线内找不到合适的Velocity
// discriminant也就是垂足到radius交点的距离的平方
float discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - math.lengthsq(lines[lineNo].point);
if (discriminant < 0.0f)
{
/\* Max speed circle fully invalidates line lineNo. \*/
return false;
}
这段代码可以用下图的几何模型来表示。
图片
设 lines[lineNo].point 为 P
1
，那么 dotProduct 就等于 dot(
OP
1
,line.direction) ，也就等于 ∣
P
1
P
2
∣ 。
设 ∣
T
1
P
2
∣ 为 x，设 ∣
OP
2
∣ 为 y。因为 ∣
OT
1
∣ 等于半径 radius，也就是 Agent.maxSpeed，是已知数。
根据勾股定理联立方程组得：
{
∣
P
1
P
2
∣
2
+y
2
=∣
OP
1
∣
2
x
2
+y
2
=∣
OT
1
∣
2
解方程组得到：
x
2
=∣
P
1
P
2
∣
2
+∣
OT
1
∣
2
−∣
OP
1
∣
2
所以 x
2
等于 RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - math.lengthsq(lines[lineNo].point)，因此 discriminant 就是
T
1
P
2
长度的平方。
因此，discriminant 小于 0，则说明半径太短，与 ORCA 没有交点，返回失败。
接下来三句代码就比较简单了。
float sqrtDiscriminant = math.sqrt(discriminant);
// Vnew左交点到line.point的距离
float tLeft = -dotProduct - sqrtDiscriminant;
// Vnew右交点到line.point的距离
float tRight = -dotProduct + sqrtDiscriminant;
其中，sqrtDiscriminant 就是 ∣
T
1
P
2
∣ ，tLeft 等于 ∣
P
1
T
2
∣ ，tRight 等于 ∣
P
1
T
1
∣ 。
以上就是当前 line 需要的数据，接下来用当前 line 和之前所有已经计算过的 line 重新计算最优的 v
new
。
for (int i = 0; i < lineNo; ++i)
{
// direction是单位向量，长度为1
float denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
// direction是单位向量，长度为1；lines[lineNo].point - lines[i].point长度不为1
float numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point);
if (math.length(denominator) <= math.EPSILON)
{
/\* lines[i]和lines[lineNo]几乎平行 \*/
if (numerator < 0.0f)
{
return false;
}
continue;
}
// 因为direction是单位向量长度为1，且根据相似三角形原理，t就是两条line的交点到lines[lineNo].point的距离
float t = numerator / denominator;
if (denominator >= 0.0f)
{
/\* 如果lines[i]在lines[lineNo]的右边 \*/
tRight = math.min(tRight, t);
}
else
{
/\* 如果lines[i]在lines[lineNo]的左边 \*/
tLeft = math.max(tLeft, t);
}
if (tLeft > tRight)
{
return false;
}
}
为了理解以上计算过程，我们建立一下几何模型帮助理解。
图片
设 lines[lineNo].direction 为
O
1
D
2
，lines[i].direction 为
O
1
D
1
，lines[lineNo].point - lines[i].point 为
P
3
P
1
。
那么 denominator 就等于平行四边形 ◇O
1
D
1
D
3
D
2
的面积，numerator 就等于平行四边形 ◇P
3
D
4
D
5
P
1
的面积。
因为
O
1
D
1
和
P
3
D
4
均为 lines[i].direction，所以平移
O
1
D
2
到
P
3
D
6
，可以构建出两个共享底边的三角形 △P
3
D
4
P
1
和 △P
3
D
4
D
6
。因此，numerator / denominator 等于
D
4
D
6
D
4
P
1
。
又因为 △D
4
P
3
D
6
和 △D
4
O
1
P
1
相似，所以
denominator
numerator
=
D
4
D
6
D
4
P
1
=
P
3
D
6
O
1
P
1
。因为 ∣
P
3
D
6
∣=1 ，所以
denominator
numerator
=∣
O
1
P
1
∣ 。也就是代码里的 t 等于 ∣
O
1
P
1
∣ 。
以上图为例，我们的 denominator 是大于 0 的，又因为 ∣
O
1
P
1
∣<∣
P
1
T
2
∣ ，所以 tRight 需要更新为 math.min(tRight, t)，也就是 ∣
O
1
P
1
∣ 。
代码第 33 行，如果出现 tLeft > tRight 的情况，可能是出现以下情形了，这样的情形下所有的 line 是没有交集的，因此返回失败。
图片
计算完所有的 line 对应的 v
new
后，如果依然成功，那么就可以得到最终实际速度 v
new
，代码里用 result 参数来返回。
float t = math.dot(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;
}
如果线性规划失败了，将会调用 LinearProgram3 函数来重新做线性规划。
2.3.4.3. LinearProgram3
LinearProgram3 函数是为了避免在密集情况下，无法寻路导致的群体移动死锁而设计的，其具体实现如下。
private void LinearProgram3(in NativeList lines, float agentWeight, int numObstLines, int beginLine, float radius, ref float2 result)
{
float distance = 0.0f;
// 从上次失败的line开始往后遍历
for (int i = beginLine; i < lines.Length; ++i)
{
// 如果result在line[i]的左侧distance远的区域以外，说明line[i]对最终的交集有影响
if (RVOMath.det(lines[i].direction, lines[i].point - result) > distance)
{
/\* result并不能满足line[i]的约束 \*/
NativeList projLines = new NativeList(Allocator.Temp);
for (int ii = 0; ii < numObstLines; ++ii)
{
projLines.Add(lines[ii]);
}
// 遍历line[i]之前的所有line
for (int j = numObstLines; j < i; ++j)
{
Line line;
float determinant = RVOMath.det(lines[i].direction, lines[j].direction);
if (math.length(determinant) <= math.EPSILON)
{
/\* Lin[i]和line[j]平行 \*/
if (math.dot(lines[i].direction, lines[j].direction) > 0.0f)
{
/\* Lin[i]和line[j]指向同一个方向 \*/
continue;
}
else
{
/\* Lin[i]和line[j]指向相反的方向 \*/
line.point = agentWeight \* (lines[i].point + lines[j].point);
}
}
else
{
// 两条line的交点
line.point = lines[i].point + (RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) \* lines[i].direction;
}
// 角平分线方向
line.direction = math.normalizesafe(lines[j].direction - lines[i].direction);
projLines.Add(line);
}
float2 tempResult = result;
if (LinearProgram2(in projLines, radius, new float2(-lines[i].direction.y, lines[i].direction.x), true, out result) < projLines.Length)
{
/\*
\* 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;
}
projLines.Dispose();
distance = RVOMath.det(lines[i].direction, lines[i].point - result);
}
}
}
其中 distance 为第三维的变量，用来排除对最终所有 line 的交集影响不大的 line。
代码中平行线的处理很简单，非平行线的 line.point 和 2.3.4.2.小节的证明过程相同，就是求两条 line 的交点。
这里比较特殊的是代码第 46 行 line.direction 的更新。算法使用 math.normalizesafe(lines[j].direction - lines[i].direction) 来更新 line.direction，也就是说新的方向设置为 lines[j].direction 和 lines[i].direction 构成的夹角的角平分线方向。
下面通过以下几何模型来说明这么更新 line 的方向的原因。
前文说过，下图这种情况可能导致所有 line 没有公共交集。
图片
但是，上图这种情况，在实际的位置空间中，所有 Agent 的分布有可能是如下图所示。
图片
这种情况下，其实圆 A 是有移动空间的，它可以沿着 v
new
的方向移动，以避开圆 B、圆 C、圆 D，因此在这种情况下，我们可以重新设定 ORCA
A∣B
τ
和 ORCA
A∣D
τ
的方向，如下图所示。
图片
可见这样调整了 ORCA
A∣B
τ
和 ORCA
A∣D
τ
的方向后，绿色阴影区域即为新的公共交集， v
new
取值在这个区域范围里，可以避免群体移动死锁。
2.4. 扩展
本文仅针对 Agent 进行避障，原版 ORCA 还考虑了静物，不过原理都差不多，后续有时间再补充。
本文仅考虑了 2D 环境下的避障，该算法可以扩展到 3D 环境。
1. 运行效果
在骁龙 425（处理器型号：MSM8917；制造工艺：28nm LP；CPU 架构：四核 A53；核心频率：1.4GHz；GPU：Adreno 308）的手机上运行，跑 1064 个 Agent 互相避障，避障行为的每帧消耗在 2ms 到 6ms 左右。
图片
602 个 Agent 互相避障，避障行为的每帧消耗在 1ms 到 5ms 左右。
图片
避障行为的效果如下图所示。
图片
引用链接
[1] 《Reciprocal n-body Collision Avoidance》: https://gamma.cs.unc.edu/ORCA/publications/ORCA.pdf