提交Unity 联机Pro

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/DefaultGraphPath.cs

@@ -0,0 +1,62 @@
+
+using System.Collections.Generic;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 描述图数据结构上的一条路径
+    /// </summary>
+    /// <typeparam name="TEdge">路径节点描述：可以是图数据结构中邻接边或节点</typeparam>
+    public class DefaultGraphPath<TEdge> : IGraphPath<TEdge>
+    {
+        /// <summary>
+        /// 路径节点的集合，节点可以用图的邻接边或节点描述
+        /// </summary>
+        public List<TEdge> nodes;
+
+        /// <summary>
+        /// Creates a {@code DefaultGraphPath} with no nodes.
+        /// </summary>
+        public DefaultGraphPath() : this(new List<TEdge>()) { }
+
+        /// <summary>
+        /// Creates a {@code DefaultGraphPath} with the given capacity and no nodes.
+        /// </summary>
+        /// <param name="capacity"></param>
+        public DefaultGraphPath(int capacity) : this(new List<TEdge>(capacity)) { }
+
+        /// <summary>
+        /// Creates a {@code DefaultGraphPath} with the given nodes.
+        /// </summary>
+        /// <param name="nodes"></param>
+        public DefaultGraphPath(List<TEdge> nodes)
+        {
+            this.nodes = nodes;
+        }
+
+        public void Clear()
+        {
+            nodes.Clear();
+        }
+
+        public int GetCount()
+        {
+            return nodes.Count;
+        }
+
+        public void Add(TEdge node)
+        {
+            nodes.Add(node);
+        }
+
+        public TEdge Get(int index)
+        {
+            return nodes[index];
+        }
+
+        public void Reverse()
+        {
+            nodes.Reverse();
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/DefaultGraphPath.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 2c513670f2d7d435ab65402918015090
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/EdgePoint.cs

@@ -0,0 +1,48 @@
+
+using System.Collections.Generic;
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    public class EdgePoint
+    {
+        /**
+		 * Triangle which must be crossed to reach the next path point.
+		 */
+        public NavTriangle toNode;
+
+        /**
+		 * Triangle which was crossed to reach this point.
+		 */
+        public NavTriangle fromNode;
+
+        /**
+		 * Path edges connected to this point. Can be used for spline generation at some
+		 * point perhaps...
+		 */
+        public List<TriangleEdge> connectingEdges = new List<TriangleEdge>(8);
+
+        /**
+		 * The point where the path crosses an edge.
+		 */
+        public LVector3 point;
+
+        public EdgePoint()
+        {
+            point = LVector3.Zero;
+            toNode = null;
+        }
+
+        public void Set(in LVector3 point, NavTriangle toNode)
+        {
+            this.point = point;
+            this.toNode = toNode;
+        }
+
+        public EdgePoint(in LVector3 point, NavTriangle toNode)
+        {
+            this.point = point;
+            this.toNode = toNode;
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/EdgePoint.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: a8be0d16ec4044fbc8b80bb7a236024b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/Funnel.cs

@@ -0,0 +1,75 @@
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 漏斗
+    /// </summary>
+    public class Funnel
+    {
+        /// <summary>
+        /// 漏斗点，路径的起点或拐点
+        /// </summary>
+        public LVector3 pivot = LVector3.Zero;
+        /// <summary>
+        /// 漏斗的边界面A
+        /// </summary>
+        public Plane leftPlane = new Plane(); // 左平面，高度为y轴
+        /// <summary>
+        /// 漏斗的边界面B
+        /// </summary>
+        public Plane rightPlane = new Plane();
+        /// <summary>
+        /// 漏斗的边界面A与三角形平面的交点
+        /// </summary>
+        public LVector3 leftPortal = LVector3.Zero; // 路径左顶点，
+        /// <summary>
+        /// 漏斗的边界面B与三角形平面的交点
+        /// </summary>
+        public LVector3 rightPortal = LVector3.Zero; // 路径右顶点
+
+        public void SetLeftPlane(in LVector3 pivot, in LVector3 leftEdgeVertex)
+        {
+            leftPlane.Set(pivot, pivot + LVector3.Up, leftEdgeVertex);
+            leftPortal = leftEdgeVertex;
+        }
+
+        public void SetRightPlane(in LVector3 pivot, in LVector3 rightEdgeVertex)
+        {
+            rightPlane.Set(pivot, pivot + LVector3.Up, rightEdgeVertex); // 高度
+            rightPlane.normal = -rightPlane.normal; // 平面方向取反
+            rightPlane.distFromOrigin = -rightPlane.distFromOrigin;
+            rightPortal = rightEdgeVertex;
+        }
+
+        /// <summary>
+        /// 设置漏斗的两个边面
+        /// </summary>
+        /// <param name="pivot">漏斗点</param>
+        /// <param name="edge">三角形的共享边</param>
+        public void SetPlanes(in LVector3 pivot, TriangleEdge edge)
+        {
+            SetLeftPlane(pivot, edge.vertexB);
+            SetRightPlane(pivot, edge.vertexA);
+        }
+
+        public EnumPlaneSide SideLeftPlane(in LVector3 point)
+        {
+            return leftPlane.TestPoint(point);
+        }
+
+        public EnumPlaneSide SideRightPlane(in LVector3 point)
+        {
+            return rightPlane.TestPoint(point);
+        }
+
+        public void Reset()
+        {
+            pivot = LVector3.Zero;
+            leftPlane.Reset();
+            rightPlane.Reset();
+            leftPortal = LVector3.Zero;
+            rightPortal = LVector3.Zero;
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/Funnel.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 55922bc76371f4895b885c6663d8d181
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/IGraphPath.cs

@@ -0,0 +1,44 @@
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 描述一条路径的接口
+    /// </summary>
+    /// <typeparam name="TEdge">路径节点描述：可以是图数据结构中邻接边或节点</typeparam>
+    public interface IGraphPath<TEdge>
+    {
+        /// <summary>
+        /// 返回路径节点总数
+        /// <para> Returns the number of items of this path. </para>
+        /// </summary>
+        /// <returns></returns>
+        int GetCount();
+
+        /// <summary>
+        /// 获取指定索引路径节点
+        /// <para> Returns the item of this path at the given index. </para>
+        /// </summary>
+        /// <param name="index"></param>
+        /// <returns></returns>
+        TEdge Get(int index);
+
+        /// <summary>
+        /// 添加一个路径节点到列表尾部
+        /// <para> Adds an item at the end of this path. </para>
+        /// </summary>
+        /// <param name="node"></param>
+        void Add(TEdge node);
+
+        /// <summary>
+        /// 清理所有的路径节点
+        /// <para> Clears this path. </para>
+        /// </summary>
+        void Clear();
+
+        /// <summary>
+        /// 翻转路径节点(节点A->节点B 变为 节点B->节点A)
+        /// <para> Reverses this path. </para>
+        /// </summary>
+        void Reverse();
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/IGraphPath.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 3b81b57e2d2d1466eadfe89f96171b11
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/IHeuristic.cs

@@ -0,0 +1,25 @@
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// <para> A {@code Heuristic} generates estimates of the cost to move from a given node to the goal. </para>
+    /// With a heuristic function pathfinding algorithms can choose the node that is most likely to lead to the optimal path. The
+    /// notion of "most likely" is controlled by a heuristic. If the heuristic is accurate, then the algorithm will be efficient. If
+    /// the heuristic is terrible, then it can perform even worse than other algorithms that don't use any heuristic function such as
+    /// Dijkstra.
+    /// </summary>
+    /// <typeparam name="TNode">Type of node</typeparam>
+    public interface IHeuristic<TNode>
+    {
+        /// <summary>
+        /// 启发式算法估价函数，计算从当前点到目标点的估价
+        /// <para>A*启发函数的g(n)</para>
+        /// <para>Calculates an estimated cost to reach the goal node from the given node.</para>
+        /// </summary>
+        /// <param name="node">the start node</param>
+        /// <param name="endNode">the end node</param>
+        /// <returns>the estimated cost</returns>
+        LFloat Estimate(TNode node, TNode endNode);
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/IHeuristic.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 736f27d5a74ce43f8923b8bc96b225b3
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/IPathFinder.cs

@@ -0,0 +1,30 @@
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 路径搜索器的接口定义
+    /// </summary>
+    /// <typeparam name="TNode">图数据结构的节点</typeparam>
+    public interface IPathFinder<TNode>
+    {
+        /// <summary>
+        /// 搜索从startNode到endNode的一条路径
+        /// </summary>
+        /// <param name="startNode">起始节点</param>
+        /// <param name="endNode">目标节点</param>
+        /// <param name="heuristic">启发函数</param>
+        /// <param name="outPath">输出结果路径：路径用图的邻接边描述</param>
+        /// <returns>是否成功搜索到一条路径</returns>
+        bool SearchPath(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic, IGraphPath<IConnection<TNode>> outPath);
+
+        /// <summary>
+        /// 搜索从startNode到endNode的一条路径
+        /// </summary>
+        /// <param name="startNode">图中的起始节点</param>
+        /// <param name="endNode">图中的目标节点</param>
+        /// <param name="heuristic">启发函数</param>
+        /// <param name="outPath">输出结果路径：路径用图的节点描述</param>
+        /// <returns>是否成功搜索到一条路径</returns>
+        bool SearchNodePath(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic, IGraphPath<TNode> outPath);
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/IPathFinder.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 09b9a4d779c64410b8720c88bd5168e5
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/IndexedAStarPathFinder.cs

@@ -0,0 +1,366 @@
+
+using System.Collections.Generic;
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 基于A*算法的路径搜索器
+    /// </summary>
+    /// <typeparam name="TNode">图的节点类型</typeparam>
+    public class IndexedAStarPathFinder<TNode> : IPathFinder<TNode>
+    {
+        /// <summary>
+        /// 节点搜索状态 - 未遍历
+        /// </summary>
+        public const int UNVISITED = 0;
+        /// <summary>
+        /// 节点搜索状态 - 处理OpenList中
+        /// </summary>
+        public const int OPEN = 1;
+        /// <summary>
+        /// 节点搜索状态 - 处理完成
+        /// </summary>
+        public const int CLOSED = 2;
+
+        /// <summary>
+        /// 寻路使用的数据源
+        /// <para>以TNode为节点构成的图数据结构，包含TNode节点集合，以及TNode的邻接关系集合</para>
+        /// </summary>
+        private readonly IIndexedGraph<TNode> m_Graph;
+        /// <summary>
+        /// 节点的搜索记录集合，数组长度为图的节点总数
+        /// </summary>
+        private readonly NodeRecord<TNode>[] m_NodeRecords;
+        /// <summary>
+        /// OpenList，用二叉堆数据结构描述
+        /// </summary>
+        private readonly GenericBinaryHeap<NodeRecord<TNode>> m_OpenList;
+
+        /// <summary>
+        /// 搜索ID，每次搜索自增，用于标记节点搜索记录
+        /// The unique ID for each search run. Used to mark nodes.
+        /// </summary>
+        private int m_SearchId = 0;
+
+        /// <summary>
+        /// 当前搜索的节点
+        /// </summary>
+        private NodeRecord<TNode> m_Current;
+
+        /// <summary>
+        /// 性能评估指标，与搜索过程无关，记录A*的搜索次数，以此评估性能
+        /// </summary>
+        public Metrics metrics;
+
+        /// <summary>
+        /// 基于A*算法的路径搜索器构造
+        /// </summary>
+        /// <param name="graph">路径搜索器使用的数据源</param>
+        /// <param name="calculateMetrics">是否开启性能评估，默认:false</param>
+        public IndexedAStarPathFinder(IIndexedGraph<TNode> graph, bool calculateMetrics = false)
+        {
+            m_Graph = graph;
+            m_NodeRecords = new NodeRecord<TNode>[graph.GetNodeCount()];
+            m_OpenList = new GenericBinaryHeap<NodeRecord<TNode>>();
+            if (calculateMetrics)
+            {
+                metrics = new Metrics();
+            }
+        }
+
+        public bool SearchPath(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic, IGraphPath<IConnection<TNode>> outPath)
+        {
+            // Perform AStar
+            bool found = Search(startNode, endNode, heuristic);
+            if (found)
+            {
+                // Create a path made of connections
+                GeneratePath(startNode, outPath);
+            }
+            return found;
+        }
+
+        public bool SearchNodePath(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic, IGraphPath<TNode> outPath)
+        {
+            // Perform AStar
+            bool found = Search(startNode, endNode, heuristic);
+            if (found)
+            {
+                // Create a path made of nodes
+                GenerateNodePath(startNode, outPath);
+            }
+            return found;
+        }
+
+        /// <summary>
+        /// 开始执行路径搜索
+        /// </summary>
+        /// <param name="startNode">图上的起始节点</param>
+        /// <param name="endNode">图上的目标节点</param>
+        /// <param name="heuristic">启发函数</param>
+        /// <returns>是否存在路径</returns>
+        protected bool Search(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic)
+        {
+            InitSearch(startNode, endNode, heuristic);
+            // Iterate through processing each node
+            do
+            {
+                // Retrieve the node with smallest estimated total cost from the open list
+                m_Current = m_OpenList.Pop();
+                m_Current.category = CLOSED;
+                // Terminate if we reached the goal node
+                if (m_Current.node.Equals(endNode)) { return true; }
+                // 遍历当前节点的邻接节点，选取评估值
+                VisitChildren(endNode, heuristic);
+            }
+            while (m_OpenList.size > 0);
+            // We've run out of nodes without finding the goal, so there's no solution
+            return false;
+        }
+
+        /// <summary>
+        /// 初始化路径搜索
+        /// </summary>
+        /// <param name="startNode">图上的起始节点</param>
+        /// <param name="endNode">图上的目标节点</param>
+        /// <param name="heuristic">启发函数</param>
+        protected void InitSearch(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic)
+        {
+            // 清空性能指标
+            metrics?.Reset();
+            // 搜索ID自增
+            if (++m_SearchId < 0) { m_SearchId = 1; }
+            // 初始化OpenList
+            m_OpenList.Clear();
+            // Initialize the record for the start node and add it to the open list
+            NodeRecord<TNode> startRecord = GetNodeRecord(startNode);
+            startRecord.node = startNode;
+            startRecord.connection = null;  // 起点的连接口设置为null
+            startRecord.costSoFar = 0.ToLFloat();
+            // 起始节点添加到OpenList中
+            AddToOpenList(startRecord, heuristic.Estimate(startNode, endNode));
+            m_Current = null;
+        }
+
+        /// <summary>
+        /// 遍历子节点
+        /// </summary>
+        /// <param name="endNode">路径的目标节点</param>
+        /// <param name="heuristic">启发函数</param>
+        protected void VisitChildren(TNode endNode, IHeuristic<TNode> heuristic)
+        {
+            // Get current node's outgoing connections
+            List<IConnection<TNode>> connections = m_Graph.GetConnections(m_Current.node);
+            // Loop through each connection in turn
+            for (int i = 0; i < connections.Count; i++)
+            {
+                if (metrics != null) { ++metrics.VisitedNodes; }
+
+                IConnection<TNode> connection = connections[i];
+                // Get the cost estimate for the node
+                TNode node = connection.GetToNode(); // 邻接边连接的目标节点
+                LFloat nodeCost = m_Current.costSoFar + connection.GetCost(); // 节点到起点的消耗(g(n))
+                LFloat nodeHeuristic;
+                NodeRecord<TNode> nodeRecord = GetNodeRecord(node);
+                if (nodeRecord.category == CLOSED)
+                { // The node is closed
+                    // If we didn't find a shorter route, skip 已经是消耗最小的目标点
+                    if (nodeRecord.costSoFar <= nodeCost) { continue; }
+                    // We can use the node's old cost values to calculate its heuristic
+                    // without calling the possibly expensive heuristic function
+                    nodeHeuristic = nodeRecord.GetEstimatedTotalCost() - nodeRecord.costSoFar;
+                }
+                else if (nodeRecord.category == OPEN)
+                { // The node is open
+                    // If our route is no better, then skip
+                    if (nodeRecord.costSoFar <= nodeCost) { continue; }
+                    // Remove it from the open list (it will be re-added with the new cost)
+                    m_OpenList.Remove(nodeRecord);
+                    // We can use the node's old cost values to calculate its heuristic
+                    // without calling the possibly expensive heuristic function
+                    nodeHeuristic = nodeRecord.GetEstimatedTotalCost() - nodeRecord.costSoFar;
+                }
+                else
+                { // the node is unvisited
+                    // We'll need to calculate the heuristic value using the function,
+                    // since we don't have a node record with a previously calculated value
+                    nodeHeuristic = heuristic.Estimate(node, endNode);
+                }
+                // Update node record's cost and connection
+                nodeRecord.costSoFar = nodeCost;
+                nodeRecord.connection = connection;
+                // Add it to the open list with the estimated total cost
+                AddToOpenList(nodeRecord, nodeCost + nodeHeuristic);
+            }
+        }
+
+        /// <summary>
+        /// 生成用图的邻接边描述的路径
+        /// </summary>
+        /// <param name="startNode">图起点</param>
+        /// <param name="outPath">输出的路径</param>
+        protected void GeneratePath(TNode startNode, IGraphPath<IConnection<TNode>> outPath)
+        {
+            // outPath.clear();
+            // Work back along the path, accumulating connections
+            while (!m_Current.node.Equals(startNode))
+            { // 沿着变成搜索的连接口，搜索到起点
+                outPath.Add(m_Current.connection);
+                m_Current = m_NodeRecords[m_Graph.GetIndex(m_Current.connection.GetFromNode())];
+            }
+            // Reverse the path
+            outPath.Reverse();
+        }
+
+        /// <summary>
+        /// 生成用图的节点描述的路径
+        /// </summary>
+        /// <param name="startNode">图起点</param>
+        /// <param name="outPath">输出的路径</param>
+        protected void GenerateNodePath(TNode startNode, IGraphPath<TNode> outPath)
+        {
+            // outPath.clear();
+            // Work back along the path, accumulating nodes
+            while (m_Current.connection != null)
+            { // 沿着变成搜索的连接口，搜索到空连接
+                outPath.Add(m_Current.node);
+                m_Current = m_NodeRecords[m_Graph.GetIndex(m_Current.connection.GetFromNode())];
+            }
+            // 额外添加起点
+            outPath.Add(startNode);
+            // Reverse the path
+            outPath.Reverse();
+        }
+
+        /// <summary>
+        /// 添加节点到OpenList，节点搜索记录状态设置为Open
+        /// </summary>
+        /// <param name="nodeRecord">节点关联的搜索记录</param>
+        /// <param name="estimatedTotalCost">当前节点的到路径终点的总消耗(g(n)+h(n))</param>
+        protected void AddToOpenList(NodeRecord<TNode> nodeRecord, LFloat estimatedTotalCost)
+        {
+            m_OpenList.Add(nodeRecord, estimatedTotalCost);
+            // 节点搜索状态更新为OPEN
+            nodeRecord.category = OPEN;
+            if (metrics != null)
+            {
+                // 更新性能指标
+                ++metrics.OpenListAdditions;
+                if (m_OpenList.size > metrics.OpenListPeak)
+                {
+                    metrics.OpenListPeak = m_OpenList.size;
+                }
+            }
+        }
+
+        /// <summary>
+        /// 获取指定图节点的搜索记录
+        /// </summary>
+        /// <param name="node">图节点</param>
+        /// <returns>图节点关联的搜索记录</returns>
+        protected NodeRecord<TNode> GetNodeRecord(TNode node)
+        {
+            int index = m_Graph.GetIndex(node);
+            NodeRecord<TNode> nr = m_NodeRecords[index];
+            if (nr != null)
+            {
+                // 不同批次的搜索，重置节点状态和关联的搜索ID
+                if (nr.searchId != m_SearchId)
+                {
+                    nr.category = UNVISITED;
+                    nr.searchId = m_SearchId;
+                }
+                return nr;
+            }
+            // 从未搜索过，创建新的节点搜索记录
+            nr = m_NodeRecords[index] = new NodeRecord<TNode>();
+            nr.node = node;
+            nr.searchId = m_SearchId;
+            return nr;
+        }
+
+        #region 二叉堆的节点数据结构定义
+        /// <summary>
+        /// 图节点搜索记录
+        /// </summary>
+        /// <typeparam name="TN"></typeparam>
+        public class NodeRecord<TN> : BinaryHeapNode
+        {
+            /// <summary>
+            /// 关联的图节点
+            /// <para>The reference to the node.</para>
+            /// </summary>
+            public TN node;
+
+            /// <summary>
+            /// 本次搜索走的邻接口
+            /// <para>The incoming connection to the node.</para>
+            /// </summary>
+            public IConnection<TN> connection;
+
+            /// <summary>
+            /// 当前节点到起点的实际消耗，A*启发函数的g(n)
+            /// <para>The actual cost from the start node.</para>
+            /// </summary>
+            public LFloat costSoFar;
+
+            /// <summary>
+            /// 节点的状态，UNVISITED / OPEN / OPEN 三者之一
+            /// <para>The node category: {@link #UNVISITED}, {@link #OPEN} or {@link #OPEN}.</para>
+            /// </summary>
+            public int category;
+
+            /// <summary>
+            /// 关联的搜索ID
+            /// <para>ID of the current search.</para>
+            /// </summary>
+            public int searchId;
+
+            /// <summary>
+            /// Creates a {@code NodeRecord}.
+            /// </summary>
+            public NodeRecord() : base(LFloat.Zero) { }
+
+            /// <summary>
+            /// 获取当前节点总的消耗：f(n) = g(n) + h(n)
+            /// </summary>
+            /// <returns>Returns the estimated total cost.</returns>
+            public LFloat GetEstimatedTotalCost()
+            {
+                return value;
+            }
+        }
+        #endregion
+
+        #region 寻路指标定义，与寻路过程无关
+        /// <summary>
+        /// 寻路指标，评测寻路性能使用
+        /// </summary>
+        public class Metrics
+        {
+            /// <summary>
+            /// 遍历节点的次数
+            /// </summary>
+            public int VisitedNodes;
+            /// <summary>
+            /// OpenList添加的次数
+            /// </summary>
+            public int OpenListAdditions;
+            /// <summary>
+            /// OpenList中最大的待处理节点数
+            /// </summary>
+            public int OpenListPeak;
+
+            public Metrics() { }
+
+            public void Reset()
+            {
+                VisitedNodes = 0;
+                OpenListAdditions = 0;
+                OpenListPeak = 0;
+            }
+        }
+        #endregion
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/IndexedAStarPathFinder.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 5243578b0e4974e358512ec1e0c2ae05
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/NodeBinaryHeap.cs

@@ -0,0 +1,317 @@
+
+using System;
+using System.Text;
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 二叉堆的节点基类
+    /// </summary>
+    public class BinaryHeapNode
+    {
+        /// <summary>
+        /// 节点排序比较值
+        /// </summary>
+        public LFloat value;
+
+        /// <summary>
+        /// 节点索引
+        /// </summary>
+        public int index;
+
+        public BinaryHeapNode(LFloat value)
+        {
+            this.value = value;
+        }
+        public override string ToString()
+        {
+            return value.ToString();
+        }
+    }
+
+    /// <summary>
+    /// 二叉堆数据结构
+    /// <para>常规二叉堆不从索引0开始，而是从索引1开始</para>
+    /// <para>索引为n的左孩子索引是2n，索引为n的右孩子索引是2n+1，索引为n的父节点索引是n/2</para>
+    /// <para>本实现从索引0开始</para>
+    /// <para>本实现索引为n的左孩子索引是2n+1，索引为n的右孩子索引是2(n+1)，索引为n的父节点索引是(n-1)/2</para> 
+    /// </summary>
+    /// <typeparam name="T">二叉堆中的数据节点</typeparam>
+    public class GenericBinaryHeap<T> where T : BinaryHeapNode
+    {
+        /// <summary>
+        /// 二叉堆中的有效节点总数，同时是当前的可插入位置
+        /// </summary>
+        public int size;
+        /// <summary>
+        /// 二叉堆节点集合，用数组存储
+        /// </summary>
+        private BinaryHeapNode[] m_Nodes;
+        /// <summary>
+        /// 是否是最大堆
+        /// <para>最大堆:任意节点权值不小于其子节点</para>
+        /// <para>最小堆:任意节点权值不大于于其子节点</para>
+        /// </summary>
+        private readonly bool m_IsMaxHeap;
+
+        /// <summary>
+        /// 二叉堆构造器
+        /// </summary>
+        public GenericBinaryHeap() : this(16, false) { }
+
+        /// <summary>
+        /// 二叉堆构造器
+        /// </summary>
+        /// <param name="capacity">初始容量</param>
+        /// <param name="isMaxHeap">是否是最大堆</param>
+        public GenericBinaryHeap(int capacity, bool isMaxHeap)
+        {
+            m_IsMaxHeap = isMaxHeap;
+            m_Nodes = new BinaryHeapNode[capacity];
+        }
+
+        /// <summary>
+        /// 添加节点到二叉堆
+        /// </summary>
+        /// <param name="node"></param>
+        /// <returns></returns>
+        public T Add(T node)
+        {
+            // Expand if necessary.
+            if (size == m_Nodes.Length)
+            {
+                BinaryHeapNode[] newNodes = new BinaryHeapNode[size << 1];
+                Array.Copy(m_Nodes, 0, newNodes, 0, size);
+                m_Nodes = newNodes;
+            }
+            // Insert at end.
+            node.index = size;
+            m_Nodes[size] = node;
+            // bubble up
+            Up(size++);
+            return node;
+        }
+
+        /// <summary>
+        /// 添加节点到二叉堆，并设置节点的排序权重为value
+        /// </summary>
+        /// <param name="node">添加到二叉堆的节点</param>
+        /// <param name="value">节点的排序权重</param>
+        /// <returns></returns>
+        public T Add(T node, LFloat value)
+        {
+            node.value = value;
+            return Add(node);
+        }
+
+        /// <summary>
+        /// 获取二叉堆顶的节点
+        /// </summary>
+        /// <returns></returns>
+        /// <exception cref="Exception"></exception>
+        public T Peek()
+        {
+            if (size == 0)
+                throw new Exception("The heap is empty.");
+            return (T)m_Nodes[0];
+        }
+
+        /// <summary>
+        /// 弹出二叉堆顶的节点
+        /// </summary>
+        /// <returns></returns>
+        public T Pop()
+        {
+            return Remove(0);
+        }
+
+        /// <summary>
+        /// 弹出指定的二叉堆中节点
+        /// </summary>
+        /// <param name="node">移除的节点</param>
+        /// <returns></returns>
+        public T Remove(T node)
+        {
+            return Remove(node.index);
+        }
+
+        /// <summary>
+        /// 移除二叉堆指定索引位置的节点
+        /// </summary>
+        /// <param name="index">移除位置索引</param>
+        /// <returns></returns>
+        private T Remove(int index)
+        {
+            BinaryHeapNode[] nodes = m_Nodes;
+            BinaryHeapNode removed = nodes[index];
+            // 用最后一个位置的节点填充移除的位置
+            nodes[index] = nodes[--size];
+            nodes[size] = null;
+            if (size > 0 && index < size)
+            {
+                Down(index);
+            }
+            return (T)removed;
+        }
+
+        /// <summary>
+        /// 清空二叉堆
+        /// TODO 优化，可以不用对nodes进行清零
+        /// </summary>
+        public void Clear()
+        {
+            for (int i = 0, n = size; i < n; ++i)
+            {
+                m_Nodes[i] = null;
+            }
+            size = 0;
+        }
+
+        /// <summary>
+        /// 修改二叉堆中一个节点的排序权重
+        /// </summary>
+        /// <param name="node">二叉堆中的一个节点</param>
+        /// <param name="value">新的排序值</param>
+        public void SetValue(T node, LFloat value)
+        {
+            LFloat oldValue = node.value;
+            node.value = value;
+            if ((value < oldValue) ^ m_IsMaxHeap)
+            {
+                Up(node.index);
+            }
+            else
+            {
+                Down(node.index);
+            }
+        }
+
+        /// <summary>
+        /// 上浮索引位置的节点，直至满足二叉堆性质
+        /// </summary>
+        /// <param name="index">上浮的节点索引</param>
+        private void Up(int index)
+        {
+            BinaryHeapNode[] nodes = this.m_Nodes;
+            BinaryHeapNode node = nodes[index];
+            LFloat value = node.value;
+            while (index > 0)
+            {
+                // 获取父节点
+                int parentIndex = (index - 1) >> 1;
+                BinaryHeapNode parent = nodes[parentIndex];
+                // 当前节点的权值与父节点比较，是否满足二叉堆性质(最大堆或最小堆)
+                if ((value < parent.value) ^ m_IsMaxHeap)
+                { // 不满足，则将父节点的值写入到当前节点的位置，当前节点索引该为父节点索引
+                    nodes[index] = parent;
+                    parent.index = index;
+                    index = parentIndex;
+                }
+                else
+                {
+                    // 满足，退出上浮
+                    break;
+                }
+            }
+            // 当前节点写入正确位置，更新节点的索引
+            nodes[index] = node;
+            node.index = index;
+        }
+
+        /// <summary>
+        /// 节点下沉，寻找符合二叉堆性质的
+        /// </summary>
+        /// <param name="index"></param>
+        private void Down(int index)
+        {
+            BinaryHeapNode[] nodes = m_Nodes;
+            int size = this.size;
+            BinaryHeapNode node = nodes[index];
+            LFloat value = node.value;
+            // 当前节点循环下沉，寻找符合二叉堆性质的索引位置
+            while (true)
+            {
+                // no child, finished
+                int leftIndex = 1 + (index << 1);
+                if (leftIndex >= size)
+                {
+                    break;
+                }
+                // Always have a left child.
+                BinaryHeapNode leftNode = nodes[leftIndex];
+                LFloat leftValue = leftNode.value;
+                // May have a right child.
+                int rightIndex = leftIndex + 1;
+                BinaryHeapNode rightNode;
+                LFloat rightValue;
+                if (rightIndex >= size)
+                {
+                    rightNode = null;
+                    rightValue = m_IsMaxHeap ? LFloat.FLT_MIN : LFloat.FLT_MAX;
+                }
+                else
+                {
+                    rightNode = nodes[rightIndex];
+                    rightValue = rightNode.value;
+                }
+                // The smallest of the three values is the parent.
+                if ((leftValue < rightValue) ^ m_IsMaxHeap)
+                { // 最大堆，左节点为两个子节点的大值 || 最小堆，左节点为两个子节点的小值
+                    if (leftValue == value || ((leftValue > value) ^ m_IsMaxHeap))
+                    { // 左节点与当前节点(父节点)权值相同，或 最大堆:左节点为权值小的 | 最小堆:左节点为权值大的。退出，下沉完成
+                        break;
+                    }
+                    // 最小堆:左节点为权值最小的 || 最大堆:左节点为权值最大的，交换当前(父)节点与左节点的位置
+                    nodes[index] = leftNode;
+                    leftNode.index = index;
+                    index = leftIndex;
+                }
+                else
+                { // 最大堆，右节点为两个子节点的大值 || 最小堆，右节点为两个子节点的小值
+                    if (rightValue == value || ((rightValue > value) ^ m_IsMaxHeap))
+                    { // 右节点与当前节点(父节点)权值相同，或 最大堆右节点为权值小的 | 最小堆:右节点为权值大的。退出，下沉完成
+                        break;
+                    }
+                    // 最小堆:右节点为权值最小的 || 最大堆:右节点为权值最大的，交换当前(父)节点与右节点的位置
+                    nodes[index] = rightNode;
+                    rightNode.index = index;
+                    index = rightIndex;
+                }
+            }
+            // 更新当前节点到最终的索引位置
+            nodes[index] = node;
+            node.index = index;
+        }
+
+        public override int GetHashCode()
+        {
+            long h = 1;
+            for (int i = 0, n = size; i < n; i++)
+            {
+                h = h * 31 + m_Nodes[i].value.rawValue;
+            }
+            return (int)h;
+        }
+
+        public override string ToString()
+        {
+            if (size == 0)
+            {
+                return "[]";
+            }
+            BinaryHeapNode[] nodes = this.m_Nodes;
+            StringBuilder buffer = new StringBuilder(32);
+            buffer.Append('[');
+            buffer.Append(nodes[0].value);
+            for (int i = 1; i < size; i++)
+            {
+                buffer.Append(", ");
+                buffer.Append(nodes[i].value);
+            }
+
+            buffer.Append(']');
+            return buffer.ToString();
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/NodeBinaryHeap.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 024aed145f1c9421faa2dcf00949e00d
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/TriangleEdgeGraphPath.cs

@@ -0,0 +1,38 @@
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 描述以三角形为图元节点的图上，从起点三角形到终点三角形的一条图上的路径。
+    /// <para>路径描节点用图的邻接边描述</para>
+    /// </summary>
+    public class TriangleEdgeGraphPath : DefaultGraphPath<IConnection<NavTriangle>>
+    {
+        /// <summary>
+        /// 路径的起点
+        /// <para>The start point when generating a point path for this triangle path</para>
+        /// </summary>
+        public LVector3 startPoint;
+
+        /// <summary>
+        /// 路径的终点
+        /// <para>The end point when generating a point path for this triangle path</para>
+        /// </summary>
+        public LVector3 endPoint;
+
+        /// <summary>
+        /// 路径的起始三角形
+        /// </summary>
+        public NavTriangle startTriangle;
+
+        /// <summary>
+        /// 获取路径上的最后一个三角形
+        /// </summary>
+        /// <returns>Last triangle in the path.</returns>
+        public NavTriangle GetEndTriangle()
+        {
+            // If the triangle path is empty, the point path will span this triangle
+            return (GetCount() > 0) ? Get(GetCount() - 1).GetToNode() : startTriangle;
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/TriangleEdgeGraphPath.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 274f209b145dc49a0b89a286ea968a06
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/TriangleHeuristic.cs

@@ -0,0 +1,97 @@
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 三角形节点寻路启发式算法
+    /// </summary>
+    public class TriangleHeuristic : IHeuristic<NavTriangle>
+    {
+        /// <summary>
+        /// 三角形A的AB边中点
+        /// </summary>
+        private LVector3 m_MidPointAB_A = LVector3.Zero;
+        /// <summary>
+        /// 三角形A的BC边中点
+        /// </summary>
+        private LVector3 m_MidPointBC_A = LVector3.Zero;
+        /// <summary>
+        /// 三角形A的CA边中点
+        /// </summary>
+        private LVector3 m_MidPointCA_A = LVector3.Zero;
+        /// <summary>
+        /// 三角形B的AB边中点
+        /// </summary>
+        private LVector3 m_MidPointAB_B = LVector3.Zero;
+        /// <summary>
+        /// 三角形B的BC边中点
+        /// </summary>
+        private LVector3 m_MidPointBC_B = LVector3.Zero;
+        /// <summary>
+        /// 三角形B的CA边中点
+        /// </summary>
+        private LVector3 m_MidPointCA_B = LVector3.Zero;
+
+        /// <summary>
+        /// 启发式算法估价函数：两个三角形节点三边中点的距离，获取最短的距离
+        /// <para>A*启发函数的g(n)</para>
+        /// </summary>
+        /// <param name="node">当前节点</param>
+        /// <param name="endNode">目标节点</param>
+        /// <returns></returns>
+        public LFloat Estimate(NavTriangle node, NavTriangle endNode)
+        {
+            LFloat minDst2 = LFloat.FLT_MAX;
+            // 三角形A的三边中点
+            m_MidPointAB_A = (node.vertexA + node.vertexB) * LFloat.Half;
+            m_MidPointBC_A = (node.vertexB + node.vertexC) * LFloat.Half;
+            m_MidPointCA_A = (node.vertexC + node.vertexA) * LFloat.Half;
+            // 三角形B的三边中点
+            m_MidPointAB_B = (endNode.vertexA + endNode.vertexB) * LFloat.Half;
+            m_MidPointBC_B = (endNode.vertexB + endNode.vertexC) * LFloat.Half;
+            m_MidPointCA_B = (endNode.vertexC + endNode.vertexA) * LFloat.Half;
+            LFloat dst2;
+            // 三角形A的边AB中点到三角形B的三边中点的距离平方
+            if ((dst2 = LVector3.Distance2(m_MidPointAB_A, m_MidPointAB_B)) < minDst2)
+            {
+                minDst2 = dst2;
+            }
+            if ((dst2 = LVector3.Distance2(m_MidPointAB_A, m_MidPointBC_B)) < minDst2)
+            {
+                minDst2 = dst2;
+            }
+            if ((dst2 = LVector3.Distance2(m_MidPointAB_A, m_MidPointCA_B)) < minDst2)
+            {
+                minDst2 = dst2;
+            }
+            // 三角形A的边BC中点到三角形B的三边中点的距离平方
+            if ((dst2 = LVector3.Distance2(m_MidPointBC_A, m_MidPointAB_B)) < minDst2)
+            {
+                minDst2 = dst2;
+            }
+            if ((dst2 = LVector3.Distance2(m_MidPointBC_A, m_MidPointBC_B)) < minDst2)
+            {
+                minDst2 = dst2;
+            }
+            if ((dst2 = LVector3.Distance2(m_MidPointBC_A, m_MidPointCA_B)) < minDst2)
+            {
+                minDst2 = dst2;
+            }
+            // 三角形A的边CA中点到三角形B的三边中点的距离平方
+            if ((dst2 = LVector3.Distance2(m_MidPointCA_A, m_MidPointAB_B)) < minDst2)
+            {
+                minDst2 = dst2;
+            }
+            if ((dst2 = LVector3.Distance2(m_MidPointCA_A, m_MidPointBC_B)) < minDst2)
+            {
+                minDst2 = dst2;
+            }
+            if ((dst2 = LVector3.Distance2(m_MidPointCA_A, m_MidPointCA_B)) < minDst2)
+            {
+                minDst2 = dst2;
+            }
+            // 返回最短距离
+            return LMath.Sqrt(minDst2);
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/TriangleHeuristic.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 29ee51a11a00042b6bcd827f0d2d634f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/TrianglePointPath.cs

@@ -0,0 +1,476 @@
+
+using System.Collections.Generic;
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 位置平面关系枚举
+    /// </summary>
+    public enum EnumPlaneSide
+    {
+        /// <summary>
+        /// 点在平面上
+        /// </summary>
+        OnPlane,
+        /// <summary>
+        /// 点在平面背面
+        /// </summary>
+        Back,
+        /// <summary>
+        /// 点在平面正面
+        /// </summary>
+        Front
+    }
+
+    /// <summary>
+    /// 将图节点路径转换为一条最短路径
+    /// <para>漏斗算法</para>
+    /// </summary>
+    public class TrianglePointPath
+    {
+        private readonly Plane m_CrossingPlane = new Plane(); // 横跨平面
+        /// <summary>
+        /// 用图边描述的路径
+        /// </summary>
+        private List<IConnection<NavTriangle>> m_EdgePaths; // 路径连接点
+        /// <summary>
+        /// 路径起点
+        /// </summary>
+        private LVector3 m_Start; // 起点
+        /// <summary>
+        /// 路径终点
+        /// </summary>
+        private LVector3 m_End; // 终点
+        /// <summary>
+        /// 起始三角形
+        /// </summary>
+        private NavTriangle m_StartTri; // 起始三角形
+        /// <summary>
+        /// 上次添加的边点
+        /// </summary>
+        private EdgePoint m_LastPointAdded;
+        /// <summary>
+        /// 最终结果路径坐标点
+        /// </summary>
+        private readonly List<LVector3> m_Vectors = new List<LVector3>();
+        /// <summary>
+        /// 最终结果边点描述
+        /// </summary>
+        private readonly List<EdgePoint> m_PathPoints = new List<EdgePoint>();
+
+        /// <summary>
+        /// 路径计算的中间射线对象，每次计算清理并复用
+        /// </summary>
+        private readonly Ray m_TempRay = new Ray();
+        /// <summary>
+        /// 路径计算的中间漏斗对象，每次计算清理并复用
+        /// </summary>
+        private readonly Funnel m_TempFunnel = new Funnel();
+
+        /// <summary>
+        /// 路径计算的最后一个边，每次计算清理并复用
+        /// </summary>
+        private TriangleEdge m_TempLastEdge = new TriangleEdge();
+
+        /// <summary>
+        /// EdgePoint临时对象队列的初始数量
+        /// </summary>
+        private const int kDefaultEdgePointCount = 16;
+        /// <summary>
+        /// EdgePoint的临时对象缓存队列，作为对象池复用
+        /// </summary>
+        private readonly Queue<EdgePoint> m_TempEdgePointQueue = new Queue<EdgePoint>(kDefaultEdgePointCount);
+
+        public TrianglePointPath()
+        {
+            // 初始化一些EdgePoint的临时对象
+            for (int i = 0; i < kDefaultEdgePointCount; ++i)
+            {
+                m_TempEdgePointQueue.Enqueue(new EdgePoint());
+            }
+        }
+
+        public void CalculateForGraphPath(TriangleEdgeGraphPath trianglePath)
+        {
+            Clear();
+            m_EdgePaths = trianglePath.nodes;
+            m_Start = trianglePath.startPoint;
+            m_End = trianglePath.endPoint;
+            m_StartTri = trianglePath.startTriangle;
+            for (int i = 0; i < m_EdgePaths.Count; ++i)
+            {
+                IConnection<NavTriangle> connection = m_EdgePaths[i];
+                NavTriangle to = connection.GetToNode();
+                m_Vectors.Add(to.center);
+            }
+            m_Vectors.Add(m_End);
+        }
+
+        /// <summary>
+        /// 通过图元中间路径转化为点路径
+        /// </summary>
+        /// <param name="trianglePath"></param>
+        /// <param name="calculateCrossPoint"></param>
+        public void CalculateForGraphPath(TriangleEdgeGraphPath trianglePath, bool calculateCrossPoint)
+        {
+            Clear();
+            m_EdgePaths = trianglePath.nodes;
+            m_Start = trianglePath.startPoint;
+            m_End = trianglePath.endPoint;
+            m_StartTri = trianglePath.startTriangle;
+            // Check that the start point is actually inside the start triangle, 
+            // if not, project it to the closest triangle edge. 
+            // Otherwise the funnel calculation might generate spurious path segments.
+            LVector3 rayOrigin = m_Start + LVector3.Up * 1000;
+            // Ray ray = new Ray(rayOrigin, LVector3.Down); 
+            // 起始坐标从上向下的射线
+            m_TempRay.origin = rayOrigin;
+            m_TempRay.direction = LVector3.Down;
+            if (!GeometryUtil.IntersectRayTriangle(m_TempRay, m_StartTri.vertexA, m_StartTri.vertexB, m_StartTri.vertexC, out var ss))
+            {
+                LFloat minDst = LFloat.FLT_MAX;
+                LVector3 projection = new LVector3(); // 规划坐标
+                LVector3 newStart = new LVector3(); // 新坐标
+                LFloat dst;
+                // A-B
+                if ((dst = GeometryUtil.NearestSegmentPointSquareDistance(ref projection, m_StartTri.vertexA, m_StartTri.vertexB,
+                        m_Start)) < minDst)
+                {
+                    minDst = dst;
+                    newStart = projection;
+                }
+
+                // B-C
+                if ((dst = GeometryUtil.NearestSegmentPointSquareDistance(ref projection, m_StartTri.vertexB, m_StartTri.vertexC,
+                        m_Start)) < minDst)
+                {
+                    minDst = dst;
+                    newStart = projection;
+                }
+
+                // C-A
+                if ((dst = GeometryUtil.NearestSegmentPointSquareDistance(ref projection, m_StartTri.vertexC, m_StartTri.vertexA,
+                        m_Start)) < minDst)
+                {
+                    minDst = dst;
+                    newStart = projection;
+                }
+
+                m_Start = newStart;
+            }
+
+            if (m_EdgePaths.Count == 0)
+            { // 起点终点在同一三角形中
+                AddPoint(m_Start, m_StartTri);
+                AddPoint(m_End, m_StartTri);
+            }
+            else
+            {
+                m_TempLastEdge.Set(m_EdgePaths[^1].GetToNode(), m_EdgePaths[^1].GetToNode(), m_End, m_End);
+                CalculateEdgePoints(calculateCrossPoint);
+            }
+        }
+
+        public void Clear()
+        {
+            m_TempFunnel.Reset();
+            m_TempRay.Reset();
+            m_Vectors.Clear();
+            // Debug.Log("RemainCount: {0}", m_TempEdgePointQueue.Count);
+            for (int i = m_PathPoints.Count - 1; i >= 0; --i)
+            {
+                RecycleEdgePoint(m_PathPoints[i]);
+            }
+            // Debug.Log("TotalCount: {0}", m_TempEdgePointQueue.Count);
+            m_PathPoints.Clear();
+            m_StartTri = null;
+            m_LastPointAdded = null;
+            m_TempLastEdge.Clear();
+        }
+
+        private TriangleEdge GetEdge(int index)
+        {
+            return (TriangleEdge)((index == m_EdgePaths.Count) ? m_TempLastEdge : m_EdgePaths[index]);
+        }
+
+        private int NumEdges()
+        {
+            return m_EdgePaths.Count + 1;
+        }
+
+        public LVector3 GetVector(int index)
+        {
+            return m_Vectors[index];
+        }
+
+        public int GetSize()
+        {
+            return m_Vectors.Count;
+        }
+
+        /** All vectors in the path     */
+        public List<LVector3> GetVectors()
+        {
+            return m_Vectors;
+        }
+
+        /** The triangle which must be crossed to reach the next path point.*/
+        public NavTriangle GetToTriangle(int index)
+        {
+            return m_PathPoints[index].toNode;
+        }
+
+        /** The triangle from which must be crossed to reach this point. */
+        public NavTriangle GetFromTriangle(int index)
+        {
+            return m_PathPoints[index].fromNode;
+        }
+
+        /** The navmesh edge(s) crossed at this path point.     */
+        public List<TriangleEdge> GetCrossedEdges(int index)
+        {
+            return m_PathPoints[index].connectingEdges;
+        }
+
+        private void AddPoint(in LVector3 point, NavTriangle toNode)
+        {
+            // AddPoint(new EdgePoint(point, toNode));
+            AddPoint(GetFreeEdgePoint(in point, toNode));
+        }
+
+        private void AddPoint(EdgePoint edgePoint)
+        {
+            m_Vectors.Add(edgePoint.point);
+            m_PathPoints.Add(edgePoint);
+            m_LastPointAdded = edgePoint;
+        }
+
+        /// <summary>
+        /// 基于漏斗算法，将图的边路径转换为最短点路径
+        /// <para>point path through the path triangles, using the Simple Stupid Funnel Algorithm.</para>
+        /// </summary>
+        /// <param name="calculateCrossPoint"></param>
+        private void CalculateEdgePoints(bool calculateCrossPoint)
+        {
+            TriangleEdge edge = GetEdge(0);
+            AddPoint(m_Start, edge.fromNode);
+            m_LastPointAdded.fromNode = edge.fromNode;
+            // 初始化第一个漏斗
+            m_TempFunnel.pivot = m_Start;
+            m_TempFunnel.SetPlanes(m_TempFunnel.pivot, edge);// 路径起点为漏斗点，设置共享边的两个端点为漏斗边界平面
+            int leftIndex = 0; // 左顶点索引
+            int rightIndex = 0; // 右顶点索引
+            int lastRestart = 0;
+            for (int i = 1; i < NumEdges(); ++i)
+            {
+                edge = GetEdge(i); // 下一条边
+                // 边的两个端点与漏斗边平面A的位置关系
+                var leftPlaneLeftDP = m_TempFunnel.SideLeftPlane(edge.vertexB);
+                var leftPlaneRightDP = m_TempFunnel.SideLeftPlane(edge.vertexA);
+                // 边的两个端点与漏斗边平面B的位置关系
+                var rightPlaneLeftDP = m_TempFunnel.SideRightPlane(edge.vertexB);
+                var rightPlaneRightDP = m_TempFunnel.SideRightPlane(edge.vertexA);
+
+                // 右顶点在右平面里面
+                if (rightPlaneRightDP != EnumPlaneSide.Front)
+                {
+                    // 右顶点在左平面里面
+                    if (leftPlaneRightDP != EnumPlaneSide.Front)
+                    {
+                        // Tighten the funnel. 缩小漏斗
+                        m_TempFunnel.SetRightPlane(m_TempFunnel.pivot, edge.vertexA);
+                        rightIndex = i;
+                    }
+                    else
+                    {
+                        // Right over left, insert left to path and restart scan from portal left point.
+                        // 右顶点在左平面外面，设置左顶点为漏斗顶点和路径点，从新已该漏斗开始扫描
+                        if (calculateCrossPoint)
+                        {
+                            CalculateEdgeCrossings(lastRestart, leftIndex, m_TempFunnel.pivot, m_TempFunnel.leftPortal);
+                        }
+                        else
+                        {
+                            m_Vectors.Add(m_TempFunnel.leftPortal);
+                        }
+
+                        m_TempFunnel.pivot = m_TempFunnel.leftPortal;
+                        i = leftIndex;
+                        rightIndex = i;
+                        if (i < NumEdges() - 1)
+                        {
+                            lastRestart = i;
+                            m_TempFunnel.SetPlanes(m_TempFunnel.pivot, GetEdge(i + 1));
+                            continue;
+                        }
+
+                        break;
+                    }
+                }
+
+                // 左顶点在左平面里面
+                if (leftPlaneLeftDP != EnumPlaneSide.Front)
+                {
+                    // 左顶点在右平面里面
+                    if (rightPlaneLeftDP != EnumPlaneSide.Front)
+                    {
+                        // Tighten the funnel.
+                        m_TempFunnel.SetLeftPlane(m_TempFunnel.pivot, edge.vertexB);
+                        leftIndex = i;
+                    }
+                    else
+                    {
+                        // Left over right, insert right to path and restart scan from portal right point.
+                        if (calculateCrossPoint)
+                        {
+                            CalculateEdgeCrossings(lastRestart, rightIndex, m_TempFunnel.pivot, m_TempFunnel.rightPortal);
+                        }
+                        else
+                        {
+                            m_Vectors.Add(m_TempFunnel.rightPortal);
+                        }
+
+                        m_TempFunnel.pivot = m_TempFunnel.rightPortal;
+                        i = rightIndex;
+                        leftIndex = i;
+                        if (i < NumEdges() - 1)
+                        {
+                            lastRestart = i;
+                            m_TempFunnel.SetPlanes(m_TempFunnel.pivot, GetEdge(i + 1));
+                            continue;
+                        }
+
+                        break;
+                    }
+                }
+            }
+
+            if (calculateCrossPoint)
+            {
+                CalculateEdgeCrossings(lastRestart, NumEdges() - 1, m_TempFunnel.pivot, m_End);
+            }
+            else
+            {
+                m_Vectors.Add(m_End);
+            }
+
+            for (int i = 1; i < m_PathPoints.Count; i++)
+            {
+                EdgePoint p = m_PathPoints[i];
+                p.fromNode = m_PathPoints[i - 1].toNode;
+            }
+
+            return;
+        }
+
+        /**
+         * Store all edge crossing points between the start and end indices. If the path
+         * crosses exactly the start or end points (which is quite likely), store the
+         * edges in order of crossing in the EdgePoint data structure.
+         * <p/>
+         * Edge crossings are calculated as intersections with the plane from the start,
+         * end and up vectors.
+         */
+        private void CalculateEdgeCrossings(int startIndex, int endIndex, LVector3 startPoint, LVector3 endPoint)
+        {
+            if (startIndex >= NumEdges() || endIndex >= NumEdges())
+            {
+                return;
+            }
+
+            m_CrossingPlane.Set(startPoint, startPoint + LVector3.Up, endPoint);
+
+            EdgePoint previousLast = m_LastPointAdded;
+
+            var edge = GetEdge(endIndex);
+            // EdgePoint end = new EdgePoint(endPoint, edge.toNode);
+            EdgePoint end = GetFreeEdgePoint(in endPoint, edge.toNode);
+            for (int i = startIndex; i < endIndex; i++)
+            {
+                edge = GetEdge(i);
+
+                if (edge.vertexA.Equals(startPoint) || edge.vertexB.Equals(startPoint))
+                {
+                    previousLast.toNode = edge.toNode;
+                    if (!previousLast.connectingEdges.Contains(edge))
+                    {
+                        previousLast.connectingEdges.Add(edge);
+                    }
+                }
+                else if (edge.vertexB.Equals(endPoint) || edge.vertexA.Equals(endPoint))
+                {
+                    if (!end.connectingEdges.Contains(edge))
+                    {
+                        end.connectingEdges.Add(edge);
+                    }
+                }
+                else if (IntersectSegmentPlane(edge.vertexB, edge.vertexA, m_CrossingPlane, out LVector3 intersection))
+                {
+                    if (i != startIndex || i == 0)
+                    {
+                        m_LastPointAdded.toNode = edge.fromNode;
+                        // EdgePoint crossing = new EdgePoint(intersection, edge.toNode);
+                        EdgePoint crossing = GetFreeEdgePoint(in intersection, edge.toNode);
+                        crossing.connectingEdges.Add(edge);
+                        AddPoint(crossing);
+                    }
+                }
+            }
+
+            if (endIndex < NumEdges() - 1)
+            {
+                end.connectingEdges.Add(GetEdge(endIndex));
+            }
+
+            if (!m_LastPointAdded.Equals(end))
+            {
+                AddPoint(end);
+            }
+        }
+
+        public static bool IntersectSegmentPlane(LVector3 start, LVector3 end, Plane plane, out LVector3 intersection)
+        {
+            intersection = LVector3.Zero;
+            LVector3 dir = end - start;
+            LFloat denom = LVector3.Dot(dir, plane.normal);
+            LFloat t = -(LVector3.Dot(start, plane.normal) + plane.distFromOrigin) / denom;
+            if (t < 0 || t > 1)
+            {
+                return false;
+            }
+            intersection = start + dir * t;
+            return true;
+        }
+
+        /// <summary>
+        /// 从临时队列中获取缓存的EdgePoint对象，并且初始化
+        /// </summary>
+        /// <param name="point"></param>
+        /// <param name="toNode"></param>
+        /// <returns></returns>
+        private EdgePoint GetFreeEdgePoint(in LVector3 point, NavTriangle toNode)
+        {
+            if (m_TempEdgePointQueue.Count == 0)
+            {
+                return new EdgePoint(in point, toNode);
+            }
+            EdgePoint edgePoint = m_TempEdgePointQueue.Dequeue();
+            edgePoint.Set(in point, toNode);
+            return edgePoint;
+        }
+
+        /// <summary>
+        /// 回收edgePoint临时对象，以备复用
+        /// </summary>
+        /// <param name="edgePoint"></param>
+        private void RecycleEdgePoint(EdgePoint edgePoint)
+        {
+            if (edgePoint == null)
+            {
+                return;
+            }
+            m_TempEdgePointQueue.Enqueue(edgePoint);
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/PathFinder/TrianglePointPath.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 8f226701410f4424d8ac316660c1ccb0
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: