diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp index 273246261b..b20b1cb971 100644 --- a/apps/openmw/mwmechanics/aiwander.cpp +++ b/apps/openmw/mwmechanics/aiwander.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #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& 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()) { diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp index 516f2c60f5..f8aa8f535f 100644 --- a/apps/openmw_test_suite/detournavigator/navigator.cpp +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -2,11 +2,14 @@ #include #include +#include #include #include #include +#include + #include #include @@ -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 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(-209.95985412597656, 129.89768981933594, -0.26253718137741089))); + + const auto distance = (*result - mStart).length(); + + EXPECT_EQ(distance, 85.260780334472656) << distance; + } } diff --git a/components/CMakeLists.txt b/components/CMakeLists.txt index 33a458b878..45526a5af6 100644 --- a/components/CMakeLists.txt +++ b/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 diff --git a/components/detournavigator/findrandompointaroundcircle.cpp b/components/detournavigator/findrandompointaroundcircle.cpp new file mode 100644 index 0000000000..c894e6681c --- /dev/null +++ b/components/detournavigator/findrandompointaroundcircle.cpp @@ -0,0 +1,45 @@ +#include "findrandompointaroundcircle.hpp" +#include "settings.hpp" +#include "findsmoothpath.hpp" + +#include + +#include +#include +#include + +namespace DetourNavigator +{ + boost::optional 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(); + + 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(); + + return boost::optional(resultPosition); + } +} diff --git a/components/detournavigator/findrandompointaroundcircle.hpp b/components/detournavigator/findrandompointaroundcircle.hpp new file mode 100644 index 0000000000..841508f67d --- /dev/null +++ b/components/detournavigator/findrandompointaroundcircle.hpp @@ -0,0 +1,20 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDRANDOMPOINTAROUNDCIRCLE_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDRANDOMPOINTAROUNDCIRCLE_H + +#include "flags.hpp" + +#include + +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + struct Settings; + + boost::optional findRandomPointAroundCircle(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, + const osg::Vec3f& start, const float maxRadius, const Flags includeFlags, const Settings& settings); +} + +#endif diff --git a/components/detournavigator/navigator.cpp b/components/detournavigator/navigator.cpp new file mode 100644 index 0000000000..a58a4f8768 --- /dev/null +++ b/components/detournavigator/navigator.cpp @@ -0,0 +1,20 @@ +#include "findrandompointaroundcircle.hpp" +#include "navigator.hpp" + +namespace DetourNavigator +{ + boost::optional 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(); + 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(); + return boost::optional(fromNavMeshCoordinates(settings, *result)); + } +} diff --git a/components/detournavigator/navigator.hpp b/components/detournavigator/navigator.hpp index 561b7f02b5..74daab5d7e 100644 --- a/components/detournavigator/navigator.hpp +++ b/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 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 findRandomPointAroundCircle(const osg::Vec3f& agentHalfExtents, + const osg::Vec3f& start, const float maxRadius, const Flags includeFlags) const; }; }