using System;
using System.Collections.Generic;
using Plugins.JNGame.Sync.Frame.Entity;
using UnityEngine;

namespace Game.Plugins.JNGame.Sync.Frame.AstarPath.RVO
{
	/// <summary>  
	/// 代理类的属性暴露。  
	/// </summary>  
	public interface JNIAgent  
	{  
		/// <summary>  
		/// 代理的位置。  
		/// 代理本身不会移动，需要一个移动脚本负责读取CalculatedTargetPoint和CalculatedSpeed属性，并以该速度朝该点移动。  
		/// 该属性应每帧设置一次。  
		///  
		/// 注意：这是一个Vector2，而不是Vector3，因为RVO在内部以2D模拟一切。 position3D = new Vector3(agent.Position.x, agent.ElevationCoordinate, agent.Position.y);  
		/// </code>  
		/// </summary>  
		Vector2 Position { get; set; }  
  
		/// <summary>  
		/// 在高度方向上分隔角色的坐标。  
		/// 由于RVO可以在2D或3D中使用，所以仅仅使用3D位置的y坐标并不简单。  
		/// 在3D中，这很可能会被设置为y坐标，但在2D（俯视）中，在大多数情况下应该设置为0，因为所有角色总是在同一平面上，但是它可以被设置为其他值，例如如果游戏是2D等距的。  
		///  
		/// 该位置假定在角色的基础部分（靠近脚部）。  
		/// </summary>  
		float ElevationCoordinate { get; set; }  
  
		/// <summary>  
		/// 避免碰撞的最优移动点。  
		/// 移动脚本应以<see cref="CalculatedSpeed"/>的速度朝此点移动。  
		///  
		/// 注意：这是一个Vector2，而不是Vector3，因为SetTarget方法接受的是Vector2。  
		///  
		/// 参考：RVOController.CalculateMovementDelta。  
		/// </summary>  
		Vector2 CalculatedTargetPoint { get; }  

		/// <summary>  
		/// 代理避免碰撞的最优速度。  
		/// 移动脚本应以此速度朝向<see cref="CalculatedTargetPoint"/>移动。  
		/// </summary>  
		float CalculatedSpeed { get; }

		/// <summary>  
		/// 代理应朝向移动的点。  
		/// 通常每帧设置一次。代理将尽量接近目标点。将在下一个模拟步骤中生效。  
		///  
		/// 注意：系统假定代理在到达目标点时会停止，  
		/// 因此，如果您只想向特定方向移动代理，请确保将目标点设置在角色前方一段距离处，  
		/// 否则系统可能无法很好地避免碰撞。简而言之，系统（以简化方式）会认为代理会在碰撞前停止，  
		/// 因此不会减速或改变方向。请参阅下面的图片。在图片中，desiredSpeed 是蓝色箭头的长度，  
		/// 目标点是黑色箭头指向的点。在上图中，代理没有避免红色代理（您可以假设红色代理的速度非常小以简化问题），  
		/// 而在下图中它确实避免了。normalized * remainingPathDistance  
		/// </code>  
		/// 其中 remainingPathDistance 是角色到达路径终点之前的距离。这很好用，因为在路径的终点处，  
		/// 到下一个路点的方向将只是到路径上最后一个点的方向，而 remainingPathDistance 将是到路径上最后一个点的距离，  
		/// 因此 targetPoint 将被设置为简单地设置为路径上的最后一个点。但是，当 remainingPathDistance 很大时，  
		/// targetPoint 将设置得如此远，以至于代理基本上会被指示朝某个方向移动，这正是我们想要的。  
		/// [打开在线文档查看图片]
		/// </summary>  
		/// <param name="targetPoint">世界空间中的目标点（XZ平面或XY平面，取决于模拟是否配置为2D或3D）。  
		///      请注意，这是一个Vector2，而不是Vector3，因为系统内部以2D方式模拟一切。因此，如果您的代理在XZ平面上移动，您必须将其提供为具有(x,z)坐标的Vector2。</param>  
		/// <param name="desiredSpeed">代理的期望速度。以世界单位每秒为单位。代理将尽量以这个速度移动。</param>  
		/// <param name="maxSpeed">代理的最大速度。以世界单位每秒为单位。如果必要（例如，如果另一个代理正朝向此代理的碰撞轨迹移动），代理可以以这个速度移动。  
		///      应该至少与desiredSpeed一样高，但建议使用比desiredSpeed稍高的值（例如desiredSpeed*1.2）。</param>
		void SetTarget(Vector2 targetPoint, float desiredSpeed, float maxSpeed);

