Skip to content

Commit

Permalink
Merge branch 'find_nearest_nav_mesh_position' into 'master'
Browse files Browse the repository at this point in the history 
Add Navigator and Lua API function to find nearest position on navmesh

See merge request OpenMW/openmw!2681
  • Loading branch information
Zackhasacat committed Nov 21, 2023
2 parents f3770a2 + 94b085a commit ad1d6c0
Show file tree
Hide file tree
Showing 9 changed files with 255 additions and 115 deletions.
37 changes: 37 additions & 0 deletions apps/openmw/mwlua/nearbybindings.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,15 @@
#include <components/detournavigator/navigator.hpp>
#include <components/detournavigator/navigatorutils.hpp>
#include <components/lua/luastate.hpp>
#include <components/misc/constants.hpp>
#include <components/settings/values.hpp>

#include "../mwbase/environment.hpp"
#include "../mwbase/world.hpp"
#include "../mwphysics/raycasting.hpp"
#include "../mwworld/cell.hpp"
#include "../mwworld/cellstore.hpp"
#include "../mwworld/scene.hpp"

#include "luamanagerimp.hpp"
#include "objectlists.hpp"
Expand Down Expand Up @@ -262,6 +266,39 @@ namespace MWLua
*MWBase::Environment::get().getWorld()->getNavigator(), agentBounds, from, to, includeFlags);
};

api["findNearestNavMeshPosition"] = [](const osg::Vec3f& position, const sol::optional<sol::table>& options) {
DetourNavigator::AgentBounds agentBounds = defaultAgentBounds;
std::optional<osg::Vec3f> searchAreaHalfExtents;
DetourNavigator::Flags includeFlags = defaultIncludeFlags;

if (options.has_value())
{
if (const auto& t = options->get<sol::optional<sol::table>>("agentBounds"))
{
if (const auto& v = t->get<sol::optional<DetourNavigator::CollisionShapeType>>("shapeType"))
agentBounds.mShapeType = *v;
if (const auto& v = t->get<sol::optional<osg::Vec3f>>("halfExtents"))
agentBounds.mHalfExtents = *v;
}
if (const auto& v = options->get<sol::optional<osg::Vec3f>>("searchAreaHalfExtents"))
searchAreaHalfExtents = *v;
if (const auto& v = options->get<sol::optional<DetourNavigator::Flags>>("includeFlags"))
includeFlags = *v;
}

if (!searchAreaHalfExtents.has_value())
{
const bool isEsm4 = MWBase::Environment::get().getWorldScene()->getCurrentCell()->getCell()->isEsm4();
const float halfExtents = isEsm4
? (1 + 2 * Constants::ESM4CellGridRadius) * Constants::ESM4CellSizeInUnits
: (1 + 2 * Constants::CellGridRadius) * Constants::CellSizeInUnits;
searchAreaHalfExtents = osg::Vec3f(halfExtents, halfExtents, halfExtents);
}

return DetourNavigator::findNearestNavMeshPosition(*MWBase::Environment::get().getWorld()->getNavigator(),
agentBounds, position, *searchAreaHalfExtents, includeFlags);
};

return LuaUtil::makeReadOnly(api);
}
}
206 changes: 102 additions & 104 deletions apps/openmw_test_suite/detournavigator/navigator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,22 @@ namespace
}
};

constexpr std::array<float, 5 * 5> defaultHeightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };

constexpr std::array<btScalar, 5 * 5> defaultHeightfieldDataScalar{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };

template <std::size_t size>
std::unique_ptr<btHeightfieldTerrainShape> makeSquareHeightfieldTerrainShape(
const std::array<btScalar, size>& values, btScalar heightScale = 1, int upAxis = 2,
Expand Down Expand Up @@ -150,14 +166,7 @@ namespace

TEST_F(DetourNavigatorNavigatorTest, update_then_find_path_should_return_path)
{
constexpr std::array<float, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
Expand All @@ -177,20 +186,31 @@ namespace
<< mPath;
}

TEST_F(DetourNavigatorNavigatorTest, find_path_to_the_start_position_should_contain_single_point)
{
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
auto updateGuard = mNavigator->makeUpdateGuard();
mNavigator->addHeightfield(mCellPosition, cellSize, surface, updateGuard.get());
mNavigator->update(mPlayerPosition, updateGuard.get());
updateGuard.reset();
mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);

EXPECT_EQ(findPath(*mNavigator, mAgentBounds, mStart, mStart, Flag_walk, mAreaCosts, mEndTolerance, mOut),
Status::Success);

EXPECT_THAT(mPath, ElementsAre(Vec3fEq(56.66666412353515625, 460, 1.99998295307159423828125))) << mPath;
}

