1
0
Fork 0
mirror of https://github.com/OpenMW/openmw.git synced 2025-05-29 13:11:31 +00:00

Merge pull request #2691 from elsid/navigator_status

Use status codes to handle navigator errors instead of exceptions
This commit is contained in:
Andrei Kortunov 2020-02-05 13:24:29 +04:00 committed by GitHub
commit b8548b8f56
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 163 additions and 87 deletions

View file

@ -341,21 +341,23 @@ namespace MWMechanics
const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags,
std::back_insert_iterator<std::deque<osg::Vec3f>> out) std::back_insert_iterator<std::deque<osg::Vec3f>> out)
{ {
try 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);
if (status == DetourNavigator::Status::NavMeshNotFound)
return false;
if (status != DetourNavigator::Status::Success)
{ {
const auto world = MWBase::Environment::get().getWorld(); Log(Debug::Debug) << "Build path by navigator error: \"" << DetourNavigator::getMessage(status)
const auto stepSize = getPathStepSize(actor);
const auto navigator = world->getNavigator();
return navigator->findPath(halfExtents, stepSize, startPoint, endPoint, flags, out).is_initialized();
}
catch (const DetourNavigator::NavigatorException& exception)
{
Log(Debug::Debug) << "Build path by navigator exception: \"" << exception.what()
<< "\" for \"" << actor.getClass().getName(actor) << "\" (" << actor.getBase() << "\" for \"" << actor.getClass().getName(actor) << "\" (" << actor.getBase()
<< ") from " << startPoint << " to " << endPoint << " with flags (" << ") from " << startPoint << " to " << endPoint << " with flags ("
<< DetourNavigator::WriteFlags {flags} << ")"; << DetourNavigator::WriteFlags {flags} << ")";
return true;
} }
return true;
} }
void PathFinder::buildPathByNavMeshToNextPoint(const MWWorld::ConstPtr& actor, const osg::Vec3f& halfExtents, void PathFinder::buildPathByNavMeshToNextPoint(const MWWorld::ConstPtr& actor, const osg::Vec3f& halfExtents,
@ -370,26 +372,30 @@ namespace MWMechanics
if (sqrDistanceIgnoreZ(mPath.front(), startPoint) <= 4 * stepSize * stepSize) if (sqrDistanceIgnoreZ(mPath.front(), startPoint) <= 4 * stepSize * stepSize)
return; return;
try const auto navigator = MWBase::Environment::get().getWorld()->getNavigator();
std::deque<osg::Vec3f> prePath;
auto prePathInserter = std::back_inserter(prePath);
const auto status = navigator->findPath(halfExtents, stepSize, startPoint, mPath.front(), flags,
prePathInserter);
if (status == DetourNavigator::Status::NavMeshNotFound)
return;
if (status != DetourNavigator::Status::Success)
{ {
const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); Log(Debug::Debug) << "Build path by navigator error: \"" << DetourNavigator::getMessage(status)
std::deque<osg::Vec3f> prePath;
navigator->findPath(halfExtents, stepSize, startPoint, mPath.front(), flags, std::back_inserter(prePath));
while (!prePath.empty() && sqrDistanceIgnoreZ(prePath.front(), startPoint) < stepSize * stepSize)
prePath.pop_front();
while (!prePath.empty() && sqrDistanceIgnoreZ(prePath.back(), mPath.front()) < stepSize * stepSize)
prePath.pop_back();
std::copy(prePath.rbegin(), prePath.rend(), std::front_inserter(mPath));
}
catch (const DetourNavigator::NavigatorException& exception)
{
Log(Debug::Debug) << "Build path by navigator exception: \"" << exception.what()
<< "\" for \"" << actor.getClass().getName(actor) << "\" (" << actor.getBase() << "\" for \"" << actor.getClass().getName(actor) << "\" (" << actor.getBase()
<< ") from " << startPoint << " to " << mPath.front() << " with flags (" << ") from " << startPoint << " to " << mPath.front() << " with flags ("
<< DetourNavigator::WriteFlags {flags} << ")"; << DetourNavigator::WriteFlags {flags} << ")";
return;
} }
while (!prePath.empty() && sqrDistanceIgnoreZ(prePath.front(), startPoint) < stepSize * stepSize)
prePath.pop_front();
while (!prePath.empty() && sqrDistanceIgnoreZ(prePath.back(), mPath.front()) < stepSize * stepSize)
prePath.pop_back();
std::copy(prePath.rbegin(), prePath.rend(), std::front_inserter(mPath));
} }
} }

