Archive/openmw
0
0
Fork 0
mirror of https://gitlab.com/OpenMW/openmw.git synced 2025-05-09 03:57:51 +03:00
Code Issues Projects Releases Packages Wiki Activity Actions 39

Merge branch 'fix_find_path' into 'master'

Browse source 
Remove stepping from findSmoothPath and other tweaks (#7241 , #7485)

Closes #7485 and #7241

See merge request OpenMW/openmw!3264
This commit is contained in:
psi29a 2023-07-27 09:34:50 +00:00
commit e76ce6b696
15 changed files with 189 additions and 809 deletions
Download patch file Download diff file Expand all files Collapse all files

22
components/detournavigator/asyncnavmeshupdater.cpp
View file

@ -339,7 +339,7 @@ namespace DetourNavigator
switch (status)
{
case JobStatus::Done:
unlockTile(job->mAgentBounds, job->mChangedTile);
unlockTile(job->mId, job->mAgentBounds, job->mChangedTile);
if (job->mGeneratedNavMeshData != nullptr)
mDbWorker->enqueueJob(job);
else
@ -565,12 +565,14 @@ namespace DetourNavigator
mWaiting.pop_front();
Log(Debug::Debug) << "Pop job " << job->mId << " by thread=" << std::this_thread::get_id();
if (job->mRecastMesh != nullptr)
return job;
if (!lockTile(job->mAgentBounds, job->mChangedTile))
if (!lockTile(job->mId, job->mAgentBounds, job->mChangedTile))
{
Log(Debug::Debug) << "Failed to lock tile by " << job->mId;
Log(Debug::Debug) << "Failed to lock tile by job " << job->mId << " try=" << job->mTryNumber;
++job->mTryNumber;
insertPrioritizedJob(job, mWaiting);
return mJobs.end();
@ -610,7 +612,7 @@ namespace DetourNavigator
void AsyncNavMeshUpdater::repost(JobIt job)
{
unlockTile(job->mAgentBounds, job->mChangedTile);
unlockTile(job->mId, job->mAgentBounds, job->mChangedTile);
if (mShouldStop || job->mTryNumber > 2)
return;
@ -628,17 +630,21 @@ namespace DetourNavigator
mJobs.erase(job);
}
bool AsyncNavMeshUpdater::lockTile(const AgentBounds& agentBounds, const TilePosition& changedTile)
bool AsyncNavMeshUpdater::lockTile(
std::size_t jobId, const AgentBounds& agentBounds, const TilePosition& changedTile)
{
Log(Debug::Debug) << "Locking tile agent=" << agentBounds << " changedTile=(" << changedTile << ")";
Log(Debug::Debug) << "Locking tile by job " << jobId << " agent=" << agentBounds << " changedTile=("
<< changedTile << ")";
return mProcessingTiles.lock()->emplace(agentBounds, changedTile).second;
}
void AsyncNavMeshUpdater::unlockTile(const AgentBounds& agentBounds, const TilePosition& changedTile)
void AsyncNavMeshUpdater::unlockTile(
std::size_t jobId, const AgentBounds& agentBounds, const TilePosition& changedTile)
{
auto locked = mProcessingTiles.lock();
locked->erase(std::tie(agentBounds, changedTile));
Log(Debug::Debug) << "Unlocked tile agent=" << agentBounds << " changedTile=(" << changedTile << ")";
Log(Debug::Debug) << "Unlocked tile by job " << jobId << " agent=" << agentBounds << " changedTile=("
<< changedTile << ")";
if (locked->empty())
mProcessed.notify_all();
}

4
components/detournavigator/asyncnavmeshupdater.hpp
View file

@ -199,9 +199,9 @@ namespace DetourNavigator
void repost(JobIt job);
bool lockTile(const AgentBounds& agentBounds, const TilePosition& changedTile);
bool lockTile(std::size_t jobId, const AgentBounds& agentBounds, const TilePosition& changedTile);
void unlockTile(const AgentBounds& agentBounds, const TilePosition& changedTile);
void unlockTile(std::size_t jobId, const AgentBounds& agentBounds, const TilePosition& changedTile);
inline std::size_t getTotalJobs() const;

2
components/detournavigator/debug.cpp
View file

@ -36,9 +36,11 @@ namespace DetourNavigator
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(TargetPolygonNotFound)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(MoveAlongSurfaceFailed)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(FindPathOverPolygonsFailed)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(InitNavMeshQueryFailed)
OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE(FindStraightPathFailed)
}
#undef OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_STATUS_MESSAGE
return stream << "DetourNavigator::Error::" << static_cast<int>(value);

9
components/detournavigator/findrandompointaroundcircle.cpp
View file

@ -1,5 +1,4 @@
#include "findrandompointaroundcircle.hpp"
#include "findsmoothpath.hpp"
#include <DetourNavMesh.h>
#include <DetourNavMeshQuery.h>
@ -13,9 +12,11 @@ namespace DetourNavigator
dtQueryFilter queryFilter;
queryFilter.setIncludeFlags(includeFlags);
dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, halfExtents * 4);
if (startRef == 0)
return std::optional<osg::Vec3f>();
dtPolyRef startRef = 0;
const dtStatus status
= navMeshQuery.findNearestPoly(start.ptr(), halfExtents.ptr(), &queryFilter, &startRef, nullptr);
if (dtStatusFailed(status))
return std::nullopt;
dtPolyRef resultRef = 0;
osg::Vec3f resultPosition;

147
components/detournavigator/findsmoothpath.cpp
View file

@ -1,147 +0,0 @@
#include "findsmoothpath.hpp"
#include <components/misc/convert.hpp>
#include <DetourCommon.h>
#include <algorithm>
#include <array>
namespace DetourNavigator
{
std::size_t fixupCorridor(std::vector<dtPolyRef>& path, std::size_t pathSize, const std::vector<dtPolyRef>& visited)
{
std::vector<dtPolyRef>::const_reverse_iterator furthestVisited;
// Find furthest common polygon.
const auto begin = path.begin();
const auto end = path.begin() + pathSize;
const std::reverse_iterator rbegin(end);
const std::reverse_iterator rend(begin);
const auto it = std::find_if(rbegin, rend, [&](dtPolyRef pathValue) {
const auto it = std::find(visited.rbegin(), visited.rend(), pathValue);
if (it == visited.rend())
return false;
furthestVisited = it;
return true;
});
// If no intersection found just return current path.
if (it == rend)
return pathSize;
const auto furthestPath = it.base() - 1;
// Concatenate paths.
// visited: a_1 ... a_n x b_1 ... b_n
// furthestVisited ^
// path: C x D E
// ^ furthestPath ^ path.size() - (furthestVisited + 1 - visited.rbegin())
// result: x b_n ... b_1 D
const std::size_t required = static_cast<std::size_t>(furthestVisited + 1 - visited.rbegin());
const auto newEnd = std::copy(furthestPath + 1, std::min(begin + path.size(), end), begin + required);
std::copy(visited.rbegin(), furthestVisited + 1, begin);
return static_cast<std::size_t>(newEnd - begin);
}
std::size_t fixupShortcuts(dtPolyRef* path, std::size_t pathSize, const dtNavMeshQuery& navQuery)
{
if (pathSize < 3)
return pathSize;
// Get connected polygons
const dtMeshTile* tile = nullptr;
const dtPoly* poly = nullptr;
if (dtStatusFailed(navQuery.getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly)))
return pathSize;
const std::size_t maxNeis = 16;
std::array<dtPolyRef, maxNeis> neis;
std::size_t nneis = 0;
for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
{
const dtLink* link = &tile->links[k];
if (link->ref != 0)
{
if (nneis < maxNeis)
neis[nneis++] = link->ref;
}
}
// If any of the neighbour polygons is within the next few polygons
// in the path, short cut to that polygon directly.
const std::size_t maxLookAhead = 6;
std::size_t cut = 0;
for (std::size_t i = std::min(maxLookAhead, pathSize) - 1; i > 1 && cut == 0; i--)
{
for (std::size_t j = 0; j < nneis; j++)
{
if (path[i] == neis[j])
{
cut = i;
break;
}
}
}
if (cut <= 1)
return pathSize;
const std::ptrdiff_t offset = static_cast<std::ptrdiff_t>(cut) - 1;
std::copy(path + offset, path + pathSize, path);
return pathSize - offset;
}
std::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& startPos,
const osg::Vec3f& endPos, const float minTargetDist, const dtPolyRef* path, const std::size_t pathSize)
{
// Find steer target.
SteerTarget result;
constexpr int maxSteerPoints = 3;
std::array<float, maxSteerPoints * 3> steerPath;
std::array<unsigned char, maxSteerPoints> steerPathFlags;
std::array<dtPolyRef, maxSteerPoints> steerPathPolys;
int nsteerPath = 0;
const dtStatus status
= navMeshQuery.findStraightPath(startPos.ptr(), endPos.ptr(), path, static_cast<int>(pathSize),
steerPath.data(), steerPathFlags.data(), steerPathPolys.data(), &nsteerPath, maxSteerPoints);
if (dtStatusFailed(status))
return std::nullopt;
assert(nsteerPath >= 0);
if (!nsteerPath)
return std::nullopt;
// Find vertex far enough to steer to.
std::size_t ns = 0;
while (ns < static_cast<std::size_t>(nsteerPath))
{
// Stop at Off-Mesh link or when point is further than slop away.
if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
|| !inRange(Misc::Convert::makeOsgVec3f(&steerPath[ns * 3]), startPos, minTargetDist))
break;
ns++;
}
// Failed to find good point to steer to.
if (ns >= static_cast<std::size_t>(nsteerPath))
return std::nullopt;
dtVcopy(result.mSteerPos.ptr(), &steerPath[ns * 3]);
result.mSteerPos.y() = startPos[1];
result.mSteerPosFlag = steerPathFlags[ns];
result.mSteerPosRef = steerPathPolys[ns];
return result;
}
dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter, const osg::Vec3f& center,
const osg::Vec3f& halfExtents)
{
dtPolyRef ref = 0;
const dtStatus status = query.findNearestPoly(center.ptr(), halfExtents.ptr(), &filter, &ref, nullptr);
if (!dtStatusSucceed(status))
return 0;
return ref;
}
}

