using System;
using System.Collections.Generic;
using Game.Plugins.JNGame.Sync.Frame.AStar.Util;
using UnityEngine;

namespace Game.Plugins.JNGame.Sync.Frame.AstarPath.RVO
{
    /// <summary>  
    /// RVO系统的内部代理。  
    /// 通常，您会使用IAgent接口进行交互。  
    ///  
    /// 参见：IAgent接口  
    /// </summary> 
    public class JNRVOAgent : JNIAgent
    {
        // 双缓冲计算的当前值  
  
        // 内部变量  
        // 代理的半径、高度、期望速度、最大速度、代理的时间视野和障碍物的时间视野。  
        internal float radius, height, desiredSpeed, maxSpeed, agentTimeHorizon, obstacleTimeHorizon;
        internal bool locked = false; // 代理是否被锁定。  
  
        JNRVOLayer layer, collidesWith; // 代理的碰撞层和与之碰撞的层。  
  
        int maxNeighbours; // 代理的最大邻居数量。  
        internal Vector2 position; // 代理的位置。  
        float elevationCoordinate; // 代理的海拔高度坐标。  
        Vector2 currentVelocity; // 代理的当前速度。  
  
        /// <summary>期望的目标点 - 在速度空间中的位置</summary>  
        Vector2 desiredTargetPointInVelocitySpace; // 代理期望在速度空间中的目标点。  
        Vector2 desiredVelocity; // 代理期望的速度。  
  
        Vector2 nextTargetPoint; // 下一个目标点。  
        float nextDesiredSpeed; // 下一个期望速度。  
        float nextMaxSpeed; // 下一个最大速度。  
        Vector2 collisionNormal; // 碰撞法线向量。  
        bool manuallyControlled; // 是否手动控制代理。  
        bool debugDraw; // 是否进行调试绘制。  
        
        
        #region IAgent Properties

		/// 代理的位置。
		public Vector2 Position { get; set; }

		/// 代理的海拔高度坐标。
		public float ElevationCoordinate { get; set; }

		///代理计算的下一个目标点。
		public Vector2 CalculatedTargetPoint { get; private set; }

		/// 代理计算的下一个速度。
		public float CalculatedSpeed { get; private set; }

		/// 代理是否被锁定的状态
		public bool Locked { get; set; }

		/// 代理的半径。
		public float Radius { get; set; }

		/// 代理的高度。
		public float Height { get; set; }

		///代理的时间视野。
		public float AgentTimeHorizon { get; set; }

		/// 障碍物的时间视野。
		public float ObstacleTimeHorizon { get; set; }

		/// 代理的最大邻居数量。
		public int MaxNeighbours { get; set; }

		/// 代理的邻居数量（私有的，外部无法直接设置）
		public int NeighbourCount { get; private set; }

		/// 代理所在的碰撞层。
		public JNRVOLayer Layer { get; set; }

		/// 与代理发生碰撞的层。
		public JNRVOLayer CollidesWith { get; set; }

		/// 是否开启调试绘制，取决于debugDraw、simulator和simulator.Multithreading的值。
		public bool DebugDraw {
			get {
				return debugDraw;
			}
			set {
				// debugDraw = value && simulator != null && !simulator.Multithreading;
			}
		}

		/// 代理的优先级。
		public float Priority { get; set; }

		/// 预计算回调方法，尚未提供具体的实现。
		public System.Action PreCalculationCallback { private get; set; }

		#endregion
		
		#region IAgent Methods
		
		/// <summary>  
		/// 设置目标点、期望速度和最大速度。  
		/// </summary>  
		/// <param name="targetPoint">目标点的位置。</param>  
		/// <param name="desiredSpeed">期望的速度。</param>  
		/// <param name="maxSpeed">代理的最大速度。</param>  
		public void SetTarget(Vector2 targetPoint, float desiredSpeed, float maxSpeed)  
		{  
			// 确保最大速度大于或等于0  
			maxSpeed = Math.Max(maxSpeed, 0);  
			// 确保期望速度在0和最大速度之间  
			desiredSpeed = Math.Min(Math.Max(desiredSpeed, 0), maxSpeed);  
			// 设置下一个目标点、期望速度和最大速度  
			nextTargetPoint = targetPoint;  
			nextDesiredSpeed = desiredSpeed;  
			nextMaxSpeed = maxSpeed;  
		}  
  
		/// <summary>  
		/// 设置碰撞法线。  
		/// </summary>  
		/// <param name="normal">碰撞的法线向量。</param>  
		public void SetCollisionNormal(Vector2 normal)  
		{  
			collisionNormal = normal;  
		}  
  
		/// <summary>  
		/// 强制设置代理的速度。此方法可能有点不精确，但假设代理在短时间内不会移动得太远。  
		/// </summary>  
		/// <param name="velocity">代理的速度向量。</param>  
		public void ForceSetVelocity(Vector2 velocity)  
		{  
			// 计算代理在1000单位时间后的位置，并设置其为下一个目标点。  
			nextTargetPoint = CalculatedTargetPoint = position + velocity * 1000;  
			// 设置代理的期望速度为速度的大小。  
			nextDesiredSpeed = CalculatedSpeed = velocity.magnitude;  
			// 将手动控制标志设置为true。  
			manuallyControlled = true;  
		}
		
		#endregion
		
		/// <summary>用于内部链表</summary>  
		/// <remarks>仅用于内部链表结构，不对外暴露</remarks>  
		internal JNRVOAgent next; // 指向下一个代理的指针  
  
		float calculatedSpeed; // 计算出的代理速度  
		Vector2 calculatedTargetPoint; // 计算出的目标点位置  
  
		/// <summary>  
		/// 处理此代理的模拟器。  
		/// 由此脚本作为引用，并防止将此代理添加到多个模拟中。  
		/// </summary>  
		/// <remarks>此模拟器用于控制和追踪代理的行为</remarks>  
		internal JNRVOSimulator simulator; // 代理所在的模拟器  
  
		List<JNRVOAgent> neighbours = new List<JNRVOAgent>(); // 邻近代理的列表  
		List<float> neighbourDists = new List<float>(); // 邻近代理的距离列表  
		List<JNObstacleVertex> obstaclesBuffered = new List<JNObstacleVertex>(); // 缓冲区中的障碍物顶点列表  
		List<JNObstacleVertex> obstacles = new List<JNObstacleVertex>(); // 障碍物顶点列表  
  
