Use navmesh to find wander destination outside pathgrid for ground based actors

Use dtNavMeshQuery::findRandomPointAroundCircle from recastnavigation

apps/openmw/mwmechanics/aiwander.cpp

@@ -3,6 +3,7 @@
 #include <components/debug/debuglog.hpp>
 #include <components/misc/rng.hpp>
 #include <components/esm/aisequence.hpp>
+#include <components/detournavigator/navigator.hpp>
 
 #include "../mwbase/world.hpp"
 #include "../mwbase/environment.hpp"
@@ -52,6 +53,14 @@ namespace MWMechanics
                 return 1;
             return COUNT_BEFORE_RESET;
         }
+
+        osg::Vec3f getRandomPointAround(const osg::Vec3f& position, const float distance)
+        {
+            const float randomDirection = Misc::Rng::rollClosedProbability() * 2.0f * osg::PI;
+            osg::Matrixf rotation;
+            rotation.makeRotate(randomDirection, osg::Vec3f(0.0, 0.0, 1.0));
+            return position + osg::Vec3f(distance, 0.0, 0.0) * rotation;
+        }
     }
 
     AiWander::AiWander(int distance, int duration, int timeOfDay, const std::vector<unsigned char>& idle, bool repeat):
@@ -310,14 +319,24 @@ namespace MWMechanics
         std::size_t attempts = 10; // If a unit can't wander out of water, don't want to hang here
         const bool isWaterCreature = actor.getClass().isPureWaterCreature(actor);
         const bool isFlyingCreature = actor.getClass().isPureFlyingCreature(actor);
+        const auto world = MWBase::Environment::get().getWorld();
+        const auto halfExtents = world->getPathfindingHalfExtents(actor);
+        const auto navigator = world->getNavigator();
+        const auto navigatorFlags = getNavigatorFlags(actor);
+
         do {
             // Determine a random location within radius of original position
             const float wanderRadius = (0.2f + Misc::Rng::rollClosedProbability() * 0.8f) * wanderDistance;
-            const float randomDirection = Misc::Rng::rollClosedProbability() * 2.0f * osg::PI;
-            const float destinationX = mInitialActorPosition.x() + wanderRadius * std::cos(randomDirection);
-            const float destinationY = mInitialActorPosition.y() + wanderRadius * std::sin(randomDirection);
-            const float destinationZ = mInitialActorPosition.z();
-            mDestination = osg::Vec3f(destinationX, destinationY, destinationZ);
+            if (!isWaterCreature && !isFlyingCreature)
+            {
+                // findRandomPointAroundCircle uses wanderDistance as limit for random and not as exact distance
+                if (const auto destination = navigator->findRandomPointAroundCircle(halfExtents, currentPosition, wanderDistance, navigatorFlags))
+                    mDestination = *destination;
+                else
+                    mDestination = getRandomPointAround(mInitialActorPosition, wanderRadius);
+            }
+            else
+                mDestination = getRandomPointAround(mInitialActorPosition, wanderRadius);
 
             // Check if land creature will walk onto water or if water creature will swim onto land
             if (!isWaterCreature && destinationIsAtWater(actor, mDestination))
@@ -327,15 +346,9 @@ namespace MWMechanics
                 continue;
 
             if (isWaterCreature || isFlyingCreature)
-            {
                 mPathFinder.buildStraightPath(mDestination);
-            }
             else
-            {
-                const osg::Vec3f halfExtents = MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(actor);
-                mPathFinder.buildPathByNavMesh(actor, currentPosition, mDestination, halfExtents,
-                    getNavigatorFlags(actor));
-            }
+                mPathFinder.buildPathByNavMesh(actor, currentPosition, mDestination, halfExtents, navigatorFlags);
 
             if (mPathFinder.isPathConstructed())
             {

apps/openmw_test_suite/detournavigator/navigator.cpp

@@ -2,11 +2,14 @@
 
 #include <components/detournavigator/navigatorimpl.hpp>
 #include <components/detournavigator/exceptions.hpp>
+#include <components/misc/rng.hpp>
 
 #include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
 #include <BulletCollision/CollisionShapes/btBoxShape.h>
 #include <BulletCollision/CollisionShapes/btCompoundShape.h>
 
+#include <boost/optional/optional_io.hpp>
+
 #include <gtest/gtest.h>
 
 #include <iterator>
@@ -655,4 +658,32 @@ namespace
             osg::Vec3f(215, -215, 1.87718021869659423828125),
         })) << mPath;
     }
+
+    TEST_F(DetourNavigatorNavigatorTest, update_then_find_random_point_around_circle_should_return_position)
+    {
+        const std::array<btScalar, 5 * 5> heightfieldData {{
+            0,   0,    0,    0,    0,
+            0, -25,  -25,  -25,  -25,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+        }};
+        btHeightfieldTerrainShape shape(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false);
+        shape.setLocalScaling(btVector3(128, 128, 1));
+
+        mNavigator->addAgent(mAgentHalfExtents);
+        mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity());
+        mNavigator->update(mPlayerPosition);
+        mNavigator->wait();
+
+        Misc::Rng::init(42);
+
+        const auto result = mNavigator->findRandomPointAroundCircle(mAgentHalfExtents, mStart, 100.0, Flag_walk);
+
+        ASSERT_EQ(result, boost::optional<osg::Vec3f>(osg::Vec3f(-209.95985412597656, 129.89768981933594, -0.26253718137741089)));
+
+        const auto distance = (*result - mStart).length();
+
+        EXPECT_EQ(distance, 85.260780334472656) << distance;
+    }
 }