249
components/detournavigator/findsmoothpath.hpp
View file

@ -7,59 +7,27 @@
#include "settingsutils.hpp"
#include "status.hpp"
#include <DetourCommon.h>
#include <DetourNavMesh.h>
#include <DetourNavMeshQuery.h>
#include <osg/Vec3f>
#include <array>
#include <cassert>
#include <functional>
#include <span>
#include <vector>
class dtNavMesh;
namespace DetourNavigator
{
struct Settings;
inline bool inRange(const osg::Vec3f& v1, const osg::Vec3f& v2, const float r)
{
return (osg::Vec2f(v1.x(), v1.z()) - osg::Vec2f(v2.x(), v2.z())).length() < r;
}
std::size_t fixupCorridor(
std::vector<dtPolyRef>& path, std::size_t pathSize, const std::vector<dtPolyRef>& visited);
// This function checks if the path has a small U-turn, that is,
// a polygon further in the path is adjacent to the first polygon
// in the path. If that happens, a shortcut is taken.
// This can happen if the target (T) location is at tile boundary,
// and we're (S) approaching it parallel to the tile edge.
// The choice at the vertex can be arbitrary,
// +---+---+
// |:::|:::|
// +-S-+-T-+
// |:::| | <-- the step can end up in here, resulting U-turn path.
// +---+---+
std::size_t fixupShortcuts(dtPolyRef* path, std::size_t pathSize, const dtNavMeshQuery& navQuery);
struct SteerTarget
{
osg::Vec3f mSteerPos;
unsigned char mSteerPosFlag;
dtPolyRef mSteerPosRef;
};
std::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos,
const osg::Vec3f& endPos, const float minTargetDist, const dtPolyRef* path, const std::size_t pathSize);
template <class OutputIterator>
template <class OutputIterator, class Function>
class OutputTransformIterator
{
public:
explicit OutputTransformIterator(OutputIterator& impl, const RecastSettings& settings)
explicit OutputTransformIterator(OutputIterator& impl, Function&& function)
: mImpl(impl)
, mSettings(settings)
, mFunction(std::forward<Function>(function))
{
}
@ -80,180 +48,64 @@ namespace DetourNavigator
OutputTransformIterator& operator=(const osg::Vec3f& value)
{
*mImpl.get() = fromNavMeshCoordinates(mSettings, value);
*mImpl.get() = mFunction(value);
return *this;
}
private:
std::reference_wrapper<OutputIterator> mImpl;
std::reference_wrapper<const RecastSettings> mSettings;
Function mFunction;
};
inline bool initNavMeshQuery(dtNavMeshQuery& value, const dtNavMesh& navMesh, const int maxNodes)
template <class OutputIterator>
auto withFromNavMeshCoordinates(OutputIterator& impl, const RecastSettings& settings)
{
const auto status = value.init(&navMesh, maxNodes);
return dtStatusSucceed(status);
}
dtPolyRef findNearestPoly(const dtNavMeshQuery& query, const dtQueryFilter& filter, const osg::Vec3f& center,
const osg::Vec3f& halfExtents);
struct MoveAlongSurfaceResult
{
osg::Vec3f mResultPos;
std::vector<dtPolyRef> mVisited;
};
inline std::optional<MoveAlongSurfaceResult> moveAlongSurface(const dtNavMeshQuery& navMeshQuery,
const dtPolyRef startRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& filter,
const std::size_t maxVisitedSize)
{
MoveAlongSurfaceResult result;
result.mVisited.resize(maxVisitedSize);
int visitedNumber = 0;
const auto status = navMeshQuery.moveAlongSurface(startRef, startPos.ptr(), endPos.ptr(), &filter,
result.mResultPos.ptr(), result.mVisited.data(), &visitedNumber, static_cast<int>(maxVisitedSize));
if (!dtStatusSucceed(status))
return {};
assert(visitedNumber >= 0);
assert(visitedNumber <= static_cast<int>(maxVisitedSize));
result.mVisited.resize(static_cast<std::size_t>(visitedNumber));
return { std::move(result) };
return OutputTransformIterator(
impl, [&settings](const osg::Vec3f& value) { return fromNavMeshCoordinates(settings, value); });
}
inline std::optional<std::size_t> findPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef,
const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter,
dtPolyRef* path, const std::size_t maxSize)
std::span<dtPolyRef> pathBuffer)
{
int pathLen = 0;
const auto status = navMeshQuery.findPath(
startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter, path, &pathLen, static_cast<int>(maxSize));
const auto status = navMeshQuery.findPath(startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter,
pathBuffer.data(), &pathLen, static_cast<int>(pathBuffer.size()));
if (!dtStatusSucceed(status))
return {};
assert(pathLen >= 0);
assert(static_cast<std::size_t>(pathLen) <= maxSize);
assert(static_cast<std::size_t>(pathLen) <= pathBuffer.size());
return static_cast<std::size_t>(pathLen);
}
template <class OutputIterator>
Status makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, const dtQueryFilter& filter,
const osg::Vec3f& start, const osg::Vec3f& end, const float stepSize, std::vector<dtPolyRef>& polygonPath,
std::size_t polygonPathSize, std::size_t maxSmoothPathSize, OutputIterator& out)
Status makeSmoothPath(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& start, const osg::Vec3f& end,
std::span<dtPolyRef> polygonPath, std::size_t polygonPathSize, std::size_t maxSmoothPathSize,
OutputIterator& out)
{
// Iterate over the path to find smooth path on the detail mesh surface.
osg::Vec3f iterPos;
navMeshQuery.closestPointOnPoly(polygonPath.front(), start.ptr(), iterPos.ptr(), nullptr);
assert(polygonPathSize <= polygonPath.size());
osg::Vec3f targetPos;
navMeshQuery.closestPointOnPoly(polygonPath[polygonPathSize - 1], end.ptr(), targetPos.ptr(), nullptr);
std::vector<float> cornerVertsBuffer(maxSmoothPathSize * 3);
std::vector<unsigned char> cornerFlagsBuffer(maxSmoothPathSize);
std::vector<dtPolyRef> cornerPolysBuffer(maxSmoothPathSize);
int cornersCount = 0;
constexpr int findStraightPathOptions = DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS;
if (const dtStatus status = navMeshQuery.findStraightPath(start.ptr(), end.ptr(), polygonPath.data(),
static_cast<int>(polygonPathSize), cornerVertsBuffer.data(), cornerFlagsBuffer.data(),
cornerPolysBuffer.data(), &cornersCount, static_cast<int>(maxSmoothPathSize), findStraightPathOptions);
dtStatusFailed(status))
return Status::FindStraightPathFailed;
constexpr float slop = 0.01f;
*out++ = iterPos;
std::size_t smoothPathSize = 1;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
while (polygonPathSize > 0 && smoothPathSize < maxSmoothPathSize)
{
// Find location to steer towards.
const auto steerTarget
= getSteerTarget(navMeshQuery, iterPos, targetPos, slop, polygonPath.data(), polygonPathSize);
if (!steerTarget)
break;
const bool endOfPath = bool(steerTarget->mSteerPosFlag & DT_STRAIGHTPATH_END);
const bool offMeshConnection = bool(steerTarget->mSteerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
// Find movement delta.
const osg::Vec3f delta = steerTarget->mSteerPos - iterPos;
float len = delta.length();
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < stepSize)
len = 1;
else
len = stepSize / len;
const osg::Vec3f moveTgt = iterPos + delta * len;
const auto result = moveAlongSurface(navMeshQuery, polygonPath.front(), iterPos, moveTgt, filter, 16);
if (!result)
return Status::MoveAlongSurfaceFailed;
polygonPathSize = fixupCorridor(polygonPath, polygonPathSize, result->mVisited);
polygonPathSize = fixupShortcuts(polygonPath.data(), polygonPathSize, navMeshQuery);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(result->mResultPos, steerTarget->mSteerPos, slop))
{
// Reached end of path.
iterPos = targetPos;
*out++ = iterPos;
++smoothPathSize;
break;
}
dtPolyRef polyRef = polygonPath.front();
osg::Vec3f polyPos = result->mResultPos;
if (offMeshConnection && inRange(polyPos, steerTarget->mSteerPos, slop))
{
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0;
std::size_t npos = 0;
while (npos < polygonPathSize && polyRef != steerTarget->mSteerPosRef)
{
prevRef = polyRef;
polyRef = polygonPath[npos];
++npos;
}
if (npos > 0)
{
std::copy(polygonPath.begin() + npos, polygonPath.begin() + polygonPathSize, polygonPath.begin());
polygonPathSize -= npos;
}
// Reached off-mesh connection.
osg::Vec3f startPos;
osg::Vec3f endPos;
// Handle the connection.
if (dtStatusSucceed(
navMesh.getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos.ptr(), endPos.ptr())))
{
*out++ = startPos;
++smoothPathSize;
// Hack to make the dotted path not visible during off-mesh connection.
if (smoothPathSize & 1)
{
*out++ = startPos;
++smoothPathSize;
}
// Move position at the other side of the off-mesh link.
polyPos = endPos;
}
}
navMeshQuery.getPolyHeight(polyRef, polyPos.ptr(), &iterPos.y());
iterPos.x() = result->mResultPos.x();
iterPos.z() = result->mResultPos.z();
// Store results.
*out++ = iterPos;
++smoothPathSize;
}
for (int i = 0; i < cornersCount; ++i)
*out++ = Misc::Convert::makeOsgVec3f(&cornerVertsBuffer[static_cast<std::size_t>(i) * 3]);
return Status::Success;
}
template <class OutputIterator>
Status findSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& halfExtents,
const float stepSize, const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags,
const AreaCosts& areaCosts, const Settings& settings, float endTolerance, OutputIterator out)
Status findSmoothPath(const dtNavMeshQuery& navMeshQuery, const osg::Vec3f& halfExtents, const osg::Vec3f& start,
const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts, const DetourSettings& settings,
float endTolerance, OutputIterator out)
{
dtQueryFilter queryFilter;
queryFilter.setIncludeFlags(includeFlags);
@ -265,18 +117,24 @@ namespace DetourNavigator
constexpr float polyDistanceFactor = 4;
const osg::Vec3f polyHalfExtents = halfExtents * polyDistanceFactor;
const dtPolyRef startRef = findNearestPoly(navMeshQuery, queryFilter, start, polyHalfExtents);
if (startRef == 0)
osg::Vec3f startNavMeshPos;
dtPolyRef startRef = 0;
if (const dtStatus status = navMeshQuery.findNearestPoly(
start.ptr(), polyHalfExtents.ptr(), &queryFilter, &startRef, startNavMeshPos.ptr());
dtStatusFailed(status) || startRef == 0)
return Status::StartPolygonNotFound;
const dtPolyRef endRef = findNearestPoly(
navMeshQuery, queryFilter, end, polyHalfExtents + osg::Vec3f(endTolerance, endTolerance, endTolerance));
if (endRef == 0)
osg::Vec3f endNavMeshPos;
const osg::Vec3f endPolyHalfExtents = polyHalfExtents + osg::Vec3f(endTolerance, endTolerance, endTolerance);
dtPolyRef endRef;
if (const dtStatus status = navMeshQuery.findNearestPoly(
end.ptr(), endPolyHalfExtents.ptr(), &queryFilter, &endRef, endNavMeshPos.ptr());
dtStatusFailed(status) || endRef == 0)
return Status::EndPolygonNotFound;
std::vector<dtPolyRef> polygonPath(settings.mDetour.mMaxPolygonPathSize);
std::vector<dtPolyRef> polygonPath(settings.mMaxPolygonPathSize);
const auto polygonPathSize
= findPath(navMeshQuery, startRef, endRef, start, end, queryFilter, polygonPath.data(), polygonPath.size());
= findPath(navMeshQuery, startRef, endRef, startNavMeshPos, endNavMeshPos, queryFilter, polygonPath);
if (!polygonPathSize.has_value())
return Status::FindPathOverPolygonsFailed;
@ -284,10 +142,15 @@ namespace DetourNavigator
if (*polygonPathSize == 0)
return Status::Success;
osg::Vec3f targetNavMeshPos;
if (const dtStatus status = navMeshQuery.closestPointOnPoly(
polygonPath[*polygonPathSize - 1], end.ptr(), targetNavMeshPos.ptr(), nullptr);
dtStatusFailed(status))
return Status::TargetPolygonNotFound;
const bool partialPath = polygonPath[*polygonPathSize - 1] != endRef;
auto outTransform = OutputTransformIterator<OutputIterator>(out, settings.mRecast);
const Status smoothStatus = makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, stepSize,
polygonPath, *polygonPathSize, settings.mDetour.mMaxSmoothPathSize, outTransform);
const Status smoothStatus = makeSmoothPath(navMeshQuery, startNavMeshPos, targetNavMeshPos, polygonPath,
*polygonPathSize, settings.mMaxSmoothPathSize, out);
if (smoothStatus != Status::Success)
return smoothStatus;