		const float DesiredVelocityWeight = 0.1f; // 理想速度的权重  
  
		/// <summary>墙壁将具有的额外权重</summary>  
		const float WallWeight = 5; // 墙壁的权重  
  
		public List<JNObstacleVertex> NeighbourObstacles {  
			get {  
				return null; // 目前返回null，可能在未来实现返回邻近障碍物的功能  
			}  
		}
		
		public JNRVOAgent (Vector2 pos, float elevationCoordinate)   
		{  
			// 设置Agent的时间视野为2  
			AgentTimeHorizon = 2;  
			// 设置障碍物的时间视野为2  
			ObstacleTimeHorizon = 2;  
			// 设置Agent的高度为5  
			Height = 5;  
			// 设置Agent的半径为5  
			Radius = 5;  
			// 设置最大邻居数为10  
			MaxNeighbours = 10;  
			// 初始化Agent的状态为未锁定  
			Locked = false;  
			// 设置Agent的位置为传入的pos参数值  
			Position = pos;  
			// 设置Agent的海拔坐标为传入的elevationCoordinate参数值  
			ElevationCoordinate = elevationCoordinate;  
			// 设置Agent所在的层为RVOLayer.DefaultAgent  
			Layer = JNRVOLayer.DefaultAgent;  
			// 设置Agent可以碰撞的层为所有层（通过将层号设为一个负数来实现）  
			CollidesWith = (JNRVOLayer)(-1);  
			// 设置Agent的优先级为0.5f  
			Priority = 0.5f;  
			// 初始化计算的下一个目标点为当前位置  
			CalculatedTargetPoint = pos;  
			// 初始化计算的下一个速度为0  
			CalculatedSpeed = 0;  
			// 使用传入的参数设置Agent的目标点、期望速度和最大速度  
			SetTarget(pos, 0, 0);  
		}
		
		/// <summary>  
		/// 从公共属性读取数据，并将这些数据存储在内部字段中。  
		/// 还会将最后计算的结果设置为CalculatedTargetPoint和CalculatedSpeed。  
		/// </summary>  
		public void BufferSwitch () {
			// 从公共属性读取数据  
			radius = Radius; // 读取半径  
			height = Height; // 读取高度  
			maxSpeed = nextMaxSpeed; // 读取最大速度  
			desiredSpeed = nextDesiredSpeed; // 读取期望速度  
			agentTimeHorizon = AgentTimeHorizon; // 读取代理的时间视野  
			obstacleTimeHorizon = ObstacleTimeHorizon; // 读取障碍物的时间视野  
			maxNeighbours = MaxNeighbours; // 读取最大邻居数  
			// 如果手动控制被启用，则覆盖代理的锁定状态  
			locked = Locked && !manuallyControlled; // 判断是否锁定  
			position = Position; // 读取位置  
			elevationCoordinate = ElevationCoordinate; // 读取海拔 坐标  
			collidesWith = CollidesWith; // 读取碰撞层  
			layer = Layer; // 读取所在层  
  
			if (locked) {  
				// 如果代理被锁定，则不会移动  
				desiredTargetPointInVelocitySpace = position; // 将位置设置为目标点  
				desiredVelocity = currentVelocity = Vector2.zero; // 设置当前速度为0向量  
			} else {  
				desiredTargetPointInVelocitySpace = nextTargetPoint - position; // 计算目标点与位置的差值作为速度空间中的目标点  
  
				// 估计当前速度（从计算出的目标点和当前位置计算）  
				currentVelocity = (CalculatedTargetPoint - position).normalized * CalculatedSpeed; // 计算当前速度  
  
				// 根据想要达到的点计算期望速度  
				desiredVelocity = desiredTargetPointInVelocitySpace.normalized*desiredSpeed; // 计算期望速度  
  
				if (collisionNormal != Vector2.zero) {  
					collisionNormal.Normalize(); // 规范化碰撞法线向量  
					var dot = Vector2.Dot(currentVelocity, collisionNormal); // 计算当前速度与碰撞法线的点积  
  
					// 检查速度是否正对着墙壁（即点积小于0）  
					if (dot < 0) {  
						// 如果如此，从速度中减去碰撞法线的分量  
						currentVelocity -= collisionNormal * dot; // 从当前速度中减去碰撞法线的分量  
					}  
  
					// 清空碰撞法线向量  
					collisionNormal = Vector2.zero; // 重置碰撞法线向量  
				}  
			}  
		}       
		
		
		public void PreCalculation () {
			if (PreCalculationCallback != null) {
				PreCalculationCallback();
			}
		}
		
		/// <summary>  
		/// 使用四叉树查询最接近的代理，并填充neighbours和neighbourDists列表  
		/// </summary>  
		public void CalculateNeighbours()  
		{  
			neighbours.Clear(); // 清除neighbours列表  
			neighbourDists.Clear(); // 清除neighbourDists列表  
  
			if (MaxNeighbours > 0 && !locked) // 如果最大邻居数大于0且未锁定  
			{  
				simulator.Quadtree.Query(position, maxSpeed, agentTimeHorizon, radius, this); // 查询四叉树中与当前代理最接近的代理  
			}  
  
			NeighbourCount = neighbours.Count; // 更新邻居数量  
		}
		
		
		public void PostCalculation () {
			// ==> Set public properties
			if (!manuallyControlled) {
				CalculatedTargetPoint = calculatedTargetPoint;
				CalculatedSpeed = calculatedSpeed;
			}

			(obstaclesBuffered, obstacles) = (obstacles, obstaclesBuffered);
			manuallyControlled = false;
		}
		
