mirror of
https://github.com/OpenMW/openmw.git
synced 2025-01-16 18:59:57 +00:00
Make AiPursue path destination to be as close as possible to target
Even when target is not reachable actor will try to run there either because target navmesh polygon is selected within extended area or because partial path is built to the closest possible polygon.
This commit is contained in:
elsid
parent
6360bdc859
commit
fea4fb6e69
14 changed files with 204 additions and 58 deletions
|
|
@ -269,7 +269,8 @@ namespace MWMechanics
|
|||
const auto navigatorFlags = getNavigatorFlags(actor);
|
||||
const auto areaCosts = getAreaCosts(actor);
|
||||
const auto pathGridGraph = getPathGridGraph(actor.getCell());
|
||||
mPathFinder.buildPath(actor, vActorPos, vTargetPos, actor.getCell(), pathGridGraph, halfExtents, navigatorFlags, areaCosts);
|
||||
mPathFinder.buildPath(actor, vActorPos, vTargetPos, actor.getCell(), pathGridGraph, halfExtents,
|
||||
navigatorFlags, areaCosts, storage.mAttackRange, PathType::Full);
|
||||
|
||||
if (!mPathFinder.isPathConstructed())
|
||||
{
|
||||
|
|
@ -281,7 +282,8 @@ namespace MWMechanics
|
|||
if (hit.has_value() && (*hit - vTargetPos).length() <= rangeAttack)
|
||||
{
|
||||
// If the point is close enough, try to find a path to that point.
|
||||
mPathFinder.buildPath(actor, vActorPos, *hit, actor.getCell(), pathGridGraph, halfExtents, navigatorFlags, areaCosts);
|
||||
mPathFinder.buildPath(actor, vActorPos, *hit, actor.getCell(), pathGridGraph, halfExtents,
|
||||
navigatorFlags, areaCosts, storage.mAttackRange, PathType::Full);
|
||||
if (mPathFinder.isPathConstructed())
|
||||
{
|
||||
// If path to that point is found use it as custom destination.
|
||||
|
|
|
|||
|
|
@ -88,7 +88,8 @@ void MWMechanics::AiPackage::reset()
|
|||
mObstacleCheck.clear();
|
||||
}
|
||||
|
||||
bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance)
|
||||
bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration,
|
||||
float destTolerance, float endTolerance, PathType pathType)
|
||||
{
|
||||
const Misc::TimerStatus timerStatus = mReaction.update(duration);
|
||||
|
||||
|
|
@ -133,7 +134,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
|
|||
{
|
||||
const auto pathfindingHalfExtents = world->getPathfindingHalfExtents(actor);
|
||||
mPathFinder.buildLimitedPath(actor, position, dest, actor.getCell(), getPathGridGraph(actor.getCell()),
|
||||
pathfindingHalfExtents, getNavigatorFlags(actor), getAreaCosts(actor));
|
||||
pathfindingHalfExtents, getNavigatorFlags(actor), getAreaCosts(actor), endTolerance, pathType);
|
||||
mRotateOnTheRunChecks = 3;
|
||||
|
||||
// give priority to go directly on target if there is minimal opportunity
|
||||
|
|
|
|||
|
|
@ -129,7 +129,8 @@ namespace MWMechanics
|
|||
protected:
|
||||
/// Handles path building and shortcutting with obstacles avoiding
|
||||
/** \return If the actor has arrived at his destination **/
|
||||
bool pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance = 0.0f);
|
||||
bool pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration,
|
||||
float destTolerance = 0.0f, float endTolerance = 0.0f, PathType pathType = PathType::Full);
|
||||
|
||||
/// Check if there aren't any obstacles along the path to make shortcut possible
|
||||
/// If a shortcut is possible then path will be cleared and filled with the destination point.
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ bool AiPursue::execute (const MWWorld::Ptr& actor, CharacterController& characte
|
|||
const float pathTolerance = 100.f;
|
||||
|
||||
// check the true distance in case the target is far away in Z-direction
|
||||
bool reached = pathTo(actor, dest, duration, pathTolerance) &&
|
||||
bool reached = pathTo(actor, dest, duration, pathTolerance, (actorPos - dest).length(), PathType::Partial) &&
|
||||
std::abs(dest.z() - actorPos.z()) < pathTolerance;
|
||||
|
||||
if (reached)
|
||||
|
|
|
|||
|
|
@ -201,8 +201,10 @@ namespace MWMechanics
|
|||
else
|
||||
{
|
||||
const osg::Vec3f halfExtents = MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(actor);
|
||||
constexpr float endTolerance = 0;
|
||||
mPathFinder.buildPath(actor, pos.asVec3(), mDestination, actor.getCell(),
|
||||
getPathGridGraph(actor.getCell()), halfExtents, getNavigatorFlags(actor), getAreaCosts(actor));
|
||||
getPathGridGraph(actor.getCell()), halfExtents, getNavigatorFlags(actor), getAreaCosts(actor),
|
||||
endTolerance, PathType::Full);
|
||||
}
|
||||
|
||||
if (mPathFinder.isPathConstructed())
|
||||
|
|
@ -361,11 +363,13 @@ namespace MWMechanics
|
|||
if (isAreaOccupiedByOtherActor(actor, mDestination))
|
||||
continue;
|
||||
|
||||
constexpr float endTolerance = 0;
|
||||
|
||||
if (isWaterCreature || isFlyingCreature)
|
||||
mPathFinder.buildStraightPath(mDestination);
|
||||
else
|
||||
mPathFinder.buildPathByNavMesh(actor, currentPosition, mDestination, halfExtents, navigatorFlags,
|
||||
areaCosts);
|
||||
areaCosts, endTolerance, PathType::Full);
|
||||
|
||||
if (mPathFinder.isPathConstructed())
|
||||
{
|
||||
|
|
|
|||
|
|
@ -364,13 +364,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 DetourNavigator::AreaCosts& areaCosts)
|
||||
const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType)
|
||||
{
|
||||
mPath.clear();
|
||||
|
||||
// If it's not possible to build path over navmesh due to disabled navmesh generation fallback to straight path
|
||||
DetourNavigator::Status status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags,
|
||||
areaCosts, std::back_inserter(mPath));
|
||||
areaCosts, endTolerance, pathType, std::back_inserter(mPath));
|
||||
|
||||
if (status != DetourNavigator::Status::Success)
|
||||
mPath.clear();
|
||||
|
|
@ -383,7 +383,8 @@ 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::AreaCosts& areaCosts)
|
||||
const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
|
||||
PathType pathType)
|
||||
{
|
||||
mPath.clear();
|
||||
mCell = cell;
|
||||
|
|
@ -392,7 +393,8 @@ namespace MWMechanics
|
|||
|
||||
if (!actor.getClass().isPureWaterCreature(actor) && !actor.getClass().isPureFlyingCreature(actor))
|
||||
{
|
||||
status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, areaCosts, std::back_inserter(mPath));
|
||||
status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, areaCosts,
|
||||
endTolerance, pathType, std::back_inserter(mPath));
|
||||
if (status != DetourNavigator::Status::Success)
|
||||
mPath.clear();
|
||||
}
|
||||
|
|
@ -400,7 +402,7 @@ namespace MWMechanics
|
|||
if (status != DetourNavigator::Status::NavMeshNotFound && mPath.empty())
|
||||
{
|
||||
status = buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents,
|
||||
flags | DetourNavigator::Flag_usePathgrid, areaCosts, std::back_inserter(mPath));
|
||||
flags | DetourNavigator::Flag_usePathgrid, areaCosts, endTolerance, pathType, std::back_inserter(mPath));
|
||||
if (status != DetourNavigator::Status::Success)
|
||||
mPath.clear();
|
||||
}
|
||||
|
|
@ -416,12 +418,17 @@ namespace MWMechanics
|
|||
|
||||
DetourNavigator::Status PathFinder::buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint,
|
||||
const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags,
|
||||
const DetourNavigator::AreaCosts& areaCosts, std::back_insert_iterator<std::deque<osg::Vec3f>> out)
|
||||
const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType,
|
||||
std::back_insert_iterator<std::deque<osg::Vec3f>> 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, areaCosts, out);
|
||||
const auto status = navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, areaCosts,
|
||||
endTolerance, out);
|
||||
|
||||
if (pathType == PathType::Partial && status == DetourNavigator::Status::PartialPath)
|
||||
return DetourNavigator::Status::Success;
|
||||
|
||||
if (status != DetourNavigator::Status::Success)
|
||||
{
|
||||
|
|
@ -449,8 +456,9 @@ namespace MWMechanics
|
|||
const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
|
||||
std::deque<osg::Vec3f> prePath;
|
||||
auto prePathInserter = std::back_inserter(prePath);
|
||||
const float endTolerance = 0;
|
||||
const auto status = navigator->findPath(halfExtents, stepSize, startPoint, mPath.front(), flags, areaCosts,
|
||||
prePathInserter);
|
||||
endTolerance, prePathInserter);
|
||||
|
||||
if (status == DetourNavigator::Status::NavMeshNotFound)
|
||||
return;
|
||||
|
|
@ -475,7 +483,8 @@ namespace MWMechanics
|
|||
|
||||
void PathFinder::buildLimitedPath(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::AreaCosts& areaCosts)
|
||||
const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
|
||||
PathType pathType)
|
||||
{
|
||||
const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
|
||||
const auto maxDistance = std::min(
|
||||
|
|
@ -485,8 +494,9 @@ namespace MWMechanics
|
|||
const auto startToEnd = endPoint - startPoint;
|
||||
const auto distance = startToEnd.length();
|
||||
if (distance <= maxDistance)
|
||||
return buildPath(actor, startPoint, endPoint, cell, pathgridGraph, halfExtents, flags, areaCosts);
|
||||
return buildPath(actor, startPoint, endPoint, cell, pathgridGraph, halfExtents, flags, areaCosts,
|
||||
endTolerance, pathType);
|
||||
const auto end = startPoint + startToEnd * maxDistance / distance;
|
||||
buildPath(actor, startPoint, end, cell, pathgridGraph, halfExtents, flags, areaCosts);
|
||||
buildPath(actor, startPoint, end, cell, pathgridGraph, halfExtents, flags, areaCosts, endTolerance, pathType);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -70,6 +70,12 @@ namespace MWMechanics
|
|||
// magnitude of pits/obstacles is defined by PATHFIND_Z_REACH
|
||||
bool checkWayIsClear(const osg::Vec3f& from, const osg::Vec3f& to, float offsetXY);
|
||||
|
||||
enum class PathType
|
||||
{
|
||||
Full,
|
||||
Partial,
|
||||
};
|
||||
|
||||
class PathFinder
|
||||
{
|
||||
public:
|
||||
|
|
@ -93,18 +99,20 @@ namespace MWMechanics
|
|||
|
||||
void buildPathByNavMesh(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint,
|
||||
const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags,
|
||||
const DetourNavigator::AreaCosts& areaCosts);
|
||||
const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType);
|
||||
|
||||
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::AreaCosts& areaCosts);
|
||||
const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
|
||||
PathType pathType);
|
||||
|
||||
void buildPathByNavMeshToNextPoint(const MWWorld::ConstPtr& actor, const osg::Vec3f& halfExtents,
|
||||
const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts);
|
||||
|
||||
void buildLimitedPath(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::AreaCosts& areaCosts);
|
||||
const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance,
|
||||
PathType pathType);
|
||||
|
||||
/// Remove front point if exist and within tolerance
|
||||
void update(const osg::Vec3f& position, float pointTolerance, float destinationTolerance,
|
||||
|
|
@ -212,7 +220,7 @@ namespace MWMechanics
|
|||
|
||||
[[nodiscard]] DetourNavigator::Status buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor,
|
||||
const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents,
|
||||
const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts,
|
||||
const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts, float endTolerance, PathType pathType,
|
||||
std::back_insert_iterator<std::deque<osg::Vec3f>> out);
|
||||
};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -46,6 +46,7 @@ namespace
|
|||
const osg::Vec2i mCellPosition {0, 0};
|
||||
const int mHeightfieldTileSize = ESM::Land::REAL_SIZE / (ESM::Land::LAND_SIZE - 1);
|
||||
const osg::Vec3f mShift {0, 0, 0};
|
||||
const float mEndTolerance = 0;
|
||||
|
||||
DetourNavigatorNavigatorTest()
|
||||
: mPlayerPosition(0, 0, 0)
|
||||
|
|
@ -135,7 +136,7 @@ namespace
|
|||
|
||||
TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty)
|
||||
{
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut),
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::NavMeshNotFound);
|
||||
EXPECT_EQ(mPath, std::deque<osg::Vec3f>());
|
||||
}
|
||||
|
|
@ -143,7 +144,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, mAreaCosts, mOut),
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::StartPolygonNotFound);
|
||||
}
|
||||
|
||||
|
|
@ -152,7 +153,7 @@ namespace
|
|||
mNavigator->addAgent(mAgentHalfExtents);
|
||||
mNavigator->addAgent(mAgentHalfExtents);
|
||||
mNavigator->removeAgent(mAgentHalfExtents);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut),
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::StartPolygonNotFound);
|
||||
}
|
||||
|
||||
|
|
@ -172,7 +173,8 @@ namespace
|
|||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204.0000152587890625, 204, 1.99998295307159423828125),
|
||||
|
|
@ -219,7 +221,8 @@ namespace
|
|||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::allJobsDone);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 1.99998295307159423828125),
|
||||
|
|
@ -252,7 +255,8 @@ namespace
|
|||
|
||||
mPath.clear();
|
||||
mOut = std::back_inserter(mPath);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 1.99998295307159423828125),
|
||||
|
|
@ -301,7 +305,8 @@ namespace
|
|||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::allJobsDone);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 1.99998295307159423828125),
|
||||
|
|
@ -337,7 +342,8 @@ namespace
|
|||
|
||||
mPath.clear();
|
||||
mOut = std::back_inserter(mPath);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 1.99998295307159423828125),
|
||||
|
|
@ -393,7 +399,8 @@ namespace
|
|||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::allJobsDone);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 1.999981403350830078125),
|
||||
|
|
@ -479,7 +486,8 @@ namespace
|
|||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::allJobsDone);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 1.99997997283935546875),
|
||||
|
|
@ -530,7 +538,8 @@ namespace
|
|||
mEnd.x() = 0;
|
||||
mEnd.z() = 300;
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(0, 204, 185.33331298828125),
|
||||
|
|
@ -574,7 +583,7 @@ namespace
|
|||
mStart.x() = 0;
|
||||
mEnd.x() = 0;
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mOut),
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
|
|
@ -619,7 +628,7 @@ namespace
|
|||
mStart.x() = 0;
|
||||
mEnd.x() = 0;
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mOut),
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
|
|
@ -664,7 +673,8 @@ namespace
|
|||
mStart.x() = 0;
|
||||
mEnd.x() = 0;
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(0, 204, -98.000030517578125),
|
||||
|
|
@ -712,7 +722,8 @@ namespace
|
|||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::allJobsDone);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 1.99998295307159423828125),
|
||||
|
|
@ -764,7 +775,8 @@ namespace
|
|||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::allJobsDone);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 1.99998295307159423828125),
|
||||
|
|
@ -858,7 +870,8 @@ namespace
|
|||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::allJobsDone);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 1.99998295307159423828125),
|
||||
|
|
@ -1006,7 +1019,8 @@ namespace
|
|||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::requiredTilesPresent);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mOut), Status::Success);
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, 101.99999237060546875),
|
||||
|
|
@ -1033,4 +1047,98 @@ namespace
|
|||
Vec3fEq(204, -204, 101.99999237060546875)
|
||||
)) << mPath;
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorNavigatorTest, for_not_reachable_destination_find_path_should_provide_partial_path)
|
||||
{
|
||||
const std::array<float, 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,
|
||||
}};
|
||||
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
|
||||
|
||||
CollisionShapeInstance compound(std::make_unique<btCompoundShape>());
|
||||
compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(204, -204, 0)),
|
||||
new btBoxShape(btVector3(200, 200, 1000)));
|
||||
|
||||
mNavigator->addAgent(mAgentHalfExtents);
|
||||
mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
|
||||
mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity());
|
||||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::allJobsDone);
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, mEndTolerance, mOut),
|
||||
Status::PartialPath);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, -2.666739940643310546875),
|
||||
Vec3fEq(-193.730682373046875, 177.59320068359375, -3.9535934925079345703125),
|
||||
Vec3fEq(-183.4613800048828125, 151.1864166259765625, -5.240451335906982421875),
|
||||
Vec3fEq(-173.1920623779296875, 124.77962493896484375, -6.527309417724609375),
|
||||
Vec3fEq(-162.922760009765625, 98.37282562255859375, -7.814167022705078125),
|
||||
Vec3fEq(-152.6534423828125, 71.96602630615234375, -9.898590087890625),
|
||||
Vec3fEq(-142.384124755859375, 45.559230804443359375, -17.641445159912109375),
|
||||
Vec3fEq(-132.1148223876953125, 19.152431488037109375, -25.3843059539794921875),
|
||||
Vec3fEq(-121.8455047607421875, -7.254369258880615234375, -27.97742462158203125),
|
||||
Vec3fEq(-111.57619476318359375, -33.66117095947265625, -16.974590301513671875),
|
||||
Vec3fEq(-101.30689239501953125, -60.06797027587890625, -5.9717559814453125),
|
||||
Vec3fEq(-91.0375823974609375, -86.47476959228515625, -2.6667339801788330078125),
|
||||
Vec3fEq(-80.76827239990234375, -112.88156890869140625, -2.6667339801788330078125),
|
||||
Vec3fEq(-70.49897003173828125, -139.2883758544921875, -2.6667339801788330078125),
|
||||
Vec3fEq(-60.229663848876953125, -165.6951751708984375, -2.6667339801788330078125),
|
||||
Vec3fEq(-49.96035003662109375, -192.1019744873046875, -2.6667339801788330078125),
|
||||
Vec3fEq(-45.333343505859375, -204, -2.6667339801788330078125)
|
||||
)) << mPath;
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorNavigatorTest, end_tolerance_should_extent_available_destinations)
|
||||
{
|
||||
const std::array<float, 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,
|
||||
}};
|
||||
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
|
||||
|
||||
CollisionShapeInstance compound(std::make_unique<btCompoundShape>());
|
||||
compound.shape().addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(204, -204, 0)),
|
||||
new btBoxShape(btVector3(100, 100, 1000)));
|
||||
|
||||
mNavigator->addAgent(mAgentHalfExtents);
|
||||
mNavigator->addHeightfield(mCellPosition, mHeightfieldTileSize * (surface.mSize - 1), mShift, surface);
|
||||
mNavigator->addObject(ObjectId(&compound.shape()), ObjectShapes(compound.instance()), btTransform::getIdentity());
|
||||
mNavigator->update(mPlayerPosition);
|
||||
mNavigator->wait(mListener, WaitConditionType::allJobsDone);
|
||||
|
||||
const float endTolerance = 1000.0f;
|
||||
|
||||
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mAreaCosts, endTolerance, mOut),
|
||||
Status::Success);
|
||||
|
||||
EXPECT_THAT(mPath, ElementsAre(
|
||||
Vec3fEq(-204, 204, -2.666739940643310546875),
|
||||
Vec3fEq(-188.745635986328125, 180.1236114501953125, -4.578275203704833984375),
|
||||
Vec3fEq(-173.49127197265625, 156.247222900390625, -6.489814281463623046875),
|
||||
Vec3fEq(-158.2369232177734375, 132.370819091796875, -8.4013538360595703125),
|
||||
Vec3fEq(-142.9825592041015625, 108.49443817138671875, -10.31289386749267578125),
|
||||
Vec3fEq(-127.7281951904296875, 84.6180419921875, -17.4810466766357421875),
|
||||
Vec3fEq(-112.47383880615234375, 60.7416534423828125, -27.6026020050048828125),
|
||||
Vec3fEq(-97.21947479248046875, 36.865261077880859375, -37.724163055419921875),
|
||||
Vec3fEq(-81.965118408203125, 12.98886966705322265625, -47.84572601318359375),
|
||||
Vec3fEq(-66.71075439453125, -10.887523651123046875, -46.691577911376953125),
|
||||
Vec3fEq(-51.45639801025390625, -34.763916015625, -32.085445404052734375),
|
||||
Vec3fEq(-36.202037811279296875, -58.640308380126953125, -28.5217914581298828125),
|
||||
Vec3fEq(-20.947673797607421875, -82.5167083740234375, -32.16143035888671875),
|
||||
Vec3fEq(-5.693310260772705078125, -106.393096923828125, -35.8010711669921875),
|
||||
Vec3fEq(9.56105327606201171875, -130.2694854736328125, -29.6399688720703125),
|
||||
Vec3fEq(24.8154163360595703125, -154.1458740234375, -17.6428318023681640625),
|
||||
Vec3fEq(40.0697784423828125, -178.0222625732421875, -10.46006107330322265625),
|
||||
Vec3fEq(55.3241424560546875, -201.8986663818359375, -3.297139644622802734375),
|
||||
Vec3fEq(56.66666412353515625, -204, -2.6667373180389404296875)
|
||||
)) << mPath;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@ namespace DetourNavigator
|
|||
switch (value)
|
||||
{
|
||||
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(Success)
|
||||
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(PartialPath)
|
||||
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(NavMeshNotFound)
|
||||
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(StartPolygonNotFound)
|
||||
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(EndPolygonNotFound)
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@ namespace DetourNavigator
|
|||
dtQueryFilter queryFilter;
|
||||
queryFilter.setIncludeFlags(includeFlags);
|
||||
|
||||
dtPolyRef startRef = findNearestPolyExpanding(navMeshQuery, queryFilter, start, halfExtents);
|
||||
dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, halfExtents * 4);
|
||||
if (startRef == 0)
|
||||
return std::optional<osg::Vec3f>();
|
||||
|
||||
|
|
|
|||
|
|
@ -146,16 +146,13 @@ namespace DetourNavigator
|
|||
return result;
|
||||
}
|
||||
|
||||
dtPolyRef findNearestPolyExpanding(const dtNavMeshQuery& query, const dtQueryFilter& filter,
|
||||
dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter,
|
||||
const osg::Vec3f& center, const osg::Vec3f& halfExtents)
|
||||
{
|
||||
dtPolyRef ref = 0;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
const dtStatus status = query.findNearestPoly(center.ptr(), (halfExtents * (1 << i)).ptr(), &filter, &ref, nullptr);
|
||||
if (!dtStatusFailed(status) && ref != 0)
|
||||
break;
|
||||
}
|
||||
const dtStatus status = query.findNearestPoly(center.ptr(), halfExtents.ptr(), &filter, &ref, nullptr);
|
||||
if (!dtStatusSucceed(status))
|
||||
return 0;
|
||||
return ref;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -99,7 +99,7 @@ namespace DetourNavigator
|
|||
return dtStatusSucceed(status);
|
||||
}
|
||||
|
||||
dtPolyRef findNearestPolyExpanding(const dtNavMeshQuery& query, const dtQueryFilter& filter,
|
||||
dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter,
|
||||
const osg::Vec3f& center, const osg::Vec3f& halfExtents);
|
||||
|
||||
struct MoveAlongSurfaceResult
|
||||
|
|
@ -256,7 +256,7 @@ namespace DetourNavigator
|
|||
template <class OutputIterator>
|
||||
Status findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, const float stepSize,
|
||||
const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts,
|
||||
const Settings& settings, OutputIterator& out)
|
||||
const Settings& settings, float endTolerance, OutputIterator& out)
|
||||
{
|
||||
dtNavMeshQuery navMeshQuery;
|
||||
if (!initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes))
|
||||
|
|
@ -269,11 +269,15 @@ namespace DetourNavigator
|
|||
queryFilter.setAreaCost(AreaType_pathgrid, areaCosts.mPathgrid);
|
||||
queryFilter.setAreaCost(AreaType_ground, areaCosts.mGround);
|
||||
|
||||
dtPolyRef startRef = findNearestPolyExpanding(navMeshQuery, queryFilter, start, halfExtents);
|
||||
constexpr float polyDistanceFactor = 4;
|
||||
const osg::Vec3f polyHalfExtents = halfExtents * polyDistanceFactor;
|
||||
|
||||
const dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, polyHalfExtents);
|
||||
if (startRef == 0)
|
||||
return Status::StartPolygonNotFound;
|
||||
|
||||
dtPolyRef endRef = findNearestPolyExpanding(navMeshQuery, queryFilter, end, halfExtents);
|
||||
const dtPolyRef endRef = findNearestPoly(navMeshQuery, queryFilter, end,
|
||||
polyHalfExtents + osg::Vec3f(endTolerance, endTolerance, endTolerance));
|
||||
if (endRef == 0)
|
||||
return Status::EndPolygonNotFound;
|
||||
|
||||
|
|
@ -283,12 +287,18 @@ namespace DetourNavigator
|
|||
if (!polygonPath)
|
||||
return Status::FindPathOverPolygonsFailed;
|
||||
|
||||
if (polygonPath->empty() || polygonPath->back() != endRef)
|
||||
if (polygonPath->empty())
|
||||
return Status::Success;
|
||||
|
||||
const bool partialPath = polygonPath->back() != endRef;
|
||||
auto outTransform = OutputTransformIterator<OutputIterator>(out, settings);
|
||||
return makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize, std::move(*polygonPath),
|
||||
settings.mMaxSmoothPathSize, outTransform);
|
||||
const Status smoothStatus = makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize,
|
||||
std::move(*polygonPath), settings.mMaxSmoothPathSize, outTransform);
|
||||
|
||||
if (smoothStatus != Status::Success)
|
||||
return smoothStatus;
|
||||
|
||||
return partialPath ? Status::PartialPath : Status::Success;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -173,13 +173,14 @@ namespace DetourNavigator
|
|||
* @param end path at given point.
|
||||
* @param includeFlags setup allowed surfaces for actor to walk.
|
||||
* @param out the beginning of the destination range.
|
||||
* @param endTolerance defines maximum allowed distance to end path point in addition to agentHalfExtents
|
||||
* @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.
|
||||
*/
|
||||
template <class OutputIterator>
|
||||
Status findPath(const osg::Vec3f& agentHalfExtents, const float stepSize, const osg::Vec3f& start,
|
||||
const osg::Vec3f& end, const Flags includeFlags, const DetourNavigator::AreaCosts& areaCosts,
|
||||
OutputIterator& out) const
|
||||
float endTolerance, OutputIterator& out) const
|
||||
{
|
||||
static_assert(
|
||||
std::is_same<
|
||||
|
|
@ -194,7 +195,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, areaCosts, settings, out);
|
||||
toNavMeshCoordinates(settings, end), includeFlags, areaCosts, settings, endTolerance, out);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@ namespace DetourNavigator
|
|||
enum class Status
|
||||
{
|
||||
Success,
|
||||
PartialPath,
|
||||
NavMeshNotFound,
|
||||
StartPolygonNotFound,
|
||||
EndPolygonNotFound,
|
||||
|
|
@ -21,6 +22,8 @@ namespace DetourNavigator
|
|||
{
|
||||
case Status::Success:
|
||||
return "success";
|
||||
case Status::PartialPath:
|
||||
return "partial path is found";
|
||||
case Status::NavMeshNotFound:
|
||||
return "navmesh is not found";
|
||||
case Status::StartPolygonNotFound:
|
||||
|
|
|
|||
Loading…
Reference in a new issue