Add Navigator and Lua API function to find nearest position on navmesh

apps/openmw/mwlua/nearbybindings.cpp

@@ -3,11 +3,15 @@
 #include <components/detournavigator/navigator.hpp>
 #include <components/detournavigator/navigatorutils.hpp>
 #include <components/lua/luastate.hpp>
+#include <components/misc/constants.hpp>
 #include <components/settings/values.hpp>
 
 #include "../mwbase/environment.hpp"
 #include "../mwbase/world.hpp"
 #include "../mwphysics/raycasting.hpp"
+#include "../mwworld/cell.hpp"
+#include "../mwworld/cellstore.hpp"
+#include "../mwworld/scene.hpp"
 
 #include "luamanagerimp.hpp"
 #include "objectlists.hpp"
@@ -262,6 +266,39 @@ namespace MWLua
                       *MWBase::Environment::get().getWorld()->getNavigator(), agentBounds, from, to, includeFlags);
               };
 
+        api["findNearestNavMeshPosition"] = [](const osg::Vec3f& position, const sol::optional<sol::table>& options) {
+            DetourNavigator::AgentBounds agentBounds = defaultAgentBounds;
+            std::optional<osg::Vec3f> searchAreaHalfExtents;
+            DetourNavigator::Flags includeFlags = defaultIncludeFlags;
+
+            if (options.has_value())
+            {
+                if (const auto& t = options->get<sol::optional<sol::table>>("agentBounds"))
+                {
+                    if (const auto& v = t->get<sol::optional<DetourNavigator::CollisionShapeType>>("shapeType"))
+                        agentBounds.mShapeType = *v;
+                    if (const auto& v = t->get<sol::optional<osg::Vec3f>>("halfExtents"))
+                        agentBounds.mHalfExtents = *v;
+                }
+                if (const auto& v = options->get<sol::optional<osg::Vec3f>>("searchAreaHalfExtents"))
+                    searchAreaHalfExtents = *v;
+                if (const auto& v = options->get<sol::optional<DetourNavigator::Flags>>("includeFlags"))
+                    includeFlags = *v;
+            }
+
+            if (!searchAreaHalfExtents.has_value())
+            {
+                const bool isEsm4 = MWBase::Environment::get().getWorldScene()->getCurrentCell()->getCell()->isEsm4();
+                const float halfExtents = isEsm4
+                    ? (1 + 2 * Constants::ESM4CellGridRadius) * Constants::ESM4CellSizeInUnits
+                    : (1 + 2 * Constants::CellGridRadius) * Constants::CellSizeInUnits;
+                searchAreaHalfExtents = osg::Vec3f(halfExtents, halfExtents, halfExtents);
+            }
+
+            return DetourNavigator::findNearestNavMeshPosition(*MWBase::Environment::get().getWorld()->getNavigator(),
+                agentBounds, position, *searchAreaHalfExtents, includeFlags);
+        };
+
         return LuaUtil::makeReadOnly(api);
     }
 }

apps/openmw_test_suite/detournavigator/navigator.cpp

@@ -944,4 +944,58 @@ namespace
 
     INSTANTIATE_TEST_SUITE_P(NotSupportedAgentBounds, DetourNavigatorNavigatorNotSupportedAgentBoundsTest,
         ValuesIn(notSupportedAgentBounds));