		/// <summary>  
		/// 由四叉树使用。  
		/// 参见：CalculateNeighbours  
		/// </summary>  
		internal float InsertAgentNeighbour(JNRVOAgent agent, float rangeSq) {  
			// 检查这个代理是否与其他代理碰撞  
			if (this == agent || (agent.layer & collidesWith) == 0) return rangeSq;  
  
			// 2D距离  
			float dist = (agent.position - position).sqrMagnitude;  
  
			if (dist < rangeSq) {  
				if (neighbours.Count < maxNeighbours) {  
					neighbours.Add(null);  
					neighbourDists.Add(float.PositiveInfinity);  
				}  
  
				// 将代理插入到邻居的有序列表中  
				int i = neighbours.Count - 1;  
				if (dist < neighbourDists[i]) {  
					while (i != 0 && dist < neighbourDists[i - 1]) {  
						neighbours[i] = neighbours[i - 1];  
						neighbourDists[i] = neighbourDists[i - 1];  
						i--;  
					}  
					neighbours[i] = agent;  
					neighbourDists[i] = dist;  
				}  
  
				if (neighbours.Count == maxNeighbours) {  
					rangeSq = neighbourDists[neighbourDists.Count - 1];  
				}  
			}  
			return rangeSq;  
		}
		
		
		internal void CalculateVelocity(WorkerContext context)  
		{  
			if (manuallyControlled) // 如果代理被手动控制  
			{  
				return; // 直接返回，不进行速度计算  
			}  
  	
			if (locked) // 如果代理被锁定  
			{  
				calculatedSpeed = 0; // 速度设置为0  
				calculatedTargetPoint = position; // 目标点设置为当前位置  
				return; // 直接返回，不进行后续操作  
			}  
  
			// 用于存放速度障碍物（VOs）的缓冲区  
			var vos = context.vos;  
			vos.Clear(); // 清除缓冲区中的内容  
  
			GenerateObstacleVOs(vos); // 生成障碍物的速度障碍物  
			GenerateNeighbourAgentVOs(vos); // 生成邻居代理的速度障碍物  
  
			bool insideAnyVO = BiasDesiredVelocity(vos, ref desiredVelocity, ref desiredTargetPointInVelocitySpace, simulator.symmetryBreakingBias); // 对期望速度进行偏置，判断是否在任何速度障碍物内  
  
			if (!insideAnyVO) // 如果不在任何速度障碍物内  
			{  
				// 由于期望速度没有在任何速度障碍物内，可以直接使用期望速度。不需要运行优化器，因为这将是全局最小值。  
				// 这是一个特殊情况，可以将计算的目标点设置为期望的目标点，而不是根据计算的速度计算一个点。  
				// 当代理非常接近目标点时，这是非常重要的。  
				// TODO: 如果desiredTargetPointInVelocitySpace.magnitude < desiredSpeed，这里并不能保证是全局最小值，可能需要做一些不同的事情。  
				calculatedTargetPoint = desiredTargetPointInVelocitySpace + position; // 计算目标点并设置到当前位置上  
				calculatedSpeed = desiredSpeed; // 速度设置为期望速度  
				// if (DebugDraw) Draw.Debug.CrossXZ(FromXZ(calculatedTargetPoint), Color.white); // 如果开启调试绘制，在计算出的目标点上绘制一个十字线  
				return; // 直接返回，不进行后续操作  
			}  
  
			Vector2 result = Vector2.zero; // 初始化结果向量  
  
			result = GradientDescent(vos, currentVelocity, desiredVelocity); // 使用梯度下降法进行优化计算结果向量  
  
			// if (DebugDraw) Draw.Debug.CrossXZ(FromXZ(result + position), Color.white); // 如果开启调试绘制，在计算出的结果点上绘制一个十字线  
			//Debug.DrawRay (To3D (position), To3D (result)); // 在3D空间中绘制一条从当前位置到计算结果的射线（调试用）  
  
			calculatedTargetPoint = position + result; // 将计算结果加到当前位置上得到目标点并设置到当前位置上  
			calculatedSpeed = Mathf.Min(result.magnitude, maxSpeed); // 速度设置为计算结果的大小和最大速度中的较小值  
		}
		
		Vector2 GradientDescent (VOBuffer vos, Vector2 sampleAround1, Vector2 sampleAround2) {  
			// 存储第一个样本点在VO缓冲区中的得分。  
			float score1;  
			// 获取第一个样本点在VO缓冲区中的最小值点。  
			var minima1 = Trace(vos, sampleAround1, out score1);  
			// // 如果启用了调试绘制，则在3D空间中绘制一个黄色十字标记来表示最小值点的位置。  
			// if (DebugDraw) Draw.Debug.CrossXZ(FromXZ(minima1 + position), Color.yellow, 0.5f);  
  
			// 以下代码可用于提高局部避免的质量，但已被注释掉。  
			// Can be uncommented for higher quality local avoidance  
			// for ( int i = 0; i < 3; i++ ) {  
			// Vector2 p = desiredVelocity + new Vector2(Mathf.Cos(Mathf.PI*2*(i/3.0f)), Mathf.Sin(Mathf.PI*2*(i/3.0f)));  
			// float score;Vector2 res = Trace ( vos, p, velocity.magnitude*simulator.qualityCutoff, out score );  
			// if ( score < best ) {  
			// result = res;  
			// best = score;  
			// }  
			// }  
  
			// 存储第二个样本点在VO缓冲区中的得分。  
			float score2;  
			// 获取第二个样本点在VO缓冲区中的最小值点。  
			Vector2 minima2 = Trace(vos, sampleAround2, out score2);  
			// // 如果启用了调试绘制，则在3D空间中绘制一个洋红色十字标记来表示第二个最小值点的位置。  
			// if (DebugDraw) Draw.Debug.CrossXZ(FromXZ(minima2 + position), Color.magenta, 0.5f);  
  
			// 根据两个最小值点的得分返回较低的那个。如果第一个得分更低，则返回第一个最小值点；否则返回第二个最小值点。  
			return score1 < score2 ? minima1 : minima2;  
		}
		
		/// <summary>  
		/// 追踪由速度障碍物向量场构建的向量。  
		/// 返回使得分最小的位置。  
		///  
		/// 参见：https://en.wikipedia.org/wiki/Gradient_descent  
		/// </summary>  
		Vector2 Trace (VOBuffer vos, Vector2 p, out float score) {  
			// 选择一个合理的初始步长  
			float stepSize = Mathf.Max(radius, 0.2f * desiredSpeed);  
  
			float bestScore = float.PositiveInfinity;  
			Vector2 bestP = p;  
  
			// TODO: 添加动量以加快收敛速度？  
  
			const int MaxIterations = 50;  
  
			for (int s = 0; s < MaxIterations; s++) {  
				float step = 1.0f - (s/(float)MaxIterations);  
				step = Sqr(step) * stepSize;  
  
				float value;  
				var gradient = EvaluateGradient(vos, p, out value);  
  
				if (value < bestScore) {  
					bestScore = value;  
					bestP = p;  
				}  
  
				// TODO: 添加性能截断  
  
				gradient.Normalize(); // 归一化梯度向量，使其长度为1。  
  
				gradient *= step; // 将梯度向量乘以步长。  
				Vector2 prev = p; // 保存当前位置。  
				p += gradient; // 沿着梯度方向移动位置。  
  
				// if (DebugDraw) Debug.DrawLine(FromXZ(prev + position), FromXZ(p + position), Rainbow(s*0.1f) * new Color(1, 1, 1, 1f)); // 在调试模式下绘制表示梯度下降过程的线条。  
				
			}  
  
			score = bestScore; // 输出最佳得分。  
			return bestP; // 返回最佳位置。  
		}
		