		/// <summary>被锁定的代理将被假定为不会移动</summary>
		bool Locked { get; set; }

		/// <summary>  
		/// 代理在世界单位中的半径。  
		/// 代理被建模为圆形/圆柱形。  
		/// </summary>  
		float Radius { get; set; }  
  
		/// <summary>  
		/// 代理在世界单位中的高度。  
		/// 代理被建模为圆形/圆柱形。  
		/// </summary>  
		float Height { get; set; }  
  
		/// <summary>  
		/// 估计用于查看未来与代理发生碰撞的最大秒数。  
		/// 事实证明，此变量也非常适合用于控制代理的避免优先级。  
		/// 值较低的代理会较少避免其他代理，因此可以通过赋予它们较低的值来创建“高优先级代理”。  
		/// </summary>  
		float AgentTimeHorizon { get; set; }
		
		/// <summary>  
		/// 估计用于查看未来与障碍物发生碰撞的最大秒数。  
		/// </summary>
		float ObstacleTimeHorizon { get; set; }

		/// <summary>  
		/// 要考虑的最大代理数量。  
		/// 减少此值可以提高性能，增加它可以提高模拟的质量。  
		/// </summary>
		int MaxNeighbours { get; set; }
		
		/// <summary>  
		/// 上一个模拟步骤中代理考虑的邻居数量。  
		/// </summary>
		int NeighbourCount { get; }

		/// <summary>  
		/// 指定此代理的避障层。  
		/// 其他代理上的<see cref="CollidesWith"/>掩码将决定它们是否会避开此代理。  
		/// </summary>
		JNRVOLayer Layer { get; set; }

		/// <summary>  
		/// 用于指定该代理会避开的图层掩码。  
		/// 可以将其设置为CollidesWith = RVOLayer.DefaultAgent | RVOLayer.Layer3 | RVOLayer.Layer6 ...  
		///  
		/// 参考：http://en.wikipedia.org/wiki/Mask_(computing)  
		/// </summary>
		JNRVOLayer CollidesWith { get; set; }

		/// <summary>  
		/// 绘制调试信息。  
		///  
		/// 注意：即使<see cref="Pathfinding.RVO.Simulator.movementPlane"/>设置为XY平面，也将在XZ平面上始终绘制调试信息。  
		/// 注意：如果模拟器组件启用了多线程，则忽略此属性。  
		/// 因为Unity的调试API只能从主线程调用。  
		/// </summary>
		bool DebugDraw { get; set; }

		/// <summary>  
		/// 在上一个模拟步骤中与代理近距离接触的障碍物段落的列表。  
		/// 例如，可以用于施加额外的墙壁避免力。  
		/// 段落由障碍物的顶点及其.next属性形成。  
		///  
		/// 错误：始终返回null。  
		/// </summary>
		List<JNObstacleVertex> NeighbourObstacles { get; }
		
		/// <summary>  
		/// 其他代理避免此代理的强烈程度。  
		/// 通常是一个介于0和1之间的值。  
		/// 具有相似优先级的代理将以相同的强度相互避免。  
		/// 如果一个代理看到另一个优先级高于自己的代理，它会更加强烈地避免该代理。  
		/// 在极端情况下（例如，此代理的优先级为0，而另一个代理的优先级为1），它会将另一个代理视为移动障碍物。  
		/// 类似地，如果一个代理看到另一个优先级低于自己的代理，它会避免该代理的强度较小。0:  
		///     avoidanceStrength = other.priority / (this.priority + other.priority);  
		/// 否则:  
		///     avoidanceStrength = 0.5  
		/// </code>  
		/// </summary>
		float Priority { get; set; }

		/// <summary>  
		/// 在避障计算开始之前将被调用的回调函数。  
		/// 用于使用最新值更新其他属性。  
		/// </summary>
		System.Action PreCalculationCallback { set; }

