ORCA-3D避障算法解析

二维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）求解方法，目的是在有多个平面约束（即避障条件）的情况下找到最优的速度向量，以确保多个智能体不会发生碰撞。

linearProgram1()：寻找线与圆形区域的交点
linearProgram2()：解决单个平面约束的最优速度
linearProgram3()：求解所有平面的初步速度
linearProgram4()：处理多个平面之间的约束冲突