diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs index 3e5ea687..0c1bf938 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Linq; using DotRecast.Core; using DotRecast.Core.Numerics; using DotRecast.Detour; @@ -341,15 +342,17 @@ public void HandleRender(NavMeshRenderer renderer) // dd.DebugDrawArc(g1.X, g1.Y, g1.Z, g2.X, g2.Y, g2.Z, 0.25f, 0.0f, 0.4f, DuRGBA(0, 0, 0, 128), 5.0f); // dd.DepthMask(true); // } + + - foreach (var path in m_guidePaths) + foreach (var edge in m_guidePaths.SelectMany(x => x.Edges).Distinct()) { dd.DepthMask(false); int spathCol = DuRGBA(0, 0, 0, 220); dd.Begin(LINES, 3.0f); - for (int i = 0; i < path.edgePaths.Count; ++i) + for (int i = 0; i < edge.EdgePaths.Count; ++i) { - dd.Vertex(path.edgePaths[i].X, path.edgePaths[i].Y + 0.1f, path.edgePaths[i].Z, spathCol); + dd.Vertex(edge.EdgePaths[i].X, edge.EdgePaths[i].Y + 0.1f, edge.EdgePaths[i].Z, spathCol); } dd.End(); @@ -793,7 +796,7 @@ private void Recalc(bool shift, bool ctrl) { if (shift) { - _tool.RemoveGuidePath(m_spos, ref m_guidePaths); + _tool.RemoveGuideNode(navQuery, m_polyPickExt, m_filter, m_spos, ref m_guidePaths); } else { diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs index b2d5983f..87f7a89f 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs @@ -1,18 +1,79 @@ using System; using System.Collections.Generic; +using System.Linq; using DotRecast.Core; using DotRecast.Core.Numerics; using DotRecast.Detour; namespace DotRecast.Recast.Toolset.Tools { + public class NodeEdge + { + public readonly GuideNode NodeA; + public readonly GuideNode NodeB; + public List EdgePaths; + + public NodeEdge(GuideNode a, GuideNode b) + { + NodeA = a; + NodeB = b; + EdgePaths = new List(); + } + + public void Fllow(Func> findFollowPath) + { + EdgePaths = findFollowPath.Invoke(NodeA, NodeB); + } + } + public class GuideNode { + public readonly long NodeId; public long PolyRef; public RcVec3f Position; public float Radius; - public List edgePaths = new List(); + public readonly List Edges; + + public GuideNode(long id) + { + NodeId = id; + Edges = new List(); + } + + public void Add(NodeEdge edge) + { + Edges.Add(edge); + } + + public void Remove(NodeEdge edge) + { + Edges.Remove(edge); + } + + public NodeEdge Intersect(RcVec3f p, float radius) + { + // 제일 가까운 엣지 찾음 + float minDistance = float.MaxValue; + NodeEdge foundEdge = null; + foreach (var edge in Edges) + { + foreach (var edgePoint in edge.EdgePaths) + { + var distance = RcVec3f.Distance(p, edgePoint); + if (distance < minDistance) + { + foundEdge = edge; + minDistance = distance; + } + } + } + + if (minDistance > radius) + return null; + + return foundEdge; + } } public class DistanceToWallResult @@ -27,6 +88,7 @@ public class DistanceToWallResult public class RcTestNavMeshTool : IRcToolable { + private RcAtomicLong _nextNodeId = new RcAtomicLong(1); public const int MAX_POLYS = 256; public const int MAX_SMOOTH = 2048; @@ -39,18 +101,59 @@ public string GetName() return "Test Navmesh"; } - public bool RemoveGuidePath(RcVec3f p, ref List paths) + public bool RemoveGuideNode(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQueryFilter filter, RcVec3f p, ref List nodes) { - for (int i = 0; i < paths.Count; ++i) + GuideNode removal = null; + for (int i = 0; i < nodes.Count; ++i) { - if (paths[i].Radius >= RcVec3f.Distance(paths[i].Position, p)) + if (nodes[i].Radius >= RcVec3f.Distance(nodes[i].Position, p)) { - paths.RemoveAt(i); - return true; + removal = nodes[i]; + nodes.RemoveAt(i); + break; } } - return false; + + if (null == removal) + return false; + + // 모든 경로에 edge 다 삭제 + var removalEdges = removal.Edges.ToList(); + var leafs = new List(); + foreach (var edge in removalEdges) + { + // 일단 삭제 + edge.NodeA.Remove(edge); + edge.NodeB.Remove(edge); + + if (removal.NodeId != edge.NodeA.NodeId) + { + leafs.Add(edge.NodeA); + } + + if (removal.NodeId != edge.NodeB.NodeId) + { + leafs.Add(edge.NodeB); + } + } + + leafs.Sort((a, b) => a.NodeId.CompareTo(b.NodeId)); + + // 재구성 + for (int i = 1; i < leafs.Count; ++i) + { + var root = leafs[0]; + var next = leafs[i]; + + var edge = new NodeEdge(root, next); + edge.Fllow((a, b) => TestFollowPath(navQuery, filter, a, b)); + root.Add(edge); + next.Add(edge); + } + + + return true; } public DtStatus MoveGuidePath(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQueryFilter filter, RcVec3f p, ref List paths) @@ -83,40 +186,36 @@ public DtStatus MoveGuidePath(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQ node.PolyRef = polyRef; node.Position = nearestPoint; - - var repathIndexes = new List(); - if (nodeIdx < paths.Count - 1) + foreach (var edge in node.Edges) { - repathIndexes.Add(nodeIdx); - } - - if (0 <= nodeIdx - 1) - { - repathIndexes.Add(nodeIdx - 1); + edge.Fllow((a, b) => TestFollowPath(navQuery, filter, a, b)); } - //Console.WriteLine($"success - {node.Position}"); - // 변경에 따른 리패스 - foreach (var repathIdx in repathIndexes) - { - var prevPath = paths[repathIdx]; - var nextPath = paths[repathIdx + 1]; - - var edgePaths = new List(); - List pathIterPolys = new List(); - int pathIterPolyCount = 0; - FindFollowPath(navQuery.GetAttachedNavMesh(), navQuery, - prevPath.PolyRef, nextPath.PolyRef, prevPath.Position, nextPath.Position, - filter, true, ref pathIterPolys, pathIterPolyCount, ref edgePaths); - prevPath.edgePaths = edgePaths; - } + return DtStatus.DT_SUCCESS; + } + private List TestFollowPath(DtNavMeshQuery navQuery, IDtQueryFilter filter, GuideNode nodeA, GuideNode nodeB) + { + var edgePaths = new List(); + List pathIterPolys = new List(); + int pathIterPolyCount = 0; - return DtStatus.DT_SUCCESS; + FindFollowPath(navQuery.GetAttachedNavMesh(), navQuery, + nodeA.PolyRef, nodeB.PolyRef, nodeA.Position, nodeB.Position, + filter, false, ref pathIterPolys, pathIterPolyCount, ref edgePaths); + + if (0 < edgePaths.Count) + { + if (0.01f <= RcVec3f.Distance(nodeB.Position, edgePaths[^1])) + { + return new List(); + } + } + return edgePaths; } - public DtStatus AddGuidePath(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQueryFilter filter, RcVec3f p, ref List paths) + public DtStatus AddGuidePath(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQueryFilter filter, RcVec3f p, ref List nodes) { // 내비메쉬가 존재 하는 곳인가? navQuery.FindNearestPoly(p, new RcVec3f(0.001f, 5.0f, 0.001f), filter, out var polyRef, out var nearestPoint, out var _); @@ -134,83 +233,76 @@ public DtStatus AddGuidePath(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQu return DtStatus.DT_FAILURE; // 벽과의 거리를 계산 - navQuery.FindDistanceToWall(polyRef, p, 100, filter, out var distance, out var __, out var _); + navQuery.FindDistanceToWall(polyRef, nearestPoint, 100, filter, out var distanceToWall, out var __, out var _); // 벽과 너무 가까이 있다면 실패 - if (0.6f >= distance) + if (0.6f >= distanceToWall) return DtStatus.DT_FAILURE; // 선택 지점에 이미 가이드 노드가 있을 경우 - float defaultDistance = Math.Min(2.0f, distance); - foreach (var path in paths) + float distance = Math.Min(2.0f, distanceToWall); + foreach (var path in nodes) { - if (path.Radius + defaultDistance >= RcVec3f.Distance(path.Position, p)) + if (path.Radius + distance >= RcVec3f.Distance(path.Position, nearestPoint)) return DtStatus.DT_FAILURE; } - var current = new GuideNode(); + var current = new GuideNode(_nextNodeId.IncrementAndGet()); current.PolyRef = polyRef; - current.Position = p; - current.Radius = defaultDistance; + current.Position = nearestPoint; + current.Radius = distance; // 노드와 노드 사이의 경로에 있을 경우, 어느 노드에서 추가 할 수 있는지 찾는다 - int intersectNodeIdx = -1; - for (int i = 0; i < paths.Count - 1; ++i) + for (int i = 0; i < nodes.Count; ++i) { - var prev = paths[i]; - - // 노드 사이의 엣지 사이에 들어갈 경우 - foreach (var edgePoint in prev.edgePaths) + var node = nodes[i]; + var edge = node.Intersect(nearestPoint, distance); + if (null != edge) { - if (current.Radius > RcVec3f.Distance(current.Position, edgePoint)) - { - intersectNodeIdx = i; - break; - } - } + var edgeA = new NodeEdge(edge.NodeA, current); + edgeA.Fllow((a, b) => TestFollowPath(navQuery, filter, a, b)); - if (0 <= intersectNodeIdx) - { - break; - } - } + var edgeB = new NodeEdge(current, edge.NodeB); + edgeB.Fllow((a, b) => TestFollowPath(navQuery, filter, a, b)); + var nodeA = edge.NodeA; + nodeA.Remove(edge); + nodeA.Add(edgeA); - List repathIndexes = new List(); - if (0 <= intersectNodeIdx) - { - paths.Insert(intersectNodeIdx + 1, current); - repathIndexes.Add(intersectNodeIdx); - repathIndexes.Add(intersectNodeIdx + 1); - } - else - { - paths.Add(current); + var nodeB = edge.NodeB; + nodeB.Remove(edge); + nodeB.Add(edgeB); - // 노드가 두개 이상일 경우, 경로 조정이 필요함 - if (2 <= paths.Count) - { - repathIndexes.Add(paths.Count - 2); + current.Add(edgeA); + current.Add(edgeB); + + // 노드펙에 넣음 + nodes.Insert(i + 1, current); + return DtStatus.DT_SUCCESS; } } + nodes.Add(current); + + // 노드가 1개 이하일 경우 + if (1 >= nodes.Count) + { + return DtStatus.DT_SUCCESS; + } + + var prev = nodes[^2]; + var newEdge = new NodeEdge(prev, current); + newEdge.Fllow((a, b) => TestFollowPath(navQuery, filter, a, b)); - // 변경에 따른 리패스 - foreach (var repathIdx in repathIndexes) + // 연결 불가능할 경우, 그냥 끊어진 상태로 유지 + if (0 >= newEdge.EdgePaths.Count) { - var prevPath = paths[repathIdx]; - var nextPath = paths[repathIdx + 1]; - - var edgePaths = new List(); - List pathIterPolys = new List(); - int pathIterPolyCount = 0; - FindFollowPath(navQuery.GetAttachedNavMesh(), navQuery, - prevPath.PolyRef, nextPath.PolyRef, prevPath.Position, nextPath.Position, - filter, true, ref pathIterPolys, pathIterPolyCount, ref edgePaths); - prevPath.edgePaths = edgePaths; + return DtStatus.DT_SUCCESS; } + prev.Add(newEdge); + current.Add(newEdge); return DtStatus.DT_SUCCESS; }