View file

@ -73,14 +73,16 @@ namespace
TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty) TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_return_empty)
{ {
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut),
Status::NavMeshNotFound);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>()); EXPECT_EQ(mPath, std::deque<osg::Vec3f>());
} }
TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception) TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception)
{ {
mNavigator->addAgent(mAgentHalfExtents); mNavigator->addAgent(mAgentHalfExtents);
EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), NavigatorException); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut),
Status::StartPolygonNotFound);
} }
TEST_F(DetourNavigatorNavigatorTest, add_agent_should_count_each_agent) TEST_F(DetourNavigatorNavigatorTest, add_agent_should_count_each_agent)
@ -88,7 +90,8 @@ namespace
mNavigator->addAgent(mAgentHalfExtents); mNavigator->addAgent(mAgentHalfExtents);
mNavigator->addAgent(mAgentHalfExtents); mNavigator->addAgent(mAgentHalfExtents);
mNavigator->removeAgent(mAgentHalfExtents); mNavigator->removeAgent(mAgentHalfExtents);
EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), NavigatorException); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut),
Status::StartPolygonNotFound);
} }
TEST_F(DetourNavigatorNavigatorTest, update_then_find_path_should_return_path) TEST_F(DetourNavigatorNavigatorTest, update_then_find_path_should_return_path)
@ -108,7 +111,7 @@ namespace
mNavigator->update(mPlayerPosition); mNavigator->update(mPlayerPosition);
mNavigator->wait(); mNavigator->wait();
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(-215, 215, 1.85963428020477294921875), osg::Vec3f(-215, 215, 1.85963428020477294921875),
@ -158,7 +161,7 @@ namespace
mNavigator->update(mPlayerPosition); mNavigator->update(mPlayerPosition);
mNavigator->wait(); mNavigator->wait();
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(-215, 215, 1.85963428020477294921875), osg::Vec3f(-215, 215, 1.85963428020477294921875),
@ -191,7 +194,8 @@ namespace
mNavigator->wait(); mNavigator->wait();
mPath.clear(); mPath.clear();
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); mOut = std::back_inserter(mPath);
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(-215, 215, 1.87826788425445556640625), osg::Vec3f(-215, 215, 1.87826788425445556640625),
@ -242,7 +246,7 @@ namespace
mNavigator->update(mPlayerPosition); mNavigator->update(mPlayerPosition);
mNavigator->wait(); mNavigator->wait();
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(-215, 215, 1.87826788425445556640625), osg::Vec3f(-215, 215, 1.87826788425445556640625),
@ -277,7 +281,8 @@ namespace
mNavigator->wait(); mNavigator->wait();
mPath.clear(); mPath.clear();
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); mOut = std::back_inserter(mPath);
EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(-215, 215, 1.85963428020477294921875), osg::Vec3f(-215, 215, 1.85963428020477294921875),
@ -334,7 +339,7 @@ namespace
mNavigator->update(mPlayerPosition); mNavigator->update(mPlayerPosition);
mNavigator->wait(); mNavigator->wait();
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(-215, 215, 1.96328866481781005859375), osg::Vec3f(-215, 215, 1.96328866481781005859375),
@ -390,7 +395,7 @@ namespace
mNavigator->update(mPlayerPosition); mNavigator->update(mPlayerPosition);
mNavigator->wait(); mNavigator->wait();
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(-215, 215, 1.9393787384033203125), osg::Vec3f(-215, 215, 1.9393787384033203125),
@ -443,7 +448,7 @@ namespace
mEnd.x() = 0; mEnd.x() = 0;
mEnd.z() = 300; mEnd.z() = 300;
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mOut); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(0, 215, 185.33331298828125), osg::Vec3f(0, 215, 185.33331298828125),
@ -489,7 +494,8 @@ namespace
mStart.x() = 0; mStart.x() = 0;
mEnd.x() = 0; mEnd.x() = 0;
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mOut); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mOut),
Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(0, 215, -94.75363922119140625), osg::Vec3f(0, 215, -94.75363922119140625),
@ -535,7 +541,8 @@ namespace
mStart.x() = 0; mStart.x() = 0;
mEnd.x() = 0; mEnd.x() = 0;
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mOut); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_swim | Flag_walk, mOut),
Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(0, 215, -94.75363922119140625), osg::Vec3f(0, 215, -94.75363922119140625),
@ -581,7 +588,7 @@ namespace
mStart.x() = 0; mStart.x() = 0;
mEnd.x() = 0; mEnd.x() = 0;
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(0, 215, -94.75363922119140625), osg::Vec3f(0, 215, -94.75363922119140625),
@ -630,7 +637,7 @@ namespace
mNavigator->update(mPlayerPosition); mNavigator->update(mPlayerPosition);
mNavigator->wait(); mNavigator->wait();
mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut); EXPECT_EQ(mNavigator->findPath(mAgentHalfExtents, mStepSize, mStart, mEnd, Flag_walk, mOut), Status::Success);
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({ EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
osg::Vec3f(-215, 215, 1.85963428020477294921875), osg::Vec3f(-215, 215, 1.85963428020477294921875),

View file

@ -2,6 +2,7 @@
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H #define OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H
#include "tilebounds.hpp" #include "tilebounds.hpp"
#include "status.hpp"
#include <osg/io_utils> #include <osg/io_utils>
@ -26,6 +27,25 @@ namespace DetourNavigator
return stream << "TileBounds {" << value.mMin << ", " << value.mMax << "}"; return stream << "TileBounds {" << value.mMin << ", " << value.mMax << "}";
} }
inline std::ostream& operator <<(std::ostream& stream, Status value)
{
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(name) \
case Status::name: return stream << "DetourNavigator::Status::"#name;
switch (value)
{
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(Success)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(NavMeshNotFound)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(StartPolygonNotFound)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(EndPolygonNotFound)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(MoveAlongSurfaceFailed)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(FindPathOverPolygonsFailed)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(GetPolyHeightFailed)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(InitNavMeshQueryFailed)
}
#undef OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE
return stream << "DetourNavigator::Error::" << static_cast<int>(value);
}
class RecastMesh; class RecastMesh;
void writeToFile(const RecastMesh& recastMesh, const std::string& pathPrefix, const std::string& revision); void writeToFile(const RecastMesh& recastMesh, const std::string& pathPrefix, const std::string& revision);