+
+    TEST_F(DetourNavigatorNavigatorTest, find_nearest_nav_mesh_position_should_return_nav_mesh_position)
+    {
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
+        const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);
+
+        ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
+        auto updateGuard = mNavigator->makeUpdateGuard();
+        mNavigator->addHeightfield(mCellPosition, cellSize, surface, updateGuard.get());
+        mNavigator->update(mPlayerPosition, updateGuard.get());
+        updateGuard.reset();
+        mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);
+
+        const osg::Vec3f position(250, 250, 0);
+        const osg::Vec3f searchAreaHalfExtents(1000, 1000, 1000);
+        EXPECT_THAT(findNearestNavMeshPosition(*mNavigator, mAgentBounds, position, searchAreaHalfExtents, Flag_walk),
+            Optional(Vec3fEq(250, 250, -62.5186)));
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, find_nearest_nav_mesh_position_should_return_nullopt_when_too_far)
+    {
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
+        const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);
+
+        ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
+        auto updateGuard = mNavigator->makeUpdateGuard();
+        mNavigator->addHeightfield(mCellPosition, cellSize, surface, updateGuard.get());
+        mNavigator->update(mPlayerPosition, updateGuard.get());
+        updateGuard.reset();
+        mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);
+
+        const osg::Vec3f position(250, 250, 250);
+        const osg::Vec3f searchAreaHalfExtents(100, 100, 100);
+        EXPECT_EQ(findNearestNavMeshPosition(*mNavigator, mAgentBounds, position, searchAreaHalfExtents, Flag_walk),
+            std::nullopt);
+    }
+
+    TEST_F(DetourNavigatorNavigatorTest, find_nearest_nav_mesh_position_should_return_nullopt_when_flags_do_not_match)
+    {
+        const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
+        const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);
+
+        ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
+        auto updateGuard = mNavigator->makeUpdateGuard();
+        mNavigator->addHeightfield(mCellPosition, cellSize, surface, updateGuard.get());
+        mNavigator->update(mPlayerPosition, updateGuard.get());
+        updateGuard.reset();
+        mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);
+
+        const osg::Vec3f position(250, 250, 0);
+        const osg::Vec3f searchAreaHalfExtents(1000, 1000, 1000);
+        EXPECT_EQ(findNearestNavMeshPosition(*mNavigator, mAgentBounds, position, searchAreaHalfExtents, Flag_swim),
+            std::nullopt);
+    }
 }

components/detournavigator/navigatorutils.cpp

@@ -1,8 +1,11 @@
 #include "navigatorutils.hpp"
+#include "debug.hpp"
 #include "findrandompointaroundcircle.hpp"
 #include "navigator.hpp"
 #include "raycast.hpp"
 
+#include <components/debug/debuglog.hpp>
+
 namespace DetourNavigator
 {
     std::optional<osg::Vec3f> findRandomPointAroundCircle(const Navigator& navigator, const AgentBounds& agentBounds,
@@ -37,4 +40,41 @@ namespace DetourNavigator
             return std::nullopt;
         return fromNavMeshCoordinates(settings.mRecast, *result);
     }
+
+    std::optional<osg::Vec3f> findNearestNavMeshPosition(const Navigator& navigator, const AgentBounds& agentBounds,
+        const osg::Vec3f& position, const osg::Vec3f& searchAreaHalfExtents, const Flags includeFlags)
+    {
+        const auto navMesh = navigator.getNavMesh(agentBounds);
+        if (navMesh == nullptr)
+            return std::nullopt;
+
+        const auto& settings = navigator.getSettings();
+        const osg::Vec3f navMeshPosition = toNavMeshCoordinates(settings.mRecast, position);
+        const auto lockedNavMesh = navMesh->lockConst();
+
+        dtNavMeshQuery navMeshQuery;
+        if (const dtStatus status
+            = navMeshQuery.init(&lockedNavMesh->getImpl(), settings.mDetour.mMaxNavMeshQueryNodes);
+            dtStatusFailed(status))
+        {
+            Log(Debug::Error) << "Failed to init dtNavMeshQuery for findNearestNavMeshPosition: "
+                              << WriteDtStatus{ status };
+            return std::nullopt;
+        }
+
+        dtQueryFilter queryFilter;
+        queryFilter.setIncludeFlags(includeFlags);
+
+        osg::Vec3f nearestNavMeshPos;
+        const osg::Vec3f endPolyHalfExtents = toNavMeshCoordinates(settings.mRecast, searchAreaHalfExtents);
+        dtPolyRef polyRef;
+        if (const dtStatus status = navMeshQuery.findNearestPoly(
+                navMeshPosition.ptr(), endPolyHalfExtents.ptr(), &queryFilter, &polyRef, nearestNavMeshPos.ptr());
+            dtStatusFailed(status) || polyRef == 0)
+        {
+            return std::nullopt;
+        }
+
+        return fromNavMeshCoordinates(settings.mRecast, nearestNavMeshPos);
+    }
 }

components/detournavigator/navigatorutils.hpp

