diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs
index d42c02e..c9680ce 100644
--- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs
+++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs
@@ -679,23 +679,12 @@ public class TestNavmeshSampleTool : ISampleTool
         }
         else if (_mode == RcTestNavmeshToolMode.RAYCAST)
         {
-            m_straightPath?.Clear();
             _tool.Raycast(navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter,
                 ref m_polys, ref m_straightPath, ref m_hitPos, ref m_hitNormal, ref m_hitResult);
         }
         else if (_mode == RcTestNavmeshToolMode.DISTANCE_TO_WALL)
         {
-            m_distanceToWall = 0;
-            if (m_sposSet && m_startRef != 0)
-            {
-                var result = navQuery.FindDistanceToWall(m_startRef, m_spos, 100.0f, m_filter, out var hitDist, out var hitPos, out var hitNormal);
-                if (result.Succeeded())
-                {
-                    m_distanceToWall = hitDist;
-                    m_hitPos = hitPos;
-                    m_hitNormal = hitNormal;
-                }
-            }
+            _tool.FindDistanceToWall(navQuery, m_startRef, m_spos, 100.0f, m_filter, ref m_distanceToWall, ref m_hitPos, ref m_hitNormal);
         }
         else if (_mode == RcTestNavmeshToolMode.FIND_POLYS_IN_CIRCLE)
         {
diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
index feb0f10..0d84751 100644
--- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
+++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
@@ -245,6 +245,9 @@ namespace DotRecast.Recast.Toolset.Tools
         {
             if (startRef == 0 || endRef == 0)
             {
+                polys?.Clear();
+                straightPath?.Clear();
+
                 return DtStatus.DT_FAILURE;
             }
 
@@ -289,6 +292,28 @@ namespace DotRecast.Recast.Toolset.Tools
             return status;
         }
 
+        public DtStatus FindDistanceToWall(DtNavMeshQuery navQuery, long startRef, RcVec3f spos, float maxRadius, IDtQueryFilter filter,
+            ref float hitDist, ref RcVec3f hitPos, ref RcVec3f hitNormal)
+        {
+            if (0 == startRef)
+            {
+                return DtStatus.DT_FAILURE;
+            }
+
+            var status = navQuery.FindDistanceToWall(startRef, spos, maxRadius, filter,
+                out var tempHitDist, out var tempHitPos, out var tempHitNormal);
+
+            if (status.Succeeded())
+            {
+                hitDist = tempHitDist;
+                hitPos = tempHitPos;
+                hitNormal = tempHitNormal;
+            }
+
+            return status;
+        }
+
+
         public DtStatus FindPolysAroundCircle(DtNavMeshQuery navQuery, long startRef, long endRef, RcVec3f spos, RcVec3f epos, IDtQueryFilter filter, ref List<long> resultRef, ref List<long> resultParent)
         {
             if (startRef == 0 || endRef == 0)