View file

@ -14,7 +14,8 @@ namespace DetourNavigator
const osg::Vec3f& start, const float maxRadius, const Flags includeFlags, const Settings& settings) const osg::Vec3f& start, const float maxRadius, const Flags includeFlags, const Settings& settings)
{ {
dtNavMeshQuery navMeshQuery; dtNavMeshQuery navMeshQuery;
initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes); if (!initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes))
return boost::optional<osg::Vec3f>();
dtQueryFilter queryFilter; dtQueryFilter queryFilter;
queryFilter.setIncludeFlags(includeFlags); queryFilter.setIncludeFlags(includeFlags);

View file

@ -7,6 +7,7 @@
#include "settings.hpp" #include "settings.hpp"
#include "settingsutils.hpp" #include "settingsutils.hpp"
#include "debug.hpp" #include "debug.hpp"
#include "status.hpp"
#include <DetourCommon.h> #include <DetourCommon.h>
#include <DetourNavMesh.h> #include <DetourNavMesh.h>
@ -97,11 +98,10 @@ namespace DetourNavigator
std::reference_wrapper<const Settings> mSettings; std::reference_wrapper<const Settings> mSettings;
}; };
inline void initNavMeshQuery(dtNavMeshQuery& value, const dtNavMesh& navMesh, const int maxNodes) inline bool initNavMeshQuery(dtNavMeshQuery& value, const dtNavMesh& navMesh, const int maxNodes)
{ {
const auto status = value.init(&navMesh, maxNodes); const auto status = value.init(&navMesh, maxNodes);
if (!dtStatusSucceed(status)) return dtStatusSucceed(status);
throw NavigatorException("Failed to init navmesh query");
} }
struct MoveAlongSurfaceResult struct MoveAlongSurfaceResult
@ -110,8 +110,8 @@ namespace DetourNavigator
std::vector<dtPolyRef> mVisited; std::vector<dtPolyRef> mVisited;
}; };
inline MoveAlongSurfaceResult moveAlongSurface(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef, inline boost::optional<MoveAlongSurfaceResult> moveAlongSurface(const dtNavMeshQuery& navMeshQuery,
const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& filter, const dtPolyRef startRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& filter,
const std::size_t maxVisitedSize) const std::size_t maxVisitedSize)
{ {
MoveAlongSurfaceResult result; MoveAlongSurfaceResult result;
@ -120,18 +120,14 @@ namespace DetourNavigator
const auto status = navMeshQuery.moveAlongSurface(startRef, startPos.ptr(), endPos.ptr(), const auto status = navMeshQuery.moveAlongSurface(startRef, startPos.ptr(), endPos.ptr(),
&filter, result.mResultPos.ptr(), result.mVisited.data(), &visitedNumber, static_cast<int>(maxVisitedSize)); &filter, result.mResultPos.ptr(), result.mVisited.data(), &visitedNumber, static_cast<int>(maxVisitedSize));
if (!dtStatusSucceed(status)) if (!dtStatusSucceed(status))
{ return {};
std::ostringstream message;
message << "Failed to move along surface from " << startPos << " to " << endPos;
throw NavigatorException(message.str());
}
assert(visitedNumber >= 0); assert(visitedNumber >= 0);
assert(visitedNumber <= static_cast<int>(maxVisitedSize)); assert(visitedNumber <= static_cast<int>(maxVisitedSize));
result.mVisited.resize(static_cast<std::size_t>(visitedNumber)); result.mVisited.resize(static_cast<std::size_t>(visitedNumber));
return result; return {std::move(result)};
} }
inline std::vector<dtPolyRef> findPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef, inline boost::optional<std::vector<dtPolyRef>> findPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef,
const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter, const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter,
const std::size_t maxSize) const std::size_t maxSize)
{ {
@ -140,34 +136,26 @@ namespace DetourNavigator
const auto status = navMeshQuery.findPath(startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter, const auto status = navMeshQuery.findPath(startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter,
result.data(), &pathLen, static_cast<int>(maxSize)); result.data(), &pathLen, static_cast<int>(maxSize));
if (!dtStatusSucceed(status)) if (!dtStatusSucceed(status))
{ return {};
std::ostringstream message;
message << "Failed to find path over polygons from " << startRef << " to " << endRef;
throw NavigatorException(message.str());
}
assert(pathLen >= 0); assert(pathLen >= 0);
assert(static_cast<std::size_t>(pathLen) <= maxSize); assert(static_cast<std::size_t>(pathLen) <= maxSize);
result.resize(static_cast<std::size_t>(pathLen)); result.resize(static_cast<std::size_t>(pathLen));
return result; return {std::move(result)};
} }
inline float getPolyHeight(const dtNavMeshQuery& navMeshQuery, const dtPolyRef ref, const osg::Vec3f& pos) inline boost::optional<float> getPolyHeight(const dtNavMeshQuery& navMeshQuery, const dtPolyRef ref, const osg::Vec3f& pos)
{ {
float result = 0.0f; float result = 0.0f;
const auto status = navMeshQuery.getPolyHeight(ref, pos.ptr(), &result); const auto status = navMeshQuery.getPolyHeight(ref, pos.ptr(), &result);
if (!dtStatusSucceed(status)) if (!dtStatusSucceed(status))
{ return {};
std::ostringstream message;
message << "Failed to get polygon height ref=" << ref << " pos=" << pos;
throw NavigatorException(message.str());
}
return result; return result;
} }
template <class OutputIterator> template <class OutputIterator>
OutputIterator makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, Status makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery,
const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end, const float stepSize, const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end, const float stepSize,
std::vector<dtPolyRef> polygonPath, std::size_t maxSmoothPathSize, OutputIterator out) std::vector<dtPolyRef> polygonPath, std::size_t maxSmoothPathSize, OutputIterator& out)
{ {
// Iterate over the path to find smooth path on the detail mesh surface. // Iterate over the path to find smooth path on the detail mesh surface.
osg::Vec3f iterPos; osg::Vec3f iterPos;
@ -207,12 +195,15 @@ namespace DetourNavigator
const osg::Vec3f moveTgt = iterPos + delta * len; const osg::Vec3f moveTgt = iterPos + delta * len;
const auto result = moveAlongSurface(navMeshQuery, polygonPath.front(), iterPos, moveTgt, filter, 16); const auto result = moveAlongSurface(navMeshQuery, polygonPath.front(), iterPos, moveTgt, filter, 16);
polygonPath = fixupCorridor(polygonPath, result.mVisited); if (!result)
return Status::MoveAlongSurfaceFailed;
polygonPath = fixupCorridor(polygonPath, result->mVisited);
polygonPath = fixupShortcuts(polygonPath, navMeshQuery); polygonPath = fixupShortcuts(polygonPath, navMeshQuery);
float h = 0; float h = 0;
navMeshQuery.getPolyHeight(polygonPath.front(), result.mResultPos.ptr(), &h); navMeshQuery.getPolyHeight(polygonPath.front(), result->mResultPos.ptr(), &h);
iterPos = result.mResultPos; iterPos = result->mResultPos;
iterPos.y() = h; iterPos.y() = h;
// Handle end of path and off-mesh links when close enough. // Handle end of path and off-mesh links when close enough.
@ -259,7 +250,12 @@ namespace DetourNavigator
// Move position at the other side of the off-mesh link. // Move position at the other side of the off-mesh link.
iterPos = endPos; iterPos = endPos;
iterPos.y() = getPolyHeight(navMeshQuery, polygonPath.front(), iterPos); const auto height = getPolyHeight(navMeshQuery, polygonPath.front(), iterPos);
if (!height)
return Status::GetPolyHeightFailed;
iterPos.y() = *height;
} }
} }
@ -268,16 +264,17 @@ namespace DetourNavigator
++smoothPathSize; ++smoothPathSize;
} }
return out; return Status::Success;
} }
template <class OutputIterator> template <class OutputIterator>
OutputIterator findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, const float stepSize, 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 Settings& settings, OutputIterator out) const Settings& settings, OutputIterator& out)
{ {
dtNavMeshQuery navMeshQuery; dtNavMeshQuery navMeshQuery;
initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes); if (!initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes))
return Status::InitNavMeshQueryFailed;
dtQueryFilter queryFilter; dtQueryFilter queryFilter;
queryFilter.setIncludeFlags(includeFlags); queryFilter.setIncludeFlags(includeFlags);
@ -293,7 +290,7 @@ namespace DetourNavigator
} }
if (startRef == 0) if (startRef == 0)
throw NavigatorException("Navmesh polygon for start point is not found"); return Status::StartPolygonNotFound;
dtPolyRef endRef = 0; dtPolyRef endRef = 0;
osg::Vec3f endPolygonPosition; osg::Vec3f endPolygonPosition;
@ -306,18 +303,20 @@ namespace DetourNavigator
} }
if (endRef == 0) if (endRef == 0)
throw NavigatorException("Navmesh polygon for end polygon is not found"); return Status::EndPolygonNotFound;
const auto polygonPath = findPath(navMeshQuery, startRef, endRef, start, end, queryFilter, const auto polygonPath = findPath(navMeshQuery, startRef, endRef, start, end, queryFilter,
settings.mMaxPolygonPathSize); settings.mMaxPolygonPathSize);
if (polygonPath.empty() || polygonPath.back() != endRef) if (!polygonPath)
return out; return Status::FindPathOverPolygonsFailed;
makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize, std::move(polygonPath), if (polygonPath->empty() || polygonPath->back() != endRef)
settings.mMaxSmoothPathSize, OutputTransformIterator<OutputIterator>(out, settings)); return Status::Success;
return out; auto outTransform = OutputTransformIterator<OutputIterator>(out, settings);
return makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize, std::move(*polygonPath),
settings.mMaxSmoothPathSize, outTransform);
} }
} }