		/// <summary>  
		/// 评估在速度p处的代价函数的梯度和值。  
		/// </summary>  
		Vector2 EvaluateGradient (VOBuffer vos, Vector2 p, out float value) {  
			// 初始化梯度为零向量  
			Vector2 gradient = Vector2.zero;  
  
			// 初始化值为0  
			value = 0;  
  
			// 避免其他代理  
			for (int i = 0; i < vos.length; i++) {  
				float w;  
				// 获取其他代理的梯度和值  
				var grad = vos.buffer[i].ScaledGradient(p, out w);  
				// 如果当前代理的值大于已知的最大值，则更新最大值和对应的梯度  
				if (w > value) {  
					value = w;  
					gradient = grad;  
				}  
			}  
  
			// 向期望速度靠近  
			var dirToDesiredVelocity = desiredVelocity - p; // 计算从当前速度到期望速度的方向  
			var distToDesiredVelocity = dirToDesiredVelocity.magnitude; // 计算当前速度与期望速度的距离  
			if (distToDesiredVelocity > 0.0001f) { // 如果距离大于一个小的阈值  
				// 添加一个力，使当前速度向期望速度靠近，并根据距离和权重计算梯度与值  
				gradient += dirToDesiredVelocity * (DesiredVelocityWeight/distToDesiredVelocity);  
				value += distToDesiredVelocity * DesiredVelocityWeight;  
			}  
  
			// 更倾向于选择低于或等于期望速度的速度，并避免选择超过最大速度的速度  
			var sqrSpeed = p.sqrMagnitude; // 计算速度的平方值  
			if (sqrSpeed > desiredSpeed*desiredSpeed) { // 如果速度超过期望速度的平方值  
				var speed = Mathf.Sqrt(sqrSpeed); // 计算实际速度值  
				if (speed > maxSpeed) { // 如果速度超过了最大速度  
					const float MaxSpeedWeight = 3; // 定义超过最大速度的权重  
					value += MaxSpeedWeight * (speed - maxSpeed); // 更新值，表示惩罚项  
					gradient -= MaxSpeedWeight * (p/speed); // 更新梯度，表示减少超过最大速度的速度部分  
				}  
				// 根据期望速度和最大速度之间的差值，对速度进行惩罚，并更新梯度和值  
				float scale = 2*DesiredVelocityWeight; // 定义一个缩放因子，确保期望速度优先于最大速度  
				value += scale * (speed - desiredSpeed); // 更新值，表示惩罚项  
				gradient -= scale * (p/speed); // 更新梯度，表示减少超过期望速度的速度部分  
			}  
  
			return gradient; // 返回计算得到的梯度向量  
		}
		
		/// <summary>  
		/// 使代理倾向于右侧。  
		/// 最多将期望速度旋转[value]弧度。1弧度≈57°  
		/// 这有助于打破对称性。  
		///  
		/// 只有当期望速度位于速度障碍物（VO）内时，才会旋转期望速度。  
		/// 如果它在其中，则不会进一步旋转到其边缘。  
		///  
		/// 目标点在速度空间中将与期望速度旋转相同的角度。  
		///  
		/// 返回：如果期望速度在任何VO内，则返回True。  
		/// </summary>  
		static bool BiasDesiredVelocity (VOBuffer vos, ref Vector2 desiredVelocity, ref Vector2 targetPointInVelocitySpace, float maxBiasRadians) {  
			// 计算期望速度的大小（长度）。  
			var desiredVelocityMagn = desiredVelocity.magnitude;  
			var maxValue = 0f;  
  
			// 遍历所有的速度障碍物。  
			for (int i = 0; i < vos.length; i++) {  
				float value;  
				// 计算期望速度与VO边缘的距离。取最大值得到期望速度最远进入VO的距离。  
				vos.buffer[i].Gradient(desiredVelocity, out value);  
				maxValue = Mathf.Max(maxValue, value);  
			}  
  
			// 检查代理是否在任何VO内。  
			var inside = maxValue > 0;  
  
			// 如果期望速度的大小很小，则避免除以零。  
			if (desiredVelocityMagn < 0.001f) {  
				return inside;  
			}  
  
			// 将期望速度顺时针（向右转）旋转最多maxBiasRadians弧度。  
			// 由于maxBiasRadians较小，我们可以直接移动它以获得大致相同的效果。  
			// 参考：https://en.wikipedia.org/wiki/Small-angle_approximation  
			var angle = Mathf.Min(maxBiasRadians, maxValue / desiredVelocityMagn);  
			desiredVelocity += new Vector2(desiredVelocity.y, -desiredVelocity.x) * angle; // 将期望速度向右旋转一定的角度。  
			targetPointInVelocitySpace += new Vector2(targetPointInVelocitySpace.y, -targetPointInVelocitySpace.x) * angle; // 将目标点在速度空间中向右旋转相同的角度。  
			return inside; // 返回代理是否在任何VO内的结果。  
		}
		