		/// <summary>  
		/// 设置代理当前与之碰撞的墙壁（或其他物体）的法线。  
		/// 这用于使RVO系统能够感知物理或其他代理被夹在导航网格上的情况。  
		/// 其他代理观察到的此代理的速度将被修改，因此不会产生撞墙的分量。  
		/// 但是，代理不会开始避开墙壁，要实现这一点，您需要添加RVO障碍物。  
		///  
		/// 在下一个模拟步骤之后，此值将被清除，通常应在碰撞仍在发生时每个帧设置此值。  
		/// </summary>
		void SetCollisionNormal(Vector2 normal);

		/// <summary>  
		/// 设置代理的当前速度。  
		/// 这将完全覆盖本地避障输入。  
		/// 如果您有一个由玩家控制的角色，并希望其他代理避开它，这很有用。  
		///  
		/// 调用此方法将使代理在下一个模拟步骤中被标记为外部控制。  
		/// 下一个模拟步骤中的本地避障计算将被跳过，但之后将恢复，除非再次调用此方法。  
		/// </summary>
		void ForceSetVelocity(Vector2 velocity);
	}
	
	/// <summary>  
	/// 描述物体主要在哪个平面上移动的枚举类型。  
	/// </summary>  
	public enum MovementPlane {  
		/// <summary>  
		/// 物体主要在XZ平面上移动（3D环境）。  
		/// </summary>  
		XZ,  
		/// <summary>  
		/// 物体主要在XY平面上移动（2D环境）。  
		/// </summary>  
		XY  
	}
	
	[System.Flags]
	public enum JNRVOLayer {
		DefaultAgent = 1 << 0,
		DefaultObstacle = 1 << 1,
		Layer2 = 1 << 2,
		Layer3 = 1 << 3,
		Layer4 = 1 << 4,
		Layer5 = 1 << 5,
		Layer6 = 1 << 6,
		Layer7 = 1 << 7,
		Layer8 = 1 << 8,
		Layer9 = 1 << 9,
		Layer10 = 1 << 10,
		Layer11 = 1 << 11,
		Layer12 = 1 << 12,
		Layer13 = 1 << 13,
		Layer14 = 1 << 14,
		Layer15 = 1 << 15,
		Layer16 = 1 << 16,
		Layer17 = 1 << 17,
		Layer18 = 1 << 18,
		Layer19 = 1 << 19,
		Layer20 = 1 << 20,
		Layer21 = 1 << 21,
		Layer22 = 1 << 22,
		Layer23 = 1 << 23,
		Layer24 = 1 << 24,
		Layer25 = 1 << 25,
		Layer26 = 1 << 26,
		Layer27 = 1 << 27,
		Layer28 = 1 << 28,
		Layer29 = 1 << 29,
		Layer30 = 1 << 30
	}
	
	
	internal class WorkerContext {
		public JNRVOAgent.VOBuffer vos = new JNRVOAgent.VOBuffer(16);

		public const int KeepCount = 3;
		public Vector2[] bestPos = new Vector2[KeepCount];
		public float[] bestSizes = new float[KeepCount];
		public float[] bestScores = new float[KeepCount+1];

		public Vector2[] samplePos = new Vector2[50];
		public float[] sampleSize = new float[50];
	}
    
    /// <summary>
    /// RVO 模拟器
    /// 本地避障模拟器。  
    /// 这个类使用Reciprocal Velocity Obstacles (RVO) 和 Optimal Reciprocal Collision Avoidance (ORCA) 方法处理多个代理的本地避障模拟。  
    ///  
    /// 这个类负责根据脚本提供的期望速度计算速度。然而，它不负责在Unity场景中移动任何物体。对于这个功能，还有其他脚本可用（详见下文）。  
    ///  
    /// 可以向模拟器添加和移除障碍物，也可以随时添加和移除代理。  
    /// 参考：RVOSimulator  
    /// 参考：RVOAgent  
    /// 参考：Pathfinding.RVO.IAgent  
    ///  
    /// 该实现使用基于采样的算法，结合梯度下降法来找到避障速度。  
    ///  
    /// 你可能主要会使用包装类RVOSimulator。  
    /// </summary>
    public class JNRVOSimulator
    {
	    //
	    // /// <summary>  
	    // /// 使用双缓冲技术。  
	    // /// 双缓冲技术是一种图形渲染技术，用于减少屏幕撕裂和闪烁现象。  
	    // /// </summary>  
	    // private readonly bool doubleBuffering = true;  
  
	    /// <summary>模拟中的代理</summary>  
	    /// 代理可以理解为模拟中的实体或对象，它们会根据模拟规则进行移动或交互。  
	    List<JNRVOAgent> agents = new List<JNRVOAgent>();  
  