View file

@ -1,4 +1,4 @@
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H #ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H #define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H
#include "findsmoothpath.hpp" #include "findsmoothpath.hpp"
@ -159,8 +159,8 @@ namespace DetourNavigator
* Equal to out if no path is found. * Equal to out if no path is found.
*/ */
template <class OutputIterator> template <class OutputIterator>
boost::optional<OutputIterator> findPath(const osg::Vec3f& agentHalfExtents, const float stepSize, const osg::Vec3f& start, 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, OutputIterator& out) const
{ {
static_assert( static_assert(
std::is_same< std::is_same<
@ -171,7 +171,7 @@ namespace DetourNavigator
); );
const auto navMesh = getNavMesh(agentHalfExtents); const auto navMesh = getNavMesh(agentHalfExtents);
if (!navMesh) if (!navMesh)
return {}; return Status::NavMeshNotFound;
const auto settings = getSettings(); const auto settings = getSettings();
return findSmoothPath(navMesh->lockConst()->getImpl(), toNavMeshCoordinates(settings, agentHalfExtents), return findSmoothPath(navMesh->lockConst()->getImpl(), toNavMeshCoordinates(settings, agentHalfExtents),
toNavMeshCoordinates(settings, stepSize), toNavMeshCoordinates(settings, start), toNavMeshCoordinates(settings, stepSize), toNavMeshCoordinates(settings, start),

View file

@ -0,0 +1,43 @@
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_STATUS_H
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_STATUS_H
namespace DetourNavigator
{
enum class Status
{
Success,
NavMeshNotFound,
StartPolygonNotFound,
EndPolygonNotFound,
MoveAlongSurfaceFailed,
FindPathOverPolygonsFailed,
GetPolyHeightFailed,
InitNavMeshQueryFailed,
};
constexpr const char* getMessage(Status value)
{
switch (value)
{
case Status::Success:
return "success";
case Status::NavMeshNotFound:
return "navmesh is not found";
case Status::StartPolygonNotFound:
return "polygon for start position is not found on navmesh";
case Status::EndPolygonNotFound:
return "polygon for end position is not found on navmesh";
case Status::MoveAlongSurfaceFailed:
return "move along surface on navmesh is failed";
case Status::FindPathOverPolygonsFailed:
return "path over navmesh polygons is not found";
case Status::GetPolyHeightFailed:
return "failed to get polygon height";
case Status::InitNavMeshQueryFailed:
return "failed to init navmesh query";
}
return "unknown error";
}
}
#endif