diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index bf9f578..04b9bb4 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -1220,11 +1220,11 @@ namespace DotRecast.Detour // position. var neighbourPos = neighbourNode.pos; var empStatus = neighbourRef == m_query.endRef - ? GetEdgeIntersectionPoint(bestNode.pos, bestRef, bestPoly, bestTile, + ? GetEdgeIntersectionPoint(bestNode.pos, bestRef, bestPoly, bestTile, m_query.endPos, neighbourRef, neighbourPoly, neighbourTile, ref neighbourPos) - : GetEdgeMidPoint(bestRef, bestPoly, bestTile, - neighbourRef, neighbourPoly, neighbourTile, + : GetEdgeMidPoint(bestRef, bestPoly, bestTile, + neighbourRef, neighbourPoly, neighbourTile, ref neighbourPos); // Calculate cost and heuristic. @@ -3108,23 +3108,28 @@ namespace DotRecast.Detour /// The normal will become unpredicable if @p hitDist is a very small number. /// /// Finds the distance from the specified position to the nearest polygon wall. - /// @param[in] startRef The reference id of the polygon containing @p centerPos. - /// @param[in] centerPos The center of the search circle. [(x, y, z)] - /// @param[in] maxRadius The radius of the search circle. - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] hitDist The distance to the nearest wall from @p centerPos. - /// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)] - /// @param[out] hitNormal The normalized ray formed from the wall point to the - /// source point. [(x, y, z)] + /// @param[in] startRef The reference id of the polygon containing @p centerPos. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] maxRadius The radius of the search circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] hitDist The distance to the nearest wall from @p centerPos. + /// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)] + /// @param[out] hitNormal The normalized ray formed from the wall point to the + /// source point. [(x, y, z)] /// @returns The status flags for the query. - public virtual Result FindDistanceToWall(long startRef, RcVec3f centerPos, float maxRadius, - IDtQueryFilter filter) + public virtual DtStatus FindDistanceToWall(long startRef, RcVec3f centerPos, float maxRadius, + IDtQueryFilter filter, + out float hitDist, out RcVec3f hitPos, out RcVec3f hitNormal) { + hitDist = 0; + hitPos = RcVec3f.Zero; + hitNormal = RcVec3f.Zero; + // Validate input if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || maxRadius < 0 || !float.IsFinite(maxRadius) || null == filter) { - return Results.InvalidParam(); + return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } m_nodePool.Clear(); @@ -3140,9 +3145,10 @@ namespace DotRecast.Detour m_openList.Push(startNode); float radiusSqr = Sqr(maxRadius); - RcVec3f hitPos = new RcVec3f(); RcVec3f? bestvj = null; RcVec3f? bestvi = null; + + var status = DtStatus.DT_SUCCSESS; while (!m_openList.IsEmpty()) { DtNode bestNode = m_openList.Pop(); @@ -3217,11 +3223,9 @@ namespace DotRecast.Detour // Hit wall, update radius. radiusSqr = distSqr; // Calculate hit pos. - hitPos.x = bestTile.data.verts[vj] + (bestTile.data.verts[vi] - bestTile.data.verts[vj]) * tseg; - hitPos.y = bestTile.data.verts[vj + 1] - + (bestTile.data.verts[vi + 1] - bestTile.data.verts[vj + 1]) * tseg; - hitPos.z = bestTile.data.verts[vj + 2] - + (bestTile.data.verts[vi + 2] - bestTile.data.verts[vj + 2]) * tseg; + hitPos.x = bestTile.data.verts[vj + 0] + (bestTile.data.verts[vi + 0] - bestTile.data.verts[vj + 0]) * tseg; + hitPos.y = bestTile.data.verts[vj + 1] + (bestTile.data.verts[vi + 1] - bestTile.data.verts[vj + 1]) * tseg; + hitPos.z = bestTile.data.verts[vj + 2] + (bestTile.data.verts[vi + 2] - bestTile.data.verts[vj + 2]) * tseg; bestvj = RcVec3f.Of(bestTile.data.verts, vj); bestvi = RcVec3f.Of(bestTile.data.verts, vi); } @@ -3261,6 +3265,11 @@ namespace DotRecast.Detour } DtNode neighbourNode = m_nodePool.GetNode(neighbourRef); + if (null == neighbourNode) + { + status |= DtStatus.DT_OUT_OF_NODES; + continue; + } if ((neighbourNode.flags & DtNode.DT_NODE_CLOSED) != 0) { @@ -3301,7 +3310,6 @@ namespace DotRecast.Detour } // Calc hit normal. - RcVec3f hitNormal = new RcVec3f(); if (bestvi != null && bestvj != null) { var tangent = bestvi.Value.Subtract(bestvj.Value); @@ -3311,7 +3319,9 @@ namespace DotRecast.Detour hitNormal.Normalize(); } - return Results.Success(new FindDistanceToWallResult((float)Math.Sqrt(radiusSqr), hitPos, hitNormal)); + hitDist = (float)Math.Sqrt(radiusSqr); + + return status; } /// Returns true if the polygon reference is valid and passes the filter restrictions. diff --git a/src/DotRecast.Detour/LegacyNavMeshQuery.cs b/src/DotRecast.Detour/LegacyNavMeshQuery.cs index b23b783..d314080 100644 --- a/src/DotRecast.Detour/LegacyNavMeshQuery.cs +++ b/src/DotRecast.Detour/LegacyNavMeshQuery.cs @@ -625,13 +625,19 @@ namespace DotRecast.Detour return Results.Of(details, path); } - public override Result FindDistanceToWall(long startRef, RcVec3f centerPos, float maxRadius, IDtQueryFilter filter) + public override DtStatus FindDistanceToWall(long startRef, RcVec3f centerPos, float maxRadius, + IDtQueryFilter filter, + out float hitDist, out RcVec3f hitPos, out RcVec3f hitNormal) { + hitDist = 0; + hitPos = RcVec3f.Zero; + hitNormal = RcVec3f.Zero; + // Validate input if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || maxRadius < 0 || !float.IsFinite(maxRadius) || null == filter) { - return Results.InvalidParam(); + return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } m_nodePool.Clear(); @@ -647,10 +653,11 @@ namespace DotRecast.Detour m_openList.Push(startNode); float radiusSqr = Sqr(maxRadius); - RcVec3f hitPos = RcVec3f.Zero; RcVec3f? bestvj = null; RcVec3f? bestvi = null; + var status = DtStatus.DT_SUCCSESS; + while (!m_openList.IsEmpty()) { DtNode bestNode = m_openList.Pop(); @@ -724,11 +731,9 @@ namespace DotRecast.Detour // Hit wall, update radius. radiusSqr = distSqr; // Calculate hit pos. - hitPos.x = bestTile.data.verts[vj] + (bestTile.data.verts[vi] - bestTile.data.verts[vj]) * tseg; - hitPos.y = bestTile.data.verts[vj + 1] - + (bestTile.data.verts[vi + 1] - bestTile.data.verts[vj + 1]) * tseg; - hitPos.z = bestTile.data.verts[vj + 2] - + (bestTile.data.verts[vi + 2] - bestTile.data.verts[vj + 2]) * tseg; + hitPos.x = bestTile.data.verts[vj + 0] + (bestTile.data.verts[vi + 0] - bestTile.data.verts[vj + 0]) * tseg; + hitPos.y = bestTile.data.verts[vj + 1] + (bestTile.data.verts[vi + 1] - bestTile.data.verts[vj + 1]) * tseg; + hitPos.z = bestTile.data.verts[vj + 2] + (bestTile.data.verts[vi + 2] - bestTile.data.verts[vj + 2]) * tseg; bestvj = RcVec3f.Of(bestTile.data.verts, vj); bestvi = RcVec3f.Of(bestTile.data.verts, vi); } @@ -768,6 +773,11 @@ namespace DotRecast.Detour } DtNode neighbourNode = m_nodePool.GetNode(neighbourRef); + if (null == neighbourNode) + { + status |= DtStatus.DT_OUT_OF_NODES; + continue; + } if ((neighbourNode.flags & DtNode.DT_NODE_CLOSED) != 0) { @@ -808,7 +818,6 @@ namespace DotRecast.Detour } // Calc hit normal. - RcVec3f hitNormal = new RcVec3f(); if (bestvi != null && bestvj != null) { var tangent = bestvi.Value.Subtract(bestvj.Value); @@ -817,8 +826,10 @@ namespace DotRecast.Detour hitNormal.z = -tangent.x; hitNormal.Normalize(); } - - return Results.Success(new FindDistanceToWallResult((float)Math.Sqrt(radiusSqr), hitPos, hitNormal)); + + hitDist = (float)Math.Sqrt(radiusSqr); + + return status; } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/QueryResults/FindDistanceToWallResult.cs b/src/DotRecast.Detour/QueryResults/FindDistanceToWallResult.cs deleted file mode 100644 index e0c5d16..0000000 --- a/src/DotRecast.Detour/QueryResults/FindDistanceToWallResult.cs +++ /dev/null @@ -1,54 +0,0 @@ -/* -Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -recast4j copyright (c) 2015-2019 Piotr Piastucki piotr@jtilia.org -DotRecast Copyright (c) 2023 Choi Ikpil ikpil@naver.com - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any damages -arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, subject to the following restrictions: -1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -using DotRecast.Core; - -namespace DotRecast.Detour.QueryResults -{ -//TODO: (PP) Add comments - public class FindDistanceToWallResult - { - private readonly float distance; - private readonly RcVec3f position; - private readonly RcVec3f normal; - - public FindDistanceToWallResult(float distance, RcVec3f position, RcVec3f normal) - { - this.distance = distance; - this.position = position; - this.normal = normal; - } - - public float GetDistance() - { - return distance; - } - - public RcVec3f GetPosition() - { - return position; - } - - public RcVec3f GetNormal() - { - return normal; - } - } -} \ No newline at end of file diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index b407ebc..5690f59 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -411,13 +411,12 @@ public class TestNavmeshTool : IRcTool if (m_sposSet && m_startRef != 0) { m_distanceToWall = 0.0f; - Result result = m_navQuery.FindDistanceToWall(m_startRef, m_spos, 100.0f, - m_filter); + var result = m_navQuery.FindDistanceToWall(m_startRef, m_spos, 100.0f, m_filter, out var hitDist, out var hitPos, out var hitNormal); if (result.Succeeded()) { - m_distanceToWall = result.result.GetDistance(); - m_hitPos = result.result.GetPosition(); - m_hitNormal = result.result.GetNormal(); + m_distanceToWall = hitDist; + m_hitPos = hitPos; + m_hitNormal = hitNormal; } } } diff --git a/test/DotRecast.Detour.Test/FindDistanceToWallTest.cs b/test/DotRecast.Detour.Test/FindDistanceToWallTest.cs index 34baeb3..0ac7c69 100644 --- a/test/DotRecast.Detour.Test/FindDistanceToWallTest.cs +++ b/test/DotRecast.Detour.Test/FindDistanceToWallTest.cs @@ -52,17 +52,17 @@ public class FindDistanceToWallTest : AbstractDetourTest for (int i = 0; i < startRefs.Length; i++) { RcVec3f startPos = startPoss[i]; - Result result = query.FindDistanceToWall(startRefs[i], startPos, 3.5f, filter); - FindDistanceToWallResult hit = result.result; - Assert.That(hit.GetDistance(), Is.EqualTo(DISTANCES_TO_WALL[i]).Within(0.001f)); - - Assert.That(hit.GetPosition().x, Is.EqualTo(HIT_POSITION[i].x).Within(0.001f)); - Assert.That(hit.GetPosition().y, Is.EqualTo(HIT_POSITION[i].y).Within(0.001f)); - Assert.That(hit.GetPosition().z, Is.EqualTo(HIT_POSITION[i].z).Within(0.001f)); + query.FindDistanceToWall(startRefs[i], startPos, 3.5f, filter, + out var hitDist, out var hitPos, out var hitNormal); + Assert.That(hitDist, Is.EqualTo(DISTANCES_TO_WALL[i]).Within(0.001f)); - Assert.That(hit.GetNormal().x, Is.EqualTo(HIT_NORMAL[i].x).Within(0.001f)); - Assert.That(hit.GetNormal().y, Is.EqualTo(HIT_NORMAL[i].y).Within(0.001f)); - Assert.That(hit.GetNormal().z, Is.EqualTo(HIT_NORMAL[i].z).Within(0.001f)); + Assert.That(hitPos.x, Is.EqualTo(HIT_POSITION[i].x).Within(0.001f)); + Assert.That(hitPos.y, Is.EqualTo(HIT_POSITION[i].y).Within(0.001f)); + Assert.That(hitPos.z, Is.EqualTo(HIT_POSITION[i].z).Within(0.001f)); + + Assert.That(hitNormal.x, Is.EqualTo(HIT_NORMAL[i].x).Within(0.001f)); + Assert.That(hitNormal.y, Is.EqualTo(HIT_NORMAL[i].y).Within(0.001f)); + Assert.That(hitNormal.z, Is.EqualTo(HIT_NORMAL[i].z).Within(0.001f)); } } } \ No newline at end of file