	    /// <summary>模拟中的障碍物</summary>  
	    /// 障碍物是模拟环境中的静态对象，代理在移动时需要避免与障碍物发生碰撞。  
	    public List<JNObstacleVertex> obstacles = new List<JNObstacleVertex>();

	    /// <summary>  
	    /// 用于此模拟的四叉树。  
	    /// 四叉树是一种数据结构，用于高效地管理和查询空间信息。在此模拟中，四叉树用于快速查找代理的邻居。  
	    /// </summary>  
	    public JNRVOQuadtree Quadtree = new JNRVOQuadtree();
  
	    private float deltaTime;  
	    /// <summary>上次模拟的时间步长</summary>  
	    private float lastStep = -99999;  
  
	    /// <summary>是否需要更新障碍物的状态</summary>  
	    private bool doUpdateObstacles = false;  
	    /// <summary>是否需要清除障碍物</summary>  
	    private bool doCleanObstacles = false;  
  
	    /// <summary>当前时间步长的持续时间</summary>  
	    public float DeltaTime { get { return deltaTime; } }  
  
	    /// <summary>是否使用多线程进行模拟</summary>  
	    public bool Multithreading  
	    {  
		    get { return false; } // 目前未启用多线程模拟功能。  
	    }
	    
	    private WorkerContext context = new WorkerContext();
	    
	    /// <summary>  
	    /// 所需的模拟帧率的倒数。  
	    /// 请参见：DesiredDeltaTime  
	    /// </summary>  
	    private float desiredDeltaTime = 0.03f;  
  
	    /// <summary>  
	    /// 每次模拟步骤之间的时间（以秒为单位）。  
	    /// 这是期望的Delta时间，模拟的帧率永远不会高于Update函数被调用的速率。  
	    /// </summary>  
	    public float DesiredDeltaTime   
	    {   
		    get { return desiredDeltaTime; }   
		    set { desiredDeltaTime = Math.Max(value, 0.0f); }   
	    }  
	    
	    /// <summary>  
	    /// 使代理在彼此之间通过右侧。  
	    /// 如果一个代理的期望速度使其与另一个代理或障碍物处于碰撞航线上，  
	    /// 其期望速度将向右旋转这个弧度数（1弧度约等于57°）。  
	    /// 这有助于打破对称性，并使更快地解决某些情况成为可能。  
	    ///  
	    /// 当许多代理具有相同的目标时，这可能会产生副作用，即围绕目标点的群体  
	    /// 可能会作为一个整体开始围绕目标点旋转。  
	    ///  
	    /// 推荐的值范围是0到0.2。  
	    ///  
	    /// 如果此值为负数，代理将偏向于在左侧彼此通过。  
	    /// </summary>  
	    public float symmetryBreakingBias = 0.1f;
	    
	    /// <summary>  
	    /// 判断用于移动的是XY（2D）平面还是XZ（3D）平面。  
	    /// </summary>
	    public readonly MovementPlane movementPlane = MovementPlane.XZ;
	    
	    public JNRVOSimulator (MovementPlane movementPlane) {
		    this.movementPlane = movementPlane;
		    Quadtree = new JNRVOQuadtree();

		    agents = new List<JNRVOAgent>();
		    obstacles = new List<JNObstacleVertex>();
	    }
	    
	    
	    void PreCalculation () {
		    for (int i = 0; i < agents.Count; i++) agents[i].PreCalculation();
	    }
	    
	    void CleanAndUpdateObstaclesIfNecessary () {
		    if (doCleanObstacles) {
			    CleanObstacles();
			    doCleanObstacles = false;
			    doUpdateObstacles = true;
		    }

		    if (doUpdateObstacles) {
			    doUpdateObstacles = false;
		    }
	    }
	    
	    private void CleanObstacles () {
	    }
	    