14
components/detournavigator/navigatorutils.hpp
View file

@ -25,9 +25,9 @@ namespace DetourNavigator
* Equal to out if no path is found.
*/
template <class OutputIterator>
inline Status findPath(const Navigator& navigator, const AgentBounds& agentBounds, const float stepSize,
const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts,
float endTolerance, OutputIterator out)
inline Status findPath(const Navigator& navigator, const AgentBounds& agentBounds, const osg::Vec3f& start,
const osg::Vec3f& end, const Flags includeFlags, const AreaCosts& areaCosts, float endTolerance,
OutputIterator out)
{
static_assert(std::is_same<typename std::iterator_traits<OutputIterator>::iterator_category,
std::output_iterator_tag>::value,
@ -36,11 +36,11 @@ namespace DetourNavigator
if (navMesh == nullptr)
return Status::NavMeshNotFound;
const Settings& settings = navigator.getSettings();
auto outTransform = withFromNavMeshCoordinates(out, settings.mRecast);
const auto locked = navMesh->lock();
return findSmoothPath(locked->getImpl(), locked->getQuery(),
toNavMeshCoordinates(settings.mRecast, agentBounds.mHalfExtents),
toNavMeshCoordinates(settings.mRecast, stepSize), toNavMeshCoordinates(settings.mRecast, start),
toNavMeshCoordinates(settings.mRecast, end), includeFlags, areaCosts, settings, endTolerance, out);
return findSmoothPath(locked->getQuery(), toNavMeshCoordinates(settings.mRecast, agentBounds.mHalfExtents),
toNavMeshCoordinates(settings.mRecast, start), toNavMeshCoordinates(settings.mRecast, end), includeFlags,
areaCosts, settings.mDetour, endTolerance, outTransform);
}
/**

6
components/detournavigator/status.hpp
View file

@ -10,9 +10,11 @@ namespace DetourNavigator
NavMeshNotFound,
StartPolygonNotFound,
EndPolygonNotFound,
TargetPolygonNotFound,
MoveAlongSurfaceFailed,
FindPathOverPolygonsFailed,
InitNavMeshQueryFailed,
FindStraightPathFailed,
};
constexpr const char* getMessage(Status value)
@ -29,12 +31,16 @@ namespace DetourNavigator
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::TargetPolygonNotFound:
return "polygon for target 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::InitNavMeshQueryFailed:
return "failed to init navmesh query";
case Status::FindStraightPathFailed:
return "failed to straight path using polygon path";
}
return "unknown error";
}