		/// <summary>  
		/// 生成邻居代理的视觉对象。  
		/// </summary>  
		/// <param name="vos">用于存储生成的视觉对象的缓冲区。</param>  
		void GenerateNeighbourAgentVOs (VOBuffer vos) {  
		    // 计算时间逆向因子，用于在计算视觉对象时进行时间逆向处理。  
		    float inverseAgentTimeHorizon = 1.0f/agentTimeHorizon;  
		  
		    // 当前代理的速度向量。  
		    Vector2 optimalVelocity = currentVelocity;  
		  
		    // 遍历所有邻居代理。  
		    for (int o = 0; o < neighbours.Count; o++) {  
		        // 获取当前迭代的邻居代理。  
		        JNRVOAgent other = neighbours[o];  
		  
		        // 如果当前代理是迭代的邻居，则跳过。  
		        if (other == this)  
		            continue;  
		  
		        // 计算两个代理的y坐标的最大值和最小值。  
		        float maxY = System.Math.Min(elevationCoordinate + height, other.elevationCoordinate + other.height);  
		        float minY = System.Math.Max(elevationCoordinate, other.elevationCoordinate);  
		  
		        // 如果最大和最小y坐标之差小于0，则跳过。  
		        if (maxY - minY < 0) {  
		            continue;  
		        }  
		  
		        // 计算两个代理的总半径。  
		        float totalRadius = radius + other.radius;  
		  
		        // 计算两个代理的位置向量差。  
		        Vector2 voBoundingOrigin = other.position - position;  
		  
		        // 计算避免碰撞的强度。根据其他代理的优先级和当前代理的优先级，避免碰撞的强度会有所不同。  
		        float avoidanceStrength;  
		        if (other.locked || other.manuallyControlled) {  
		            avoidanceStrength = 1; // 如果其他代理被锁定或手动控制，避免碰撞的强度为1。  
		        } else if (other.Priority > 0.00001f || Priority > 0.00001f) { // 如果两个代理都有较高的优先级，则根据它们的优先级计算避免碰撞的强度。  
		            avoidanceStrength = other.Priority / (Priority + other.Priority);  
		        } else { // 否则，避免碰撞的强度为0.5。  
		            avoidanceStrength = 0.5f;  
		        }  
		  
		        // 如果两个代理的优先级相似，假设它们将继续以大致相同的速度移动。如果其他代理的优先级高于当前代理，则假设它将更倾向于沿着其期望速度移动。这会导致其他代理试图为高优先级的代理清出一条路径。如果不这样做，即使高优先级的代理在拥挤的环境中也可能被困住并被迫减速。  
		        Vector2 otherOptimalVelocity = Vector2.Lerp(other.currentVelocity, other.desiredVelocity, 2*avoidanceStrength - 1); // 根据避免碰撞的强度计算其他代理的最优速度。  
		  
		        // 根据当前代理和邻居代理的速度，计算视觉对象的中心点。  
		        var voCenter = Vector2.Lerp(optimalVelocity, otherOptimalVelocity, avoidanceStrength); // 使用线性插值计算视觉对象的中心点。  
		  
		        // 向缓冲区添加生成的视觉对象。包括位置向量差、视觉对象的中心点、总半径、时间逆向因子和时间步长倒数。  
		        vos.Add(new VO(voBoundingOrigin, voCenter, totalRadius, inverseAgentTimeHorizon, 1 / simulator.DeltaTime)); // 将视觉对象添加到缓冲区中。  
		        // if (DebugDraw) // 如果启用了调试绘制，则绘制视觉对象。  
		        //     DrawVO(position + voBoundingOrigin * inverseAgentTimeHorizon + voCenter, totalRadius * inverseAgentTimeHorizon, position + voCenter); // 在指定位置绘制视觉对象。  
		    } 
		} 
		
		
		// 定义一个函数，用于生成障碍物的视觉观察  
		void GenerateObstacleVOs (VOBuffer vos) {  
		    // 计算最大速度与障碍物时间视野（obstacleTimeHorizon）的乘积，得到一个范围值  
		    var range = maxSpeed * obstacleTimeHorizon;  
		  
		    // 遍历模拟器中的所有障碍物  
		    for (int i = 0; i < simulator.obstacles.Count; i++) {  
		        var obstacle = simulator.obstacles[i]; // 获取当前障碍物  
		        var vertex = obstacle; // 定义顶点变量，初始值为当前障碍物的第一个顶点  
		  
		        // 遍历障碍物的所有顶点  
		        do {  
		            // 如果当前顶点被忽略，或者当前顶点的层与需要碰撞的层不匹配，则跳过该顶点并继续下一个顶点  
		            if (vertex.ignore || (vertex.layer & collidesWith) == 0) {  
		                vertex = vertex.next;  
		                continue;  
		            }  
		  
		            float elevation1, elevation2; // 定义两个浮点变量，用于存储两个顶点的海拔 值  
		            var p1 = To2D(vertex.position, out elevation1); // 将当前顶点的位置转换为2D坐标，并获取其海拔值  
		            var p2 = To2D(vertex.next.position, out elevation2); // 将下一个顶点的位置转换为2D坐标，并获取其海拔值  
		  
		            Vector2 dir = (p2 - p1).normalized; // 计算两个顶点之间的单位向量  
		  
		            // TODO: 可以进行优化  
		            float dist = VO.SignedDistanceFromLine(p1, dir, position); // 计算当前位置到由p1和p2确定的直线的带符号距离  
		  
		            // 如果距离在给定的范围内（包括-0.01f），则进行以下判断和处理  
		            if (dist >= -0.01f && dist < range) {  
		                float factorAlongSegment = Vector2.Dot(position - p1, p2 - p1) / (p2 - p1).sqrMagnitude; // 计算当前位置与线段之间的因子  
		  
		                var segmentY = Mathf.Lerp(elevation1, elevation2, factorAlongSegment); // 使用线性插值计算线段的海拔值  
		  
		                var sqrDistToSegment = (Vector2.Lerp(p1, p2, factorAlongSegment) - position).sqrMagnitude; // 计算当前位置到线段的平方距离  
		  
		                // 如果点到线段的距离小于给定的范围平方，并且移动平面是XY平面或者是高度在允许范围内，则添加一个障碍物视觉观察到VOBuffer中  
		                if (sqrDistToSegment < range*range && (simulator.movementPlane == MovementPlane.XY || (elevationCoordinate <= segmentY + vertex.height && elevationCoordinate+height >= segmentY))) {  
		                    vos.Add(VO.SegmentObstacle(p2 - position, p1 - position, Vector2.zero, radius * 0.01f, 1f / ObstacleTimeHorizon, 1f / simulator.DeltaTime)); // 添加视觉观察到VOBuffer中  
		                }  
		            }  
		  
		            vertex = vertex.next; // 移动到下一个顶点，继续循环直到回到起始顶点或达到空顶点为止  
		        } while (vertex != obstacle && vertex != null && vertex.next != null); // 结束循环条件：回到起始顶点、达到空顶点或达到空障碍物链表末尾  
		    }  
		}
		
		/// <summary>  
		/// 将一个数字进行平方。  
		/// </summary>
		static float Sqr (float x) {
			return x*x;
		}
		
