Skip to content

Commit

Permalink
노드와 엣지로 구분하고, 노드는 엣지를 이용하여 트리 구조로 변경
Browse files Browse the repository at this point in the history
  • Loading branch information
@ikpil
ikpil committed Oct 1, 2024
1 parent 4627622 commit e431720
Show file tree
Hide file tree
Showing 2 changed files with 183 additions and 88 deletions.
11 changes: 7 additions & 4 deletions src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
using System;
using System.Collections.Generic;
using System.Linq;
using DotRecast.Core;
using DotRecast.Core.Numerics;
using DotRecast.Detour;
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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
{
Expand Down
260 changes: 176 additions & 84 deletions src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
View file
Original file line number Diff line number Diff line change
@@ -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<RcVec3f> EdgePaths;

public NodeEdge(GuideNode a, GuideNode b)
{
NodeA = a;
NodeB = b;
EdgePaths = new List<RcVec3f>();
}

public void Fllow(Func<GuideNode, GuideNode, List<RcVec3f>> findFollowPath)
{
EdgePaths = findFollowPath.Invoke(NodeA, NodeB);
}
}

public class GuideNode
{
public readonly long NodeId;
public long PolyRef;
public RcVec3f Position;
public float Radius;

public List<RcVec3f> edgePaths = new List<RcVec3f>();
public readonly List<NodeEdge> Edges;

public GuideNode(long id)
{
NodeId = id;
Edges = new List<NodeEdge>();
}

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
Expand All @@ -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;

Expand All @@ -39,18 +101,59 @@ public string GetName()
return "Test Navmesh";
}

public bool RemoveGuidePath(RcVec3f p, ref List<GuideNode> paths)
public bool RemoveGuideNode(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQueryFilter filter, RcVec3f p, ref List<GuideNode> 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<GuideNode>();
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<GuideNode> paths)
Expand Down Expand Up @@ -83,40 +186,36 @@ public DtStatus MoveGuidePath(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQ

node.PolyRef = polyRef;
node.Position = nearestPoint;

var repathIndexes = new List<int>();
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<RcVec3f>();
List<long> pathIterPolys = new List<long>();
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<RcVec3f> TestFollowPath(DtNavMeshQuery navQuery, IDtQueryFilter filter, GuideNode nodeA, GuideNode nodeB)
{
var edgePaths = new List<RcVec3f>();
List<long> pathIterPolys = new List<long>();
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<RcVec3f>();
}
}
return edgePaths;
}

public DtStatus AddGuidePath(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQueryFilter filter, RcVec3f p, ref List<GuideNode> paths)
public DtStatus AddGuidePath(DtNavMeshQuery navQuery, RcVec3f halfExtents, IDtQueryFilter filter, RcVec3f p, ref List<GuideNode> nodes)
{
// 내비메쉬가 존재 하는 곳인가?
navQuery.FindNearestPoly(p, new RcVec3f(0.001f, 5.0f, 0.001f), filter, out var polyRef, out var nearestPoint, out var _);
Expand All @@ -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<int> repathIndexes = new List<int>();

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<RcVec3f>();
List<long> pathIterPolys = new List<long>();
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;
}
Expand Down

0 comments on commit e431720

Please sign in to comment.