	    /// <summary>  
	    /// 从模拟中移除所有代理。  
	    /// </summary>  
	    public void ClearAgents () {  
		    // 遍历代理列表，将每个代理的模拟器设置为 null。  
		    for (int i = 0; i < agents.Count; i++) {  
			    agents[i].simulator = null;  
		    }  
		    // 清空代理列表。  
		    agents.Clear();  
	    }
	    /// <summary>  
	    /// 在指定位置添加一个代理。  
	    /// 您可以使用返回的接口来读取和写入参数，并设置例如半径和要移动到的目标点。  
	    ///  
	    /// 查看：RemoveAgent  
	    /// </summary>  
	    /// <param name="position">参见IAgent.Position</param>  
	    /// <param name="elevationCoordinate">参见IAgent.ElevationCoordinate</param>  
	    public JNIAgent AddAgent (Vector2 position, float elevationCoordinate) {  
		    // 创建一个新的代理实例，并传入位置和海拔坐标。  
		    JNRVOAgent agent = new JNRVOAgent(position, elevationCoordinate);  
		    // 调用另一个AddAgent方法来添加代理。  
		    return AddAgent(agent);  
	    }
	    
	    /// <summary>  
	    /// 添加先前已从模拟中移除的代理到模拟中。  
	    /// 查看：RemoveAgent  
	    /// </summary>  
	    public JNIAgent AddAgent (JNIAgent agent) {  
		    // 检查代理是否为空，如果为空则抛出异常  
		    if (agent == null) throw new System.ArgumentNullException("Agent must not be null");  
  
		    // 尝试将代理转换为 Agent 类型，并检查转换是否成功  
		    JNRVOAgent agentReal = agent as JNRVOAgent;  
		    if (agentReal == null) throw new System.ArgumentException("The agent must be of type Agent. Agent was of type "+agent.GetType());  
  
		    // 检查代理是否已经存在于模拟中，如果存在则抛出异常  
		    if (agentReal.simulator != null && agentReal.simulator == this) throw new System.ArgumentException("The agent is already in the simulation");  
		    else if (agentReal.simulator != null) throw new System.ArgumentException("The agent is already added to another simulation");  
  
		    // 如果代理当前没有模拟器，则为该代理设置当前模拟器  
		    agentReal.simulator = this;  
  
		    // 将代理添加到代理列表中  
		    agents.Add(agentReal);  
		    return agent; // 返回添加的代理对象  
	    }
	    
	    /// <summary>  
	    /// 将之前移除的障碍物重新添加到模拟中。  
	    /// 此方法不会检查障碍物是否已经被添加到模拟中，因此请确保不要多次添加同一个障碍物。  
	    /// 假设这是一个有效的障碍物。  
	    /// </summary>  
	    /// <param name="v">要添加的障碍物顶点。</param>  
	    /// <returns>返回添加的障碍物顶点。</returns>  
	    /// <exception cref="System.ArgumentNullException">如果传入的障碍物顶点为 null，则抛出此异常。</exception>
	    public JNObstacleVertex AddObstacle(JNObstacleVertex v)  
	    {  
		    // 如果传入的障碍物顶点为 null，则抛出一个 ArgumentNullException 异常，  
		    // 异常消息指出障碍物不能为空。  
		    if (v == null) throw new System.ArgumentNullException("Obstacle must not be null");  
  
		    // 将障碍物顶点添加到 obstacles 集合中。  
		    obstacles.Add(v);  
		    UpdateObstacles();
  
		    // 返回添加的障碍物顶点。  
		    return v;  
	    }
	    
	    /// <summary>  
	    /// 根据顶点数组添加障碍物。  
	    ///  
	    /// 请注意，此方法与 RemoveObstacle 方法相关，用于在模拟中添加和移除障碍物。  
	    /// </summary>  
	    /// <param name="vertices">描述障碍物的顶点数组。</param>  
	    /// <param name="height">障碍物的高度。</param>  
	    /// <param name="matrix">用于转换顶点坐标的4x4矩阵。</param>  
	    /// <param name="layer">障碍物所在的RVO层，默认为DefaultObstacle层。</param>  
	    /// <param name="cycle">是否将障碍物的顶点连接成循环链表，默认为true。</param>  
	    /// <returns>返回添加的障碍物的第一个顶点。</returns>  
	    /// <exception cref="System.ArgumentNullException">如果传入的顶点数组为null，则抛出此异常。</exception>  
	    /// <exception cref="System.ArgumentException">如果顶点数组中的顶点数量少于2个，则抛出此异常。</exception>
	    /// 
	    