		/// <summary>将(x, 0, y)转换为Vector3类型。</summary>  
		/// <param name="p">一个Vector2类型的向量。</param>  
		/// <returns>返回一个Vector3类型的向量，其x和y坐标与输入相同，z坐标为0。</returns>  
		static Vector3 FromXZ (Vector2 p) {  
			return new Vector3(p.x, 0, p.y);  
		}  
  
		/// <summary>将(x, z)转换为Vector2类型。</summary>  
		/// <param name="p">一个Vector3类型的向量。</param>  
		/// <returns>返回一个Vector2类型的向量，其x和z坐标与输入相同。</returns>  
		static Vector2 ToXZ (Vector3 p) {  
			return new Vector2(p.x, p.z);  
		}  
  
		/// <summary>  
		/// 将3D向量转换为2D向量，转换的平面取决于movementPlane的值。  
		/// 如果movementPlane是XZ，则将向量投影到XZ平面上，并将Y坐标作为elevation返回。  
		/// 否则，将向量投影到XY平面上，并将Z坐标作为elevation返回。  
		/// </summary>  
		/// <param name="p">一个Vector3类型的向量。</param>  
		/// <param name="elevation">输出的elevation值。</param>  
		/// <returns>返回一个Vector2类型的向量，其坐标取决于movementPlane的值。</returns>  
		Vector2 To2D (Vector3 p, out float elevation) {  
			if (simulator.movementPlane == MovementPlane.XY) {  
				elevation = -p.z; // 将Z坐标取反作为elevation值，因为XY平面的Z坐标是负的。  
				return new Vector2(p.x, p.y); // 返回XY平面的X和Y坐标。  
			} else {  
				elevation = p.y; // Y坐标作为elevation值。  
				return new Vector2(p.x, p.z); // 返回XZ平面的X和Z坐标。  
			}  
		}
		
		
		/// <summary>  
	    /// 非常简单的列表。  
	    /// 不能使用 List<T>，因为当 T 是结构体（VO）时，对 List<T> 进行索引操作会导致整个结构体被复制。  
	    /// 而对数组进行索引时，可以跳过复制操作。  
	    /// </summary>  
	    internal class VOBuffer  
	    {  
		    public VO[] buffer;  
		    public int length;  
	  
		    public void Clear()  
		    {  
			    length = 0;  
		    }  
	  
		    public VOBuffer(int n)  
		    {  
			    buffer = new VO[n];  
			    length = 0;  
		    }  
	  
		    public void Add(VO vo)  
		    {  
			    if (length >= buffer.Length)  
			    {  
				    var nbuffer = new VO[buffer.Length * 2];  
				    Array.Copy(buffer, nbuffer, buffer.Length);  
				    buffer = nbuffer;  
			    }  
			    buffer[length++] = vo;  
		    }  
	    }

	    /// <summary>  
	    /// 速度障碍（Velocity Obstacle）。  
	    /// 这是一个结构体，用于避免过多的内存分配。  
	    ///  
	    /// 参考：https://en.wikipedia.org/wiki/Velocity_obstacle  
	    /// </summary>  
	    internal struct VO
	    {
		    // 定义四个向量，用于表示线段1和线段2的方向和位置  
		    Vector2 line1, line2, dir1, dir2;

		    // 定义两个向量，分别表示截断线和截断方向的位置和方向  
		    Vector2 cutoffLine, cutoffDir;

		    // 定义一个向量，表示圆心位置  
		    Vector2 circleCenter;

		    // 表示当前速度障碍是否与其它障碍物碰撞的布尔值  
		    bool colliding;

		    // 速度障碍的半径  
		    float radius;

		    // 权重因子，用于计算速度障碍的影响力  
		    float weightFactor;

		    // 权重加分，用于进一步调整权重因子  
		    float weightBonus;

		    // 定义一个线段的起点和终点位置  
		    Vector2 segmentStart, segmentEnd;

		    // 表示是否使用线段来表示速度障碍的布尔值  
		    bool segment;

		    /// <summary>创建一个用于避免其他agent的速度障碍。</summary>  
		    /// <param name="center">相对于此agent的其他agent的位置。</param>  
		    /// <param name="offset">速度障碍的偏移量。例如，用于考虑agent的相对速度。</param>  
		    /// <param name="radius">两个agent的组合半径（radius1 + radius2）。</param>  
		    /// <param name="inverseDt">1除以本地避障时间范围（例如，避免将在未来2秒内击中的agent）。</param>  
		    /// <param name="inverseDeltaTime">1除以时间步长。</param>
		    public VO(Vector2 center, Vector2 offset, float radius, float inverseDt, float inverseDeltaTime)
		    {
			    // 默认权重因子为1，权重加分设为0  
			    this.weightFactor = 1;
			    weightBonus = 0;

			    // 计算全局中心位置，考虑时间步长的影响  
			    Vector2 globalCenter;
			    circleCenter = center * inverseDt + offset;

			    // 根据中心位置和半径计算权重因子，这里使用了指数函数来调整权重因子  
			    this.weightFactor = 4 * Mathf.Exp(-Sqr(center.sqrMagnitude / (radius * radius))) + 1;
			    // 检查是否发生碰撞  
			    if (center.magnitude < radius)
			    {
				    colliding = true; // 表示发生碰撞  
				    // 如果中心点到原点的距离小于半径，则设置line1的长度，并计算方向dir1  
				    line1 = center.normalized * (center.magnitude - radius - 0.001f) * 0.3f * inverseDeltaTime;
				    dir1 = new Vector2(line1.y, -line1.x).normalized; // 计算线段的方向  
				    line1 += offset; // 将线段位置更新为全局位置  
				    cutoffDir = Vector2.zero; // 截断线的方向设为零向量  
				    cutoffLine = Vector2.zero; // 截断线的位置设为零向量  
				    dir2 = Vector2.zero; // 第二个线段的方向设为零向量  
				    line2 = Vector2.zero; // 第二个线段的位置设为零向量  
				    this.radius = 0; // 设置半径为0，表示当前速度障碍无效  
			    }
			    else
			    {
				    colliding = false; // 表示未发生碰撞  
				    // 根据时间步长和半径调整全局中心位置和半径值  
				    center *= inverseDt;
				    radius *= inverseDt;
				    globalCenter = center + offset;
				    // 计算截断线的距离，并设置截断线和截断方向的位置和方向  
				    var cutoffDistance = center.magnitude - radius + 0.001f; // 确保截断距离不是非常小，避免向量归一化后的值为零向量导致速度障碍无效  
				    cutoffLine = center.normalized * cutoffDistance; // 计算截断线的位置  
				    cutoffDir = new Vector2(-cutoffLine.y, cutoffLine.x).normalized; // 计算截断线的方向，并对其进行归一化处理  
				    cutoffLine += offset; // 将截断线的位置更新为全局位置  
				    float alpha = Mathf.Atan2(-center.y, -center.x); // 计算中心点相对于原点的角度值（弧度制）  
				    float delta = Mathf.Abs(Mathf.Acos(radius / center.magnitude)); // 计算与中心点距离的差值（弧度制）  
				    this.radius = radius; // 设置半径值  
				    // 计算线段1的位置和方向，这里使用了三角函数和向量的点乘运算来实现坐标转换和向量归一化处理。同时设置线段的方向dir1，并根据需要调整其他变量的值。最后将线段的位置和方向分别赋值给line1和dir1。以上代码实现了速度障碍的初始化设置，包括计算权重因子、判断是否发生碰撞、设置线段的位置和方向等操作。如果发生碰撞，则将线段的位置、方向和半径等设置为无效值。如果未发生碰撞，则根据时间步长和半径等参数进行相应的计算和调整。最后将线段的位置和方向分别赋值给line1和dir1。
				    // 计算边界线1上的点  
				    line1 = new Vector2(Mathf.Cos(alpha + delta), Mathf.Sin(alpha + delta));
				    // 计算与圆切线方向相同的向量，用于表示边界线的方向  
				    dir1 = new Vector2(line1.y, -line1.x);

				    // 计算边界线2上的点  
				    line2 = new Vector2(Mathf.Cos(alpha - delta), Mathf.Sin(alpha - delta));
				    // 计算与圆切线方向相同的向量，用于表示边界线的方向  
				    dir2 = new Vector2(line2.y, -line2.x);

				    // 根据半径和全局中心位置更新边界线的位置  
				    line1 = line1 * radius + globalCenter;
				    line2 = line2 * radius + globalCenter;
			    }
			    
			    segmentStart = Vector2.zero;
			    segmentEnd = Vector2.zero;
			    segment = false;
		    }

