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

change ClosestPointOnPolyBoundary

This commit is contained in:
ikpil 2023-06-18 11:43:01 +09:00
parent 0971bf2c3c
commit 8a5614eca4
2 changed files with 30 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -515,7 +515,7 @@ namespace DotRecast.Detour.Crowd
} }
} }
public void TrimInvalidPath(long safeRef, float[] safePos, DtNavMeshQuery navquery, IDtQueryFilter filter) public bool TrimInvalidPath(long safeRef, float[] safePos, DtNavMeshQuery navquery, IDtQueryFilter filter)
{ {
// Keep valid path as far as possible. // Keep valid path as far as possible.
int n = 0; int n = 0;
@ -524,7 +524,12 @@ namespace DotRecast.Detour.Crowd
n++; n++;
} }
if (n == 0) if (m_path.Count == n)
{
// All valid, no need to fix.
return true;
}
else if (n == 0)
{ {
// The first polyref is bad, use current safe values. // The first polyref is bad, use current safe values.
m_pos.Set(safePos); m_pos.Set(safePos);
@ -533,16 +538,13 @@ namespace DotRecast.Detour.Crowd
} }
else if (n < m_path.Count) else if (n < m_path.Count)
{ {
m_path = m_path.GetRange(0, n);
// The path is partially usable. // The path is partially usable.
m_path = m_path.GetRange(0, n);
} }
// Clamp target pos to last poly // Clamp target pos to last poly
var result = navquery.ClosestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target); navquery.ClosestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target, out m_target);
if (result.Succeeded()) return true;
{
m_target = result.result;
}
} }
/** /**

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

@ -448,35 +448,31 @@ namespace DotRecast.Detour
/// ///
/// Much faster than ClosestPointOnPoly(). /// Much faster than ClosestPointOnPoly().
/// ///
/// If the provided position lies within the polygon's xz-bounds (above or below), /// If the provided position lies within the polygon's xz-bounds (above or below),
/// then @p pos and @p closest will be equal. /// then @p pos and @p closest will be equal.
/// ///
/// The height of @p closest will be the polygon boundary. The height detail is not used. /// The height of @p closest will be the polygon boundary. The height detail is not used.
/// ///
/// @p pos does not have to be within the bounds of the polybon or the navigation mesh. /// @p pos does not have to be within the bounds of the polybon or the navigation mesh.
/// ///
/// Returns a point on the boundary closest to the source point if the source point is outside the /// Returns a point on the boundary closest to the source point if the source point is outside the
/// polygon's xz-bounds. /// polygon's xz-bounds.
/// @param[in] ref The reference id to the polygon. /// @param[in] ref The reference id to the polygon.
/// @param[in] pos The position to check. [(x, y, z)] /// @param[in] pos The position to check. [(x, y, z)]
/// @param[out] closest The closest point. [(x, y, z)] /// @param[out] closest The closest point. [(x, y, z)]
/// @returns The status flags for the query. /// @returns The status flags for the query.
public Result<RcVec3f> ClosestPointOnPolyBoundary(long refs, RcVec3f pos) public DtStatus ClosestPointOnPolyBoundary(long refs, RcVec3f pos, out RcVec3f closest)
{ {
closest = pos;
var status = m_nav.GetTileAndPolyByRef(refs, out var tile, out var poly); var status = m_nav.GetTileAndPolyByRef(refs, out var tile, out var poly);
if (status.Failed()) if (status.Failed())
{ {
return Results.Of<RcVec3f>(status, ""); return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
} }
if (tile == null) if (tile == null || !RcVec3f.IsFinite(pos))
{ {
return Results.InvalidParam<RcVec3f>("Invalid tile"); return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
if (!RcVec3f.IsFinite(pos))
{
return Results.InvalidParam<RcVec3f>();
} }
// Collect vertices. // Collect vertices.
@ -489,7 +485,6 @@ namespace DotRecast.Detour
Array.Copy(tile.data.verts, poly.verts[i] * 3, verts, i * 3, 3); Array.Copy(tile.data.verts, poly.verts[i] * 3, verts, i * 3, 3);
} }
RcVec3f closest;
if (DetourCommon.DistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) if (DetourCommon.DistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
{ {
closest = pos; closest = pos;
@ -513,7 +508,7 @@ namespace DotRecast.Detour
closest = RcVec3f.Lerp(verts, va, vb, edget[imin]); closest = RcVec3f.Lerp(verts, va, vb, edget[imin]);
} }
return Results.Success(closest); return DtStatus.DT_SUCCSESS;
} }
/// @par /// @par
@ -1545,20 +1540,18 @@ namespace DotRecast.Detour
} }
// TODO: Should this be callers responsibility? // TODO: Should this be callers responsibility?
Result<RcVec3f> closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos); var closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos, out var closestStartPos);
if (closestStartPosRes.Failed()) if (closestStartPosRes.Failed())
{ {
return Results.InvalidParam<List<StraightPathItem>>("Cannot find start position"); return Results.InvalidParam<List<StraightPathItem>>("Cannot find start position");
} }
var closestStartPos = closestStartPosRes.result; var closestEndPosRes = ClosestPointOnPolyBoundary(path[path.Count - 1], endPos, out var closestEndPos);
var closestEndPosRes = ClosestPointOnPolyBoundary(path[path.Count - 1], endPos);
if (closestEndPosRes.Failed()) if (closestEndPosRes.Failed())
{ {
return Results.InvalidParam<List<StraightPathItem>>("Cannot find end position"); return Results.InvalidParam<List<StraightPathItem>>("Cannot find end position");
} }
var closestEndPos = closestEndPosRes.result;
// Add start point. // Add start point.
DtStatus stat = AppendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], straightPath, maxStraightPath); DtStatus stat = AppendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], straightPath, maxStraightPath);
if (!stat.InProgress()) if (!stat.InProgress())
@ -1592,18 +1585,17 @@ namespace DotRecast.Detour
int fromType; // // fromType is ignored. int fromType; // // fromType is ignored.
// Next portal. // Next portal.
var portalPoints = GetPortalPoints(path[i], path[i + 1], out left, out right, out fromType, out toType); var ppStatus = GetPortalPoints(path[i], path[i + 1], out left, out right, out fromType, out toType);
if (portalPoints.Failed()) if (ppStatus.Failed())
{ {
// Failed to get portal points, in practice this means that path[i+1] is invalid polygon. // Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
// Clamp the end point to path[i], and return the path so far. // Clamp the end point to path[i], and return the path so far.
closestEndPosRes = ClosestPointOnPolyBoundary(path[i], endPos); var cpStatus = ClosestPointOnPolyBoundary(path[i], endPos, out closestEndPos);
if (closestEndPosRes.Failed()) if (cpStatus.Failed())
{ {
return Results.InvalidParam<List<StraightPathItem>>(); return Results.InvalidParam<List<StraightPathItem>>();
} }
closestEndPos = closestEndPosRes.result;
// Append portals along the current straight path segment. // Append portals along the current straight path segment.
if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0) if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
{ {