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

remove FindNearestPolyResult 1st

This commit is contained in:
ikpil 2023-06-11 13:28:05 +09:00
parent 34e04f010e
commit 64315ca56e
11 changed files with 116 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/DotRecast.Detour.Crowd/DtCrowd.cs
View File

@ -245,10 +245,13 @@ namespace DotRecast.Detour.Crowd
UpdateAgentParameters(ag, option);
// Find nearest position on navmesh and place the agent there.
Result<FindNearestPolyResult> nearestPoly = _navQuery.FindNearestPoly(pos, _ext, _filters[ag.option.queryFilterType]);
var status = _navQuery.FindNearestPoly(pos, _ext, _filters[ag.option.queryFilterType], out var refs, out var nearest, out var _);
if (status.Failed())
{
nearest = pos;
refs = 0;
}
var nearest = nearestPoly.Succeeded() ? nearestPoly.result.GetNearestPos() : pos;
long refs = nearestPoly.Succeeded() ? nearestPoly.result.GetNearestRef() : 0L;
ag.corridor.Reset(refs, nearest);
ag.boundary.Reset();
ag.partial = false;
@ -462,13 +465,8 @@ namespace DotRecast.Detour.Crowd
{
// Current location is not valid, try to reposition.
// TODO: this can snap agents, how to handle that?
Result<FindNearestPolyResult> nearestPoly = _navQuery.FindNearestPoly(ag.npos, _ext,
_filters[ag.option.queryFilterType]);
agentRef = nearestPoly.Succeeded() ? nearestPoly.result.GetNearestRef() : 0L;
if (nearestPoly.Succeeded())
{
agentPos = nearestPoly.result.GetNearestPos();
}
_navQuery.FindNearestPoly(ag.npos, _ext, _filters[ag.option.queryFilterType], out agentRef, out var nearest, out var _);
agentPos = nearest;
if (agentRef == 0)
{
@ -507,14 +505,8 @@ namespace DotRecast.Detour.Crowd
if (!_navQuery.IsValidPolyRef(ag.targetRef, _filters[ag.option.queryFilterType]))
{
// Current target is not valid, try to reposition.
Result<FindNearestPolyResult> fnp = _navQuery.FindNearestPoly(ag.targetPos, _ext,
_filters[ag.option.queryFilterType]);
ag.targetRef = fnp.Succeeded() ? fnp.result.GetNearestRef() : 0L;
if (fnp.Succeeded())
{
ag.targetPos = fnp.result.GetNearestPos();
}
_navQuery.FindNearestPoly(ag.targetPos, _ext, _filters[ag.option.queryFilterType], out ag.targetRef, out var nearest, out var _);
ag.targetPos = nearest;
replan = true;
}

48
src/DotRecast.Detour/DtFindNearestPolyQuery.cs
View File

@ -6,30 +6,30 @@ namespace DotRecast.Detour
{
public class DtFindNearestPolyQuery : IDtPolyQuery
{
private readonly DtNavMeshQuery query;
private readonly RcVec3f center;
private long nearestRef;
private RcVec3f nearestPt;
private bool overPoly;
private float nearestDistanceSqr;
private readonly DtNavMeshQuery _query;
private readonly RcVec3f _center;
private long _nearestRef;
private RcVec3f _nearestPt;
private bool _overPoly;
private float _nearestDistanceSqr;
public DtFindNearestPolyQuery(DtNavMeshQuery query, RcVec3f center)
{
this.query = query;
this.center = center;
nearestDistanceSqr = float.MaxValue;
nearestPt = center;
this._query = query;
this._center = center;
_nearestDistanceSqr = float.MaxValue;
_nearestPt = center;
}
public void Process(DtMeshTile tile, DtPoly poly, long refs)
{
// Find nearest polygon amongst the nearby polygons.
query.ClosestPointOnPoly(refs, center, out var closestPtPoly, out var posOverPoly);
_query.ClosestPointOnPoly(refs, _center, out var closestPtPoly, out var posOverPoly);
// If a point is directly over a polygon and closer than
// climb height, favor that instead of straight line nearest point.
float d = 0;
RcVec3f diff = center.Subtract(closestPtPoly);
RcVec3f diff = _center.Subtract(closestPtPoly);
if (posOverPoly)
{
d = Math.Abs(diff.y) - tile.data.header.walkableClimb;
@ -40,18 +40,28 @@ namespace DotRecast.Detour
d = RcVec3f.LenSqr(diff);
}
if (d < nearestDistanceSqr)
if (d < _nearestDistanceSqr)
{
nearestPt = closestPtPoly;
nearestDistanceSqr = d;
nearestRef = refs;
overPoly = posOverPoly;
_nearestPt = closestPtPoly;
_nearestDistanceSqr = d;
_nearestRef = refs;
_overPoly = posOverPoly;
}
}
public FindNearestPolyResult Result()
public long NearestRef()
{
return new FindNearestPolyResult(nearestRef, nearestPt, overPoly);
return _nearestRef;
}
public RcVec3f NearestPt()
{
return _nearestPt;
}
public bool OverPoly()
{
return _overPoly;
}
}
}

37
src/DotRecast.Detour/DtNavMeshQuery.cs
View File

@ -559,29 +559,36 @@ namespace DotRecast.Detour
return null != height ? Results.Success(height.Value) : Results.InvalidParam<float>();
}
/**
* Finds the polygon nearest to the specified center point. If center and nearestPt point to an equal position,
* isOverPoly will be true; however there's also a special case of climb height inside the polygon
*
* @param center
* The center of the search box. [(x, y, z)]
* @param halfExtents
* The search distance along each axis. [(x, y, z)]
* @param filter
* The polygon filter to apply to the query.
* @return FindNearestPolyResult containing nearestRef, nearestPt and overPoly
*/
public Result<FindNearestPolyResult> FindNearestPoly(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter)
/// Finds the polygon nearest to the specified center point.
/// [opt] means the specified parameter can be a null pointer, in that case the output parameter will not be set.
///
/// @param[in] center The center of the search box. [(x, y, z)]
/// @param[in] halfExtents The search distance along each axis. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] nearestRef The reference id of the nearest polygon. Will be set to 0 if no polygon is found.
/// @param[out] nearestPt The nearest point on the polygon. Unchanged if no polygon is found. [opt] [(x, y, z)]
/// @param[out] isOverPoly Set to true if the point's X/Z coordinate lies inside the polygon, false otherwise. Unchanged if no polygon is found. [opt]
/// @returns The status flags for the query.
public DtStatus FindNearestPoly(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter,
out long nearestRef, out RcVec3f nearestPt, out bool isOverPoly)
{
nearestRef = 0;
nearestPt = center;
isOverPoly = false;
// Get nearby polygons from proximity grid.
DtFindNearestPolyQuery query = new DtFindNearestPolyQuery(this, center);
DtStatus status = QueryPolygons(center, halfExtents, filter, query);
if (status.Failed())
{
return Results.Of<FindNearestPolyResult>(status, "");
return status;
}
return Results.Success(query.Result());
nearestRef = query.NearestRef();
nearestPt = query.NearestPt();
isOverPoly = query.OverPoly();
return DtStatus.DT_SUCCSESS;
}
// FIXME: (PP) duplicate?

12
src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs
View File

@ -279,10 +279,10 @@ public class CrowdProfilingTool
private void MoveMob(DtNavMeshQuery navquery, IDtQueryFilter filter, DtCrowdAgent ag, CrowdAgentData crowAgentData)
{
// Move somewhere
Result<FindNearestPolyResult> nearestPoly = navquery.FindNearestPoly(ag.npos, crowd.GetQueryExtents(), filter);
if (nearestPoly.Succeeded())
var status = navquery.FindNearestPoly(ag.npos, crowd.GetQueryExtents(), filter, out var nearestRef, out var nearest, out var _);
if (status.Succeeded())
{
var status = navquery.FindRandomPointAroundCircle(nearestPoly.result.GetNearestRef(), crowAgentData.home, zoneRadius * 2f, filter, rnd,
status = navquery.FindRandomPointAroundCircle(nearestRef, crowAgentData.home, zoneRadius * 2f, filter, rnd,
out var randomRef, out var randomPt);
if (status.Succeeded())
{
@ -294,10 +294,10 @@ public class CrowdProfilingTool
private void MoveVillager(DtNavMeshQuery navquery, IDtQueryFilter filter, DtCrowdAgent ag, CrowdAgentData crowAgentData)
{
// Move somewhere close
Result<FindNearestPolyResult> nearestPoly = navquery.FindNearestPoly(ag.npos, crowd.GetQueryExtents(), filter);
if (nearestPoly.Succeeded())
var status = navquery.FindNearestPoly(ag.npos, crowd.GetQueryExtents(), filter, out var nearestRef, out var nearest, out var _);
if (status.Succeeded())
{
var status = navquery.FindRandomPointAroundCircle(nearestPoly.result.GetNearestRef(), crowAgentData.home, zoneRadius * 0.2f, filter, rnd,
status = navquery.FindRandomPointAroundCircle(nearestRef, crowAgentData.home, zoneRadius * 0.2f, filter, rnd,
out var randomRef, out var randomPt);
if (status.Succeeded())
{

9
src/DotRecast.Recast.Demo/Tools/CrowdTool.cs
View File

@ -158,8 +158,7 @@ public class CrowdTool : ITool
{
IDtQueryFilter filter = new DtQueryDefaultFilter();
RcVec3f halfExtents = crowd.GetQueryExtents();
Result<FindNearestPolyResult> result = navquery.FindNearestPoly(p, halfExtents, filter);
long refs = result.result.GetNearestRef();
navquery.FindNearestPoly(p, halfExtents, filter, out var refs, out var nearest, out var _);
if (refs != 0)
{
Result<int> flags = nav.GetPolyFlags(refs);
@ -288,9 +287,7 @@ public class CrowdTool : ITool
}
else
{
Result<FindNearestPolyResult> result = navquery.FindNearestPoly(p, halfExtents, filter);
m_targetRef = result.result.GetNearestRef();
m_targetPos = result.result.GetNearestPos();
navquery.FindNearestPoly(p, halfExtents, filter, out m_targetRef, out m_targetPos, out var _);
if (m_agentDebug.agent != null)
{
crowd.RequestMoveTarget(m_agentDebug.agent, m_targetRef, m_targetPos);
@ -801,7 +798,5 @@ public class CrowdTool : ITool
public void HandleClickRay(RcVec3f start, RcVec3f direction, bool shift)
{
}
}

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

@ -168,7 +168,7 @@ public class TestNavmeshTool : ITool
DtNavMeshQuery m_navQuery = m_sample.GetNavMeshQuery();
if (m_sposSet)
{
m_startRef = m_navQuery.FindNearestPoly(m_spos, m_polyPickExt, m_filter).result?.GetNearestRef() ?? 0;
m_navQuery.FindNearestPoly(m_spos, m_polyPickExt, m_filter, out m_startRef, out var _, out var _);
}
else
{
@ -177,7 +177,7 @@ public class TestNavmeshTool : ITool
if (m_eposSet)
{
m_endRef = m_navQuery.FindNearestPoly(m_epos, m_polyPickExt, m_filter).result?.GetNearestRef() ?? 0;
m_navQuery.FindNearestPoly(m_epos, m_polyPickExt, m_filter, out m_endRef, out var _, out var _);
}
else
{

4
test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs
View File

@ -143,10 +143,10 @@ public class AbstractCrowdTest
}
else
{
Result<FindNearestPolyResult> nearest = query.FindNearestPoly(pos, ext, filter);
query.FindNearestPoly(pos, ext, filter, out var refs, out var nearest, out var _);
foreach (DtCrowdAgent ag in crowd.GetActiveAgents())
{
crowd.RequestMoveTarget(ag, nearest.result.GetNearestRef(), nearest.result.GetNearestPos());
crowd.RequestMoveTarget(ag, refs, nearest);
}
}
}

30
test/DotRecast.Detour.Dynamic.Test/DynamicNavMeshTest.cs
View File

@ -34,30 +34,35 @@ public class DynamicNavMeshTest
Task<bool> future = mesh.Build(Task.Factory);
// wait for build to complete
bool _ = future.Result;
// create new query
DtNavMeshQuery query = new DtNavMeshQuery(mesh.NavMesh());
IDtQueryFilter filter = new DtQueryDefaultFilter();
// find path
FindNearestPolyResult start = query.FindNearestPoly(START_POS, EXTENT, filter).result;
FindNearestPolyResult end = query.FindNearestPoly(END_POS, EXTENT, filter).result;
List<long> path = query.FindPath(start.GetNearestRef(), end.GetNearestRef(), start.GetNearestPos(),
end.GetNearestPos(), filter, DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result;
query.FindNearestPoly(START_POS, EXTENT, filter, out var startRef, out var startPt, out var _);
query.FindNearestPoly(END_POS, EXTENT, filter, out var endRef, out var endPt, out var _);
List<long> path = query.FindPath(startRef, endRef, startPt, endPt, filter, DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result;
// check path length without any obstacles
Assert.That(path.Count, Is.EqualTo(16));
// place obstacle
ICollider colldier = new SphereCollider(SPHERE_POS, 20, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_GROUND, 0.1f);
long colliderId = mesh.AddCollider(colldier);
// update navmesh asynchronously
future = mesh.Update(Task.Factory);
// wait for update to complete
_ = future.Result;
// create new query
query = new DtNavMeshQuery(mesh.NavMesh());
// find path again
start = query.FindNearestPoly(START_POS, EXTENT, filter).result;
end = query.FindNearestPoly(END_POS, EXTENT, filter).result;
path = query.FindPath(start.GetNearestRef(), end.GetNearestRef(), start.GetNearestPos(), end.GetNearestPos(), filter,
DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result;
query.FindNearestPoly(START_POS, EXTENT, filter, out startRef, out startPt, out var _);
query.FindNearestPoly(END_POS, EXTENT, filter, out endRef, out endPt, out var _);
path = query.FindPath(startRef, endRef, startPt, endPt, filter, DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result;
// check path length with obstacles
Assert.That(path.Count, Is.EqualTo(19));
// remove obstacle
@ -68,11 +73,12 @@ public class DynamicNavMeshTest
_ = future.Result;
// create new query
query = new DtNavMeshQuery(mesh.NavMesh());
// find path one more time
start = query.FindNearestPoly(START_POS, EXTENT, filter).result;
end = query.FindNearestPoly(END_POS, EXTENT, filter).result;
path = query.FindPath(start.GetNearestRef(), end.GetNearestRef(), start.GetNearestPos(), end.GetNearestPos(), filter,
DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result;
query.FindNearestPoly(START_POS, EXTENT, filter, out startRef, out startPt, out var _);
query.FindNearestPoly(END_POS, EXTENT, filter, out endRef, out endPt, out var _);
path = query.FindPath(startRef, endRef, startPt, endPt, filter, DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result;
// path length should be back to the initial value
Assert.That(path.Count, Is.EqualTo(16));
}

23
test/DotRecast.Detour.Extras.Test/Unity/Astar/UnityAStarPathfindingImporterTest.cs
View File

@ -66,15 +66,15 @@ public class UnityAStarPathfindingImporterTest
DtMeshData data = tile.data;
DtBVNode[] bvNodes = data.bvTree;
data.bvTree = null; // set BV-Tree empty to get 'clear' search poly without BV
FindNearestPolyResult clearResult = GetNearestPolys(mesh, position)[0]; // check poly to exists
var clearResult = GetNearestPolys(mesh, position)[0]; // check poly to exists
// restore BV-Tree and try search again
// important aspect in that test: BV result must equals result without BV
// if poly not found or found other poly - tile bounds is wrong!
data.bvTree = bvNodes;
FindNearestPolyResult bvResult = GetNearestPolys(mesh, position)[0];
var bvResult = GetNearestPolys(mesh, position)[0];
Assert.That(bvResult.GetNearestRef(), Is.EqualTo(clearResult.GetNearestRef()));
Assert.That(bvResult.refs, Is.EqualTo(clearResult.refs));
}
private DtNavMesh LoadNavMesh(string filename)
@ -95,24 +95,25 @@ public class UnityAStarPathfindingImporterTest
DtNavMeshQuery query = new DtNavMeshQuery(mesh);
IDtQueryFilter filter = new DtQueryDefaultFilter();
FindNearestPolyResult[] polys = GetNearestPolys(mesh, startPos, endPos);
return query.FindPath(polys[0].GetNearestRef(), polys[1].GetNearestRef(), startPos, endPos, filter);
var polys = GetNearestPolys(mesh, startPos, endPos);
return query.FindPath(polys[0].refs, polys[1].refs, startPos, endPos, filter);
}
private FindNearestPolyResult[] GetNearestPolys(DtNavMesh mesh, params RcVec3f[] positions)
private DtPolyPoint[] GetNearestPolys(DtNavMesh mesh, params RcVec3f[] positions)
{
DtNavMeshQuery query = new DtNavMeshQuery(mesh);
IDtQueryFilter filter = new DtQueryDefaultFilter();
RcVec3f extents = RcVec3f.Of(0.1f, 0.1f, 0.1f);
FindNearestPolyResult[] results = new FindNearestPolyResult[positions.Length];
var results = new DtPolyPoint[positions.Length];
for (int i = 0; i < results.Length; i++)
{
RcVec3f position = positions[i];
Result<FindNearestPolyResult> result = query.FindNearestPoly(position, extents, filter);
Assert.That(result.Succeeded(), Is.True);
Assert.That(result.result.GetNearestPos(), Is.Not.EqualTo(RcVec3f.Zero), "Nearest start position is null!");
results[i] = result.result;
var status = query.FindNearestPoly(position, extents, filter, out var nearestRef, out var nearest, out var _);
Assert.That(status.Succeeded(), Is.True);
Assert.That(nearest, Is.Not.EqualTo(RcVec3f.Zero), "Nearest start position is null!");
results[i] = new DtPolyPoint(nearestRef, nearest);
}
return results;

16
test/DotRecast.Detour.Test/FindNearestPolyTest.cs
View File

@ -42,12 +42,12 @@ public class FindNearestPolyTest : AbstractDetourTest
for (int i = 0; i < startRefs.Length; i++)
{
RcVec3f startPos = startPoss[i];
Result<FindNearestPolyResult> poly = query.FindNearestPoly(startPos, extents, filter);
Assert.That(poly.Succeeded(), Is.True);
Assert.That(poly.result.GetNearestRef(), Is.EqualTo(POLY_REFS[i]));
var status = query.FindNearestPoly(startPos, extents, filter, out var nearestRef, out var nearest, out var _);
Assert.That(status.Succeeded(), Is.True);
Assert.That(nearestRef, Is.EqualTo(POLY_REFS[i]));
for (int v = 0; v < POLY_POS[i].Length; v++)
{
Assert.That(poly.result.GetNearestPos()[v], Is.EqualTo(POLY_POS[i][v]).Within(0.001f));
Assert.That(nearest[v], Is.EqualTo(POLY_POS[i][v]).Within(0.001f));
}
}
}
@ -61,12 +61,12 @@ public class FindNearestPolyTest : AbstractDetourTest
for (int i = 0; i < startRefs.Length; i++)
{
RcVec3f startPos = startPoss[i];
Result<FindNearestPolyResult> poly = query.FindNearestPoly(startPos, extents, filter);
Assert.That(poly.Succeeded(), Is.True);
Assert.That(poly.result.GetNearestRef(), Is.EqualTo(0L));
var status = query.FindNearestPoly(startPos, extents, filter, out var nearestRef, out var nearest, out var _);
Assert.That(status.Succeeded(), Is.True);
Assert.That(nearestRef, Is.EqualTo(0L));
for (int v = 0; v < POLY_POS[i].Length; v++)
{
Assert.That(poly.result.GetNearestPos()[v], Is.EqualTo(startPos[v]).Within(0.001f));
Assert.That(nearest[v], Is.EqualTo(startPos[v]).Within(0.001f));
}
}
}

9
test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs
View File

@ -49,12 +49,9 @@ public class TileCacheFindPathTest : AbstractTileCacheTest
{
IDtQueryFilter filter = new DtQueryDefaultFilter();
RcVec3f extents = RcVec3f.Of(2f, 4f, 2f);
Result<FindNearestPolyResult> findPolyStart = query.FindNearestPoly(start, extents, filter);
Result<FindNearestPolyResult> findPolyEnd = query.FindNearestPoly(end, extents, filter);
long startRef = findPolyStart.result.GetNearestRef();
long endRef = findPolyEnd.result.GetNearestRef();
RcVec3f startPos = findPolyStart.result.GetNearestPos();
RcVec3f endPos = findPolyEnd.result.GetNearestPos();
query.FindNearestPoly(start, extents, filter, out var startRef, out var startPos, out var _);
query.FindNearestPoly(end, extents, filter, out var endRef, out var endPos, out var _);
Result<List<long>> path = query.FindPath(startRef, endRef, startPos, endPos, filter);
int maxStraightPath = 256;
int options = 0;