		    /// <summary>  
		    /// 创建一个用于避免另一个agent的VO（视觉障碍物）。  
		    /// 注意，这个VO是定向的，agent会想要位于segment的左侧。  
		    /// </summary>
		    public static VO SegmentObstacle(Vector2 segmentStart, Vector2 segmentEnd, Vector2 offset, float radius,
			    float inverseDt, float inverseDeltaTime)
		    {
			    
			    // 创建一个新的视觉障碍物（VO）对象  
				var vo = new VO();  
				  
				// 设置权重因子为1  
				vo.weightFactor = 1;  
				  
				// 设置权重奖励为半径和1之间的最大值的40倍  
				vo.weightBonus = Mathf.Max(radius, 1) * 40;  
				  
				// 计算线段上距离原点最近的点  
				var closestOnSegment = VectorMath.ClosestPointOnSegment(segmentStart, segmentEnd, Vector2.zero);  
				  
				// 如果最近点与线段的距离小于或等于半径，表示与线段有碰撞  
				if (closestOnSegment.magnitude <= radius) {  
				    vo.colliding = true;  
				    // 计算与线段相切的线，并设置其方向  
				    vo.line1 = closestOnSegment.normalized * (closestOnSegment.magnitude - radius) * 0.3f * inverseDeltaTime;  
				    vo.dir1 = new Vector2(vo.line1.y, -vo.line1.x).normalized;  
				    vo.line1 += offset;  
				    // 设置其他属性，重置线段起点、终点等属性  
				    vo.cutoffDir = Vector2.zero;  
				    vo.cutoffLine = Vector2.zero;  
				    vo.dir2 = Vector2.zero;  
				    vo.line2 = Vector2.zero;  
				    vo.radius = 0;  
				    vo.segmentStart = Vector2.zero;  
				    vo.segmentEnd = Vector2.zero;  
				    vo.segment = false;  
				} else {  
				    vo.colliding = false;  
				    // 将线段起点、终点和半径乘以时间倒数（inverseDt）进行缩放  
				    segmentStart *= inverseDt;  
				    segmentEnd *= inverseDt;  
				    radius *= inverseDt;  
				    // 计算切线方向（单位向量）  
				    var cutoffTangent = (segmentEnd - segmentStart).normalized;  
				    vo.cutoffDir = cutoffTangent;  
				    // 根据切线方向和半径计算切线线段，并加上偏移量  
				    vo.cutoffLine = segmentStart + new Vector2(-cutoffTangent.y, cutoffTangent.x) * radius;  
				    vo.cutoffLine += offset;  
				    // 计算线段起点的平方长度和线段终点的平方长度  
				    var startSqrMagnitude = segmentStart.sqrMagnitude;  
				    // 计算法线向量（normal1）和线段终点的平方长度  
				    var endSqrMagnitude = segmentEnd.sqrMagnitude;  
				    // 根据法线向量和半径计算线的位置，并加上偏移量  
				    var normal1 = -VectorMath.ComplexMultiply(segmentStart, new Vector2(radius, Mathf.Sqrt(Mathf.Max(0, startSqrMagnitude - radius*radius)))) / startSqrMagnitude;  
				    var normal2 = -VectorMath.ComplexMultiply(segmentEnd, new Vector2(radius, -Mathf.Sqrt(Mathf.Max(0, endSqrMagnitude - radius*radius)))) / endSqrMagnitude;  
				    // 根据法线向量和半径计算线的位置，并加上偏移量  
				    vo.line1 = segmentStart + normal1 * radius + offset;  
				    vo.line2 = segmentEnd + normal2 * radius + offset;  
				    // 计算方向向量（dir1）和方向向量（dir2）  
				    vo.dir1 = new Vector2(normal1.y, -normal1.x);  
				    vo.dir2 = new Vector2(normal2.y, -normal2.x);  
				    // 设置线段起点、终点等属性值  
				    vo.segmentStart = segmentStart;  
				    vo.segmentEnd = segmentEnd;  
				    vo.radius = radius;  
				    vo.segment = true;  
				}
			    
				return vo;
		    }