	    public JNObstacleVertex AddObstacle(Vector3[] vertices, float height, Matrix4x4 matrix, JNRVOLayer layer = JNRVOLayer.DefaultObstacle, bool cycle = true)  
	    {  
		    // 如果传入的顶点数组为null，则抛出ArgumentNullException异常。  
		    if (vertices == null) throw new System.ArgumentNullException("Vertices must not be null");  
		    // 如果顶点数组中的顶点数量少于2个，则抛出ArgumentException异常。  
		    if (vertices.Length < 2) throw new System.ArgumentException("Less than 2 vertices in an obstacle");  
  
		    JNObstacleVertex first = null; // 障碍物的第一个顶点  
		    JNObstacleVertex prev = null;  // 前一个顶点，用于构建顶点链表  
  
		    // 遍历顶点数组，为每个顶点创建一个ObstacleVertex对象，并构建顶点之间的链表关系。  
		    for (int i = 0; i < vertices.Length; i++)  
		    {  
			    var v = new JNObstacleVertex  
			    {  
				    prev = prev,  // 前一个顶点  
				    layer = layer, // 障碍物所在的层  
				    height = height // 障碍物的高度  
			    };  
  
			    if (first == null) first = v; // 如果是第一个顶点，则将其设置为first  
			    else prev.next = v;           // 否则，将前一个顶点的next指向当前顶点  
  
			    prev = v; // 更新prev为当前顶点，用于下一个循环迭代  
		    }  
  
		    // 如果cycle为true，则将障碍物的顶点连接成循环链表。  
		    if (cycle)  
		    {  
			    prev.next = first; // 最后一个顶点的next指向第一个顶点  
			    first.prev = prev;  // 第一个顶点的prev指向最后一个顶点  
		    }  
  
		    // 更新障碍物的信息，可能是根据顶点数组和转换矩阵来设置障碍物的最终位置和形状。  
		    UpdateObstacle(first, vertices, matrix);  
  
		    // 将障碍物的第一个顶点添加到obstacles集合中。  
		    obstacles.Add(first);  
  
		    // 返回添加的障碍物的第一个顶点。  
		    return first;  
	    }
	    
	    /// <summary>  
		/// 更新障碍物的顶点。  
		///  
		/// 障碍物中的顶点数量不能改变，只能移动现有的顶点。  
		/// </summary>  
		/// <param name="obstacle">要更新的障碍物</param>  
		/// <param name="vertices">障碍物的新顶点数组，必须至少包含原始障碍物中的顶点数量</param>  
		/// <param name="matrix">在更新障碍物之前用于乘以顶点的矩阵</param>  
		public void UpdateObstacle(JNObstacleVertex obstacle, Vector3[] vertices, Matrix4x4 matrix)  
		{  
		    // 如果传入的顶点数组为空，则抛出异常，表示顶点不能为空  
		    if (vertices == null) throw new System.ArgumentNullException("Vertices must not be null");  
		    // 如果传入的障碍物对象为空，则抛出异常，表示障碍物不能为空  
		    if (obstacle == null) throw new System.ArgumentNullException("Obstacle must not be null");  
		  
		    // 如果顶点数组中的顶点数量少于2个，则抛出异常，因为障碍物至少需要2个顶点  
		    if (vertices.Length < 2) throw new System.ArgumentException("Less than 2 vertices in an obstacle");  
		  
		    // 检查矩阵是否是单位矩阵，单位矩阵乘以任何点都不会改变点的位置  
		    bool identity = matrix == Matrix4x4.identity;  
		  
		    int count = 0;  
		  
		    // 障碍物使用链表表示  
		    var vertex = obstacle;  
		    do   
		    {  
		        // 如果已经处理的顶点数量超过传入的新顶点数组长度，则抛出异常  
		        if (count >= vertices.Length)  
		        {  
		            // 使用红色线条在调试时绘制连接上一个顶点和当前顶点的线段  
		            Debug.DrawLine(vertex.prev.position, vertex.position, Color.red);  
		            throw new System.ArgumentException("Obstacle has more vertices than supplied for updating (" + vertices.Length + " supplied)");  
		        }  
		  
		        // 如果矩阵是单位矩阵，则直接赋值；否则使用矩阵乘以顶点得到新的位置  
		        // 这里的优化可能不是必要的，因为即使matrix不是单位矩阵，MultiplyPoint3x4也是一个很高效的操作  
		        vertex.position = identity ? vertices[count] : matrix.MultiplyPoint3x4(vertices[count]);  
		  
		        // 移动到下一个顶点  
		        vertex = vertex.next;  
		        count++;  
		    }   
		    while (vertex != obstacle && vertex != null); // 循环直到回到障碍物的第一个顶点或链表结束  
		  
		    // 重新计算每个顶点的方向向量  
		    vertex = obstacle;  
		    do   
		    {  
		        if (vertex.next == null)  
		        {  
		            // 如果是链表的最后一个顶点，则方向向量为零向量  
		            vertex.dir = Vector2.zero;  
		        }  
		        else  
		        {  
		            // 计算当前顶点到下一个顶点的方向向量，并标准化  
		            Vector3 dir = vertex.next.position - vertex.position;  
		            vertex.dir = new Vector2(dir.x, dir.z).normalized;  
		        }  
		  
		        // 移动到下一个顶点  
		        vertex = vertex.next;  
		    }   
		    while (vertex != obstacle && vertex != null); // 循环直到回到障碍物的第一个顶点或链表结束  
		  
		    // 安排清理障碍物（可能是移除不再需要的障碍物）  
		    ScheduleCleanObstacles();  
		    // 更新所有障碍物（可能是重新计算位置、碰撞等）  
		    UpdateObstacles();  
		}
	    