components/CMakeLists.txt

@@ -172,6 +172,8 @@ add_component_dir(detournavigator
     recastmeshobject
     navmeshtilescache
     settings
+    navigator
+    findrandompointaroundcircle
     )
 
 set (ESM_UI ${CMAKE_SOURCE_DIR}/files/ui/contentselector.ui

components/detournavigator/findrandompointaroundcircle.cpp

@@ -0,0 +1,45 @@
+#include "findrandompointaroundcircle.hpp"
+#include "settings.hpp"
+#include "findsmoothpath.hpp"
+
+#include <components/misc/rng.hpp>
+
+#include <DetourCommon.h>
+#include <DetourNavMesh.h>
+#include <DetourNavMeshQuery.h>
+
+namespace DetourNavigator
+{
+    boost::optional<osg::Vec3f> findRandomPointAroundCircle(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents,
+        const osg::Vec3f& start, const float maxRadius, const Flags includeFlags, const Settings& settings)
+    {
+        dtNavMeshQuery navMeshQuery;
+        initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes);
+
+        dtQueryFilter queryFilter;
+        queryFilter.setIncludeFlags(includeFlags);
+
+        dtPolyRef startRef = 0;
+        osg::Vec3f startPolygonPosition;
+        for (int i = 0; i < 3; ++i)
+        {
+            const auto status = navMeshQuery.findNearestPoly(start.ptr(), (halfExtents * (1 << i)).ptr(), &queryFilter,
+                &startRef, startPolygonPosition.ptr());
+            if (!dtStatusFailed(status) && startRef != 0)
+                break;
+        }
+
+        if (startRef == 0)
+            return boost::optional<osg::Vec3f>();
+
+        dtPolyRef resultRef = 0;
+        osg::Vec3f resultPosition;
+        navMeshQuery.findRandomPointAroundCircle(startRef, start.ptr(), maxRadius, &queryFilter,
+            &Misc::Rng::rollProbability, &resultRef, resultPosition.ptr());
+
+        if (resultRef == 0)
+            return boost::optional<osg::Vec3f>();
+
+        return boost::optional<osg::Vec3f>(resultPosition);
+    }
+}

components/detournavigator/findrandompointaroundcircle.hpp

@@ -0,0 +1,20 @@
+#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDRANDOMPOINTAROUNDCIRCLE_H
+#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDRANDOMPOINTAROUNDCIRCLE_H
+
+#include "flags.hpp"
+
+#include <boost/optional.hpp>
+
+#include <osg/Vec3f>
+
+class dtNavMesh;
+
+namespace DetourNavigator
+{
+    struct Settings;
+
+    boost::optional<osg::Vec3f> findRandomPointAroundCircle(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents,
+        const osg::Vec3f& start, const float maxRadius, const Flags includeFlags, const Settings& settings);
+}
+
+#endif

components/detournavigator/navigator.cpp

@@ -0,0 +1,20 @@
+#include "findrandompointaroundcircle.hpp"
+#include "navigator.hpp"
+
+namespace DetourNavigator
+{
+    boost::optional<osg::Vec3f> Navigator::findRandomPointAroundCircle(const osg::Vec3f& agentHalfExtents,
+        const osg::Vec3f& start, const float maxRadius, const Flags includeFlags) const
+    {
+        const auto navMesh = getNavMesh(agentHalfExtents);
+        if (!navMesh)
+            return boost::optional<osg::Vec3f>();
+        const auto settings = getSettings();
+        const auto result = DetourNavigator::findRandomPointAroundCircle(navMesh->lockConst()->getImpl(),
+            toNavMeshCoordinates(settings, agentHalfExtents), toNavMeshCoordinates(settings, start),
+            toNavMeshCoordinates(settings, maxRadius), includeFlags, settings);
+        if (!result)
+            return boost::optional<osg::Vec3f>();
+        return boost::optional<osg::Vec3f>(fromNavMeshCoordinates(settings, *result));
+    }
+}

components/detournavigator/navigator.hpp

@@ -157,7 +157,6 @@ namespace DetourNavigator
          * @param out the beginning of the destination range.
          * @return Output iterator to the element in the destination range, one past the last element of found path.
          * Equal to out if no path is found.
-         * @throws InvalidArgument if there is no navmesh for given agentHalfExtents.
          */
         template <class OutputIterator>
         OutputIterator findPath(const osg::Vec3f& agentHalfExtents, const float stepSize, const osg::Vec3f& start,
@@ -194,6 +193,17 @@ namespace DetourNavigator
         virtual const Settings& getSettings() const = 0;
 
         virtual void reportStats(unsigned int frameNumber, osg::Stats& stats) const = 0;
+
+        /**
+         * @brief findRandomPointAroundCircle returns random location on navmesh within the reach of specified location.
+         * @param agentHalfExtents allows to find navmesh for given actor.
+         * @param start path from given point.
+         * @param maxRadius limit maximum distance from start.
+         * @param includeFlags setup allowed surfaces for actor to walk.
+         * @return not empty optional with position if point is found and empty optional if point is not found.
+         */
+        boost::optional<osg::Vec3f> findRandomPointAroundCircle(const osg::Vec3f& agentHalfExtents,
+            const osg::Vec3f& start, const float maxRadius, const Flags includeFlags) const;
     };
 }