		    /// <summary>  
		    /// 如果点 p 位于直线 a 和 dir 方向上，那么返回一个负数。  
		    /// 该数值可以视为三角形 {a, a+dir, p} 的双倍带符号面积与 dir 长度的乘积。  
		    /// 如果 dir 的长度为 1，那么这个值也是点 p 到直线 {a, a+dir} 的距离。  
		    /// </summary>  
		    /// <param name="a">直线上的一个点</param>  
		    /// <param name="dir">直线的方向向量</param>  
		    /// <param name="p">待判断的点</param>  
		    /// <returns>如果 p 在直线的左侧，则返回负数；否则返回0。</returns>  
		    public static float SignedDistanceFromLine(Vector2 a, Vector2 dir, Vector2 p) {  
			    return (p.x - a.x) * dir.y - dir.x * (p.y - a.y);  
		    }  
	  
		    /// <summary>  
		    /// 计算视觉障碍物（VO）的成本函数的梯度和值。  
		    /// 该方法与 <see cref="Gradient"/> 方法类似，但梯度和值经过了轻微的缩放和调整。  
		    /// </summary>  
		    /// <param name="p">点坐标</param>  
		    /// <param name="weight">输出的权重值</param>  
		    /// <returns>缩放后的梯度向量</returns>  
		    public Vector2 ScaledGradient(Vector2 p, out float weight) {  
			    var grad = Gradient(p, out weight); // 调用 Gradient 方法计算梯度和值  
	  
			    if (weight > 0) {  
				    const float Scale = 2; // 缩放因子为2  
				    grad *= Scale * weightFactor; // 梯度缩放  
				    weight *= Scale * weightFactor; // 权重缩放  
				    weight += 1 + weightBonus; // 权重调整  
			    }  
	  
			    return grad; // 返回缩放后的梯度向量  
		    }
		    
		    /// <summary>  
		    /// 计算此VO（视觉对象）的成本函数的梯度和值。  
		    /// 这个VO有一个成本函数，该函数在VO外部的值为0，  
		    /// 当点向VO内部移动时，函数值会增加。  
		    ///  
		    /// 这是该函数的负梯度以及它的值（权重）。负梯度指向函数值减少最快的方向。  
		    ///  
		    /// 该函数的值是点到VO最短边缘的距离，梯度是经过归一化的。  
		    /// </summary>
			public Vector2 Gradient (Vector2 p, out float weight) {
				// 判断点p是否与线相撞  
				if (colliding) {  
				    // 计算由点{line1, line1+dir1, p}组成的三角形的双带符号面积  
				    float l1 = SignedDistanceFromLine(line1, dir1, p);  
				  
				    // 用于检查点p在线的哪一侧  
				    // 如果l1大于等于0，则点p在线的右侧  
				    if (l1 >= 0) {  
				        weight = l1;  
				        return new Vector2(-dir1.y, dir1.x); // 返回向量(-dir1.y, dir1.x)作为梯度  
				    } else {  
				        weight = 0;  
				        return new Vector2(0, 0); // 返回(0,0)作为梯度  
				    }  
				}  
				  
				// 计算点p到切割线cutoffLine的距离  
				float det3 = SignedDistanceFromLine(cutoffLine, cutoffDir, p);  
				if (det3 <= 0) {  
				    weight = 0;  
				    return Vector2.zero; // 返回(0,0)作为梯度  
				} else {  
				    // 计算点p到线line1的距离  
				    float det1 = SignedDistanceFromLine(line1, dir1, p);  
				    // 计算点p到线line2的距离  
				    float det2 = SignedDistanceFromLine(line2, dir2, p);  
				    if (det1 >= 0 && det2 >= 0) {  
				        Vector2 gradient; // 定义梯度变量  
				  
				        // 如果点p在线line1的右侧，并且在line2的左侧  
				        if (Vector2.Dot(p - line1, dir1) > 0 && Vector2.Dot(p - line2, dir2) < 0) {  
				            if (segment) { // 如果线是线段类型  
				                // 如果点p与线段的距离小于半径radius  
				                if (det3 < radius) {  
				                    var closestPointOnLine = (Vector2)VectorMath.ClosestPointOnSegment(segmentStart, segmentEnd, p); // 获取线段上离点p最近的点  
				                    var dirFromCenter = p - closestPointOnLine; // 计算从中心点到点p的方向向量  
				                    float distToCenter; // 定义点到中心点的距离变量  
				                    gradient = VectorMath.Normalize(dirFromCenter, out distToCenter); // 对方向向量进行归一化，并计算其长度  
				                    weight = radius - distToCenter; // 设置权重为半径减去点到中心点的距离  
				                    return gradient; // 返回梯度向量  
				                }  
				            } else { // 如果线是圆形类型  
				                var dirFromCenter = p - circleCenter; // 计算从圆心到点p的方向向量  
				                float distToCenter; // 定义点到圆心的距离变量  
				                gradient = VectorMath.Normalize(dirFromCenter, out distToCenter); // 对方向向量进行归一化，并计算其长度  
				                weight = radius - distToCenter; // 设置权重为半径减去点到圆心的距离  
				                return gradient; // 返回梯度向量  
				            }  
				        }  
				  
				        // 如果点p与切割线cutoffLine的距离小于与line1和line2的距离，并且线是线段类型  
				        if (segment && det3 < det1 && det3 < det2) {  
				            weight = det3; // 设置权重为点p与切割线的距离  
				            gradient = new Vector2(-cutoffDir.y, cutoffDir.x); // 返回梯度向量(-cutoffDir.y, cutoffDir.x)  
				            return gradient; // 返回梯度向量  
				        }  
				  
				        // 根据点p与line1和line2的距离确定权重和梯度向量  
				        if (det1 < det2) { // 如果点p与line1的距离小于与line2的距离  
				            weight = det1; // 设置权重为点p与line1的距离  
				            gradient = new Vector2(-dir1.y, dir1.x); // 返回梯度向量(-dir1.y, dir1.x)作为梯度  
				        } else { // 否则，即点p与line2的距离小于与line1的距离  
				            weight = det2; // 设置权重为点p与line2的距离  
				            gradient = new Vector2(-dir2.y, dir2.x); // 返回梯度向量(-dir2.y, dir2.x)作为梯度  
				        }
						return gradient;
					}

					weight = 0;
					return Vector2.zero;
				}
			}
		    

	    }
		
    }
    
    

}