diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 86385e2..fae330c 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -31,14 +31,17 @@ namespace DotRecast.Detour /// < Add a vertex at every polygon edge crossing. protected readonly DtNavMesh m_nav; + protected readonly DtNodePool m_tinyNodePool; protected readonly DtNodePool m_nodePool; protected readonly DtNodeQueue m_openList; + protected DtQueryData m_query; /// < Sliced query state. public DtNavMeshQuery(DtNavMesh nav) { m_nav = nav; + m_tinyNodePool = new DtNodePool(); m_nodePool = new DtNodePool(); m_openList = new DtNodeQueue(); } @@ -1799,9 +1802,9 @@ namespace DotRecast.Detour return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } - DtNodePool tinyNodePool = new DtNodePool(); + m_tinyNodePool.Clear(); - DtNode startNode = tinyNodePool.GetNode(startRef); + DtNode startNode = m_tinyNodePool.GetNode(startRef); startNode.pidx = 0; startNode.cost = 0; startNode.total = 0; @@ -1906,7 +1909,7 @@ namespace DotRecast.Detour { for (int k = 0; k < nneis; ++k) { - DtNode neighbourNode = tinyNodePool.GetNode(neis[k]); + DtNode neighbourNode = m_tinyNodePool.GetNode(neis[k]); // Skip if already visited. if ((neighbourNode.flags & DtNodeFlags.DT_NODE_CLOSED) != 0) { @@ -1924,7 +1927,7 @@ namespace DotRecast.Detour } // Mark as the node as visited and push to queue. - neighbourNode.pidx = tinyNodePool.GetNodeIdx(curNode); + neighbourNode.pidx = m_tinyNodePool.GetNodeIdx(curNode); neighbourNode.flags |= DtNodeFlags.DT_NODE_CLOSED; stack.AddLast(neighbourNode); } @@ -1939,8 +1942,8 @@ namespace DotRecast.Detour DtNode node = bestNode; do { - DtNode next = tinyNodePool.GetNodeAtIdx(node.pidx); - node.pidx = tinyNodePool.GetNodeIdx(prev); + DtNode next = m_tinyNodePool.GetNodeAtIdx(node.pidx); + node.pidx = m_tinyNodePool.GetNodeIdx(prev); prev = node; node = next; } while (node != null); @@ -1950,7 +1953,7 @@ namespace DotRecast.Detour do { visited.Add(node.id); - node = tinyNodePool.GetNodeAtIdx(node.pidx); + node = m_tinyNodePool.GetNodeAtIdx(node.pidx); } while (node != null); } @@ -2856,9 +2859,9 @@ namespace DotRecast.Detour resultRef.Clear(); resultParent.Clear(); - DtNodePool tinyNodePool = new DtNodePool(); + m_tinyNodePool.Clear(); - DtNode startNode = tinyNodePool.GetNode(startRef); + DtNode startNode = m_tinyNodePool.GetNode(startRef); startNode.pidx = 0; startNode.id = startRef; startNode.flags = DtNodeFlags.DT_NODE_CLOSED; @@ -2894,7 +2897,7 @@ namespace DotRecast.Detour continue; } - DtNode neighbourNode = tinyNodePool.GetNode(neighbourRef); + DtNode neighbourNode = m_tinyNodePool.GetNode(neighbourRef); // Skip visited. if ((neighbourNode.flags & DtNodeFlags.DT_NODE_CLOSED) != 0) { @@ -2934,7 +2937,7 @@ namespace DotRecast.Detour // Mark node visited, this is done before the overlap test so that // we will not visit the poly again if the test fails. neighbourNode.flags |= DtNodeFlags.DT_NODE_CLOSED; - neighbourNode.pidx = tinyNodePool.GetNodeIdx(curNode); + neighbourNode.pidx = m_tinyNodePool.GetNodeIdx(curNode); // Check that the polygon does not collide with existing polygons. diff --git a/src/DotRecast.Detour/DtNode.cs b/src/DotRecast.Detour/DtNode.cs index c1ef5cf..e899c10 100644 --- a/src/DotRecast.Detour/DtNode.cs +++ b/src/DotRecast.Detour/DtNode.cs @@ -25,7 +25,7 @@ namespace DotRecast.Detour { public class DtNode { - public readonly int index; + public readonly int ptr; public RcVec3f pos; // Position of the node. public float cost; // Cost from previous node to current node. @@ -36,9 +36,9 @@ namespace DotRecast.Detour public long id; // Polygon ref the node corresponds to. public List shortcut; // Shortcut found by raycast. - public DtNode(int index) + public DtNode(int ptr) { - this.index = index; + this.ptr = ptr; } public static int ComparisonNodeTotal(DtNode a, DtNode b) diff --git a/src/DotRecast.Detour/DtNodePool.cs b/src/DotRecast.Detour/DtNodePool.cs index 473e960..85f8f7e 100644 --- a/src/DotRecast.Detour/DtNodePool.cs +++ b/src/DotRecast.Detour/DtNodePool.cs @@ -25,6 +25,8 @@ namespace DotRecast.Detour public class DtNodePool { private readonly Dictionary> m_map; + + private int m_nodeCount; private readonly List m_nodes; public DtNodePool() @@ -35,8 +37,8 @@ namespace DotRecast.Detour public void Clear() { - m_nodes.Clear(); m_map.Clear(); + m_nodeCount = 0; } public int FindNodes(long id, out List nodes) @@ -52,7 +54,7 @@ namespace DotRecast.Detour public DtNode FindNode(long id) { - var hasNode = m_map.TryGetValue(id, out var nodes); + m_map.TryGetValue(id, out var nodes); if (nodes != null && 0 != nodes.Count) { return nodes[0]; @@ -63,7 +65,7 @@ namespace DotRecast.Detour public DtNode GetNode(long id, int state) { - var hasNode = m_map.TryGetValue(id, out var nodes); + m_map.TryGetValue(id, out var nodes); if (nodes != null) { foreach (DtNode node in nodes) @@ -85,10 +87,22 @@ namespace DotRecast.Detour private DtNode Create(long id, int state, List nodes) { - DtNode node = new DtNode(m_nodes.Count + 1); + if (m_nodes.Count <= m_nodeCount) + { + var newNode = new DtNode(m_nodeCount); + m_nodes.Add(newNode); + } + + int i = m_nodeCount; + m_nodeCount++; + var node = m_nodes[i]; + node.pidx = 0; + node.cost = 0; + node.total = 0; node.id = id; node.state = state; - m_nodes.Add(node); + node.flags = 0; + node.shortcut = null; nodes.Add(node); return node; @@ -96,12 +110,16 @@ namespace DotRecast.Detour public int GetNodeIdx(DtNode node) { - return node != null ? node.index : 0; + return node != null + ? node.ptr + 1 + : 0; } public DtNode GetNodeAtIdx(int idx) { - return idx != 0 ? m_nodes[idx - 1] : null; + return idx != 0 + ? m_nodes[idx - 1] + : null; } public DtNode GetNode(long refs)