@@ -16,10 +16,10 @@ namespace DetourNavigator
 {
     /**
      * @brief findPath fills output iterator with points of scene surfaces to be used for actor to walk through.
-     * @param agentBounds allows to find navmesh for given actor.
+     * @param agentBounds defines which navmesh to use.
      * @param start path from given point.
      * @param end path at given point.
-     * @param includeFlags setup allowed surfaces for actor to walk.
+     * @param includeFlags setup allowed navmesh areas.
      * @param out the beginning of the destination range.
      * @param endTolerance defines maximum allowed distance to end path point in addition to agentHalfExtents
      * @return Status.
@@ -42,10 +42,10 @@ namespace DetourNavigator
 
     /**
      * @brief findRandomPointAroundCircle returns random location on navmesh within the reach of specified location.
-     * @param agentBounds allows to find navmesh for given actor.
+     * @param agentBounds defines which navmesh to use.
      * @param start is a position where the search starts.
      * @param maxRadius limit maximum distance from start.
-     * @param includeFlags setup allowed surfaces for actor to walk.
+     * @param includeFlags setup allowed navmesh areas.
      * @return not empty optional with position if point is found and empty optional if point is not found.
      */
     std::optional<osg::Vec3f> findRandomPointAroundCircle(const Navigator& navigator, const AgentBounds& agentBounds,
@@ -53,14 +53,25 @@ namespace DetourNavigator
 
     /**
      * @brief raycast finds farest navmesh point from start on a line from start to end that has path from start.
-     * @param agentBounds allows to find navmesh for given actor.
+     * @param agentBounds defines which navmesh to use.
      * @param start of the line
      * @param end of the line
-     * @param includeFlags setup allowed surfaces for actor to walk.
+     * @param includeFlags setup allowed navmesh areas.
      * @return not empty optional with position if point is found and empty optional if point is not found.
      */
     std::optional<osg::Vec3f> raycast(const Navigator& navigator, const AgentBounds& agentBounds,
         const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags);
+
+    /**
+     * @brief findNearestNavMeshPosition finds nearest position on navmesh within given area having given flags.
+     * @param agentBounds defines which navmesh to use.
+     * @param position is a center of the search area.
+     * @param searchAreaHalfExtents defines AABB like area around given postion.
+     * @param includeFlags setup allowed navmesh areas.
+     * @return not empty optional with position if position is found and empty optional if position is not found.
+     */
+    std::optional<osg::Vec3f> findNearestNavMeshPosition(const Navigator& navigator, const AgentBounds& agentBounds,
+        const osg::Vec3f& position, const osg::Vec3f& searchAreaHalfExtents, const Flags includeFlags);
 }
 
 #endif

files/lua_api/openmw/nearby.lua

@@ -182,6 +182,17 @@
 -- values (default: @{#NAVIGATOR_FLAGS.Walk} + @{#NAVIGATOR_FLAGS.Swim} + @{#NAVIGATOR_FLAGS.OpenDoor}
 -- + @{#NAVIGATOR_FLAGS.UsePathgrid}).
 
+---
+-- A table of parameters for @{#nearby.findNearestNavMeshPosition}
+-- @type FindNearestNavMeshPositionOptions
+-- @field [parent=#NavMeshOptions] #AgentBounds agentBounds Identifies which navmesh to use.
+-- @field [parent=#NavMeshOptions] #number includeFlags Allowed areas for agent to move, a sum of @{#NAVIGATOR_FLAGS}
+-- values (default: @{#NAVIGATOR_FLAGS.Walk} + @{#NAVIGATOR_FLAGS.Swim} + @{#NAVIGATOR_FLAGS.OpenDoor}
+-- + @{#NAVIGATOR_FLAGS.UsePathgrid}).
+-- @field [parent=#NavMeshOptions] openmw.util#Vector3 searchAreaHalfExtents Defines AABB like area half extents around
+-- given position (default: (1 + 2 * CellGridRadius) * CellSize * (1, 1, 1) where CellGridRadius and depends on cell
+-- type to cover the whole active grid).
+
 ---
 -- Find path over navigation mesh from source to destination with given options. Result is unstable since navigation
 -- mesh generation is asynchronous.
@@ -234,4 +245,22 @@
 --     agentBounds = Actor.getPathfindingAgentBounds(self),
 -- })
 
+---
+-- Finds a nearest position on navigation mesh to the given position within given search area.
+-- @function [parent=#nearby] findNearestNavMeshPosition
+-- @param openmw.util#Vector3 position Search area center.
+-- @param #FindNearestNavMeshPositionOptions options An optional table with additional optional arguments.
+-- @return openmw.util#Vector3, #nil
+-- @usage local navMeshPosition = nearby.findNearestNavMeshPosition(position)
+-- @usage local navMeshPosition = nearby.findNearestNavMeshPosition(position, {
+--     includeFlags = nearby.NAVIGATOR_FLAGS.Swim,
+-- })
+-- @usage local navMeshPosition = nearby.findNearestNavMeshPosition(position, {
+--     agentBounds = Actor.getPathfindingAgentBounds(self),
+-- })
+-- @usage local navMeshPosition = nearby.findNearestNavMeshPosition(position, {
+--     searchAreaHalfExtents = util.vector3(1000, 1000, 1000),
+--     includeFlags = nearby.NAVIGATOR_FLAGS.Walk,
+-- })
+
 return nil

scripts/data/integration_tests/test_lua_api/player.lua

@@ -82,7 +82,8 @@ testing.registerLocalTest('findPath',
         }
         local status, path = nearby.findPath(src, dst, options)
         testing.expectEqual(status, nearby.FIND_PATH_STATUS.Success, 'Status')
-        testing.expectLessOrEqual((path[path:size()] - dst):length(), 1, 'Last path point')
+        testing.expectLessOrEqual((path[path:size()] - dst):length(), 1,
+            'Last path point '  .. testing.formatActualExpected(path[path:size()], dst))
     end)
 
 testing.registerLocalTest('findRandomPointAroundCircle',
@@ -94,7 +95,8 @@ testing.registerLocalTest('findRandomPointAroundCircle',
             includeFlags = nearby.NAVIGATOR_FLAGS.Walk,
         }
         local result = nearby.findRandomPointAroundCircle(position, maxRadius, options)
-        testing.expectGreaterThan((result - position):length(), 1, 'Random point')
+        testing.expectGreaterThan((result - position):length(), 1,
+            'Random point ' .. testing.formatActualExpected(result, position))
     end)
 
 testing.registerLocalTest('castNavigationRay',
@@ -106,7 +108,22 @@ testing.registerLocalTest('castNavigationRay',
             includeFlags = nearby.NAVIGATOR_FLAGS.Walk + nearby.NAVIGATOR_FLAGS.Swim,
         }
         local result = nearby.castNavigationRay(src, dst, options)
-        testing.expectLessOrEqual((result - dst):length(), 1, 'Navigation hit point')
+        testing.expectLessOrEqual((result - dst):length(), 1,
+            'Navigation hit point ' .. testing.formatActualExpected(result, dst))
+    end)
+
+testing.registerLocalTest('findNearestNavMeshPosition',
+    function()
+        local position = util.vector3(4096, 4096, 1000)
+        local options = {
+            agentBounds = types.Actor.getPathfindingAgentBounds(self),
+            includeFlags = nearby.NAVIGATOR_FLAGS.Walk + nearby.NAVIGATOR_FLAGS.Swim,
+            searchAreaHalfExtents = util.vector3(1000, 1000, 1000),
+        }
+        local result = nearby.findNearestNavMeshPosition(position, options)
+        local expected = util.vector3(4096, 4096, 872.674)
+        testing.expectLessOrEqual((result - expected):length(), 1,
+            'Navigation mesh position ' .. testing.formatActualExpected(result, expected))
     end)
 
 return {

scripts/data/integration_tests/test_lua_api/test.lua

@@ -95,6 +95,10 @@ tests = {
         initPlayer()
         testing.runLocalTest(player, 'castNavigationRay')
     end},
+    {'findNearestNavMeshPosition', function()
+        initPlayer()
+        testing.runLocalTest(player, 'findNearestNavMeshPosition')
+    end},
     {'teleport', testTeleport},
     {'getGMST', testGetGMST},
 }

scripts/data/integration_tests/testing_util/testing_util.lua

@@ -154,6 +154,10 @@ function M.expectThat(value, matcher, msg)
     end
 end
 
+function M.formatActualExpected(actual, expected)
+    return string.format('actual: %s, expected: %s', actual, expected)
+end
+
 local localTests = {}
 local localTestRunner = nil