From b095ca6c867285e3cccb9c8675e21b5b6503d0ea Mon Sep 17 00:00:00 2001 From: elsid Date: Sat, 13 Jun 2020 01:56:04 +0200 Subject: [PATCH] Use actor speed to define area cost for pathfinding --- apps/openmw/mwmechanics/aipackage.cpp | 26 ++++++++++++++- apps/openmw/mwmechanics/aipackage.hpp | 3 ++ apps/openmw/mwmechanics/aiwander.cpp | 9 +++-- apps/openmw/mwmechanics/pathfinding.cpp | 19 ++++++----- apps/openmw/mwmechanics/pathfinding.hpp | 10 +++--- .../detournavigator/navigator.cpp | 33 ++++++++++--------- components/detournavigator/areatype.hpp | 8 +++++ components/detournavigator/findsmoothpath.hpp | 7 +++- components/detournavigator/navigator.hpp | 5 +-- 9 files changed, 84 insertions(+), 36 deletions(-) diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp index 0c06697dc..e322f57ce 100644 --- a/apps/openmw/mwmechanics/aipackage.cpp +++ b/apps/openmw/mwmechanics/aipackage.cpp @@ -114,7 +114,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& { const auto pathfindingHalfExtents = world->getPathfindingHalfExtents(actor); mPathFinder.buildPath(actor, position, dest, actor.getCell(), getPathGridGraph(actor.getCell()), - pathfindingHalfExtents, getNavigatorFlags(actor)); + pathfindingHalfExtents, getNavigatorFlags(actor), getAreaCosts(actor)); mRotateOnTheRunChecks = 3; // give priority to go directly on target if there is minimal opportunity @@ -402,3 +402,27 @@ DetourNavigator::Flags MWMechanics::AiPackage::getNavigatorFlags(const MWWorld:: return result; } + +DetourNavigator::AreaCosts MWMechanics::AiPackage::getAreaCosts(const MWWorld::Ptr& actor) const +{ + DetourNavigator::AreaCosts costs; + const DetourNavigator::Flags flags = getNavigatorFlags(actor); + const MWWorld::Class& actorClass = actor.getClass(); + + if (flags & DetourNavigator::Flag_swim) + costs.mWater = costs.mWater / actorClass.getSwimSpeed(actor); + + if (flags & DetourNavigator::Flag_walk) + { + float walkCost; + if (getTypeId() == TypeIdWander) + walkCost = 1.0 / actorClass.getWalkSpeed(actor); + else + walkCost = 1.0 / actorClass.getRunSpeed(actor); + costs.mDoor = costs.mDoor * walkCost; + costs.mPathgrid = costs.mPathgrid * walkCost; + costs.mGround = costs.mGround * walkCost; + } + + return costs; +} diff --git a/apps/openmw/mwmechanics/aipackage.hpp b/apps/openmw/mwmechanics/aipackage.hpp index c32fb93aa..477976616 100644 --- a/apps/openmw/mwmechanics/aipackage.hpp +++ b/apps/openmw/mwmechanics/aipackage.hpp @@ -4,6 +4,7 @@ #include #include +#include #include "pathfinding.hpp" #include "obstacle.hpp" @@ -167,6 +168,8 @@ namespace MWMechanics DetourNavigator::Flags getNavigatorFlags(const MWWorld::Ptr& actor) const; + DetourNavigator::AreaCosts getAreaCosts(const MWWorld::Ptr& actor) const; + const TypeId mTypeId; const Options mOptions; diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp index 015859c4b..11c50dc09 100644 --- a/apps/openmw/mwmechanics/aiwander.cpp +++ b/apps/openmw/mwmechanics/aiwander.cpp @@ -202,7 +202,7 @@ namespace MWMechanics { const osg::Vec3f halfExtents = MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(actor); mPathFinder.buildPath(actor, pos.asVec3(), mDestination, actor.getCell(), - getPathGridGraph(actor.getCell()), halfExtents, getNavigatorFlags(actor)); + getPathGridGraph(actor.getCell()), halfExtents, getNavigatorFlags(actor), getAreaCosts(actor)); } if (mPathFinder.isPathConstructed()) @@ -337,6 +337,7 @@ namespace MWMechanics const auto halfExtents = world->getPathfindingHalfExtents(actor); const auto navigator = world->getNavigator(); const auto navigatorFlags = getNavigatorFlags(actor); + const auto areaCosts = getAreaCosts(actor); do { // Determine a random location within radius of original position @@ -365,7 +366,8 @@ namespace MWMechanics if (isWaterCreature || isFlyingCreature) mPathFinder.buildStraightPath(mDestination); else - mPathFinder.buildPathByNavMesh(actor, currentPosition, mDestination, halfExtents, navigatorFlags); + mPathFinder.buildPathByNavMesh(actor, currentPosition, mDestination, halfExtents, navigatorFlags, + areaCosts); if (mPathFinder.isPathConstructed()) { @@ -496,7 +498,8 @@ namespace MWMechanics if (mUsePathgrid) { const auto halfExtents = MWBase::Environment::get().getWorld()->getHalfExtents(actor); - mPathFinder.buildPathByNavMeshToNextPoint(actor, halfExtents, getNavigatorFlags(actor)); + mPathFinder.buildPathByNavMeshToNextPoint(actor, halfExtents, getNavigatorFlags(actor), + getAreaCosts(actor)); } if (mObstacleCheck.isEvading()) diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index d00f2615e..4d4b5be51 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -309,12 +309,13 @@ namespace MWMechanics } void PathFinder::buildPathByNavMesh(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, - const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags) + const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, + const DetourNavigator::AreaCosts& areaCosts) { mPath.clear(); // If it's not possible to build path over navmesh due to disabled navmesh generation fallback to straight path - if (!buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, std::back_inserter(mPath))) + if (!buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, areaCosts, std::back_inserter(mPath))) mPath.push_back(endPoint); mConstructed = true; @@ -322,7 +323,7 @@ namespace MWMechanics void PathFinder::buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, - const DetourNavigator::Flags flags) + const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts) { mPath.clear(); mCell = cell; @@ -330,11 +331,11 @@ namespace MWMechanics bool hasNavMesh = false; if (!actor.getClass().isPureWaterCreature(actor) && !actor.getClass().isPureFlyingCreature(actor)) - hasNavMesh = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, std::back_inserter(mPath)); + hasNavMesh = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, areaCosts, std::back_inserter(mPath)); if (hasNavMesh && mPath.empty()) buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, - flags | DetourNavigator::Flag_usePathgrid, std::back_inserter(mPath)); + flags | DetourNavigator::Flag_usePathgrid, areaCosts, std::back_inserter(mPath)); if (mPath.empty()) buildPathByPathgridImpl(startPoint, endPoint, pathgridGraph, std::back_inserter(mPath)); @@ -347,12 +348,12 @@ namespace MWMechanics bool PathFinder::buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, - std::back_insert_iterator> out) + const DetourNavigator::AreaCosts& areaCosts, std::back_insert_iterator> out) { const auto world = MWBase::Environment::get().getWorld(); const auto stepSize = getPathStepSize(actor); const auto navigator = world->getNavigator(); - const auto status = navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, out); + const auto status = navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, areaCosts, out); if (status == DetourNavigator::Status::NavMeshNotFound) return false; @@ -369,7 +370,7 @@ namespace MWMechanics } void PathFinder::buildPathByNavMeshToNextPoint(const MWWorld::ConstPtr& actor, const osg::Vec3f& halfExtents, - const DetourNavigator::Flags flags) + const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts) { if (mPath.empty()) return; @@ -383,7 +384,7 @@ namespace MWMechanics const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); std::deque prePath; auto prePathInserter = std::back_inserter(prePath); - const auto status = navigator->findPath(halfExtents, stepSize, startPoint, mPath.front(), flags, + const auto status = navigator->findPath(halfExtents, stepSize, startPoint, mPath.front(), flags, areaCosts, prePathInserter); if (status == DetourNavigator::Status::NavMeshNotFound) diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index cb33471ca..5af822fee 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -90,14 +91,15 @@ namespace MWMechanics const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph); void buildPathByNavMesh(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, - const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags); + const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, + const DetourNavigator::AreaCosts& areaCosts); void buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, - const DetourNavigator::Flags flags); + const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts); void buildPathByNavMeshToNextPoint(const MWWorld::ConstPtr& actor, const osg::Vec3f& halfExtents, - const DetourNavigator::Flags flags); + const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts); /// Remove front point if exist and within tolerance void update(const osg::Vec3f& position, const float pointTolerance, const float destinationTolerance); @@ -203,7 +205,7 @@ namespace MWMechanics bool buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, - std::back_insert_iterator> out); + const DetourNavigator::AreaCosts& areaCosts, std::back_insert_iterator> out); }; } diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp index 276877508..5a92d5d28 100644 --- a/apps/openmw_test_suite/detournavigator/navigator.cpp +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -37,6 +37,7 @@ namespace std::deque mPath; std::back_insert_iterator> mOut; float mStepSize; + AreaCosts mAreaCosts; DetourNavigatorNavigatorTest() : mPlayerPosition(0, 0, 0) @@ -80,7 +81,7 @@ namespace TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty) { - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::NavMeshNotFound); EXPECT_EQ(mPath, std::deque()); } @@ -88,7 +89,7 @@ namespace TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception) { mNavigator->addAgent(mAgentHalfExtents); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::StartPolygonNotFound); } @@ -97,7 +98,7 @@ namespace mNavigator->addAgent(mAgentHalfExtents); mNavigator->addAgent(mAgentHalfExtents); mNavigator->removeAgent(mAgentHalfExtents); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::StartPolygonNotFound); } @@ -118,7 +119,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-215, 215, 1.85963428020477294921875), @@ -168,7 +169,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-215, 215, 1.85963428020477294921875), @@ -202,7 +203,7 @@ namespace mPath.clear(); mOut = std::back_inserter(mPath); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-215, 215, 1.87826788425445556640625), @@ -253,7 +254,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-215, 215, 1.87826788425445556640625), @@ -289,7 +290,7 @@ namespace mPath.clear(); mOut = std::back_inserter(mPath); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-215, 215, 1.85963428020477294921875), @@ -346,7 +347,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-215, 215, 1.96328866481781005859375), @@ -402,7 +403,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-215, 215, 1.9393787384033203125), @@ -455,7 +456,7 @@ namespace mEnd.x() = 0; mEnd.z() = 300; - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mAreaCosts, mOut), Status::Success); EXPECT_EQ(mPath, std::deque({ osg::Vec3f(0, 215, 185.33331298828125), @@ -501,7 +502,7 @@ namespace mStart.x() = 0; mEnd.x() = 0; - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_EQ(mPath, std::deque({ @@ -548,7 +549,7 @@ namespace mStart.x() = 0; mEnd.x() = 0; - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mOut), + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_EQ(mPath, std::deque({ @@ -595,7 +596,7 @@ namespace mStart.x() = 0; mEnd.x() = 0; - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(0, 215, -94.75363922119140625), @@ -644,7 +645,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-215, 215, 1.85963428020477294921875), @@ -739,7 +740,7 @@ namespace mNavigator->update(mPlayerPosition); mNavigator->wait(); - EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success); + EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success); EXPECT_THAT(mPath, ElementsAre( Vec3fEq(-215, 215, 1.8782780170440673828125), diff --git a/components/detournavigator/areatype.hpp b/components/detournavigator/areatype.hpp index 962a67847..9d99421af 100644 --- a/components/detournavigator/areatype.hpp +++ b/components/detournavigator/areatype.hpp @@ -13,6 +13,14 @@ namespace DetourNavigator AreaType_pathgrid, AreaType_ground = RC_WALKABLE_AREA, }; + + struct AreaCosts + { + float mWater = 1.0f; + float mDoor = 2.0f; + float mPathgrid = 1.0f; + float mGround = 1.0f; + }; } #endif diff --git a/components/detournavigator/findsmoothpath.hpp b/components/detournavigator/findsmoothpath.hpp index 0f8f2c09a..f1de71207 100644 --- a/components/detournavigator/findsmoothpath.hpp +++ b/components/detournavigator/findsmoothpath.hpp @@ -8,6 +8,7 @@ #include "settingsutils.hpp" #include "debug.hpp" #include "status.hpp" +#include "areatype.hpp" #include #include @@ -269,7 +270,7 @@ namespace DetourNavigator template Status findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, const float stepSize, - const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, + const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts, const Settings& settings, OutputIterator& out) { dtNavMeshQuery navMeshQuery; @@ -278,6 +279,10 @@ namespace DetourNavigator dtQueryFilter queryFilter; queryFilter.setIncludeFlags(includeFlags); + queryFilter.setAreaCost(AreaType_water, areaCosts.mWater); + queryFilter.setAreaCost(AreaType_door, areaCosts.mDoor); + queryFilter.setAreaCost(AreaType_pathgrid, areaCosts.mPathgrid); + queryFilter.setAreaCost(AreaType_ground, areaCosts.mGround); dtPolyRef startRef = 0; osg::Vec3f startPolygonPosition; diff --git a/components/detournavigator/navigator.hpp b/components/detournavigator/navigator.hpp index cfdf92232..391c63022 100644 --- a/components/detournavigator/navigator.hpp +++ b/components/detournavigator/navigator.hpp @@ -172,7 +172,8 @@ namespace DetourNavigator */ template Status findPath(const osg::Vec3f& agentHalfExtents, const float stepSize, const osg::Vec3f& start, - const osg::Vec3f& end, const Flags includeFlags, OutputIterator& out) const + const osg::Vec3f& end, const Flags includeFlags, const DetourNavigator::AreaCosts& areaCosts, + OutputIterator& out) const { static_assert( std::is_same< @@ -187,7 +188,7 @@ namespace DetourNavigator const auto settings = getSettings(); return findSmoothPath(navMesh->lockConst()->getImpl(), toNavMeshCoordinates(settings, agentHalfExtents), toNavMeshCoordinates(settings, stepSize), toNavMeshCoordinates(settings, start), - toNavMeshCoordinates(settings, end), includeFlags, settings, out); + toNavMeshCoordinates(settings, end), includeFlags, areaCosts, settings, out); } /**