	    /// <summary>  
	    /// 在下一个模拟帧中重建障碍物树。  
	    /// 添加和移除障碍物的函数会自动调用此函数。  
	    /// </summary>  
	    public void UpdateObstacles () {  
		    // 在下一帧更新障碍物  
		    doUpdateObstacles = true;  
	    }
	    
	    private void ScheduleCleanObstacles () {
		    doCleanObstacles = true;
	    }

	    
	    /// <summary>  
	    /// 每帧调用一次  
	    /// </summary>  
	    public void Update()  
	    {  
		    if (lastStep < 0) {
			    lastStep = 0;
			    deltaTime = DesiredDeltaTime;
		    }

		    if (JNTime.Time.time - lastStep >= DesiredDeltaTime)
		    {
			    deltaTime = JNTime.Time.time - lastStep;
			    lastStep = JNTime.Time.time;

			    deltaTime = Math.Max(deltaTime, 1.0f / 2000f);
			    
			    PreCalculation(); // 预计算  
			    CleanAndUpdateObstaclesIfNecessary(); // 清理并更新障碍物  
			    BuildQuadtree(); // 构建四叉树  
  
			    // 遍历所有代理，切换缓冲区  
			    for (int i = 0; i < agents.Count; i++)  
			    {  
				    agents[i].BufferSwitch();  
			    }  
  
			    // 遍历所有代理，计算邻居和速度  
			    for (int i = 0; i < agents.Count; i++)  
			    {  
				    agents[i].CalculateNeighbours();  
				    agents[i].CalculateVelocity(context);  
			    }  
  
			    // 遍历所有代理，后计算  
			    for (int i = 0; i < agents.Count; i++)  
			    {  
				    agents[i].PostCalculation();  
			    }  
		    }
		    
	    }  
  
	    /// <summary>  
	    /// 构建四叉树  
	    /// </summary>  
	    void BuildQuadtree()  
	    {  
		    Quadtree.Clear(); // 清除四叉树中的所有元素  
		    if (agents.Count > 0) // 如果代理数量大于0  
		    {  
			    Rect bounds = Rect.MinMaxRect(agents[0].position.x, agents[0].position.y, agents[0].position.x, agents[0].position.y); // 获取第一个代理的位置作为矩形边界的初始值  
			    for (int i = 1; i < agents.Count; i++) // 遍历所有代理，更新矩形边界的最小和最大值  
			    {  
				    Vector2 p = agents[i].position;  
				    bounds = Rect.MinMaxRect(Mathf.Min(bounds.xMin, p.x), Mathf.Min(bounds.yMin, p.y), Mathf.Max(bounds.xMax, p.x), Mathf.Max(bounds.yMax, p.y));  
			    }  
			    Quadtree.SetBounds(bounds); // 设置四叉树的边界  
			    for (int i = 0; i < agents.Count; i++) // 遍历所有代理，将代理插入四叉树中  
			    {  
				    Quadtree.Insert(agents[i]);  
			    }  
			    //quadtree.DebugDraw(); // 可选：绘制四叉树用于调试（如果需要）  
		    }  
		    Quadtree.CalculateSpeeds(); // 计算四叉树中所有元素的速度（用于碰撞检测等）  
	    }

    }
}