TEST_F(DetourNavigatorNavigatorTest, add_object_should_change_navmesh)
{
mSettings.mWaitUntilMinDistanceToPlayer = 0;
mNavigator.reset(new NavigatorImpl(
mSettings, std::make_unique<NavMeshDb>(":memory:", std::numeric_limits<std::uint64_t>::max())));

const std::array<float, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

CollisionShapeInstance compound(std::make_unique<btCompoundShape>());
Expand Down Expand Up @@ -235,14 +255,7 @@ namespace

TEST_F(DetourNavigatorNavigatorTest, update_changed_object_should_change_navmesh)
{
const std::array<float, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

CollisionShapeInstance compound(std::make_unique<btCompoundShape>());
Expand Down Expand Up @@ -288,14 +301,7 @@ namespace

TEST_F(DetourNavigatorNavigatorTest, for_overlapping_heightfields_objects_should_use_higher)
{
const std::array<btScalar, 5 * 5> heightfieldData1{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
CollisionShapeInstance heightfield1(makeSquareHeightfieldTerrainShape(heightfieldData1));
CollisionShapeInstance heightfield1(makeSquareHeightfieldTerrainShape(defaultHeightfieldDataScalar));
heightfield1.shape().setLocalScaling(btVector3(128, 128, 1));

const std::array<btScalar, 5 * 5> heightfieldData2{ {
Expand Down Expand Up @@ -328,14 +334,7 @@ namespace

TEST_F(DetourNavigatorNavigatorTest, only_one_heightfield_per_cell_is_allowed)
{
const std::array<float, 5 * 5> heightfieldData1{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface1 = makeSquareHeightfieldSurface(heightfieldData1);
const HeightfieldSurface surface1 = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize1 = mHeightfieldTileSize * (surface1.mSize - 1);

const std::array<float, 5 * 5> heightfieldData2{ {
Expand Down Expand Up @@ -366,14 +365,8 @@ namespace
{
osg::ref_ptr<Resource::BulletShape> bulletShape(new Resource::BulletShape);

std::array<btScalar, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
std::unique_ptr<btHeightfieldTerrainShape> shapePtr = makeSquareHeightfieldTerrainShape(heightfieldData);
std::unique_ptr<btHeightfieldTerrainShape> shapePtr
= makeSquareHeightfieldTerrainShape(defaultHeightfieldDataScalar);
shapePtr->setLocalScaling(btVector3(128, 128, 1));
bulletShape->mCollisionShape.reset(shapePtr.release());

Expand Down Expand Up @@ -542,14 +535,7 @@ namespace

TEST_F(DetourNavigatorNavigatorTest, update_object_remove_and_update_then_find_path_should_return_path)
{
const std::array<btScalar, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
CollisionShapeInstance heightfield(makeSquareHeightfieldTerrainShape(heightfieldData));
CollisionShapeInstance heightfield(makeSquareHeightfieldTerrainShape(defaultHeightfieldDataScalar));
heightfield.shape().setLocalScaling(btVector3(128, 128, 1));

ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
Expand Down Expand Up @@ -579,14 +565,7 @@ namespace

TEST_F(DetourNavigatorNavigatorTest, update_heightfield_remove_and_update_then_find_path_should_return_path)
{
const std::array<float, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
Expand Down Expand Up @@ -649,14 +628,7 @@ namespace
mNavigator.reset(new NavigatorImpl(
mSettings, std::make_unique<NavMeshDb>(":memory:", std::numeric_limits<std::uint64_t>::max())));

const std::array<float, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);
const btVector3 shift = getHeightfieldShift(mCellPosition, cellSize, surface.mMinHeight, surface.mMaxHeight);

Expand Down Expand Up @@ -745,14 +717,7 @@ namespace

TEST_F(DetourNavigatorNavigatorTest, update_then_raycast_should_return_position)
{
const std::array<float, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
Expand All @@ -771,14 +736,7 @@ namespace
TEST_F(DetourNavigatorNavigatorTest,
update_for_oscillating_object_that_does_not_change_navmesh_should_not_trigger_navmesh_update)
{
const std::array<float, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

CollisionShapeInstance oscillatingBox(std::make_unique<btBoxShape>(btVector3(20, 20, 20)));
Expand Down Expand Up @@ -837,14 +795,7 @@ namespace

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, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

CollisionShapeInstance compound(std::make_unique<btCompoundShape>());
Expand All @@ -870,14 +821,7 @@ namespace

TEST_F(DetourNavigatorNavigatorTest, end_tolerance_should_extent_available_destinations)
{
const std::array<float, 5 * 5> heightfieldData{ {
0, 0, 0, 0, 0, // row 0
0, -25, -25, -25, -25, // row 1
0, -25, -100, -100, -100, // row 2
0, -25, -100, -100, -100, // row 3
0, -25, -100, -100, -100, // row 4
} };
const HeightfieldSurface surface = makeSquareHeightfieldSurface(heightfieldData);
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

CollisionShapeInstance compound(std::make_unique<btCompoundShape>());
Expand Down Expand Up @@ -1000,4 +944,58 @@ namespace

INSTANTIATE_TEST_SUITE_P(NotSupportedAgentBounds, DetourNavigatorNavigatorNotSupportedAgentBoundsTest,
ValuesIn(notSupportedAgentBounds));

TEST_F(DetourNavigatorNavigatorTest, find_nearest_nav_mesh_position_should_return_nav_mesh_position)
{
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
auto updateGuard = mNavigator->makeUpdateGuard();
mNavigator->addHeightfield(mCellPosition, cellSize, surface, updateGuard.get());
mNavigator->update(mPlayerPosition, updateGuard.get());
updateGuard.reset();
mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);

const osg::Vec3f position(250, 250, 0);
const osg::Vec3f searchAreaHalfExtents(1000, 1000, 1000);
EXPECT_THAT(findNearestNavMeshPosition(*mNavigator, mAgentBounds, position, searchAreaHalfExtents, Flag_walk),
Optional(Vec3fEq(250, 250, -62.5186)));
}

TEST_F(DetourNavigatorNavigatorTest, find_nearest_nav_mesh_position_should_return_nullopt_when_too_far)
{
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
auto updateGuard = mNavigator->makeUpdateGuard();
mNavigator->addHeightfield(mCellPosition, cellSize, surface, updateGuard.get());
mNavigator->update(mPlayerPosition, updateGuard.get());
updateGuard.reset();
mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);

const osg::Vec3f position(250, 250, 250);
const osg::Vec3f searchAreaHalfExtents(100, 100, 100);
EXPECT_EQ(findNearestNavMeshPosition(*mNavigator, mAgentBounds, position, searchAreaHalfExtents, Flag_walk),
std::nullopt);
}

TEST_F(DetourNavigatorNavigatorTest, find_nearest_nav_mesh_position_should_return_nullopt_when_flags_do_not_match)
{
const HeightfieldSurface surface = makeSquareHeightfieldSurface(defaultHeightfieldData);
const int cellSize = mHeightfieldTileSize * (surface.mSize - 1);

ASSERT_TRUE(mNavigator->addAgent(mAgentBounds));
auto updateGuard = mNavigator->makeUpdateGuard();
mNavigator->addHeightfield(mCellPosition, cellSize, surface, updateGuard.get());
mNavigator->update(mPlayerPosition, updateGuard.get());
updateGuard.reset();
mNavigator->wait(WaitConditionType::requiredTilesPresent, &mListener);

const osg::Vec3f position(250, 250, 0);
const osg::Vec3f searchAreaHalfExtents(1000, 1000, 1000);
EXPECT_EQ(findNearestNavMeshPosition(*mNavigator, mAgentBounds, position, searchAreaHalfExtents, Flag_swim),
std::nullopt);
}
}
4 changes: 2 additions & 2 deletions components/detournavigator/findsmoothpath.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ namespace DetourNavigator
std::reference_wrapper<const RecastSettings> mSettings;
};

inline std::optional<std::size_t> findPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef,
inline std::optional<std::size_t> findPolygonPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef,
const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter,
std::span<dtPolyRef> pathBuffer)
{
Expand Down Expand Up @@ -132,7 +132,7 @@ namespace DetourNavigator

std::vector<dtPolyRef> polygonPath(settings.mMaxPolygonPathSize);
const auto polygonPathSize
= findPath(navMeshQuery, startRef, endRef, startNavMeshPos, endNavMeshPos, queryFilter, polygonPath);
= findPolygonPath(navMeshQuery, startRef, endRef, startNavMeshPos, endNavMeshPos, queryFilter, polygonPath);

if (!polygonPathSize.has_value())
return Status::FindPathOverPolygonsFailed;
Expand Down
Loading

0 comments on commit ad1d6c0

Please sign in to comment.