Use recastnavigation to find path
elsid
7 years ago
parent
3d97e96f55
commit
fafba8ea0c
@ -0,0 +1,159 @@
|
||||
#include "operators.hpp"
|
||||
|
||||
#include <components/detournavigator/navigator.hpp>
|
||||
#include <components/detournavigator/exceptions.hpp>
|
||||
|
||||
#include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <iterator>
|
||||
#include <deque>
|
||||
|
||||
namespace
|
||||
{
|
||||
using namespace testing;
|
||||
using namespace DetourNavigator;
|
||||
|
||||
struct DetourNavigatorNavigatorTest : Test
|
||||
{
|
||||
Settings mSettings;
|
||||
std::unique_ptr<Navigator> mNavigator;
|
||||
osg::Vec3f mPlayerPosition;
|
||||
osg::Vec3f mAgentHalfExtents;
|
||||
osg::Vec3f mStart;
|
||||
osg::Vec3f mEnd;
|
||||
std::deque<osg::Vec3f> mPath;
|
||||
std::back_insert_iterator<std::deque<osg::Vec3f>> mOut;
|
||||
|
||||
DetourNavigatorNavigatorTest()
|
||||
: mPlayerPosition(0, 0, 0)
|
||||
, mAgentHalfExtents(29, 29, 66)
|
||||
, mStart(-215, 215, 1)
|
||||
, mEnd(215, -215, 1)
|
||||
, mOut(mPath)
|
||||
{
|
||||
mSettings.mCellHeight = 0.2f;
|
||||
mSettings.mCellSize = 0.2f;
|
||||
mSettings.mDetailSampleDist = 6;
|
||||
mSettings.mDetailSampleMaxError = 1;
|
||||
mSettings.mMaxClimb = 34;
|
||||
mSettings.mMaxSimplificationError = 1.3f;
|
||||
mSettings.mMaxSlope = 49;
|
||||
mSettings.mRecastScaleFactor = 0.017647058823529415f;
|
||||
mSettings.mMaxEdgeLen = 12;
|
||||
mSettings.mMaxNavMeshQueryNodes = 2048;
|
||||
mSettings.mMaxVertsPerPoly = 6;
|
||||
mSettings.mRegionMergeSize = 20;
|
||||
mSettings.mRegionMinSize = 8;
|
||||
mSettings.mTileSize = 64;
|
||||
mSettings.mMaxPolygonPathSize = 1024;
|
||||
mSettings.mMaxSmoothPathSize = 1024;
|
||||
mSettings.mTrianglesPerChunk = 256;
|
||||
mNavigator.reset(new Navigator(mSettings));
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(DetourNavigatorNavigatorTest, update_then_find_path_should_return_path)
|
||||
{
|
||||
const std::array<btScalar, 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,
|
||||
}};
|
||||
btHeightfieldTerrainShape shape(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false);
|
||||
shape.setLocalScaling(btVector3(128, 128, 1));
|
||||
|
||||
mNavigator->addAgent(mAgentHalfExtents);
|
||||
mNavigator->addObject(1, shape, btTransform::getIdentity());
|
||||
mNavigator->update();
|
||||
mNavigator->wait();
|
||||
|
||||
mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut);
|
||||
|
||||
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
|
||||
osg::Vec3f(-215, 215, 1.85963428020477294921875),
|
||||
osg::Vec3f(-194.9653167724609375, 194.9653167724609375, -6.5760211944580078125),
|
||||
osg::Vec3f(-174.930633544921875, 174.930633544921875, -15.01167774200439453125),
|
||||
osg::Vec3f(-154.8959503173828125, 154.8959503173828125, -23.4473323822021484375),
|
||||
osg::Vec3f(-134.86126708984375, 134.86126708984375, -31.8829898834228515625),
|
||||
osg::Vec3f(-114.82657623291015625, 114.82657623291015625, -40.3186492919921875),
|
||||
osg::Vec3f(-94.7918853759765625, 94.7918853759765625, -47.39907073974609375),
|
||||
osg::Vec3f(-74.75719451904296875, 74.75719451904296875, -53.7258148193359375),
|
||||
osg::Vec3f(-54.722499847412109375, 54.722499847412109375, -60.052555084228515625),
|
||||
osg::Vec3f(-34.68780517578125, 34.68780517578125, -66.37929534912109375),
|
||||
osg::Vec3f(-14.6531162261962890625, 14.6531162261962890625, -72.70604705810546875),
|
||||
osg::Vec3f(5.3815765380859375, -5.3815765380859375, -75.35065460205078125),
|
||||
osg::Vec3f(25.41626739501953125, -25.41626739501953125, -67.96945953369140625),
|
||||
osg::Vec3f(45.450958251953125, -45.450958251953125, -60.58824920654296875),
|
||||
osg::Vec3f(65.48564910888671875, -65.48564910888671875, -53.20705413818359375),
|
||||
osg::Vec3f(85.5203399658203125, -85.5203399658203125, -45.825855255126953125),
|
||||
osg::Vec3f(105.55503082275390625, -105.55503082275390625, -38.44464874267578125),
|
||||
osg::Vec3f(125.5897216796875, -125.5897216796875, -31.063449859619140625),
|
||||
osg::Vec3f(145.6244049072265625, -145.6244049072265625, -23.6822509765625),
|
||||
osg::Vec3f(165.659088134765625, -165.659088134765625, -16.3010540008544921875),
|
||||
osg::Vec3f(185.6937713623046875, -185.6937713623046875, -8.91985416412353515625),
|
||||
osg::Vec3f(205.7284698486328125, -205.7284698486328125, -1.53864824771881103515625),
|
||||
osg::Vec3f(215, -215, 1.877177715301513671875),
|
||||
})) << mPath;
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorNavigatorTest, for_overlapping_heightfields_should_use_higher)
|
||||
{
|
||||
const std::array<btScalar, 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,
|
||||
}};
|
||||
btHeightfieldTerrainShape shape(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false);
|
||||
shape.setLocalScaling(btVector3(128, 128, 1));
|
||||
|
||||
const std::array<btScalar, 5 * 5> heightfieldData2 {{
|
||||
-25, -25, -25, -25, -25,
|
||||
-25, -25, -25, -25, -25,
|
||||
-25, -25, -25, -25, -25,
|
||||
-25, -25, -25, -25, -25,
|
||||
-25, -25, -25, -25, -25,
|
||||
}};
|
||||
btHeightfieldTerrainShape shape2(5, 5, heightfieldData2.data(), 1, 0, 0, 2, PHY_FLOAT, false);
|
||||
shape2.setLocalScaling(btVector3(128, 128, 1));
|
||||
|
||||
mNavigator->addAgent(mAgentHalfExtents);
|
||||
mNavigator->addObject(1, shape, btTransform::getIdentity());
|
||||
mNavigator->addObject(2, shape2, btTransform::getIdentity());
|
||||
mNavigator->update();
|
||||
mNavigator->wait();
|
||||
|
||||
mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, mOut);
|
||||
|
||||
EXPECT_EQ(mPath, std::deque<osg::Vec3f>({
|
||||
osg::Vec3f(-215, 215, 1.96328866481781005859375),
|
||||
osg::Vec3f(-194.9653167724609375, 194.9653167724609375, -0.2422157227993011474609375),
|
||||
osg::Vec3f(-174.930633544921875, 174.930633544921875, -2.44772052764892578125),
|
||||
osg::Vec3f(-154.8959503173828125, 154.8959503173828125, -4.653223514556884765625),
|
||||
osg::Vec3f(-134.86126708984375, 134.86126708984375, -6.858728885650634765625),
|
||||
osg::Vec3f(-114.82657623291015625, 114.82657623291015625, -9.0642337799072265625),
|
||||
osg::Vec3f(-94.7918853759765625, 94.7918853759765625, -11.26973724365234375),
|
||||
osg::Vec3f(-74.75719451904296875, 74.75719451904296875, -13.26497173309326171875),
|
||||
osg::Vec3f(-54.722499847412109375, 54.722499847412109375, -15.24860286712646484375),
|
||||
osg::Vec3f(-34.68780517578125, 34.68780517578125, -17.2322368621826171875),
|
||||
osg::Vec3f(-14.6531162261962890625, 14.6531162261962890625, -19.2158660888671875),
|
||||
osg::Vec3f(5.3815765380859375, -5.3815765380859375, -20.1338443756103515625),
|
||||
osg::Vec3f(25.41626739501953125, -25.41626739501953125, -18.150211334228515625),
|
||||
osg::Vec3f(45.450958251953125, -45.450958251953125, -16.1665802001953125),
|
||||
osg::Vec3f(65.48564910888671875, -65.48564910888671875, -14.18294811248779296875),
|
||||
osg::Vec3f(85.5203399658203125, -85.5203399658203125, -12.19931507110595703125),
|
||||
osg::Vec3f(105.55503082275390625, -105.55503082275390625, -10.08488559722900390625),
|
||||
osg::Vec3f(125.5897216796875, -125.5897216796875, -7.879383563995361328125),
|
||||
osg::Vec3f(145.6244049072265625, -145.6244049072265625, -5.673877239227294921875),
|
||||
osg::Vec3f(165.659088134765625, -165.659088134765625, -3.4683735370635986328125),
|
||||
osg::Vec3f(185.6937713623046875, -185.6937713623046875, -1.2628715038299560546875),
|
||||
osg::Vec3f(205.7284698486328125, -205.7284698486328125, 0.9426348209381103515625),
|
||||
osg::Vec3f(215, -215, 1.96328866481781005859375),
|
||||
})) << mPath;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,40 @@
|
||||
#ifndef OPENMW_TEST_SUITE_DETOURNAVIGATOR_OPERATORS_H
|
||||
#define OPENMW_TEST_SUITE_DETOURNAVIGATOR_OPERATORS_H
|
||||
|
||||
#include <components/bullethelpers/operators.hpp>
|
||||
#include <components/detournavigator/debug.hpp>
|
||||
#include <components/osghelpers/operators.hpp>
|
||||
|
||||
#include <deque>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
static inline bool operator ==(const TileBounds& lhs, const TileBounds& rhs)
|
||||
{
|
||||
return lhs.mMin == rhs.mMin && lhs.mMax == rhs.mMax;
|
||||
}
|
||||
}
|
||||
|
||||
namespace testing
|
||||
{
|
||||
template <>
|
||||
inline testing::Message& Message::operator <<(const std::deque<osg::Vec3f>& value)
|
||||
{
|
||||
(*this) << "{\n";
|
||||
for (const auto& v : value)
|
||||
{
|
||||
std::ostringstream stream;
|
||||
stream << v;
|
||||
(*this) << stream.str() << ",\n";
|
||||
}
|
||||
return (*this) << "}";
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,81 @@
|
||||
#include "operators.hpp"
|
||||
|
||||
#include <components/detournavigator/recastmeshbuilder.hpp>
|
||||
#include <components/detournavigator/settings.hpp>
|
||||
#include <components/detournavigator/recastmesh.hpp>
|
||||
#include <components/detournavigator/exceptions.hpp>
|
||||
|
||||
#include <BulletCollision/CollisionShapes/btBoxShape.h>
|
||||
#include <BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h>
|
||||
#include <BulletCollision/CollisionShapes/btTriangleMesh.h>
|
||||
#include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
|
||||
#include <BulletCollision/CollisionShapes/btCompoundShape.h>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
namespace
|
||||
{
|
||||
using namespace testing;
|
||||
using namespace DetourNavigator;
|
||||
|
||||
struct DetourNavigatorRecastMeshBuilderTest : Test
|
||||
{
|
||||
Settings mSettings;
|
||||
RecastMeshBuilder mBuilder;
|
||||
|
||||
DetourNavigatorRecastMeshBuilderTest()
|
||||
: mBuilder(mSettings)
|
||||
{
|
||||
mSettings.mRecastScaleFactor = 1.0f;
|
||||
mSettings.mTrianglesPerChunk = 256;
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(DetourNavigatorRecastMeshBuilderTest, add_bhv_triangle_mesh_shape)
|
||||
{
|
||||
btTriangleMesh mesh;
|
||||
mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0));
|
||||
btBvhTriangleMeshShape shape(&mesh, true);
|
||||
mBuilder.addObject(shape, btTransform::getIdentity());
|
||||
const auto recastMesh = mBuilder.create();
|
||||
EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
|
||||
1, 0, -1,
|
||||
-1, 0, 1,
|
||||
-1, 0, -1,
|
||||
}));
|
||||
EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorRecastMeshBuilderTest, add_transformed_bhv_triangle_mesh_shape)
|
||||
{
|
||||
btTriangleMesh mesh;
|
||||
mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0));
|
||||
btBvhTriangleMeshShape shape(&mesh, true);
|
||||
mBuilder.addObject(shape,
|
||||
btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)));
|
||||
const auto recastMesh = mBuilder.create();
|
||||
EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
|
||||
2, 3, 0,
|
||||
0, 3, 4,
|
||||
0, 3, 0,
|
||||
}));
|
||||
EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2}));
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorRecastMeshBuilderTest, add_heightfield_terrian_shape)
|
||||
{
|
||||
const std::array<btScalar, 4> heightfieldData {{0, 0, 0, 0}};
|
||||
btHeightfieldTerrainShape shape(2, 2, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false);
|
||||
mBuilder.addObject(shape, btTransform::getIdentity());
|
||||
const auto recastMesh = mBuilder.create();
|
||||
EXPECT_EQ(recastMesh->getVertices(), std::vector<float>({
|
||||
-0.5, 0, -0.5,
|
||||
-0.5, 0, 0.5,
|
||||
0.5, 0, -0.5,
|
||||
0.5, 0, -0.5,
|
||||
-0.5, 0, 0.5,
|
||||
0.5, 0, 0.5,
|
||||
}));
|
||||
EXPECT_EQ(recastMesh->getIndices(), std::vector<int>({0, 1, 2, 3, 4, 5}));
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,68 @@
|
||||
#include "operators.hpp"
|
||||
|
||||
#include <components/detournavigator/settingsutils.hpp>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
namespace
|
||||
{
|
||||
using namespace testing;
|
||||
using namespace DetourNavigator;
|
||||
|
||||
struct DetourNavigatorGetTilePositionTest : Test
|
||||
{
|
||||
Settings mSettings;
|
||||
|
||||
DetourNavigatorGetTilePositionTest()
|
||||
{
|
||||
mSettings.mCellSize = 0.5;
|
||||
mSettings.mTileSize = 64;
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(DetourNavigatorGetTilePositionTest, for_zero_coordinates_should_return_zero_tile_position)
|
||||
{
|
||||
EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(0, 0, 0)), TilePosition(0, 0));
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorGetTilePositionTest, tile_size_should_be_multiplied_by_cell_size)
|
||||
{
|
||||
EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(32, 0, 0)), TilePosition(1, 0));
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorGetTilePositionTest, tile_position_calculates_by_floor)
|
||||
{
|
||||
EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(31, 0, 0)), TilePosition(0, 0));
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorGetTilePositionTest, tile_position_depends_on_x_and_z_coordinates)
|
||||
{
|
||||
EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(32, 64, 128)), TilePosition(1, 4));
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorGetTilePositionTest, tile_position_works_for_negative_coordinates)
|
||||
{
|
||||
EXPECT_EQ(getTilePosition(mSettings, osg::Vec3f(-31, 0, -32)), TilePosition(-1, -1));
|
||||
}
|
||||
|
||||
struct DetourNavigatorMakeTileBoundsTest : Test
|
||||
{
|
||||
Settings mSettings;
|
||||
|
||||
DetourNavigatorMakeTileBoundsTest()
|
||||
{
|
||||
mSettings.mCellSize = 0.5;
|
||||
mSettings.mTileSize = 64;
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(DetourNavigatorMakeTileBoundsTest, tile_bounds_depend_on_tile_size_and_cell_size)
|
||||
{
|
||||
EXPECT_EQ(makeTileBounds(mSettings, TilePosition(0, 0)), (TileBounds {osg::Vec2f(0, 0), osg::Vec2f(32, 32)}));
|
||||
}
|
||||
|
||||
TEST_F(DetourNavigatorMakeTileBoundsTest, tile_bounds_are_multiplied_by_tile_position)
|
||||
{
|
||||
EXPECT_EQ(makeTileBounds(mSettings, TilePosition(1, 2)), (TileBounds {osg::Vec2f(32, 64), osg::Vec2f(64, 96)}));
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,113 @@
|
||||
#include "asyncnavmeshupdater.hpp"
|
||||
#include "debug.hpp"
|
||||
#include "makenavmesh.hpp"
|
||||
#include "settings.hpp"
|
||||
|
||||
#include <components/debug/debuglog.hpp>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
AsyncNavMeshUpdater::AsyncNavMeshUpdater(const Settings& settings)
|
||||
: mSettings(settings)
|
||||
, mMaxRevision(0)
|
||||
, mShouldStop()
|
||||
, mThread([&] { process(); })
|
||||
{
|
||||
}
|
||||
|
||||
AsyncNavMeshUpdater::~AsyncNavMeshUpdater()
|
||||
{
|
||||
mShouldStop = true;
|
||||
std::unique_lock<std::mutex> lock(mMutex);
|
||||
mJobs.clear();
|
||||
mHasJob.notify_all();
|
||||
lock.unlock();
|
||||
mThread.join();
|
||||
}
|
||||
|
||||
void AsyncNavMeshUpdater::post(const osg::Vec3f& agentHalfExtents, const std::shared_ptr<RecastMesh>& recastMesh,
|
||||
const std::shared_ptr<NavMeshCacheItem>& navMeshCacheItem)
|
||||
{
|
||||
const std::lock_guard<std::mutex> lock(mMutex);
|
||||
mJobs[agentHalfExtents] = Job {agentHalfExtents, recastMesh, navMeshCacheItem};
|
||||
mHasJob.notify_all();
|
||||
}
|
||||
|
||||
void AsyncNavMeshUpdater::wait()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mMutex);
|
||||
mDone.wait(lock, [&] { return mJobs.empty(); });
|
||||
}
|
||||
|
||||
void AsyncNavMeshUpdater::process() throw()
|
||||
{
|
||||
log("start process jobs");
|
||||
while (!mShouldStop)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (const auto job = getNextJob())
|
||||
processJob(*job);
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
DetourNavigator::log("AsyncNavMeshUpdater::process exception: ", e.what());
|
||||
::Log(Debug::Error) << "Exception while process navmesh updated job: " << e.what();
|
||||
}
|
||||
}
|
||||
log("stop process jobs");
|
||||
}
|
||||
|
||||
void AsyncNavMeshUpdater::processJob(const Job& job)
|
||||
{
|
||||
log("process job for agent=", job.mAgentHalfExtents,
|
||||
" revision=", job.mNavMeshCacheItem->mRevision,
|
||||
" max_revision=", mMaxRevision);
|
||||
|
||||
if (job.mNavMeshCacheItem->mRevision < mMaxRevision)
|
||||
return;
|
||||
|
||||
mMaxRevision = job.mNavMeshCacheItem->mRevision;
|
||||
|
||||
const auto start = std::chrono::steady_clock::now();
|
||||
|
||||
job.mNavMeshCacheItem->mValue = makeNavMesh(job.mAgentHalfExtents, *job.mRecastMesh, mSettings);
|
||||
|
||||
const auto finish = std::chrono::steady_clock::now();
|
||||
|
||||
writeDebugFiles(job);
|
||||
|
||||
using FloatMs = std::chrono::duration<float, std::milli>;
|
||||
|
||||
log("cache updated for agent=", job.mAgentHalfExtents,
|
||||
" time=", std::chrono::duration_cast<FloatMs>(finish - start).count(), "ms");
|
||||
}
|
||||
|
||||
boost::optional<AsyncNavMeshUpdater::Job> AsyncNavMeshUpdater::getNextJob()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mMutex);
|
||||
if (mJobs.empty())
|
||||
mHasJob.wait_for(lock, std::chrono::milliseconds(10));
|
||||
if (mJobs.empty())
|
||||
{
|
||||
mDone.notify_all();
|
||||
return boost::none;
|
||||
}
|
||||
log("got ", mJobs.size(), " jobs");
|
||||
const auto job = mJobs.begin()->second;
|
||||
mJobs.erase(mJobs.begin());
|
||||
return job;
|
||||
}
|
||||
|
||||
void AsyncNavMeshUpdater::writeDebugFiles(const Job& job) const
|
||||
{
|
||||
#ifdef OPENMW_WRITE_TO_FILE
|
||||
const auto revision = std::to_string((std::chrono::steady_clock::now()
|
||||
- std::chrono::steady_clock::time_point()).count());
|
||||
writeToFile(*job.mRecastMesh, revision);
|
||||
writeToFile(*job.mNavMeshCacheItem->mValue, revision);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,75 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H
|
||||
|
||||
#include "recastmesh.hpp"
|
||||
|
||||
#include <osg/Vec3f>
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
#include <atomic>
|
||||
#include <condition_variable>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
class dtNavMesh;
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
using NavMeshConstPtr = std::shared_ptr<const dtNavMesh>;
|
||||
|
||||
struct NavMeshCacheItem
|
||||
{
|
||||
NavMeshConstPtr mValue = nullptr;
|
||||
std::size_t mRevision;
|
||||
|
||||
NavMeshCacheItem(std::size_t mRevision)
|
||||
: mRevision(mRevision)
|
||||
{}
|
||||
};
|
||||
|
||||
class AsyncNavMeshUpdater
|
||||
{
|
||||
public:
|
||||
AsyncNavMeshUpdater(const Settings& settings);
|
||||
~AsyncNavMeshUpdater();
|
||||
|
||||
void post(const osg::Vec3f& agentHalfExtents, const std::shared_ptr<RecastMesh>& recastMesh,
|
||||
const std::shared_ptr<NavMeshCacheItem>& navMeshCacheItem);
|
||||
|
||||
void wait();
|
||||
|
||||
private:
|
||||
struct Job
|
||||
{
|
||||
osg::Vec3f mAgentHalfExtents;
|
||||
std::shared_ptr<RecastMesh> mRecastMesh;
|
||||
std::shared_ptr<NavMeshCacheItem> mNavMeshCacheItem;
|
||||
};
|
||||
|
||||
using Jobs = std::map<osg::Vec3f, Job>;
|
||||
|
||||
std::reference_wrapper<const Settings> mSettings;
|
||||
std::atomic_size_t mMaxRevision;
|
||||
std::atomic_bool mShouldStop;
|
||||
std::mutex mMutex;
|
||||
std::condition_variable mHasJob;
|
||||
std::condition_variable mDone;
|
||||
Jobs mJobs;
|
||||
std::thread mThread;
|
||||
|
||||
void process() throw();
|
||||
|
||||
void processJob(const Job& job);
|
||||
|
||||
boost::optional<Job> getNextJob();
|
||||
|
||||
void notifyHasJob();
|
||||
|
||||
void writeDebugFiles(const Job& job) const;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,25 @@
|
||||
#include "cachedrecastmeshmanager.hpp"
|
||||
#include "debug.hpp"
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
CachedRecastMeshManager::CachedRecastMeshManager(const Settings& settings)
|
||||
: mImpl(settings)
|
||||
{
|
||||
}
|
||||
|
||||
bool CachedRecastMeshManager::removeObject(std::size_t id)
|
||||
{
|
||||
if (!mImpl.removeObject(id))
|
||||
return false;
|
||||
mCached.reset();
|
||||
return true;
|
||||
}
|
||||
|
||||
std::shared_ptr<RecastMesh> CachedRecastMeshManager::getMesh()
|
||||
{
|
||||
if (!mCached)
|
||||
mCached = mImpl.getMesh();
|
||||
return mCached;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,34 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_CACHEDRECASTMESHMANAGER_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_CACHEDRECASTMESHMANAGER_H
|
||||
|
||||
#include "recastmeshmanager.hpp"
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
class CachedRecastMeshManager
|
||||
{
|
||||
public:
|
||||
CachedRecastMeshManager(const Settings& settings);
|
||||
|
||||
template <class T>
|
||||
bool addObject(std::size_t id, const T& shape, const btTransform& transform)
|
||||
{
|
||||
if (!mImpl.addObject(id, shape, transform))
|
||||
return false;
|
||||
mCached.reset();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool removeObject(std::size_t id);
|
||||
|
||||
std::shared_ptr<RecastMesh> getMesh();
|
||||
|
||||
private:
|
||||
RecastMeshManager mImpl;
|
||||
std::shared_ptr<RecastMesh> mCached;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,174 @@
|
||||
#include "chunkytrimesh.hpp"
|
||||
|
||||
#include <osg/Vec2f>
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
namespace
|
||||
{
|
||||
struct BoundsItem
|
||||
{
|
||||
Rect mBounds;
|
||||
std::ptrdiff_t mOffset;
|
||||
};
|
||||
|
||||
template <std::size_t axis>
|
||||
struct LessBoundsItem
|
||||
{
|
||||
bool operator ()(const BoundsItem& lhs, const BoundsItem& rhs) const
|
||||
{
|
||||
return lhs.mBounds.mMinBound[axis] < rhs.mBounds.mMinBound[axis];
|
||||
}
|
||||
};
|
||||
|
||||
void calcExtends(const std::vector<BoundsItem>& items, const std::size_t imin, const std::size_t imax,
|
||||
Rect& bounds)
|
||||
{
|
||||
bounds = items[imin].mBounds;
|
||||
|
||||
std::for_each(
|
||||
items.begin() + static_cast<std::ptrdiff_t>(imin) + 1,
|
||||
items.begin() + static_cast<std::ptrdiff_t>(imax),
|
||||
[&] (const BoundsItem& item)
|
||||
{
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
bounds.mMinBound[i] = std::min(bounds.mMinBound[i], item.mBounds.mMinBound[i]);
|
||||
bounds.mMaxBound[i] = std::max(bounds.mMaxBound[i], item.mBounds.mMaxBound[i]);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
void subdivide(std::vector<BoundsItem>& items, const std::size_t imin, const std::size_t imax,
|
||||
const std::size_t trisPerChunk, const std::vector<int>& inIndices,
|
||||
std::size_t& curNode, std::vector<ChunkyTriMeshNode>& nodes, std::size_t& curTri,
|
||||
std::vector<int>& outIndices)
|
||||
{
|
||||
const auto inum = imax - imin;
|
||||
const auto icur = curNode;
|
||||
|
||||
if (curNode > nodes.size())
|
||||
return;
|
||||
|
||||
ChunkyTriMeshNode& node = nodes[curNode++];
|
||||
|
||||
if (inum <= trisPerChunk)
|
||||
{
|
||||
// Leaf
|
||||
calcExtends(items, imin, imax, node.mBounds);
|
||||
|
||||
// Copy triangles.
|
||||
node.mOffset = static_cast<std::ptrdiff_t>(curTri);
|
||||
node.mSize = inum;
|
||||
|
||||
for (std::size_t i = imin; i < imax; ++i)
|
||||
{
|
||||
std::copy(
|
||||
inIndices.begin() + items[i].mOffset * 3,
|
||||
inIndices.begin() + items[i].mOffset * 3 + 3,
|
||||
outIndices.begin() + static_cast<std::ptrdiff_t>(curTri) * 3
|
||||
);
|
||||
curTri++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Split
|
||||
calcExtends(items, imin, imax, node.mBounds);
|
||||
|
||||
if (node.mBounds.mMaxBound.x() - node.mBounds.mMinBound.x()
|
||||
>= node.mBounds.mMaxBound.y() - node.mBounds.mMinBound.y())
|
||||
{
|
||||
// Sort along x-axis
|
||||
std::sort(
|
||||
items.begin() + static_cast<std::ptrdiff_t>(imin),
|
||||
items.begin() + static_cast<std::ptrdiff_t>(imax),
|
||||
LessBoundsItem<0> {}
|
||||
);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Sort along y-axis
|
||||
std::sort(
|
||||
items.begin() + static_cast<std::ptrdiff_t>(imin),
|
||||
items.begin() + static_cast<std::ptrdiff_t>(imax),
|
||||
LessBoundsItem<1> {}
|
||||
);
|
||||
}
|
||||
|
||||
const auto isplit = imin + inum / 2;
|
||||
|
||||
// Left
|
||||
subdivide(items, imin, isplit, trisPerChunk, inIndices, curNode, nodes, curTri, outIndices);
|
||||
// Right
|
||||
subdivide(items, isplit, imax, trisPerChunk, inIndices, curNode, nodes, curTri, outIndices);
|
||||
|
||||
const auto iescape = static_cast<std::ptrdiff_t>(curNode) - static_cast<std::ptrdiff_t>(icur);
|
||||
// Negative index means escape.
|
||||
node.mOffset = -iescape;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ChunkyTriMesh::ChunkyTriMesh(const std::vector<float>& verts, const std::vector<int>& indices,
|
||||
const std::size_t trisPerChunk)
|
||||
: mMaxTrisPerChunk(0)
|
||||
{
|
||||
const auto trianglesCount = indices.size() / 3;
|
||||
|
||||
if (trianglesCount == 0)
|
||||
return;
|
||||
|
||||
const auto nchunks = (trianglesCount + trisPerChunk - 1) / trisPerChunk;
|
||||
|
||||
mNodes.resize(nchunks * 4);
|
||||
mIndices.resize(trianglesCount * 3);
|
||||
|
||||
// Build tree
|
||||
std::vector<BoundsItem> items(trianglesCount);
|
||||
|
||||
for (std::size_t i = 0; i < trianglesCount; i++)
|
||||
{
|
||||
auto& item = items[i];
|
||||
|
||||
item.mOffset = static_cast<std::ptrdiff_t>(i);
|
||||
|
||||
// Calc triangle XZ bounds.
|
||||
const auto baseIndex = static_cast<std::size_t>(indices[i * 3]) * 3;
|
||||
|
||||
item.mBounds.mMinBound.x() = item.mBounds.mMaxBound.x() = verts[baseIndex + 0];
|
||||
item.mBounds.mMinBound.y() = item.mBounds.mMaxBound.y() = verts[baseIndex + 2];
|
||||
|
||||
for (std::size_t j = 1; j < 3; ++j)
|
||||
{
|
||||
const auto index = static_cast<std::size_t>(indices[i * 3 + j]) * 3;
|
||||
|
||||
item.mBounds.mMinBound.x() = std::min(item.mBounds.mMinBound.x(), verts[index + 0]);
|
||||
item.mBounds.mMinBound.y() = std::min(item.mBounds.mMinBound.y(), verts[index + 2]);
|
||||
|
||||
item.mBounds.mMaxBound.x() = std::max(item.mBounds.mMaxBound.x(), verts[index + 0]);
|
||||
item.mBounds.mMaxBound.y() = std::max(item.mBounds.mMaxBound.y(), verts[index + 2]);
|
||||
}
|
||||
}
|
||||
|
||||
std::size_t curTri = 0;
|
||||
std::size_t curNode = 0;
|
||||
subdivide(items, 0, trianglesCount, trisPerChunk, indices, curNode, mNodes, curTri, mIndices);
|
||||
|
||||
items.clear();
|
||||
|
||||
mNodes.resize(curNode);
|
||||
|
||||
// Calc max tris per node.
|
||||
for (auto& node : mNodes)
|
||||
{
|
||||
const bool isLeaf = node.mOffset >= 0;
|
||||
if (!isLeaf)
|
||||
continue;
|
||||
if (node.mSize > mMaxTrisPerChunk)
|
||||
mMaxTrisPerChunk = node.mSize;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,93 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_CHUNKYTRIMESH_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_CHUNKYTRIMESH_H
|
||||
|
||||
#include <osg/Vec2f>
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
struct Rect
|
||||
{
|
||||
osg::Vec2f mMinBound;
|
||||
osg::Vec2f mMaxBound;
|
||||
};
|
||||
|
||||
struct ChunkyTriMeshNode
|
||||
{
|
||||
Rect mBounds;
|
||||
std::ptrdiff_t mOffset;
|
||||
std::size_t mSize;
|
||||
};
|
||||
|
||||
struct Chunk
|
||||
{
|
||||
const int* const mIndices;
|
||||
const std::size_t mSize;
|
||||
};
|
||||
|
||||
inline bool checkOverlapRect(const Rect& lhs, const Rect& rhs)
|
||||
{
|
||||
bool overlap = true;
|
||||
overlap = (lhs.mMinBound.x() > rhs.mMaxBound.x() || lhs.mMaxBound.x() < rhs.mMinBound.x()) ? false : overlap;
|
||||
overlap = (lhs.mMinBound.y() > rhs.mMaxBound.y() || lhs.mMaxBound.y() < rhs.mMinBound.y()) ? false : overlap;
|
||||
return overlap;
|
||||
}
|
||||
|
||||
class ChunkyTriMesh
|
||||
{
|
||||
public:
|
||||
/// Creates partitioned triangle mesh (AABB tree),
|
||||
/// where each node contains at max trisPerChunk triangles.
|
||||
ChunkyTriMesh(const std::vector<float>& verts, const std::vector<int>& tris, std::size_t trisPerChunk);
|
||||
|
||||
ChunkyTriMesh(const ChunkyTriMesh&) = delete;
|
||||
ChunkyTriMesh& operator=(const ChunkyTriMesh&) = delete;
|
||||
|
||||
/// Returns the chunk indices which overlap the input rectable.
|
||||
template <class OutputIterator>
|
||||
void getChunksOverlappingRect(const Rect& rect, OutputIterator out) const
|
||||
{
|
||||
// Traverse tree
|
||||
for (std::size_t i = 0; i < mNodes.size(); )
|
||||
{
|
||||
const ChunkyTriMeshNode* node = &mNodes[i];
|
||||
const bool overlap = checkOverlapRect(rect, node->mBounds);
|
||||
const bool isLeafNode = node->mOffset >= 0;
|
||||
|
||||
if (isLeafNode && overlap)
|
||||
*out++ = i;
|
||||
|
||||
if (overlap || isLeafNode)
|
||||
i++;
|
||||
else
|
||||
{
|
||||
const auto escapeIndex = -node->mOffset;
|
||||
i += static_cast<std::size_t>(escapeIndex);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Chunk getChunk(const std::size_t chunkId) const
|
||||
{
|
||||
const auto& node = mNodes[chunkId];
|
||||
return Chunk {
|
||||
mIndices.data() + node.mOffset * 3,
|
||||
node.mSize
|
||||
};
|
||||
}
|
||||
|
||||
std::size_t getMaxTrisPerChunk() const
|
||||
{
|
||||
return mMaxTrisPerChunk;
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<ChunkyTriMeshNode> mNodes;
|
||||
std::vector<int> mIndices;
|
||||
std::size_t mMaxTrisPerChunk;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,163 @@
|
||||
#include "debug.hpp"
|
||||
|
||||
#define OPENMW_WRITE_TO_FILE
|
||||
#define OPENMW_WRITE_OBJ
|
||||
|
||||
#ifdef OPENMW_WRITE_OBJ
|
||||
#include "exceptions.hpp"
|
||||
|
||||
#include <fstream>
|
||||
#endif
|
||||
|
||||
#ifdef OPENMW_WRITE_TO_FILE
|
||||
#include "exceptions.hpp"
|
||||
#include "recastmesh.hpp"
|
||||
|
||||
#include <DetourNavMesh.h>
|
||||
|
||||
#include <fstream>
|
||||
#endif
|
||||
|
||||
#include <iomanip>
|
||||
#include <limits>
|
||||
|
||||
namespace
|
||||
{
|
||||
#ifdef OPENMW_WRITE_TO_FILE
|
||||
static const int NAVMESHSET_MAGIC = 'M' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'MSET';
|
||||
static const int NAVMESHSET_VERSION = 1;
|
||||
|
||||
struct NavMeshSetHeader
|
||||
{
|
||||
int magic;
|
||||
int version;
|
||||
int numTiles;
|
||||
dtNavMeshParams params;
|
||||
};
|
||||
|
||||
struct NavMeshTileHeader
|
||||
{
|
||||
dtTileRef tileRef;
|
||||
int dataSize;
|
||||
};
|
||||
#endif
|
||||
}
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
// Use to dump scene to load from recastnavigation demo tool
|
||||
#ifdef OPENMW_WRITE_OBJ
|
||||
void writeObj(const std::vector<float>& vertices, const std::vector<int>& indices)
|
||||
{
|
||||
const auto path = std::string("scene.") + std::to_string(std::time(nullptr)) + ".obj";
|
||||
std::ofstream file(path);
|
||||
if (!file.is_open())
|
||||
throw NavigatorException("Open file failed: " + path);
|
||||
file.exceptions(std::ios::failbit | std::ios::badbit);
|
||||
file.precision(std::numeric_limits<float>::max_exponent10);
|
||||
std::size_t count = 0;
|
||||
for (auto v : vertices)
|
||||
{
|
||||
if (count % 3 == 0)
|
||||
{
|
||||
if (count != 0)
|
||||
file << '\n';
|
||||
file << 'v';
|
||||
}
|
||||
file << ' ' << v;
|
||||
++count;
|
||||
}
|
||||
file << '\n';
|
||||
count = 0;
|
||||
for (auto v : indices)
|
||||
{
|
||||
if (count % 3 == 0)
|
||||
{
|
||||
if (count != 0)
|
||||
file << '\n';
|
||||
file << 'f';
|
||||
}
|
||||
file << ' ' << (v + 1);
|
||||
++count;
|
||||
}
|
||||
file << '\n';
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef OPENMW_WRITE_TO_FILE
|
||||
void writeToFile(const RecastMesh& recastMesh, const std::string& revision)
|
||||
{
|
||||
const auto path = "recastmesh." + revision + ".obj";
|
||||
std::ofstream file(path);
|
||||
if (!file.is_open())
|
||||
throw NavigatorException("Open file failed: " + path);
|
||||
file.exceptions(std::ios::failbit | std::ios::badbit);
|
||||
file.precision(std::numeric_limits<float>::max_exponent10);
|
||||
std::size_t count = 0;
|
||||
for (auto v : recastMesh.getVertices())
|
||||
{
|
||||
if (count % 3 == 0)
|
||||
{
|
||||
if (count != 0)
|
||||
file << '\n';
|
||||
file << 'v';
|
||||
}
|
||||
file << ' ' << v;
|
||||
++count;
|
||||
}
|
||||
file << '\n';
|
||||
count = 0;
|
||||
for (auto v : recastMesh.getIndices())
|
||||
{
|
||||
if (count % 3 == 0)
|
||||
{
|
||||
if (count != 0)
|
||||
file << '\n';
|
||||
file << 'f';
|
||||
}
|
||||
file << ' ' << (v + 1);
|
||||
++count;
|
||||
}
|
||||
file << '\n';
|
||||
}
|
||||
|
||||
void writeToFile(const dtNavMesh& navMesh, const std::string& revision)
|
||||
{
|
||||
const auto path = "navmesh." + revision + ".bin";
|
||||
std::ofstream file(path, std::ios::binary);
|
||||
if (!file.is_open())
|
||||
throw NavigatorException("Open file failed: " + path);
|
||||
file.exceptions(std::ios::failbit | std::ios::badbit);
|
||||
|
||||
NavMeshSetHeader header;
|
||||
header.magic = NAVMESHSET_MAGIC;
|
||||
header.version = NAVMESHSET_VERSION;
|
||||
header.numTiles = 0;
|
||||
for (int i = 0; i < navMesh.getMaxTiles(); ++i)
|
||||
{
|
||||
const auto tile = navMesh.getTile(i);
|
||||
if (!tile || !tile->header || !tile->dataSize)
|
||||
continue;
|
||||
header.numTiles++;
|
||||
}
|
||||
header.params = *navMesh.getParams();
|
||||
|
||||
using const_char_ptr = const char*;
|
||||
file.write(const_char_ptr(&header), sizeof(header));
|
||||
|
||||
for (int i = 0; i < navMesh.getMaxTiles(); ++i)
|
||||
{
|
||||
const auto tile = navMesh.getTile(i);
|
||||
if (!tile || !tile->header || !tile->dataSize)
|
||||
continue;
|
||||
|
||||
NavMeshTileHeader tileHeader;
|
||||
tileHeader.tileRef = navMesh.getTileRef(tile);
|
||||
tileHeader.dataSize = tile->dataSize;
|
||||
|
||||
file.write(const_char_ptr(&tileHeader), sizeof(tileHeader));
|
||||
file.write(const_char_ptr(tile->data), tile->dataSize);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
@ -0,0 +1,63 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H
|
||||
|
||||
#include "tilebounds.hpp"
|
||||
|
||||
#include <components/bullethelpers/operators.hpp>
|
||||
#include <components/osghelpers/operators.hpp>
|
||||
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
#ifdef OPENMW_WRITE_OBJ
|
||||
#include <vector>
|
||||
#endif
|
||||
|
||||
#ifdef OPENMW_WRITE_TO_FILE
|
||||
class dtNavMesh;
|
||||
#endif
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
inline std::ostream& operator <<(std::ostream& stream, const TileBounds& value)
|
||||
{
|
||||
return stream << "TileBounds {" << value.mMin << ", " << value.mMax << "}";
|
||||
}
|
||||
|
||||
// Use to dump scene to load from recastnavigation demo tool
|
||||
#ifdef OPENMW_WRITE_OBJ
|
||||
void writeObj(const std::vector<float>& vertices, const std::vector<int>& indices);
|
||||
#endif
|
||||
|
||||
#ifdef OPENMW_WRITE_TO_FILE
|
||||
class RecastMesh;
|
||||
|
||||
void writeToFile(const RecastMesh& recastMesh, const std::string& revision);
|
||||
|
||||
void writeToFile(const dtNavMesh& navMesh, const std::string& revision);
|
||||
#endif
|
||||
|
||||
inline void write(std::ostream& stream)
|
||||
{
|
||||
stream << '\n';
|
||||
}
|
||||
|
||||
template <class Head, class ... Tail>
|
||||
void write(std::ostream& stream, const Head& head, const Tail& ... tail)
|
||||
{
|
||||
stream << head;
|
||||
write(stream, tail ...);
|
||||
}
|
||||
|
||||
template <class ... Ts>
|
||||
void log(Ts&& ... values)
|
||||
{
|
||||
std::ostringstream stream;
|
||||
write(stream, std::forward<Ts>(values) ...);
|
||||
std::cout << stream.str();
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,66 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H
|
||||
|
||||
#include "exceptions.hpp"
|
||||
|
||||
#include <DetourStatus.h>
|
||||
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
struct WriteDtStatus
|
||||
{
|
||||
dtStatus status;
|
||||
};
|
||||
|
||||
static const std::vector<std::pair<const dtStatus, const char* const>> dtStatuses {
|
||||
{DT_FAILURE, "DT_FAILURE"},
|
||||
{DT_SUCCESS, "DT_SUCCESS"},
|
||||
{DT_IN_PROGRESS, "DT_IN_PROGRESS"},
|
||||
{DT_WRONG_MAGIC, "DT_WRONG_MAGIC"},
|
||||
{DT_WRONG_VERSION, "DT_WRONG_VERSION"},
|
||||
{DT_OUT_OF_MEMORY, "DT_OUT_OF_MEMORY"},
|
||||
{DT_INVALID_PARAM, "DT_INVALID_PARAM"},
|
||||
{DT_BUFFER_TOO_SMALL, "DT_BUFFER_TOO_SMALL"},
|
||||
{DT_OUT_OF_NODES, "DT_OUT_OF_NODES"},
|
||||
{DT_PARTIAL_RESULT, "DT_PARTIAL_RESULT"},
|
||||
};
|
||||
|
||||
inline std::ostream& operator <<(std::ostream& stream, const WriteDtStatus& value)
|
||||
{
|
||||
for (const auto& status : dtStatuses)
|
||||
if (value.status & status.first)
|
||||
stream << status.second << " ";
|
||||
return stream;
|
||||
}
|
||||
|
||||
inline void checkDtStatus(dtStatus status, const char* call, int line)
|
||||
{
|
||||
if (!dtStatusSucceed(status))
|
||||
{
|
||||
std::ostringstream message;
|
||||
message << call << " failed with status=" << WriteDtStatus {status} << " at " __FILE__ ":" << line;
|
||||
throw NavigatorException(message.str());
|
||||
}
|
||||
}
|
||||
|
||||
inline void checkDtResult(bool result, const char* call, int line)
|
||||
{
|
||||
if (!result)
|
||||
{
|
||||
std::ostringstream message;
|
||||
message << call << " failed at " __FILE__ ":" << line;
|
||||
throw NavigatorException(message.str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#define OPENMW_CHECK_DT_STATUS(call) \
|
||||
do { DetourNavigator::checkDtStatus((call), #call, __LINE__); } while (false)
|
||||
|
||||
#define OPENMW_CHECK_DT_RESULT(call) \
|
||||
do { DetourNavigator::checkDtResult((call), #call, __LINE__); } while (false)
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,15 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_EXCEPTIONS_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_EXCEPTIONS_H
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
struct NavigatorException : std::runtime_error
|
||||
{
|
||||
NavigatorException(const std::string& message) : std::runtime_error(message) {}
|
||||
NavigatorException(const char* message) : std::runtime_error(message) {}
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,143 @@
|
||||
#include "findsmoothpath.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
std::vector<dtPolyRef> fixupCorridor(const std::vector<dtPolyRef>& path, const std::vector<dtPolyRef>& visited)
|
||||
{
|
||||
std::vector<dtPolyRef>::const_reverse_iterator furthestVisited;
|
||||
|
||||
// Find furthest common polygon.
|
||||
const auto it = std::find_if(path.rbegin(), path.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 == path.rend())
|
||||
return path;
|
||||
const auto furthestPath = it.base() - 1;
|
||||
|
||||
// Concatenate paths.
|
||||
|
||||
// visited: a_1 ... a_n x b_1 ... b_n
|
||||
// furthestVisited ^
|
||||
// path: C x D
|
||||
// ^ furthestPath
|
||||
// result: x b_n ... b_1 D
|
||||
|
||||
std::vector<dtPolyRef> result;
|
||||
result.reserve(static_cast<std::size_t>(furthestVisited - visited.rbegin())
|
||||
+ static_cast<std::size_t>(path.end() - furthestPath) - 1);
|
||||
std::copy(visited.rbegin(), furthestVisited + 1, std::back_inserter(result));
|
||||
std::copy(furthestPath + 1, path.end(), std::back_inserter(result));
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 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::vector<dtPolyRef> fixupShortcuts(const std::vector<dtPolyRef>& path, const dtNavMeshQuery& navQuery)
|
||||
{
|
||||
if (path.size() < 3)
|
||||
return path;
|
||||
|
||||
// Get connected polygons
|
||||
const dtMeshTile* tile = 0;
|
||||
const dtPoly* poly = 0;
|
||||
if (dtStatusFailed(navQuery.getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly)))
|
||||
return path;
|
||||
|
||||
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, path.size()) - 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 path;
|
||||
|
||||
std::vector<dtPolyRef> result;
|
||||
const auto offset = cut - 1;
|
||||
result.reserve(1 + path.size() - offset);
|
||||
result.push_back(path.front());
|
||||
std::copy(path.begin() + std::ptrdiff_t(offset), path.end(), std::back_inserter(result));
|
||||
return result;
|
||||
}
|
||||
|
||||
boost::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos,
|
||||
const osg::Vec3f& endPos, const float minTargetDist, const std::vector<dtPolyRef>& path)
|
||||
{
|
||||
// Find steer target.
|
||||
SteerTarget result;
|
||||
const int MAX_STEER_POINTS = 3;
|
||||
std::array<float, MAX_STEER_POINTS * 3> steerPath;
|
||||
std::array<unsigned char, MAX_STEER_POINTS> steerPathFlags;
|
||||
std::array<dtPolyRef, MAX_STEER_POINTS> steerPathPolys;
|
||||
int nsteerPath = 0;
|
||||
navQuery.findStraightPath(startPos.ptr(), endPos.ptr(), path.data(), int(path.size()), steerPath.data(),
|
||||
steerPathFlags.data(), steerPathPolys.data(), &nsteerPath, MAX_STEER_POINTS);
|
||||
assert(nsteerPath >= 0);
|
||||
if (!nsteerPath)
|
||||
return boost::none;
|
||||
|
||||
// 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(makeOsgVec3f(&steerPath[ns * 3]), startPos, minTargetDist, 1000.0f))
|
||||
break;
|
||||
ns++;
|
||||
}
|
||||
// Failed to find good point to steer to.
|
||||
if (ns >= static_cast<std::size_t>(nsteerPath))
|
||||
return boost::none;
|
||||
|
||||
dtVcopy(result.steerPos.ptr(), &steerPath[ns * 3]);
|
||||
result.steerPos.y() = startPos[1];
|
||||
result.steerPosFlag = steerPathFlags[ns];
|
||||
result.steerPosRef = steerPathPolys[ns];
|
||||
|
||||
return result;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,266 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H
|
||||
|
||||
#include "dtstatus.hpp"
|
||||
#include "exceptions.hpp"
|
||||
#include "settings.hpp"
|
||||
#include "settingsutils.hpp"
|
||||
|
||||
#include <DetourCommon.h>
|
||||
#include <DetourNavMesh.h>
|
||||
#include <DetourNavMeshQuery.h>
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
#include <osg/Vec3f>
|
||||
|
||||
#include <vector>
|
||||
|
||||
class dtNavMesh;
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
struct Settings;
|
||||
|
||||
inline osg::Vec3f makeOsgVec3f(const float* values)
|
||||
{
|
||||
return osg::Vec3f(values[0], values[1], values[2]);
|
||||
}
|
||||
|
||||
inline bool inRange(const osg::Vec3f& v1, const osg::Vec3f& v2, const float r, const float h)
|
||||
{
|
||||
const auto d = v2 - v1;
|
||||
return (d.x() * d.x() + d.z() * d.z()) < r * r && std::abs(d.y()) < h;
|
||||
}
|
||||
|
||||
std::vector<dtPolyRef> fixupCorridor(const std::vector<dtPolyRef>& path, 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::vector<dtPolyRef> fixupShortcuts(const std::vector<dtPolyRef>& path, const dtNavMeshQuery& navQuery);
|
||||
|
||||
struct SteerTarget
|
||||
{
|
||||
osg::Vec3f steerPos;
|
||||
unsigned char steerPosFlag;
|
||||
dtPolyRef steerPosRef;
|
||||
};
|
||||
|
||||
boost::optional<SteerTarget> getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos,
|
||||
const osg::Vec3f& endPos, const float minTargetDist, const std::vector<dtPolyRef>& path);
|
||||
|
||||
template <class OutputIterator>
|
||||
class OutputTransformIterator
|
||||
{
|
||||
public:
|
||||
OutputTransformIterator(OutputIterator& impl, const Settings& settings)
|
||||
: mImpl(impl), mSettings(settings)
|
||||
{
|
||||
}
|
||||
|
||||
OutputTransformIterator& operator *()
|
||||
{
|
||||
return *this;
|
||||
}
|
||||
|
||||
OutputTransformIterator& operator ++(int)
|
||||
{
|
||||
mImpl++;
|
||||
return *this;
|
||||
}
|
||||
|
||||
OutputTransformIterator& operator =(const osg::Vec3f& value)
|
||||
{
|
||||
*mImpl = fromNavMeshCoordinates(mSettings, value);
|
||||
return *this;
|
||||
}
|
||||
|
||||
private:
|
||||
OutputIterator& mImpl;
|
||||
const Settings& mSettings;
|
||||
};
|
||||
|
||||
template <class OutputIterator>
|
||||
OutputIterator makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery,
|
||||
const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end,
|
||||
std::vector<dtPolyRef> polygonPath, 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(), 0);
|
||||
|
||||
osg::Vec3f targetPos;
|
||||
navMeshQuery.closestPointOnPoly(polygonPath.back(), end.ptr(), targetPos.ptr(), 0);
|
||||
|
||||
const float STEP_SIZE = 0.5f;
|
||||
const 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 (!polygonPath.empty() && smoothPathSize < maxSmoothPathSize)
|
||||
{
|
||||
// Find location to steer towards.
|
||||
const auto steerTarget = getSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, polygonPath);
|
||||
|
||||
if (!steerTarget)
|
||||
break;
|
||||
|
||||
const bool endOfPath = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_END);
|
||||
const bool offMeshConnection = bool(steerTarget->steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
|
||||
|
||||
// Find movement delta.
|
||||
const osg::Vec3f delta = steerTarget->steerPos - 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 < STEP_SIZE)
|
||||
len = 1;
|
||||
else
|
||||
len = STEP_SIZE / len;
|
||||
|
||||
const osg::Vec3f moveTgt = iterPos + delta * len;
|
||||
|
||||
// Move
|
||||
osg::Vec3f result;
|
||||
std::vector<dtPolyRef> visited(16);
|
||||
int nvisited = 0;
|
||||
OPENMW_CHECK_DT_STATUS(navMeshQuery.moveAlongSurface(polygonPath.front(), iterPos.ptr(), moveTgt.ptr(),
|
||||
&filter, result.ptr(), visited.data(), &nvisited, int(visited.size())));
|
||||
|
||||
assert(nvisited >= 0);
|
||||
assert(nvisited <= int(visited.size()));
|
||||
visited.resize(static_cast<std::size_t>(nvisited));
|
||||
|
||||
polygonPath = fixupCorridor(polygonPath, visited);
|
||||
polygonPath = fixupShortcuts(polygonPath, navMeshQuery);
|
||||
|
||||
float h = 0;
|
||||
navMeshQuery.getPolyHeight(polygonPath.front(), result.ptr(), &h);
|
||||
result.y() = h;
|
||||
iterPos = result;
|
||||
|
||||
// Handle end of path and off-mesh links when close enough.
|
||||
if (endOfPath && inRange(iterPos, steerTarget->steerPos, SLOP, 1.0f))
|
||||
{
|
||||
// Reached end of path.
|
||||
iterPos = targetPos;
|
||||
*out++ = iterPos;
|
||||
++smoothPathSize;
|
||||
break;
|
||||
}
|
||||
else if (offMeshConnection && inRange(iterPos, steerTarget->steerPos, SLOP, 1.0f))
|
||||
{
|
||||
// Advance the path up to and over the off-mesh connection.
|
||||
dtPolyRef prevRef = 0;
|
||||
dtPolyRef polyRef = polygonPath.front();
|
||||
std::size_t npos = 0;
|
||||
while (npos < polygonPath.size() && polyRef != steerTarget->steerPosRef)
|
||||
{
|
||||
prevRef = polyRef;
|
||||
polyRef = polygonPath[npos];
|
||||
++npos;
|
||||
}
|
||||
std::copy(polygonPath.begin() + std::ptrdiff_t(npos), polygonPath.end(), polygonPath.begin());
|
||||
polygonPath.resize(polygonPath.size() - 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.
|
||||
iterPos = endPos;
|
||||
float eh = 0.0f;
|
||||
OPENMW_CHECK_DT_STATUS(navMeshQuery.getPolyHeight(polygonPath.front(), iterPos.ptr(), &eh));
|
||||
iterPos.y() = eh;
|
||||
}
|
||||
}
|
||||
|
||||
// Store results.
|
||||
*out++ = iterPos;
|
||||
++smoothPathSize;
|
||||
}
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
template <class OutputIterator>
|
||||
OutputIterator findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents,
|
||||
const osg::Vec3f& start, const osg::Vec3f& end, const Settings& settings, OutputIterator out)
|
||||
{
|
||||
dtNavMeshQuery navMeshQuery;
|
||||
OPENMW_CHECK_DT_STATUS(navMeshQuery.init(&navMesh, settings.mMaxNavMeshQueryNodes));
|
||||
|
||||
dtQueryFilter queryFilter;
|
||||
|
||||
dtPolyRef startRef = 0;
|
||||
osg::Vec3f startPolygonPosition;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
const auto status = navMeshQuery.findNearestPoly(start.ptr(), (halfExtents * (1 << i)).ptr(), &queryFilter,
|
||||
&startRef, startPolygonPosition.ptr());
|
||||
if (!dtStatusFailed(status) && startRef != 0)
|
||||
break;
|
||||
}
|
||||
|
||||
if (startRef == 0)
|
||||
throw NavigatorException("start polygon is not found at " __FILE__ ":" + std::to_string(__LINE__));
|
||||
|
||||
dtPolyRef endRef = 0;
|
||||
osg::Vec3f endPolygonPosition;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
const auto status = navMeshQuery.findNearestPoly(end.ptr(), (halfExtents * (1 << i)).ptr(), &queryFilter,
|
||||
&endRef, endPolygonPosition.ptr());
|
||||
if (!dtStatusFailed(status) && endRef != 0)
|
||||
break;
|
||||
}
|
||||
|
||||
if (endRef == 0)
|
||||
throw NavigatorException("end polygon is not found at " __FILE__ ":" + std::to_string(__LINE__));
|
||||
|
||||
std::vector<dtPolyRef> polygonPath(settings.mMaxPolygonPathSize);
|
||||
int pathLen = 0;
|
||||
OPENMW_CHECK_DT_STATUS(navMeshQuery.findPath(startRef, endRef, start.ptr(), end.ptr(), &queryFilter,
|
||||
polygonPath.data(), &pathLen, static_cast<int>(polygonPath.size())));
|
||||
|
||||
assert(pathLen >= 0);
|
||||
|
||||
polygonPath.resize(static_cast<std::size_t>(pathLen));
|
||||
|
||||
if (polygonPath.empty() || polygonPath.back() != endRef)
|
||||
return out;
|
||||
|
||||
makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, std::move(polygonPath),
|
||||
settings.mMaxSmoothPathSize, OutputTransformIterator<OutputIterator>(out, settings));
|
||||
|
||||
return out;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,288 @@
|
||||
#include "makenavmesh.hpp"
|
||||
#include "chunkytrimesh.hpp"
|
||||
#include "dtstatus.hpp"
|
||||
#include "exceptions.hpp"
|
||||
#include "recastmesh.hpp"
|
||||
#include "settings.hpp"
|
||||
#include "settingsutils.hpp"
|
||||
|
||||
#include <DetourNavMesh.h>
|
||||
#include <DetourNavMeshBuilder.h>
|
||||
#include <Recast.h>
|
||||
#include <RecastAlloc.h>
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
namespace
|
||||
{
|
||||
using namespace DetourNavigator;
|
||||
|
||||
void initPolyMeshDetail(rcPolyMeshDetail& value)
|
||||
{
|
||||
value.meshes = nullptr;
|
||||
value.verts = nullptr;
|
||||
value.tris = nullptr;
|
||||
}
|
||||
|
||||
struct PolyMeshDetailStackDeleter
|
||||
{
|
||||
void operator ()(rcPolyMeshDetail* value) const
|
||||
{
|
||||
rcFree(value->meshes);
|
||||
rcFree(value->verts);
|
||||
rcFree(value->tris);
|
||||
}
|
||||
};
|
||||
|
||||
using PolyMeshDetailStackPtr = std::unique_ptr<rcPolyMeshDetail, PolyMeshDetailStackDeleter>;
|
||||
|
||||
struct NavMeshDataValueDeleter
|
||||
{
|
||||
void operator ()(unsigned char* value) const
|
||||
{
|
||||
dtFree(value);
|
||||
}
|
||||
};
|
||||
|
||||
using NavMeshDataValue = std::unique_ptr<unsigned char, NavMeshDataValueDeleter>;
|
||||
|
||||
struct NavMeshData
|
||||
{
|
||||
NavMeshDataValue mValue;
|
||||
int mSize;
|
||||
|
||||
NavMeshData() = default;
|
||||
|
||||
NavMeshData(unsigned char* value, int size)
|
||||
: mValue(value)
|
||||
, mSize(size)
|
||||
{}
|
||||
};
|
||||
|
||||
NavMeshData makeNavMeshTileData(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
|
||||
const int tileX, const int tileY, const osg::Vec3f& boundsMin, const osg::Vec3f& boundsMax,
|
||||
const Settings& settings)
|
||||
{
|
||||
rcContext context;
|
||||
rcConfig config;
|
||||
|
||||
config.cs = settings.mCellSize;
|
||||
config.ch = settings.mCellHeight;
|
||||
config.walkableSlopeAngle = settings.mMaxSlope;
|
||||
config.walkableHeight = static_cast<int>(std::ceil(getHeight(settings, agentHalfExtents) / config.ch));
|
||||
config.walkableClimb = static_cast<int>(std::floor(getMaxClimb(settings) / config.ch));
|
||||
config.walkableRadius = static_cast<int>(std::ceil(getRadius(settings, agentHalfExtents) / config.cs));
|
||||
config.maxEdgeLen = static_cast<int>(std::round(settings.mMaxEdgeLen / config.cs));
|
||||
config.maxSimplificationError = settings.mMaxSimplificationError;
|
||||
config.minRegionArea = settings.mRegionMinSize * settings.mRegionMinSize;
|
||||
config.mergeRegionArea = settings.mRegionMergeSize * settings.mRegionMergeSize;
|
||||
config.maxVertsPerPoly = settings.mMaxVertsPerPoly;
|
||||
config.detailSampleDist = settings.mDetailSampleDist < 0.9f ? 0 : config.cs * settings.mDetailSampleDist;
|
||||
config.detailSampleMaxError = config.ch * settings.mDetailSampleMaxError;
|
||||
config.borderSize = config.walkableRadius + 3;
|
||||
config.width = settings.mTileSize + config.borderSize * 2;
|
||||
config.height = settings.mTileSize + config.borderSize * 2;
|
||||
rcVcopy(config.bmin, boundsMin.ptr());
|
||||
rcVcopy(config.bmax, boundsMax.ptr());
|
||||
config.bmin[0] -= config.borderSize * config.cs;
|
||||
config.bmin[2] -= config.borderSize * config.cs;
|
||||
config.bmax[0] += config.borderSize * config.cs;
|
||||
config.bmax[2] += config.borderSize * config.cs;
|
||||
|
||||
rcHeightfield solid;
|
||||
OPENMW_CHECK_DT_RESULT(rcCreateHeightfield(nullptr, solid, config.width, config.height,
|
||||
config.bmin, config.bmax, config.cs, config.ch));
|
||||
|
||||
{
|
||||
const auto& chunkyMesh = recastMesh.getChunkyTriMesh();
|
||||
std::vector<unsigned char> areas(chunkyMesh.getMaxTrisPerChunk(), 0);
|
||||
const osg::Vec2f tileBoundsMin(config.bmin[0], config.bmin[2]);
|
||||
const osg::Vec2f tileBoundsMax(config.bmax[0], config.bmax[2]);
|
||||
std::vector<std::size_t> cids;
|
||||
chunkyMesh.getChunksOverlappingRect(Rect {tileBoundsMin, tileBoundsMax}, std::back_inserter(cids));
|
||||
|
||||
if (cids.empty())
|
||||
return NavMeshData();
|
||||
|
||||
for (const auto cid : cids)
|
||||
{
|
||||
const auto chunk = chunkyMesh.getChunk(cid);
|
||||
|
||||
std::fill(
|
||||
areas.begin(),
|
||||
std::min(areas.begin() + static_cast<std::ptrdiff_t>(chunk.mSize),
|
||||
areas.end()),
|
||||
0
|
||||
);
|
||||
|
||||
rcMarkWalkableTriangles(
|
||||
&context,
|
||||
config.walkableSlopeAngle,
|
||||
recastMesh.getVertices().data(),
|
||||
static_cast<int>(recastMesh.getVerticesCount()),
|
||||
chunk.mIndices,
|
||||
static_cast<int>(chunk.mSize),
|
||||
areas.data()
|
||||
);
|
||||
|
||||
OPENMW_CHECK_DT_RESULT(rcRasterizeTriangles(
|
||||
&context,
|
||||
recastMesh.getVertices().data(),
|
||||
static_cast<int>(recastMesh.getVerticesCount()),
|
||||
chunk.mIndices,
|
||||
areas.data(),
|
||||
static_cast<int>(chunk.mSize),
|
||||
solid,
|
||||
config.walkableClimb
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
rcFilterLowHangingWalkableObstacles(&context, config.walkableClimb, solid);
|
||||
rcFilterLedgeSpans(&context, config.walkableHeight, config.walkableClimb, solid);
|
||||
rcFilterWalkableLowHeightSpans(&context, config.walkableHeight, solid);
|
||||
|
||||
rcPolyMesh polyMesh;
|
||||
rcPolyMeshDetail polyMeshDetail;
|
||||
initPolyMeshDetail(polyMeshDetail);
|
||||
const PolyMeshDetailStackPtr polyMeshDetailPtr(&polyMeshDetail);
|
||||
{
|
||||
rcCompactHeightfield compact;
|
||||
|
||||
OPENMW_CHECK_DT_RESULT(rcBuildCompactHeightfield(&context, config.walkableHeight, config.walkableClimb,
|
||||
solid, compact));
|
||||
OPENMW_CHECK_DT_RESULT(rcErodeWalkableArea(&context, config.walkableRadius, compact));
|
||||
OPENMW_CHECK_DT_RESULT(rcBuildDistanceField(&context, compact));
|
||||
OPENMW_CHECK_DT_RESULT(rcBuildRegions(&context, compact, config.borderSize, config.minRegionArea,
|
||||
config.mergeRegionArea));
|
||||
|
||||
rcContourSet contourSet;
|
||||
OPENMW_CHECK_DT_RESULT(rcBuildContours(&context, compact, config.maxSimplificationError, config.maxEdgeLen,
|
||||
contourSet));
|
||||
|
||||
if (contourSet.nconts == 0)
|
||||
return NavMeshData();
|
||||
|
||||
OPENMW_CHECK_DT_RESULT(rcBuildPolyMesh(&context, contourSet, config.maxVertsPerPoly, polyMesh));
|
||||
OPENMW_CHECK_DT_RESULT(rcBuildPolyMeshDetail(&context, polyMesh, compact, config.detailSampleDist,
|
||||
config.detailSampleMaxError, polyMeshDetail));
|
||||
}
|
||||
|
||||
for (int i = 0; i < polyMesh.npolys; ++i)
|
||||
if (polyMesh.areas[i] == RC_WALKABLE_AREA)
|
||||
polyMesh.flags[i] = 1;
|
||||
|
||||
dtNavMeshCreateParams params;
|
||||
params.verts = polyMesh.verts;
|
||||
params.vertCount = polyMesh.nverts;
|
||||
params.polys = polyMesh.polys;
|
||||
params.polyAreas = polyMesh.areas;
|
||||
params.polyFlags = polyMesh.flags;
|
||||
params.polyCount = polyMesh.npolys;
|
||||
params.nvp = polyMesh.nvp;
|
||||
params.detailMeshes = polyMeshDetail.meshes;
|
||||
params.detailVerts = polyMeshDetail.verts;
|
||||
params.detailVertsCount = polyMeshDetail.nverts;
|
||||
params.detailTris = polyMeshDetail.tris;
|
||||
params.detailTriCount = polyMeshDetail.ntris;
|
||||
params.offMeshConVerts = nullptr;
|
||||
params.offMeshConRad = nullptr;
|
||||
params.offMeshConDir = nullptr;
|
||||
params.offMeshConAreas = nullptr;
|
||||
params.offMeshConFlags = nullptr;
|
||||
params.offMeshConUserID = nullptr;
|
||||
params.offMeshConCount = 0;
|
||||
params.walkableHeight = getHeight(settings, agentHalfExtents);
|
||||
params.walkableRadius = getRadius(settings, agentHalfExtents);
|
||||
params.walkableClimb = getMaxClimb(settings);
|
||||
rcVcopy(params.bmin, polyMesh.bmin);
|
||||
rcVcopy(params.bmax, polyMesh.bmax);
|
||||
params.cs = config.cs;
|
||||
params.ch = config.ch;
|
||||
params.buildBvTree = true;
|
||||
params.userId = 0;
|
||||
params.tileX = tileX;
|
||||
params.tileY = tileY;
|
||||
params.tileLayer = 0;
|
||||
|
||||
unsigned char* navMeshData;
|
||||
int navMeshDataSize;
|
||||
OPENMW_CHECK_DT_RESULT(dtCreateNavMeshData(¶ms, &navMeshData, &navMeshDataSize));
|
||||
|
||||
return NavMeshData(navMeshData, navMeshDataSize);
|
||||
}
|
||||
|
||||
int nextPow2(int v)
|
||||
{
|
||||
v--;
|
||||
v |= v >> 1;
|
||||
v |= v >> 2;
|
||||
v |= v >> 4;
|
||||
v |= v >> 8;
|
||||
v |= v >> 16;
|
||||
v++;
|
||||
return v;
|
||||
}
|
||||
|
||||
NavMeshPtr makeNavMeshWithMultiTiles(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh,
|
||||
const Settings& settings)
|
||||
{
|
||||
osg::Vec3f boundsMin;
|
||||
osg::Vec3f boundsMax;
|
||||
rcCalcBounds(recastMesh.getVertices().data(), static_cast<int>(recastMesh.getVerticesCount()),
|
||||
boundsMin.ptr(), boundsMax.ptr());
|
||||
|
||||
const auto minTilePosition = getTilePosition(settings, boundsMin);
|
||||
const auto maxTilePosition = getTilePosition(settings, boundsMax);
|
||||
|
||||
const auto tileWidth = maxTilePosition.x() - minTilePosition.x() + 1;
|
||||
const auto tileHeight = maxTilePosition.y() - minTilePosition.y() + 1;
|
||||
|
||||
// Max tiles and max polys affect how the tile IDs are caculated.
|
||||
// There are 22 bits available for identifying a tile and a polygon.
|
||||
const auto tileBits = std::min(static_cast<int>(std::log2(nextPow2(tileWidth * tileHeight))), 14);
|
||||
const auto polyBits = 22 - tileBits;
|
||||
const auto maxTiles = 1 << tileBits;
|
||||
const auto maxPolysPerTile = 1 << polyBits;
|
||||
|
||||
dtNavMeshParams params;
|
||||
std::fill_n(params.orig, 3, 0.0f);
|
||||
params.tileWidth = getTileSize(settings);
|
||||
params.tileHeight = getTileSize(settings);
|
||||
params.maxTiles = maxTiles;
|
||||
params.maxPolys = maxPolysPerTile;
|
||||
|
||||
NavMeshPtr navMesh(dtAllocNavMesh(), &dtFreeNavMesh);
|
||||
OPENMW_CHECK_DT_STATUS(navMesh->init(¶ms));
|
||||
|
||||
for (int y = minTilePosition.y(); y <= maxTilePosition.y(); ++y)
|
||||
{
|
||||
for (int x = minTilePosition.x(); x <= maxTilePosition.x(); ++x)
|
||||
{
|
||||
const auto tileBounds = makeTileBounds(settings, TilePosition(x, y));
|
||||
const osg::Vec3f tileBorderMin(tileBounds.mMin.x(), boundsMin.y() - 1, tileBounds.mMin.y());
|
||||
const osg::Vec3f tileBorderMax(tileBounds.mMax.x(), boundsMax.y() + 1, tileBounds.mMax.y());
|
||||
|
||||
auto navMeshData = makeNavMeshTileData(agentHalfExtents, recastMesh, x, y,
|
||||
tileBorderMin, tileBorderMax, settings);
|
||||
|
||||
if (!navMeshData.mValue)
|
||||
continue;
|
||||
|
||||
OPENMW_CHECK_DT_STATUS(navMesh->addTile(navMeshData.mValue.get(), navMeshData.mSize,
|
||||
DT_TILE_FREE_DATA, 0, 0));
|
||||
navMeshData.mValue.release();
|
||||
}
|
||||
}
|
||||
|
||||
return navMesh;
|
||||
}
|
||||
}
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
NavMeshPtr makeNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, const Settings& settings)
|
||||
{
|
||||
return makeNavMeshWithMultiTiles(agentHalfExtents, recastMesh, settings);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,20 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H
|
||||
|
||||
#include <osg/Vec3f>
|
||||
|
||||
#include <memory>
|
||||
|
||||
class dtNavMesh;
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
class RecastMesh;
|
||||
struct Settings;
|
||||
|
||||
using NavMeshPtr = std::shared_ptr<dtNavMesh>;
|
||||
|
||||
NavMeshPtr makeNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, const Settings& settings);
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,42 @@
|
||||
#include "navigator.hpp"
|
||||
#include "debug.hpp"
|
||||
#include "settingsutils.hpp"
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
Navigator::Navigator(const Settings& settings)
|
||||
: mSettings(settings)
|
||||
, mNavMeshManager(mSettings)
|
||||
{
|
||||
}
|
||||
|
||||
void Navigator::addAgent(const osg::Vec3f& agentHalfExtents)
|
||||
{
|
||||
++mAgents[agentHalfExtents];
|
||||
}
|
||||
|
||||
void Navigator::removeAgent(const osg::Vec3f& agentHalfExtents)
|
||||
{
|
||||
const auto it = mAgents.find(agentHalfExtents);
|
||||
if (it == mAgents.end() || --it->second)
|
||||
return;
|
||||
mAgents.erase(it);
|
||||
mNavMeshManager.reset(agentHalfExtents);
|
||||
}
|
||||
|
||||
bool Navigator::removeObject(std::size_t id)
|
||||
{
|
||||
return mNavMeshManager.removeObject(id);
|
||||
}
|
||||
|
||||
void Navigator::update()
|
||||
{
|
||||
for (const auto& v : mAgents)
|
||||
mNavMeshManager.update(v.first);
|
||||
}
|
||||
|
||||
void Navigator::wait()
|
||||
{
|
||||
mNavMeshManager.wait();
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,50 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H
|
||||
|
||||
#include "findsmoothpath.hpp"
|
||||
#include "navmeshmanager.hpp"
|
||||
#include "settings.hpp"
|
||||
#include "settingsutils.hpp"
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
class Navigator
|
||||
{
|
||||
public:
|
||||
Navigator(const Settings& settings);
|
||||
|
||||
void addAgent(const osg::Vec3f& agentHalfExtents);
|
||||
|
||||
void removeAgent(const osg::Vec3f& agentHalfExtents);
|
||||
|
||||
template <class T>
|
||||
bool addObject(std::size_t id, const T& shape, const btTransform& transform)
|
||||
{
|
||||
return mNavMeshManager.addObject(id, shape, transform);
|
||||
}
|
||||
|
||||
bool removeObject(std::size_t id);
|
||||
|
||||
void update();
|
||||
|
||||
void wait();
|
||||
|
||||
template <class OutputIterator>
|
||||
OutputIterator findPath(const osg::Vec3f& agentHalfExtents, const osg::Vec3f& start,
|
||||
const osg::Vec3f& end, OutputIterator out) const
|
||||
{
|
||||
const auto navMesh = mNavMeshManager.getNavMesh(agentHalfExtents);
|
||||
if (!navMesh)
|
||||
return out;
|
||||
return findSmoothPath(*navMesh, toNavMeshCoordinates(mSettings, agentHalfExtents),
|
||||
toNavMeshCoordinates(mSettings, start), toNavMeshCoordinates(mSettings, end), mSettings, out);
|
||||
}
|
||||
|
||||
private:
|
||||
Settings mSettings;
|
||||
NavMeshManager mNavMeshManager;
|
||||
std::map<osg::Vec3f, std::size_t> mAgents;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,50 @@
|
||||
#include "navmeshmanager.hpp"
|
||||
#include "debug.hpp"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
NavMeshManager::NavMeshManager(const Settings& settings)
|
||||
: mRecastMeshManager(settings)
|
||||
, mAsyncNavMeshUpdater(settings)
|
||||
{
|
||||
}
|
||||
|
||||
bool NavMeshManager::removeObject(std::size_t id)
|
||||
{
|
||||
if (!mRecastMeshManager.removeObject(id))
|
||||
return false;
|
||||
++mRevision;
|
||||
return true;
|
||||
}
|
||||
|
||||
void NavMeshManager::reset(const osg::Vec3f& agentHalfExtents)
|
||||
{
|
||||
mCache.erase(agentHalfExtents);
|
||||
}
|
||||
|
||||
void NavMeshManager::update(const osg::Vec3f& agentHalfExtents)
|
||||
{
|
||||
auto it = mCache.find(agentHalfExtents);
|
||||
if (it == mCache.end())
|
||||
it = mCache.insert(std::make_pair(agentHalfExtents, std::make_shared<NavMeshCacheItem>(mRevision))).first;
|
||||
else if (it->second->mRevision >= mRevision)
|
||||
return;
|
||||
it->second->mRevision = mRevision;
|
||||
mAsyncNavMeshUpdater.post(agentHalfExtents, mRecastMeshManager.getMesh(), it->second);
|
||||
}
|
||||
|
||||
void NavMeshManager::wait()
|
||||
{
|
||||
mAsyncNavMeshUpdater.wait();
|
||||
}
|
||||
|
||||
NavMeshConstPtr NavMeshManager::getNavMesh(const osg::Vec3f& agentHalfExtents) const
|
||||
{
|
||||
const auto it = mCache.find(agentHalfExtents);
|
||||
if (it == mCache.end())
|
||||
return nullptr;
|
||||
return it->second->mValue;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,48 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHMANAGER_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHMANAGER_H
|
||||
|
||||
#include "asyncnavmeshupdater.hpp"
|
||||
#include "cachedrecastmeshmanager.hpp"
|
||||
|
||||
#include <osg/Vec3f>
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
|
||||
class dtNavMesh;
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
class NavMeshManager
|
||||
{
|
||||
public:
|
||||
NavMeshManager(const Settings& settings);
|
||||
|
||||
template <class T>
|
||||
bool addObject(std::size_t id, const T& shape, const btTransform& transform)
|
||||
{
|
||||
if (!mRecastMeshManager.addObject(id, shape, transform))
|
||||
return false;
|
||||
++mRevision;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool removeObject(std::size_t id);
|
||||
|
||||
void reset(const osg::Vec3f& agentHalfExtents);
|
||||
|
||||
void update(const osg::Vec3f& agentHalfExtents);
|
||||
|
||||
void wait();
|
||||
|
||||
NavMeshConstPtr getNavMesh(const osg::Vec3f& agentHalfExtents) const;
|
||||
|
||||
private:
|
||||
std::size_t mRevision = 0;
|
||||
CachedRecastMeshManager mRecastMeshManager;
|
||||
std::map<osg::Vec3f, std::shared_ptr<NavMeshCacheItem>> mCache;
|
||||
AsyncNavMeshUpdater mAsyncNavMeshUpdater;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,12 @@
|
||||
#include "recastmesh.hpp"
|
||||
#include "settings.hpp"
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
RecastMesh::RecastMesh(std::vector<int> indices, std::vector<float> vertices, const Settings& settings)
|
||||
: mIndices(std::move(indices))
|
||||
, mVertices(std::move(vertices))
|
||||
, mChunkyTriMesh(mVertices, mIndices, settings.mTrianglesPerChunk)
|
||||
{
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,50 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESH_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESH_H
|
||||
|
||||
#include "chunkytrimesh.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
struct Settings;
|
||||
|
||||
class RecastMesh
|
||||
{
|
||||
public:
|
||||
RecastMesh(std::vector<int> indices, std::vector<float> vertices, const Settings& settings);
|
||||
|
||||
const std::vector<int>& getIndices() const
|
||||
{
|
||||
return mIndices;
|
||||
}
|
||||
|
||||
const std::vector<float>& getVertices() const
|
||||
{
|
||||
return mVertices;
|
||||
}
|
||||
|
||||
std::size_t getVerticesCount() const
|
||||
{
|
||||
return mVertices.size() / 3;
|
||||
}
|
||||
|
||||
std::size_t getTrianglesCount() const
|
||||
{
|
||||
return mIndices.size() / 3;
|
||||
}
|
||||
|
||||
const ChunkyTriMesh& getChunkyTriMesh() const
|
||||
{
|
||||
return mChunkyTriMesh;
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<int> mIndices;
|
||||
std::vector<float> mVertices;
|
||||
ChunkyTriMesh mChunkyTriMesh;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,72 @@
|
||||
#include "recastmeshbuilder.hpp"
|
||||
#include "settings.hpp"
|
||||
#include "settingsutils.hpp"
|
||||
|
||||
#include <components/bullethelpers/processtrianglecallback.hpp>
|
||||
|
||||
#include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
|
||||
#include <BulletCollision/CollisionShapes/btConcaveShape.h>
|
||||
|
||||
namespace
|
||||
{
|
||||
osg::Vec3f makeOsgVec3f(const btVector3& value)
|
||||
{
|
||||
return osg::Vec3f(value.x(), value.y(), value.z());
|
||||
}
|
||||
}
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
using BulletHelpers::makeProcessTriangleCallback;
|
||||
|
||||
RecastMeshBuilder::RecastMeshBuilder(const Settings& settings)
|
||||
: mSettings(settings)
|
||||
{
|
||||
}
|
||||
|
||||
void RecastMeshBuilder::addObject(const btConcaveShape& shape, const btTransform& transform)
|
||||
{
|
||||
return addObject(shape, makeProcessTriangleCallback([&] (btVector3* triangle, int, int)
|
||||
{
|
||||
for (std::size_t i = 3; i > 0; --i)
|
||||
addVertex(transform(triangle[i - 1]));
|
||||
}));
|
||||
}
|
||||
|
||||
void RecastMeshBuilder::addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform)
|
||||
{
|
||||
return addObject(shape, makeProcessTriangleCallback([&] (btVector3* triangle, int, int)
|
||||
{
|
||||
for (std::size_t i = 0; i < 3; ++i)
|
||||
addVertex(transform(triangle[i]));
|
||||
}));
|
||||
}
|
||||
|
||||
std::shared_ptr<RecastMesh> RecastMeshBuilder::create() const
|
||||
{
|
||||
return std::make_shared<RecastMesh>(mIndices, mVertices, mSettings);
|
||||
}
|
||||
|
||||
void RecastMeshBuilder::reset()
|
||||
{
|
||||
mIndices.clear();
|
||||
mVertices.clear();
|
||||
}
|
||||
|
||||
void RecastMeshBuilder::addObject(const btConcaveShape& shape, btTriangleCallback&& callback)
|
||||
{
|
||||
btVector3 aabbMin;
|
||||
btVector3 aabbMax;
|
||||
shape.getAabb(btTransform::getIdentity(), aabbMin, aabbMax);
|
||||
shape.processAllTriangles(&callback, aabbMin, aabbMax);
|
||||
}
|
||||
|
||||
void RecastMeshBuilder::addVertex(const btVector3& worldPosition)
|
||||
{
|
||||
const auto navMeshPosition = toNavMeshCoordinates(mSettings, makeOsgVec3f(worldPosition));
|
||||
mIndices.push_back(static_cast<int>(mIndices.size()));
|
||||
mVertices.push_back(navMeshPosition.x());
|
||||
mVertices.push_back(navMeshPosition.y());
|
||||
mVertices.push_back(navMeshPosition.z());
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,38 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHBUILDER_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHBUILDER_H
|
||||
|
||||
#include "recastmesh.hpp"
|
||||
|
||||
class btConcaveShape;
|
||||
class btHeightfieldTerrainShape;
|
||||
class btTransform;
|
||||
class btTriangleCallback;
|
||||
class btVector3;
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
class RecastMeshBuilder
|
||||
{
|
||||
public:
|
||||
RecastMeshBuilder(const Settings& settings);
|
||||
|
||||
void addObject(const btConcaveShape& shape, const btTransform& transform);
|
||||
|
||||
void addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform);
|
||||
|
||||
std::shared_ptr<RecastMesh> create() const;
|
||||
|
||||
void reset();
|
||||
|
||||
private:
|
||||
std::reference_wrapper<const Settings> mSettings;
|
||||
std::vector<int> mIndices;
|
||||
std::vector<float> mVertices;
|
||||
|
||||
void addObject(const btConcaveShape& shape, btTriangleCallback&& callback);
|
||||
|
||||
void addVertex(const btVector3& worldPosition);
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,57 @@
|
||||
#include "recastmeshmanager.hpp"
|
||||
|
||||
#include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
RecastMeshManager::RecastMeshManager(const Settings& settings)
|
||||
: mShouldRebuild(false)
|
||||
, mMeshBuilder(settings)
|
||||
{
|
||||
}
|
||||
|
||||
bool RecastMeshManager::addObject(std::size_t id, const btHeightfieldTerrainShape& shape, const btTransform& transform)
|
||||
{
|
||||
if (!mObjects.insert(std::make_pair(id, Object {&shape, transform})).second)
|
||||
return false;
|
||||
mShouldRebuild = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool RecastMeshManager::addObject(std::size_t id, const btConcaveShape& shape, const btTransform& transform)
|
||||
{
|
||||
if (!mObjects.insert(std::make_pair(id, Object {&shape, transform})).second)
|
||||
return false;
|
||||
mShouldRebuild = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool RecastMeshManager::removeObject(std::size_t id)
|
||||
{
|
||||
if (!mObjects.erase(id))
|
||||
return false;
|
||||
mShouldRebuild = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::shared_ptr<RecastMesh> RecastMeshManager::getMesh()
|
||||
{
|
||||
rebuild();
|
||||
return mMeshBuilder.create();
|
||||
}
|
||||
|
||||
void RecastMeshManager::rebuild()
|
||||
{
|
||||
if (!mShouldRebuild)
|
||||
return;
|
||||
mMeshBuilder.reset();
|
||||
for (const auto& v : mObjects)
|
||||
{
|
||||
if (v.second.mShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
|
||||
mMeshBuilder.addObject(*static_cast<const btHeightfieldTerrainShape*>(v.second.mShape), v.second.mTransform);
|
||||
else if (v.second.mShape->isConcave())
|
||||
mMeshBuilder.addObject(*static_cast<const btConcaveShape*>(v.second.mShape), v.second.mTransform);
|
||||
}
|
||||
mShouldRebuild = false;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,42 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHMANAGER_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHMANAGER_H
|
||||
|
||||
#include "recastmeshbuilder.hpp"
|
||||
|
||||
#include <LinearMath/btTransform.h>
|
||||
|
||||
#include <unordered_map>
|
||||
|
||||
class btCollisionShape;
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
class RecastMeshManager
|
||||
{
|
||||
public:
|
||||
RecastMeshManager(const Settings& settings);
|
||||
|
||||
bool addObject(std::size_t id, const btHeightfieldTerrainShape& shape, const btTransform& transform);
|
||||
|
||||
bool addObject(std::size_t id, const btConcaveShape& shape, const btTransform& transform);
|
||||
|
||||
bool removeObject(std::size_t id);
|
||||
|
||||
std::shared_ptr<RecastMesh> getMesh();
|
||||
|
||||
private:
|
||||
struct Object
|
||||
{
|
||||
const btCollisionShape* mShape;
|
||||
btTransform mTransform;
|
||||
};
|
||||
|
||||
bool mShouldRebuild;
|
||||
RecastMeshBuilder mMeshBuilder;
|
||||
std::unordered_map<std::size_t, Object> mObjects;
|
||||
|
||||
void rebuild();
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,30 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGS_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGS_H
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
struct Settings
|
||||
{
|
||||
float mCellHeight;
|
||||
float mCellSize;
|
||||
float mDetailSampleDist;
|
||||
float mDetailSampleMaxError;
|
||||
float mMaxClimb;
|
||||
float mMaxSimplificationError;
|
||||
float mMaxSlope;
|
||||
float mRecastScaleFactor;
|
||||
int mMaxEdgeLen;
|
||||
int mMaxNavMeshQueryNodes;
|
||||
int mMaxVertsPerPoly;
|
||||
int mRegionMergeSize;
|
||||
int mRegionMinSize;
|
||||
int mTileSize;
|
||||
std::size_t mMaxPolygonPathSize;
|
||||
std::size_t mMaxSmoothPathSize;
|
||||
std::size_t mTrianglesPerChunk;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,65 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGSUTILS_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGSUTILS_H
|
||||
|
||||
#include "settings.hpp"
|
||||
#include "tileposition.hpp"
|
||||
#include "tilebounds.hpp"
|
||||
|
||||
#include <osg/Vec3f>
|
||||
|
||||
#include <utility>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
inline float getHeight(const Settings& settings,const osg::Vec3f& agentHalfExtents)
|
||||
{
|
||||
return 2.0f * agentHalfExtents.z() * settings.mRecastScaleFactor;
|
||||
}
|
||||
|
||||
inline float getMaxClimb(const Settings& settings)
|
||||
{
|
||||
return settings.mMaxClimb * settings.mRecastScaleFactor;
|
||||
}
|
||||
|
||||
inline float getRadius(const Settings& settings, const osg::Vec3f& agentHalfExtents)
|
||||
{
|
||||
return agentHalfExtents.x() * settings.mRecastScaleFactor;
|
||||
}
|
||||
|
||||
inline osg::Vec3f toNavMeshCoordinates(const Settings& settings, osg::Vec3f position)
|
||||
{
|
||||
std::swap(position.y(), position.z());
|
||||
return position * settings.mRecastScaleFactor;
|
||||
}
|
||||
|
||||
inline osg::Vec3f fromNavMeshCoordinates(const Settings& settings, osg::Vec3f position)
|
||||
{
|
||||
const auto factor = 1.0f / settings.mRecastScaleFactor;
|
||||
position *= factor;
|
||||
std::swap(position.y(), position.z());
|
||||
return position;
|
||||
}
|
||||
|
||||
inline float getTileSize(const Settings& settings)
|
||||
{
|
||||
return settings.mTileSize * settings.mCellSize;
|
||||
}
|
||||
|
||||
inline TilePosition getTilePosition(const Settings& settings, const osg::Vec3f& position)
|
||||
{
|
||||
return TilePosition(
|
||||
static_cast<int>(std::floor(position.x() / getTileSize(settings))),
|
||||
static_cast<int>(std::floor(position.z() / getTileSize(settings)))
|
||||
);
|
||||
}
|
||||
|
||||
inline TileBounds makeTileBounds(const Settings& settings, const TilePosition& tilePosition)
|
||||
{
|
||||
return TileBounds {
|
||||
osg::Vec2f(tilePosition.x(), tilePosition.y()) * getTileSize(settings),
|
||||
osg::Vec2f(tilePosition.x() + 1, tilePosition.y() + 1) * getTileSize(settings),
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,15 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEBOUNDS_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEBOUNDS_H
|
||||
|
||||
#include <osg/Vec2f>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
struct TileBounds
|
||||
{
|
||||
osg::Vec2f mMin;
|
||||
osg::Vec2f mMax;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,11 @@
|
||||
#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEPOSITION_H
|
||||
#define OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEPOSITION_H
|
||||
|
||||
#include <osg/Vec2i>
|
||||
|
||||
namespace DetourNavigator
|
||||
{
|
||||
using TilePosition = osg::Vec2i;
|
||||
}
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue