二维ORCA原理参考：
https://zhuanlan.zhihu.com/p/669426124
ORCA原理图解+代码解释
1. 找到避障速度增量 u
碰撞处理分为三种情况：
（1）没有发生碰撞，且相对速度落在小圆里
（2）没有发生碰撞，且相对速度落在圆锥里
（3）发生碰撞，马上做出反应
timeStep 决定了仿真每一步的时间更新间隔，是系统的时间推进基础。较小的 timeStep 可以提高仿真的精度，但会增加计算量。
timeHorizon 决定了智能体在进行避障计算时预测的时间范围。较大的 timeHorizon 值使得智能体可以更早预测潜在碰撞，但会减少它的速度选择自由度。
timeStep 是碰撞时需要计算的调整u所需的时间
timeHorizon 是未发生碰撞时，需要计算的u所化的时间，他是一种提前预测
2. 添加速度障碍平面
表示一个平面需要法向量和平面上的点
1和2对应代码如下
// 它使用ORCA（Optimal Reciprocal Collision Avoidance）方法来计算智能体之间的避碰行为
void Agent::computeNewVelocity()
{
orcaPlanes_.clear(); // 清空ORCA平面列表
const float invTimeHorizon = 1.0f / timeHorizon_; // 计算时间视野的倒数
/* 创建智能体的ORCA平面 */
for (size_t i = 0; i < agentNeighbors_.size(); ++i)
{ // 遍历每个邻居智能体
const Agent *const other = agentNeighbors_[i].second; // 获取邻居智能体指针
//这里的position_是在rvo->updateState添加的当前agent的位置
// 改这块就好了===============================
const Vector3 relativePosition = other->position_ - position_; // 计算相对位置
const Vector3 relativeVelocity = velocity_ - other->velocity_; // 计算相对速度
// const Vector3 relativePosition = relative_position_; // 计算相对位置
// const Vector3 relativeVelocity = relative_velocity_; // 计算相对速度
const float distSq = absSq(relativePosition); // 计算相对位置的平方距离
const float combinedRadius = radius_ + other->radius_; // 计算合并半径
const float combinedRadiusSq = sqr(combinedRadius); // 计算合并半径的平方
Plane plane; // 定义一个平面对象
Vector3 u; // 定义速度调整向量
if (distSq > combinedRadiusSq)
{ // 如果没有发生碰撞
// w表示给定时间视野TimeHorizon内，两个智能题之间的相对速度偏移量
const Vector3 w = relativeVelocity - invTimeHorizon * relativePosition; // 计算从截断中心到相对速度的向量
const float wLengthSq = absSq(w); // 计算w向量的平方长度
const float dotProduct = w * relativePosition; // 计算w向量和相对位置的点积
// 1. 如果投影在截断圆上
// dotProduct表示相差的速度和相差的位置的点乘，要是点乘小于0，表示在靠近
if (dotProduct < 0.0f && sqr(dotProduct) > combinedRadiusSq * wLengthSq)
{
const float wLength = std::sqrt(wLengthSq); // 计算w向量的长度
const Vector3 unitW = w / wLength; // 计算w向量的单位向量
plane.normal = unitW; // 设置平面的法向量
u = (combinedRadius * invTimeHorizon - wLength) * unitW; // 计算速度调整向量
}
// 2. 如果投影在圆锥上
else
{
const float a = distSq; // 设置系数a
const float b = relativePosition * relativeVelocity; // 设置系数b
const float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq); // 设置系数c
// t表示圆锥中心线到斜线的距离 对于 半径的倍数
const float t = (b + std::sqrt(sqr(b) - a * c)) / a; // 计算t值
const Vector3 w = relativeVelocity - t * relativePosition; // 计算w向量
const float wLength = abs(w); // 计算w向量的长度
const Vector3 unitW = w / wLength; // 计算w向量的单位向量
plane.normal = unitW; // 设置平面的法向量
u = (combinedRadius * t - wLength) * unitW; // 计算速度调整向量
}
}
// 3. 如果发生碰撞
else
{
const float invTimeStep = 1.0f / sim_->timeStep_; // 计算时间步长的倒数
const Vector3 w = relativeVelocity - invTimeStep * relativePosition; // 计算w向量
const float wLength = abs(w); // 计算w向量的长度
const Vector3 unitW = w / wLength; // 计算w向量的单位向量
plane.normal = unitW; // 设置平面的法向量
u = (combinedRadius * invTimeStep - wLength) * unitW; // 计算速度调整向量
}
// 有多少个neighbor，就有多少个orca平面
plane.point = velocity_ + 0.5f * u; // 计算平面上的点
orcaPlanes_.push_back(plane); // 将平面添加到ORCA平面列表中
}
const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_); // 计算新的速度，如果失败返回失败的平面索引
if (planeFail < orcaPlanes_.size())
{ // 如果存在失败的平面
linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_); // 调用备用算法处理失败的平面
}
}
3. 线性规划求解出最优速度
linearProgram几个函数实现了一套线性规划（Linear Programming, LP）求解方法，目的是在有多个平面约束（即避障条件）的情况下找到最优的速度向量，以确保多个智能体不会发生碰撞。
初始调用
// 调用 linearProgram3 来计算满足所有约束（平面）的新的速度向量。如果失败，则返回失败的平面索引。
const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_); // 计算新的速度，如果失败返回失败的平面索引
if (planeFail < orcaPlanes_.size()) // 如果存在失败的平面
{
// 调用备用算法处理失败的平面
linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_);
}
3.1 linearProgram3()：求解所有平面的初步速度
size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
{
if(directionOpt) /* 如果使用方向优化，即只考虑速度方向，而不考虑速度大小。*/
{
result = optVelocity * radius; // 如果优化方向，将最优速度扩展到给定的半径
}
else if (absSq(optVelocity) > sqr(radius))
{
/* 优化最近点并且在圆外。 ？？ 是不是为了安全性啊，本来optVelocity不就是单位向量了吗*/
result = normalize(optVelocity) * radius; // 如果最优速度的平方长度大于半径的平方，将其归一化并扩展到给定的半径
}
else
{ /* 优化最近点并且在圆内。 */
result = optVelocity; // 如果最优速度的平方长度小于或等于半径的平方，直接使用最优速度
}
for (size_t i = 0; i < planes.size(); ++i)
{
// result 位于平面的外侧，不满足orca约束
if (planes[i].normal * (planes[i].point - result) > 0.0f)
{
const Vector3 tempResult = result; // 保存当前结果
if (!linearProgram2(planes, i, radius, optVelocity, directionOpt, result))
{
result = tempResult; // 如果 linearProgram2 返回 false，恢复之前的结果
return i; // 返回不满足的平面的索引
}
}
}
return planes.size(); // 如果所有约束都满足，返回 planes.size()
}
3.2 linearProgram2()：解决单个平面约束的最优速度
如图求出初步速度之后，这是满足了planeNo的约束，但可能破坏之前平面的约束，因此需要遍历planeNo之前的平面做检查
// 用于计算满足给定约束的速度向量。这个函数的主要目的是在智能体的避碰算法中，在一个半径为radius的球体内找到一个速度向量，使其尽可能接近给定的最优速度optVelocity，同时满足所有给定的平面约束。
bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
{
const float planeDist = planes[planeNo].point * planes[planeNo].normal; // 计算平面与原点的距离
const float planeDistSq = sqr(planeDist); // 计算距离的平方
const float radiusSq = sqr(radius); // 计算半径的平方
// 用超过最大速度的速度才能达到orca避障平面
if (planeDistSq > radiusSq)
{
/* 最大速度球完全使平面 planeNo 无效。 */
return false; // 如果平面距离的平方大于半径的平方，则返回false，表示无解
}
// 勾股定理计算最大速度radiusSq与平面中线的另外一边的平方
const float planeRadiusSq = radiusSq - planeDistSq;
// 计算原点到平面中心的线
const Vector3 planeCenter = planeDist * planes[planeNo].normal;
if (directionOpt)
{
/* 投影方向 optVelocity 到平面 planeNo 上。 */
// optVelocity * planes[planeNo].normal 表示 optVelocity 在 planes[planeNo].normal 方向上的投影长度，这是一个标量，再乘以法向量，使之变为矢量
const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal; // 计算平面上的最优速度
const float planeOptVelocityLengthSq = absSq(planeOptVelocity); // 计算平面上最优速度的平方长度
if (planeOptVelocityLengthSq <= RVO_EPSILON)
{
result = planeCenter; // 如果最优速度的平方长度小于一个很小的值，则结果为平面中心
}
else
{
// 否则，计算结果为平面中心加上最优速度在平面上的投影
result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity;
}
}
else
{
// 结果是optVelocity + optVelocity顶点离平面的最小距离向量
result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal; // 计算点在平面上的投影
// 就是结果超过最大速度
if (absSq(result) > radiusSq)
{
const Vector3 planeResult = result - planeCenter; // 计算结果相对于平面中心的向量
const float planeResultLengthSq = absSq(planeResult); // 计算该向量的平方长度
// 结果就是最大圆与平面的交点，并且里原始方向近的那个交点形成的向量
result = planeCenter + std::sqrt(planeRadiusSq / planeResultLengthSq) * planeResult;
}
}
// 新的result被求出，满足了planeNo的约束，但可能破坏之前的约束，因此需要检查
for (size_t i = 0; i < planeNo; ++i)
{
if (planes[i].normal * (planes[i].point - result) > 0.0f)
{
// 计算两个平面的法向量的叉积
Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal);
// 平面 planeNo 和 i（几乎）平行，因为平面 i 失败了，所以之前求出的平面 planeNo 也失败了
if (absSq(crossProduct) <= RVO_EPSILON)
{
return false; // 返回false
}
// 算出交线，result指到交线，那就可以同时满足这两个平面约束了呀
Line line;
line.direction = normalize(crossProduct); //平面交线方向
// 平面planeNo上并垂直于交线的线
const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal);
// ((planes[i].point - planes[planeNo].point) * planes[i].normal)两平面点连线在平面i法向量上的投影
line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal;
if (!linearProgram1(planes, i, line, radius, optVelocity, directionOpt, result))
{
return false; // 如果 linearProgram1 返回 false，表示无解，返回 false
}
}
}
return true; // 返回 true，表示找到了解
}
3.3 linearProgram1()：寻找线与圆形区域的交点
// 用于在一个圆形区域内（以给定的半径为界限）找到一条线的交点。
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
{
const float dotProduct = line.point * line.direction; // 计算点和方向的点积
const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(line.point); // 计算判别式，用于判断交点是否在圆形区域内
if (discriminant < 0.0f)
{
// 如果判别式小于0，表示没有交点，返回false
return false;
}
const float sqrtDiscriminant = std::sqrt(discriminant); // 计算判别式的平方根
float tLeft = -dotProduct - sqrtDiscriminant; // 计算t的左边界
float tRight = -dotProduct + sqrtDiscriminant; // 计算t的右边界
for (size_t i = 0; i < planeNo; ++i)
{
const float numerator = (planes[i].point - line.point) * planes[i].normal; // 计算分子
const float denominator = line.direction * planes[i].normal; // 计算分母
if (sqr(denominator) <= RVO_EPSILON)
{
/* 线几乎与平面i平行。 */
if (numerator > 0.0f)
{
return false; // 如果分子大于0，返回false，表示无解
}
else
{
continue; // 否则继续下一个平面
}
}
const float t = numerator / denominator; // 计算t值
if (denominator >= 0.0f)
{
/* 平面i限制线的左边界。 */
tLeft = std::max(tLeft, t); // 更新t的左边界
}
else
{
/* 平面i限制线的右边界。 */
tRight = std::min(tRight, t); // 更新t的右边界
}
if (tLeft > tRight)
{
return false; // 如果左边界超过右边界，返回false，表示无解
}
}
// 优化方向
if (directionOpt)
{
if (optVelocity * line.direction > 0.0f)
{
// 如果方向优化，则选择tRight作为结果
result = line.point + tRight * line.direction;
}
else
{
// 否则选择tLeft作为结果
result = line.point + tLeft * line.direction;
}
}
else
{
/* 优化最近点。 */
const float t = line.direction * (optVelocity - line.point); // 计算最优t值
if (t < tLeft)
{
result = line.point + tLeft * line.direction; // 如果t小于左边界，选择tLeft作为结果
}
else if (t > tRight)
{
result = line.point + tRight * line.direction; // 如果t大于右边界，选择tRight作为结果
}
else
{
result = line.point + t * line.direction; // 否则选择t作为结果
}
}
return true; // 返回true，表示找到了解
}
3.4linearProgram4()：处理多个平面之间的约束冲突
其实这个代码是在构造一个新的平面集合projPlanes，然后重新调用linearProgram3()求解
projPlanes是对从有冲突的平面开始拿出一个平面i，然后找到这个平面i之前的平面j，用这两个平面i和平面j构造出一个中间平面重新调用linearProgram3()来解决问题
所谓的中间平面就是：
- 当两平面平行，就是中间的平行平面
- 当两平面相交，就是夹角那个方向的平面
其实我不是很理解这个中间平面的构造，但可以大致想一下就是因为两个平面ij限制太多了才找不到解，反正解也都是在平面边缘处找到的，不如找一个折中的平面，尝试一下这个位置是不是能找到。。。（待定）
// 当 linearProgram3 从beginPlane无法满足约束时，linearProgram4 进一步处理这些情况。
void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result)
{
float distance = 0.0f; // 初始化距离为0
for (size_t i = beginPlane; i < planes.size(); ++i)
{
if (planes[i].normal * (planes[i].point - result) > distance)
{
std::vector<Plane> projPlanes;
for (size_t j = 0; j < i; ++j)
{
Plane plane;
// 计算两个平面的法向量的叉积，可以表示两个平面的交线的方向
const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal);
// 利用叉乘判断平面是否平行，绝对值小于0.1，则平行
if (absSq(crossProduct) <= RVO_EPSILON) //RVO_EPSILON=0.1
{
// 利用点乘判断平行平面方向，平面 i 和平面 j 指向相同方向
if (planes[i].normal * planes[j].normal > 0.0f)
{
continue;
}
else // 平面 i 和平面 j 指向相反方向。
{
// 平面点是两个平面点的中点
plane.point = 0.5f * (planes[i].point + planes[j].point);
}
}
else
{
// 在平面 i 内部，且垂直于交线方向的向量
const Vector3 lineNormal = cross(crossProduct, planes[i].normal);
// (planes[j].point - planes[i].point) * planes[j].normal 是两点连线在平面 j 法向量方向上的投影
// (lineNormal * planes[j].normal) 表示 lineNormal 在平面 j 法向量方向上的投影。
plane.point = planes[i].point + (((planes[j].point - planes[i].point) * planes[j].normal) / (lineNormal * planes[j].normal)) * lineNormal; // 计算交点
}
plane.normal = normalize(planes[j].normal - planes[i].normal); // 计算投影平面的法向量并归一化
projPlanes.push_back(plane); // 将投影平面添加到列表中
}
const Vector3 tempResult = result; // 保存当前结果
if (linearProgram3(projPlanes, radius, planes[i].normal, true, result) < projPlanes.size())
{
/* 原则上不应该发生。这是因为结果已经在这个线性规划的可行区域内。如果失败，是由于小的浮点误差，并保持当前结果。 */
result = tempResult;
}
distance = planes[i].normal * (planes[i].point - result); // 更新距离
}
}
}
}
