a.samsonkin
/
DotRecastNetSim
forked from bit/DotRecastNetSim
1
0
Fork 0
Code Pull Requests Activity

Removed fixupCorridor and replaced with dtMergeCorridorStartMoved (#637)

This commit is contained in:
ikpil 2023-06-26 21:19:44 +09:00
parent 6d8062b41e
commit 5dd072f3a3
3 changed files with 139 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
src/DotRecast.Detour.Crowd/DtPathCorridor.cs
View File

@ -70,126 +70,6 @@ namespace DotRecast.Detour.Crowd
private RcVec3f m_target = new RcVec3f(); private RcVec3f m_target = new RcVec3f();
private List<long> m_path; private List<long> m_path;
protected List<long> MergeCorridorStartMoved(List<long> path, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = path.Count - 1; i >= 0; --i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
{
break;
}
}
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1)
{
return path;
}
// Concatenate paths.
// Adjust beginning of the buffer to include the visited.
List<long> result = new List<long>();
// Store visited
for (int i = visited.Count - 1; i > furthestVisited; --i)
{
result.Add(visited[i]);
}
result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath));
return result;
}
protected List<long> MergeCorridorEndMoved(List<long> path, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = 0; i < path.Count; ++i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
{
break;
}
}
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1)
{
return path;
}
// Concatenate paths.
List<long> result = path.GetRange(0, furthestPath);
result.AddRange(visited.GetRange(furthestVisited, visited.Count - furthestVisited));
return result;
}
protected List<long> MergeCorridorStartShortcut(List<long> path, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = path.Count - 1; i >= 0; --i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
{
break;
}
}
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited <= 0)
{
return path;
}
// Concatenate paths.
// Adjust beginning of the buffer to include the visited.
List<long> result = visited.GetRange(0, furthestVisited);
result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath));
return result;
}
/** /**
* Allocates the corridor's path buffer. * Allocates the corridor's path buffer.
@ -321,7 +201,7 @@ namespace DotRecast.Detour.Crowd
{ {
if (rayHit.path.Count > 1 && rayHit.t > 0.99f) if (rayHit.path.Count > 1 && rayHit.t > 0.99f)
{ {
m_path = MergeCorridorStartShortcut(m_path, rayHit.path); m_path = PathUtils.MergeCorridorStartShortcut(m_path, rayHit.path);
} }
} }
} }
@ -356,7 +236,7 @@ namespace DotRecast.Detour.Crowd
if (status.Succeeded() && res.Count > 0) if (status.Succeeded() && res.Count > 0)
{ {
m_path = MergeCorridorStartShortcut(m_path, res); m_path = PathUtils.MergeCorridorStartShortcut(m_path, res);
return true; return true;
} }
@ -426,7 +306,7 @@ namespace DotRecast.Detour.Crowd
var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, out var visited); var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, out var visited);
if (status.Succeeded()) if (status.Succeeded())
{ {
m_path = MergeCorridorStartMoved(m_path, visited); m_path = PathUtils.MergeCorridorStartMoved(m_path, visited);
// Adjust the position to stay on top of the navmesh. // Adjust the position to stay on top of the navmesh.
m_pos = result; m_pos = result;
@ -465,7 +345,7 @@ namespace DotRecast.Detour.Crowd
var status = navquery.MoveAlongSurface(m_path[m_path.Count - 1], m_target, npos, filter, out var result, out var visited); var status = navquery.MoveAlongSurface(m_path[m_path.Count - 1], m_target, npos, filter, out var result, out var visited);
if (status.Succeeded()) if (status.Succeeded())
{ {
m_path = MergeCorridorEndMoved(m_path, visited); m_path = PathUtils.MergeCorridorEndMoved(m_path, visited);
// TODO: should we do that? // TODO: should we do that?
// Adjust the position to stay on top of the navmesh. // Adjust the position to stay on top of the navmesh.
/* /*

168
src/DotRecast.Detour/PathUtils.cs
View File

@ -83,53 +83,6 @@ namespace DotRecast.Detour
return (dx * dx + dz * dz) < r * r && Math.Abs(dy) < h; return (dx * dx + dz * dz) < r * r && Math.Abs(dy) < h;
} }
public static List<long> FixupCorridor(List<long> path, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = path.Count - 1; i >= 0; --i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
break;
}
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1)
return path;
// Concatenate paths.
// Adjust beginning of the buffer to include the visited.
int req = visited.Count - furthestVisited;
int orig = Math.Min(furthestPath + 1, path.Count);
int size = Math.Max(0, path.Count - orig);
List<long> fixupPath = new List<long>();
// Store visited
for (int i = 0; i < req; ++i)
{
fixupPath.Add(visited[(visited.Count - 1) - i]);
}
for (int i = 0; i < size; i++)
{
fixupPath.Add(path[orig + i]);
}
return fixupPath;
}
// This function checks if the path has a small U-turn, that is, // This function checks if the path has a small U-turn, that is,
// a polygon further in the path is adjacent to the first polygon // a polygon further in the path is adjacent to the first polygon
@ -194,5 +147,126 @@ namespace DotRecast.Detour
return path; return path;
} }
public static List<long> MergeCorridorStartMoved(List<long> path, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = path.Count - 1; i >= 0; --i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
{
break;
}
}
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1)
{
return path;
}
// Concatenate paths.
// Adjust beginning of the buffer to include the visited.
List<long> result = new List<long>();
// Store visited
for (int i = visited.Count - 1; i > furthestVisited; --i)
{
result.Add(visited[i]);
}
result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath));
return result;
}
public static List<long> MergeCorridorEndMoved(List<long> path, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = 0; i < path.Count; ++i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
{
break;
}
}
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1)
{
return path;
}
// Concatenate paths.
List<long> result = path.GetRange(0, furthestPath);
result.AddRange(visited.GetRange(furthestVisited, visited.Count - furthestVisited));
return result;
}
public static List<long> MergeCorridorStartShortcut(List<long> path, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = path.Count - 1; i >= 0; --i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
{
break;
}
}
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited <= 0)
{
return path;
}
// Concatenate paths.
// Adjust beginning of the buffer to include the visited.
List<long> result = visited.GetRange(0, furthestVisited);
result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath));
return result;
}
} }
} }

28
src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs
View File

@ -2,7 +2,6 @@ using System;
using System.Collections.Generic; using System.Collections.Generic;
using DotRecast.Core; using DotRecast.Core;
using DotRecast.Detour; using DotRecast.Detour;
using DotRecast.Recast.DemoTool.Builder; using DotRecast.Recast.DemoTool.Builder;
using DotRecast.Recast.Demo.Draw; using DotRecast.Recast.Demo.Draw;
using DotRecast.Recast.DemoTool; using DotRecast.Recast.DemoTool;
@ -200,10 +199,9 @@ public class TestNavmeshTool : IRcTool
new(enableRaycast ? DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue)); new(enableRaycast ? DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue));
if (0 < m_polys.Count) if (0 < m_polys.Count)
{ {
List<long> polys = new(m_polys);
// Iterate over the path to find smooth path on the detail mesh surface. // Iterate over the path to find smooth path on the detail mesh surface.
m_navQuery.ClosestPointOnPoly(m_startRef, m_spos, out var iterPos, out var _); m_navQuery.ClosestPointOnPoly(m_startRef, m_spos, out var iterPos, out var _);
m_navQuery.ClosestPointOnPoly(polys[polys.Count - 1], m_epos, out var targetPos, out var _); m_navQuery.ClosestPointOnPoly(m_polys[m_polys.Count - 1], m_epos, out var targetPos, out var _);
float STEP_SIZE = 0.5f; float STEP_SIZE = 0.5f;
float SLOP = 0.01f; float SLOP = 0.01f;
@ -213,11 +211,10 @@ public class TestNavmeshTool : IRcTool
// Move towards target a small advancement at a time until target reached or // Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path. // when ran out of memory to store the path.
while (0 < polys.Count && m_smoothPath.Count < MAX_SMOOTH) while (0 < m_polys.Count && m_smoothPath.Count < MAX_SMOOTH)
{ {
// Find location to steer towards. // Find location to steer towards.
SteerTarget steerTarget = PathUtils.GetSteerTarget(m_navQuery, iterPos, targetPos, SteerTarget steerTarget = PathUtils.GetSteerTarget(m_navQuery, iterPos, targetPos, SLOP, m_polys);
SLOP, polys);
if (null == steerTarget) if (null == steerTarget)
{ {
break; break;
@ -247,14 +244,14 @@ public class TestNavmeshTool : IRcTool
RcVec3f moveTgt = RcVec3f.Mad(iterPos, delta, len); RcVec3f moveTgt = RcVec3f.Mad(iterPos, delta, len);
// Move // Move
m_navQuery.MoveAlongSurface(polys[0], iterPos, moveTgt, m_filter, out var result, out var visited); m_navQuery.MoveAlongSurface(m_polys[0], iterPos, moveTgt, m_filter, out var result, out var visited);
iterPos = result; iterPos = result;
polys = PathUtils.FixupCorridor(polys, visited); m_polys = PathUtils.MergeCorridorStartMoved(m_polys, visited);
polys = PathUtils.FixupShortcuts(polys, m_navQuery); m_polys = PathUtils.FixupShortcuts(m_polys, m_navQuery);
var status = m_navQuery.GetPolyHeight(polys[0], result, out var h); var status = m_navQuery.GetPolyHeight(m_polys[0], result, out var h);
if (status.Succeeded()) if (status.Succeeded())
{ {
iterPos.y = h; iterPos.y = h;
@ -280,16 +277,16 @@ public class TestNavmeshTool : IRcTool
// Advance the path up to and over the off-mesh connection. // Advance the path up to and over the off-mesh connection.
long prevRef = 0; long prevRef = 0;
long polyRef = polys[0]; long polyRef = m_polys[0];
int npos = 0; int npos = 0;
while (npos < polys.Count && polyRef != steerTarget.steerPosRef) while (npos < m_polys.Count && polyRef != steerTarget.steerPosRef)
{ {
prevRef = polyRef; prevRef = polyRef;
polyRef = polys[npos]; polyRef = m_polys[npos];
npos++; npos++;
} }
polys = polys.GetRange(npos, polys.Count - npos); m_polys = m_polys.GetRange(npos, m_polys.Count - npos);
// Handle the connection. // Handle the connection.
var status2 = m_navMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPos, ref endPos); var status2 = m_navMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPos, ref endPos);
@ -307,7 +304,7 @@ public class TestNavmeshTool : IRcTool
// Move position at the other side of the off-mesh link. // Move position at the other side of the off-mesh link.
iterPos = endPos; iterPos = endPos;
m_navQuery.GetPolyHeight(polys[0], iterPos, out var eh); m_navQuery.GetPolyHeight(m_polys[0], iterPos, out var eh);
iterPos.y = eh; iterPos.y = eh;
} }
} }
@ -357,6 +354,7 @@ public class TestNavmeshTool : IRcTool
{ {
m_polys = null; m_polys = null;
m_straightPath = null; m_straightPath = null;
if (m_sposSet && m_eposSet && m_startRef != 0 && m_endRef != 0) if (m_sposSet && m_eposSet && m_startRef != 0 && m_endRef != 0)
{ {
m_pathFindStatus = m_navQuery.InitSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, m_filter, m_pathFindStatus = m_navQuery.InitSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, m_filter,