diff --git a/CHANGELOG.md b/CHANGELOG.md index 96baa5365..524ce2cc2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,10 @@ +0.46.0 +------ + + Feature #2229: Improve pathfinding AI + Feature #3442: Default values for fallbacks from ini file + + 0.45.0 ------ diff --git a/CI/before_install.linux.sh b/CI/before_install.linux.sh old mode 100755 new mode 100644 index 6ba772680..241db283c --- a/CI/before_install.linux.sh +++ b/CI/before_install.linux.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/sh -e echo -n | openssl s_client -connect scan.coverity.com:443 | sed -ne '/-BEGIN CERTIFICATE-/,/-END CERTIFICATE-/p' | sudo tee -a /etc/ssl/certs/ca- # Set up compilers diff --git a/CI/before_install.osx.sh b/CI/before_install.osx.sh index 51b4b90be..da72563a2 100755 --- a/CI/before_install.osx.sh +++ b/CI/before_install.osx.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/sh -e brew update diff --git a/CI/before_script.msvc.sh b/CI/before_script.msvc.sh index 8ee4b7fd9..c328dd882 100644 --- a/CI/before_script.msvc.sh +++ b/CI/before_script.msvc.sh @@ -304,6 +304,10 @@ if ! [ -z $UNITY_BUILD ]; then add_cmake_opts "-DOPENMW_UNITY_BUILD=True" fi +if [ ${BITS} -eq 64 ]; then + GENERATOR="${GENERATOR} Win64" +fi + echo echo "===================================" echo "Starting prebuild on MSVC${MSVC_DISPLAY_YEAR} WIN${BITS}" @@ -324,7 +328,7 @@ if [ -z $SKIP_DOWNLOAD ]; then if [ -z $APPVEYOR ]; then download "Boost 1.67.0" \ "https://sourceforge.net/projects/boost/files/boost-binaries/1.67.0/boost_1_67_0-msvc-${MSVC_VER}-${BITS}.exe" \ - "boost-1.67.0-msvc${MSVC_YEAR}-win${BITS}.exe" + "boost-1.67.0-msvc${MSVC_VER}-win${BITS}.exe" fi # Bullet @@ -430,7 +434,7 @@ fi rm -rf Boost CI_EXTRA_INNO_OPTIONS="" [ -n "$CI" ] && CI_EXTRA_INNO_OPTIONS="//SUPPRESSMSGBOXES //LOG='boost_install.log'" - "${DEPS}/boost-1.67.0-msvc${MSVC_YEAR}-win${BITS}.exe" //DIR="${CWD_DRIVE_ROOT}" //VERYSILENT //NORESTART ${CI_EXTRA_INNO_OPTIONS} + "${DEPS}/boost-1.67.0-msvc${MSVC_VER}-win${BITS}.exe" //DIR="${CWD_DRIVE_ROOT}" //VERYSILENT //NORESTART ${CI_EXTRA_INNO_OPTIONS} mv "${CWD_DRIVE_ROOT_BASH}" "${BOOST_SDK}" fi add_cmake_opts -DBOOST_ROOT="$BOOST_SDK" \ @@ -449,7 +453,7 @@ fi else LIB_SUFFIX="0" fi - + add_cmake_opts -DBOOST_ROOT="$BOOST_SDK" \ -DBOOST_LIBRARYDIR="${BOOST_SDK}/lib${BITS}-msvc-${MSVC_VER}.${LIB_SUFFIX}" add_cmake_opts -DBoost_COMPILER="-${TOOLSET}" diff --git a/CI/before_script.osx.sh b/CI/before_script.osx.sh index 8ee01b652..16f461639 100755 --- a/CI/before_script.osx.sh +++ b/CI/before_script.osx.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/sh -e export CXX=clang++ export CC=clang diff --git a/CMakeLists.txt b/CMakeLists.txt index 44c6dc7dd..844b6ed0f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -289,9 +289,6 @@ if (APPLE) "${APP_BUNDLE_DIR}/Contents/Resources/OpenMW.icns" COPYONLY) endif (APPLE) -# Set up DEBUG define -set_directory_properties(PROPERTIES COMPILE_DEFINITIONS_DEBUG DEBUG=1) - if (NOT APPLE) set(OPENMW_MYGUI_FILES_ROOT ${OpenMW_BINARY_DIR}) set(OPENMW_SHADERS_ROOT ${OpenMW_BINARY_DIR}) @@ -578,13 +575,16 @@ if(WIN32) endif(WIN32) # Extern -IF(BUILD_OPENMW OR BUILD_OPENCS) +set(RECASTNAVIGATION_DEMO OFF CACHE BOOL "Do not build RecastDemo") +set(RECASTNAVIGATION_STATIC ON CACHE BOOL "Build recastnavigation static libraries") +set(RECASTNAVIGATION_TESTS OFF CACHE BOOL "Do not build recastnavigation tests") + +add_subdirectory (extern/recastnavigation) add_subdirectory (extern/osg-ffmpeg-videoplayer) add_subdirectory (extern/oics) if (BUILD_OPENCS) add_subdirectory (extern/osgQt) endif() -ENDIF(BUILD_OPENMW OR BUILD_OPENCS) # Components add_subdirectory (components) @@ -658,7 +658,7 @@ if (WIN32) if (USE_DEBUG_CONSOLE AND BUILD_OPENMW) set_target_properties(tes3mp PROPERTIES LINK_FLAGS_DEBUG "/SUBSYSTEM:CONSOLE") set_target_properties(tes3mp PROPERTIES LINK_FLAGS_RELWITHDEBINFO "/SUBSYSTEM:CONSOLE") - set_target_properties(tes3mp PROPERTIES COMPILE_DEFINITIONS_DEBUG "_CONSOLE") + set_target_properties(tes3mp PROPERTIES COMPILE_DEFINITIONS $<$:_CONSOLE>) elseif (BUILD_OPENMW) # Turn off debug console, debug output will be written to visual studio output instead set_target_properties(tes3mp PROPERTIES LINK_FLAGS_DEBUG "/SUBSYSTEM:WINDOWS") @@ -668,7 +668,6 @@ if (WIN32) if (BUILD_OPENMW) # Release builds don't use the debug console set_target_properties(tes3mp PROPERTIES LINK_FLAGS_RELEASE "/SUBSYSTEM:WINDOWS") - set_target_properties(tes3mp PROPERTIES COMPILE_DEFINITIONS_RELEASE "_WINDOWS") set_target_properties(tes3mp PROPERTIES LINK_FLAGS_MINSIZEREL "/SUBSYSTEM:WINDOWS") endif() diff --git a/apps/openmw/CMakeLists.txt b/apps/openmw/CMakeLists.txt index 7475def1a..488d9a59a 100644 --- a/apps/openmw/CMakeLists.txt +++ b/apps/openmw/CMakeLists.txt @@ -21,7 +21,7 @@ add_openmw_dir (mwrender actors objects renderingmanager animation rotatecontroller sky npcanimation vismask creatureanimation effectmanager util renderinginterface pathgrid rendermode weaponanimation bulletdebugdraw globalmap characterpreview camera localmap water terrainstorage ripplesimulation - renderbin actoranimation landmanager + renderbin actoranimation landmanager navmesh actorspaths ) add_openmw_dir (mwinput @@ -70,7 +70,7 @@ add_openmw_dir (mwworld ) add_openmw_dir (mwphysics - physicssystem trace collisiontype actor convert + physicssystem trace collisiontype actor convert object heightfield ) add_openmw_dir (mwclass @@ -177,6 +177,7 @@ target_link_libraries(tes3mp ${FFmpeg_LIBRARIES} ${MyGUI_LIBRARIES} ${SDL2_LIBRARY} + ${RecastNavigation_LIBRARIES} "osg-ffmpeg-videoplayer" "oics" components diff --git a/apps/openmw/main.cpp b/apps/openmw/main.cpp index dfbc3c5d7..b4cb18822 100644 --- a/apps/openmw/main.cpp +++ b/apps/openmw/main.cpp @@ -17,7 +17,6 @@ */ #if defined(_WIN32) -// For OutputDebugString #ifndef WIN32_LEAN_AND_MEAN #define WIN32_LEAN_AND_MEAN #endif diff --git a/apps/openmw/mwbase/world.hpp b/apps/openmw/mwbase/world.hpp index 590fcdcbc..6bd942166 100644 --- a/apps/openmw/mwbase/world.hpp +++ b/apps/openmw/mwbase/world.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -55,6 +56,11 @@ namespace MWMechanics struct Movement; } +namespace DetourNavigator +{ + class Navigator; +} + namespace MWWorld { class CellStore; @@ -767,6 +773,15 @@ namespace MWBase /// Preload VFX associated with this effect list virtual void preloadEffects(const ESM::EffectList* effectList) = 0; + + virtual DetourNavigator::Navigator* getNavigator() const = 0; + + virtual void updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const = 0; + + virtual void removeActorPath(const MWWorld::ConstPtr& actor) const = 0; + + virtual void setNavMeshNumberToRender(const std::size_t value) = 0; }; } diff --git a/apps/openmw/mwmechanics/actors.cpp b/apps/openmw/mwmechanics/actors.cpp index 7769503e3..ac766b5ac 100644 --- a/apps/openmw/mwmechanics/actors.cpp +++ b/apps/openmw/mwmechanics/actors.cpp @@ -1789,6 +1789,7 @@ namespace MWMechanics continue; } + MWBase::Environment::get().getWorld()->removeActorPath(iter->first); CharacterController::KillResult killResult = iter->second->getCharacterController()->kill(); if (killResult == CharacterController::Result_DeathAnimStarted) { diff --git a/apps/openmw/mwmechanics/aiactivate.cpp b/apps/openmw/mwmechanics/aiactivate.cpp index 7e50b1229..9fc7c2300 100644 --- a/apps/openmw/mwmechanics/aiactivate.cpp +++ b/apps/openmw/mwmechanics/aiactivate.cpp @@ -63,14 +63,13 @@ namespace MWMechanics actor.getClass().getCreatureStats(actor).setDrawState(DrawState_Nothing); - if (target == MWWorld::Ptr() || - !target.getRefData().getCount() || !target.getRefData().isEnabled() // Really we should check whether the target is currently registered - // with the MechanicsManager - ) - return true; //Target doesn't exist + // Stop if the target doesn't exist + // Really we should be checking whether the target is currently registered with the MechanicsManager + if (target == MWWorld::Ptr() || !target.getRefData().getCount() || !target.getRefData().isEnabled()) + return true; //Set the target destination for the actor - ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos; + const osg::Vec3f dest = target.getRefData().getPosition().asVec3(); if (pathTo(actor, dest, duration, MWBase::Environment::get().getWorld()->getMaxActivationDistance())) //Stop when you get in activation range { diff --git a/apps/openmw/mwmechanics/aibreathe.cpp b/apps/openmw/mwmechanics/aibreathe.cpp index 36acc75d5..4955f683c 100644 --- a/apps/openmw/mwmechanics/aibreathe.cpp +++ b/apps/openmw/mwmechanics/aibreathe.cpp @@ -26,9 +26,9 @@ bool MWMechanics::AiBreathe::execute (const MWWorld::Ptr& actor, CharacterContro { if (actorClass.getNpcStats(actor).getTimeToStartDrowning() < fHoldBreathTime / 2) { - actor.getClass().getCreatureStats(actor).setMovementFlag(CreatureStats::Flag_Run, true); + actorClass.getCreatureStats(actor).setMovementFlag(CreatureStats::Flag_Run, true); - actor.getClass().getMovementSettings(actor).mPosition[1] = 1; + actorClass.getMovementSettings(actor).mPosition[1] = 1; smoothTurn(actor, -180, 0); return false; diff --git a/apps/openmw/mwmechanics/aicast.cpp b/apps/openmw/mwmechanics/aicast.cpp index 948ffb3aa..38dc2fa73 100644 --- a/apps/openmw/mwmechanics/aicast.cpp +++ b/apps/openmw/mwmechanics/aicast.cpp @@ -37,7 +37,7 @@ bool MWMechanics::AiCast::execute(const MWWorld::Ptr& actor, MWMechanics::Charac if (!target) return true; - if (!mManual && !pathTo(actor, target.getRefData().getPosition().pos, duration, mDistance)) + if (!mManual && !pathTo(actor, target.getRefData().getPosition().asVec3(), duration, mDistance)) { return false; } diff --git a/apps/openmw/mwmechanics/aicombat.cpp b/apps/openmw/mwmechanics/aicombat.cpp index c6ed63de7..d990d50cc 100644 --- a/apps/openmw/mwmechanics/aicombat.cpp +++ b/apps/openmw/mwmechanics/aicombat.cpp @@ -141,7 +141,7 @@ namespace MWMechanics float targetReachedTolerance = 0.0f; if (storage.mLOS) targetReachedTolerance = storage.mAttackRange; - bool is_target_reached = pathTo(actor, target.getRefData().getPosition().pos, duration, targetReachedTolerance); + const bool is_target_reached = pathTo(actor, target.getRefData().getPosition().asVec3(), duration, targetReachedTolerance); if (is_target_reached) storage.mReadyToAttack = true; } @@ -373,7 +373,7 @@ namespace MWMechanics osg::Vec3f localPos = actor.getRefData().getPosition().asVec3(); coords.toLocal(localPos); - int closestPointIndex = PathFinder::GetClosestPoint(pathgrid, localPos); + int closestPointIndex = PathFinder::getClosestPoint(pathgrid, localPos); for (int i = 0; i < static_cast(pathgrid->mPoints.size()); i++) { if (i != closestPointIndex && getPathGridGraph(storage.mCell).isPointConnected(closestPointIndex, i)) @@ -425,7 +425,7 @@ namespace MWMechanics float dist = (actor.getRefData().getPosition().asVec3() - target.getRefData().getPosition().asVec3()).length(); if ((dist > fFleeDistance && !storage.mLOS) - || pathTo(actor, storage.mFleeDest, duration)) + || pathTo(actor, PathFinder::makeOsgVec3(storage.mFleeDest), duration)) { state = AiCombatStorage::FleeState_Idle; } diff --git a/apps/openmw/mwmechanics/aiescort.cpp b/apps/openmw/mwmechanics/aiescort.cpp index a86d13d75..3d018d780 100644 --- a/apps/openmw/mwmechanics/aiescort.cpp +++ b/apps/openmw/mwmechanics/aiescort.cpp @@ -83,24 +83,13 @@ namespace MWMechanics actor.getClass().getCreatureStats(actor).setMovementFlag(CreatureStats::Flag_Run, false); const MWWorld::Ptr follower = MWBase::Environment::get().getWorld()->getPtr(mTargetActorRefId, false); - const float* const leaderPos = actor.getRefData().getPosition().pos; - const float* const followerPos = follower.getRefData().getPosition().pos; - double differenceBetween[3]; + const osg::Vec3f leaderPos = actor.getRefData().getPosition().asVec3(); + const osg::Vec3f followerPos = follower.getRefData().getPosition().asVec3(); - for (short counter = 0; counter < 3; counter++) - differenceBetween[counter] = (leaderPos[counter] - followerPos[counter]); - - double distanceBetweenResult = - (differenceBetween[0] * differenceBetween[0]) + (differenceBetween[1] * differenceBetween[1]) + (differenceBetween[2] * - differenceBetween[2]); - - if (distanceBetweenResult <= mMaxDist * mMaxDist) + if ((leaderPos - followerPos).length2() <= mMaxDist * mMaxDist) { - ESM::Pathgrid::Point point(static_cast(mX), static_cast(mY), static_cast(mZ)); - point.mAutogenerated = 0; - point.mConnectionNum = 0; - point.mUnknown = 0; - if (pathTo(actor,point,duration)) //Returns true on path complete + const osg::Vec3f dest(mX, mY, mZ); + if (pathTo(actor, dest, duration)) //Returns true on path complete { mRemainingDuration = mDuration; return true; diff --git a/apps/openmw/mwmechanics/aifollow.cpp b/apps/openmw/mwmechanics/aifollow.cpp index f6d6ee2f8..325da26af 100644 --- a/apps/openmw/mwmechanics/aifollow.cpp +++ b/apps/openmw/mwmechanics/aifollow.cpp @@ -74,12 +74,12 @@ AiFollow::AiFollow(const ESM::AiSequence::AiFollow *follow) bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characterController, AiState& state, float duration) { - MWWorld::Ptr target = getTarget(); + const MWWorld::Ptr target = getTarget(); - if (target.isEmpty() || !target.getRefData().getCount() || !target.getRefData().isEnabled() // Really we should be checking whether the target is currently registered - // with the MechanicsManager - ) - return false; // Target is not here right now, wait for it to return + // Target is not here right now, wait for it to return + // Really we should be checking whether the target is currently registered with the MechanicsManager + if (target == MWWorld::Ptr() || !target.getRefData().getCount() || !target.getRefData().isEnabled()) + return false; actor.getClass().getCreatureStats(actor).setDrawState(DrawState_Nothing); @@ -105,6 +105,10 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte End of tes3mp addition */ + const osg::Vec3f actorPos(actor.getRefData().getPosition().asVec3()); + const osg::Vec3f targetPos(target.getRefData().getPosition().asVec3()); + const osg::Vec3f targetDir = targetPos - actorPos; + // AiFollow requires the target to be in range and within sight for the initial activation if (!mActive) { @@ -112,9 +116,7 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte if (storage.mTimer < 0) { - if ((actor.getRefData().getPosition().asVec3() - target.getRefData().getPosition().asVec3()).length2() - < 500*500 - && MWBase::Environment::get().getWorld()->getLOS(actor, target)) + if (targetDir.length2() < 500*500 && MWBase::Environment::get().getWorld()->getLOS(actor, target)) mActive = true; storage.mTimer = 0.5f; } @@ -122,8 +124,6 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte if (!mActive) return false; - ESM::Position pos = actor.getRefData().getPosition(); //position of the actor - // The distances below are approximations based on observations of the original engine. // If only one actor is following the target, it uses 186. // If there are multiple actors following the same target, they form a group with each group member at 313 + (130 * i) distance to the target. @@ -156,9 +156,8 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte } } - if ((pos.pos[0]-mX)*(pos.pos[0]-mX) + - (pos.pos[1]-mY)*(pos.pos[1]-mY) + - (pos.pos[2]-mZ)*(pos.pos[2]-mZ) < followDistance*followDistance) //Close-ish to final position + osg::Vec3f finalPos(mX, mY, mZ); + if ((actorPos-finalPos).length2() < followDistance*followDistance) //Close-ish to final position { if (actor.getCell()->isExterior()) //Outside? { @@ -173,8 +172,6 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte } } - //Set the target destination from the actor - ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos; short baseFollowDistance = followDistance; short threshold = 30; // to avoid constant switching between moving/stopping @@ -183,15 +180,9 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte else followDistance += threshold; - osg::Vec3f targetPos(target.getRefData().getPosition().asVec3()); - osg::Vec3f actorPos(actor.getRefData().getPosition().asVec3()); - - osg::Vec3f dir = targetPos - actorPos; - float targetDistSqr = dir.length2(); - - if (targetDistSqr <= followDistance * followDistance) + if (targetDir.length2() <= followDistance * followDistance) { - float faceAngleRadians = std::atan2(dir.x(), dir.y()); + float faceAngleRadians = std::atan2(targetDir.x(), targetDir.y()); if (!zTurn(actor, faceAngleRadians, osg::DegreesToRadians(45.f))) { @@ -202,16 +193,14 @@ bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characte return false; } - storage.mMoving = !pathTo(actor, dest, duration, baseFollowDistance); // Go to the destination + storage.mMoving = !pathTo(actor, targetPos, duration, baseFollowDistance); // Go to the destination if (storage.mMoving) { //Check if you're far away - float dist = distance(dest, pos.pos[0], pos.pos[1], pos.pos[2]); - - if (dist > 450) + if (targetDir.length2() > 450 * 450) actor.getClass().getCreatureStats(actor).setMovementFlag(MWMechanics::CreatureStats::Flag_Run, true); //Make NPC run - else if (dist < 325) //Have a bit of a dead zone, otherwise npc will constantly flip between running and not when right on the edge of the running threshold + else if (targetDir.length2() < 325 * 325) //Have a bit of a dead zone, otherwise npc will constantly flip between running and not when right on the edge of the running threshold actor.getClass().getCreatureStats(actor).setMovementFlag(MWMechanics::CreatureStats::Flag_Run, false); //make NPC walk } diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp index 7045d28e5..c2e5c9afb 100644 --- a/apps/openmw/mwmechanics/aipackage.cpp +++ b/apps/openmw/mwmechanics/aipackage.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "../mwbase/world.hpp" #include "../mwbase/environment.hpp" @@ -25,7 +26,7 @@ MWMechanics::AiPackage::~AiPackage() {} -MWMechanics::AiPackage::AiPackage() : +MWMechanics::AiPackage::AiPackage() : mTimer(AI_REACTION_TIME + 1.0f), // to force initial pathbuild mTargetActorRefId(""), mTargetActorId(-1), @@ -90,70 +91,65 @@ void MWMechanics::AiPackage::reset() mTimer = AI_REACTION_TIME + 1.0f; mIsShortcutting = false; mShortcutProhibited = false; - mShortcutFailPos = ESM::Pathgrid::Point(); + mShortcutFailPos = osg::Vec3f(); mPathFinder.clearPath(); mObstacleCheck.clear(); } -bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest, float duration, float destTolerance) +bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance) { mTimer += duration; //Update timer - ESM::Position pos = actor.getRefData().getPosition(); //position of the actor + const osg::Vec3f position = actor.getRefData().getPosition().asVec3(); //position of the actor + MWBase::World* world = MWBase::Environment::get().getWorld(); + + const osg::Vec3f halfExtents = world->getHalfExtents(actor); /// Stops the actor when it gets too close to a unloaded cell //... At current time, this test is unnecessary. AI shuts down when actor is more than "actors processing range" setting value //... units from player, and exterior cells are 8192 units long and wide. //... But AI processing distance may increase in the future. - if (isNearInactiveCell(pos)) + if (isNearInactiveCell(position)) { actor.getClass().getMovementSettings(actor).mPosition[1] = 0; + world->updateActorPath(actor, mPathFinder.getPath(), halfExtents, position, dest); return false; } - // handle path building and shortcutting - ESM::Pathgrid::Point start = pos.pos; - - float distToTarget = distance(start, dest); - bool isDestReached = (distToTarget <= destTolerance); + const float distToTarget = distance(position, dest); + const bool isDestReached = (distToTarget <= destTolerance); if (!isDestReached && mTimer > AI_REACTION_TIME) { if (actor.getClass().isBipedal(actor)) openDoors(actor); - bool wasShortcutting = mIsShortcutting; + const bool wasShortcutting = mIsShortcutting; bool destInLOS = false; - - const MWWorld::Class& actorClass = actor.getClass(); - MWBase::World* world = MWBase::Environment::get().getWorld(); - - // check if actor can move along z-axis - bool actorCanMoveByZ = (actorClass.canSwim(actor) && MWBase::Environment::get().getWorld()->isSwimming(actor)) - || world->isFlying(actor); + const bool actorCanMoveByZ = canActorMoveByZAxis(actor); // Prohibit shortcuts for AiWander, if the actor can not move in 3 dimensions. - if (actorCanMoveByZ || getTypeId() != TypeIdWander) - mIsShortcutting = shortcutPath(start, dest, actor, &destInLOS, actorCanMoveByZ); // try to shortcut first + mIsShortcutting = actorCanMoveByZ + && shortcutPath(position, dest, actor, &destInLOS, actorCanMoveByZ); // try to shortcut first if (!mIsShortcutting) { if (wasShortcutting || doesPathNeedRecalc(dest, actor.getCell())) // if need to rebuild path { - mPathFinder.buildSyncedPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell())); + const osg::Vec3f playerHalfExtents = world->getHalfExtents(getPlayer()); // Using player half extents for better performance + mPathFinder.buildPath(actor, position, dest, actor.getCell(), getPathGridGraph(actor.getCell()), + playerHalfExtents, getNavigatorFlags(actor)); mRotateOnTheRunChecks = 3; // give priority to go directly on target if there is minimal opportunity if (destInLOS && mPathFinder.getPath().size() > 1) { // get point just before dest - std::list::const_iterator pPointBeforeDest = mPathFinder.getPath().end(); - --pPointBeforeDest; - --pPointBeforeDest; + auto pPointBeforeDest = mPathFinder.getPath().rbegin() + 1; // if start point is closer to the target then last point of path (excluding target itself) then go straight on the target - if (distance(start, dest) <= distance(dest, *pPointBeforeDest)) + if (distance(position, dest) <= distance(dest, *pPointBeforeDest)) { mPathFinder.clearPath(); mPathFinder.addPointToPath(dest); @@ -163,7 +159,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgr if (!mPathFinder.getPath().empty()) //Path has points in it { - ESM::Pathgrid::Point lastPos = mPathFinder.getPath().back(); //Get the end of the proposed path + const osg::Vec3f& lastPos = mPathFinder.getPath().back(); //Get the end of the proposed path if(distance(dest, lastPos) > 100) //End of the path is far from the destination mPathFinder.addPointToPath(dest); //Adds the final destination to the path, to try to get to where you want to go @@ -173,41 +169,44 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const ESM::Pathgr mTimer = 0; } - if (isDestReached || mPathFinder.checkPathCompleted(pos.pos[0], pos.pos[1])) // if path is finished + const float pointTolerance = std::min(actor.getClass().getSpeed(actor), DEFAULT_TOLERANCE); + + mPathFinder.update(position, pointTolerance, DEFAULT_TOLERANCE); + + if (isDestReached || mPathFinder.checkPathCompleted()) // if path is finished { // turn to destination point - zTurn(actor, getZAngleToPoint(start, dest)); - smoothTurn(actor, getXAngleToPoint(start, dest), 0); + zTurn(actor, getZAngleToPoint(position, dest)); + smoothTurn(actor, getXAngleToPoint(position, dest), 0); + world->removeActorPath(actor); return true; } - else - { - if (mRotateOnTheRunChecks == 0 - || isReachableRotatingOnTheRun(actor, *mPathFinder.getPath().begin())) // to prevent circling around a path point - { - actor.getClass().getMovementSettings(actor).mPosition[1] = 1; // move to the target - if (mRotateOnTheRunChecks > 0) mRotateOnTheRunChecks--; - } - // handle obstacles on the way - evadeObstacles(actor, duration, pos); + world->updateActorPath(actor, mPathFinder.getPath(), halfExtents, position, dest); + + if (mRotateOnTheRunChecks == 0 + || isReachableRotatingOnTheRun(actor, *mPathFinder.getPath().begin())) // to prevent circling around a path point + { + actor.getClass().getMovementSettings(actor).mPosition[1] = 1; // move to the target + if (mRotateOnTheRunChecks > 0) mRotateOnTheRunChecks--; } // turn to next path point by X,Z axes - zTurn(actor, mPathFinder.getZAngleToNext(pos.pos[0], pos.pos[1])); - smoothTurn(actor, mPathFinder.getXAngleToNext(pos.pos[0], pos.pos[1], pos.pos[2]), 0); + zTurn(actor, mPathFinder.getZAngleToNext(position.x(), position.y())); + smoothTurn(actor, mPathFinder.getXAngleToNext(position.x(), position.y(), position.z()), 0); + + mObstacleCheck.update(actor, duration); + + // handle obstacles on the way + evadeObstacles(actor); return false; } -void MWMechanics::AiPackage::evadeObstacles(const MWWorld::Ptr& actor, float duration, const ESM::Position& pos) +void MWMechanics::AiPackage::evadeObstacles(const MWWorld::Ptr& actor) { - zTurn(actor, mPathFinder.getZAngleToNext(pos.pos[0], pos.pos[1])); - - MWMechanics::Movement& movement = actor.getClass().getMovementSettings(actor); - // check if stuck due to obstacles - if (!mObstacleCheck.check(actor, duration)) return; + if (!mObstacleCheck.isEvading()) return; // first check if obstacle is a door static float distance = MWBase::Environment::get().getWorld()->getMaxActivationDistance(); @@ -219,13 +218,14 @@ void MWMechanics::AiPackage::evadeObstacles(const MWWorld::Ptr& actor, float dur } else { - mObstacleCheck.takeEvasiveAction(movement); + mObstacleCheck.takeEvasiveAction(actor.getClass().getMovementSettings(actor)); } } void MWMechanics::AiPackage::openDoors(const MWWorld::Ptr& actor) { - static float distance = MWBase::Environment::get().getWorld()->getMaxActivationDistance(); + MWBase::World* world = MWBase::Environment::get().getWorld(); + static float distance = world->getMaxActivationDistance(); const MWWorld::Ptr door = getNearbyDoor(actor, distance); if (door == MWWorld::Ptr()) @@ -236,7 +236,7 @@ void MWMechanics::AiPackage::openDoors(const MWWorld::Ptr& actor) { if ((door.getCellRef().getTrap().empty() && door.getCellRef().getLockLevel() <= 0 )) { - MWBase::Environment::get().getWorld()->activate(door, actor); + world->activate(door, actor); return; } @@ -248,7 +248,7 @@ void MWMechanics::AiPackage::openDoors(const MWWorld::Ptr& actor) MWWorld::Ptr keyPtr = invStore.search(keyId); if (!keyPtr.isEmpty()) - MWBase::Environment::get().getWorld()->activate(door, actor); + world->activate(door, actor); } } @@ -266,14 +266,15 @@ const MWMechanics::PathgridGraph& MWMechanics::AiPackage::getPathGridGraph(const return *cache[id].get(); } -bool MWMechanics::AiPackage::shortcutPath(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor, bool *destInLOS, bool isPathClear) +bool MWMechanics::AiPackage::shortcutPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::Ptr& actor, bool *destInLOS, bool isPathClear) { - if (!mShortcutProhibited || (PathFinder::MakeOsgVec3(mShortcutFailPos) - PathFinder::MakeOsgVec3(startPoint)).length() >= PATHFIND_SHORTCUT_RETRY_DIST) + if (!mShortcutProhibited || (mShortcutFailPos - startPoint).length() >= PATHFIND_SHORTCUT_RETRY_DIST) { // check if target is clearly visible isPathClear = !MWBase::Environment::get().getWorld()->castRay( - static_cast(startPoint.mX), static_cast(startPoint.mY), static_cast(startPoint.mZ), - static_cast(endPoint.mX), static_cast(endPoint.mY), static_cast(endPoint.mZ)); + startPoint.x(), startPoint.y(), startPoint.z(), + endPoint.x(), endPoint.y(), endPoint.z()); if (destInLOS != nullptr) *destInLOS = isPathClear; @@ -294,46 +295,44 @@ bool MWMechanics::AiPackage::shortcutPath(const ESM::Pathgrid::Point& startPoint return false; } -bool MWMechanics::AiPackage::checkWayIsClearForActor(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor) +bool MWMechanics::AiPackage::checkWayIsClearForActor(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::Ptr& actor) { - bool actorCanMoveByZ = (actor.getClass().canSwim(actor) && MWBase::Environment::get().getWorld()->isSwimming(actor)) - || MWBase::Environment::get().getWorld()->isFlying(actor); - - if (actorCanMoveByZ) + if (canActorMoveByZAxis(actor)) return true; - float actorSpeed = actor.getClass().getSpeed(actor); - float maxAvoidDist = AI_REACTION_TIME * actorSpeed + actorSpeed / MAX_VEL_ANGULAR_RADIANS * 2; // *2 - for reliability - osg::Vec3f::value_type distToTarget = osg::Vec3f(static_cast(endPoint.mX), static_cast(endPoint.mY), 0).length(); - - float offsetXY = distToTarget > maxAvoidDist*1.5? maxAvoidDist : maxAvoidDist/2; + const float actorSpeed = actor.getClass().getSpeed(actor); + const float maxAvoidDist = AI_REACTION_TIME * actorSpeed + actorSpeed / MAX_VEL_ANGULAR_RADIANS * 2; // *2 - for reliability + const float distToTarget = osg::Vec2f(endPoint.x(), endPoint.y()).length(); - bool isClear = checkWayIsClear(PathFinder::MakeOsgVec3(startPoint), PathFinder::MakeOsgVec3(endPoint), offsetXY); + const float offsetXY = distToTarget > maxAvoidDist*1.5? maxAvoidDist : maxAvoidDist/2; // update shortcut prohibit state - if (isClear) + if (checkWayIsClear(startPoint, endPoint, offsetXY)) { if (mShortcutProhibited) { mShortcutProhibited = false; - mShortcutFailPos = ESM::Pathgrid::Point(); + mShortcutFailPos = osg::Vec3f(); } + return true; } - if (!isClear) + else { - if (mShortcutFailPos.mX == 0 && mShortcutFailPos.mY == 0 && mShortcutFailPos.mZ == 0) + if (mShortcutFailPos == osg::Vec3f()) { mShortcutProhibited = true; mShortcutFailPos = startPoint; } } - return isClear; + return false; } -bool MWMechanics::AiPackage::doesPathNeedRecalc(const ESM::Pathgrid::Point& newDest, const MWWorld::CellStore* currentCell) +bool MWMechanics::AiPackage::doesPathNeedRecalc(const osg::Vec3f& newDest, const MWWorld::CellStore* currentCell) { - return mPathFinder.getPath().empty() || (distance(mPathFinder.getPath().back(), newDest) > 10) || mPathFinder.getPathCell() != currentCell; + return mPathFinder.getPath().empty() + || (distance(mPathFinder.getPath().back(), newDest) > 10) + || mPathFinder.getPathCell() != currentCell; } bool MWMechanics::AiPackage::isTargetMagicallyHidden(const MWWorld::Ptr& target) @@ -343,14 +342,13 @@ bool MWMechanics::AiPackage::isTargetMagicallyHidden(const MWWorld::Ptr& target) || (magicEffects.get(ESM::MagicEffect::Chameleon).getMagnitude() > 75); } -bool MWMechanics::AiPackage::isNearInactiveCell(const ESM::Position& actorPos) +bool MWMechanics::AiPackage::isNearInactiveCell(osg::Vec3f position) { const ESM::Cell* playerCell(getPlayer().getCell()->getCell()); if (playerCell->isExterior()) { // get actor's distance from origin of center cell - osg::Vec3f actorOffset(actorPos.asVec3()); - CoordinateConverter(playerCell).toLocal(actorOffset); + CoordinateConverter(playerCell).toLocal(position); // currently assumes 3 x 3 grid for exterior cells, with player at center cell. // ToDo: (Maybe) use "exterior cell load distance" setting to get count of actual active cells @@ -358,8 +356,8 @@ bool MWMechanics::AiPackage::isNearInactiveCell(const ESM::Position& actorPos) const float distanceFromEdge = 200.0; float minThreshold = (-1.0f * ESM::Land::REAL_SIZE) + distanceFromEdge; float maxThreshold = (2.0f * ESM::Land::REAL_SIZE) - distanceFromEdge; - return (actorOffset[0] < minThreshold) || (maxThreshold < actorOffset[0]) - || (actorOffset[1] < minThreshold) || (maxThreshold < actorOffset[1]); + return (position.x() < minThreshold) || (maxThreshold < position.x()) + || (position.y() < minThreshold) || (maxThreshold < position.y()); } else { @@ -367,7 +365,7 @@ bool MWMechanics::AiPackage::isNearInactiveCell(const ESM::Position& actorPos) } } -bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest) +bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const osg::Vec3f& dest) { // get actor's shortest radius for moving in circle float speed = actor.getClass().getSpeed(actor); @@ -381,17 +379,39 @@ bool MWMechanics::AiPackage::isReachableRotatingOnTheRun(const MWWorld::Ptr& act osg::Vec3f radiusDir = dir ^ osg::Z_AXIS; // radius is perpendicular to a tangent radiusDir.normalize(); radiusDir *= radius; - + // pick up the nearest center candidate - osg::Vec3f dest_ = PathFinder::MakeOsgVec3(dest); osg::Vec3f pos = actor.getRefData().getPosition().asVec3(); osg::Vec3f center1 = pos - radiusDir; osg::Vec3f center2 = pos + radiusDir; - osg::Vec3f center = (center1 - dest_).length2() < (center2 - dest_).length2() ? center1 : center2; + osg::Vec3f center = (center1 - dest).length2() < (center2 - dest).length2() ? center1 : center2; - float distToDest = (center - dest_).length(); + float distToDest = (center - dest).length(); // if pathpoint is reachable for the actor rotating on the run: // no points of actor's circle should be farther from the center than destination point return (radius <= distToDest); } + +DetourNavigator::Flags MWMechanics::AiPackage::getNavigatorFlags(const MWWorld::Ptr& actor) const +{ + const MWWorld::Class& actorClass = actor.getClass(); + DetourNavigator::Flags result = DetourNavigator::Flag_none; + + if (actorClass.isPureWaterCreature(actor) || (getTypeId() != TypeIdWander && actorClass.canSwim(actor))) + result |= DetourNavigator::Flag_swim; + + if (actorClass.canWalk(actor)) + result |= DetourNavigator::Flag_walk; + + if (actorClass.isBipedal(actor) && getTypeId() != TypeIdWander) + result |= DetourNavigator::Flag_openDoor; + + return result; +} + +bool MWMechanics::AiPackage::canActorMoveByZAxis(const MWWorld::Ptr& actor) const +{ + MWBase::World* world = MWBase::Environment::get().getWorld(); + return (actor.getClass().canSwim(actor) && world->isSwimming(actor)) || world->isFlying(actor); +} diff --git a/apps/openmw/mwmechanics/aipackage.hpp b/apps/openmw/mwmechanics/aipackage.hpp index 5a468ae30..5be858b0b 100644 --- a/apps/openmw/mwmechanics/aipackage.hpp +++ b/apps/openmw/mwmechanics/aipackage.hpp @@ -7,6 +7,8 @@ #include "obstacle.hpp" #include "aistate.hpp" +#include + namespace MWWorld { class Ptr; @@ -105,29 +107,35 @@ namespace MWMechanics bool isTargetMagicallyHidden(const MWWorld::Ptr& target); /// Return if actor's rotation speed is sufficient to rotate to the destination pathpoint on the run. Otherwise actor should rotate while standing. - static bool isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest); + static bool isReachableRotatingOnTheRun(const MWWorld::Ptr& actor, const osg::Vec3f& dest); protected: /// Handles path building and shortcutting with obstacles avoiding /** \return If the actor has arrived at his destination **/ - bool pathTo(const MWWorld::Ptr& actor, const ESM::Pathgrid::Point& dest, float duration, float destTolerance = 0.0f); + bool pathTo(const MWWorld::Ptr& actor, const osg::Vec3f& dest, float duration, float destTolerance = 0.0f); /// Check if there aren't any obstacles along the path to make shortcut possible /// If a shortcut is possible then path will be cleared and filled with the destination point. /// \param destInLOS If not nullptr function will return ray cast check result /// \return If can shortcut the path - bool shortcutPath(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor, bool *destInLOS, bool isPathClear); + bool shortcutPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::Ptr& actor, + bool *destInLOS, bool isPathClear); /// Check if the way to the destination is clear, taking into account actor speed - bool checkWayIsClearForActor(const ESM::Pathgrid::Point& startPoint, const ESM::Pathgrid::Point& endPoint, const MWWorld::Ptr& actor); + bool checkWayIsClearForActor(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, const MWWorld::Ptr& actor); + + bool doesPathNeedRecalc(const osg::Vec3f& newDest, const MWWorld::CellStore* currentCell); - virtual bool doesPathNeedRecalc(const ESM::Pathgrid::Point& newDest, const MWWorld::CellStore* currentCell); + void evadeObstacles(const MWWorld::Ptr& actor); - void evadeObstacles(const MWWorld::Ptr& actor, float duration, const ESM::Position& pos); void openDoors(const MWWorld::Ptr& actor); const PathgridGraph& getPathGridGraph(const MWWorld::CellStore* cell); + DetourNavigator::Flags getNavigatorFlags(const MWWorld::Ptr& actor) const; + + bool canActorMoveByZAxis(const MWWorld::Ptr& actor) const; + // TODO: all this does not belong here, move into temporary storage PathFinder mPathFinder; ObstacleCheck mObstacleCheck; @@ -137,16 +145,14 @@ namespace MWMechanics std::string mTargetActorRefId; mutable int mTargetActorId; - osg::Vec3f mLastActorPos; - short mRotateOnTheRunChecks; // attempts to check rotation to the pathpoint on the run possibility bool mIsShortcutting; // if shortcutting at the moment bool mShortcutProhibited; // shortcutting may be prohibited after unsuccessful attempt - ESM::Pathgrid::Point mShortcutFailPos; // position of last shortcut fail + osg::Vec3f mShortcutFailPos; // position of last shortcut fail private: - bool isNearInactiveCell(const ESM::Position& actorPos); + bool isNearInactiveCell(osg::Vec3f position); }; } diff --git a/apps/openmw/mwmechanics/aipursue.cpp b/apps/openmw/mwmechanics/aipursue.cpp index ec6bf4fe2..e5c136980 100644 --- a/apps/openmw/mwmechanics/aipursue.cpp +++ b/apps/openmw/mwmechanics/aipursue.cpp @@ -48,15 +48,15 @@ bool AiPursue::execute (const MWWorld::Ptr& actor, CharacterController& characte const MWWorld::Ptr target = MWBase::Environment::get().getWorld()->searchPtrViaActorId(mTargetActorId); //The target to follow - if(target == MWWorld::Ptr() || !target.getRefData().getCount() || !target.getRefData().isEnabled() // Really we should be checking whether the target is currently registered - // with the MechanicsManager - ) - return true; //Target doesn't exist + // Stop if the target doesn't exist + // Really we should be checking whether the target is currently registered with the MechanicsManager + if (target == MWWorld::Ptr() || !target.getRefData().getCount() || !target.getRefData().isEnabled()) + return true; if (isTargetMagicallyHidden(target)) return true; - if(target.getClass().getCreatureStats(target).isDead()) + if (target.getClass().getCreatureStats(target).isDead()) return true; /* @@ -81,14 +81,14 @@ bool AiPursue::execute (const MWWorld::Ptr& actor, CharacterController& characte actor.getClass().getCreatureStats(actor).setDrawState(DrawState_Nothing); - //Set the target desition from the actor - ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos; - ESM::Position aPos = actor.getRefData().getPosition(); + //Set the target destination + const osg::Vec3f dest = target.getRefData().getPosition().asVec3(); + const osg::Vec3f actorPos = actor.getRefData().getPosition().asVec3(); - float pathTolerance = 100.0; + const float pathTolerance = 100.f; if (pathTo(actor, dest, duration, pathTolerance) && - std::abs(dest.mZ - aPos.pos[2]) < pathTolerance) // check the true distance in case the target is far away in Z-direction + std::abs(dest.z() - actorPos.z()) < pathTolerance) // check the true distance in case the target is far away in Z-direction { /* Start of tes3mp addition diff --git a/apps/openmw/mwmechanics/aitravel.cpp b/apps/openmw/mwmechanics/aitravel.cpp index 90beb9ead..d87029809 100644 --- a/apps/openmw/mwmechanics/aitravel.cpp +++ b/apps/openmw/mwmechanics/aitravel.cpp @@ -21,8 +21,8 @@ bool isWithinMaxRange(const osg::Vec3f& pos1, const osg::Vec3f& pos2) { // Maximum travel distance for vanilla compatibility. // Was likely meant to prevent NPCs walking into non-loaded exterior cells, but for some reason is used in interior cells as well. - // We can make this configurable at some point, but the default *must* be the below value. Anything else will break shoddily-written content (*cough* MW *cough*) in bizarre ways. - bool aiDistance = MWBase::Environment::get().getMechanicsManager()->getActorsProcessingRange(); + // The specific range below is configurable, but its limit is currently 7168 units. Anything greater will break shoddily-written content (*cough* MW *cough*) in bizarre ways. + float aiDistance = MWBase::Environment::get().getMechanicsManager()->getActorsProcessingRange(); return (pos1 - pos2).length2() <= aiDistance*aiDistance; } @@ -47,19 +47,19 @@ namespace MWMechanics bool AiTravel::execute (const MWWorld::Ptr& actor, CharacterController& characterController, AiState& state, float duration) { - ESM::Position pos = actor.getRefData().getPosition(); + const osg::Vec3f actorPos(actor.getRefData().getPosition().asVec3()); + const osg::Vec3f targetPos(mX, mY, mZ); actor.getClass().getCreatureStats(actor).setMovementFlag(CreatureStats::Flag_Run, false); actor.getClass().getCreatureStats(actor).setDrawState(DrawState_Nothing); - if (!isWithinMaxRange(osg::Vec3f(mX, mY, mZ), pos.asVec3())) + if (!isWithinMaxRange(targetPos, actorPos)) return false; // Unfortunately, with vanilla assets destination is sometimes blocked by other actor. // If we got close to target, check for actors nearby. If they are, finish AI package. int destinationTolerance = 64; - ESM::Pathgrid::Point dest(static_cast(mX), static_cast(mY), static_cast(mZ)); - if (distance(pos.pos, dest) <= destinationTolerance) + if (distance(actorPos, targetPos) <= destinationTolerance) { std::vector targetActors; std::pair result = MWBase::Environment::get().getWorld()->getHitContact(actor, destinationTolerance, targetActors); @@ -71,7 +71,7 @@ namespace MWMechanics } } - if (pathTo(actor, dest, duration)) + if (pathTo(actor, targetPos, duration)) { actor.getClass().getMovementSettings(actor).mPosition[1] = 0; return true; diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp index caacf8cb6..833a37127 100644 --- a/apps/openmw/mwmechanics/aiwander.cpp +++ b/apps/openmw/mwmechanics/aiwander.cpp @@ -50,7 +50,8 @@ namespace MWMechanics AiWander::AiWander(int distance, int duration, int timeOfDay, const std::vector& idle, bool repeat): mDistance(distance), mDuration(duration), mRemainingDuration(duration), mTimeOfDay(timeOfDay), mIdle(idle), - mRepeat(repeat), mStoredInitialActorPosition(false), mInitialActorPosition(osg::Vec3f(0, 0, 0)), mHasDestination(false), mDestination(osg::Vec3f(0, 0, 0)) + mRepeat(repeat), mStoredInitialActorPosition(false), mInitialActorPosition(osg::Vec3f(0, 0, 0)), + mHasDestination(false), mDestination(osg::Vec3f(0, 0, 0)), mUsePathgrid(false) { mIdle.resize(8, 0); init(); @@ -123,14 +124,13 @@ namespace MWMechanics */ bool AiWander::execute (const MWWorld::Ptr& actor, CharacterController& characterController, AiState& state, float duration) { - // get or create temporary storage - AiWanderStorage& storage = state.get(); - - const MWWorld::CellStore*& currentCell = storage.mCell; MWMechanics::CreatureStats& cStats = actor.getClass().getCreatureStats(actor); - if(cStats.isDead() || cStats.getHealth().getCurrent() <= 0) + if (cStats.isDead() || cStats.getHealth().getCurrent() <= 0) return true; // Don't bother with dead actors + // get or create temporary storage + AiWanderStorage& storage = state.get(); + const MWWorld::CellStore*& currentCell = storage.mCell; bool cellChange = currentCell && (actor.getCell() != currentCell); if(!currentCell || cellChange) { @@ -151,16 +151,23 @@ namespace MWMechanics // rebuild a path to it if (!mPathFinder.isPathConstructed() && mHasDestination) { - ESM::Pathgrid::Point dest(PathFinder::MakePathgridPoint(mDestination)); - ESM::Pathgrid::Point start(PathFinder::MakePathgridPoint(pos)); - - mPathFinder.buildSyncedPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell())); + if (mUsePathgrid) + { + mPathFinder.buildPathByPathgrid(pos.asVec3(), mDestination, actor.getCell(), + getPathGridGraph(actor.getCell())); + } + else + { + const osg::Vec3f playerHalfExtents = MWBase::Environment::get().getWorld()->getHalfExtents(getPlayer()); // Using player half extents for better performance + mPathFinder.buildPath(actor, pos.asVec3(), mDestination, actor.getCell(), + getPathGridGraph(actor.getCell()), playerHalfExtents, getNavigatorFlags(actor)); + } if (mPathFinder.isPathConstructed()) storage.setState(AiWanderStorage::Wander_Walking); } - doPerFrameActionsForState(actor, duration, storage, pos); + doPerFrameActionsForState(actor, duration, storage); playIdleDialogueRandomly(actor); @@ -169,14 +176,14 @@ namespace MWMechanics if (AI_REACTION_TIME <= lastReaction) { lastReaction = 0; - return reactionTimeActions(actor, storage, currentCell, cellChange, pos, duration); + return reactionTimeActions(actor, storage, currentCell, cellChange, pos); } else return false; } bool AiWander::reactionTimeActions(const MWWorld::Ptr& actor, AiWanderStorage& storage, - const MWWorld::CellStore*& currentCell, bool cellChange, ESM::Position& pos, float duration) + const MWWorld::CellStore*& currentCell, bool cellChange, ESM::Position& pos) { if (mDistance <= 0) storage.mCanWanderAlongPathGrid = false; @@ -226,7 +233,7 @@ namespace MWMechanics } // If Wandering manually and hit an obstacle, stop - if (storage.mIsWanderingManually && mObstacleCheck.check(actor, duration, 2.0f)) { + if (storage.mIsWanderingManually && mObstacleCheck.isEvading()) { completeManualWalking(actor, storage); } @@ -251,7 +258,9 @@ namespace MWMechanics setPathToAnAllowedNode(actor, storage, pos); } } - } else if (storage.mIsWanderingManually && mPathFinder.checkPathCompleted(pos.pos[0], pos.pos[1], DESTINATION_TOLERANCE)) { + } + else if (storage.mIsWanderingManually && mPathFinder.checkPathCompleted()) + { completeManualWalking(actor, storage); } @@ -268,9 +277,7 @@ namespace MWMechanics if (mHasDestination) return mDestination; - const ESM::Pathgrid::Point currentPosition = actor.getRefData().getPosition().pos; - const osg::Vec3f currentPositionVec3f = osg::Vec3f(currentPosition.mX, currentPosition.mY, currentPosition.mZ); - return currentPositionVec3f; + return actor.getRefData().getPosition().asVec3(); } bool AiWander::isPackageCompleted(const MWWorld::Ptr& actor, AiWanderStorage& storage) @@ -292,11 +299,9 @@ namespace MWMechanics * Commands actor to walk to a random location near original spawn location. */ void AiWander::wanderNearStart(const MWWorld::Ptr &actor, AiWanderStorage &storage, int wanderDistance) { - const ESM::Pathgrid::Point currentPosition = actor.getRefData().getPosition().pos; - const osg::Vec3f currentPositionVec3f = osg::Vec3f(currentPosition.mX, currentPosition.mY, currentPosition.mZ); + const auto currentPosition = actor.getRefData().getPosition().asVec3(); std::size_t attempts = 10; // If a unit can't wander out of water, don't want to hang here - ESM::Pathgrid::Point destinationPosition; bool isWaterCreature = actor.getClass().isPureWaterCreature(actor); do { // Determine a random location within radius of original position @@ -305,19 +310,23 @@ namespace MWMechanics const float destinationX = mInitialActorPosition.x() + wanderRadius * std::cos(randomDirection); const float destinationY = mInitialActorPosition.y() + wanderRadius * std::sin(randomDirection); const float destinationZ = mInitialActorPosition.z(); - destinationPosition = ESM::Pathgrid::Point(destinationX, destinationY, destinationZ); mDestination = osg::Vec3f(destinationX, destinationY, destinationZ); // Check if land creature will walk onto water or if water creature will swim onto land if ((!isWaterCreature && !destinationIsAtWater(actor, mDestination)) || - (isWaterCreature && !destinationThroughGround(currentPositionVec3f, mDestination))) { - mPathFinder.buildSyncedPath(currentPosition, destinationPosition, actor.getCell(), getPathGridGraph(actor.getCell())); - mPathFinder.addPointToPath(destinationPosition); + (isWaterCreature && !destinationThroughGround(currentPosition, mDestination))) + { + // Using player half extents for better performance + const osg::Vec3f playerHalfExtents = MWBase::Environment::get().getWorld()->getHalfExtents(getPlayer()); + mPathFinder.buildPath(actor, currentPosition, mDestination, actor.getCell(), + getPathGridGraph(actor.getCell()), playerHalfExtents, getNavigatorFlags(actor)); + mPathFinder.addPointToPath(mDestination); if (mPathFinder.isPathConstructed()) { storage.setState(AiWanderStorage::Wander_Walking, true); mHasDestination = true; + mUsePathgrid = false; } return; } @@ -348,7 +357,7 @@ namespace MWMechanics storage.setState(AiWanderStorage::Wander_IdleNow); } - void AiWander::doPerFrameActionsForState(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage, ESM::Position& pos) + void AiWander::doPerFrameActionsForState(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage) { switch (storage.mState) { @@ -357,7 +366,7 @@ namespace MWMechanics break; case AiWanderStorage::Wander_Walking: - onWalkingStatePerFrameActions(actor, duration, storage, pos); + onWalkingStatePerFrameActions(actor, duration, storage); break; case AiWanderStorage::Wander_ChooseAction: @@ -413,11 +422,10 @@ namespace MWMechanics } } - void AiWander::onWalkingStatePerFrameActions(const MWWorld::Ptr& actor, - float duration, AiWanderStorage& storage, ESM::Position& pos) + void AiWander::onWalkingStatePerFrameActions(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage) { // Is there no destination or are we there yet? - if ((!mPathFinder.isPathConstructed()) || pathTo(actor, ESM::Pathgrid::Point(mPathFinder.getPath().back()), duration, DESTINATION_TOLERANCE)) + if ((!mPathFinder.isPathConstructed()) || pathTo(actor, osg::Vec3f(mPathFinder.getPath().back()), duration, DESTINATION_TOLERANCE)) { stopWalking(actor, storage); storage.setState(AiWanderStorage::Wander_ChooseAction); @@ -425,7 +433,7 @@ namespace MWMechanics else { // have not yet reached the destination - evadeObstacles(actor, storage, duration, pos); + evadeObstacles(actor, storage); } } @@ -456,7 +464,7 @@ namespace MWMechanics storage.setState(AiWanderStorage::Wander_IdleNow); } - void AiWander::evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage, float duration, ESM::Position& pos) + void AiWander::evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage) { if (mObstacleCheck.isEvading()) { @@ -507,9 +515,9 @@ namespace MWMechanics // Only say Idle voices when player is in LOS // A bit counterintuitive, likely vanilla did this to reduce the appearance of // voices going through walls? - const ESM::Position& pos = actor.getRefData().getPosition(); - if (roll < x && (player.getRefData().getPosition().asVec3() - pos.asVec3()).length2() - < 3000 * 3000 // maybe should be fAudioVoiceDefaultMaxDistance*fAudioMaxDistanceMult instead + const osg::Vec3f playerPos(player.getRefData().getPosition().asVec3()); + const osg::Vec3f actorPos(actor.getRefData().getPosition().asVec3()); + if (roll < x && (playerPos - actorPos).length2() < 3000 * 3000 // maybe should be fAudioVoiceDefaultMaxDistance*fAudioMaxDistanceMult instead && MWBase::Environment::get().getWorld()->getLOS(player, actor)) MWBase::Environment::get().getDialogueManager()->say(actor, "idle"); } @@ -528,13 +536,12 @@ namespace MWMechanics MWWorld::Ptr player = getPlayer(); osg::Vec3f playerPos(player.getRefData().getPosition().asVec3()); osg::Vec3f actorPos(actor.getRefData().getPosition().asVec3()); - float playerDistSqr = (playerPos - actorPos).length2(); int& greetingTimer = storage.mGreetingTimer; AiWanderStorage::GreetingState& greetingState = storage.mSaidGreeting; if (greetingState == AiWanderStorage::Greet_None) { - if ((playerDistSqr <= helloDistance*helloDistance) && + if ((playerPos - actorPos).length2() <= helloDistance*helloDistance && !player.getClass().getCreatureStats(player).isDead() && MWBase::Environment::get().getWorld()->getLOS(player, actor) && MWBase::Environment::get().getMechanicsManager()->awarenessCheck(player, actor)) greetingTimer++; @@ -570,7 +577,7 @@ namespace MWMechanics if (greetingState == AiWanderStorage::Greet_Done) { float resetDist = 2 * helloDistance; - if (playerDistSqr >= resetDist*resetDist) + if ((playerPos - actorPos).length2() >= resetDist*resetDist) greetingState = AiWanderStorage::Greet_None; } } @@ -592,15 +599,17 @@ namespace MWMechanics ToWorldCoordinates(dest, storage.mCell->getCell()); // actor position is already in world coordinates - ESM::Pathgrid::Point start(PathFinder::MakePathgridPoint(actorPos)); + const osg::Vec3f start = actorPos.asVec3(); // don't take shortcuts for wandering - mPathFinder.buildSyncedPath(start, dest, actor.getCell(), getPathGridGraph(actor.getCell())); + const osg::Vec3f destVec3f = PathFinder::makeOsgVec3(dest); + mPathFinder.buildPathByPathgrid(start, destVec3f, actor.getCell(), getPathGridGraph(actor.getCell())); if (mPathFinder.isPathConstructed()) { - mDestination = osg::Vec3f(dest.mX, dest.mY, dest.mZ); + mDestination = destVec3f; mHasDestination = true; + mUsePathgrid = true; // Remove this node as an option and add back the previously used node (stops NPC from picking the same node): ESM::Pathgrid::Point temp = storage.mAllowedNodes[randNode]; storage.mAllowedNodes.erase(storage.mAllowedNodes.begin() + randNode); @@ -631,15 +640,15 @@ namespace MWMechanics // Every now and then check whether one of the doors is opened. (maybe // at the end of playing idle?) If the door is opened then re-calculate // allowed nodes starting from the spawn point. - std::list paths = pathfinder.getPath(); + auto paths = pathfinder.getPath(); while(paths.size() >= 2) { - ESM::Pathgrid::Point pt = paths.back(); + const auto pt = paths.back(); for(unsigned int j = 0; j < nodes.size(); j++) { // FIXME: doesn't handle a door with the same X/Y // coordinates but with a different Z - if(nodes[j].mX == pt.mX && nodes[j].mY == pt.mY) + if (std::abs(nodes[j].mX - pt.x()) <= 0.5 && std::abs(nodes[j].mY - pt.y()) <= 0.5) { nodes.erase(nodes.begin() + j); break; @@ -731,7 +740,7 @@ namespace MWMechanics ESM::Pathgrid::Point worldDest = dest; ToWorldCoordinates(worldDest, actor.getCell()->getCell()); - bool isPathGridOccupied = MWBase::Environment::get().getMechanicsManager()->isAnyActorInRange(PathFinder::MakeOsgVec3(worldDest), 60); + bool isPathGridOccupied = MWBase::Environment::get().getMechanicsManager()->isAnyActorInRange(PathFinder::makeOsgVec3(worldDest), 60); // add offset only if the selected pathgrid is occupied by another actor if (isPathGridOccupied) @@ -752,18 +761,18 @@ namespace MWMechanics ESM::Pathgrid::Point connDest = points[randomIndex]; // add an offset towards random neighboring node - osg::Vec3f dir = PathFinder::MakeOsgVec3(connDest) - PathFinder::MakeOsgVec3(dest); + osg::Vec3f dir = PathFinder::makeOsgVec3(connDest) - PathFinder::makeOsgVec3(dest); float length = dir.length(); dir.normalize(); for (int j = 1; j <= 3; j++) { // move for 5-15% towards random neighboring node - dest = PathFinder::MakePathgridPoint(PathFinder::MakeOsgVec3(dest) + dir * (j * 5 * length / 100.f)); + dest = PathFinder::makePathgridPoint(PathFinder::makeOsgVec3(dest) + dir * (j * 5 * length / 100.f)); worldDest = dest; ToWorldCoordinates(worldDest, actor.getCell()->getCell()); - isOccupied = MWBase::Environment::get().getMechanicsManager()->isAnyActorInRange(PathFinder::MakeOsgVec3(worldDest), 60); + isOccupied = MWBase::Environment::get().getMechanicsManager()->isAnyActorInRange(PathFinder::makeOsgVec3(worldDest), 60); if (!isOccupied) break; @@ -799,7 +808,7 @@ namespace MWMechanics const ESM::Pathgrid *pathgrid = MWBase::Environment::get().getWorld()->getStore().get().search(*currentCell->getCell()); - int index = PathFinder::GetClosestPoint(pathgrid, PathFinder::MakeOsgVec3(dest)); + int index = PathFinder::getClosestPoint(pathgrid, PathFinder::makeOsgVec3(dest)); getPathGridGraph(currentCell).getNeighbouringPoints(index, points); } @@ -832,7 +841,7 @@ namespace MWMechanics CoordinateConverter(cell).toLocal(npcPos); // Find closest pathgrid point - int closestPointIndex = PathFinder::GetClosestPoint(pathgrid, npcPos); + int closestPointIndex = PathFinder::getClosestPoint(pathgrid, npcPos); // mAllowedNodes for this actor with pathgrid point indexes based on mDistance // and if the point is connected to the closest current point @@ -840,7 +849,7 @@ namespace MWMechanics int pointIndex = 0; for(unsigned int counter = 0; counter < pathgrid->mPoints.size(); counter++) { - osg::Vec3f nodePos(PathFinder::MakeOsgVec3(pathgrid->mPoints[counter])); + osg::Vec3f nodePos(PathFinder::makeOsgVec3(pathgrid->mPoints[counter])); if((npcPos - nodePos).length2() <= mDistance * mDistance && getPathGridGraph(cellStore).isPointConnected(closestPointIndex, counter)) { @@ -867,7 +876,7 @@ namespace MWMechanics // 2. Partway along the path between the point and its connected points. void AiWander::AddNonPathGridAllowedPoints(osg::Vec3f npcPos, const ESM::Pathgrid * pathGrid, int pointIndex, AiWanderStorage& storage) { - storage.mAllowedNodes.push_back(PathFinder::MakePathgridPoint(npcPos)); + storage.mAllowedNodes.push_back(PathFinder::makePathgridPoint(npcPos)); for (std::vector::const_iterator it = pathGrid->mEdges.begin(); it != pathGrid->mEdges.end(); ++it) { if (it->mV0 == pointIndex) @@ -879,8 +888,8 @@ namespace MWMechanics void AiWander::AddPointBetweenPathGridPoints(const ESM::Pathgrid::Point& start, const ESM::Pathgrid::Point& end, AiWanderStorage& storage) { - osg::Vec3f vectorStart = PathFinder::MakeOsgVec3(start); - osg::Vec3f delta = PathFinder::MakeOsgVec3(end) - vectorStart; + osg::Vec3f vectorStart = PathFinder::makeOsgVec3(start); + osg::Vec3f delta = PathFinder::makeOsgVec3(end) - vectorStart; float length = delta.length(); delta.normalize(); @@ -889,7 +898,7 @@ namespace MWMechanics // must not travel longer than distance between waypoints or NPC goes past waypoint distance = std::min(distance, static_cast(length)); delta *= distance; - storage.mAllowedNodes.push_back(PathFinder::MakePathgridPoint(vectorStart + delta)); + storage.mAllowedNodes.push_back(PathFinder::makePathgridPoint(vectorStart + delta)); } void AiWander::SetCurrentNodeToClosestAllowedNode(const osg::Vec3f& npcPos, AiWanderStorage& storage) @@ -898,7 +907,7 @@ namespace MWMechanics unsigned int index = 0; for (unsigned int counterThree = 0; counterThree < storage.mAllowedNodes.size(); counterThree++) { - osg::Vec3f nodePos(PathFinder::MakeOsgVec3(storage.mAllowedNodes[counterThree])); + osg::Vec3f nodePos(PathFinder::makeOsgVec3(storage.mAllowedNodes[counterThree])); float tempDist = (npcPos - nodePos).length2(); if (tempDist < distanceToClosestNode) { diff --git a/apps/openmw/mwmechanics/aiwander.hpp b/apps/openmw/mwmechanics/aiwander.hpp index 479ed4869..bba6f7113 100644 --- a/apps/openmw/mwmechanics/aiwander.hpp +++ b/apps/openmw/mwmechanics/aiwander.hpp @@ -137,15 +137,15 @@ namespace MWMechanics short unsigned getRandomIdle(); void setPathToAnAllowedNode(const MWWorld::Ptr& actor, AiWanderStorage& storage, const ESM::Position& actorPos); void playGreetingIfPlayerGetsTooClose(const MWWorld::Ptr& actor, AiWanderStorage& storage); - void evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage, float duration, ESM::Position& pos); + void evadeObstacles(const MWWorld::Ptr& actor, AiWanderStorage& storage); void playIdleDialogueRandomly(const MWWorld::Ptr& actor); void turnActorToFacePlayer(const osg::Vec3f& actorPosition, const osg::Vec3f& playerPosition, AiWanderStorage& storage); - void doPerFrameActionsForState(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage, ESM::Position& pos); + void doPerFrameActionsForState(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage); void onIdleStatePerFrameActions(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage); - void onWalkingStatePerFrameActions(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage, ESM::Position& pos); + void onWalkingStatePerFrameActions(const MWWorld::Ptr& actor, float duration, AiWanderStorage& storage); void onChooseActionStatePerFrameActions(const MWWorld::Ptr& actor, AiWanderStorage& storage); bool reactionTimeActions(const MWWorld::Ptr& actor, AiWanderStorage& storage, - const MWWorld::CellStore*& currentCell, bool cellChange, ESM::Position& pos, float duration); + const MWWorld::CellStore*& currentCell, bool cellChange, ESM::Position& pos); bool isPackageCompleted(const MWWorld::Ptr& actor, AiWanderStorage& storage); void wanderNearStart(const MWWorld::Ptr &actor, AiWanderStorage &storage, int wanderDistance); bool destinationIsAtWater(const MWWorld::Ptr &actor, const osg::Vec3f& destination); @@ -164,6 +164,7 @@ namespace MWMechanics bool mHasDestination; osg::Vec3f mDestination; + bool mUsePathgrid; void getNeighbouringNodes(ESM::Pathgrid::Point dest, const MWWorld::CellStore* currentCell, ESM::Pathgrid::PointList& points); diff --git a/apps/openmw/mwmechanics/coordinateconverter.cpp b/apps/openmw/mwmechanics/coordinateconverter.cpp index 0a2d99f92..04155ea49 100644 --- a/apps/openmw/mwmechanics/coordinateconverter.cpp +++ b/apps/openmw/mwmechanics/coordinateconverter.cpp @@ -33,11 +33,12 @@ namespace MWMechanics point.y() -= static_cast(mCellY); } - osg::Vec3f CoordinateConverter::toLocalVec3(const ESM::Pathgrid::Point& point) + osg::Vec3f CoordinateConverter::toLocalVec3(const osg::Vec3f& point) { return osg::Vec3f( - static_cast(point.mX - mCellX), - static_cast(point.mY - mCellY), - static_cast(point.mZ)); + point.x() - static_cast(mCellX), + point.y() - static_cast(mCellY), + point.z() + ); } } diff --git a/apps/openmw/mwmechanics/coordinateconverter.hpp b/apps/openmw/mwmechanics/coordinateconverter.hpp index cd855e84a..f7dda33cb 100644 --- a/apps/openmw/mwmechanics/coordinateconverter.hpp +++ b/apps/openmw/mwmechanics/coordinateconverter.hpp @@ -26,7 +26,7 @@ namespace MWMechanics /// in-place conversion from world to local void toLocal(osg::Vec3f& point); - osg::Vec3f toLocalVec3(const ESM::Pathgrid::Point& point); + osg::Vec3f toLocalVec3(const osg::Vec3f& point); private: int mCellX; diff --git a/apps/openmw/mwmechanics/mechanicsmanagerimp.hpp b/apps/openmw/mwmechanics/mechanicsmanagerimp.hpp index 2a3f88a31..64565c031 100644 --- a/apps/openmw/mwmechanics/mechanicsmanagerimp.hpp +++ b/apps/openmw/mwmechanics/mechanicsmanagerimp.hpp @@ -54,61 +54,61 @@ namespace MWMechanics MechanicsManager(); - virtual void add (const MWWorld::Ptr& ptr); + virtual void add (const MWWorld::Ptr& ptr) override; ///< Register an object for management - virtual void remove (const MWWorld::Ptr& ptr); + virtual void remove (const MWWorld::Ptr& ptr) override; ///< Deregister an object for management - virtual void updateCell(const MWWorld::Ptr &old, const MWWorld::Ptr &ptr); + virtual void updateCell(const MWWorld::Ptr &old, const MWWorld::Ptr &ptr) override; ///< Moves an object to a new cell - virtual void drop(const MWWorld::CellStore *cellStore); + virtual void drop(const MWWorld::CellStore *cellStore) override; ///< Deregister all objects in the given cell. - virtual void watchActor(const MWWorld::Ptr& ptr); + virtual void watchActor(const MWWorld::Ptr& ptr) override; ///< On each update look for changes in a previously registered actor and update the /// GUI accordingly. - virtual void update (float duration, bool paused); + virtual void update (float duration, bool paused) override; ///< Update objects /// /// \param paused In game type does not currently advance (this usually means some GUI /// component is up). - virtual void advanceTime (float duration); + virtual void advanceTime (float duration) override; - virtual void setPlayerName (const std::string& name); + virtual void setPlayerName (const std::string& name) override; ///< Set player name. - virtual void setPlayerRace (const std::string& id, bool male, const std::string &head, const std::string &hair); + virtual void setPlayerRace (const std::string& id, bool male, const std::string &head, const std::string &hair) override; ///< Set player race. - virtual void setPlayerBirthsign (const std::string& id); + virtual void setPlayerBirthsign (const std::string& id) override; ///< Set player birthsign. - virtual void setPlayerClass (const std::string& id); + virtual void setPlayerClass (const std::string& id) override; ///< Set player class to stock class. - virtual void setPlayerClass (const ESM::Class& class_); + virtual void setPlayerClass (const ESM::Class& class_) override; ///< Set player class to custom class. - virtual void restoreDynamicStats(MWWorld::Ptr actor, bool sleep); + virtual void restoreDynamicStats(MWWorld::Ptr actor, bool sleep) override; - virtual void rest(bool sleep); + virtual void rest(bool sleep) override; ///< If the player is sleeping or waiting, this should be called every hour. /// @param sleep is the player sleeping or waiting? - virtual int getHoursToRest() const; + virtual int getHoursToRest() const override; ///< Calculate how many hours the player needs to rest in order to be fully healed - virtual int getBarterOffer(const MWWorld::Ptr& ptr,int basePrice, bool buying); + virtual int getBarterOffer(const MWWorld::Ptr& ptr,int basePrice, bool buying) override; ///< This is used by every service to determine the price of objects given the trading skills of the player and NPC. - virtual int getDerivedDisposition(const MWWorld::Ptr& ptr, bool addTemporaryDispositionChange = true); + virtual int getDerivedDisposition(const MWWorld::Ptr& ptr, bool addTemporaryDispositionChange = true) override; ///< Calculate the diposition of an NPC toward the player. - virtual int countDeaths (const std::string& id) const; + virtual int countDeaths (const std::string& id) const override; ///< Return the number of deaths for actors with the given ID. /* @@ -121,14 +121,14 @@ namespace MWMechanics End of tes3mp addition */ - virtual void getPersuasionDispositionChange (const MWWorld::Ptr& npc, PersuasionType type, bool& success, float& tempChange, float& permChange); + virtual void getPersuasionDispositionChange (const MWWorld::Ptr& npc, PersuasionType type, bool& success, float& tempChange, float& permChange) override; ///< Perform a persuasion action on NPC /// Check if \a observer is potentially aware of \a ptr. Does not do a line of sight check! - virtual bool awarenessCheck (const MWWorld::Ptr& ptr, const MWWorld::Ptr& observer); + virtual bool awarenessCheck (const MWWorld::Ptr& ptr, const MWWorld::Ptr& observer) override; /// Makes \a ptr fight \a target. Also shouts a combat taunt. - virtual void startCombat (const MWWorld::Ptr& ptr, const MWWorld::Ptr& target); + virtual void startCombat (const MWWorld::Ptr& ptr, const MWWorld::Ptr& target) override; /** * @note victim may be empty @@ -138,104 +138,98 @@ namespace MWMechanics * @return was the crime seen? */ virtual bool commitCrime (const MWWorld::Ptr& ptr, const MWWorld::Ptr& victim, - OffenseType type, int arg=0, bool victimAware=false); + OffenseType type, int arg=0, bool victimAware=false) override; /// @return false if the attack was considered a "friendly hit" and forgiven - virtual bool actorAttacked (const MWWorld::Ptr& victim, const MWWorld::Ptr& attacker); + virtual bool actorAttacked (const MWWorld::Ptr& victim, const MWWorld::Ptr& attacker) override; /// Notify that actor was killed, add a murder bounty if applicable /// @note No-op for non-player attackers - virtual void actorKilled (const MWWorld::Ptr& victim, const MWWorld::Ptr& attacker); - - /// Checks if commiting a crime is currently valid - /// @param victim The actor being attacked - /// @param attacker The actor commiting the crime - /// @return true if the victim is a valid target for crime - virtual bool canCommitCrimeAgainst(const MWWorld::Ptr& victim, const MWWorld::Ptr& attacker); + virtual void actorKilled (const MWWorld::Ptr& victim, const MWWorld::Ptr& attacker) override; /// Utility to check if taking this item is illegal and calling commitCrime if so /// @param container The container the item is in; may be empty for an item in the world virtual void itemTaken (const MWWorld::Ptr& ptr, const MWWorld::Ptr& item, const MWWorld::Ptr& container, - int count, bool alarm = true); + int count, bool alarm = true) override; /// Utility to check if opening (i.e. unlocking) this object is illegal and calling commitCrime if so - virtual void objectOpened (const MWWorld::Ptr& ptr, const MWWorld::Ptr& item); + virtual void objectOpened (const MWWorld::Ptr& ptr, const MWWorld::Ptr& item) override; /// Attempt sleeping in a bed. If this is illegal, call commitCrime. /// @return was it illegal, and someone saw you doing it? Also returns fail when enemies are nearby - virtual bool sleepInBed (const MWWorld::Ptr& ptr, const MWWorld::Ptr& bed); + virtual bool sleepInBed (const MWWorld::Ptr& ptr, const MWWorld::Ptr& bed) override; - virtual void forceStateUpdate(const MWWorld::Ptr &ptr); + virtual void forceStateUpdate(const MWWorld::Ptr &ptr) override; /// Attempt to play an animation group /// @return Success or error - virtual bool playAnimationGroup(const MWWorld::Ptr& ptr, const std::string& groupName, int mode, int number, bool persist=false); - virtual void skipAnimation(const MWWorld::Ptr& ptr); - virtual bool checkAnimationPlaying(const MWWorld::Ptr& ptr, const std::string &groupName); - virtual void persistAnimationStates(); + virtual bool playAnimationGroup(const MWWorld::Ptr& ptr, const std::string& groupName, int mode, int number, bool persist=false) override; + virtual void skipAnimation(const MWWorld::Ptr& ptr) override; + virtual bool checkAnimationPlaying(const MWWorld::Ptr& ptr, const std::string &groupName) override; + virtual void persistAnimationStates() override; /// Update magic effects for an actor. Usually done automatically once per frame, but if we're currently /// paused we may want to do it manually (after equipping permanent enchantment) - virtual void updateMagicEffects (const MWWorld::Ptr& ptr); + virtual void updateMagicEffects (const MWWorld::Ptr& ptr) override; - virtual void getObjectsInRange (const osg::Vec3f& position, float radius, std::vector& objects); - virtual void getActorsInRange(const osg::Vec3f &position, float radius, std::vector &objects); + virtual void getObjectsInRange (const osg::Vec3f& position, float radius, std::vector& objects) override; + virtual void getActorsInRange(const osg::Vec3f &position, float radius, std::vector &objects) override; /// Check if there are actors in selected range - virtual bool isAnyActorInRange(const osg::Vec3f &position, float radius); + virtual bool isAnyActorInRange(const osg::Vec3f &position, float radius) override; - virtual std::list getActorsSidingWith(const MWWorld::Ptr& actor); - virtual std::list getActorsFollowing(const MWWorld::Ptr& actor); - virtual std::list getActorsFollowingIndices(const MWWorld::Ptr& actor); + virtual std::list getActorsSidingWith(const MWWorld::Ptr& actor) override; + virtual std::list getActorsFollowing(const MWWorld::Ptr& actor) override; + virtual std::list getActorsFollowingIndices(const MWWorld::Ptr& actor) override; - virtual std::list getActorsFighting(const MWWorld::Ptr& actor); - virtual std::list getEnemiesNearby(const MWWorld::Ptr& actor); + virtual std::list getActorsFighting(const MWWorld::Ptr& actor) override; + virtual std::list getEnemiesNearby(const MWWorld::Ptr& actor) override; /// Recursive version of getActorsFollowing - virtual void getActorsFollowing(const MWWorld::Ptr& actor, std::set& out); + virtual void getActorsFollowing(const MWWorld::Ptr& actor, std::set& out) override; /// Recursive version of getActorsSidingWith - virtual void getActorsSidingWith(const MWWorld::Ptr& actor, std::set& out); + virtual void getActorsSidingWith(const MWWorld::Ptr& actor, std::set& out) override; - virtual bool toggleAI(); - virtual bool isAIActive(); + virtual bool toggleAI() override; + virtual bool isAIActive() override; - virtual void playerLoaded(); + virtual void playerLoaded() override; - virtual int countSavedGameRecords() const; + virtual int countSavedGameRecords() const override; - virtual void write (ESM::ESMWriter& writer, Loading::Listener& listener) const; + virtual void write (ESM::ESMWriter& writer, Loading::Listener& listener) const override; - virtual void readRecord (ESM::ESMReader& reader, uint32_t type); + virtual void readRecord (ESM::ESMReader& reader, uint32_t type) override; - virtual void clear(); + virtual void clear() override; - virtual bool isAggressive (const MWWorld::Ptr& ptr, const MWWorld::Ptr& target); + virtual bool isAggressive (const MWWorld::Ptr& ptr, const MWWorld::Ptr& target) override; - virtual void keepPlayerAlive(); + virtual void keepPlayerAlive() override; - virtual bool isCastingSpell (const MWWorld::Ptr& ptr) const; + virtual bool isCastingSpell (const MWWorld::Ptr& ptr) const override; - virtual bool isReadyToBlock (const MWWorld::Ptr& ptr) const; + virtual bool isReadyToBlock (const MWWorld::Ptr& ptr) const override; /// Is \a ptr casting spell or using weapon now? - virtual bool isAttackingOrSpell(const MWWorld::Ptr &ptr) const; + virtual bool isAttackingOrSpell(const MWWorld::Ptr &ptr) const override; - virtual void castSpell(const MWWorld::Ptr& ptr, const std::string spellId, bool manualSpell=false); + virtual void castSpell(const MWWorld::Ptr& ptr, const std::string spellId, bool manualSpell=false) override; void processChangedSettings(const Settings::CategorySettingVector& settings) override; - virtual float getActorsProcessingRange() const; + virtual float getActorsProcessingRange() const override; /// Check if the target actor was detected by an observer /// If the observer is a non-NPC, check all actors in AI processing distance as observers - virtual bool isActorDetected(const MWWorld::Ptr& actor, const MWWorld::Ptr& observer); + virtual bool isActorDetected(const MWWorld::Ptr& actor, const MWWorld::Ptr& observer) override; - virtual void confiscateStolenItems (const MWWorld::Ptr& player, const MWWorld::Ptr& targetContainer); + virtual void confiscateStolenItems (const MWWorld::Ptr& player, const MWWorld::Ptr& targetContainer) override; /// List the owners that the player has stolen this item from (the owner can be an NPC or a faction). /// - virtual std::vector > getStolenItemOwners(const std::string& itemid); + virtual std::vector > getStolenItemOwners(const std::string& itemid) override; /// Has the player stolen this item from the given owner? - virtual bool isItemStolenFrom(const std::string& itemid, const MWWorld::Ptr& ptr); + virtual bool isItemStolenFrom(const std::string& itemid, const MWWorld::Ptr& ptr) override; - virtual bool isBoundItem(const MWWorld::Ptr& item); + virtual bool isBoundItem(const MWWorld::Ptr& item) override; /* Start of tes3mp addition @@ -248,26 +242,25 @@ namespace MWMechanics */ /// @return is \a ptr allowed to take/use \a target or is it a crime? - virtual bool isAllowedToUse (const MWWorld::Ptr& ptr, const MWWorld::Ptr& target, MWWorld::Ptr& victim); + virtual bool isAllowedToUse (const MWWorld::Ptr& ptr, const MWWorld::Ptr& target, MWWorld::Ptr& victim) override; - virtual void setWerewolf(const MWWorld::Ptr& actor, bool werewolf); - virtual void applyWerewolfAcrobatics(const MWWorld::Ptr& actor); + virtual void setWerewolf(const MWWorld::Ptr& actor, bool werewolf) override; + virtual void applyWerewolfAcrobatics(const MWWorld::Ptr& actor) override; - virtual void cleanupSummonedCreature(const MWWorld::Ptr& caster, int creatureActorId); + virtual void cleanupSummonedCreature(const MWWorld::Ptr& caster, int creatureActorId) override; - virtual void confiscateStolenItemToOwner(const MWWorld::Ptr &player, const MWWorld::Ptr &item, const MWWorld::Ptr& victim, int count); + virtual void confiscateStolenItemToOwner(const MWWorld::Ptr &player, const MWWorld::Ptr &item, const MWWorld::Ptr& victim, int count) override; - virtual bool isAttackPreparing(const MWWorld::Ptr& ptr); - virtual bool isRunning(const MWWorld::Ptr& ptr); - virtual bool isSneaking(const MWWorld::Ptr& ptr); + virtual bool isAttackPreparing(const MWWorld::Ptr& ptr) override; + virtual bool isRunning(const MWWorld::Ptr& ptr) override; + virtual bool isSneaking(const MWWorld::Ptr& ptr) override; private: + bool canCommitCrimeAgainst(const MWWorld::Ptr& victim, const MWWorld::Ptr& attacker); bool canReportCrime(const MWWorld::Ptr &actor, const MWWorld::Ptr &victim, std::set &playerFollowers); bool reportCrime (const MWWorld::Ptr& ptr, const MWWorld::Ptr& victim, OffenseType type, int arg=0); - - }; } diff --git a/apps/openmw/mwmechanics/obstacle.cpp b/apps/openmw/mwmechanics/obstacle.cpp index 0635a5520..9562c74df 100644 --- a/apps/openmw/mwmechanics/obstacle.cpp +++ b/apps/openmw/mwmechanics/obstacle.cpp @@ -96,11 +96,6 @@ namespace MWMechanics mEvadeDuration = 0; } - bool ObstacleCheck::isNormalState() const - { - return mWalkState == State_Norm; - } - bool ObstacleCheck::isEvading() const { return mWalkState == State_Evade; @@ -128,7 +123,7 @@ namespace MWMechanics * u = how long to move sideways * */ - bool ObstacleCheck::check(const MWWorld::Ptr& actor, float duration, float scaleMinimumDistance) + void ObstacleCheck::update(const MWWorld::Ptr& actor, float duration, float scaleMinimumDistance) { const MWWorld::Class& cls = actor.getClass(); ESM::Position pos = actor.getRefData().getPosition(); @@ -180,9 +175,7 @@ namespace MWMechanics case State_Evade: { mEvadeDuration += duration; - if(mEvadeDuration < DURATION_TO_EVADE) - return true; - else + if(mEvadeDuration >= DURATION_TO_EVADE) { // tried to evade, assume all is ok and start again mWalkState = State_Norm; @@ -191,10 +184,9 @@ namespace MWMechanics } /* NO DEFAULT CASE */ } - return false; // no obstacles to evade (yet) } - void ObstacleCheck::takeEvasiveAction(MWMechanics::Movement& actorMovement) + void ObstacleCheck::takeEvasiveAction(MWMechanics::Movement& actorMovement) const { actorMovement.mPosition[0] = evadeDirections[mEvadeDirectionIndex][0]; actorMovement.mPosition[1] = evadeDirections[mEvadeDirectionIndex][1]; diff --git a/apps/openmw/mwmechanics/obstacle.hpp b/apps/openmw/mwmechanics/obstacle.hpp index c55374501..d7e582f8c 100644 --- a/apps/openmw/mwmechanics/obstacle.hpp +++ b/apps/openmw/mwmechanics/obstacle.hpp @@ -27,15 +27,13 @@ namespace MWMechanics // Clear the timers and set the state machine to default void clear(); - bool isNormalState() const; bool isEvading() const; - // Returns true if there is an obstacle and an evasive action - // should be taken - bool check(const MWWorld::Ptr& actor, float duration, float scaleMinimumDistance = 1.0f); + // Updates internal state, call each frame for moving actor + void update(const MWWorld::Ptr& actor, float duration, float scaleMinimumDistance = 1.0f); // change direction to try to fix "stuck" actor - void takeEvasiveAction(MWMechanics::Movement& actorMovement); + void takeEvasiveAction(MWMechanics::Movement& actorMovement) const; private: diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index c16cff9e1..8c7d6fce9 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -1,13 +1,20 @@ #include "pathfinding.hpp" +#include #include +#include +#include +#include +#include + #include "../mwbase/world.hpp" #include "../mwbase/environment.hpp" #include "../mwphysics/collisiontype.hpp" #include "../mwworld/cellstore.hpp" +#include "../mwworld/class.hpp" #include "pathgrid.hpp" #include "coordinateconverter.hpp" @@ -29,7 +36,7 @@ namespace // points to a quadtree may help for(unsigned int counter = 0; counter < grid->mPoints.size(); counter++) { - float potentialDistBetween = MWMechanics::PathFinder::DistanceSquared(grid->mPoints[counter], pos); + float potentialDistBetween = MWMechanics::PathFinder::distanceSquared(grid->mPoints[counter], pos); if (potentialDistBetween < closestDistanceReachable) { // found a closer one @@ -57,56 +64,19 @@ namespace (closestReachableIndex, closestReachableIndex == closestIndex); } -} - -namespace MWMechanics -{ - float sqrDistanceIgnoreZ(const ESM::Pathgrid::Point& point, float x, float y) - { - x -= point.mX; - y -= point.mY; - return (x * x + y * y); - } - - float distance(const ESM::Pathgrid::Point& point, float x, float y, float z) - { - x -= point.mX; - y -= point.mY; - z -= point.mZ; - return sqrt(x * x + y * y + z * z); - } - - float distance(const ESM::Pathgrid::Point& a, const ESM::Pathgrid::Point& b) - { - float x = static_cast(a.mX - b.mX); - float y = static_cast(a.mY - b.mY); - float z = static_cast(a.mZ - b.mZ); - return sqrt(x * x + y * y + z * z); - } - - float getZAngleToDir(const osg::Vec3f& dir) - { - return std::atan2(dir.x(), dir.y()); - } - - float getXAngleToDir(const osg::Vec3f& dir) - { - float dirLen = dir.length(); - return (dirLen != 0) ? -std::asin(dir.z() / dirLen) : 0; - } - - float getZAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest) + float sqrDistance(const osg::Vec2f& lhs, const osg::Vec2f& rhs) { - osg::Vec3f dir = PathFinder::MakeOsgVec3(dest) - PathFinder::MakeOsgVec3(origin); - return getZAngleToDir(dir); + return (lhs - rhs).length2(); } - float getXAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest) + float sqrDistanceIgnoreZ(const osg::Vec3f& lhs, const osg::Vec3f& rhs) { - osg::Vec3f dir = PathFinder::MakeOsgVec3(dest) - PathFinder::MakeOsgVec3(origin); - return getXAngleToDir(dir); + return sqrDistance(osg::Vec2f(lhs.x(), lhs.y()), osg::Vec2f(rhs.x(), rhs.y())); } +} +namespace MWMechanics +{ bool checkWayIsClear(const osg::Vec3f& from, const osg::Vec3f& to, float offsetXY) { osg::Vec3f dir = to - from; @@ -121,18 +91,6 @@ namespace MWMechanics return (std::abs(from.z() - h) <= PATHFIND_Z_REACH); } - PathFinder::PathFinder() - : mPathgrid(nullptr) - , mCell(nullptr) - { - } - - void PathFinder::clearPath() - { - if(!mPath.empty()) - mPath.clear(); - } - /* * NOTE: This method may fail to find a path. The caller must check the * result before using it. If there is no path the AI routies need to @@ -150,7 +108,7 @@ namespace MWMechanics * pathgrid point (e.g. wander) then it may be worth while to call * pop_back() to remove the redundant entry. * - * NOTE: coordinates must be converted prior to calling GetClosestPoint() + * NOTE: coordinates must be converted prior to calling getClosestPoint() * * | * | cell @@ -169,52 +127,44 @@ namespace MWMechanics * j = @.x in local coordinates (i.e. within the cell) * k = @.x in world coordinates */ - void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint, - const ESM::Pathgrid::Point &endPoint, - const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph) + void PathFinder::buildPathByPathgridImpl(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const PathgridGraph& pathgridGraph, std::back_insert_iterator> out) { - mPath.clear(); - - // TODO: consider removing mCell / mPathgrid in favor of mPathgridGraph - if(mCell != cell || !mPathgrid) - { - mCell = cell; - mPathgrid = pathgridGraph.getPathgrid(); - } + const auto pathgrid = pathgridGraph.getPathgrid(); // Refer to AiWander reseach topic on openmw forums for some background. // Maybe there is no pathgrid for this cell. Just go to destination and let // physics take care of any blockages. - if(!mPathgrid || mPathgrid->mPoints.empty()) + if(!pathgrid || pathgrid->mPoints.empty()) { - mPath.push_back(endPoint); + *out++ = endPoint; return; } - // NOTE: GetClosestPoint expects local coordinates + // NOTE: getClosestPoint expects local coordinates CoordinateConverter converter(mCell->getCell()); - // NOTE: It is possible that GetClosestPoint returns a pathgrind point index + // NOTE: It is possible that getClosestPoint returns a pathgrind point index // that is unreachable in some situations. e.g. actor is standing // outside an area enclosed by walls, but there is a pathgrid // point right behind the wall that is closer than any pathgrid // point outside the wall osg::Vec3f startPointInLocalCoords(converter.toLocalVec3(startPoint)); - int startNode = GetClosestPoint(mPathgrid, startPointInLocalCoords); + int startNode = getClosestPoint(pathgrid, startPointInLocalCoords); osg::Vec3f endPointInLocalCoords(converter.toLocalVec3(endPoint)); - std::pair endNode = getClosestReachablePoint(mPathgrid, &pathgridGraph, + std::pair endNode = getClosestReachablePoint(pathgrid, &pathgridGraph, endPointInLocalCoords, startNode); // if it's shorter for actor to travel from start to end, than to travel from either // start or end to nearest pathgrid point, just travel from start to end. float startToEndLength2 = (endPointInLocalCoords - startPointInLocalCoords).length2(); - float endTolastNodeLength2 = DistanceSquared(mPathgrid->mPoints[endNode.first], endPointInLocalCoords); - float startTo1stNodeLength2 = DistanceSquared(mPathgrid->mPoints[startNode], startPointInLocalCoords); + float endTolastNodeLength2 = distanceSquared(pathgrid->mPoints[endNode.first], endPointInLocalCoords); + float startTo1stNodeLength2 = distanceSquared(pathgrid->mPoints[startNode], startPointInLocalCoords); if ((startToEndLength2 < startTo1stNodeLength2) || (startToEndLength2 < endTolastNodeLength2)) { - mPath.push_back(endPoint); + *out++ = endPoint; return; } @@ -225,21 +175,21 @@ namespace MWMechanics // nodes are the same if(startNode == endNode.first) { - ESM::Pathgrid::Point temp(mPathgrid->mPoints[startNode]); + ESM::Pathgrid::Point temp(pathgrid->mPoints[startNode]); converter.toWorld(temp); - mPath.push_back(temp); + *out++ = makeOsgVec3(temp); } else { - mPath = pathgridGraph.aStarSearch(startNode, endNode.first); + auto path = pathgridGraph.aStarSearch(startNode, endNode.first); // If nearest path node is in opposite direction from second, remove it from path. // Especially useful for wandering actors, if the nearest node is blocked for some reason. - if (mPath.size() > 1) + if (path.size() > 1) { - ESM::Pathgrid::Point secondNode = *(++mPath.begin()); - osg::Vec3f firstNodeVec3f = MakeOsgVec3(mPathgrid->mPoints[startNode]); - osg::Vec3f secondNodeVec3f = MakeOsgVec3(secondNode); + ESM::Pathgrid::Point secondNode = *(++path.begin()); + osg::Vec3f firstNodeVec3f = makeOsgVec3(pathgrid->mPoints[startNode]); + osg::Vec3f secondNodeVec3f = makeOsgVec3(secondNode); osg::Vec3f toSecondNodeVec3f = secondNodeVec3f - firstNodeVec3f; osg::Vec3f toStartPointVec3f = startPointInLocalCoords - firstNodeVec3f; if (toSecondNodeVec3f * toStartPointVec3f > 0) @@ -248,23 +198,25 @@ namespace MWMechanics converter.toWorld(temp); // Add Z offset since path node can overlap with other objects. // Also ignore doors in raytesting. - int mask = MWPhysics::CollisionType_World; + const int mask = MWPhysics::CollisionType_World; bool isPathClear = !MWBase::Environment::get().getWorld()->castRay( - startPoint.mX, startPoint.mY, startPoint.mZ+16, temp.mX, temp.mY, temp.mZ+16, mask); + startPoint.x(), startPoint.y(), startPoint.z() + 16, temp.mX, temp.mY, temp.mZ + 16, mask); if (isPathClear) - mPath.pop_front(); + path.pop_front(); } } // convert supplied path to world coordinates - for (std::list::iterator iter(mPath.begin()); iter != mPath.end(); ++iter) - { - converter.toWorld(*iter); - } + std::transform(path.begin(), path.end(), out, + [&] (ESM::Pathgrid::Point& point) + { + converter.toWorld(point); + return makeOsgVec3(point); + }); } // If endNode found is NOT the closest PathGrid point to the endPoint, - // assume endPoint is not reachable from endNode. In which case, + // assume endPoint is not reachable from endNode. In which case, // path ends at endNode. // // So only add the destination (which may be different to the closest @@ -276,7 +228,7 @@ namespace MWMechanics // // The AI routines will have to deal with such situations. if(endNode.second) - mPath.push_back(endPoint); + *out++ = endPoint; } float PathFinder::getZAngleToNext(float x, float y) const @@ -286,9 +238,9 @@ namespace MWMechanics if(mPath.empty()) return 0.; - const ESM::Pathgrid::Point &nextPoint = *mPath.begin(); - float directionX = nextPoint.mX - x; - float directionY = nextPoint.mY - y; + const auto& nextPoint = mPath.front(); + const float directionX = nextPoint.x() - x; + const float directionY = nextPoint.y() - y; return std::atan2(directionX, directionY); } @@ -300,62 +252,64 @@ namespace MWMechanics if(mPath.empty()) return 0.; - const ESM::Pathgrid::Point &nextPoint = *mPath.begin(); - osg::Vec3f dir = MakeOsgVec3(nextPoint) - osg::Vec3f(x,y,z); + const osg::Vec3f dir = mPath.front() - osg::Vec3f(x, y, z); return getXAngleToDir(dir); } - bool PathFinder::checkPathCompleted(float x, float y, float tolerance) + void PathFinder::update(const osg::Vec3f& position, const float pointTolerance, const float destinationTolerance) { - if(mPath.empty()) - return true; + if (mPath.empty()) + return; - const ESM::Pathgrid::Point& nextPoint = *mPath.begin(); - if (sqrDistanceIgnoreZ(nextPoint, x, y) < tolerance*tolerance) - { + const auto tolerance = mPath.size() > 1 ? pointTolerance : destinationTolerance; + + if (sqrDistanceIgnoreZ(mPath.front(), position) < tolerance * tolerance) mPath.pop_front(); - if(mPath.empty()) - { - return true; - } - } + } + + void PathFinder::buildPathByPathgrid(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph) + { + mPath.clear(); + mCell = cell; - return false; + buildPathByPathgridImpl(startPoint, endPoint, pathgridGraph, std::back_inserter(mPath)); + + mConstructed = true; + } + + void PathFinder::buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, + const DetourNavigator::Flags flags) + { + mPath.clear(); + mCell = cell; + + buildPathByNavigatorImpl(actor, startPoint, endPoint, halfExtents, flags, std::back_inserter(mPath)); + + if (mPath.empty()) + buildPathByPathgridImpl(startPoint, endPoint, pathgridGraph, std::back_inserter(mPath)); + + mConstructed = true; } - // see header for the rationale - void PathFinder::buildSyncedPath(const ESM::Pathgrid::Point &startPoint, - const ESM::Pathgrid::Point &endPoint, - const MWWorld::CellStore* cell, const MWMechanics::PathgridGraph& pathgridGraph) + void PathFinder::buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, + const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, + std::back_insert_iterator> out) { - if (mPath.size() < 2) + try { - // if path has one point, then it's the destination. - // don't need to worry about bad path for this case - buildPath(startPoint, endPoint, cell, pathgridGraph); + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + navigator->findPath(halfExtents, startPoint, endPoint, flags, out); } - else + catch (const DetourNavigator::NavigatorException& exception) { - const ESM::Pathgrid::Point oldStart(*getPath().begin()); - buildPath(startPoint, endPoint, cell, pathgridGraph); - if (mPath.size() >= 2) - { - // if 2nd waypoint of new path == 1st waypoint of old, - // delete 1st waypoint of new path. - std::list::iterator iter = ++mPath.begin(); - if (iter->mX == oldStart.mX - && iter->mY == oldStart.mY - && iter->mZ == oldStart.mZ) - { - mPath.pop_front(); - } - } + DetourNavigator::log("PathFinder::buildPathByNavigator navigator exception: ", exception.what()); + Log(Debug::Verbose) << "Build path by navigator exception: \"" << exception.what() + << "\" for \"" << actor.getClass().getName(actor) << "\" (" << actor.getBase() + << ") from " << startPoint << " to " << endPoint << " with flags (" + << DetourNavigator::WriteFlags {flags} << ")"; } } - - const MWWorld::CellStore* PathFinder::getPathCell() const - { - return mCell; - } } diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index ebc22f10d..567056fa5 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -1,33 +1,57 @@ #ifndef GAME_MWMECHANICS_PATHFINDING_H #define GAME_MWMECHANICS_PATHFINDING_H -#include +#include #include +#include +#include #include #include namespace MWWorld { class CellStore; + class ConstPtr; } namespace MWMechanics { class PathgridGraph; - float distance(const ESM::Pathgrid::Point& point, float x, float y, float); - float distance(const ESM::Pathgrid::Point& a, const ESM::Pathgrid::Point& b); - float getZAngleToDir(const osg::Vec3f& dir); - float getXAngleToDir(const osg::Vec3f& dir); - float getZAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest); - float getXAngleToPoint(const ESM::Pathgrid::Point &origin, const ESM::Pathgrid::Point &dest); + inline float distance(const osg::Vec3f& lhs, const osg::Vec3f& rhs) + { + return (lhs - rhs).length(); + } + + inline float getZAngleToDir(const osg::Vec3f& dir) + { + return std::atan2(dir.x(), dir.y()); + } + + inline float getXAngleToDir(const osg::Vec3f& dir) + { + float dirLen = dir.length(); + return (dirLen != 0) ? -std::asin(dir.z() / dirLen) : 0; + } + + inline float getZAngleToPoint(const osg::Vec3f& origin, const osg::Vec3f& dest) + { + return getZAngleToDir(dest - origin); + } + + inline float getXAngleToPoint(const osg::Vec3f& origin, const osg::Vec3f& dest) + { + return getXAngleToDir(dest - origin); + } - const float PATHFIND_Z_REACH = 50.0f; + const float PATHFIND_Z_REACH = 50.0f; //static const float sMaxSlope = 49.0f; // duplicate as in physicssystem // distance after which actor (failed previously to shortcut) will try again const float PATHFIND_SHORTCUT_RETRY_DIST = 300.0f; + const float DEFAULT_TOLERANCE = 32.0f; + // cast up-down ray with some offset from actor position to check for pits/obstacles on the way to target; // magnitude of pits/obstacles is defined by PATHFIND_Z_REACH bool checkWayIsClear(const osg::Vec3f& from, const osg::Vec3f& to, float offsetXY); @@ -35,31 +59,33 @@ namespace MWMechanics class PathFinder { public: - PathFinder(); - - static const int PathTolerance = 32; - - static float sgn(float val) + PathFinder() + : mConstructed(false) + , mCell(nullptr) { - if(val > 0) - return 1.0; - return -1.0; } - static int sgn(int a) + void clearPath() { - if(a > 0) - return 1; - return -1; + mConstructed = false; + mPath.clear(); + mCell = nullptr; } - void clearPath(); + void buildPathByPathgrid(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph); + + void buildPath(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph, const osg::Vec3f& halfExtents, + const DetourNavigator::Flags flags); - void buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint, - const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph); + /// Remove front point if exist and within tolerance + void update(const osg::Vec3f& position, const float pointTolerance, const float destinationTolerance); - bool checkPathCompleted(float x, float y, float tolerance = PathTolerance); - ///< \Returns true if we are within \a tolerance units of the last path point. + bool checkPathCompleted() const + { + return mConstructed && mPath.empty(); + } /// In radians float getZAngleToNext(float x, float y) const; @@ -68,60 +94,54 @@ namespace MWMechanics bool isPathConstructed() const { - return !mPath.empty(); + return mConstructed && !mPath.empty(); } - int getPathSize() const + std::size_t getPathSize() const { return mPath.size(); } - const std::list& getPath() const + const std::deque& getPath() const { return mPath; } - const MWWorld::CellStore* getPathCell() const; - - /** Synchronize new path with old one to avoid visiting 1 waypoint 2 times - @note - BuildPath() takes closest PathGrid point to NPC as first point of path. - This is undesirable if NPC has just passed a Pathgrid point, as this - makes the 2nd point of the new path == the 1st point of old path. - Which results in NPC "running in a circle" back to the just passed waypoint. - */ - void buildSyncedPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint, - const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph); + const MWWorld::CellStore* getPathCell() const + { + return mCell; + } - void addPointToPath(const ESM::Pathgrid::Point &point) + void addPointToPath(const osg::Vec3f& point) { + mConstructed = true; mPath.push_back(point); } /// utility function to convert a osg::Vec3f to a Pathgrid::Point - static ESM::Pathgrid::Point MakePathgridPoint(const osg::Vec3f& v) + static ESM::Pathgrid::Point makePathgridPoint(const osg::Vec3f& v) { return ESM::Pathgrid::Point(static_cast(v[0]), static_cast(v[1]), static_cast(v[2])); } /// utility function to convert an ESM::Position to a Pathgrid::Point - static ESM::Pathgrid::Point MakePathgridPoint(const ESM::Position& p) + static ESM::Pathgrid::Point makePathgridPoint(const ESM::Position& p) { return ESM::Pathgrid::Point(static_cast(p.pos[0]), static_cast(p.pos[1]), static_cast(p.pos[2])); } - static osg::Vec3f MakeOsgVec3(const ESM::Pathgrid::Point& p) + static osg::Vec3f makeOsgVec3(const ESM::Pathgrid::Point& p) { return osg::Vec3f(static_cast(p.mX), static_cast(p.mY), static_cast(p.mZ)); } - + // Slightly cheaper version for comparisons. // Caller needs to be careful for very short distances (i.e. less than 1) // or when accumuating the results i.e. (a + b)^2 != a^2 + b^2 // - static float DistanceSquared(ESM::Pathgrid::Point point, const osg::Vec3f& pos) + static float distanceSquared(ESM::Pathgrid::Point point, const osg::Vec3f& pos) { - return (MWMechanics::PathFinder::MakeOsgVec3(point) - pos).length2(); + return (MWMechanics::PathFinder::makeOsgVec3(point) - pos).length2(); } // Return the closest pathgrid point index from the specified position @@ -130,18 +150,18 @@ namespace MWMechanics // // NOTE: pos is expected to be in local coordinates, as is grid->mPoints // - static int GetClosestPoint(const ESM::Pathgrid* grid, const osg::Vec3f& pos) + static int getClosestPoint(const ESM::Pathgrid* grid, const osg::Vec3f& pos) { assert(grid && !grid->mPoints.empty()); - float distanceBetween = DistanceSquared(grid->mPoints[0], pos); + float distanceBetween = distanceSquared(grid->mPoints[0], pos); int closestIndex = 0; // TODO: if this full scan causes performance problems mapping pathgrid // points to a quadtree may help for(unsigned int counter = 1; counter < grid->mPoints.size(); counter++) { - float potentialDistBetween = DistanceSquared(grid->mPoints[counter], pos); + float potentialDistBetween = distanceSquared(grid->mPoints[counter], pos); if(potentialDistBetween < distanceBetween) { distanceBetween = potentialDistBetween; @@ -153,10 +173,17 @@ namespace MWMechanics } private: - std::list mPath; + bool mConstructed; + std::deque mPath; - const ESM::Pathgrid *mPathgrid; const MWWorld::CellStore* mCell; + + void buildPathByPathgridImpl(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint, + const PathgridGraph& pathgridGraph, std::back_insert_iterator> out); + + void buildPathByNavigatorImpl(const MWWorld::ConstPtr& actor, const osg::Vec3f& startPoint, + const osg::Vec3f& endPoint, const osg::Vec3f& halfExtents, const DetourNavigator::Flags flags, + std::back_insert_iterator> out); }; } diff --git a/apps/openmw/mwmechanics/pathgrid.cpp b/apps/openmw/mwmechanics/pathgrid.cpp index 6b5db64ea..7bcdc8926 100644 --- a/apps/openmw/mwmechanics/pathgrid.cpp +++ b/apps/openmw/mwmechanics/pathgrid.cpp @@ -257,10 +257,9 @@ namespace MWMechanics * pathgrid points form (currently they are converted to world * coordinates). Essentially trading speed w/ memory. */ - std::list PathgridGraph::aStarSearch(const int start, - const int goal) const + std::deque PathgridGraph::aStarSearch(const int start, const int goal) const { - std::list path; + std::deque path; if(!isPointConnected(start, goal)) { return path; // there is no path, return an empty path diff --git a/apps/openmw/mwmechanics/pathgrid.hpp b/apps/openmw/mwmechanics/pathgrid.hpp index 0c71c4561..6b67bb507 100644 --- a/apps/openmw/mwmechanics/pathgrid.hpp +++ b/apps/openmw/mwmechanics/pathgrid.hpp @@ -1,7 +1,7 @@ #ifndef GAME_MWMECHANICS_PATHGRID_H #define GAME_MWMECHANICS_PATHGRID_H -#include +#include #include @@ -38,8 +38,8 @@ namespace MWMechanics // cells) coordinates // // NOTE: if start equals end an empty path is returned - std::list aStarSearch(const int start, - const int end) const; + std::deque aStarSearch(const int start, const int end) const; + private: const ESM::Cell *mCell; diff --git a/apps/openmw/mwphysics/actor.hpp b/apps/openmw/mwphysics/actor.hpp index 8ec94200f..31dd22a22 100644 --- a/apps/openmw/mwphysics/actor.hpp +++ b/apps/openmw/mwphysics/actor.hpp @@ -3,7 +3,7 @@ #include -#include "../mwworld/ptr.hpp" +#include "ptrholder.hpp" #include #include @@ -22,30 +22,6 @@ namespace Resource namespace MWPhysics { - class PtrHolder - { - public: - virtual ~PtrHolder() {} - - void updatePtr(const MWWorld::Ptr& updated) - { - mPtr = updated; - } - - MWWorld::Ptr getPtr() - { - return mPtr; - } - - MWWorld::ConstPtr getPtr() const - { - return mPtr; - } - - protected: - MWWorld::Ptr mPtr; - }; - class Actor : public PtrHolder { public: diff --git a/apps/openmw/mwphysics/heightfield.cpp b/apps/openmw/mwphysics/heightfield.cpp new file mode 100644 index 000000000..52aed9c07 --- /dev/null +++ b/apps/openmw/mwphysics/heightfield.cpp @@ -0,0 +1,54 @@ +#include "heightfield.hpp" + +#include + +#include +#include + +#include + +namespace MWPhysics +{ + HeightField::HeightField(const float* heights, int x, int y, float triSize, float sqrtVerts, float minH, float maxH, const osg::Object* holdObject) + { + mShape = new btHeightfieldTerrainShape( + sqrtVerts, sqrtVerts, heights, 1, + minH, maxH, 2, + PHY_FLOAT, false + ); + mShape->setUseDiamondSubdivision(true); + mShape->setLocalScaling(btVector3(triSize, triSize, 1)); + + btTransform transform(btQuaternion::getIdentity(), + btVector3((x+0.5f) * triSize * (sqrtVerts-1), + (y+0.5f) * triSize * (sqrtVerts-1), + (maxH+minH)*0.5f)); + + mCollisionObject = new btCollisionObject; + mCollisionObject->setCollisionShape(mShape); + mCollisionObject->setWorldTransform(transform); + + mHoldObject = holdObject; + } + + HeightField::~HeightField() + { + delete mCollisionObject; + delete mShape; + } + + btCollisionObject* HeightField::getCollisionObject() + { + return mCollisionObject; + } + + const btCollisionObject* HeightField::getCollisionObject() const + { + return mCollisionObject; + } + + const btHeightfieldTerrainShape* HeightField::getShape() const + { + return mShape; + } +} diff --git a/apps/openmw/mwphysics/heightfield.hpp b/apps/openmw/mwphysics/heightfield.hpp new file mode 100644 index 000000000..f248186db --- /dev/null +++ b/apps/openmw/mwphysics/heightfield.hpp @@ -0,0 +1,36 @@ +#ifndef OPENMW_MWPHYSICS_HEIGHTFIELD_H +#define OPENMW_MWPHYSICS_HEIGHTFIELD_H + +#include + +class btCollisionObject; +class btHeightfieldTerrainShape; + +namespace osg +{ + class Object; +} + +namespace MWPhysics +{ + class HeightField + { + public: + HeightField(const float* heights, int x, int y, float triSize, float sqrtVerts, float minH, float maxH, const osg::Object* holdObject); + ~HeightField(); + + btCollisionObject* getCollisionObject(); + const btCollisionObject* getCollisionObject() const; + const btHeightfieldTerrainShape* getShape() const; + + private: + btHeightfieldTerrainShape* mShape; + btCollisionObject* mCollisionObject; + osg::ref_ptr mHoldObject; + + void operator=(const HeightField&); + HeightField(const HeightField&); + }; +} + +#endif diff --git a/apps/openmw/mwphysics/object.cpp b/apps/openmw/mwphysics/object.cpp new file mode 100644 index 000000000..2f2866821 --- /dev/null +++ b/apps/openmw/mwphysics/object.cpp @@ -0,0 +1,130 @@ +#include "object.hpp" +#include "convert.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +namespace MWPhysics +{ + Object::Object(const MWWorld::Ptr& ptr, osg::ref_ptr shapeInstance) + : mShapeInstance(shapeInstance) + , mSolid(true) + { + mPtr = ptr; + + mCollisionObject.reset(new btCollisionObject); + mCollisionObject->setCollisionShape(shapeInstance->getCollisionShape()); + + mCollisionObject->setUserPointer(static_cast(this)); + + setScale(ptr.getCellRef().getScale()); + setRotation(toBullet(ptr.getRefData().getBaseNode()->getAttitude())); + const float* pos = ptr.getRefData().getPosition().pos; + setOrigin(btVector3(pos[0], pos[1], pos[2])); + } + + const Resource::BulletShapeInstance* Object::getShapeInstance() const + { + return mShapeInstance.get(); + } + + void Object::setScale(float scale) + { + mShapeInstance->setLocalScaling(btVector3(scale, scale, scale)); + } + + void Object::setRotation(const btQuaternion& quat) + { + mCollisionObject->getWorldTransform().setRotation(quat); + } + + void Object::setOrigin(const btVector3& vec) + { + mCollisionObject->getWorldTransform().setOrigin(vec); + } + + btCollisionObject* Object::getCollisionObject() + { + return mCollisionObject.get(); + } + + const btCollisionObject* Object::getCollisionObject() const + { + return mCollisionObject.get(); + } + + bool Object::isSolid() const + { + return mSolid; + } + + void Object::setSolid(bool solid) + { + mSolid = solid; + } + + bool Object::isAnimated() const + { + return !mShapeInstance->mAnimatedShapes.empty(); + } + + void Object::animateCollisionShapes(btCollisionWorld* collisionWorld) + { + if (mShapeInstance->mAnimatedShapes.empty()) + return; + + assert (mShapeInstance->getCollisionShape()->isCompound()); + + btCompoundShape* compound = static_cast(mShapeInstance->getCollisionShape()); + for (std::map::const_iterator it = mShapeInstance->mAnimatedShapes.begin(); it != mShapeInstance->mAnimatedShapes.end(); ++it) + { + int recIndex = it->first; + int shapeIndex = it->second; + + std::map::iterator nodePathFound = mRecIndexToNodePath.find(recIndex); + if (nodePathFound == mRecIndexToNodePath.end()) + { + NifOsg::FindGroupByRecIndex visitor(recIndex); + mPtr.getRefData().getBaseNode()->accept(visitor); + if (!visitor.mFound) + { + Log(Debug::Warning) << "Warning: animateCollisionShapes can't find node " << recIndex << " for " << mPtr.getCellRef().getRefId(); + + // Remove nonexistent nodes from animated shapes map and early out + mShapeInstance->mAnimatedShapes.erase(recIndex); + return; + } + osg::NodePath nodePath = visitor.mFoundPath; + nodePath.erase(nodePath.begin()); + nodePathFound = mRecIndexToNodePath.insert(std::make_pair(recIndex, nodePath)).first; + } + + osg::NodePath& nodePath = nodePathFound->second; + osg::Matrixf matrix = osg::computeLocalToWorld(nodePath); + matrix.orthoNormalize(matrix); + + btTransform transform; + transform.setOrigin(toBullet(matrix.getTrans()) * compound->getLocalScaling()); + for (int i=0; i<3; ++i) + for (int j=0; j<3; ++j) + transform.getBasis()[i][j] = matrix(j,i); // NB column/row major difference + + // Note: we can not apply scaling here for now since we treat scaled shapes + // as new shapes (btScaledBvhTriangleMeshShape) with 1.0 scale for now + if (!(transform == compound->getChildTransform(shapeIndex))) + compound->updateChildTransform(shapeIndex, transform); + } + + collisionWorld->updateSingleAabb(mCollisionObject.get()); + } +} diff --git a/apps/openmw/mwphysics/object.hpp b/apps/openmw/mwphysics/object.hpp new file mode 100644 index 000000000..b948433e3 --- /dev/null +++ b/apps/openmw/mwphysics/object.hpp @@ -0,0 +1,48 @@ +#ifndef OPENMW_MWPHYSICS_OBJECT_H +#define OPENMW_MWPHYSICS_OBJECT_H + +#include "ptrholder.hpp" + +#include + +#include +#include + +namespace Resource +{ + class BulletShapeInstance; +} + +class btCollisionObject; +class btCollisionWorld; +class btQuaternion; +class btVector3; + +namespace MWPhysics +{ + class Object : public PtrHolder + { + public: + Object(const MWWorld::Ptr& ptr, osg::ref_ptr shapeInstance); + + const Resource::BulletShapeInstance* getShapeInstance() const; + void setScale(float scale); + void setRotation(const btQuaternion& quat); + void setOrigin(const btVector3& vec); + btCollisionObject* getCollisionObject(); + const btCollisionObject* getCollisionObject() const; + /// Return solid flag. Not used by the object itself, true by default. + bool isSolid() const; + void setSolid(bool solid); + bool isAnimated() const; + void animateCollisionShapes(btCollisionWorld* collisionWorld); + + private: + std::unique_ptr mCollisionObject; + osg::ref_ptr mShapeInstance; + std::map mRecIndexToNodePath; + bool mSolid; + }; +} + +#endif diff --git a/apps/openmw/mwphysics/physicssystem.cpp b/apps/openmw/mwphysics/physicssystem.cpp index 5445d16bd..ab384cf23 100644 --- a/apps/openmw/mwphysics/physicssystem.cpp +++ b/apps/openmw/mwphysics/physicssystem.cpp @@ -1,6 +1,11 @@ -#include "physicssystem.hpp" +#include "physicssystem.hpp" #include +#include +#include +#include + +#include #include @@ -16,6 +21,12 @@ #include +#include +#include +#include +#include +#include + #include #include #include @@ -48,12 +59,12 @@ #include "actor.hpp" #include "convert.hpp" #include "trace.h" +#include "object.hpp" +#include "heightfield.hpp" namespace MWPhysics { - static const float sMaxSlope = 49.0f; - static const float sStepSizeUp = 34.0f; static const float sStepSizeDown = 62.0f; static const float sMinStep = 10.f; static const float sGroundOffset = 1.0f; @@ -507,175 +518,6 @@ namespace MWPhysics // --------------------------------------------------------------- - class HeightField - { - public: - HeightField(const float* heights, int x, int y, float triSize, float sqrtVerts, float minH, float maxH, const osg::Object* holdObject) - { - mShape = new btHeightfieldTerrainShape( - sqrtVerts, sqrtVerts, heights, 1, - minH, maxH, 2, - PHY_FLOAT, false - ); - mShape->setUseDiamondSubdivision(true); - mShape->setLocalScaling(btVector3(triSize, triSize, 1)); - - btTransform transform(btQuaternion::getIdentity(), - btVector3((x+0.5f) * triSize * (sqrtVerts-1), - (y+0.5f) * triSize * (sqrtVerts-1), - (maxH+minH)*0.5f)); - - mCollisionObject = new btCollisionObject; - mCollisionObject->setCollisionShape(mShape); - mCollisionObject->setWorldTransform(transform); - - mHoldObject = holdObject; - } - ~HeightField() - { - delete mCollisionObject; - delete mShape; - } - btCollisionObject* getCollisionObject() - { - return mCollisionObject; - } - - private: - btHeightfieldTerrainShape* mShape; - btCollisionObject* mCollisionObject; - osg::ref_ptr mHoldObject; - - void operator=(const HeightField&); - HeightField(const HeightField&); - }; - - // -------------------------------------------------------------- - - class Object : public PtrHolder - { - public: - Object(const MWWorld::Ptr& ptr, osg::ref_ptr shapeInstance) - : mShapeInstance(shapeInstance) - , mSolid(true) - { - mPtr = ptr; - - mCollisionObject.reset(new btCollisionObject); - mCollisionObject->setCollisionShape(shapeInstance->getCollisionShape()); - - mCollisionObject->setUserPointer(static_cast(this)); - - setScale(ptr.getCellRef().getScale()); - setRotation(toBullet(ptr.getRefData().getBaseNode()->getAttitude())); - const float* pos = ptr.getRefData().getPosition().pos; - setOrigin(btVector3(pos[0], pos[1], pos[2])); - } - - const Resource::BulletShapeInstance* getShapeInstance() const - { - return mShapeInstance.get(); - } - - void setScale(float scale) - { - mShapeInstance->getCollisionShape()->setLocalScaling(btVector3(scale,scale,scale)); - } - - void setRotation(const btQuaternion& quat) - { - mCollisionObject->getWorldTransform().setRotation(quat); - } - - void setOrigin(const btVector3& vec) - { - mCollisionObject->getWorldTransform().setOrigin(vec); - } - - btCollisionObject* getCollisionObject() - { - return mCollisionObject.get(); - } - - const btCollisionObject* getCollisionObject() const - { - return mCollisionObject.get(); - } - - /// Return solid flag. Not used by the object itself, true by default. - bool isSolid() const - { - return mSolid; - } - - void setSolid(bool solid) - { - mSolid = solid; - } - - bool isAnimated() const - { - return !mShapeInstance->mAnimatedShapes.empty(); - } - - void animateCollisionShapes(btCollisionWorld* collisionWorld) - { - if (mShapeInstance->mAnimatedShapes.empty()) - return; - - assert (mShapeInstance->getCollisionShape()->isCompound()); - - btCompoundShape* compound = static_cast(mShapeInstance->getCollisionShape()); - for (std::map::const_iterator it = mShapeInstance->mAnimatedShapes.begin(); it != mShapeInstance->mAnimatedShapes.end(); ++it) - { - int recIndex = it->first; - int shapeIndex = it->second; - - std::map::iterator nodePathFound = mRecIndexToNodePath.find(recIndex); - if (nodePathFound == mRecIndexToNodePath.end()) - { - NifOsg::FindGroupByRecIndex visitor(recIndex); - mPtr.getRefData().getBaseNode()->accept(visitor); - if (!visitor.mFound) - { - Log(Debug::Warning) << "Warning: animateCollisionShapes can't find node " << recIndex << " for " << mPtr.getCellRef().getRefId(); - - // Remove nonexistent nodes from animated shapes map and early out - mShapeInstance->mAnimatedShapes.erase(recIndex); - return; - } - osg::NodePath nodePath = visitor.mFoundPath; - nodePath.erase(nodePath.begin()); - nodePathFound = mRecIndexToNodePath.insert(std::make_pair(recIndex, nodePath)).first; - } - - osg::NodePath& nodePath = nodePathFound->second; - osg::Matrixf matrix = osg::computeLocalToWorld(nodePath); - matrix.orthoNormalize(matrix); - - btTransform transform; - transform.setOrigin(toBullet(matrix.getTrans()) * compound->getLocalScaling()); - for (int i=0; i<3; ++i) - for (int j=0; j<3; ++j) - transform.getBasis()[i][j] = matrix(j,i); // NB column/row major difference - - // Note: we can not apply scaling here for now since we treat scaled shapes - // as new shapes (btScaledBvhTriangleMeshShape) with 1.0 scale for now - if (!(transform == compound->getChildTransform(shapeIndex))) - compound->updateChildTransform(shapeIndex, transform); - } - - collisionWorld->updateSingleAabb(mCollisionObject.get()); - } - - private: - std::unique_ptr mCollisionObject; - osg::ref_ptr mShapeInstance; - std::map mRecIndexToNodePath; - bool mSolid; - }; - - // --------------------------------------------------------------- PhysicsSystem::PhysicsSystem(Resource::ResourceSystem* resourceSystem, osg::ref_ptr parentNode) : mShapeManager(new Resource::BulletShapeManager(resourceSystem->getVFS(), resourceSystem->getSceneManager(), resourceSystem->getNifFileManager())) @@ -1193,6 +1035,14 @@ namespace MWPhysics } } + const HeightField* PhysicsSystem::getHeightField(int x, int y) const + { + const auto heightField = mHeightFields.find(std::make_pair(x, y)); + if (heightField == mHeightFields.end()) + return nullptr; + return heightField->second; + } + void PhysicsSystem::addObject (const MWWorld::Ptr& ptr, const std::string& mesh, int collisionType) { osg::ref_ptr shapeInstance = mShapeManager->getInstance(mesh); diff --git a/apps/openmw/mwphysics/physicssystem.hpp b/apps/openmw/mwphysics/physicssystem.hpp index c72188d71..25091d482 100644 --- a/apps/openmw/mwphysics/physicssystem.hpp +++ b/apps/openmw/mwphysics/physicssystem.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -49,6 +50,9 @@ namespace MWPhysics class Object; class Actor; + static const float sMaxSlope = 49.0f; + static const float sStepSizeUp = 34.0f; + class PhysicsSystem { public: @@ -85,6 +89,8 @@ namespace MWPhysics void removeHeightField (int x, int y); + const HeightField* getHeightField(int x, int y) const; + bool toggleCollisionMode(); bool isActorCollisionEnabled(const MWWorld::Ptr& ptr); void setActorCollisionMode(const MWWorld::Ptr& ptr, bool enabled); @@ -182,6 +188,12 @@ namespace MWPhysics End of tes3mp addition */ + template + void forEachAnimatedObject(Function&& function) const + { + std::for_each(mAnimatedObjects.begin(), mAnimatedObjects.end(), function); + } + private: void updateWater(); diff --git a/apps/openmw/mwphysics/ptrholder.hpp b/apps/openmw/mwphysics/ptrholder.hpp new file mode 100644 index 000000000..f8188b43e --- /dev/null +++ b/apps/openmw/mwphysics/ptrholder.hpp @@ -0,0 +1,33 @@ +#ifndef OPENMW_MWPHYSICS_PTRHOLDER_H +#define OPENMW_MWPHYSICS_PTRHOLDER_H + +#include "../mwworld/ptr.hpp" + +namespace MWPhysics +{ + class PtrHolder + { + public: + virtual ~PtrHolder() {} + + void updatePtr(const MWWorld::Ptr& updated) + { + mPtr = updated; + } + + MWWorld::Ptr getPtr() + { + return mPtr; + } + + MWWorld::ConstPtr getPtr() const + { + return mPtr; + } + + protected: + MWWorld::Ptr mPtr; + }; +} + +#endif diff --git a/apps/openmw/mwrender/actorspaths.cpp b/apps/openmw/mwrender/actorspaths.cpp new file mode 100644 index 000000000..35b255355 --- /dev/null +++ b/apps/openmw/mwrender/actorspaths.cpp @@ -0,0 +1,99 @@ +#include "actorspaths.hpp" +#include "vismask.hpp" + +#include + +#include + +namespace MWRender +{ + ActorsPaths::ActorsPaths(const osg::ref_ptr& root, bool enabled) + : mRootNode(root) + , mEnabled(enabled) + { + } + + ActorsPaths::~ActorsPaths() + { + if (mEnabled) + disable(); + } + + bool ActorsPaths::toggle() + { + if (mEnabled) + disable(); + else + enable(); + + return mEnabled; + } + + void ActorsPaths::update(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end, + const DetourNavigator::Settings& settings) + { + if (!mEnabled) + return; + + const auto group = mGroups.find(actor); + if (group != mGroups.end()) + mRootNode->removeChild(group->second); + + const auto newGroup = SceneUtil::createAgentPathGroup(path, halfExtents, start, end, settings); + if (newGroup) + { + newGroup->setNodeMask(Mask_Debug); + mRootNode->addChild(newGroup); + mGroups[actor] = newGroup; + } + } + + void ActorsPaths::remove(const MWWorld::ConstPtr& actor) + { + const auto group = mGroups.find(actor); + if (group != mGroups.end()) + { + mRootNode->removeChild(group->second); + mGroups.erase(group); + } + } + + void ActorsPaths::removeCell(const MWWorld::CellStore* const store) + { + for (auto it = mGroups.begin(); it != mGroups.end(); ) + { + if (it->first.getCell() == store) + { + mRootNode->removeChild(it->second); + it = mGroups.erase(it); + } + else + ++it; + } + } + + void ActorsPaths::updatePtr(const MWWorld::ConstPtr& old, const MWWorld::ConstPtr& updated) + { + const auto it = mGroups.find(old); + if (it == mGroups.end()) + return; + auto group = std::move(it->second); + mGroups.erase(it); + mGroups.insert(std::make_pair(updated, std::move(group))); + } + + void ActorsPaths::enable() + { + std::for_each(mGroups.begin(), mGroups.end(), + [&] (const Groups::value_type& v) { mRootNode->addChild(v.second); }); + mEnabled = true; + } + + void ActorsPaths::disable() + { + std::for_each(mGroups.begin(), mGroups.end(), + [&] (const Groups::value_type& v) { mRootNode->removeChild(v.second); }); + mEnabled = false; + } +} diff --git a/apps/openmw/mwrender/actorspaths.hpp b/apps/openmw/mwrender/actorspaths.hpp new file mode 100644 index 000000000..1f61834d4 --- /dev/null +++ b/apps/openmw/mwrender/actorspaths.hpp @@ -0,0 +1,51 @@ +#ifndef OPENMW_MWRENDER_AGENTSPATHS_H +#define OPENMW_MWRENDER_AGENTSPATHS_H + +#include + +#include + +#include + +#include +#include + +namespace osg +{ + class Group; +} + +namespace MWRender +{ + class ActorsPaths + { + public: + ActorsPaths(const osg::ref_ptr& root, bool enabled); + ~ActorsPaths(); + + bool toggle(); + + void update(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end, + const DetourNavigator::Settings& settings); + + void remove(const MWWorld::ConstPtr& actor); + + void removeCell(const MWWorld::CellStore* const store); + + void updatePtr(const MWWorld::ConstPtr& old, const MWWorld::ConstPtr& updated); + + void enable(); + + void disable(); + + private: + using Groups = std::map>; + + osg::ref_ptr mRootNode; + Groups mGroups; + bool mEnabled; + }; +} + +#endif diff --git a/apps/openmw/mwrender/navmesh.cpp b/apps/openmw/mwrender/navmesh.cpp new file mode 100644 index 000000000..331f506ab --- /dev/null +++ b/apps/openmw/mwrender/navmesh.cpp @@ -0,0 +1,71 @@ +#include "navmesh.hpp" +#include "vismask.hpp" + +#include + +#include + +namespace MWRender +{ + NavMesh::NavMesh(const osg::ref_ptr& root, bool enabled) + : mRootNode(root) + , mEnabled(enabled) + , mGeneration(0) + , mRevision(0) + { + } + + NavMesh::~NavMesh() + { + if (mEnabled) + disable(); + } + + bool NavMesh::toggle() + { + if (mEnabled) + disable(); + else + enable(); + + return mEnabled; + } + + void NavMesh::update(const dtNavMesh& navMesh, const std::size_t id, + const std::size_t generation, const std::size_t revision, const DetourNavigator::Settings& settings) + { + if (!mEnabled || (mId == id && mGeneration >= generation && mRevision >= revision)) + return; + + mId = id; + mGeneration = generation; + mRevision = revision; + if (mGroup) + mRootNode->removeChild(mGroup); + mGroup = SceneUtil::createNavMeshGroup(navMesh, settings); + if (mGroup) + { + mGroup->setNodeMask(Mask_Debug); + mRootNode->addChild(mGroup); + } + } + + void NavMesh::reset() + { + if (mGroup) + mRootNode->removeChild(mGroup); + } + + void NavMesh::enable() + { + if (mGroup) + mRootNode->addChild(mGroup); + mEnabled = true; + } + + void NavMesh::disable() + { + reset(); + mEnabled = false; + } +} diff --git a/apps/openmw/mwrender/navmesh.hpp b/apps/openmw/mwrender/navmesh.hpp new file mode 100644 index 000000000..29205ca27 --- /dev/null +++ b/apps/openmw/mwrender/navmesh.hpp @@ -0,0 +1,43 @@ +#ifndef OPENMW_MWRENDER_NAVMESH_H +#define OPENMW_MWRENDER_NAVMESH_H + +#include + +#include + +namespace osg +{ + class Group; + class Geometry; +} + +namespace MWRender +{ + class NavMesh + { + public: + NavMesh(const osg::ref_ptr& root, bool enabled); + ~NavMesh(); + + bool toggle(); + + void update(const dtNavMesh& navMesh, const std::size_t number, const std::size_t generation, + const std::size_t revision, const DetourNavigator::Settings& settings); + + void reset(); + + void enable(); + + void disable(); + + private: + osg::ref_ptr mRootNode; + bool mEnabled; + std::size_t mId = std::numeric_limits::max(); + std::size_t mGeneration; + std::size_t mRevision; + osg::ref_ptr mGroup; + }; +} + +#endif diff --git a/apps/openmw/mwrender/renderingmanager.cpp b/apps/openmw/mwrender/renderingmanager.cpp index 476beb990..4d79fd3de 100644 --- a/apps/openmw/mwrender/renderingmanager.cpp +++ b/apps/openmw/mwrender/renderingmanager.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include #include "../mwworld/cellstore.hpp" @@ -63,6 +65,8 @@ #include "water.hpp" #include "terrainstorage.hpp" #include "util.hpp" +#include "navmesh.hpp" +#include "actorspaths.hpp" namespace { @@ -189,13 +193,16 @@ namespace MWRender Resource::ResourceSystem* mResourceSystem; }; - RenderingManager::RenderingManager(osgViewer::Viewer* viewer, osg::ref_ptr rootNode, Resource::ResourceSystem* resourceSystem, SceneUtil::WorkQueue* workQueue, - const Fallback::Map* fallback, const std::string& resourcePath) + RenderingManager::RenderingManager(osgViewer::Viewer* viewer, osg::ref_ptr rootNode, + Resource::ResourceSystem* resourceSystem, SceneUtil::WorkQueue* workQueue, + const Fallback::Map* fallback, const std::string& resourcePath, + DetourNavigator::Navigator& navigator) : mViewer(viewer) , mRootNode(rootNode) , mResourceSystem(resourceSystem) , mWorkQueue(workQueue) , mUnrefQueue(new SceneUtil::UnrefQueue) + , mNavigator(navigator) , mLandFogStart(0.f) , mLandFogEnd(std::numeric_limits::max()) , mUnderwaterFogStart(0.f) @@ -228,6 +235,8 @@ namespace MWRender mRootNode->addChild(mSceneRoot); + mNavMesh.reset(new NavMesh(mRootNode, Settings::Manager::getBool("enable nav mesh render", "Navigator"))); + mActorsPaths.reset(new ActorsPaths(mRootNode, Settings::Manager::getBool("enable agents paths render", "Navigator"))); mPathgrid.reset(new Pathgrid(mRootNode)); mObjects.reset(new Objects(mResourceSystem, sceneRoot, mUnrefQueue.get())); @@ -465,6 +474,7 @@ namespace MWRender void RenderingManager::removeCell(const MWWorld::CellStore *store) { mPathgrid->removeCell(store); + mActorsPaths->removeCell(store); mObjects->removeCell(store); if (store->getCell()->isExterior()) @@ -482,7 +492,7 @@ namespace MWRender { mSky->setEnabled(enabled); } - + bool RenderingManager::toggleBorders() { mBorders = !mBorders; @@ -516,6 +526,14 @@ namespace MWRender mViewer->getCamera()->setCullMask(mask); return enabled; } + else if (mode == Render_NavMesh) + { + return mNavMesh->toggle(); + } + else if (mode == Render_ActorsPaths) + { + return mActorsPaths->toggle(); + } return false; } @@ -581,6 +599,28 @@ namespace MWRender mWater->update(dt); } + const auto navMeshes = mNavigator.getNavMeshes(); + + auto it = navMeshes.begin(); + for (std::size_t i = 0; it != navMeshes.end() && i < mNavMeshNumber; ++i) + ++it; + if (it == navMeshes.end()) + { + mNavMesh->reset(); + } + else + { + try + { + const auto locked = it->second.lockConst(); + mNavMesh->update(locked->getValue(), mNavMeshNumber, locked->getGeneration(), + locked->getNavMeshRevision(), mNavigator.getSettings()); + } + catch (const std::exception& e) + { + Log(Debug::Error) << "NavMesh render update exception: " << e.what(); + } + } mCamera->update(dt, paused); osg::Vec3f focal, cameraPos; @@ -642,6 +682,7 @@ namespace MWRender void RenderingManager::removeObject(const MWWorld::Ptr &ptr) { + mActorsPaths->remove(ptr); mObjects->removeObject(ptr); mWater->removeEmitter(ptr); } @@ -749,8 +790,8 @@ namespace MWRender osg::Vec3 directions[6] = { rawCubemap ? osg::Vec3(1,0,0) : osg::Vec3(0,0,1), - osg::Vec3(0,0,-1), - osg::Vec3(-1,0,0), + osg::Vec3(0,0,-1), + osg::Vec3(-1,0,0), rawCubemap ? osg::Vec3(0,0,1) : osg::Vec3(1,0,0), osg::Vec3(0,1,0), osg::Vec3(0,-1,0)}; @@ -789,7 +830,7 @@ namespace MWRender mFieldOfView = fovBackup; if (rawCubemap) // for raw cubemap don't run on GPU, just merge the images - { + { image->allocateImage(cubeSize * 6,cubeSize,images[0]->r(),images[0]->getPixelFormat(),images[0]->getDataType()); for (int i = 0; i < 6; ++i) @@ -797,7 +838,7 @@ namespace MWRender return true; } - + // run on GPU now: osg::ref_ptr cubeTexture (new osg::TextureCubeMap); @@ -1025,6 +1066,7 @@ namespace MWRender void RenderingManager::updatePtr(const MWWorld::Ptr &old, const MWWorld::Ptr &updated) { mObjects->updatePtr(old, updated); + mActorsPaths->updatePtr(old, updated); } void RenderingManager::spawnEffect(const std::string &model, const std::string &texture, const osg::Vec3f &worldPosition, float scale, bool isMagicVFX) @@ -1345,5 +1387,19 @@ namespace MWRender return mTerrainStorage->getLandManager(); } + void RenderingManager::updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const + { + mActorsPaths->update(actor, path, halfExtents, start, end, mNavigator.getSettings()); + } + + void RenderingManager::removeActorPath(const MWWorld::ConstPtr& actor) const + { + mActorsPaths->remove(actor); + } + void RenderingManager::setNavMeshNumber(const std::size_t value) + { + mNavMeshNumber = value; + } } diff --git a/apps/openmw/mwrender/renderingmanager.hpp b/apps/openmw/mwrender/renderingmanager.hpp index b4473c3b4..7a4500ac9 100644 --- a/apps/openmw/mwrender/renderingmanager.hpp +++ b/apps/openmw/mwrender/renderingmanager.hpp @@ -12,6 +12,8 @@ #include "renderinginterface.hpp" #include "rendermode.hpp" +#include + namespace osg { class Group; @@ -55,6 +57,12 @@ namespace SceneUtil class UnrefQueue; } +namespace DetourNavigator +{ + class Navigator; + struct Settings; +} + namespace MWRender { @@ -68,12 +76,16 @@ namespace MWRender class Water; class TerrainStorage; class LandManager; + class NavMesh; + class ActorsPaths; class RenderingManager : public MWRender::RenderingInterface { public: - RenderingManager(osgViewer::Viewer* viewer, osg::ref_ptr rootNode, Resource::ResourceSystem* resourceSystem, SceneUtil::WorkQueue* workQueue, - const Fallback::Map* fallback, const std::string& resourcePath); + RenderingManager(osgViewer::Viewer* viewer, osg::ref_ptr rootNode, + Resource::ResourceSystem* resourceSystem, SceneUtil::WorkQueue* workQueue, + const Fallback::Map* fallback, const std::string& resourcePath, + DetourNavigator::Navigator& navigator); ~RenderingManager(); MWRender::Objects& getObjects(); @@ -211,6 +223,13 @@ namespace MWRender bool toggleBorders(); + void updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const; + + void removeActorPath(const MWWorld::ConstPtr& actor) const; + + void setNavMeshNumber(const std::size_t value); + private: void updateProjectionMatrix(); void updateTextureFiltering(); @@ -235,6 +254,10 @@ namespace MWRender osg::ref_ptr mSunLight; + DetourNavigator::Navigator& mNavigator; + std::unique_ptr mNavMesh; + std::size_t mNavMeshNumber = 0; + std::unique_ptr mActorsPaths; std::unique_ptr mPathgrid; std::unique_ptr mObjects; std::unique_ptr mWater; diff --git a/apps/openmw/mwrender/rendermode.hpp b/apps/openmw/mwrender/rendermode.hpp index 2847888d1..077710f4f 100644 --- a/apps/openmw/mwrender/rendermode.hpp +++ b/apps/openmw/mwrender/rendermode.hpp @@ -10,7 +10,9 @@ namespace MWRender Render_Wireframe, Render_Pathgrid, Render_Water, - Render_Scene + Render_Scene, + Render_NavMesh, + Render_ActorsPaths, }; } diff --git a/apps/openmw/mwscript/docs/vmformat.txt b/apps/openmw/mwscript/docs/vmformat.txt index fed780be7..d346ab6e4 100644 --- a/apps/openmw/mwscript/docs/vmformat.txt +++ b/apps/openmw/mwscript/docs/vmformat.txt @@ -455,5 +455,8 @@ op 0x2000304: Show op 0x2000305: Show, explicit op 0x2000306: OnActivate, explicit op 0x2000307: ToggleBorders, tb +op 0x2000308: ToggleNavMesh +op 0x2000309: ToggleActorsPaths +op 0x200030a: SetNavMeshNumber -opcodes 0x2000308-0x3ffffff unused +opcodes 0x200030b-0x3ffffff unused diff --git a/apps/openmw/mwscript/miscextensions.cpp b/apps/openmw/mwscript/miscextensions.cpp index 2a3efad3d..6d0c75a36 100644 --- a/apps/openmw/mwscript/miscextensions.cpp +++ b/apps/openmw/mwscript/miscextensions.cpp @@ -1436,6 +1436,53 @@ namespace MWScript } }; + class OpToggleNavMesh : public Interpreter::Opcode0 + { + public: + + virtual void execute (Interpreter::Runtime& runtime) + { + bool enabled = + MWBase::Environment::get().getWorld()->toggleRenderMode (MWRender::Render_NavMesh); + + runtime.getContext().report (enabled ? + "Navigation Mesh Rendering -> On" : "Navigation Mesh Rendering -> Off"); + } + }; + + class OpToggleActorsPaths : public Interpreter::Opcode0 + { + public: + + virtual void execute (Interpreter::Runtime& runtime) + { + bool enabled = + MWBase::Environment::get().getWorld()->toggleRenderMode (MWRender::Render_ActorsPaths); + + runtime.getContext().report (enabled ? + "Agents Paths Rendering -> On" : "Agents Paths Rendering -> Off"); + } + }; + + class OpSetNavMeshNumberToRender : public Interpreter::Opcode0 + { + public: + + virtual void execute (Interpreter::Runtime& runtime) + { + const auto navMeshNumber = runtime[0].mInteger; + runtime.pop(); + + if (navMeshNumber < 0) + { + runtime.getContext().report("Invalid navmesh number: use not less than zero values"); + return; + } + + MWBase::Environment::get().getWorld()->setNavMeshNumberToRender(static_cast(navMeshNumber)); + } + }; + void installOpcodes (Interpreter::Interpreter& interpreter) { interpreter.installSegment5 (Compiler::Misc::opcodeXBox, new OpXBox); @@ -1536,6 +1583,9 @@ namespace MWScript interpreter.installSegment3 (Compiler::Misc::opcodeShowSceneGraph, new OpShowSceneGraph); interpreter.installSegment3 (Compiler::Misc::opcodeShowSceneGraphExplicit, new OpShowSceneGraph); interpreter.installSegment5 (Compiler::Misc::opcodeToggleBorders, new OpToggleBorders); + interpreter.installSegment5 (Compiler::Misc::opcodeToggleNavMesh, new OpToggleNavMesh); + interpreter.installSegment5 (Compiler::Misc::opcodeToggleActorsPaths, new OpToggleActorsPaths); + interpreter.installSegment5 (Compiler::Misc::opcodeSetNavMeshNumberToRender, new OpSetNavMeshNumberToRender); } } } diff --git a/apps/openmw/mwworld/cellstore.cpp b/apps/openmw/mwworld/cellstore.cpp index 5a05465d4..6d3608989 100644 --- a/apps/openmw/mwworld/cellstore.cpp +++ b/apps/openmw/mwworld/cellstore.cpp @@ -618,7 +618,7 @@ namespace MWWorld mHasState = true; } - int CellStore::count() const + std::size_t CellStore::count() const { return mMergedRefs.size(); } diff --git a/apps/openmw/mwworld/cellstore.hpp b/apps/openmw/mwworld/cellstore.hpp index ac68924df..e5df8fb5c 100644 --- a/apps/openmw/mwworld/cellstore.hpp +++ b/apps/openmw/mwworld/cellstore.hpp @@ -300,7 +300,7 @@ namespace MWWorld ESM::FogState* getFog () const; - int count() const; + std::size_t count() const; ///< Return total number of references, including deleted ones. void load (); @@ -343,7 +343,7 @@ namespace MWWorld /// \attention This function also lists deleted (count 0) objects! /// \return Iteration completed? template - bool forEachConst (Visitor& visitor) const + bool forEachConst (Visitor&& visitor) const { if (mState != State_Loaded) return false; diff --git a/apps/openmw/mwworld/scene.cpp b/apps/openmw/mwworld/scene.cpp index ffd42d9ad..d6c02d6c0 100644 --- a/apps/openmw/mwworld/scene.cpp +++ b/apps/openmw/mwworld/scene.cpp @@ -2,6 +2,20 @@ #include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + /* Start of tes3mp addition @@ -13,13 +27,6 @@ End of tes3mp addition */ -#include -#include -#include -#include -#include -#include - #include "../mwbase/environment.hpp" #include "../mwbase/world.hpp" #include "../mwbase/soundmanager.hpp" @@ -30,6 +37,10 @@ #include "../mwrender/landmanager.hpp" #include "../mwphysics/physicssystem.hpp" +#include "../mwphysics/actor.hpp" +#include "../mwphysics/object.hpp" +#include "../mwphysics/heightfield.hpp" +#include "../mwphysics/convert.hpp" #include "player.hpp" #include "localscripts.hpp" @@ -41,25 +52,45 @@ namespace { + osg::Quat makeActorOsgQuat(const ESM::Position& position) + { + return osg::Quat(position.rot[2], osg::Vec3(0, 0, -1)); + } - void setNodeRotation(const MWWorld::Ptr& ptr, MWRender::RenderingManager& rendering, bool inverseRotationOrder) + osg::Quat makeInversedOrderObjectOsgQuat(const ESM::Position& position) + { + const float xr = position.rot[0]; + const float yr = position.rot[1]; + const float zr = position.rot[2]; + + return osg::Quat(xr, osg::Vec3(-1, 0, 0)) + * osg::Quat(yr, osg::Vec3(0, -1, 0)) + * osg::Quat(zr, osg::Vec3(0, 0, -1)); + } + + osg::Quat makeObjectOsgQuat(const ESM::Position& position) + { + const float xr = position.rot[0]; + const float yr = position.rot[1]; + const float zr = position.rot[2]; + + return osg::Quat(zr, osg::Vec3(0, 0, -1)) + * osg::Quat(yr, osg::Vec3(0, -1, 0)) + * osg::Quat(xr, osg::Vec3(-1, 0, 0)); + } + + void setNodeRotation(const MWWorld::Ptr& ptr, MWRender::RenderingManager& rendering, const bool inverseRotationOrder) { if (!ptr.getRefData().getBaseNode()) return; - osg::Quat worldRotQuat(ptr.getRefData().getPosition().rot[2], osg::Vec3(0,0,-1)); - if (!ptr.getClass().isActor()) - { - float xr = ptr.getRefData().getPosition().rot[0]; - float yr = ptr.getRefData().getPosition().rot[1]; - if (!inverseRotationOrder) - worldRotQuat = worldRotQuat * osg::Quat(yr, osg::Vec3(0,-1,0)) * - osg::Quat(xr, osg::Vec3(-1,0,0)); - else - worldRotQuat = osg::Quat(xr, osg::Vec3(-1,0,0)) * osg::Quat(yr, osg::Vec3(0,-1,0)) * worldRotQuat; - } - - rendering.rotateObject(ptr, worldRotQuat); + rendering.rotateObject(ptr, + ptr.getClass().isActor() + ? makeActorOsgQuat(ptr.getRefData().getPosition()) + : (inverseRotationOrder + ? makeInversedOrderObjectOsgQuat(ptr.getRefData().getPosition()) + : makeObjectOsgQuat(ptr.getRefData().getPosition())) + ); } void addObject(const MWWorld::Ptr& ptr, MWPhysics::PhysicsSystem& physics, @@ -95,6 +126,73 @@ namespace MWBase::Environment::get().getWorld()->applyLoopingParticles(ptr); } + void addObject(const MWWorld::Ptr& ptr, const MWPhysics::PhysicsSystem& physics, DetourNavigator::Navigator& navigator) + { + if (const auto object = physics.getObject(ptr)) + { + if (ptr.getClass().isDoor() && !ptr.getCellRef().getTeleport()) + { + const auto shape = object->getShapeInstance()->getCollisionShape(); + + btVector3 aabbMin; + btVector3 aabbMax; + shape->getAabb(btTransform::getIdentity(), aabbMin, aabbMax); + + const auto center = (aabbMax + aabbMin) * 0.5f; + + const auto distanceFromDoor = MWBase::Environment::get().getWorld()->getMaxActivationDistance() * 0.5f; + const auto toPoint = aabbMax.x() - aabbMin.x() < aabbMax.y() - aabbMin.y() + ? btVector3(distanceFromDoor, 0, 0) + : btVector3(0, distanceFromDoor, 0); + + const auto& transform = object->getCollisionObject()->getWorldTransform(); + const btTransform closedDoorTransform( + MWPhysics::toBullet(makeObjectOsgQuat(ptr.getCellRef().getPosition())), + transform.getOrigin() + ); + + const auto start = DetourNavigator::makeOsgVec3f(closedDoorTransform(center + toPoint)); + const auto startPoint = physics.castRay(start, start - osg::Vec3f(0, 0, 1000), ptr, {}, + MWPhysics::CollisionType_World | MWPhysics::CollisionType_HeightMap | MWPhysics::CollisionType_Water); + const auto connectionStart = startPoint.mHit ? startPoint.mHitPos : start; + + const auto end = DetourNavigator::makeOsgVec3f(closedDoorTransform(center - toPoint)); + const auto endPoint = physics.castRay(end, end - osg::Vec3f(0, 0, 1000), ptr, {}, + MWPhysics::CollisionType_World | MWPhysics::CollisionType_HeightMap | MWPhysics::CollisionType_Water); + const auto connectionEnd = endPoint.mHit ? endPoint.mHitPos : end; + + navigator.addObject( + DetourNavigator::ObjectId(object), + DetourNavigator::DoorShapes( + *shape, + object->getShapeInstance()->getAvoidCollisionShape(), + connectionStart, + connectionEnd + ), + transform + ); + } + else + { + navigator.addObject( + DetourNavigator::ObjectId(object), + DetourNavigator::ObjectShapes { + *object->getShapeInstance()->getCollisionShape(), + object->getShapeInstance()->getAvoidCollisionShape() + }, + object->getCollisionObject()->getWorldTransform() + ); + } + } + else if (const auto actor = physics.getActor(ptr)) + { + const auto halfExtents = ptr.getCell()->isExterior() + ? physics.getHalfExtents(MWBase::Environment::get().getWorld()->getPlayerPtr()) + : actor->getHalfExtents(); + navigator.addAgent(halfExtents); + } + } + void updateObjectRotation (const MWWorld::Ptr& ptr, MWPhysics::PhysicsSystem& physics, MWRender::RenderingManager& rendering, bool inverseRotationOrder) { @@ -121,24 +219,19 @@ namespace MWWorld::CellStore& mCell; bool mRescale; Loading::Listener& mLoadingListener; - MWPhysics::PhysicsSystem& mPhysics; - MWRender::RenderingManager& mRendering; std::vector mToInsert; - InsertVisitor (MWWorld::CellStore& cell, bool rescale, Loading::Listener& loadingListener, - MWPhysics::PhysicsSystem& physics, MWRender::RenderingManager& rendering); + InsertVisitor (MWWorld::CellStore& cell, bool rescale, Loading::Listener& loadingListener); bool operator() (const MWWorld::Ptr& ptr); - void insert(); + + template + void insert(AddObject&& addObject); }; - InsertVisitor::InsertVisitor (MWWorld::CellStore& cell, bool rescale, - Loading::Listener& loadingListener, MWPhysics::PhysicsSystem& physics, - MWRender::RenderingManager& rendering) - : mCell (cell), mRescale (rescale), mLoadingListener (loadingListener), - mPhysics (physics), - mRendering (rendering) + InsertVisitor::InsertVisitor (MWWorld::CellStore& cell, bool rescale, Loading::Listener& loadingListener) + : mCell (cell), mRescale (rescale), mLoadingListener (loadingListener) {} bool InsertVisitor::operator() (const MWWorld::Ptr& ptr) @@ -149,7 +242,8 @@ namespace return true; } - void InsertVisitor::insert() + template + void InsertVisitor::insert(AddObject&& addObject) { for (std::vector::iterator it = mToInsert.begin(); it != mToInsert.end(); ++it) { @@ -166,7 +260,7 @@ namespace { try { - addObject(ptr, mPhysics, mRendering); + addObject(ptr); } catch (const std::exception& e) { @@ -189,6 +283,11 @@ namespace } }; + int getCellPositionDistanceToOrigin(const std::pair& cellPosition) + { + return std::abs(cellPosition.first) + std::abs(cellPosition.second); + } + } @@ -244,6 +343,9 @@ namespace MWWorld void Scene::unloadCell (CellStoreCollection::iterator iter) { Log(Debug::Info) << "Unloading cell " << (*iter)->getCell()->getDescription(); + DetourNavigator::log("unload cell ", (*iter)->getCell()->getDescription()); + + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); ListAndResetObjectsVisitor visitor; /* @@ -258,12 +360,23 @@ namespace MWWorld */ (*iter)->forEach(visitor); - for (std::vector::const_iterator iter2 (visitor.mObjects.begin()); - iter2!=visitor.mObjects.end(); ++iter2) + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + const auto playerHalfExtents = mPhysics->getHalfExtents(player); + for (const auto& ptr : visitor.mObjects) { - mPhysics->remove(*iter2); + if (const auto object = mPhysics->getObject(ptr)) + navigator->removeObject(DetourNavigator::ObjectId(object)); + else if (const auto actor = mPhysics->getActor(ptr)) + { + navigator->removeAgent(ptr.getCell()->isExterior() ? playerHalfExtents : actor->getHalfExtents()); + mRendering.removeActorPath(ptr); + } + mPhysics->remove(ptr); } + const auto cellX = (*iter)->getCell()->getGridX(); + const auto cellY = (*iter)->getCell()->getGridY(); + if ((*iter)->getCell()->isExterior()) { const ESM::Land* land = @@ -272,9 +385,18 @@ namespace MWWorld (*iter)->getCell()->getGridY() ); if (land && land->mDataTypes&ESM::Land::DATA_VHGT) - mPhysics->removeHeightField ((*iter)->getCell()->getGridX(), (*iter)->getCell()->getGridY()); + { + if (const auto heightField = mPhysics->getHeightField(cellX, cellY)) + navigator->removeObject(DetourNavigator::ObjectId(heightField)); + mPhysics->removeHeightField(cellX, cellY); + } } + if ((*iter)->getCell()->hasWater()) + navigator->removeWater(osg::Vec2i(cellX, cellY)); + + navigator->update(player.getRefData().getPosition().asVec3()); + MWBase::Environment::get().getMechanicsManager()->drop (*iter); mRendering.removeCell(*iter); @@ -303,20 +425,24 @@ namespace MWWorld if(result.second) { Log(Debug::Info) << "Loading cell " << cell->getCell()->getDescription(); + DetourNavigator::log("load cell ", cell->getCell()->getDescription()); float verts = ESM::Land::LAND_SIZE; float worldsize = ESM::Land::REAL_SIZE; + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + + const int cellX = cell->getCell()->getGridX(); + const int cellY = cell->getCell()->getGridY(); + // Load terrain physics first... if (cell->getCell()->isExterior()) { - int cellX = cell->getCell()->getGridX(); - int cellY = cell->getCell()->getGridY(); osg::ref_ptr land = mRendering.getLandManager()->getLand(cellX, cellY); const ESM::Land::LandData* data = land ? land->getData(ESM::Land::DATA_VHGT) : 0; if (data) { - mPhysics->addHeightField (data->mHeights, cellX, cell->getCell()->getGridY(), worldsize / (verts-1), verts, data->mMinHeight, data->mMaxHeight, land.get()); + mPhysics->addHeightField (data->mHeights, cellX, cellY, worldsize / (verts-1), verts, data->mMinHeight, data->mMaxHeight, land.get()); } else { @@ -324,6 +450,10 @@ namespace MWWorld defaultHeight.resize(verts*verts, ESM::Land::DEFAULT_HEIGHT); mPhysics->addHeightField (&defaultHeight[0], cell->getCell()->getGridX(), cell->getCell()->getGridY(), worldsize / (verts-1), verts, ESM::Land::DEFAULT_HEIGHT, ESM::Land::DEFAULT_HEIGHT, land.get()); } + + if (const auto heightField = mPhysics->getHeightField(cellX, cellY)) + navigator->addObject(DetourNavigator::ObjectId(heightField), *heightField->getShape(), + heightField->getCollisionObject()->getWorldTransform()); } // register local scripts @@ -356,10 +486,25 @@ namespace MWWorld { mPhysics->enableWater(waterLevel); mRendering.setWaterHeight(waterLevel); + + if (cell->getCell()->isExterior()) + { + if (const auto heightField = mPhysics->getHeightField(cellX, cellY)) + navigator->addWater(osg::Vec2i(cellX, cellY), ESM::Land::REAL_SIZE, + cell->getWaterLevel(), heightField->getCollisionObject()->getWorldTransform()); + } + else + { + navigator->addWater(osg::Vec2i(cellX, cellY), std::numeric_limits::max(), + cell->getWaterLevel(), btTransform::getIdentity()); + } } else mPhysics->disableWater(); + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + navigator->update(player.getRefData().getPosition().asVec3()); + if (!cell->isExterior() && !(cell->getCell()->mData.mFlags & ESM::Cell::QuasiEx)) mRendering.configureAmbient(cell->getCell()); @@ -390,6 +535,10 @@ namespace MWWorld void Scene::playerMoved(const osg::Vec3f &pos) { + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + navigator->update(player.getRefData().getPosition().asVec3()); + if (!mCurrentCell || !mCurrentCell->isExterior()) return; @@ -408,7 +557,7 @@ namespace MWWorld } } - void Scene::changeCellGrid (int X, int Y, bool changeEvent) + void Scene::changeCellGrid (int playerCellX, int playerCellY, bool changeEvent) { Loading::Listener* loadingListener = MWBase::Environment::get().getWindowManager()->getLoadingScreen(); Loading::ScopedLoad load(loadingListener); @@ -422,8 +571,8 @@ namespace MWWorld { if ((*active)->getCell()->isExterior()) { - if (std::abs (X-(*active)->getCell()->getGridX())<=mHalfGridSize && - std::abs (Y-(*active)->getCell()->getGridY())<=mHalfGridSize) + if (std::abs (playerCellX-(*active)->getCell()->getGridX())<=mHalfGridSize && + std::abs (playerCellY-(*active)->getCell()->getGridY())<=mHalfGridSize) { // keep cells within the new grid ++active; @@ -433,11 +582,12 @@ namespace MWWorld unloadCell (active++); } - int refsToLoad = 0; + std::size_t refsToLoad = 0; + std::vector> cellsPositionsToLoad; // get the number of refs to load - for (int x=X-mHalfGridSize; x<=X+mHalfGridSize; ++x) + for (int x = playerCellX - mHalfGridSize; x <= playerCellX + mHalfGridSize; ++x) { - for (int y=Y-mHalfGridSize; y<=Y+mHalfGridSize; ++y) + for (int y = playerCellY - mHalfGridSize; y <= playerCellY + mHalfGridSize; ++y) { CellStoreCollection::iterator iter = mActiveCells.begin(); @@ -453,36 +603,54 @@ namespace MWWorld } if (iter==mActiveCells.end()) + { refsToLoad += MWBase::Environment::get().getWorld()->getExterior(x, y)->count(); + cellsPositionsToLoad.push_back(std::make_pair(x, y)); + } } } loadingListener->setProgressRange(refsToLoad); + const auto getDistanceToPlayerCell = [&] (const std::pair& cellPosition) + { + return std::abs(cellPosition.first - playerCellX) + std::abs(cellPosition.second - playerCellY); + }; + + const auto getCellPositionPriority = [&] (const std::pair& cellPosition) + { + return std::make_pair(getDistanceToPlayerCell(cellPosition), getCellPositionDistanceToOrigin(cellPosition)); + }; + + std::sort(cellsPositionsToLoad.begin(), cellsPositionsToLoad.end(), + [&] (const std::pair& lhs, const std::pair& rhs) { + return getCellPositionPriority(lhs) < getCellPositionPriority(rhs); + }); + // Load cells - for (int x=X-mHalfGridSize; x<=X+mHalfGridSize; ++x) + for (const auto& cellPosition : cellsPositionsToLoad) { - for (int y=Y-mHalfGridSize; y<=Y+mHalfGridSize; ++y) - { - CellStoreCollection::iterator iter = mActiveCells.begin(); + const auto x = cellPosition.first; + const auto y = cellPosition.second; - while (iter!=mActiveCells.end()) - { - assert ((*iter)->getCell()->isExterior()); + CellStoreCollection::iterator iter = mActiveCells.begin(); - if (x==(*iter)->getCell()->getGridX() && - y==(*iter)->getCell()->getGridY()) - break; + while (iter != mActiveCells.end()) + { + assert ((*iter)->getCell()->isExterior()); - ++iter; - } + if (x == (*iter)->getCell()->getGridX() && + y == (*iter)->getCell()->getGridY()) + break; - if (iter==mActiveCells.end()) - { - CellStore *cell = MWBase::Environment::get().getWorld()->getExterior(x, y); + ++iter; + } - loadCell (cell, loadingListener, changeEvent); - } + if (iter == mActiveCells.end()) + { + CellStore *cell = MWBase::Environment::get().getWorld()->getExterior(x, y); + + loadCell (cell, loadingListener, changeEvent); } } @@ -501,7 +669,7 @@ namespace MWWorld End of tes3mp addition */ - CellStore* current = MWBase::Environment::get().getWorld()->getExterior(X,Y); + CellStore* current = MWBase::Environment::get().getWorld()->getExterior(playerCellX, playerCellY); MWBase::Environment::get().getWindowManager()->changeCell(current); if (changeEvent) @@ -545,8 +713,9 @@ namespace MWWorld mLastPlayerPos = pos.asVec3(); } - Scene::Scene (MWRender::RenderingManager& rendering, MWPhysics::PhysicsSystem *physics) - : mCurrentCell (0), mCellChanged (false), mPhysics(physics), mRendering(rendering) + Scene::Scene (MWRender::RenderingManager& rendering, MWPhysics::PhysicsSystem *physics, + DetourNavigator::Navigator& navigator) + : mCurrentCell (0), mCellChanged (false), mPhysics(physics), mRendering(rendering), mNavigator(navigator) , mPreloadTimer(0.f) , mHalfGridSize(Settings::Manager::getInt("exterior cell load distance", "Cells")) , mCellLoadingThreshold(1024.f) @@ -623,8 +792,7 @@ namespace MWWorld while (active!=mActiveCells.end()) unloadCell (active++); - int refsToLoad = cell->count(); - loadingListener->setProgressRange(refsToLoad); + loadingListener->setProgressRange(cell->count()); // Load cell. loadCell (cell, loadingListener, changeEvent); @@ -691,9 +859,10 @@ namespace MWWorld void Scene::insertCell (CellStore &cell, bool rescale, Loading::Listener* loadingListener) { - InsertVisitor insertVisitor (cell, rescale, *loadingListener, *mPhysics, mRendering); + InsertVisitor insertVisitor (cell, rescale, *loadingListener); cell.forEach (insertVisitor); - insertVisitor.insert(); + insertVisitor.insert([&] (const MWWorld::Ptr& ptr) { addObject(ptr, *mPhysics, mRendering); }); + insertVisitor.insert([&] (const MWWorld::Ptr& ptr) { addObject(ptr, *mPhysics, mNavigator); }); // do adjustPosition (snapping actors to ground) after objects are loaded, so we don't depend on the loading order AdjustPositionVisitor adjustPosVisitor; @@ -705,7 +874,11 @@ namespace MWWorld try { addObject(ptr, *mPhysics, mRendering); + addObject(ptr, *mPhysics, mNavigator); MWBase::Environment::get().getWorld()->scaleObject(ptr, ptr.getCellRef().getScale()); + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + navigator->update(player.getRefData().getPosition().asVec3()); } catch (std::exception& e) { @@ -717,6 +890,20 @@ namespace MWWorld { MWBase::Environment::get().getMechanicsManager()->remove (ptr); MWBase::Environment::get().getSoundManager()->stopSound3D (ptr); + const auto navigator = MWBase::Environment::get().getWorld()->getNavigator(); + if (const auto object = mPhysics->getObject(ptr)) + { + navigator->removeObject(DetourNavigator::ObjectId(object)); + const auto player = MWBase::Environment::get().getWorld()->getPlayerPtr(); + navigator->update(player.getRefData().getPosition().asVec3()); + } + else if (const auto actor = mPhysics->getActor(ptr)) + { + const auto& halfExtents = ptr.getCell()->isExterior() + ? mPhysics->getHalfExtents(MWBase::Environment::get().getWorld()->getPlayerPtr()) + : actor->getHalfExtents(); + navigator->removeAgent(halfExtents); + } mPhysics->remove(ptr); mRendering.removeObject (ptr); if (ptr.getClass().isActor()) diff --git a/apps/openmw/mwworld/scene.hpp b/apps/openmw/mwworld/scene.hpp index e2fac6438..00f5f98b8 100644 --- a/apps/openmw/mwworld/scene.hpp +++ b/apps/openmw/mwworld/scene.hpp @@ -6,6 +6,7 @@ #include #include +#include namespace osg { @@ -27,6 +28,12 @@ namespace Loading class Listener; } +namespace DetourNavigator +{ + class Navigator; + class Water; +} + namespace MWRender { class SkyManager; @@ -57,6 +64,7 @@ namespace MWWorld bool mCellChanged; MWPhysics::PhysicsSystem *mPhysics; MWRender::RenderingManager& mRendering; + DetourNavigator::Navigator& mNavigator; std::unique_ptr mPreloader; float mPreloadTimer; int mHalfGridSize; @@ -74,7 +82,7 @@ namespace MWWorld void insertCell (CellStore &cell, bool rescale, Loading::Listener* loadingListener); // Load and unload cells as necessary to create a cell grid with "X" and "Y" in the center - void changeCellGrid (int X, int Y, bool changeEvent = true); + void changeCellGrid (int playerCellX, int playerCellY, bool changeEvent = true); void getGridCenter(int& cellX, int& cellY); @@ -85,7 +93,8 @@ namespace MWWorld public: - Scene (MWRender::RenderingManager& rendering, MWPhysics::PhysicsSystem *physics); + Scene (MWRender::RenderingManager& rendering, MWPhysics::PhysicsSystem *physics, + DetourNavigator::Navigator& navigator); ~Scene(); diff --git a/apps/openmw/mwworld/worldimp.cpp b/apps/openmw/mwworld/worldimp.cpp index 08d53b286..e5296eec7 100644 --- a/apps/openmw/mwworld/worldimp.cpp +++ b/apps/openmw/mwworld/worldimp.cpp @@ -5,6 +5,9 @@ #include #include +#include +#include + /* Start of tes3mp addition @@ -36,10 +39,15 @@ #include +#include #include #include +#include +#include +#include + #include "../mwbase/environment.hpp" #include "../mwbase/soundmanager.hpp" #include "../mwbase/mechanicsmanager.hpp" @@ -68,6 +76,7 @@ #include "../mwphysics/physicssystem.hpp" #include "../mwphysics/actor.hpp" #include "../mwphysics/collisiontype.hpp" +#include "../mwphysics/object.hpp" #include "player.hpp" #include "manualref.hpp" @@ -180,12 +189,6 @@ namespace MWWorld mLevitationEnabled(true), mGoToJail(false), mDaysInPrison(0), mPlayerTraveling(false), mPlayerInJail(false), mSpellPreloadTimer(0.f) { - mPhysics.reset(new MWPhysics::PhysicsSystem(resourceSystem, rootNode)); - mRendering.reset(new MWRender::RenderingManager(viewer, rootNode, resourceSystem, workQueue, &mFallback, resourcePath)); - mProjectileManager.reset(new ProjectileManager(mRendering->getLightRoot(), resourceSystem, mRendering.get(), mPhysics.get())); - - mRendering->preloadCommonAssets(); - mEsm.resize(contentFiles.size()); Loading::Listener* listener = MWBase::Environment::get().getWindowManager()->getLoadingScreen(); listener->loadingOn(); @@ -214,9 +217,50 @@ namespace MWWorld mSwimHeightScale = mStore.get().find("fSwimHeightScale")->mValue.getFloat(); + mPhysics.reset(new MWPhysics::PhysicsSystem(resourceSystem, rootNode)); + + DetourNavigator::Settings navigatorSettings; + navigatorSettings.mBorderSize = Settings::Manager::getInt("border size", "Navigator"); + navigatorSettings.mCellHeight = Settings::Manager::getFloat("cell height", "Navigator"); + navigatorSettings.mCellSize = Settings::Manager::getFloat("cell size", "Navigator"); + navigatorSettings.mDetailSampleDist = Settings::Manager::getFloat("detail sample dist", "Navigator"); + navigatorSettings.mDetailSampleMaxError = Settings::Manager::getFloat("detail sample max error", "Navigator"); + navigatorSettings.mMaxClimb = MWPhysics::sStepSizeUp; + navigatorSettings.mMaxSimplificationError = Settings::Manager::getFloat("max simplification error", "Navigator"); + navigatorSettings.mMaxSlope = MWPhysics::sMaxSlope; + navigatorSettings.mRecastScaleFactor = Settings::Manager::getFloat("recast scale factor", "Navigator"); + navigatorSettings.mSwimHeightScale = mSwimHeightScale; + navigatorSettings.mMaxEdgeLen = Settings::Manager::getInt("max edge len", "Navigator"); + navigatorSettings.mMaxNavMeshQueryNodes = Settings::Manager::getInt("max nav mesh query nodes", "Navigator"); + navigatorSettings.mMaxPolys = Settings::Manager::getInt("max polygons per tile", "Navigator"); + navigatorSettings.mMaxVertsPerPoly = Settings::Manager::getInt("max verts per poly", "Navigator"); + navigatorSettings.mRegionMergeSize = Settings::Manager::getInt("region merge size", "Navigator"); + navigatorSettings.mRegionMinSize = Settings::Manager::getInt("region min size", "Navigator"); + navigatorSettings.mTileSize = Settings::Manager::getInt("tile size", "Navigator"); + navigatorSettings.mAsyncNavMeshUpdaterThreads = static_cast(Settings::Manager::getInt("async nav mesh updater threads", "Navigator")); + navigatorSettings.mMaxNavMeshTilesCacheSize = static_cast(Settings::Manager::getInt("max nav mesh tiles cache size", "Navigator")); + navigatorSettings.mMaxPolygonPathSize = static_cast(Settings::Manager::getInt("max polygon path size", "Navigator")); + navigatorSettings.mMaxSmoothPathSize = static_cast(Settings::Manager::getInt("max smooth path size", "Navigator")); + navigatorSettings.mTrianglesPerChunk = static_cast(Settings::Manager::getInt("triangles per chunk", "Navigator")); + navigatorSettings.mEnableWriteRecastMeshToFile = Settings::Manager::getBool("enable write recast mesh to file", "Navigator"); + navigatorSettings.mEnableWriteNavMeshToFile = Settings::Manager::getBool("enable write nav mesh to file", "Navigator"); + navigatorSettings.mRecastMeshPathPrefix = Settings::Manager::getString("recast mesh path prefix", "Navigator"); + navigatorSettings.mNavMeshPathPrefix = Settings::Manager::getString("nav mesh path prefix", "Navigator"); + navigatorSettings.mEnableRecastMeshFileNameRevision = Settings::Manager::getBool("enable recast mesh file name revision", "Navigator"); + navigatorSettings.mEnableNavMeshFileNameRevision = Settings::Manager::getBool("enable nav mesh file name revision", "Navigator"); + if (Settings::Manager::getBool("enable log", "Navigator")) + DetourNavigator::Log::instance().setSink(std::unique_ptr( + new DetourNavigator::FileSink(Settings::Manager::getString("log path", "Navigator")))); + DetourNavigator::RecastGlobalAllocator::init(); + mNavigator.reset(new DetourNavigator::Navigator(navigatorSettings)); + + mRendering.reset(new MWRender::RenderingManager(viewer, rootNode, resourceSystem, workQueue, &mFallback, resourcePath, *mNavigator)); + mProjectileManager.reset(new ProjectileManager(mRendering->getLightRoot(), resourceSystem, mRendering.get(), mPhysics.get())); + mRendering->preloadCommonAssets(); + mWeatherManager.reset(new MWWorld::WeatherManager(*mRendering, mFallback, mStore)); - mWorldScene.reset(new Scene(*mRendering.get(), mPhysics.get())); + mWorldScene.reset(new Scene(*mRendering.get(), mPhysics.get(), *mNavigator)); } void World::fillGlobalVariables() @@ -1736,6 +1780,32 @@ namespace MWWorld moveObjectImp(player->first, player->second.x(), player->second.y(), player->second.z(), false); } + void World::updateNavigator() + { + bool updated = false; + + mPhysics->forEachAnimatedObject([&] (const MWPhysics::Object* object) + { + updated = updateNavigatorObject(object) || updated; + }); + + for (const auto& door : mDoorStates) + if (const auto object = mPhysics->getObject(door.first)) + updated = updateNavigatorObject(object) || updated; + + if (updated) + mNavigator->update(getPlayerPtr().getRefData().getPosition().asVec3()); + } + + bool World::updateNavigatorObject(const MWPhysics::Object* object) + { + const DetourNavigator::ObjectShapes shapes { + *object->getShapeInstance()->getCollisionShape(), + object->getShapeInstance()->getAvoidCollisionShape() + }; + return mNavigator->updateObject(DetourNavigator::ObjectId(object), shapes, object->getCollisionObject()->getWorldTransform()); + } + bool World::castRay (float x1, float y1, float z1, float x2, float y2, float z2) { int mask = MWPhysics::CollisionType_World | MWPhysics::CollisionType_Door; @@ -1943,7 +2013,10 @@ namespace MWWorld updateWeather(duration, paused); if (!paused) + { doPhysics (duration); + updateNavigator(); + } updatePlayer(); @@ -2528,7 +2601,7 @@ namespace MWWorld float waterlevel = cell->getWaterLevel(); - // SwimHeightScale affects the upper z position an actor can swim to + // SwimHeightScale affects the upper z position an actor can swim to // while in water. Based on observation from the original engine, // the upper z position you get with a +1 SwimHeightScale is the depth // limit for being able to cast water walking on an underwater target. @@ -2597,6 +2670,7 @@ namespace MWWorld { // Remove the old CharacterController MWBase::Environment::get().getMechanicsManager()->remove(getPlayerPtr()); + mNavigator->removeAgent(mPhysics->getHalfExtents(getPlayerPtr())); mPhysics->remove(getPlayerPtr()); mRendering->removePlayer(getPlayerPtr()); @@ -2631,6 +2705,8 @@ namespace MWWorld mPhysics->addActor(getPlayerPtr(), model); applyLoopingParticles(player); + + mNavigator->addAgent(mPhysics->getHalfExtents(getPlayerPtr())); } World::RestPermitted World::canRest () const @@ -2671,7 +2747,7 @@ namespace MWWorld { mRendering->screenshot(image, w, h); } - + bool World::screenshot360(osg::Image* image, std::string settingStr) { return mRendering->screenshot360(image,settingStr); @@ -3984,7 +4060,7 @@ namespace MWWorld continue; } else - mRendering->spawnEffect("meshes\\" + areaStatic->mModel, texture, origin, static_cast(effectIt->mArea * 2)); + mRendering->spawnEffect("meshes\\" + areaStatic->mModel, texture, origin, static_cast(effectIt->mArea * 2)); // Play explosion sound (make sure to use NoTrack, since we will delete the projectile now) static const std::string schools[] = { @@ -4136,4 +4212,25 @@ namespace MWWorld } } + DetourNavigator::Navigator* World::getNavigator() const + { + return mNavigator.get(); + } + + void World::updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const + { + mRendering->updateActorPath(actor, path, halfExtents, start, end); + } + + void World::removeActorPath(const MWWorld::ConstPtr& actor) const + { + mRendering->removeActorPath(actor); + } + + void World::setNavMeshNumberToRender(const std::size_t value) + { + mRendering->setNavMeshNumber(value); + } + } diff --git a/apps/openmw/mwworld/worldimp.hpp b/apps/openmw/mwworld/worldimp.hpp index 3faeb7d7c..799a03600 100644 --- a/apps/openmw/mwworld/worldimp.hpp +++ b/apps/openmw/mwworld/worldimp.hpp @@ -61,6 +61,11 @@ namespace ToUTF8 struct ContentLoader; +namespace MWPhysics +{ + class Object; +} + namespace MWWorld { class WeatherManager; @@ -94,6 +99,7 @@ namespace MWWorld std::unique_ptr mPlayer; std::unique_ptr mPhysics; + std::unique_ptr mNavigator; std::unique_ptr mRendering; std::unique_ptr mWorldScene; std::unique_ptr mWeatherManager; @@ -157,6 +163,10 @@ namespace MWWorld void doPhysics(float duration); ///< Run physics simulation and modify \a world accordingly. + void updateNavigator(); + + bool updateNavigatorObject(const MWPhysics::Object* object); + void ensureNeededRecords(); void fillGlobalVariables(); @@ -852,6 +862,15 @@ namespace MWWorld /// Preload VFX associated with this effect list void preloadEffects(const ESM::EffectList* effectList) override; + + DetourNavigator::Navigator* getNavigator() const override; + + void updateActorPath(const MWWorld::ConstPtr& actor, const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end) const override; + + void removeActorPath(const MWWorld::ConstPtr& actor) const override; + + void setNavMeshNumberToRender(const std::size_t value) override; }; } diff --git a/apps/openmw_test_suite/CMakeLists.txt b/apps/openmw_test_suite/CMakeLists.txt index bc39bc2be..7f10ab2fb 100644 --- a/apps/openmw_test_suite/CMakeLists.txt +++ b/apps/openmw_test_suite/CMakeLists.txt @@ -17,6 +17,13 @@ if (GTEST_FOUND AND GMOCK_FOUND) misc/test_stringops.cpp nifloader/testbulletnifloader.cpp + + detournavigator/navigator.cpp + detournavigator/settingsutils.cpp + detournavigator/recastmeshbuilder.cpp + detournavigator/gettilespositions.cpp + detournavigator/recastmeshobject.cpp + detournavigator/navmeshtilescache.cpp ) source_group(apps\\openmw_test_suite FILES openmw_test_suite.cpp ${UNITTEST_SRC_FILES}) diff --git a/apps/openmw_test_suite/detournavigator/gettilespositions.cpp b/apps/openmw_test_suite/detournavigator/gettilespositions.cpp new file mode 100644 index 000000000..1ad5c063d --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/gettilespositions.cpp @@ -0,0 +1,104 @@ +#include +#include + +#include +#include + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct CollectTilesPositions + { + std::vector& mTilesPositions; + + void operator ()(const TilePosition& value) + { + mTilesPositions.push_back(value); + } + }; + + struct DetourNavigatorGetTilesPositionsTest : Test + { + Settings mSettings; + std::vector mTilesPositions; + CollectTilesPositions mCollect {mTilesPositions}; + + DetourNavigatorGetTilesPositionsTest() + { + mSettings.mBorderSize = 0; + mSettings.mCellSize = 0.5; + mSettings.mRecastScaleFactor = 1; + mSettings.mTileSize = 64; + } + }; + + TEST_F(DetourNavigatorGetTilesPositionsTest, for_object_in_single_tile_should_return_one_tile) + { + getTilesPositions(osg::Vec3f(2, 2, 0), osg::Vec3f(31, 31, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0))); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, for_object_with_x_bounds_in_two_tiles_should_return_two_tiles) + { + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(32, 31, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0), TilePosition(1, 0))); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, for_object_with_y_bounds_in_two_tiles_should_return_two_tiles) + { + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(31, 32, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0), TilePosition(0, 1))); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, tiling_works_only_for_x_and_y_coordinates) + { + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(31, 31, 32), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0))); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, tiling_should_work_with_negative_coordinates) + { + getTilesPositions(osg::Vec3f(-31, -31, 0), osg::Vec3f(31, 31, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre( + TilePosition(-1, -1), + TilePosition(-1, 0), + TilePosition(0, -1), + TilePosition(0, 0) + )); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, border_size_should_extend_tile_bounds) + { + mSettings.mBorderSize = 1; + + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(31.5, 31.5, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre( + TilePosition(-1, -1), + TilePosition(-1, 0), + TilePosition(-1, 1), + TilePosition(0, -1), + TilePosition(0, 0), + TilePosition(0, 1), + TilePosition(1, -1), + TilePosition(1, 0), + TilePosition(1, 1) + )); + } + + TEST_F(DetourNavigatorGetTilesPositionsTest, should_apply_recast_scale_factor) + { + mSettings.mRecastScaleFactor = 0.5; + + getTilesPositions(osg::Vec3f(0, 0, 0), osg::Vec3f(32, 32, 1), mSettings, mCollect); + + EXPECT_THAT(mTilesPositions, ElementsAre(TilePosition(0, 0))); + } +} diff --git a/apps/openmw_test_suite/detournavigator/navigator.cpp b/apps/openmw_test_suite/detournavigator/navigator.cpp new file mode 100644 index 000000000..febbc0387 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/navigator.cpp @@ -0,0 +1,661 @@ +#include "operators.hpp" + +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorNavigatorTest : Test + { + Settings mSettings; + std::unique_ptr mNavigator; + osg::Vec3f mPlayerPosition; + osg::Vec3f mAgentHalfExtents; + osg::Vec3f mStart; + osg::Vec3f mEnd; + std::deque mPath; + std::back_insert_iterator> mOut; + + DetourNavigatorNavigatorTest() + : mPlayerPosition(0, 0, 0) + , mAgentHalfExtents(29, 29, 66) + , mStart(-215, 215, 1) + , mEnd(215, -215, 1) + , mOut(mPath) + { + mSettings.mEnableWriteRecastMeshToFile = false; + mSettings.mEnableWriteNavMeshToFile = false; + mSettings.mEnableRecastMeshFileNameRevision = false; + mSettings.mEnableNavMeshFileNameRevision = false; + mSettings.mBorderSize = 16; + 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.mSwimHeightScale = 0.89999997615814208984375f; + mSettings.mMaxEdgeLen = 12; + mSettings.mMaxNavMeshQueryNodes = 2048; + mSettings.mMaxVertsPerPoly = 6; + mSettings.mRegionMergeSize = 20; + mSettings.mRegionMinSize = 8; + mSettings.mTileSize = 64; + mSettings.mAsyncNavMeshUpdaterThreads = 1; + mSettings.mMaxNavMeshTilesCacheSize = 1024 * 1024; + mSettings.mMaxPolygonPathSize = 1024; + mSettings.mMaxSmoothPathSize = 1024; + mSettings.mTrianglesPerChunk = 256; + mSettings.mMaxPolys = 4096; + mNavigator.reset(new Navigator(mSettings)); + } + }; + + TEST_F(DetourNavigatorNavigatorTest, find_path_for_empty_should_throw_exception) + { + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), InvalidArgument); + } + + TEST_F(DetourNavigatorNavigatorTest, find_path_for_existing_agent_with_no_navmesh_should_throw_exception) + { + mNavigator->addAgent(mAgentHalfExtents); + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), NavigatorException); + } + + TEST_F(DetourNavigatorNavigatorTest, find_path_for_removed_agent_should_throw_exception) + { + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->removeAgent(mAgentHalfExtents); + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), InvalidArgument); + } + + TEST_F(DetourNavigatorNavigatorTest, add_agent_should_count_each_agent) + { + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->removeAgent(mAgentHalfExtents); + EXPECT_THROW(mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut), NavigatorException); + } + + TEST_F(DetourNavigatorNavigatorTest, update_then_find_path_should_return_path) + { + const std::array 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(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + 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, add_object_should_change_navmesh) + { + const std::array 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 heightfieldShape(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + heightfieldShape.setLocalScaling(btVector3(128, 128, 1)); + + btBoxShape boxShape(btVector3(20, 20, 100)); + btCompoundShape compoundShape; + compoundShape.addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(0, 0, 0)), &boxShape); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(ObjectId(&heightfieldShape), heightfieldShape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); + + EXPECT_EQ(mPath, std::deque({ + 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; + + mNavigator->addObject(ObjectId(&compoundShape), compoundShape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mPath.clear(); + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(-215, 215, 1.87827122211456298828125), + osg::Vec3f(-199.7968292236328125, 191.09100341796875, -3.54876613616943359375), + osg::Vec3f(-184.5936431884765625, 167.1819915771484375, -8.97847270965576171875), + osg::Vec3f(-169.3904571533203125, 143.2729949951171875, -14.40817737579345703125), + osg::Vec3f(-154.1872711181640625, 119.36397552490234375, -19.837890625), + osg::Vec3f(-138.9840850830078125, 95.45496368408203125, -25.2675952911376953125), + osg::Vec3f(-123.78090667724609375, 71.54595184326171875, -30.6972980499267578125), + osg::Vec3f(-108.57772064208984375, 47.636936187744140625, -36.12701416015625), + osg::Vec3f(-93.3745269775390625, 23.7279262542724609375, -40.754688262939453125), + osg::Vec3f(-78.17134857177734375, -0.18108306825160980224609375, -37.128787994384765625), + osg::Vec3f(-62.968158721923828125, -24.0900936126708984375, -33.50289154052734375), + osg::Vec3f(-47.764972686767578125, -47.999103546142578125, -30.797946929931640625), + osg::Vec3f(-23.852447509765625, -63.196765899658203125, -33.97112274169921875), + osg::Vec3f(0.0600789971649646759033203125, -78.39443206787109375, -37.14543914794921875), + osg::Vec3f(23.97260284423828125, -93.5920867919921875, -40.7740936279296875), + osg::Vec3f(47.885128021240234375, -108.78974151611328125, -36.051288604736328125), + osg::Vec3f(71.7976531982421875, -123.98740386962890625, -30.62355804443359375), + osg::Vec3f(95.71018218994140625, -139.18505859375, -25.1958160400390625), + osg::Vec3f(119.6226959228515625, -154.382720947265625, -19.7680912017822265625), + osg::Vec3f(143.53521728515625, -169.58038330078125, -14.3403491973876953125), + osg::Vec3f(167.4477386474609375, -184.778045654296875, -8.91261768341064453125), + osg::Vec3f(191.360260009765625, -199.9757080078125, -3.484879016876220703125), + osg::Vec3f(215, -215, 1.87827455997467041015625), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, update_changed_object_should_change_navmesh) + { + const std::array 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 heightfieldShape(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + heightfieldShape.setLocalScaling(btVector3(128, 128, 1)); + + btBoxShape boxShape(btVector3(20, 20, 100)); + btCompoundShape compoundShape; + compoundShape.addChildShape(btTransform(btMatrix3x3::getIdentity(), btVector3(0, 0, 0)), &boxShape); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(ObjectId(&heightfieldShape), heightfieldShape, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&compoundShape), compoundShape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, std::back_inserter(mPath)); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(-215, 215, 1.87827122211456298828125), + osg::Vec3f(-199.7968292236328125, 191.09100341796875, -3.54876613616943359375), + osg::Vec3f(-184.5936431884765625, 167.1819915771484375, -8.97847270965576171875), + osg::Vec3f(-169.3904571533203125, 143.2729949951171875, -14.40817737579345703125), + osg::Vec3f(-154.1872711181640625, 119.36397552490234375, -19.837890625), + osg::Vec3f(-138.9840850830078125, 95.45496368408203125, -25.2675952911376953125), + osg::Vec3f(-123.78090667724609375, 71.54595184326171875, -30.6972980499267578125), + osg::Vec3f(-108.57772064208984375, 47.636936187744140625, -36.12701416015625), + osg::Vec3f(-93.3745269775390625, 23.7279262542724609375, -40.754688262939453125), + osg::Vec3f(-78.17134857177734375, -0.18108306825160980224609375, -37.128787994384765625), + osg::Vec3f(-62.968158721923828125, -24.0900936126708984375, -33.50289154052734375), + osg::Vec3f(-47.764972686767578125, -47.999103546142578125, -30.797946929931640625), + osg::Vec3f(-23.852447509765625, -63.196765899658203125, -33.97112274169921875), + osg::Vec3f(0.0600789971649646759033203125, -78.39443206787109375, -37.14543914794921875), + osg::Vec3f(23.97260284423828125, -93.5920867919921875, -40.7740936279296875), + osg::Vec3f(47.885128021240234375, -108.78974151611328125, -36.051288604736328125), + osg::Vec3f(71.7976531982421875, -123.98740386962890625, -30.62355804443359375), + osg::Vec3f(95.71018218994140625, -139.18505859375, -25.1958160400390625), + osg::Vec3f(119.6226959228515625, -154.382720947265625, -19.7680912017822265625), + osg::Vec3f(143.53521728515625, -169.58038330078125, -14.3403491973876953125), + osg::Vec3f(167.4477386474609375, -184.778045654296875, -8.91261768341064453125), + osg::Vec3f(191.360260009765625, -199.9757080078125, -3.484879016876220703125), + osg::Vec3f(215, -215, 1.87827455997467041015625), + })) << mPath; + + compoundShape.updateChildTransform(0, btTransform(btMatrix3x3::getIdentity(), btVector3(1000, 0, 0))); + + mNavigator->updateObject(ObjectId(&compoundShape), compoundShape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mPath.clear(); + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + 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 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 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(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&shape2), shape2, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + 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; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_around_avoid_shape) + { + std::array 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)); + + std::array heightfieldDataAvoid {{ + -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 shapeAvoid(5, 5, heightfieldDataAvoid.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shapeAvoid.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(ObjectId(&shape), ObjectShapes {shape, &shapeAvoid}, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(-215, 215, 1.9393787384033203125), + osg::Vec3f(-200.8159637451171875, 190.47265625, -0.639537751674652099609375), + osg::Vec3f(-186.6319427490234375, 165.9453125, -3.2184507846832275390625), + osg::Vec3f(-172.447906494140625, 141.41796875, -5.797363758087158203125), + osg::Vec3f(-158.263885498046875, 116.8906097412109375, -8.37627887725830078125), + osg::Vec3f(-144.079864501953125, 92.3632659912109375, -10.95519161224365234375), + osg::Vec3f(-129.89581298828125, 67.83591461181640625, -13.534107208251953125), + osg::Vec3f(-115.7117919921875, 43.308563232421875, -16.1130199432373046875), + osg::Vec3f(-101.5277557373046875, 18.7812137603759765625, -18.6919345855712890625), + osg::Vec3f(-87.34372711181640625, -5.7461376190185546875, -20.4680538177490234375), + osg::Vec3f(-67.02922821044921875, -25.4970550537109375, -20.514247894287109375), + osg::Vec3f(-46.714717864990234375, -45.2479705810546875, -20.5604457855224609375), + osg::Vec3f(-26.40021514892578125, -64.99889373779296875, -20.6066417694091796875), + osg::Vec3f(-6.085712432861328125, -84.74980926513671875, -20.652835845947265625), + osg::Vec3f(14.22879505157470703125, -104.50072479248046875, -18.151393890380859375), + osg::Vec3f(39.05098724365234375, -118.16222381591796875, -15.6674861907958984375), + osg::Vec3f(63.87317657470703125, -131.82373046875, -13.18357944488525390625), + osg::Vec3f(88.69537353515625, -145.4852142333984375, -10.69967365264892578125), + osg::Vec3f(113.51757049560546875, -159.146697998046875, -8.21576690673828125), + osg::Vec3f(138.3397674560546875, -172.808197021484375, -5.731858730316162109375), + osg::Vec3f(163.1619720458984375, -186.469696044921875, -3.2479503154754638671875), + osg::Vec3f(187.984161376953125, -200.1311798095703125, -0.764044582843780517578125), + osg::Vec3f(212.8063507080078125, -213.7926788330078125, 1.7198636531829833984375), + osg::Vec3f(215, -215, 1.93937528133392333984375), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_ground_lower_than_water_with_only_swim_flag) + { + std::array heightfieldData {{ + -50, -50, -50, -50, 0, + -50, -100, -150, -100, -50, + -50, -150, -200, -150, -100, + -50, -100, -150, -100, -100, + 0, -50, -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->addWater(osg::Vec2i(0, 0), 128 * 4, 300, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mStart.x() = 0; + mStart.z() = 300; + mEnd.x() = 0; + mEnd.z() = 300; + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_swim, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(0, 215, 185.33331298828125), + osg::Vec3f(0, 186.6666717529296875, 185.33331298828125), + osg::Vec3f(0, 158.333343505859375, 185.33331298828125), + osg::Vec3f(0, 130.0000152587890625, 185.33331298828125), + osg::Vec3f(0, 101.66667938232421875, 185.33331298828125), + osg::Vec3f(0, 73.333343505859375, 185.33331298828125), + osg::Vec3f(0, 45.0000152587890625, 185.33331298828125), + osg::Vec3f(0, 16.6666812896728515625, 185.33331298828125), + osg::Vec3f(0, -11.66664981842041015625, 185.33331298828125), + osg::Vec3f(0, -39.999980926513671875, 185.33331298828125), + osg::Vec3f(0, -68.33331298828125, 185.33331298828125), + osg::Vec3f(0, -96.66664886474609375, 185.33331298828125), + osg::Vec3f(0, -124.99997711181640625, 185.33331298828125), + osg::Vec3f(0, -153.33331298828125, 185.33331298828125), + osg::Vec3f(0, -181.6666412353515625, 185.33331298828125), + osg::Vec3f(0, -209.999969482421875, 185.33331298828125), + osg::Vec3f(0, -215, 185.33331298828125), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_when_ground_cross_water_with_swim_and_walk_flags) + { + std::array heightfieldData {{ + 0, 0, 0, 0, 0, 0, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -150, -200, -150, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, 0, 0, 0, 0, 0, 0, + }}; + btHeightfieldTerrainShape shape(7, 7, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, -25, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mStart.x() = 0; + mEnd.x() = 0; + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_swim | Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(0, 215, -94.75363922119140625), + osg::Vec3f(0, 186.6666717529296875, -106.0000152587890625), + osg::Vec3f(0, 158.333343505859375, -115.85507965087890625), + osg::Vec3f(0, 130.0000152587890625, -125.71016693115234375), + osg::Vec3f(0, 101.66667938232421875, -135.5652313232421875), + osg::Vec3f(0, 73.333343505859375, -143.3333587646484375), + osg::Vec3f(0, 45.0000152587890625, -143.3333587646484375), + osg::Vec3f(0, 16.6666812896728515625, -143.3333587646484375), + osg::Vec3f(0, -11.66664981842041015625, -143.3333587646484375), + osg::Vec3f(0, -39.999980926513671875, -143.3333587646484375), + osg::Vec3f(0, -68.33331298828125, -143.3333587646484375), + osg::Vec3f(0, -96.66664886474609375, -137.3043670654296875), + osg::Vec3f(0, -124.99997711181640625, -127.44930267333984375), + osg::Vec3f(0, -153.33331298828125, -117.5942230224609375), + osg::Vec3f(0, -181.6666412353515625, -107.7391510009765625), + osg::Vec3f(0, -209.999969482421875, -97.79712677001953125), + osg::Vec3f(0, -215, -94.753631591796875), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_water_when_ground_cross_water_with_max_int_cells_size_and_swim_and_walk_flags) + { + std::array heightfieldData {{ + 0, 0, 0, 0, 0, 0, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -150, -200, -150, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, 0, 0, 0, 0, 0, 0, + }}; + btHeightfieldTerrainShape shape(7, 7, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->addWater(osg::Vec2i(0, 0), std::numeric_limits::max(), -25, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mStart.x() = 0; + mEnd.x() = 0; + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_swim | Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(0, 215, -94.75363922119140625), + osg::Vec3f(0, 186.6666717529296875, -106.0000152587890625), + osg::Vec3f(0, 158.333343505859375, -115.85507965087890625), + osg::Vec3f(0, 130.0000152587890625, -125.71016693115234375), + osg::Vec3f(0, 101.66667938232421875, -135.5652313232421875), + osg::Vec3f(0, 73.333343505859375, -143.3333587646484375), + osg::Vec3f(0, 45.0000152587890625, -143.3333587646484375), + osg::Vec3f(0, 16.6666812896728515625, -143.3333587646484375), + osg::Vec3f(0, -11.66664981842041015625, -143.3333587646484375), + osg::Vec3f(0, -39.999980926513671875, -143.3333587646484375), + osg::Vec3f(0, -68.33331298828125, -143.3333587646484375), + osg::Vec3f(0, -96.66664886474609375, -137.3043670654296875), + osg::Vec3f(0, -124.99997711181640625, -127.44930267333984375), + osg::Vec3f(0, -153.33331298828125, -117.5942230224609375), + osg::Vec3f(0, -181.6666412353515625, -107.7391510009765625), + osg::Vec3f(0, -209.999969482421875, -97.79712677001953125), + osg::Vec3f(0, -215, -94.753631591796875), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, path_should_be_over_ground_when_ground_cross_water_with_only_walk_flag) + { + std::array heightfieldData {{ + 0, 0, 0, 0, 0, 0, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -150, -200, -150, -100, 0, + 0, -100, -150, -150, -150, -100, 0, + 0, -100, -100, -100, -100, -100, 0, + 0, 0, 0, 0, 0, 0, 0, + }}; + btHeightfieldTerrainShape shape(7, 7, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + shape.setLocalScaling(btVector3(128, 128, 1)); + + mNavigator->addAgent(mAgentHalfExtents); + mNavigator->addWater(osg::Vec2i(0, 0), 128 * 4, -25, btTransform::getIdentity()); + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mStart.x() = 0; + mEnd.x() = 0; + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + osg::Vec3f(0, 215, -94.75363922119140625), + osg::Vec3f(9.8083515167236328125, 188.4185333251953125, -105.19994354248046875), + osg::Vec3f(19.6167049407958984375, 161.837066650390625, -114.25496673583984375), + osg::Vec3f(29.42505645751953125, 135.255615234375, -123.309967041015625), + osg::Vec3f(39.23340606689453125, 108.674163818359375, -132.3649749755859375), + osg::Vec3f(49.04175567626953125, 82.09270477294921875, -137.2874755859375), + osg::Vec3f(58.8501129150390625, 55.5112457275390625, -139.2451171875), + osg::Vec3f(68.6584625244140625, 28.9297885894775390625, -141.2027740478515625), + osg::Vec3f(78.4668121337890625, 2.3483295440673828125, -143.1604156494140625), + osg::Vec3f(88.27516937255859375, -24.233127593994140625, -141.3894805908203125), + osg::Vec3f(83.73651885986328125, -52.2005767822265625, -142.3761444091796875), + osg::Vec3f(79.19786834716796875, -80.16802978515625, -143.114837646484375), + osg::Vec3f(64.8477935791015625, -104.598602294921875, -137.840911865234375), + osg::Vec3f(50.497714996337890625, -129.0291748046875, -131.45831298828125), + osg::Vec3f(36.147632598876953125, -153.459747314453125, -121.42321014404296875), + osg::Vec3f(21.7975559234619140625, -177.8903350830078125, -111.38809967041015625), + osg::Vec3f(7.44747829437255859375, -202.3209075927734375, -101.1938323974609375), + osg::Vec3f(0, -215, -94.753631591796875), + })) << mPath; + } + + TEST_F(DetourNavigatorNavigatorTest, update_remove_and_update_then_find_path_should_return_path) + { + const std::array 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(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->removeObject(ObjectId(&shape)); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity()); + mNavigator->update(mPlayerPosition); + mNavigator->wait(); + + mNavigator->findPath(mAgentHalfExtents, mStart, mEnd, Flag_walk, mOut); + + EXPECT_EQ(mPath, std::deque({ + 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; + } +} diff --git a/apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp b/apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp new file mode 100644 index 000000000..17b17b97c --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/navmeshtilescache.cpp @@ -0,0 +1,336 @@ +#include "operators.hpp" + +#include +#include +#include + +#include + +#include + +namespace DetourNavigator +{ + static inline bool operator ==(const NavMeshDataRef& lhs, const NavMeshDataRef& rhs) + { + return std::make_pair(lhs.mValue, lhs.mSize) == std::make_pair(rhs.mValue, rhs.mSize); + } +} + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorNavMeshTilesCacheTest : Test + { + const osg::Vec3f mAgentHalfExtents {1, 2, 3}; + const TilePosition mTilePosition {0, 0}; + const std::vector mIndices {{0, 1, 2}}; + const std::vector mVertices {{0, 0, 0, 1, 0, 0, 1, 1, 0}}; + const std::vector mAreaTypes {1, AreaType_ground}; + const std::vector mWater {}; + const std::size_t mTrianglesPerChunk {1}; + const RecastMesh mRecastMesh {mIndices, mVertices, mAreaTypes, mWater, mTrianglesPerChunk}; + const std::vector mOffMeshConnections {}; + unsigned char* const mData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData mNavMeshData {mData, 1}; + }; + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_for_empty_cache_should_return_empty_value) + { + const std::size_t maxSize = 0; + NavMeshTilesCache cache(maxSize); + + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_for_not_enought_cache_size_should_return_empty_value) + { + const std::size_t maxSize = 0; + NavMeshTilesCache cache(maxSize); + + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData))); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_return_cached_value) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 49; + const std::size_t maxSize = navMeshDataSize + 2 * navMeshKeySize; + NavMeshTilesCache cache(maxSize); + + const auto result = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + ASSERT_TRUE(result); + EXPECT_EQ(result.get(), (NavMeshDataRef {mData, 1})); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_existing_element_should_throw_exception) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 49; + const std::size_t maxSize = 2 * (navMeshDataSize + 2 * navMeshKeySize); + NavMeshTilesCache cache(maxSize); + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_THROW( + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(anotherNavMeshData)), + InvalidArgument + ); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_should_return_cached_value) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 49; + const std::size_t maxSize = navMeshDataSize + 2 * navMeshKeySize; + NavMeshTilesCache cache(maxSize); + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + const auto result = cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections); + ASSERT_TRUE(result); + EXPECT_EQ(result.get(), (NavMeshDataRef {mData, 1})); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_for_cache_miss_by_agent_half_extents_should_return_empty_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + const osg::Vec3f unexsistentAgentHalfExtents {1, 1, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_FALSE(cache.get(unexsistentAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_for_cache_miss_by_tile_position_should_return_empty_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + const TilePosition unexistentTilePosition {1, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_FALSE(cache.get(mAgentHalfExtents, unexistentTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, get_for_cache_miss_by_recast_mesh_should_return_empty_value) + { + const std::size_t maxSize = 1; + NavMeshTilesCache cache(maxSize); + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh unexistentRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, unexistentRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_replace_unused_value) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 117; + const std::size_t maxSize = navMeshDataSize + 2 * navMeshKeySize; + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + const auto result = cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData)); + ASSERT_TRUE(result); + EXPECT_EQ(result.get(), (NavMeshDataRef {anotherData, 1})); + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_not_replace_used_value) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 49; + const std::size_t maxSize = navMeshDataSize + 2 * navMeshKeySize; + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + const auto value = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData))); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_replace_unused_least_recently_set_value) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 117; + const std::size_t maxSize = 2 * (navMeshDataSize + 2 * navMeshKeySize); + NavMeshTilesCache cache(maxSize); + + const std::vector leastRecentlySetWater {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh leastRecentlySetRecastMesh {mIndices, mVertices, mAreaTypes, leastRecentlySetWater, + mTrianglesPerChunk}; + const auto leastRecentlySetData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData leastRecentlySetNavMeshData {leastRecentlySetData, 1}; + + const std::vector mostRecentlySetWater {1, RecastMesh::Water {2, btTransform::getIdentity()}}; + const RecastMesh mostRecentlySetRecastMesh {mIndices, mVertices, mAreaTypes, mostRecentlySetWater, + mTrianglesPerChunk}; + const auto mostRecentlySetData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData mostRecentlySetNavMeshData {mostRecentlySetData, 1}; + + ASSERT_TRUE(cache.set(mAgentHalfExtents, mTilePosition, leastRecentlySetRecastMesh, mOffMeshConnections, + std::move(leastRecentlySetNavMeshData))); + ASSERT_TRUE(cache.set(mAgentHalfExtents, mTilePosition, mostRecentlySetRecastMesh, mOffMeshConnections, + std::move(mostRecentlySetNavMeshData))); + + const auto result = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + EXPECT_EQ(result.get(), (NavMeshDataRef {mData, 1})); + + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, leastRecentlySetRecastMesh, mOffMeshConnections)); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mostRecentlySetRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_replace_unused_least_recently_used_value) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 117; + const std::size_t maxSize = 2 * (navMeshDataSize + 2 * navMeshKeySize); + NavMeshTilesCache cache(maxSize); + + const std::vector leastRecentlyUsedWater {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh leastRecentlyUsedRecastMesh {mIndices, mVertices, mAreaTypes, leastRecentlyUsedWater, + mTrianglesPerChunk}; + const auto leastRecentlyUsedData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData leastRecentlyUsedNavMeshData {leastRecentlyUsedData, 1}; + + const std::vector mostRecentlyUsedWater {1, RecastMesh::Water {2, btTransform::getIdentity()}}; + const RecastMesh mostRecentlyUsedRecastMesh {mIndices, mVertices, mAreaTypes, mostRecentlyUsedWater, + mTrianglesPerChunk}; + const auto mostRecentlyUsedData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData mostRecentlyUsedNavMeshData {mostRecentlyUsedData, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, leastRecentlyUsedRecastMesh, mOffMeshConnections, + std::move(leastRecentlyUsedNavMeshData)); + cache.set(mAgentHalfExtents, mTilePosition, mostRecentlyUsedRecastMesh, mOffMeshConnections, + std::move(mostRecentlyUsedNavMeshData)); + + { + const auto value = cache.get(mAgentHalfExtents, mTilePosition, leastRecentlyUsedRecastMesh, mOffMeshConnections); + ASSERT_TRUE(value); + ASSERT_EQ(value.get(), (NavMeshDataRef {leastRecentlyUsedData, 1})); + } + + { + const auto value = cache.get(mAgentHalfExtents, mTilePosition, mostRecentlyUsedRecastMesh, mOffMeshConnections); + ASSERT_TRUE(value); + ASSERT_EQ(value.get(), (NavMeshDataRef {mostRecentlyUsedData, 1})); + } + + const auto result = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + EXPECT_EQ(result.get(), (NavMeshDataRef {mData, 1})); + + EXPECT_FALSE(cache.get(mAgentHalfExtents, mTilePosition, leastRecentlyUsedRecastMesh, mOffMeshConnections)); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mostRecentlyUsedRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_not_replace_unused_least_recently_used_value_when_item_does_not_not_fit_cache_max_size) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 49; + const std::size_t maxSize = 2 * (navMeshDataSize + 2 * navMeshKeySize); + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh tooLargeRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto tooLargeData = reinterpret_cast(dtAlloc(2, DT_ALLOC_PERM)); + NavMeshData tooLargeNavMeshData {tooLargeData, 2}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, tooLargeRecastMesh, mOffMeshConnections, + std::move(tooLargeNavMeshData))); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, set_should_not_replace_unused_least_recently_used_value_when_item_does_not_not_fit_size_of_unused_items) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize1 = 49; + const std::size_t navMeshKeySize2 = 117; + const std::size_t maxSize = 2 * navMeshDataSize + 2 * navMeshKeySize1 + 2 * navMeshKeySize2; + NavMeshTilesCache cache(maxSize); + + const std::vector anotherWater {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, anotherWater, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + const std::vector tooLargeWater {1, RecastMesh::Water {2, btTransform::getIdentity()}}; + const RecastMesh tooLargeRecastMesh {mIndices, mVertices, mAreaTypes, tooLargeWater, mTrianglesPerChunk}; + const auto tooLargeData = reinterpret_cast(dtAlloc(2, DT_ALLOC_PERM)); + NavMeshData tooLargeNavMeshData {tooLargeData, 2}; + + const auto value = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, + std::move(mNavMeshData)); + ASSERT_TRUE(value); + ASSERT_TRUE(cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData))); + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, tooLargeRecastMesh, mOffMeshConnections, + std::move(tooLargeNavMeshData))); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, release_used_after_set_then_used_by_get_item_should_left_this_item_available) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 49; + const std::size_t maxSize = navMeshDataSize + 2 * navMeshKeySize; + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + const auto firstCopy = cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + ASSERT_TRUE(firstCopy); + { + const auto secondCopy = cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections); + ASSERT_TRUE(secondCopy); + } + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData))); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } + + TEST_F(DetourNavigatorNavMeshTilesCacheTest, release_twice_used_item_should_left_this_item_available) + { + const std::size_t navMeshDataSize = 1; + const std::size_t navMeshKeySize = 49; + const std::size_t maxSize = navMeshDataSize + 2 * navMeshKeySize; + NavMeshTilesCache cache(maxSize); + + const std::vector water {1, RecastMesh::Water {1, btTransform::getIdentity()}}; + const RecastMesh anotherRecastMesh {mIndices, mVertices, mAreaTypes, water, mTrianglesPerChunk}; + const auto anotherData = reinterpret_cast(dtAlloc(1, DT_ALLOC_PERM)); + NavMeshData anotherNavMeshData {anotherData, 1}; + + cache.set(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections, std::move(mNavMeshData)); + const auto firstCopy = cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections); + ASSERT_TRUE(firstCopy); + { + const auto secondCopy = cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections); + ASSERT_TRUE(secondCopy); + } + EXPECT_FALSE(cache.set(mAgentHalfExtents, mTilePosition, anotherRecastMesh, mOffMeshConnections, + std::move(anotherNavMeshData))); + EXPECT_TRUE(cache.get(mAgentHalfExtents, mTilePosition, mRecastMesh, mOffMeshConnections)); + } +} diff --git a/apps/openmw_test_suite/detournavigator/operators.hpp b/apps/openmw_test_suite/detournavigator/operators.hpp new file mode 100644 index 000000000..16d8f38f5 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/operators.hpp @@ -0,0 +1,39 @@ +#ifndef OPENMW_TEST_SUITE_DETOURNAVIGATOR_OPERATORS_H +#define OPENMW_TEST_SUITE_DETOURNAVIGATOR_OPERATORS_H + +#include +#include + +#include +#include +#include +#include +#include + +#include + +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& value) + { + (*this) << "{\n"; + for (const auto& v : value) + { + std::ostringstream stream; + stream << v; + (*this) << stream.str() << ",\n"; + } + return (*this) << "}"; + } +} + +#endif diff --git a/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp b/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp new file mode 100644 index 000000000..38b1ab361 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/recastmeshbuilder.cpp @@ -0,0 +1,413 @@ +#include "operators.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace DetourNavigator +{ + static inline bool operator ==(const RecastMesh::Water& lhs, const RecastMesh::Water& rhs) + { + return lhs.mCellSize == rhs.mCellSize && lhs.mTransform == rhs.mTransform; + } +} + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorRecastMeshBuilderTest : Test + { + Settings mSettings; + TileBounds mBounds; + + DetourNavigatorRecastMeshBuilderTest() + { + mSettings.mRecastScaleFactor = 1.0f; + mSettings.mTrianglesPerChunk = 256; + mBounds.mMin = osg::Vec2f(-std::numeric_limits::max() * std::numeric_limits::epsilon(), + -std::numeric_limits::max() * std::numeric_limits::epsilon()); + mBounds.mMax = osg::Vec2f(std::numeric_limits::max() * std::numeric_limits::epsilon(), + std::numeric_limits::max() * std::numeric_limits::epsilon()); + } + }; + + TEST_F(DetourNavigatorRecastMeshBuilderTest, create_for_empty_should_return_empty) + { + RecastMeshBuilder builder(mSettings, mBounds); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector()); + EXPECT_EQ(recastMesh->getIndices(), std::vector()); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector()); + } + + 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); + + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject(static_cast(shape), btTransform::getIdentity(), AreaType_ground); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 0, -1, + -1, 0, 1, + -1, 0, -1, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + 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); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 2, 3, 0, + 0, 3, 4, + 0, 3, 0, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_heightfield_terrian_shape) + { + const std::array heightfieldData {{0, 0, 0, 0}}; + btHeightfieldTerrainShape shape(2, 2, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject(static_cast(shape), btTransform::getIdentity(), AreaType_ground); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + -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({0, 1, 2, 3, 4, 5})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground, AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_box_shape_should_produce_12_triangles) + { + btBoxShape shape(btVector3(1, 1, 2)); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject(static_cast(shape), btTransform::getIdentity(), AreaType_ground); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 2, 1, + -1, 2, 1, + 1, 2, -1, + -1, 2, -1, + 1, -2, 1, + -1, -2, 1, + 1, -2, -1, + -1, -2, -1, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({ + 0, 2, 3, + 3, 1, 0, + 0, 4, 6, + 6, 2, 0, + 0, 1, 5, + 5, 4, 0, + 7, 5, 1, + 1, 3, 7, + 7, 3, 2, + 2, 6, 7, + 7, 6, 4, + 4, 5, 7, + })); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector(12, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_compound_shape) + { + btTriangleMesh mesh1; + mesh1.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape triangle1(&mesh1, true); + btBoxShape box(btVector3(1, 1, 2)); + btTriangleMesh mesh2; + mesh2.addTriangle(btVector3(1, 1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape triangle2(&mesh2, true); + btCompoundShape shape; + shape.addChildShape(btTransform::getIdentity(), &triangle1); + shape.addChildShape(btTransform::getIdentity(), &box); + shape.addChildShape(btTransform::getIdentity(), &triangle2); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform::getIdentity(), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 0, -1, + -1, 0, 1, + -1, 0, -1, + 1, 2, 1, + -1, 2, 1, + 1, 2, -1, + -1, 2, -1, + 1, -2, 1, + -1, -2, 1, + 1, -2, -1, + -1, -2, -1, + 1, 0, -1, + -1, 0, 1, + 1, 0, 1, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({ + 0, 1, 2, + 3, 5, 6, + 6, 4, 3, + 3, 7, 9, + 9, 5, 3, + 3, 4, 8, + 8, 7, 3, + 10, 8, 4, + 4, 6, 10, + 10, 6, 5, + 5, 9, 10, + 10, 9, 7, + 7, 8, 10, + 11, 12, 13, + })); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector(14, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_transformed_compound_shape) + { + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape triangle(&mesh, true); + btCompoundShape shape; + shape.addChildShape(btTransform::getIdentity(), &triangle); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 2, 3, 0, + 0, 3, 4, + 0, 3, 0, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_transformed_compound_shape_with_transformed_bhv_triangle_shape) + { + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape triangle(&mesh, true); + btCompoundShape shape; + shape.addChildShape(btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)), + &triangle); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btMatrix3x3::getIdentity().scaled(btVector3(1, 2, 3)), btVector3(1, 2, 3)), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 3, 12, 2, + 1, 12, 10, + 1, 12, 2, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, without_bounds_add_bhv_triangle_shape_should_not_filter_by_bounds) + { + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + mesh.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform::getIdentity(), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 0, -1, + -1, 0, 1, + -1, 0, -1, + -2, 0, -3, + -3, 0, -2, + -3, 0, -3, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2, 3, 4, 5})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector(2, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_bhv_triangle_shape_should_filter_by_bounds) + { + mSettings.mRecastScaleFactor = 0.1f; + mBounds.mMin = osg::Vec2f(-3, -3) * mSettings.mRecastScaleFactor; + mBounds.mMax = osg::Vec2f(-2, -2) * mSettings.mRecastScaleFactor; + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + mesh.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform::getIdentity(), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + -0.2f, 0, -0.3f, + -0.3f, 0, -0.2f, + -0.3f, 0, -0.3f, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_rotated_by_x_bhv_triangle_shape_should_filter_by_bounds) + { + mBounds.mMin = osg::Vec2f(-5, -5); + mBounds.mMax = osg::Vec2f(5, -2); + btTriangleMesh mesh; + mesh.addTriangle(btVector3(0, -1, -1), btVector3(0, -1, -1), btVector3(0, 1, -1)); + mesh.addTriangle(btVector3(0, -3, -3), btVector3(0, -3, -2), btVector3(0, -2, -3)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btQuaternion(btVector3(1, 0, 0), + static_cast(-osg::PI_4))), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 0, -0.70710659027099609375, -3.535533905029296875, + 0, 0.707107067108154296875, -3.535533905029296875, + 0, 2.384185791015625e-07, -4.24264049530029296875, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_rotated_by_y_bhv_triangle_shape_should_filter_by_bounds) + { + mBounds.mMin = osg::Vec2f(-5, -5); + mBounds.mMax = osg::Vec2f(-3, 5); + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, 0, -1), btVector3(-1, 0, 1), btVector3(1, 0, -1)); + mesh.addTriangle(btVector3(-3, 0, -3), btVector3(-3, 0, -2), btVector3(-2, 0, -3)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btQuaternion(btVector3(0, 1, 0), + static_cast(osg::PI_4))), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + -3.535533905029296875, -0.70710659027099609375, 0, + -3.535533905029296875, 0.707107067108154296875, 0, + -4.24264049530029296875, 2.384185791015625e-07, 0, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, with_bounds_add_rotated_by_z_bhv_triangle_shape_should_filter_by_bounds) + { + mBounds.mMin = osg::Vec2f(-5, -5); + mBounds.mMax = osg::Vec2f(-1, -1); + btTriangleMesh mesh; + mesh.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + mesh.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0)); + btBvhTriangleMeshShape shape(&mesh, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape), + btTransform(btQuaternion(btVector3(0, 0, 1), + static_cast(osg::PI_4))), + AreaType_ground + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 0.707107067108154296875, 0, -3.535533905029296875, + -0.70710659027099609375, 0, -3.535533905029296875, + 2.384185791015625e-07, 0, -4.24264049530029296875, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, flags_values_should_be_corresponding_to_added_objects) + { + btTriangleMesh mesh1; + mesh1.addTriangle(btVector3(-1, -1, 0), btVector3(-1, 1, 0), btVector3(1, -1, 0)); + btBvhTriangleMeshShape shape1(&mesh1, true); + btTriangleMesh mesh2; + mesh2.addTriangle(btVector3(-3, -3, 0), btVector3(-3, -2, 0), btVector3(-2, -3, 0)); + btBvhTriangleMeshShape shape2(&mesh2, true); + RecastMeshBuilder builder(mSettings, mBounds); + builder.addObject( + static_cast(shape1), + btTransform::getIdentity(), + AreaType_ground + ); + builder.addObject( + static_cast(shape2), + btTransform::getIdentity(), + AreaType_null + ); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getVertices(), std::vector({ + 1, 0, -1, + -1, 0, 1, + -1, 0, -1, + -2, 0, -3, + -3, 0, -2, + -3, 0, -3, + })); + EXPECT_EQ(recastMesh->getIndices(), std::vector({0, 1, 2, 3, 4, 5})); + EXPECT_EQ(recastMesh->getAreaTypes(), std::vector({AreaType_ground, AreaType_null})); + } + + TEST_F(DetourNavigatorRecastMeshBuilderTest, add_water_then_get_water_should_return_it) + { + RecastMeshBuilder builder(mSettings, mBounds); + builder.addWater(1000, btTransform(btMatrix3x3::getIdentity(), btVector3(100, 200, 300))); + const auto recastMesh = builder.create(); + EXPECT_EQ(recastMesh->getWater(), std::vector({ + RecastMesh::Water {1000, btTransform(btMatrix3x3::getIdentity(), btVector3(100, 200, 300))} + })); + } +} diff --git a/apps/openmw_test_suite/detournavigator/recastmeshobject.cpp b/apps/openmw_test_suite/detournavigator/recastmeshobject.cpp new file mode 100644 index 000000000..9b30cadd7 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/recastmeshobject.cpp @@ -0,0 +1,72 @@ +#include "operators.hpp" + +#include + +#include +#include + +#include + +namespace +{ + using namespace testing; + using namespace DetourNavigator; + + struct DetourNavigatorRecastMeshObjectTest : Test + { + btBoxShape mBoxShape {btVector3(1, 2, 3)}; + btCompoundShape mCompoundShape {btVector3(1, 2, 3)}; + btTransform mTransform {btQuaternion(btVector3(1, 2, 3), 1), btVector3(1, 2, 3)}; + + DetourNavigatorRecastMeshObjectTest() + { + mCompoundShape.addChildShape(mTransform, std::addressof(mBoxShape)); + } + }; + + TEST_F(DetourNavigatorRecastMeshObjectTest, constructed_object_should_have_shape_and_transform) + { + const RecastMeshObject object(mBoxShape, mTransform, AreaType_ground); + EXPECT_EQ(std::addressof(object.getShape()), std::addressof(mBoxShape)); + EXPECT_EQ(object.getTransform(), mTransform); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_with_same_transform_for_not_compound_shape_should_return_false) + { + RecastMeshObject object(mBoxShape, mTransform, AreaType_ground); + EXPECT_FALSE(object.update(mTransform, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_with_different_transform_should_return_true) + { + RecastMeshObject object(mBoxShape, mTransform, AreaType_ground); + EXPECT_TRUE(object.update(btTransform::getIdentity(), AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_with_different_flags_should_return_true) + { + RecastMeshObject object(mBoxShape, mTransform, AreaType_ground); + EXPECT_TRUE(object.update(mTransform, AreaType_null)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_for_compound_shape_with_same_transform_and_not_changed_child_transform_should_return_false) + { + RecastMeshObject object(mCompoundShape, mTransform, AreaType_ground); + EXPECT_FALSE(object.update(mTransform, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, update_for_compound_shape_with_same_transform_and_changed_child_transform_should_return_true) + { + RecastMeshObject object(mCompoundShape, mTransform, AreaType_ground); + mCompoundShape.updateChildTransform(0, btTransform::getIdentity()); + EXPECT_TRUE(object.update(mTransform, AreaType_ground)); + } + + TEST_F(DetourNavigatorRecastMeshObjectTest, repeated_update_for_compound_shape_without_changes_should_return_false) + { + RecastMeshObject object(mCompoundShape, mTransform, AreaType_ground); + mCompoundShape.updateChildTransform(0, btTransform::getIdentity()); + object.update(mTransform, AreaType_ground); + EXPECT_FALSE(object.update(mTransform, AreaType_ground)); + } +} diff --git a/apps/openmw_test_suite/detournavigator/settingsutils.cpp b/apps/openmw_test_suite/detournavigator/settingsutils.cpp new file mode 100644 index 000000000..ffed64ab8 --- /dev/null +++ b/apps/openmw_test_suite/detournavigator/settingsutils.cpp @@ -0,0 +1,68 @@ +#include "operators.hpp" + +#include + +#include + +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)})); + } +} diff --git a/apps/openmw_test_suite/nifloader/testbulletnifloader.cpp b/apps/openmw_test_suite/nifloader/testbulletnifloader.cpp index 2361718b1..29f60b1f5 100644 --- a/apps/openmw_test_suite/nifloader/testbulletnifloader.cpp +++ b/apps/openmw_test_suite/nifloader/testbulletnifloader.cpp @@ -115,17 +115,6 @@ static std::ostream& operator <<(std::ostream& stream, const btCollisionShape* v return value ? stream << "&" << *value : stream << "nullptr"; } -namespace osg -{ - static std::ostream& operator <<(std::ostream& stream, const Vec3f& value) - { - return stream << "osg::Vec3f {" - << value.x() << ", " - << value.y() << ", " - << value.z() << "}"; - } -} - namespace std { static std::ostream& operator <<(std::ostream& stream, const map& value) @@ -142,6 +131,7 @@ namespace Resource static bool operator ==(const Resource::BulletShape& lhs, const Resource::BulletShape& rhs) { return compareObjects(lhs.mCollisionShape, rhs.mCollisionShape) + && compareObjects(lhs.mAvoidCollisionShape, rhs.mAvoidCollisionShape) && lhs.mCollisionBoxHalfExtents == rhs.mCollisionBoxHalfExtents && lhs.mCollisionBoxTranslate == rhs.mCollisionBoxTranslate && lhs.mAnimatedShapes == rhs.mAnimatedShapes; @@ -151,7 +141,8 @@ namespace Resource { return stream << "Resource::BulletShape {" << value.mCollisionShape << ", " - << value.mCollisionBoxHalfExtents << ", " + << value.mAvoidCollisionShape << ", " + << "osg::Vec3f {" << value.mCollisionBoxHalfExtents << "}" << ", " << value.mAnimatedShapes << "}"; } @@ -266,7 +257,8 @@ namespace value.target = Nif::ControlledPtr(nullptr); } - void copy(const btTransform& src, Nif::Transformation& dst) { + void copy(const btTransform& src, Nif::Transformation& dst) + { dst.pos = osg::Vec3f(src.getOrigin().x(), src.getOrigin().y(), src.getOrigin().z()); for (int row = 0; row < 3; ++row) for (int column = 0; column < 3; ++column) @@ -837,7 +829,10 @@ namespace EXPECT_CALL(mNifFile, getFilename()).WillOnce(Return("test.nif")); const auto result = mLoader.load(mNifFile); + std::unique_ptr triangles(new btTriangleMesh(false)); + triangles->addTriangle(btVector3(0, 0, 0), btVector3(1, 0, 0), btVector3(1, 1, 0)); Resource::BulletShape expected; + expected.mAvoidCollisionShape = new Resource::TriangleMeshShape(triangles.release(), false); EXPECT_EQ(*result, expected); } diff --git a/appveyor.yml b/appveyor.yml index 9877d2105..5f5657285 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -14,7 +14,7 @@ environment: APPVEYOR_BUILD_WORKER_IMAGE: Visual Studio 2017 matrix: allow_failures: - - msvc: 2015 + - msvc: 2015 platform: # - Win32 @@ -52,6 +52,7 @@ install: - set PATH=C:\Program Files\Git\mingw64\bin;%PATH% before_build: + - cmd: git submodule update --init --recursive - cmd: sh %APPVEYOR_BUILD_FOLDER%\CI\before_script.msvc.sh -c %configuration% -p %PLATFORM% -v %msvc% -V build_script: @@ -68,7 +69,7 @@ test: off #notifications: # - provider: Email # to: -# - +# - # on_build_failure: true # on_build_status_changed: true diff --git a/components/CMakeLists.txt b/components/CMakeLists.txt index 8d8a33b91..8754cec85 100644 --- a/components/CMakeLists.txt +++ b/components/CMakeLists.txt @@ -52,7 +52,7 @@ add_component_dir (shader add_component_dir (sceneutil clone attach visitor util statesetupdater controller skeleton riggeometry morphgeometry lightcontroller lightmanager lightutil positionattitudetransform workqueue unrefqueue pathgridutil waterutil writescene serialize optimizer - actorutil + actorutil detourdebugdraw navmesh agentpath ) add_component_dir (nif @@ -215,6 +215,29 @@ add_component_dir (fallback fallback validate ) +if(NOT WIN32 AND NOT ANDROID) + add_component_dir (crashcatcher + crashcatcher + ) +endif() + +add_component_dir(detournavigator + debug + makenavmesh + findsmoothpath + recastmeshbuilder + recastmeshmanager + cachedrecastmeshmanager + navmeshmanager + navigator + asyncnavmeshupdater + chunkytrimesh + recastmesh + tilecachedrecastmeshmanager + recastmeshobject + navmeshtilescache + ) + set (ESM_UI ${CMAKE_SOURCE_DIR}/files/ui/contentselector.ui ) @@ -275,6 +298,9 @@ target_link_libraries(components ${SDL2_LIBRARIES} ${OPENGL_gl_LIBRARY} ${MyGUI_LIBRARIES} + RecastNavigation::DebugUtils + RecastNavigation::Detour + RecastNavigation::Recast ) if (WIN32) diff --git a/components/bullethelpers/operators.hpp b/components/bullethelpers/operators.hpp new file mode 100644 index 000000000..ea88deddf --- /dev/null +++ b/components/bullethelpers/operators.hpp @@ -0,0 +1,71 @@ +#ifndef OPENMW_COMPONENTS_BULLETHELPERS_OPERATORS_H +#define OPENMW_COMPONENTS_BULLETHELPERS_OPERATORS_H + +#include +#include +#include + +#include +#include +#include +#include + +inline std::ostream& operator <<(std::ostream& stream, const btVector3& value) +{ + return stream << "btVector3(" << std::setprecision(std::numeric_limits::max_exponent10) << value.x() + << ", " << std::setprecision(std::numeric_limits::max_exponent10) << value.y() + << ", " << std::setprecision(std::numeric_limits::max_exponent10) << value.z() + << ')'; +} + +inline std::ostream& operator <<(std::ostream& stream, BroadphaseNativeTypes value) +{ + switch (value) + { +#ifndef SHAPE_NAME +#define SHAPE_NAME(name) case name: return stream << #name; + SHAPE_NAME(BOX_SHAPE_PROXYTYPE) + SHAPE_NAME(TRIANGLE_SHAPE_PROXYTYPE) + SHAPE_NAME(TETRAHEDRAL_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_HULL_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE) + SHAPE_NAME(CUSTOM_POLYHEDRAL_SHAPE_TYPE) + SHAPE_NAME(IMPLICIT_CONVEX_SHAPES_START_HERE) + SHAPE_NAME(SPHERE_SHAPE_PROXYTYPE) + SHAPE_NAME(MULTI_SPHERE_SHAPE_PROXYTYPE) + SHAPE_NAME(CAPSULE_SHAPE_PROXYTYPE) + SHAPE_NAME(CONE_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_SHAPE_PROXYTYPE) + SHAPE_NAME(CYLINDER_SHAPE_PROXYTYPE) + SHAPE_NAME(UNIFORM_SCALING_SHAPE_PROXYTYPE) + SHAPE_NAME(MINKOWSKI_SUM_SHAPE_PROXYTYPE) + SHAPE_NAME(MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE) + SHAPE_NAME(BOX_2D_SHAPE_PROXYTYPE) + SHAPE_NAME(CONVEX_2D_SHAPE_PROXYTYPE) + SHAPE_NAME(CUSTOM_CONVEX_SHAPE_TYPE) + SHAPE_NAME(CONCAVE_SHAPES_START_HERE) + SHAPE_NAME(TRIANGLE_MESH_SHAPE_PROXYTYPE) + SHAPE_NAME(SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + SHAPE_NAME(FAST_CONCAVE_MESH_PROXYTYPE) + SHAPE_NAME(TERRAIN_SHAPE_PROXYTYPE) + SHAPE_NAME(GIMPACT_SHAPE_PROXYTYPE) + SHAPE_NAME(MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE) + SHAPE_NAME(EMPTY_SHAPE_PROXYTYPE) + SHAPE_NAME(STATIC_PLANE_PROXYTYPE) + SHAPE_NAME(CUSTOM_CONCAVE_SHAPE_TYPE) + SHAPE_NAME(CONCAVE_SHAPES_END_HERE) + SHAPE_NAME(COMPOUND_SHAPE_PROXYTYPE) + SHAPE_NAME(SOFTBODY_SHAPE_PROXYTYPE) + SHAPE_NAME(HFFLUID_SHAPE_PROXYTYPE) + SHAPE_NAME(HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE) + SHAPE_NAME(INVALID_SHAPE_PROXYTYPE) + SHAPE_NAME(MAX_BROADPHASE_COLLISION_TYPES) +#undef SHAPE_NAME +#endif + default: + return stream << "undefined(" << static_cast(value) << ")"; + } +} + +#endif diff --git a/components/compiler/extensions0.cpp b/components/compiler/extensions0.cpp index 54b8c03cb..1b5558a3a 100644 --- a/components/compiler/extensions0.cpp +++ b/components/compiler/extensions0.cpp @@ -320,6 +320,10 @@ namespace Compiler extensions.registerInstruction ("removefromlevitem", "ccl", opcodeRemoveFromLevItem); extensions.registerInstruction ("tb", "", opcodeToggleBorders); extensions.registerInstruction ("toggleborders", "", opcodeToggleBorders); + extensions.registerInstruction ("togglenavmesh", "", opcodeToggleNavMesh); + extensions.registerInstruction ("tap", "", opcodeToggleActorsPaths); + extensions.registerInstruction ("toggleactorspaths", "", opcodeToggleActorsPaths); + extensions.registerInstruction ("setnavmeshnumber", "l", opcodeSetNavMeshNumberToRender); } } diff --git a/components/compiler/opcodes.hpp b/components/compiler/opcodes.hpp index c141eec68..a2d8a9467 100644 --- a/components/compiler/opcodes.hpp +++ b/components/compiler/opcodes.hpp @@ -296,6 +296,9 @@ namespace Compiler const int opcodeShowSceneGraph = 0x2002f; const int opcodeShowSceneGraphExplicit = 0x20030; const int opcodeToggleBorders = 0x2000307; + const int opcodeToggleNavMesh = 0x2000308; + const int opcodeToggleActorsPaths = 0x2000309; + const int opcodeSetNavMeshNumberToRender = 0x200030a; } namespace Sky diff --git a/components/debug/debugging.hpp b/components/debug/debugging.hpp index f47f58e45..361f321cb 100644 --- a/components/debug/debugging.hpp +++ b/components/debug/debugging.hpp @@ -44,6 +44,10 @@ namespace Debug }; #if defined(_WIN32) && defined(_DEBUG) +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN 1 +#endif // !WIN32_LEAN_AND_MEAN +#include class DebugOutput : public DebugOutputBase { public: diff --git a/components/debug/debuglog.hpp b/components/debug/debuglog.hpp index f5cdffeda..0676f7689 100644 --- a/components/debug/debuglog.hpp +++ b/components/debug/debuglog.hpp @@ -4,6 +4,8 @@ #include #include +#include + namespace Debug { enum Level diff --git a/components/detournavigator/areatype.hpp b/components/detournavigator/areatype.hpp new file mode 100644 index 000000000..0daa524ca --- /dev/null +++ b/components/detournavigator/areatype.hpp @@ -0,0 +1,16 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_AREATYPE_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_AREATYPE_H + +#include + +namespace DetourNavigator +{ + enum AreaType : unsigned char + { + AreaType_null = RC_NULL_AREA, + AreaType_water, + AreaType_ground = RC_WALKABLE_AREA, + }; +} + +#endif diff --git a/components/detournavigator/asyncnavmeshupdater.cpp b/components/detournavigator/asyncnavmeshupdater.cpp new file mode 100644 index 000000000..f4efc744b --- /dev/null +++ b/components/detournavigator/asyncnavmeshupdater.cpp @@ -0,0 +1,198 @@ +#include "asyncnavmeshupdater.hpp" +#include "debug.hpp" +#include "makenavmesh.hpp" +#include "settings.hpp" + +#include + +#include + +namespace +{ + using DetourNavigator::ChangeType; + using DetourNavigator::TilePosition; + + int getManhattanDistance(const TilePosition& lhs, const TilePosition& rhs) + { + return std::abs(lhs.x() - rhs.x()) + std::abs(lhs.y() - rhs.y()); + } + + std::tuple makePriority(const TilePosition& position, const ChangeType changeType, + const TilePosition& playerTile) + { + return std::make_tuple( + changeType, + getManhattanDistance(position, playerTile), + getManhattanDistance(position, TilePosition {0, 0}) + ); + } +} + +namespace DetourNavigator +{ + static std::ostream& operator <<(std::ostream& stream, UpdateNavMeshStatus value) + { + switch (value) + { + case UpdateNavMeshStatus::ignore: + return stream << "ignore"; + case UpdateNavMeshStatus::removed: + return stream << "removed"; + case UpdateNavMeshStatus::add: + return stream << "add"; + case UpdateNavMeshStatus::replaced: + return stream << "replaced"; + } + return stream << "unknown"; + } + + AsyncNavMeshUpdater::AsyncNavMeshUpdater(const Settings& settings, TileCachedRecastMeshManager& recastMeshManager, + OffMeshConnectionsManager& offMeshConnectionsManager) + : mSettings(settings) + , mRecastMeshManager(recastMeshManager) + , mOffMeshConnectionsManager(offMeshConnectionsManager) + , mShouldStop() + , mNavMeshTilesCache(settings.mMaxNavMeshTilesCacheSize) + { + for (std::size_t i = 0; i < mSettings.get().mAsyncNavMeshUpdaterThreads; ++i) + mThreads.emplace_back([&] { process(); }); + } + + AsyncNavMeshUpdater::~AsyncNavMeshUpdater() + { + mShouldStop = true; + std::unique_lock lock(mMutex); + mJobs = decltype(mJobs)(); + mHasJob.notify_all(); + lock.unlock(); + for (auto& thread : mThreads) + thread.join(); + } + + void AsyncNavMeshUpdater::post(const osg::Vec3f& agentHalfExtents, + const SharedNavMeshCacheItem& navMeshCacheItem, const TilePosition& playerTile, + const std::map& changedTiles) + { + *mPlayerTile.lock() = playerTile; + + if (changedTiles.empty()) + return; + + const std::lock_guard lock(mMutex); + + for (const auto& changedTile : changedTiles) + { + if (mPushed[agentHalfExtents].insert(changedTile.first).second) + mJobs.push(Job {agentHalfExtents, navMeshCacheItem, changedTile.first, + makePriority(changedTile.first, changedTile.second, playerTile)}); + } + + log("posted ", mJobs.size(), " jobs"); + + mHasJob.notify_all(); + } + + void AsyncNavMeshUpdater::wait() + { + std::unique_lock 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("stop process jobs"); + } + + void AsyncNavMeshUpdater::processJob(const Job& job) + { + log("process job for agent=", job.mAgentHalfExtents); + + const auto start = std::chrono::steady_clock::now(); + + const auto firstStart = setFirstStart(start); + + const auto recastMesh = mRecastMeshManager.get().getMesh(job.mChangedTile); + const auto playerTile = *mPlayerTile.lockConst(); + const auto offMeshConnections = mOffMeshConnectionsManager.get().get(job.mChangedTile); + + const auto status = updateNavMesh(job.mAgentHalfExtents, recastMesh.get(), job.mChangedTile, playerTile, + offMeshConnections, mSettings, job.mNavMeshCacheItem, mNavMeshTilesCache); + + const auto finish = std::chrono::steady_clock::now(); + + writeDebugFiles(job, recastMesh.get()); + + using FloatMs = std::chrono::duration; + + const auto locked = job.mNavMeshCacheItem.lockConst(); + log("cache updated for agent=", job.mAgentHalfExtents, " status=", status, + " generation=", locked->getGeneration(), + " revision=", locked->getNavMeshRevision(), + " time=", std::chrono::duration_cast(finish - start).count(), "ms", + " total_time=", std::chrono::duration_cast(finish - firstStart).count(), "ms"); + } + + boost::optional AsyncNavMeshUpdater::getNextJob() + { + std::unique_lock lock(mMutex); + if (mJobs.empty()) + mHasJob.wait_for(lock, std::chrono::milliseconds(10)); + if (mJobs.empty()) + { + mFirstStart.lock()->reset(); + mDone.notify_all(); + return boost::none; + } + log("got ", mJobs.size(), " jobs"); + const auto job = mJobs.top(); + mJobs.pop(); + const auto pushed = mPushed.find(job.mAgentHalfExtents); + pushed->second.erase(job.mChangedTile); + if (pushed->second.empty()) + mPushed.erase(pushed); + return job; + } + + void AsyncNavMeshUpdater::writeDebugFiles(const Job& job, const RecastMesh* recastMesh) const + { + std::string revision; + std::string recastMeshRevision; + std::string navMeshRevision; + if ((mSettings.get().mEnableWriteNavMeshToFile || mSettings.get().mEnableWriteRecastMeshToFile) + && (mSettings.get().mEnableRecastMeshFileNameRevision || mSettings.get().mEnableNavMeshFileNameRevision)) + { + revision = "." + std::to_string((std::chrono::steady_clock::now() + - std::chrono::steady_clock::time_point()).count()); + if (mSettings.get().mEnableRecastMeshFileNameRevision) + recastMeshRevision = revision; + if (mSettings.get().mEnableNavMeshFileNameRevision) + navMeshRevision = revision; + } + if (recastMesh && mSettings.get().mEnableWriteRecastMeshToFile) + writeToFile(*recastMesh, mSettings.get().mRecastMeshPathPrefix + std::to_string(job.mChangedTile.x()) + + "_" + std::to_string(job.mChangedTile.y()) + "_", recastMeshRevision); + if (mSettings.get().mEnableWriteNavMeshToFile) + writeToFile(job.mNavMeshCacheItem.lockConst()->getValue(), mSettings.get().mNavMeshPathPrefix, navMeshRevision); + } + + std::chrono::steady_clock::time_point AsyncNavMeshUpdater::setFirstStart(const std::chrono::steady_clock::time_point& value) + { + const auto locked = mFirstStart.lock(); + if (!*locked) + *locked = value; + return *locked.get(); + } +} diff --git a/components/detournavigator/asyncnavmeshupdater.hpp b/components/detournavigator/asyncnavmeshupdater.hpp new file mode 100644 index 000000000..39898e48e --- /dev/null +++ b/components/detournavigator/asyncnavmeshupdater.hpp @@ -0,0 +1,89 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_ASYNCNAVMESHUPDATER_H + +#include "navmeshcacheitem.hpp" +#include "offmeshconnectionsmanager.hpp" +#include "tilecachedrecastmeshmanager.hpp" +#include "tileposition.hpp" +#include "navmeshtilescache.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + enum class ChangeType + { + remove = 0, + mixed = 1, + add = 2, + update = 3, + }; + + class AsyncNavMeshUpdater + { + public: + AsyncNavMeshUpdater(const Settings& settings, TileCachedRecastMeshManager& recastMeshManager, + OffMeshConnectionsManager& offMeshConnectionsManager); + ~AsyncNavMeshUpdater(); + + void post(const osg::Vec3f& agentHalfExtents, const SharedNavMeshCacheItem& mNavMeshCacheItem, + const TilePosition& playerTile, const std::map& changedTiles); + + void wait(); + + private: + struct Job + { + osg::Vec3f mAgentHalfExtents; + SharedNavMeshCacheItem mNavMeshCacheItem; + TilePosition mChangedTile; + std::tuple mPriority; + + friend inline bool operator <(const Job& lhs, const Job& rhs) + { + return lhs.mPriority > rhs.mPriority; + } + }; + + using Jobs = std::priority_queue>; + + std::reference_wrapper mSettings; + std::reference_wrapper mRecastMeshManager; + std::reference_wrapper mOffMeshConnectionsManager; + std::atomic_bool mShouldStop; + std::mutex mMutex; + std::condition_variable mHasJob; + std::condition_variable mDone; + Jobs mJobs; + std::map> mPushed; + Misc::ScopeGuarded mPlayerTile; + Misc::ScopeGuarded> mFirstStart; + NavMeshTilesCache mNavMeshTilesCache; + std::vector mThreads; + + void process() throw(); + + void processJob(const Job& job); + + boost::optional getNextJob(); + + void writeDebugFiles(const Job& job, const RecastMesh* recastMesh) const; + + std::chrono::steady_clock::time_point setFirstStart(const std::chrono::steady_clock::time_point& value); + }; +} + +#endif diff --git a/components/detournavigator/bounds.hpp b/components/detournavigator/bounds.hpp new file mode 100644 index 000000000..a31e410cb --- /dev/null +++ b/components/detournavigator/bounds.hpp @@ -0,0 +1,20 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_BOUNDS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_BOUNDS_H + +#include + +namespace DetourNavigator +{ + struct Bounds + { + osg::Vec3f mMin; + osg::Vec3f mMax; + }; + + inline bool isEmpty(const Bounds& value) + { + return value.mMin == value.mMax; + } +} + +#endif diff --git a/components/detournavigator/cachedrecastmeshmanager.cpp b/components/detournavigator/cachedrecastmeshmanager.cpp new file mode 100644 index 000000000..5e145486b --- /dev/null +++ b/components/detournavigator/cachedrecastmeshmanager.cpp @@ -0,0 +1,63 @@ +#include "cachedrecastmeshmanager.hpp" +#include "debug.hpp" + +namespace DetourNavigator +{ + CachedRecastMeshManager::CachedRecastMeshManager(const Settings& settings, const TileBounds& bounds) + : mImpl(settings, bounds) + {} + + bool CachedRecastMeshManager::addObject(const ObjectId id, const btCollisionShape& shape, + const btTransform& transform, const AreaType areaType) + { + if (!mImpl.addObject(id, shape, transform, areaType)) + return false; + mCached.reset(); + return true; + } + + bool CachedRecastMeshManager::updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType) + { + if (!mImpl.updateObject(id, transform, areaType)) + return false; + mCached.reset(); + return true; + } + + boost::optional CachedRecastMeshManager::removeObject(const ObjectId id) + { + const auto object = mImpl.removeObject(id); + if (object) + mCached.reset(); + return object; + } + + bool CachedRecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, + const btTransform& transform) + { + if (!mImpl.addWater(cellPosition, cellSize, transform)) + return false; + mCached.reset(); + return true; + } + + boost::optional CachedRecastMeshManager::removeWater(const osg::Vec2i& cellPosition) + { + const auto water = mImpl.removeWater(cellPosition); + if (water) + mCached.reset(); + return water; + } + + std::shared_ptr CachedRecastMeshManager::getMesh() + { + if (!mCached) + mCached = mImpl.getMesh(); + return mCached; + } + + bool CachedRecastMeshManager::isEmpty() const + { + return mImpl.isEmpty(); + } +} diff --git a/components/detournavigator/cachedrecastmeshmanager.hpp b/components/detournavigator/cachedrecastmeshmanager.hpp new file mode 100644 index 000000000..528e8dabc --- /dev/null +++ b/components/detournavigator/cachedrecastmeshmanager.hpp @@ -0,0 +1,36 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_CACHEDRECASTMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_CACHEDRECASTMESHMANAGER_H + +#include "recastmeshmanager.hpp" + +#include + +namespace DetourNavigator +{ + class CachedRecastMeshManager + { + public: + CachedRecastMeshManager(const Settings& settings, const TileBounds& bounds); + + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType); + + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform); + + boost::optional removeWater(const osg::Vec2i& cellPosition); + + boost::optional removeObject(const ObjectId id); + + std::shared_ptr getMesh(); + + bool isEmpty() const; + + private: + RecastMeshManager mImpl; + std::shared_ptr mCached; + }; +} + +#endif diff --git a/components/detournavigator/chunkytrimesh.cpp b/components/detournavigator/chunkytrimesh.cpp new file mode 100644 index 000000000..3a8fc3480 --- /dev/null +++ b/components/detournavigator/chunkytrimesh.cpp @@ -0,0 +1,179 @@ +#include "chunkytrimesh.hpp" +#include "exceptions.hpp" + +#include + +#include + +namespace DetourNavigator +{ + namespace + { + struct BoundsItem + { + Rect mBounds; + std::ptrdiff_t mOffset; + unsigned char mAreaTypes; + }; + + template + 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& items, const std::size_t imin, const std::size_t imax, + Rect& bounds) + { + bounds = items[imin].mBounds; + + std::for_each( + items.begin() + static_cast(imin) + 1, + items.begin() + static_cast(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& items, const std::size_t imin, const std::size_t imax, + const std::size_t trisPerChunk, const std::vector& inIndices, const std::vector& inAreaTypes, + std::size_t& curNode, std::vector& nodes, std::size_t& curTri, + std::vector& outIndices, std::vector& outAreaTypes) + { + 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(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(curTri) * 3 + ); + outAreaTypes[curTri] = inAreaTypes[static_cast(items[i].mOffset)]; + 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(imin), + items.begin() + static_cast(imax), + LessBoundsItem<0> {} + ); + } + else + { + // Sort along y-axis + std::sort( + items.begin() + static_cast(imin), + items.begin() + static_cast(imax), + LessBoundsItem<1> {} + ); + } + + const auto isplit = imin + inum / 2; + + // Left + subdivide(items, imin, isplit, trisPerChunk, inIndices, inAreaTypes, curNode, nodes, curTri, outIndices, outAreaTypes); + // Right + subdivide(items, isplit, imax, trisPerChunk, inIndices, inAreaTypes, curNode, nodes, curTri, outIndices, outAreaTypes); + + const auto iescape = static_cast(curNode) - static_cast(icur); + // Negative index means escape. + node.mOffset = -iescape; + } + } + } + + ChunkyTriMesh::ChunkyTriMesh(const std::vector& verts, const std::vector& indices, + const std::vector& flags, 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); + mAreaTypes.resize(trianglesCount); + + // Build tree + std::vector items(trianglesCount); + + for (std::size_t i = 0; i < trianglesCount; i++) + { + auto& item = items[i]; + + item.mOffset = static_cast(i); + item.mAreaTypes = flags[i]; + + // Calc triangle XZ bounds. + const auto baseIndex = static_cast(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(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, flags, curNode, mNodes, curTri, mIndices, mAreaTypes); + + 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; + } + } +} diff --git a/components/detournavigator/chunkytrimesh.hpp b/components/detournavigator/chunkytrimesh.hpp new file mode 100644 index 000000000..9f6275ec8 --- /dev/null +++ b/components/detournavigator/chunkytrimesh.hpp @@ -0,0 +1,99 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_CHUNKYTRIMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_CHUNKYTRIMESH_H + +#include "areatype.hpp" + +#include + +#include +#include + +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 AreaType* const mAreaTypes; + 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& verts, const std::vector& tris, + const std::vector& flags, const std::size_t trisPerChunk); + + ChunkyTriMesh(const ChunkyTriMesh&) = delete; + ChunkyTriMesh& operator=(const ChunkyTriMesh&) = delete; + + /// Returns the chunk indices which overlap the input rectable. + template + void forEachChunksOverlappingRect(const Rect& rect, Function&& function) 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) + function(i); + + if (overlap || isLeafNode) + i++; + else + { + const auto escapeIndex = -node->mOffset; + i += static_cast(escapeIndex); + } + } + } + + Chunk getChunk(const std::size_t chunkId) const + { + const auto& node = mNodes[chunkId]; + return Chunk { + mIndices.data() + node.mOffset * 3, + mAreaTypes.data() + node.mOffset, + node.mSize + }; + } + + std::size_t getMaxTrisPerChunk() const + { + return mMaxTrisPerChunk; + } + + private: + std::vector mNodes; + std::vector mIndices; + std::vector mAreaTypes; + std::size_t mMaxTrisPerChunk; + }; +} + +#endif diff --git a/components/detournavigator/debug.cpp b/components/detournavigator/debug.cpp new file mode 100644 index 000000000..0ddf002d9 --- /dev/null +++ b/components/detournavigator/debug.cpp @@ -0,0 +1,102 @@ +#include "debug.hpp" +#include "exceptions.hpp" +#include "recastmesh.hpp" + +#include + +#include + +namespace DetourNavigator +{ + void writeToFile(const RecastMesh& recastMesh, const std::string& pathPrefix, const std::string& revision) + { + const auto path = pathPrefix + "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::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& pathPrefix, const std::string& revision) + { + const int navMeshSetMagic = 'M' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'MSET'; + const int navMeshSetVersion = 1; + + struct NavMeshSetHeader + { + int magic; + int version; + int numTiles; + dtNavMeshParams params; + }; + + struct NavMeshTileHeader + { + dtTileRef tileRef; + int dataSize; + }; + + const auto path = pathPrefix + "all_tiles_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 = navMeshSetMagic; + header.version = navMeshSetVersion; + 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); + } + } +} diff --git a/components/detournavigator/debug.hpp b/components/detournavigator/debug.hpp new file mode 100644 index 000000000..05d212e2f --- /dev/null +++ b/components/detournavigator/debug.hpp @@ -0,0 +1,134 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DEBUG_H + +#include "tilebounds.hpp" + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + inline std::ostream& operator <<(std::ostream& stream, const TileBounds& value) + { + return stream << "TileBounds {" << value.mMin << ", " << value.mMax << "}"; + } + + class RecastMesh; + + inline std::ostream& operator <<(std::ostream& stream, const std::chrono::steady_clock::time_point& value) + { + using float_s = std::chrono::duration>; + return stream << std::fixed << std::setprecision(4) + << std::chrono::duration_cast(value.time_since_epoch()).count(); + } + + struct Sink + { + virtual ~Sink() = default; + virtual void write(const std::string& text) = 0; + }; + + class FileSink final : public Sink + { + public: + FileSink(std::string path) + : mPath(std::move(path)) + { + mFile.exceptions(std::ios::failbit | std::ios::badbit); + } + + void write(const std::string& text) override + { + if (!mFile.is_open()) + { + mFile.open(mPath); + } + mFile << text << std::flush; + } + + private: + std::string mPath; + std::ofstream mFile; + }; + + class StdoutSink final : public Sink + { + public: + void write(const std::string& text) override + { + std::cout << text << std::flush; + } + }; + + class Log + { + public: + void setSink(std::unique_ptr sink) + { + *mSink.lock() = std::move(sink); + } + + bool isEnabled() + { + return bool(*mSink.lockConst()); + } + + void write(const std::string& text) + { + const auto sink = mSink.lock(); + if (*sink) + sink.get()->write(text); + } + + static Log& instance() + { + static Log value; + return value; + } + + private: + Misc::ScopeGuarded> mSink; + }; + + inline void write(std::ostream& stream) + { + stream << '\n'; + } + + template + void write(std::ostream& stream, const Head& head, const Tail& ... tail) + { + stream << head; + write(stream, tail ...); + } + + template + void log(Ts&& ... values) + { + auto& log = Log::instance(); + if (!log.isEnabled()) + return; + std::ostringstream stream; + stream << '[' << std::chrono::steady_clock::now() << "] [" << std::this_thread::get_id() << "] "; + write(stream, std::forward(values) ...); + log.write(stream.str()); + } + + void writeToFile(const RecastMesh& recastMesh, const std::string& pathPrefix, const std::string& revision); + void writeToFile(const dtNavMesh& navMesh, const std::string& pathPrefix, const std::string& revision); +} + +#endif diff --git a/components/detournavigator/dtstatus.hpp b/components/detournavigator/dtstatus.hpp new file mode 100644 index 000000000..a73d33be1 --- /dev/null +++ b/components/detournavigator/dtstatus.hpp @@ -0,0 +1,38 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_DTSTATUS_H + +#include + +#include +#include + +namespace DetourNavigator +{ + struct WriteDtStatus + { + dtStatus status; + }; + + static const std::vector> 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; + } +} + +#endif diff --git a/components/detournavigator/exceptions.hpp b/components/detournavigator/exceptions.hpp new file mode 100644 index 000000000..fb31172ee --- /dev/null +++ b/components/detournavigator/exceptions.hpp @@ -0,0 +1,21 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_EXCEPTIONS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_EXCEPTIONS_H + +#include + +namespace DetourNavigator +{ + struct NavigatorException : std::runtime_error + { + NavigatorException(const std::string& message) : std::runtime_error(message) {} + NavigatorException(const char* message) : std::runtime_error(message) {} + }; + + struct InvalidArgument : std::invalid_argument + { + InvalidArgument(const std::string& message) : std::invalid_argument(message) {} + InvalidArgument(const char* message) : std::invalid_argument(message) {} + }; +} + +#endif diff --git a/components/detournavigator/findsmoothpath.cpp b/components/detournavigator/findsmoothpath.cpp new file mode 100644 index 000000000..e59b80114 --- /dev/null +++ b/components/detournavigator/findsmoothpath.cpp @@ -0,0 +1,143 @@ +#include "findsmoothpath.hpp" + +#include +#include + +namespace DetourNavigator +{ + std::vector fixupCorridor(const std::vector& path, const std::vector& visited) + { + std::vector::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 result; + result.reserve(static_cast(furthestVisited - visited.rbegin()) + + static_cast(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 fixupShortcuts(const std::vector& 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 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 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 getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos, + const osg::Vec3f& endPos, const float minTargetDist, const std::vector& path) + { + // Find steer target. + SteerTarget result; + const int MAX_STEER_POINTS = 3; + std::array steerPath; + std::array steerPathFlags; + std::array 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(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(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; + } +} diff --git a/components/detournavigator/findsmoothpath.hpp b/components/detournavigator/findsmoothpath.hpp new file mode 100644 index 000000000..81b732b74 --- /dev/null +++ b/components/detournavigator/findsmoothpath.hpp @@ -0,0 +1,326 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FINDSMOOTHPATH_H + +#include "dtstatus.hpp" +#include "exceptions.hpp" +#include "flags.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" +#include "debug.hpp" + +#include +#include +#include + +#include + +#include + +#include + +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + struct Settings; + + inline osg::Vec3f makeOsgVec3f(const float* values) + { + return osg::Vec3f(values[0], values[1], values[2]); + } + + inline osg::Vec3f makeOsgVec3f(const btVector3& value) + { + return osg::Vec3f(value.x(), value.y(), value.z()); + } + + 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 fixupCorridor(const std::vector& path, const std::vector& 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 fixupShortcuts(const std::vector& path, const dtNavMeshQuery& navQuery); + + struct SteerTarget + { + osg::Vec3f steerPos; + unsigned char steerPosFlag; + dtPolyRef steerPosRef; + }; + + boost::optional getSteerTarget(const dtNavMeshQuery& navQuery, const osg::Vec3f& startPos, + const osg::Vec3f& endPos, const float minTargetDist, const std::vector& path); + + template + 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; + }; + + inline void initNavMeshQuery(dtNavMeshQuery& value, const dtNavMesh& navMesh, const int maxNodes) + { + const auto status = value.init(&navMesh, maxNodes); + if (!dtStatusSucceed(status)) + throw NavigatorException("Failed to init navmesh query"); + } + + struct MoveAlongSurfaceResult + { + osg::Vec3f mResultPos; + std::vector mVisited; + }; + + inline MoveAlongSurfaceResult moveAlongSurface(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef, + const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& filter, + const std::size_t maxVisitedSize) + { + MoveAlongSurfaceResult result; + result.mVisited.resize(maxVisitedSize); + int visitedNumber = 0; + const auto status = navMeshQuery.moveAlongSurface(startRef, startPos.ptr(), endPos.ptr(), + &filter, result.mResultPos.ptr(), result.mVisited.data(), &visitedNumber, static_cast(maxVisitedSize)); + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << "Failed to move along surface from " << startPos << " to " << endPos; + throw NavigatorException(message.str()); + } + assert(visitedNumber >= 0); + assert(visitedNumber <= static_cast(maxVisitedSize)); + result.mVisited.resize(static_cast(visitedNumber)); + return result; + } + + inline std::vector findPath(const dtNavMeshQuery& navMeshQuery, const dtPolyRef startRef, + const dtPolyRef endRef, const osg::Vec3f& startPos, const osg::Vec3f& endPos, const dtQueryFilter& queryFilter, + const std::size_t maxSize) + { + int pathLen = 0; + std::vector result(maxSize); + const auto status = navMeshQuery.findPath(startRef, endRef, startPos.ptr(), endPos.ptr(), &queryFilter, + result.data(), &pathLen, static_cast(maxSize)); + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << "Failed to find path over polygons from " << startRef << " to " << endRef; + throw NavigatorException(message.str()); + } + assert(pathLen >= 0); + assert(static_cast(pathLen) <= maxSize); + result.resize(static_cast(pathLen)); + return result; + } + + inline float getPolyHeight(const dtNavMeshQuery& navMeshQuery, const dtPolyRef ref, const osg::Vec3f& pos) + { + float result = 0.0f; + const auto status = navMeshQuery.getPolyHeight(ref, pos.ptr(), &result); + if (!dtStatusSucceed(status)) + { + std::ostringstream message; + message << "Failed to get polygon height ref=" << ref << " pos=" << pos; + throw NavigatorException(message.str()); + } + return result; + } + + template + OutputIterator makeSmoothPath(const dtNavMesh& navMesh, const dtNavMeshQuery& navMeshQuery, + const dtQueryFilter& filter, const osg::Vec3f& start, const osg::Vec3f& end, + std::vector 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; + const auto result = moveAlongSurface(navMeshQuery, polygonPath.front(), iterPos, moveTgt, filter, 16); + + polygonPath = fixupCorridor(polygonPath, result.mVisited); + polygonPath = fixupShortcuts(polygonPath, navMeshQuery); + + float h = 0; + navMeshQuery.getPolyHeight(polygonPath.front(), result.mResultPos.ptr(), &h); + iterPos = result.mResultPos; + iterPos.y() = h; + + // 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; + iterPos.y() = getPolyHeight(navMeshQuery, polygonPath.front(), iterPos); + } + } + + // Store results. + *out++ = iterPos; + ++smoothPathSize; + } + + return out; + } + + template + OutputIterator findSmoothPath(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents, + const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, + const Settings& settings, OutputIterator out) + { + dtNavMeshQuery navMeshQuery; + initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes); + + dtQueryFilter queryFilter; + queryFilter.setIncludeFlags(includeFlags); + + 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("Navmesh polygon for start point is not found"); + + 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("Navmesh polygon for end polygon is not found"); + + const auto polygonPath = findPath(navMeshQuery, startRef, endRef, start, end, queryFilter, + settings.mMaxPolygonPathSize); + + if (polygonPath.empty() || polygonPath.back() != endRef) + return out; + + makeSmoothPath(navMesh, navMeshQuery, queryFilter, start, end, std::move(polygonPath), + settings.mMaxSmoothPathSize, OutputTransformIterator(out, settings)); + + return out; + } +} + +#endif diff --git a/components/detournavigator/flags.hpp b/components/detournavigator/flags.hpp new file mode 100644 index 000000000..684d4fbba --- /dev/null +++ b/components/detournavigator/flags.hpp @@ -0,0 +1,65 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_FLAGS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_FLAGS_H + +#include + +namespace DetourNavigator +{ + using Flags = unsigned short; + + enum Flag : Flags + { + Flag_none = 0, + Flag_walk = 1 << 0, + Flag_swim = 1 << 1, + Flag_openDoor = 1 << 2, + }; + + inline std::ostream& operator <<(std::ostream& stream, const Flag value) + { + switch (value) + { + case Flag_none: + return stream << "none"; + case Flag_walk: + return stream << "walk"; + case Flag_swim: + return stream << "swim"; + case Flag_openDoor: + return stream << "openDoor"; + } + + return stream; + } + + struct WriteFlags + { + Flags mValue; + + friend inline std::ostream& operator <<(std::ostream& stream, const WriteFlags& value) + { + if (value.mValue == Flag_none) + { + return stream << Flag_none; + } + else + { + bool first = true; + for (const auto flag : {Flag_walk, Flag_swim, Flag_openDoor}) + { + if (value.mValue & flag) + { + if (!first) + stream << " | "; + first = false; + stream << flag; + } + } + + return stream; + } + } + }; +} + +#endif diff --git a/components/detournavigator/gettilespositions.hpp b/components/detournavigator/gettilespositions.hpp new file mode 100644 index 000000000..86ce77402 --- /dev/null +++ b/components/detournavigator/gettilespositions.hpp @@ -0,0 +1,73 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_GETTILESPOSITIONS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_GETTILESPOSITIONS_H + +#include "settings.hpp" +#include "settingsutils.hpp" +#include "tileposition.hpp" + +#include + +#include + +namespace DetourNavigator +{ + inline osg::Vec3f makeOsgVec3f(const btVector3& value) + { + return osg::Vec3f(value.x(), value.y(), value.z()); + } + + template + void getTilesPositions(const osg::Vec3f& aabbMin, const osg::Vec3f& aabbMax, + const Settings& settings, Callback&& callback) + { + auto min = toNavMeshCoordinates(settings, aabbMin); + auto max = toNavMeshCoordinates(settings, aabbMax); + + const auto border = getBorderSize(settings); + min -= osg::Vec3f(border, border, border); + max += osg::Vec3f(border, border, border); + + auto minTile = getTilePosition(settings, min); + auto maxTile = getTilePosition(settings, max); + + if (minTile.x() > maxTile.x()) + std::swap(minTile.x(), maxTile.x()); + + if (minTile.y() > maxTile.y()) + std::swap(minTile.y(), maxTile.y()); + + for (int tileX = minTile.x(); tileX <= maxTile.x(); ++tileX) + for (int tileY = minTile.y(); tileY <= maxTile.y(); ++tileY) + callback(TilePosition {tileX, tileY}); + } + + template + void getTilesPositions(const btCollisionShape& shape, const btTransform& transform, + const Settings& settings, Callback&& callback) + { + btVector3 aabbMin; + btVector3 aabbMax; + shape.getAabb(transform, aabbMin, aabbMax); + + getTilesPositions(makeOsgVec3f(aabbMin), makeOsgVec3f(aabbMax), settings, std::forward(callback)); + } + + template + void getTilesPositions(const int cellSize, const btTransform& transform, + const Settings& settings, Callback&& callback) + { + const auto halfCellSize = cellSize / 2; + auto aabbMin = transform(btVector3(-halfCellSize, -halfCellSize, 0)); + auto aabbMax = transform(btVector3(halfCellSize, halfCellSize, 0)); + + aabbMin.setX(std::min(aabbMin.x(), aabbMax.x())); + aabbMin.setY(std::min(aabbMin.y(), aabbMax.y())); + + aabbMax.setX(std::max(aabbMin.x(), aabbMax.x())); + aabbMax.setY(std::max(aabbMin.y(), aabbMax.y())); + + getTilesPositions(makeOsgVec3f(aabbMin), makeOsgVec3f(aabbMax), settings, std::forward(callback)); + } +} + +#endif diff --git a/components/detournavigator/makenavmesh.cpp b/components/detournavigator/makenavmesh.cpp new file mode 100644 index 000000000..3ee730386 --- /dev/null +++ b/components/detournavigator/makenavmesh.cpp @@ -0,0 +1,632 @@ +#include "makenavmesh.hpp" +#include "chunkytrimesh.hpp" +#include "debug.hpp" +#include "dtstatus.hpp" +#include "exceptions.hpp" +#include "recastmesh.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" +#include "sharednavmesh.hpp" +#include "settingsutils.hpp" +#include "flags.hpp" +#include "navmeshtilescache.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ + using namespace DetourNavigator; + + static const int doNotTransferOwnership = 0; + + 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; + + osg::Vec3f makeOsgVec3f(const btVector3& value) + { + return osg::Vec3f(value.x(), value.y(), value.z()); + } + + struct WaterBounds + { + osg::Vec3f mMin; + osg::Vec3f mMax; + }; + + WaterBounds getWaterBounds(const RecastMesh::Water& water, const Settings& settings, + const osg::Vec3f& agentHalfExtents) + { + if (water.mCellSize == std::numeric_limits::max()) + { + const auto transform = getSwimLevelTransform(settings, water.mTransform, agentHalfExtents.z()); + const auto min = toNavMeshCoordinates(settings, makeOsgVec3f(transform(btVector3(-1, -1, 0)))); + const auto max = toNavMeshCoordinates(settings, makeOsgVec3f(transform(btVector3(1, 1, 0)))); + return WaterBounds { + osg::Vec3f(-std::numeric_limits::max(), min.y(), -std::numeric_limits::max()), + osg::Vec3f(std::numeric_limits::max(), max.y(), std::numeric_limits::max()) + }; + } + else + { + const auto transform = getSwimLevelTransform(settings, water.mTransform, agentHalfExtents.z()); + const auto halfCellSize = water.mCellSize / 2.0f; + return WaterBounds { + toNavMeshCoordinates(settings, makeOsgVec3f(transform(btVector3(-halfCellSize, -halfCellSize, 0)))), + toNavMeshCoordinates(settings, makeOsgVec3f(transform(btVector3(halfCellSize, halfCellSize, 0)))) + }; + } + } + + std::vector getOffMeshVerts(const std::vector& connections) + { + std::vector result; + + result.reserve(connections.size() * 6); + + const auto add = [&] (const osg::Vec3f& v) + { + result.push_back(v.x()); + result.push_back(v.y()); + result.push_back(v.z()); + }; + + for (const auto& v : connections) + { + add(v.mStart); + add(v.mEnd); + } + + return result; + } + + rcConfig makeConfig(const osg::Vec3f& agentHalfExtents, const osg::Vec3f& boundsMin, const osg::Vec3f& boundsMax, + const Settings& settings) + { + rcConfig config; + + config.cs = settings.mCellSize; + config.ch = settings.mCellHeight; + config.walkableSlopeAngle = settings.mMaxSlope; + config.walkableHeight = static_cast(std::ceil(getHeight(settings, agentHalfExtents) / config.ch)); + config.walkableClimb = static_cast(std::floor(getMaxClimb(settings) / config.ch)); + config.walkableRadius = static_cast(std::ceil(getRadius(settings, agentHalfExtents) / config.cs)); + config.maxEdgeLen = static_cast(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 = settings.mBorderSize; + 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] -= getBorderSize(settings); + config.bmin[2] -= getBorderSize(settings); + config.bmax[0] += getBorderSize(settings); + config.bmax[2] += getBorderSize(settings); + + return config; + } + + void createHeightfield(rcContext& context, rcHeightfield& solid, int width, int height, const float* bmin, + const float* bmax, const float cs, const float ch) + { + const auto result = rcCreateHeightfield(&context, solid, width, height, bmin, bmax, cs, ch); + + if (!result) + throw NavigatorException("Failed to create heightfield for navmesh"); + } + + bool rasterizeSolidObjectsTriangles(rcContext& context, const RecastMesh& recastMesh, const rcConfig& config, + rcHeightfield& solid) + { + const auto& chunkyMesh = recastMesh.getChunkyTriMesh(); + std::vector areas(chunkyMesh.getMaxTrisPerChunk(), AreaType_null); + const osg::Vec2f tileBoundsMin(config.bmin[0], config.bmin[2]); + const osg::Vec2f tileBoundsMax(config.bmax[0], config.bmax[2]); + bool result = false; + + chunkyMesh.forEachChunksOverlappingRect(Rect {tileBoundsMin, tileBoundsMax}, + [&] (const std::size_t cid) + { + const auto chunk = chunkyMesh.getChunk(cid); + + std::fill( + areas.begin(), + std::min(areas.begin() + static_cast(chunk.mSize), + areas.end()), + AreaType_null + ); + + rcMarkWalkableTriangles( + &context, + config.walkableSlopeAngle, + recastMesh.getVertices().data(), + static_cast(recastMesh.getVerticesCount()), + chunk.mIndices, + static_cast(chunk.mSize), + areas.data() + ); + + for (std::size_t i = 0; i < chunk.mSize; ++i) + areas[i] = chunk.mAreaTypes[i]; + + rcClearUnwalkableTriangles( + &context, + config.walkableSlopeAngle, + recastMesh.getVertices().data(), + static_cast(recastMesh.getVerticesCount()), + chunk.mIndices, + static_cast(chunk.mSize), + areas.data() + ); + + const auto trianglesRasterized = rcRasterizeTriangles( + &context, + recastMesh.getVertices().data(), + static_cast(recastMesh.getVerticesCount()), + chunk.mIndices, + areas.data(), + static_cast(chunk.mSize), + solid, + config.walkableClimb + ); + + if (!trianglesRasterized) + throw NavigatorException("Failed to create rasterize triangles from recast mesh for navmesh"); + + result = true; + }); + + return result; + } + + void rasterizeWaterTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, + const Settings& settings, const rcConfig& config, rcHeightfield& solid) + { + const std::array areas {{AreaType_water, AreaType_water}}; + + for (const auto& water : recastMesh.getWater()) + { + const auto bounds = getWaterBounds(water, settings, agentHalfExtents); + + const osg::Vec2f tileBoundsMin( + std::min(config.bmax[0], std::max(config.bmin[0], bounds.mMin.x())), + std::min(config.bmax[2], std::max(config.bmin[2], bounds.mMin.z())) + ); + const osg::Vec2f tileBoundsMax( + std::min(config.bmax[0], std::max(config.bmin[0], bounds.mMax.x())), + std::min(config.bmax[2], std::max(config.bmin[2], bounds.mMax.z())) + ); + + if (tileBoundsMax == tileBoundsMin) + continue; + + const std::array vertices {{ + osg::Vec3f(tileBoundsMin.x(), bounds.mMin.y(), tileBoundsMin.y()), + osg::Vec3f(tileBoundsMin.x(), bounds.mMin.y(), tileBoundsMax.y()), + osg::Vec3f(tileBoundsMax.x(), bounds.mMin.y(), tileBoundsMax.y()), + osg::Vec3f(tileBoundsMax.x(), bounds.mMin.y(), tileBoundsMin.y()), + }}; + + std::array convertedVertices; + auto convertedVerticesIt = convertedVertices.begin(); + + for (const auto& vertex : vertices) + convertedVerticesIt = std::copy(vertex.ptr(), vertex.ptr() + 3, convertedVerticesIt); + + const std::array indices {{ + 0, 1, 2, + 0, 2, 3, + }}; + + const auto trianglesRasterized = rcRasterizeTriangles( + &context, + convertedVertices.data(), + static_cast(convertedVertices.size() / 3), + indices.data(), + areas.data(), + static_cast(areas.size()), + solid, + config.walkableClimb + ); + + if (!trianglesRasterized) + throw NavigatorException("Failed to create rasterize water triangles for navmesh"); + } + } + + bool rasterizeTriangles(rcContext& context, const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, + const rcConfig& config, const Settings& settings, rcHeightfield& solid) + { + if (!rasterizeSolidObjectsTriangles(context, recastMesh, config, solid)) + return false; + + rasterizeWaterTriangles(context, agentHalfExtents, recastMesh, settings, config, solid); + + return true; + } + + void buildCompactHeightfield(rcContext& context, const int walkableHeight, const int walkableClimb, + rcHeightfield& solid, rcCompactHeightfield& compact) + { + const auto result = rcBuildCompactHeightfield(&context, walkableHeight, + walkableClimb, solid, compact); + + if (!result) + throw NavigatorException("Failed to build compact heightfield for navmesh"); + } + + void erodeWalkableArea(rcContext& context, int walkableRadius, rcCompactHeightfield& compact) + { + const auto result = rcErodeWalkableArea(&context, walkableRadius, compact); + + if (!result) + throw NavigatorException("Failed to erode walkable area for navmesh"); + } + + void buildDistanceField(rcContext& context, rcCompactHeightfield& compact) + { + const auto result = rcBuildDistanceField(&context, compact); + + if (!result) + throw NavigatorException("Failed to build distance field for navmesh"); + } + + void buildRegions(rcContext& context, rcCompactHeightfield& compact, const int borderSize, + const int minRegionArea, const int mergeRegionArea) + { + const auto result = rcBuildRegions(&context, compact, borderSize, minRegionArea, mergeRegionArea); + + if (!result) + throw NavigatorException("Failed to build distance field for navmesh"); + } + + void buildContours(rcContext& context, rcCompactHeightfield& compact, const float maxError, const int maxEdgeLen, + rcContourSet& contourSet, const int buildFlags = RC_CONTOUR_TESS_WALL_EDGES) + { + const auto result = rcBuildContours(&context, compact, maxError, maxEdgeLen, contourSet, buildFlags); + + if (!result) + throw NavigatorException("Failed to build contours for navmesh"); + } + + void buildPolyMesh(rcContext& context, rcContourSet& contourSet, const int maxVertsPerPoly, rcPolyMesh& polyMesh) + { + const auto result = rcBuildPolyMesh(&context, contourSet, maxVertsPerPoly, polyMesh); + + if (!result) + throw NavigatorException("Failed to build poly mesh for navmesh"); + } + + void buildPolyMeshDetail(rcContext& context, const rcPolyMesh& polyMesh, const rcCompactHeightfield& compact, + const float sampleDist, const float sampleMaxError, rcPolyMeshDetail& polyMeshDetail) + { + const auto result = rcBuildPolyMeshDetail(&context, polyMesh, compact, sampleDist, sampleMaxError, + polyMeshDetail); + + if (!result) + throw NavigatorException("Failed to build detail poly mesh for navmesh"); + } + + void setPolyMeshFlags(rcPolyMesh& polyMesh) + { + for (int i = 0; i < polyMesh.npolys; ++i) + { + if (polyMesh.areas[i] == AreaType_ground) + polyMesh.flags[i] = Flag_walk; + else if (polyMesh.areas[i] == AreaType_water) + polyMesh.flags[i] = Flag_swim; + } + } + + bool fillPolyMesh(rcContext& context, const rcConfig& config, rcHeightfield& solid, rcPolyMesh& polyMesh, + rcPolyMeshDetail& polyMeshDetail) + { + rcCompactHeightfield compact; + buildCompactHeightfield(context, config.walkableHeight, config.walkableClimb, solid, compact); + + erodeWalkableArea(context, config.walkableRadius, compact); + buildDistanceField(context, compact); + buildRegions(context, compact, config.borderSize, config.minRegionArea, config.mergeRegionArea); + + rcContourSet contourSet; + buildContours(context, compact, config.maxSimplificationError, config.maxEdgeLen, contourSet); + + if (contourSet.nconts == 0) + return false; + + buildPolyMesh(context, contourSet, config.maxVertsPerPoly, polyMesh); + + buildPolyMeshDetail(context, polyMesh, compact, config.detailSampleDist, config.detailSampleMaxError, + polyMeshDetail); + + setPolyMeshFlags(polyMesh); + + return true; + } + + NavMeshData makeNavMeshTileData(const osg::Vec3f& agentHalfExtents, const RecastMesh& recastMesh, + const std::vector& offMeshConnections, const TilePosition& tile, + const osg::Vec3f& boundsMin, const osg::Vec3f& boundsMax, const Settings& settings) + { + rcContext context; + const auto config = makeConfig(agentHalfExtents, boundsMin, boundsMax, settings); + + rcHeightfield solid; + createHeightfield(context, solid, config.width, config.height, config.bmin, config.bmax, config.cs, config.ch); + + if (!rasterizeTriangles(context, agentHalfExtents, recastMesh, config, settings, solid)) + return NavMeshData(); + + 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); + if (!fillPolyMesh(context, config, solid, polyMesh, polyMeshDetail)) + return NavMeshData(); + + const auto offMeshConVerts = getOffMeshVerts(offMeshConnections); + const std::vector offMeshConRad(offMeshConnections.size(), getRadius(settings, agentHalfExtents)); + const std::vector offMeshConDir(offMeshConnections.size(), DT_OFFMESH_CON_BIDIR); + const std::vector offMeshConAreas(offMeshConnections.size(), AreaType_ground); + const std::vector offMeshConFlags(offMeshConnections.size(), Flag_openDoor); + + 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 = offMeshConVerts.data(); + params.offMeshConRad = offMeshConRad.data(); + params.offMeshConDir = offMeshConDir.data(); + params.offMeshConAreas = offMeshConAreas.data(); + params.offMeshConFlags = offMeshConFlags.data(); + params.offMeshConUserID = nullptr; + params.offMeshConCount = static_cast(offMeshConnections.size()); + 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 = tile.x(); + params.tileY = tile.y(); + params.tileLayer = 0; + + unsigned char* navMeshData; + int navMeshDataSize; + const auto navMeshDataCreated = dtCreateNavMeshData(¶ms, &navMeshData, &navMeshDataSize); + + if (!navMeshDataCreated) + throw NavigatorException("Failed to create navmesh tile data"); + + return NavMeshData(navMeshData, navMeshDataSize); + } + + UpdateNavMeshStatus makeUpdateNavMeshStatus(bool removed, bool add) + { + if (removed && add) + return UpdateNavMeshStatus::replaced; + else if (removed) + return UpdateNavMeshStatus::removed; + else if (add) + return UpdateNavMeshStatus::add; + else + return UpdateNavMeshStatus::ignore; + } + + template + unsigned long getMinValuableBitsNumber(const T value) + { + unsigned long power = 0; + while (power < sizeof(T) * 8 && (static_cast(1) << power) < value) + ++power; + return power; + } +} + +namespace DetourNavigator +{ + NavMeshPtr makeEmptyNavMesh(const Settings& settings) + { + // 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 int polysAndTilesBits = 22; + const auto polysBits = getMinValuableBitsNumber(settings.mMaxPolys); + + if (polysBits >= polysAndTilesBits) + throw InvalidArgument("Too many polygons per tile"); + + const auto tilesBits = polysAndTilesBits - polysBits; + + dtNavMeshParams params; + std::fill_n(params.orig, 3, 0.0f); + params.tileWidth = settings.mTileSize * settings.mCellSize; + params.tileHeight = settings.mTileSize * settings.mCellSize; + params.maxTiles = 1 << tilesBits; + params.maxPolys = 1 << polysBits; + + NavMeshPtr navMesh(dtAllocNavMesh(), &dtFreeNavMesh); + const auto status = navMesh->init(¶ms); + + if (!dtStatusSucceed(status)) + throw NavigatorException("Failed to init navmesh"); + + return navMesh; + } + + UpdateNavMeshStatus updateNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh* recastMesh, + const TilePosition& changedTile, const TilePosition& playerTile, + const std::vector& offMeshConnections, const Settings& settings, + const SharedNavMeshCacheItem& navMeshCacheItem, NavMeshTilesCache& navMeshTilesCache) + { + log("update NavMesh with mutiple tiles:", + " agentHeight=", std::setprecision(std::numeric_limits::max_exponent10), + getHeight(settings, agentHalfExtents), + " agentMaxClimb=", std::setprecision(std::numeric_limits::max_exponent10), + getMaxClimb(settings), + " agentRadius=", std::setprecision(std::numeric_limits::max_exponent10), + getRadius(settings, agentHalfExtents), + " changedTile=", changedTile, + " playerTile=", playerTile, + " changedTileDistance=", getDistance(changedTile, playerTile)); + + const auto params = *navMeshCacheItem.lockConst()->getValue().getParams(); + const osg::Vec3f origin(params.orig[0], params.orig[1], params.orig[2]); + + const auto x = changedTile.x(); + const auto y = changedTile.y(); + + const auto removeTile = [&] { + const auto locked = navMeshCacheItem.lock(); + auto& navMesh = locked->getValue(); + const auto tileRef = navMesh.getTileRefAt(x, y, 0); + const auto removed = dtStatusSucceed(navMesh.removeTile(tileRef, nullptr, nullptr)); + if (removed) + locked->removeUsedTile(changedTile); + return makeUpdateNavMeshStatus(removed, false); + }; + + if (!recastMesh) + { + log("ignore add tile: recastMesh is null"); + return removeTile(); + } + + auto recastMeshBounds = recastMesh->getBounds(); + + for (const auto& water : recastMesh->getWater()) + { + const auto waterBounds = getWaterBounds(water, settings, agentHalfExtents); + recastMeshBounds.mMin.y() = std::min(recastMeshBounds.mMin.y(), waterBounds.mMin.y()); + recastMeshBounds.mMax.y() = std::max(recastMeshBounds.mMax.y(), waterBounds.mMax.y()); + } + + if (isEmpty(recastMeshBounds)) + { + log("ignore add tile: recastMesh is empty"); + return removeTile(); + } + + if (!shouldAddTile(changedTile, playerTile, params.maxTiles)) + { + log("ignore add tile: too far from player"); + return removeTile(); + } + + auto cachedNavMeshData = navMeshTilesCache.get(agentHalfExtents, changedTile, *recastMesh, offMeshConnections); + + if (!cachedNavMeshData) + { + const auto tileBounds = makeTileBounds(settings, changedTile); + const osg::Vec3f tileBorderMin(tileBounds.mMin.x(), recastMeshBounds.mMin.y() - 1, tileBounds.mMin.y()); + const osg::Vec3f tileBorderMax(tileBounds.mMax.x(), recastMeshBounds.mMax.y() + 1, tileBounds.mMax.y()); + + auto navMeshData = makeNavMeshTileData(agentHalfExtents, *recastMesh, offMeshConnections, changedTile, + tileBorderMin, tileBorderMax, settings); + + if (!navMeshData.mValue) + { + log("ignore add tile: NavMeshData is null"); + return removeTile(); + } + + try + { + cachedNavMeshData = navMeshTilesCache.set(agentHalfExtents, changedTile, *recastMesh, + offMeshConnections, std::move(navMeshData)); + } + catch (const InvalidArgument&) + { + cachedNavMeshData = navMeshTilesCache.get(agentHalfExtents, changedTile, *recastMesh, + offMeshConnections); + } + + if (!cachedNavMeshData) + { + log("cache overflow"); + + const auto locked = navMeshCacheItem.lock(); + auto& navMesh = locked->getValue(); + const auto tileRef = navMesh.getTileRefAt(x, y, 0); + const auto removed = dtStatusSucceed(navMesh.removeTile(tileRef, nullptr, nullptr)); + const auto addStatus = navMesh.addTile(navMeshData.mValue.get(), navMeshData.mSize, + doNotTransferOwnership, 0, 0); + + if (dtStatusSucceed(addStatus)) + { + locked->setUsedTile(changedTile, std::move(navMeshData)); + return makeUpdateNavMeshStatus(removed, true); + } + else + { + if (removed) + locked->removeUsedTile(changedTile); + log("failed to add tile with status=", WriteDtStatus {addStatus}); + return makeUpdateNavMeshStatus(removed, false); + } + } + } + + const auto locked = navMeshCacheItem.lock(); + auto& navMesh = locked->getValue(); + const auto tileRef = navMesh.getTileRefAt(x, y, 0); + const auto removed = dtStatusSucceed(navMesh.removeTile(tileRef, nullptr, nullptr)); + const auto addStatus = navMesh.addTile(cachedNavMeshData.get().mValue, cachedNavMeshData.get().mSize, + doNotTransferOwnership, 0, 0); + + if (dtStatusSucceed(addStatus)) + { + locked->setUsedTile(changedTile, std::move(cachedNavMeshData)); + return makeUpdateNavMeshStatus(removed, true); + } + else + { + if (removed) + locked->removeUsedTile(changedTile); + log("failed to add tile with status=", WriteDtStatus {addStatus}); + return makeUpdateNavMeshStatus(removed, false); + } + } +} diff --git a/components/detournavigator/makenavmesh.hpp b/components/detournavigator/makenavmesh.hpp new file mode 100644 index 000000000..55d3e261c --- /dev/null +++ b/components/detournavigator/makenavmesh.hpp @@ -0,0 +1,55 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_MAKENAVMESH_H + +#include "offmeshconnectionsmanager.hpp" +#include "settings.hpp" +#include "navmeshcacheitem.hpp" +#include "tileposition.hpp" +#include "tilebounds.hpp" +#include "sharednavmesh.hpp" +#include "navmeshtilescache.hpp" + +#include + +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + class RecastMesh; + struct Settings; + + enum class UpdateNavMeshStatus + { + ignore, + removed, + add, + replaced + }; + + inline float getLength(const osg::Vec2i& value) + { + return std::sqrt(float(osg::square(value.x()) + osg::square(value.y()))); + } + + inline float getDistance(const TilePosition& lhs, const TilePosition& rhs) + { + return getLength(lhs - rhs); + } + + inline bool shouldAddTile(const TilePosition& changedTile, const TilePosition& playerTile, int maxTiles) + { + const auto expectedTilesCount = std::ceil(osg::PI * osg::square(getDistance(changedTile, playerTile))); + return expectedTilesCount * 3 <= maxTiles; + } + + NavMeshPtr makeEmptyNavMesh(const Settings& settings); + + UpdateNavMeshStatus updateNavMesh(const osg::Vec3f& agentHalfExtents, const RecastMesh* recastMesh, + const TilePosition& changedTile, const TilePosition& playerTile, + const std::vector& offMeshConnections, const Settings& settings, + const SharedNavMeshCacheItem& navMeshCacheItem, NavMeshTilesCache& navMeshTilesCache); +} + +#endif diff --git a/components/detournavigator/navigator.cpp b/components/detournavigator/navigator.cpp new file mode 100644 index 000000000..73537ff6f --- /dev/null +++ b/components/detournavigator/navigator.cpp @@ -0,0 +1,154 @@ +#include "navigator.hpp" +#include "debug.hpp" +#include "settingsutils.hpp" + +#include + +namespace DetourNavigator +{ + Navigator::Navigator(const Settings& settings) + : mSettings(settings) + , mNavMeshManager(mSettings) + { + } + + void Navigator::addAgent(const osg::Vec3f& agentHalfExtents) + { + ++mAgents[agentHalfExtents]; + mNavMeshManager.addAgent(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::addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform) + { + return mNavMeshManager.addObject(id, shape, transform, AreaType_ground); + } + + bool Navigator::addObject(const ObjectId id, const ObjectShapes& shapes, const btTransform& transform) + { + bool result = addObject(id, shapes.mShape, transform); + if (shapes.mAvoid) + { + const ObjectId avoidId(shapes.mAvoid); + if (mNavMeshManager.addObject(avoidId, *shapes.mAvoid, transform, AreaType_null)) + { + updateAvoidShapeId(id, avoidId); + result = true; + } + } + return result; + } + + bool Navigator::addObject(const ObjectId id, const DoorShapes& shapes, const btTransform& transform) + { + if (addObject(id, static_cast(shapes), transform)) + { + mNavMeshManager.addOffMeshConnection( + id, + toNavMeshCoordinates(mSettings, shapes.mConnectionStart), + toNavMeshCoordinates(mSettings, shapes.mConnectionEnd) + ); + return true; + } + return false; + } + + bool Navigator::updateObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform) + { + return mNavMeshManager.updateObject(id, shape, transform, AreaType_ground); + } + + bool Navigator::updateObject(const ObjectId id, const ObjectShapes& shapes, const btTransform& transform) + { + bool result = updateObject(id, shapes.mShape, transform); + if (shapes.mAvoid) + { + const ObjectId avoidId(shapes.mAvoid); + if (mNavMeshManager.updateObject(avoidId, *shapes.mAvoid, transform, AreaType_null)) + { + updateAvoidShapeId(id, avoidId); + result = true; + } + } + return result; + } + + bool Navigator::updateObject(const ObjectId id, const DoorShapes& shapes, const btTransform& transform) + { + return updateObject(id, static_cast(shapes), transform); + } + + bool Navigator::removeObject(const ObjectId id) + { + bool result = mNavMeshManager.removeObject(id); + const auto avoid = mAvoidIds.find(id); + if (avoid != mAvoidIds.end()) + result = mNavMeshManager.removeObject(avoid->second) || result; + const auto water = mWaterIds.find(id); + if (water != mWaterIds.end()) + result = mNavMeshManager.removeObject(water->second) || result; + mNavMeshManager.removeOffMeshConnection(id); + return result; + } + + bool Navigator::addWater(const osg::Vec2i& cellPosition, const int cellSize, const btScalar level, + const btTransform& transform) + { + return mNavMeshManager.addWater(cellPosition, cellSize, + btTransform(transform.getBasis(), btVector3(transform.getOrigin().x(), transform.getOrigin().y(), level))); + } + + bool Navigator::removeWater(const osg::Vec2i& cellPosition) + { + return mNavMeshManager.removeWater(cellPosition); + } + + void Navigator::update(const osg::Vec3f& playerPosition) + { + for (const auto& v : mAgents) + mNavMeshManager.update(playerPosition, v.first); + } + + void Navigator::wait() + { + mNavMeshManager.wait(); + } + + std::map Navigator::getNavMeshes() const + { + return mNavMeshManager.getNavMeshes(); + } + + const Settings& Navigator::getSettings() const + { + return mSettings; + } + + void Navigator::updateAvoidShapeId(const ObjectId id, const ObjectId avoidId) + { + updateId(id, avoidId, mWaterIds); + } + + void Navigator::updateWaterShapeId(const ObjectId id, const ObjectId waterId) + { + updateId(id, waterId, mWaterIds); + } + + void Navigator::updateId(const ObjectId id, const ObjectId updateId, std::unordered_map& ids) + { + auto inserted = ids.insert(std::make_pair(id, updateId)); + if (!inserted.second) + { + mNavMeshManager.removeObject(inserted.first->second); + inserted.first->second = updateId; + } + } +} diff --git a/components/detournavigator/navigator.hpp b/components/detournavigator/navigator.hpp new file mode 100644 index 000000000..351e0f9f8 --- /dev/null +++ b/components/detournavigator/navigator.hpp @@ -0,0 +1,205 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVIGATOR_H + +#include "findsmoothpath.hpp" +#include "flags.hpp" +#include "navmeshmanager.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" + +namespace DetourNavigator +{ + struct ObjectShapes + { + const btCollisionShape& mShape; + const btCollisionShape* mAvoid; + + ObjectShapes(const btCollisionShape& shape, const btCollisionShape* avoid) + : mShape(shape), mAvoid(avoid) + {} + }; + + struct DoorShapes : ObjectShapes + { + osg::Vec3f mConnectionStart; + osg::Vec3f mConnectionEnd; + + DoorShapes(const btCollisionShape& shape, const btCollisionShape* avoid, + const osg::Vec3f& connectionStart,const osg::Vec3f& connectionEnd) + : ObjectShapes(shape, avoid) + , mConnectionStart(connectionStart) + , mConnectionEnd(connectionEnd) + {} + }; + + /** + * @brief Top level class of detournavigator componenet. Navigator allows to build a scene with navmesh and find + * a path for an agent there. Scene contains agents, geometry objects and water. Agent are distinguished only by + * half extents. Each object has unique identifier and could be added, updated or removed. Water could be added once + * for each world cell at given level of height. Navmesh builds asynchronously in separate threads. To start build + * navmesh call update method. + */ + class Navigator + { + public: + /** + * @brief Navigator constructor initializes all internal data. Constructed object is ready to build a scene. + * @param settings allows to customize navigator work. Constructor is only place to set navigator settings. + */ + Navigator(const Settings& settings); + + /** + * @brief addAgent should be called for each agent even if all of them has same half extents. + * @param agentHalfExtents allows to setup bounding cylinder for each agent, for each different half extents + * there is different navmesh. + */ + void addAgent(const osg::Vec3f& agentHalfExtents); + + /** + * @brief removeAgent should be called for each agent even if all of them has same half extents + * @param agentHalfExtents allows determine which agent to remove + */ + void removeAgent(const osg::Vec3f& agentHalfExtents); + + /** + * @brief addObject is used to add object represented by single btCollisionShape and btTransform. + * @param id is used to distinguish different objects. + * @param shape must live until object is updated by another shape removed from Navigator. + * @param transform allows to setup object geometry according to its world state. + * @return true if object is added, false if there is already object with given id. + */ + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform); + + /** + * @brief addObject is used to add complex object with allowed to walk and avoided to walk shapes + * @param id is used to distinguish different objects + * @param shape members must live until object is updated by another shape removed from Navigator + * @param transform allows to setup objects geometry according to its world state + * @return true if object is added, false if there is already object with given id + */ + bool addObject(const ObjectId id, const ObjectShapes& shapes, const btTransform& transform); + + /** + * @brief addObject is used to add doors. + * @param id is used to distinguish different objects. + * @param shape members must live until object is updated by another shape or removed from Navigator. + * @param transform allows to setup objects geometry according to its world state. + * @return true if object is added, false if there is already object with given id. + */ + bool addObject(const ObjectId id, const DoorShapes& shapes, const btTransform& transform); + + /** + * @brief updateObject replace object geometry by given data. + * @param id is used to find object. + * @param shape must live until object is updated by another shape removed from Navigator. + * @param transform allows to setup objects geometry according to its world state. + * @return true if object is updated, false if there is no object with given id. + */ + bool updateObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform); + + /** + * @brief updateObject replace object geometry by given data. + * @param id is used to find object. + * @param shape members must live until object is updated by another shape removed from Navigator. + * @param transform allows to setup objects geometry according to its world state. + * @return true if object is updated, false if there is no object with given id. + */ + bool updateObject(const ObjectId id, const ObjectShapes& shapes, const btTransform& transform); + + /** + * @brief updateObject replace object geometry by given data. + * @param id is used to find object. + * @param shape members must live until object is updated by another shape removed from Navigator. + * @param transform allows to setup objects geometry according to its world state. + * @return true if object is updated, false if there is no object with given id. + */ + bool updateObject(const ObjectId id, const DoorShapes& shapes, const btTransform& transform); + + /** + * @brief removeObject to make it no more available at the scene. + * @param id is used to find object. + * @return true if object is removed, false if there is no object with given id. + */ + bool removeObject(const ObjectId id); + + /** + * @brief addWater is used to set water level at given world cell. + * @param cellPosition allows to distinguish cells if there is many in current world. + * @param cellSize set cell borders. std::numeric_limits::max() disables cell borders. + * @param level set z coordinate of water surface at the scene. + * @param transform set global shift of cell center. + * @return true if there was no water at given cell if cellSize != std::numeric_limits::max() or there is + * at least single object is added to the scene, false if there is already water for given cell or there is no + * any other objects. + */ + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btScalar level, + const btTransform& transform); + + /** + * @brief removeWater to make it no more available at the scene. + * @param cellPosition allows to find cell. + * @return true if there was water at given cell. + */ + bool removeWater(const osg::Vec2i& cellPosition); + + /** + * @brief update start background navmesh update using current scene state. + * @param playerPosition setup initial point to order build tiles of navmesh. + */ + void update(const osg::Vec3f& playerPosition); + + /** + * @brief wait locks thread until all tiles are updated from last update call. + */ + void wait(); + + /** + * @brief findPath fills output iterator with points of scene surfaces to be used for actor to walk through. + * @param agentHalfExtents allows to find navmesh for given actor. + * @param start path from given point. + * @param end path at given point. + * @param includeFlags setup allowed surfaces for actor to walk. + * @param out the beginning of the destination range. + * @return Output iterator to the element in the destination range, one past the last element of found path. + * Equal to out if no path is found. + * @throws InvalidArgument if there is no navmesh for given agentHalfExtents. + */ + template + OutputIterator findPath(const osg::Vec3f& agentHalfExtents, const osg::Vec3f& start, + const osg::Vec3f& end, const Flags includeFlags, OutputIterator out) const + { + static_assert( + std::is_same< + typename std::iterator_traits::iterator_category, + std::output_iterator_tag + >::value, + "out is not an OutputIterator" + ); + const auto navMesh = mNavMeshManager.getNavMesh(agentHalfExtents); + return findSmoothPath(navMesh.lock()->getValue(), toNavMeshCoordinates(mSettings, agentHalfExtents), + toNavMeshCoordinates(mSettings, start), toNavMeshCoordinates(mSettings, end), includeFlags, + mSettings, out); + } + + /** + * @brief getNavMeshes returns all current navmeshes + * @return map of agent half extents to navmesh + */ + std::map getNavMeshes() const; + + const Settings& getSettings() const; + + private: + Settings mSettings; + NavMeshManager mNavMeshManager; + std::map mAgents; + std::unordered_map mAvoidIds; + std::unordered_map mWaterIds; + + void updateAvoidShapeId(const ObjectId id, const ObjectId avoidId); + void updateWaterShapeId(const ObjectId id, const ObjectId waterId); + void updateId(const ObjectId id, const ObjectId waterId, std::unordered_map& ids); + }; +} + +#endif diff --git a/components/detournavigator/navmeshcacheitem.hpp b/components/detournavigator/navmeshcacheitem.hpp new file mode 100644 index 000000000..e64b9d138 --- /dev/null +++ b/components/detournavigator/navmeshcacheitem.hpp @@ -0,0 +1,70 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHCACHEITEM_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHCACHEITEM_H + +#include "sharednavmesh.hpp" +#include "tileposition.hpp" +#include "navmeshtilescache.hpp" + +#include + +#include + +namespace DetourNavigator +{ + class NavMeshCacheItem + { + public: + NavMeshCacheItem(const NavMeshPtr& value, std::size_t generation) + : mValue(value), mGeneration(generation), mNavMeshRevision(0) + { + } + + const dtNavMesh& getValue() const + { + return *mValue; + } + + dtNavMesh& getValue() + { + return *mValue; + } + + std::size_t getGeneration() const + { + return mGeneration; + } + + std::size_t getNavMeshRevision() const + { + return mNavMeshRevision; + } + + void setUsedTile(const TilePosition& tilePosition, NavMeshTilesCache::Value value) + { + mUsedTiles[tilePosition] = std::make_pair(std::move(value), NavMeshData()); + ++mNavMeshRevision; + } + + void setUsedTile(const TilePosition& tilePosition, NavMeshData value) + { + mUsedTiles[tilePosition] = std::make_pair(NavMeshTilesCache::Value(), std::move(value)); + ++mNavMeshRevision; + } + + void removeUsedTile(const TilePosition& tilePosition) + { + mUsedTiles.erase(tilePosition); + ++mNavMeshRevision; + } + + private: + NavMeshPtr mValue; + std::size_t mGeneration; + std::size_t mNavMeshRevision; + std::map> mUsedTiles; + }; + + using SharedNavMeshCacheItem = Misc::SharedGuarded; +} + +#endif diff --git a/components/detournavigator/navmeshdata.hpp b/components/detournavigator/navmeshdata.hpp new file mode 100644 index 000000000..8ce79614b --- /dev/null +++ b/components/detournavigator/navmeshdata.hpp @@ -0,0 +1,35 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHDATA_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHDATA_H + +#include + +#include +#include + +namespace DetourNavigator +{ + struct NavMeshDataValueDeleter + { + void operator ()(unsigned char* value) const + { + dtFree(value); + } + }; + + using NavMeshDataValue = std::unique_ptr; + + struct NavMeshData + { + NavMeshDataValue mValue; + int mSize; + + NavMeshData() = default; + + NavMeshData(unsigned char* value, int size) + : mValue(value) + , mSize(size) + {} + }; +} + +#endif diff --git a/components/detournavigator/navmeshmanager.cpp b/components/detournavigator/navmeshmanager.cpp new file mode 100644 index 000000000..66bf39aaf --- /dev/null +++ b/components/detournavigator/navmeshmanager.cpp @@ -0,0 +1,230 @@ +#include "navmeshmanager.hpp" +#include "debug.hpp" +#include "exceptions.hpp" +#include "gettilespositions.hpp" +#include "makenavmesh.hpp" +#include "navmeshcacheitem.hpp" +#include "settings.hpp" +#include "sharednavmesh.hpp" + +#include + +#include + +#include + +namespace +{ + using DetourNavigator::ChangeType; + + ChangeType addChangeType(const ChangeType current, const ChangeType add) + { + return current == add ? current : ChangeType::mixed; + } +} + +namespace DetourNavigator +{ + NavMeshManager::NavMeshManager(const Settings& settings) + : mSettings(settings) + , mRecastMeshManager(settings) + , mOffMeshConnectionsManager(settings) + , mAsyncNavMeshUpdater(settings, mRecastMeshManager, mOffMeshConnectionsManager) + {} + + bool NavMeshManager::addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + { + if (!mRecastMeshManager.addObject(id, shape, transform, areaType)) + return false; + addChangedTiles(shape, transform, ChangeType::add); + return true; + } + + bool NavMeshManager::updateObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + { + if (!mRecastMeshManager.updateObject(id, transform, areaType)) + return false; + addChangedTiles(shape, transform, ChangeType::update); + return true; + } + + bool NavMeshManager::removeObject(const ObjectId id) + { + const auto object = mRecastMeshManager.removeObject(id); + if (!object) + return false; + addChangedTiles(object->mShape, object->mTransform, ChangeType::remove); + return true; + } + + bool NavMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform) + { + if (!mRecastMeshManager.addWater(cellPosition, cellSize, transform)) + return false; + addChangedTiles(cellSize, transform, ChangeType::add); + return true; + } + + bool NavMeshManager::removeWater(const osg::Vec2i& cellPosition) + { + const auto water = mRecastMeshManager.removeWater(cellPosition); + if (!water) + return false; + addChangedTiles(water->mCellSize, water->mTransform, ChangeType::remove); + return true; + } + + void NavMeshManager::addAgent(const osg::Vec3f& agentHalfExtents) + { + auto cached = mCache.find(agentHalfExtents); + if (cached != mCache.end()) + return; + mCache.insert(std::make_pair(agentHalfExtents, + std::make_shared(makeEmptyNavMesh(mSettings), ++mGenerationCounter))); + log("cache add for agent=", agentHalfExtents); + } + + void NavMeshManager::reset(const osg::Vec3f& agentHalfExtents) + { + mCache.erase(agentHalfExtents); + } + + void NavMeshManager::addOffMeshConnection(const ObjectId id, const osg::Vec3f& start, const osg::Vec3f& end) + { + if (!mOffMeshConnectionsManager.add(id, OffMeshConnection {start, end})) + return; + + const auto startTilePosition = getTilePosition(mSettings, start); + const auto endTilePosition = getTilePosition(mSettings, end); + + addChangedTile(startTilePosition, ChangeType::add); + + if (startTilePosition != endTilePosition) + addChangedTile(endTilePosition, ChangeType::add); + } + + void NavMeshManager::removeOffMeshConnection(const ObjectId id) + { + if (const auto connection = mOffMeshConnectionsManager.remove(id)) + { + const auto startTilePosition = getTilePosition(mSettings, connection->mStart); + const auto endTilePosition = getTilePosition(mSettings, connection->mEnd); + + addChangedTile(startTilePosition, ChangeType::remove); + + if (startTilePosition != endTilePosition) + addChangedTile(endTilePosition, ChangeType::remove); + } + } + + void NavMeshManager::update(osg::Vec3f playerPosition, const osg::Vec3f& agentHalfExtents) + { + const auto playerTile = getTilePosition(mSettings, toNavMeshCoordinates(mSettings, playerPosition)); + auto& lastRevision = mLastRecastMeshManagerRevision[agentHalfExtents]; + auto lastPlayerTile = mPlayerTile.find(agentHalfExtents); + if (lastRevision >= mRecastMeshManager.getRevision() && lastPlayerTile != mPlayerTile.end() + && lastPlayerTile->second == playerTile) + return; + lastRevision = mRecastMeshManager.getRevision(); + if (lastPlayerTile == mPlayerTile.end()) + lastPlayerTile = mPlayerTile.insert(std::make_pair(agentHalfExtents, playerTile)).first; + else + lastPlayerTile->second = playerTile; + std::map tilesToPost; + const auto& cached = getCached(agentHalfExtents); + const auto changedTiles = mChangedTiles.find(agentHalfExtents); + { + const auto locked = cached.lock(); + const auto& navMesh = locked->getValue(); + if (changedTiles != mChangedTiles.end()) + { + for (const auto& tile : changedTiles->second) + if (navMesh.getTileAt(tile.first.x(), tile.first.y(), 0)) + { + auto tileToPost = tilesToPost.find(tile.first); + if (tileToPost == tilesToPost.end()) + tilesToPost.insert(tile); + else + tileToPost->second = addChangeType(tileToPost->second, tile.second); + } + for (const auto& tile : tilesToPost) + changedTiles->second.erase(tile.first); + if (changedTiles->second.empty()) + mChangedTiles.erase(changedTiles); + } + const auto maxTiles = navMesh.getParams()->maxTiles; + mRecastMeshManager.forEachTilePosition([&] (const TilePosition& tile) + { + if (tilesToPost.count(tile)) + return; + const auto shouldAdd = shouldAddTile(tile, playerTile, maxTiles); + const auto presentInNavMesh = bool(navMesh.getTileAt(tile.x(), tile.y(), 0)); + if (shouldAdd && !presentInNavMesh) + tilesToPost.insert(std::make_pair(tile, ChangeType::add)); + else if (!shouldAdd && presentInNavMesh) + tilesToPost.insert(std::make_pair(tile, ChangeType::mixed)); + }); + } + mAsyncNavMeshUpdater.post(agentHalfExtents, cached, playerTile, tilesToPost); + log("cache update posted for agent=", agentHalfExtents, + " playerTile=", lastPlayerTile->second, + " recastMeshManagerRevision=", lastRevision); + } + + void NavMeshManager::wait() + { + mAsyncNavMeshUpdater.wait(); + } + + SharedNavMeshCacheItem NavMeshManager::getNavMesh(const osg::Vec3f& agentHalfExtents) const + { + return getCached(agentHalfExtents); + } + + std::map NavMeshManager::getNavMeshes() const + { + return mCache; + } + + void NavMeshManager::addChangedTiles(const btCollisionShape& shape, const btTransform& transform, + const ChangeType changeType) + { + getTilesPositions(shape, transform, mSettings, + [&] (const TilePosition& v) { addChangedTile(v, changeType); }); + } + + void NavMeshManager::addChangedTiles(const int cellSize, const btTransform& transform, + const ChangeType changeType) + { + if (cellSize == std::numeric_limits::max()) + return; + + getTilesPositions(cellSize, transform, mSettings, + [&] (const TilePosition& v) { addChangedTile(v, changeType); }); + } + + void NavMeshManager::addChangedTile(const TilePosition& tilePosition, const ChangeType changeType) + { + for (const auto& cached : mCache) + { + auto& tiles = mChangedTiles[cached.first]; + auto tile = tiles.find(tilePosition); + if (tile == tiles.end()) + tiles.insert(std::make_pair(tilePosition, changeType)); + else + tile->second = addChangeType(tile->second, changeType); + } + } + + const SharedNavMeshCacheItem& NavMeshManager::getCached(const osg::Vec3f& agentHalfExtents) const + { + const auto cached = mCache.find(agentHalfExtents); + if (cached != mCache.end()) + return cached->second; + std::ostringstream stream; + stream << "Agent with half extents is not found: " << agentHalfExtents; + throw InvalidArgument(stream.str()); + } +} diff --git a/components/detournavigator/navmeshmanager.hpp b/components/detournavigator/navmeshmanager.hpp new file mode 100644 index 000000000..f44cdd251 --- /dev/null +++ b/components/detournavigator/navmeshmanager.hpp @@ -0,0 +1,74 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHMANAGER_H + +#include "asyncnavmeshupdater.hpp" +#include "cachedrecastmeshmanager.hpp" +#include "offmeshconnectionsmanager.hpp" +#include "sharednavmesh.hpp" + +#include + +#include + +#include +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + class NavMeshManager + { + public: + NavMeshManager(const Settings& settings); + + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool updateObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool removeObject(const ObjectId id); + + void addAgent(const osg::Vec3f& agentHalfExtents); + + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform); + + bool removeWater(const osg::Vec2i& cellPosition); + + void reset(const osg::Vec3f& agentHalfExtents); + + void addOffMeshConnection(const ObjectId id, const osg::Vec3f& start, const osg::Vec3f& end); + + void removeOffMeshConnection(const ObjectId id); + + void update(osg::Vec3f playerPosition, const osg::Vec3f& agentHalfExtents); + + void wait(); + + SharedNavMeshCacheItem getNavMesh(const osg::Vec3f& agentHalfExtents) const; + + std::map getNavMeshes() const; + + private: + const Settings& mSettings; + TileCachedRecastMeshManager mRecastMeshManager; + OffMeshConnectionsManager mOffMeshConnectionsManager; + AsyncNavMeshUpdater mAsyncNavMeshUpdater; + std::map mCache; + std::map> mChangedTiles; + std::size_t mGenerationCounter = 0; + std::map mPlayerTile; + std::map mLastRecastMeshManagerRevision; + + void addChangedTiles(const btCollisionShape& shape, const btTransform& transform, const ChangeType changeType); + + void addChangedTiles(const int cellSize, const btTransform& transform, const ChangeType changeType); + + void addChangedTile(const TilePosition& tilePosition, const ChangeType changeType); + + const SharedNavMeshCacheItem& getCached(const osg::Vec3f& agentHalfExtents) const; + }; +} + +#endif diff --git a/components/detournavigator/navmeshtilescache.cpp b/components/detournavigator/navmeshtilescache.cpp new file mode 100644 index 000000000..6bfbfb395 --- /dev/null +++ b/components/detournavigator/navmeshtilescache.cpp @@ -0,0 +1,168 @@ +#include "navmeshtilescache.hpp" +#include "exceptions.hpp" + +#include + +namespace DetourNavigator +{ + namespace + { + inline std::string makeNavMeshKey(const RecastMesh& recastMesh, + const std::vector& offMeshConnections) + { + std::string result; + result.reserve( + recastMesh.getIndices().size() * sizeof(int) + + recastMesh.getVertices().size() * sizeof(float) + + recastMesh.getAreaTypes().size() * sizeof(AreaType) + + recastMesh.getWater().size() * sizeof(RecastMesh::Water) + + offMeshConnections.size() * sizeof(OffMeshConnection) + ); + std::copy( + reinterpret_cast(recastMesh.getIndices().data()), + reinterpret_cast(recastMesh.getIndices().data() + recastMesh.getIndices().size()), + std::back_inserter(result) + ); + std::copy( + reinterpret_cast(recastMesh.getVertices().data()), + reinterpret_cast(recastMesh.getVertices().data() + recastMesh.getVertices().size()), + std::back_inserter(result) + ); + std::copy( + reinterpret_cast(recastMesh.getAreaTypes().data()), + reinterpret_cast(recastMesh.getAreaTypes().data() + recastMesh.getAreaTypes().size()), + std::back_inserter(result) + ); + std::copy( + reinterpret_cast(recastMesh.getWater().data()), + reinterpret_cast(recastMesh.getWater().data() + recastMesh.getWater().size()), + std::back_inserter(result) + ); + std::copy( + reinterpret_cast(offMeshConnections.data()), + reinterpret_cast(offMeshConnections.data() + offMeshConnections.size()), + std::back_inserter(result) + ); + return result; + } + } + + NavMeshTilesCache::NavMeshTilesCache(const std::size_t maxNavMeshDataSize) + : mMaxNavMeshDataSize(maxNavMeshDataSize), mUsedNavMeshDataSize(0), mFreeNavMeshDataSize(0) {} + + NavMeshTilesCache::Value NavMeshTilesCache::get(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, + const RecastMesh& recastMesh, const std::vector& offMeshConnections) + { + const std::lock_guard lock(mMutex); + + const auto agentValues = mValues.find(agentHalfExtents); + if (agentValues == mValues.end()) + return Value(); + + const auto tileValues = agentValues->second.find(changedTile); + if (tileValues == agentValues->second.end()) + return Value(); + + // TODO: use different function to make key to avoid unnecessary std::string allocation + const auto tile = tileValues->second.Map.find(makeNavMeshKey(recastMesh, offMeshConnections)); + if (tile == tileValues->second.Map.end()) + return Value(); + + acquireItemUnsafe(tile->second); + + return Value(*this, tile->second); + } + + NavMeshTilesCache::Value NavMeshTilesCache::set(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, + const RecastMesh& recastMesh, const std::vector& offMeshConnections, + NavMeshData&& value) + { + const auto navMeshSize = static_cast(value.mSize); + + const std::lock_guard lock(mMutex); + + if (navMeshSize > mMaxNavMeshDataSize) + return Value(); + + if (navMeshSize > mFreeNavMeshDataSize + (mMaxNavMeshDataSize - mUsedNavMeshDataSize)) + return Value(); + + const auto navMeshKey = makeNavMeshKey(recastMesh, offMeshConnections); + const auto itemSize = navMeshSize + 2 * navMeshKey.size(); + + if (itemSize > mFreeNavMeshDataSize + (mMaxNavMeshDataSize - mUsedNavMeshDataSize)) + return Value(); + + while (!mFreeItems.empty() && mUsedNavMeshDataSize + itemSize > mMaxNavMeshDataSize) + removeLeastRecentlyUsed(); + + const auto iterator = mFreeItems.emplace(mFreeItems.end(), agentHalfExtents, changedTile, navMeshKey); + // TODO: use std::string_view or some alternative to avoid navMeshKey copy into both mFreeItems and mValues + const auto emplaced = mValues[agentHalfExtents][changedTile].Map.emplace(navMeshKey, iterator); + + if (!emplaced.second) + { + mFreeItems.erase(iterator); + throw InvalidArgument("Set existing cache value"); + } + + iterator->mNavMeshData = std::move(value); + mUsedNavMeshDataSize += itemSize; + mFreeNavMeshDataSize += itemSize; + + acquireItemUnsafe(iterator); + + return Value(*this, iterator); + } + + void NavMeshTilesCache::removeLeastRecentlyUsed() + { + const auto& item = mFreeItems.back(); + + const auto agentValues = mValues.find(item.mAgentHalfExtents); + if (agentValues == mValues.end()) + return; + + const auto tileValues = agentValues->second.find(item.mChangedTile); + if (tileValues == agentValues->second.end()) + return; + + const auto value = tileValues->second.Map.find(item.mNavMeshKey); + if (value == tileValues->second.Map.end()) + return; + + mUsedNavMeshDataSize -= getSize(item); + mFreeNavMeshDataSize -= getSize(item); + mFreeItems.pop_back(); + + tileValues->second.Map.erase(value); + if (!tileValues->second.Map.empty()) + return; + + agentValues->second.erase(tileValues); + if (!agentValues->second.empty()) + return; + + mValues.erase(agentValues); + } + + void NavMeshTilesCache::acquireItemUnsafe(ItemIterator iterator) + { + if (++iterator->mUseCount > 1) + return; + + mBusyItems.splice(mBusyItems.end(), mFreeItems, iterator); + mFreeNavMeshDataSize -= getSize(*iterator); + } + + void NavMeshTilesCache::releaseItem(ItemIterator iterator) + { + if (--iterator->mUseCount > 0) + return; + + const std::lock_guard lock(mMutex); + + mFreeItems.splice(mFreeItems.begin(), mBusyItems, iterator); + mFreeNavMeshDataSize += getSize(*iterator); + } +} diff --git a/components/detournavigator/navmeshtilescache.hpp b/components/detournavigator/navmeshtilescache.hpp new file mode 100644 index 000000000..45c0a6bb3 --- /dev/null +++ b/components/detournavigator/navmeshtilescache.hpp @@ -0,0 +1,138 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHTILESCACHE_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_NAVMESHTILESCACHE_H + +#include "offmeshconnection.hpp" +#include "navmeshdata.hpp" +#include "recastmesh.hpp" +#include "tileposition.hpp" + +#include +#include +#include +#include + +namespace DetourNavigator +{ + struct NavMeshDataRef + { + unsigned char* mValue; + int mSize; + }; + + class NavMeshTilesCache + { + public: + struct Item + { + std::atomic mUseCount; + osg::Vec3f mAgentHalfExtents; + TilePosition mChangedTile; + std::string mNavMeshKey; + NavMeshData mNavMeshData; + + Item(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, std::string navMeshKey) + : mUseCount(0) + , mAgentHalfExtents(agentHalfExtents) + , mChangedTile(changedTile) + , mNavMeshKey(std::move(navMeshKey)) + {} + }; + + using ItemIterator = std::list::iterator; + + class Value + { + public: + Value() + : mOwner(nullptr), mIterator() {} + + Value(NavMeshTilesCache& owner, ItemIterator iterator) + : mOwner(&owner), mIterator(iterator) + { + } + + Value(const Value& other) = delete; + + Value(Value&& other) + : mOwner(other.mOwner), mIterator(other.mIterator) + { + other.mIterator = ItemIterator(); + } + + ~Value() + { + if (mIterator != ItemIterator()) + mOwner->releaseItem(mIterator); + } + + Value& operator =(const Value& other) = delete; + + Value& operator =(Value&& other) + { + if (mIterator == other.mIterator) + return *this; + + if (mIterator != ItemIterator()) + mOwner->releaseItem(mIterator); + + mOwner = other.mOwner; + mIterator = other.mIterator; + + other.mIterator = ItemIterator(); + + return *this; + } + + NavMeshDataRef get() const + { + return NavMeshDataRef {mIterator->mNavMeshData.mValue.get(), mIterator->mNavMeshData.mSize}; + } + + operator bool() const + { + return mIterator != ItemIterator(); + } + + private: + NavMeshTilesCache* mOwner; + ItemIterator mIterator; + }; + + NavMeshTilesCache(const std::size_t maxNavMeshDataSize); + + Value get(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, + const RecastMesh& recastMesh, const std::vector& offMeshConnections); + + Value set(const osg::Vec3f& agentHalfExtents, const TilePosition& changedTile, + const RecastMesh& recastMesh, const std::vector& offMeshConnections, + NavMeshData&& value); + + private: + + struct TileMap + { + std::map Map; + }; + + std::mutex mMutex; + std::size_t mMaxNavMeshDataSize; + std::size_t mUsedNavMeshDataSize; + std::size_t mFreeNavMeshDataSize; + std::list mBusyItems; + std::list mFreeItems; + std::map> mValues; + + void removeLeastRecentlyUsed(); + + void acquireItemUnsafe(ItemIterator iterator); + + void releaseItem(ItemIterator iterator); + + static std::size_t getSize(const Item& item) + { + return static_cast(item.mNavMeshData.mSize) + 2 * item.mNavMeshKey.size(); + } + }; +} + +#endif diff --git a/components/detournavigator/objectid.hpp b/components/detournavigator/objectid.hpp new file mode 100644 index 000000000..3b56924b1 --- /dev/null +++ b/components/detournavigator/objectid.hpp @@ -0,0 +1,50 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_OBJECTID_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_OBJECTID_H + +#include +#include + +namespace DetourNavigator +{ + class ObjectId + { + public: + template + explicit ObjectId(const T value) throw() + : mValue(reinterpret_cast(value)) + { + } + + std::size_t value() const throw() + { + return mValue; + } + + friend bool operator <(const ObjectId lhs, const ObjectId rhs) throw() + { + return lhs.mValue < rhs.mValue; + } + + friend bool operator ==(const ObjectId lhs, const ObjectId rhs) throw() + { + return lhs.mValue == rhs.mValue; + } + + private: + std::size_t mValue; + }; +} + +namespace std +{ + template <> + struct hash + { + std::size_t operator ()(const DetourNavigator::ObjectId value) const throw() + { + return value.value(); + } + }; +} + +#endif diff --git a/components/detournavigator/offmeshconnection.hpp b/components/detournavigator/offmeshconnection.hpp new file mode 100644 index 000000000..60e8ecbbb --- /dev/null +++ b/components/detournavigator/offmeshconnection.hpp @@ -0,0 +1,15 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_OFFMESHCONNECTION_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_OFFMESHCONNECTION_H + +#include + +namespace DetourNavigator +{ + struct OffMeshConnection + { + osg::Vec3f mStart; + osg::Vec3f mEnd; + }; +} + +#endif diff --git a/components/detournavigator/offmeshconnectionsmanager.hpp b/components/detournavigator/offmeshconnectionsmanager.hpp new file mode 100644 index 000000000..30d7976ae --- /dev/null +++ b/components/detournavigator/offmeshconnectionsmanager.hpp @@ -0,0 +1,115 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_OFFMESHCONNECTIONSMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_OFFMESHCONNECTIONSMANAGER_H + +#include "settings.hpp" +#include "settingsutils.hpp" +#include "tileposition.hpp" +#include "objectid.hpp" +#include "offmeshconnection.hpp" + +#include + +#include + +#include + +#include +#include +#include +#include +#include + +namespace DetourNavigator +{ + class OffMeshConnectionsManager + { + public: + OffMeshConnectionsManager(const Settings& settings) + : mSettings(settings) + {} + + bool add(const ObjectId id, const OffMeshConnection& value) + { + const auto values = mValues.lock(); + + if (!values->mById.insert(std::make_pair(id, value)).second) + return false; + + const auto startTilePosition = getTilePosition(mSettings, value.mStart); + const auto endTilePosition = getTilePosition(mSettings, value.mEnd); + + values->mByTilePosition[startTilePosition].insert(id); + + if (startTilePosition != endTilePosition) + values->mByTilePosition[endTilePosition].insert(id); + + return true; + } + + boost::optional remove(const ObjectId id) + { + const auto values = mValues.lock(); + + const auto itById = values->mById.find(id); + + if (itById == values->mById.end()) + return boost::none; + + const auto result = itById->second; + + values->mById.erase(itById); + + const auto startTilePosition = getTilePosition(mSettings, result.mStart); + const auto endTilePosition = getTilePosition(mSettings, result.mEnd); + + removeByTilePosition(values->mByTilePosition, startTilePosition, id); + + if (startTilePosition != endTilePosition) + removeByTilePosition(values->mByTilePosition, endTilePosition, id); + + return result; + } + + std::vector get(const TilePosition& tilePosition) + { + std::vector result; + + const auto values = mValues.lock(); + + const auto itByTilePosition = values->mByTilePosition.find(tilePosition); + + if (itByTilePosition == values->mByTilePosition.end()) + return result; + + std::for_each(itByTilePosition->second.begin(), itByTilePosition->second.end(), + [&] (const ObjectId v) + { + const auto itById = values->mById.find(v); + if (itById != values->mById.end()) + result.push_back(itById->second); + }); + + return result; + } + + private: + struct Values + { + std::unordered_map mById; + std::map> mByTilePosition; + }; + + const Settings& mSettings; + Misc::ScopeGuarded mValues; + + void removeByTilePosition(std::map>& valuesByTilePosition, + const TilePosition& tilePosition, const ObjectId id) + { + const auto it = valuesByTilePosition.find(tilePosition); + if (it != valuesByTilePosition.end()) + it->second.erase(id); + } + }; +} + +#endif diff --git a/components/detournavigator/recastallocutils.hpp b/components/detournavigator/recastallocutils.hpp new file mode 100644 index 000000000..7b083d139 --- /dev/null +++ b/components/detournavigator/recastallocutils.hpp @@ -0,0 +1,102 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTALLOCUTILS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTALLOCUTILS_H + +#include + +#include + +namespace DetourNavigator +{ + static_assert(sizeof(std::size_t) == sizeof(void*), ""); + + enum BufferType : std::size_t + { + BufferType_perm, + BufferType_temp, + BufferType_unused, + }; + + inline BufferType* tempPtrBufferType(void* ptr) + { + return reinterpret_cast(static_cast(ptr) + 1); + } + + inline BufferType getTempPtrBufferType(void* ptr) + { + return *tempPtrBufferType(ptr); + } + + inline void setTempPtrBufferType(void* ptr, BufferType value) + { + *tempPtrBufferType(ptr) = value; + } + + inline void** tempPtrPrev(void* ptr) + { + return static_cast(ptr); + } + + inline void* getTempPtrPrev(void* ptr) + { + return *tempPtrPrev(ptr); + } + + inline void setTempPtrPrev(void* ptr, void* value) + { + *tempPtrPrev(ptr) = value; + } + + inline void* getTempPtrDataPtr(void* ptr) + { + return reinterpret_cast(static_cast(ptr) + 2); + } + + inline BufferType* dataPtrBufferType(void* dataPtr) + { + return reinterpret_cast(static_cast(dataPtr) - 1); + } + + inline BufferType getDataPtrBufferType(void* dataPtr) + { + return *dataPtrBufferType(dataPtr); + } + + inline void setDataPtrBufferType(void* dataPtr, BufferType value) + { + *dataPtrBufferType(dataPtr) = value; + } + + inline void* getTempDataPtrStackPtr(void* dataPtr) + { + return static_cast(dataPtr) - 2; + } + + inline void* getPermDataPtrHeapPtr(void* dataPtr) + { + return static_cast(dataPtr) - 1; + } + + inline void setPermPtrBufferType(void* ptr, BufferType value) + { + *static_cast(ptr) = value; + } + + inline void* getPermPtrDataPtr(void* ptr) + { + return static_cast(ptr) + 1; + } + + // TODO: use std::align + inline void* align(std::size_t align, std::size_t size, void*& ptr, std::size_t& space) noexcept + { + const auto intptr = reinterpret_cast(ptr); + const auto aligned = (intptr - 1u + align) & - align; + const auto diff = aligned - intptr; + if ((size + diff) > space) + return nullptr; + space -= diff; + return ptr = reinterpret_cast(aligned); + } +} + +#endif diff --git a/components/detournavigator/recastglobalallocator.hpp b/components/detournavigator/recastglobalallocator.hpp new file mode 100644 index 000000000..7c4b2c534 --- /dev/null +++ b/components/detournavigator/recastglobalallocator.hpp @@ -0,0 +1,68 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTGLOBALALLOCATOR_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTGLOBALALLOCATOR_H + +#include "recasttempallocator.hpp" + +namespace DetourNavigator +{ + class RecastGlobalAllocator + { + public: + static void init() + { + instance(); + } + + static void* alloc(size_t size, rcAllocHint hint) + { + void* result = nullptr; + if (rcLikely(hint == RC_ALLOC_TEMP)) + result = tempAllocator().alloc(size); + if (rcUnlikely(!result)) + result = allocPerm(size); + return result; + } + + static void free(void* ptr) + { + if (rcUnlikely(!ptr)) + return; + if (rcLikely(BufferType_temp == getDataPtrBufferType(ptr))) + tempAllocator().free(ptr); + else + { + assert(BufferType_perm == getDataPtrBufferType(ptr)); + ::free(getPermDataPtrHeapPtr(ptr)); + } + } + + private: + RecastGlobalAllocator() + { + rcAllocSetCustom(&RecastGlobalAllocator::alloc, &RecastGlobalAllocator::free); + } + + static RecastGlobalAllocator& instance() + { + static RecastGlobalAllocator value; + return value; + } + + static RecastTempAllocator& tempAllocator() + { + static thread_local RecastTempAllocator value(1024ul * 1024ul); + return value; + } + + static void* allocPerm(size_t size) + { + const auto ptr = ::malloc(size + sizeof(std::size_t)); + if (rcUnlikely(!ptr)) + return ptr; + setPermPtrBufferType(ptr, BufferType_perm); + return getPermPtrDataPtr(ptr); + } + }; +} + +#endif diff --git a/components/detournavigator/recastmesh.cpp b/components/detournavigator/recastmesh.cpp new file mode 100644 index 000000000..476799c1f --- /dev/null +++ b/components/detournavigator/recastmesh.cpp @@ -0,0 +1,22 @@ +#include "recastmesh.hpp" +#include "exceptions.hpp" + +#include + +namespace DetourNavigator +{ + RecastMesh::RecastMesh(std::vector indices, std::vector vertices, std::vector areaTypes, + std::vector water, const std::size_t trianglesPerChunk) + : mIndices(std::move(indices)) + , mVertices(std::move(vertices)) + , mAreaTypes(std::move(areaTypes)) + , mWater(std::move(water)) + , mChunkyTriMesh(mVertices, mIndices, mAreaTypes, trianglesPerChunk) + { + if (getTrianglesCount() != mAreaTypes.size()) + throw InvalidArgument("Number of flags doesn't match number of triangles: triangles=" + + std::to_string(getTrianglesCount()) + ", areaTypes=" + std::to_string(mAreaTypes.size())); + if (getVerticesCount()) + rcCalcBounds(mVertices.data(), static_cast(getVerticesCount()), mBounds.mMin.ptr(), mBounds.mMax.ptr()); + } +} diff --git a/components/detournavigator/recastmesh.hpp b/components/detournavigator/recastmesh.hpp new file mode 100644 index 000000000..47d5f7963 --- /dev/null +++ b/components/detournavigator/recastmesh.hpp @@ -0,0 +1,80 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESH_H + +#include "areatype.hpp" +#include "chunkytrimesh.hpp" +#include "bounds.hpp" + +#include +#include +#include + +#include + +#include + +namespace DetourNavigator +{ + class RecastMesh + { + public: + struct Water + { + int mCellSize; + btTransform mTransform; + }; + + RecastMesh(std::vector indices, std::vector vertices, std::vector areaTypes, + std::vector water, const std::size_t trianglesPerChunk); + + const std::vector& getIndices() const + { + return mIndices; + } + + const std::vector& getVertices() const + { + return mVertices; + } + + const std::vector& getAreaTypes() const + { + return mAreaTypes; + } + + const std::vector& getWater() const + { + return mWater; + } + + std::size_t getVerticesCount() const + { + return mVertices.size() / 3; + } + + std::size_t getTrianglesCount() const + { + return mIndices.size() / 3; + } + + const ChunkyTriMesh& getChunkyTriMesh() const + { + return mChunkyTriMesh; + } + + const Bounds& getBounds() const + { + return mBounds; + } + + private: + std::vector mIndices; + std::vector mVertices; + std::vector mAreaTypes; + std::vector mWater; + ChunkyTriMesh mChunkyTriMesh; + Bounds mBounds; + }; +} + +#endif diff --git a/components/detournavigator/recastmeshbuilder.cpp b/components/detournavigator/recastmeshbuilder.cpp new file mode 100644 index 000000000..e325b7eaf --- /dev/null +++ b/components/detournavigator/recastmeshbuilder.cpp @@ -0,0 +1,183 @@ +#include "recastmeshbuilder.hpp" +#include "chunkytrimesh.hpp" +#include "debug.hpp" +#include "settings.hpp" +#include "settingsutils.hpp" +#include "exceptions.hpp" + +#include + +#include +#include +#include +#include +#include + +#include + +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, const TileBounds& bounds) + : mSettings(settings) + , mBounds(bounds) + { + mBounds.mMin /= mSettings.get().mRecastScaleFactor; + mBounds.mMax /= mSettings.get().mRecastScaleFactor; + } + + void RecastMeshBuilder::addObject(const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + { + if (shape.isCompound()) + return addObject(static_cast(shape), transform, areaType); + else if (shape.getShapeType() == TERRAIN_SHAPE_PROXYTYPE) + return addObject(static_cast(shape), transform, areaType); + else if (shape.isConcave()) + return addObject(static_cast(shape), transform, areaType); + else if (shape.getShapeType() == BOX_SHAPE_PROXYTYPE) + return addObject(static_cast(shape), transform, areaType); + std::ostringstream message; + message << "Unsupported shape type: " << BroadphaseNativeTypes(shape.getShapeType()); + throw InvalidArgument(message.str()); + } + + void RecastMeshBuilder::addObject(const btCompoundShape& shape, const btTransform& transform, + const AreaType areaType) + { + for (int i = 0, num = shape.getNumChildShapes(); i < num; ++i) + addObject(*shape.getChildShape(i), transform * shape.getChildTransform(i), areaType); + } + + void RecastMeshBuilder::addObject(const btConcaveShape& shape, const btTransform& transform, + const AreaType areaType) + { + return addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* triangle, int, int) + { + for (std::size_t i = 3; i > 0; --i) + addTriangleVertex(transform(triangle[i - 1])); + mAreaTypes.push_back(areaType); + })); + } + + void RecastMeshBuilder::addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform, + const AreaType areaType) + { + return addObject(shape, transform, makeProcessTriangleCallback([&] (btVector3* triangle, int, int) + { + for (std::size_t i = 0; i < 3; ++i) + addTriangleVertex(transform(triangle[i])); + mAreaTypes.push_back(areaType); + })); + } + + void RecastMeshBuilder::addObject(const btBoxShape& shape, const btTransform& transform, const AreaType areaType) + { + const auto indexOffset = static_cast(mVertices.size() / 3); + + for (int vertex = 0, count = shape.getNumVertices(); vertex < count; ++vertex) + { + btVector3 position; + shape.getVertex(vertex, position); + addVertex(transform(position)); + } + + const std::array indices {{ + 0, 2, 3, + 3, 1, 0, + 0, 4, 6, + 6, 2, 0, + 0, 1, 5, + 5, 4, 0, + 7, 5, 1, + 1, 3, 7, + 7, 3, 2, + 2, 6, 7, + 7, 6, 4, + 4, 5, 7, + }}; + + std::transform(indices.begin(), indices.end(), std::back_inserter(mIndices), + [&] (int index) { return index + indexOffset; }); + + std::generate_n(std::back_inserter(mAreaTypes), 12, [=] { return areaType; }); + } + + void RecastMeshBuilder::addWater(const int cellSize, const btTransform& transform) + { + mWater.push_back(RecastMesh::Water {cellSize, transform}); + } + + std::shared_ptr RecastMeshBuilder::create() const + { + return std::make_shared(mIndices, mVertices, mAreaTypes, mWater, mSettings.get().mTrianglesPerChunk); + } + + void RecastMeshBuilder::reset() + { + mIndices.clear(); + mVertices.clear(); + mAreaTypes.clear(); + mWater.clear(); + } + + void RecastMeshBuilder::addObject(const btConcaveShape& shape, const btTransform& transform, + btTriangleCallback&& callback) + { + btVector3 aabbMin; + btVector3 aabbMax; + + shape.getAabb(btTransform::getIdentity(), aabbMin, aabbMax); + + aabbMin = transform(aabbMin); + aabbMax = transform(aabbMax); + + aabbMin.setX(std::max(mBounds.mMin.x(), aabbMin.x())); + aabbMin.setX(std::min(mBounds.mMax.x(), aabbMin.x())); + aabbMin.setY(std::max(mBounds.mMin.y(), aabbMin.y())); + aabbMin.setY(std::min(mBounds.mMax.y(), aabbMin.y())); + + aabbMax.setX(std::max(mBounds.mMin.x(), aabbMax.x())); + aabbMax.setX(std::min(mBounds.mMax.x(), aabbMax.x())); + aabbMax.setY(std::max(mBounds.mMin.y(), aabbMax.y())); + aabbMax.setY(std::min(mBounds.mMax.y(), aabbMax.y())); + + const auto inversedTransform = transform.inverse(); + + aabbMin = inversedTransform(aabbMin); + aabbMax = inversedTransform(aabbMax); + + aabbMin.setX(std::min(aabbMin.x(), aabbMax.x())); + aabbMin.setY(std::min(aabbMin.y(), aabbMax.y())); + aabbMin.setZ(std::min(aabbMin.z(), aabbMax.z())); + + aabbMax.setX(std::max(aabbMin.x(), aabbMax.x())); + aabbMax.setY(std::max(aabbMin.y(), aabbMax.y())); + aabbMax.setZ(std::max(aabbMin.z(), aabbMax.z())); + + shape.processAllTriangles(&callback, aabbMin, aabbMax); + } + + void RecastMeshBuilder::addTriangleVertex(const btVector3& worldPosition) + { + mIndices.push_back(static_cast(mVertices.size() / 3)); + addVertex(worldPosition); + } + + void RecastMeshBuilder::addVertex(const btVector3& worldPosition) + { + const auto navMeshPosition = toNavMeshCoordinates(mSettings, makeOsgVec3f(worldPosition)); + mVertices.push_back(navMeshPosition.x()); + mVertices.push_back(navMeshPosition.y()); + mVertices.push_back(navMeshPosition.z()); + } +} diff --git a/components/detournavigator/recastmeshbuilder.hpp b/components/detournavigator/recastmeshbuilder.hpp new file mode 100644 index 000000000..2f9d0373d --- /dev/null +++ b/components/detournavigator/recastmeshbuilder.hpp @@ -0,0 +1,57 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHBUILDER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHBUILDER_H + +#include "recastmesh.hpp" +#include "tilebounds.hpp" + +#include + +class btBoxShape; +class btCollisionShape; +class btCompoundShape; +class btConcaveShape; +class btHeightfieldTerrainShape; +class btTriangleCallback; + +namespace DetourNavigator +{ + struct Settings; + + class RecastMeshBuilder + { + public: + RecastMeshBuilder(const Settings& settings, const TileBounds& bounds); + + void addObject(const btCollisionShape& shape, const btTransform& transform, const AreaType areaType); + + void addObject(const btCompoundShape& shape, const btTransform& transform, const AreaType areaType); + + void addObject(const btConcaveShape& shape, const btTransform& transform, const AreaType areaType); + + void addObject(const btHeightfieldTerrainShape& shape, const btTransform& transform, const AreaType areaType); + + void addObject(const btBoxShape& shape, const btTransform& transform, const AreaType areaType); + + void addWater(const int mCellSize, const btTransform& transform); + + std::shared_ptr create() const; + + void reset(); + + private: + std::reference_wrapper mSettings; + TileBounds mBounds; + std::vector mIndices; + std::vector mVertices; + std::vector mAreaTypes; + std::vector mWater; + + void addObject(const btConcaveShape& shape, const btTransform& transform, btTriangleCallback&& callback); + + void addTriangleVertex(const btVector3& worldPosition); + + void addVertex(const btVector3& worldPosition); + }; +} + +#endif diff --git a/components/detournavigator/recastmeshmanager.cpp b/components/detournavigator/recastmeshmanager.cpp new file mode 100644 index 000000000..1c3a72b59 --- /dev/null +++ b/components/detournavigator/recastmeshmanager.cpp @@ -0,0 +1,97 @@ +#include "recastmeshmanager.hpp" + +#include +#include + +namespace DetourNavigator +{ + RecastMeshManager::RecastMeshManager(const Settings& settings, const TileBounds& bounds) + : mShouldRebuild(false) + , mMeshBuilder(settings, bounds) + { + } + + bool RecastMeshManager::addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + { + const auto iterator = mObjectsOrder.emplace(mObjectsOrder.end(), RecastMeshObject(shape, transform, areaType)); + if (!mObjects.emplace(id, iterator).second) + { + mObjectsOrder.erase(iterator); + return false; + } + mShouldRebuild = true; + return mShouldRebuild; + } + + bool RecastMeshManager::updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType) + { + const auto object = mObjects.find(id); + if (object == mObjects.end()) + return false; + if (!object->second->update(transform, areaType)) + return false; + mShouldRebuild = true; + return mShouldRebuild; + } + + boost::optional RecastMeshManager::removeObject(const ObjectId id) + { + const auto object = mObjects.find(id); + if (object == mObjects.end()) + return boost::none; + const RemovedRecastMeshObject result {object->second->getShape(), object->second->getTransform()}; + mObjectsOrder.erase(object->second); + mObjects.erase(object); + mShouldRebuild = true; + return result; + } + + bool RecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, + const btTransform& transform) + { + const auto iterator = mWaterOrder.emplace(mWaterOrder.end(), Water {cellSize, transform}); + if (!mWater.emplace(cellPosition, iterator).second) + { + mWaterOrder.erase(iterator); + return false; + } + mShouldRebuild = true; + return true; + } + + boost::optional RecastMeshManager::removeWater(const osg::Vec2i& cellPosition) + { + const auto water = mWater.find(cellPosition); + if (water == mWater.end()) + return boost::none; + mShouldRebuild = true; + const auto result = *water->second; + mWaterOrder.erase(water->second); + mWater.erase(water); + return result; + } + + std::shared_ptr RecastMeshManager::getMesh() + { + rebuild(); + return mMeshBuilder.create(); + } + + bool RecastMeshManager::isEmpty() const + { + return mObjects.empty(); + } + + void RecastMeshManager::rebuild() + { + if (!mShouldRebuild) + return; + mMeshBuilder.reset(); + for (const auto& v : mWaterOrder) + mMeshBuilder.addWater(v.mCellSize, v.mTransform); + for (const auto& v : mObjectsOrder) + mMeshBuilder.addObject(v.getShape(), v.getTransform(), v.getAreaType()); + mShouldRebuild = false; + } +} diff --git a/components/detournavigator/recastmeshmanager.hpp b/components/detournavigator/recastmeshmanager.hpp new file mode 100644 index 000000000..f8602e514 --- /dev/null +++ b/components/detournavigator/recastmeshmanager.hpp @@ -0,0 +1,66 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHMANAGER_H + +#include "recastmeshbuilder.hpp" +#include "recastmeshobject.hpp" +#include "objectid.hpp" + +#include + +#include + +#include + +#include +#include +#include + +class btCollisionShape; + +namespace DetourNavigator +{ + struct RemovedRecastMeshObject + { + std::reference_wrapper mShape; + btTransform mTransform; + }; + + class RecastMeshManager + { + public: + struct Water + { + int mCellSize; + btTransform mTransform; + }; + + RecastMeshManager(const Settings& settings, const TileBounds& bounds); + + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType); + + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform); + + boost::optional removeWater(const osg::Vec2i& cellPosition); + + boost::optional removeObject(const ObjectId id); + + std::shared_ptr getMesh(); + + bool isEmpty() const; + + private: + bool mShouldRebuild; + RecastMeshBuilder mMeshBuilder; + std::list mObjectsOrder; + std::unordered_map::iterator> mObjects; + std::list mWaterOrder; + std::map::iterator> mWater; + + void rebuild(); + }; +} + +#endif diff --git a/components/detournavigator/recastmeshobject.cpp b/components/detournavigator/recastmeshobject.cpp new file mode 100644 index 000000000..acaf398c1 --- /dev/null +++ b/components/detournavigator/recastmeshobject.cpp @@ -0,0 +1,68 @@ +#include "recastmeshobject.hpp" + +#include + +#include + +#include +#include + +namespace DetourNavigator +{ + RecastMeshObject::RecastMeshObject(const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType) + : mShape(shape) + , mTransform(transform) + , mAreaType(areaType) + , mChildren(makeChildrenObjects(shape, mAreaType)) + { + } + + bool RecastMeshObject::update(const btTransform& transform, const AreaType areaType) + { + bool result = false; + if (!(mTransform == transform)) + { + mTransform = transform; + result = true; + } + if (mAreaType != areaType) + { + mAreaType = areaType; + result = true; + } + if (mShape.get().isCompound()) + result = updateCompoundObject(static_cast(mShape.get()), mAreaType, mChildren) + || result; + return result; + } + + bool RecastMeshObject::updateCompoundObject(const btCompoundShape& shape, + const AreaType areaType, std::vector& children) + { + assert(static_cast(shape.getNumChildShapes()) == children.size()); + bool result = false; + for (int i = 0, num = shape.getNumChildShapes(); i < num; ++i) + { + assert(shape.getChildShape(i) == std::addressof(children[static_cast(i)].mShape.get())); + result = children[static_cast(i)].update(shape.getChildTransform(i), areaType) || result; + } + return result; + } + + std::vector makeChildrenObjects(const btCollisionShape& shape, const AreaType areaType) + { + if (shape.isCompound()) + return makeChildrenObjects(static_cast(shape), areaType); + else + return std::vector(); + } + + std::vector makeChildrenObjects(const btCompoundShape& shape, const AreaType areaType) + { + std::vector result; + for (int i = 0, num = shape.getNumChildShapes(); i < num; ++i) + result.emplace_back(*shape.getChildShape(i), shape.getChildTransform(i), areaType); + return result; + } +} diff --git a/components/detournavigator/recastmeshobject.hpp b/components/detournavigator/recastmeshobject.hpp new file mode 100644 index 000000000..aff468122 --- /dev/null +++ b/components/detournavigator/recastmeshobject.hpp @@ -0,0 +1,53 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHOBJECT_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTMESHOBJECT_H + +#include "areatype.hpp" + +#include + +#include +#include + +class btCollisionShape; +class btCompoundShape; + +namespace DetourNavigator +{ + class RecastMeshObject + { + public: + RecastMeshObject(const btCollisionShape& shape, const btTransform& transform, const AreaType areaType); + + bool update(const btTransform& transform, const AreaType areaType); + + const btCollisionShape& getShape() const + { + return mShape; + } + + const btTransform& getTransform() const + { + return mTransform; + } + + AreaType getAreaType() const + { + return mAreaType; + } + + private: + std::reference_wrapper mShape; + btTransform mTransform; + AreaType mAreaType; + std::vector mChildren; + + static bool updateCompoundObject(const btCompoundShape& shape, const AreaType areaType, + std::vector& children); + }; + + std::vector makeChildrenObjects(const btCollisionShape& shape, const AreaType areaType); + + std::vector makeChildrenObjects(const btCompoundShape& shape, const AreaType areaType); +} + +#endif diff --git a/components/detournavigator/recasttempallocator.hpp b/components/detournavigator/recasttempallocator.hpp new file mode 100644 index 000000000..e369b4224 --- /dev/null +++ b/components/detournavigator/recasttempallocator.hpp @@ -0,0 +1,65 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTTEMPALLOCATOR_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RECASTTEMPALLOCATOR_H + +#include "recastallocutils.hpp" + +#include +#include +#include + +namespace DetourNavigator +{ + class RecastTempAllocator + { + public: + RecastTempAllocator(std::size_t capacity) + : mStack(capacity), mTop(mStack.data()), mPrev(nullptr) + {} + + void* alloc(std::size_t size) + { + std::size_t space = mStack.size() - getUsedSize(); + void* top = mTop; + const auto itemSize = 2 * sizeof(std::size_t) + size; + if (rcUnlikely(!align(sizeof(std::size_t), itemSize, top, space))) + return nullptr; + setTempPtrBufferType(top, BufferType_temp); + setTempPtrPrev(top, mPrev); + mTop = static_cast(top) + itemSize; + mPrev = static_cast(top); + return getTempPtrDataPtr(top); + } + + void free(void* ptr) + { + if (rcUnlikely(!ptr)) + return; + assert(BufferType_temp == getDataPtrBufferType(ptr)); + if (!mPrev || getTempDataPtrStackPtr(ptr) != mPrev) + { + setDataPtrBufferType(ptr, BufferType_unused); + return; + } + mTop = getTempDataPtrStackPtr(ptr); + mPrev = getTempPtrPrev(mTop); + while (mPrev && BufferType_unused == getTempPtrBufferType(mPrev)) + { + mTop = mPrev; + mPrev = getTempPtrPrev(mTop); + } + return; + } + + private: + std::vector mStack; + void* mTop; + void* mPrev; + + std::size_t getUsedSize() const + { + return static_cast(static_cast(mTop) - mStack.data()); + } + }; +} + +#endif diff --git a/components/detournavigator/settings.hpp b/components/detournavigator/settings.hpp new file mode 100644 index 000000000..3e537c2fb --- /dev/null +++ b/components/detournavigator/settings.hpp @@ -0,0 +1,41 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGS_H + +#include + +namespace DetourNavigator +{ + struct Settings + { + bool mEnableWriteRecastMeshToFile; + bool mEnableWriteNavMeshToFile; + bool mEnableRecastMeshFileNameRevision; + bool mEnableNavMeshFileNameRevision; + float mCellHeight; + float mCellSize; + float mDetailSampleDist; + float mDetailSampleMaxError; + float mMaxClimb; + float mMaxSimplificationError; + float mMaxSlope; + float mRecastScaleFactor; + float mSwimHeightScale; + int mBorderSize; + int mMaxEdgeLen; + int mMaxNavMeshQueryNodes; + int mMaxPolys; + int mMaxVertsPerPoly; + int mRegionMergeSize; + int mRegionMinSize; + int mTileSize; + std::size_t mAsyncNavMeshUpdaterThreads; + std::size_t mMaxNavMeshTilesCacheSize; + std::size_t mMaxPolygonPathSize; + std::size_t mMaxSmoothPathSize; + std::size_t mTrianglesPerChunk; + std::string mRecastMeshPathPrefix; + std::string mNavMeshPathPrefix; + }; +} + +#endif diff --git a/components/detournavigator/settingsutils.hpp b/components/detournavigator/settingsutils.hpp new file mode 100644 index 000000000..d96ea53cc --- /dev/null +++ b/components/detournavigator/settingsutils.hpp @@ -0,0 +1,89 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGSUTILS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SETTINGSUTILS_H + +#include "settings.hpp" +#include "tilebounds.hpp" +#include "tileposition.hpp" +#include "tilebounds.hpp" + +#include + +#include +#include +#include + +#include + +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(std::floor(position.x() / getTileSize(settings))), + static_cast(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), + }; + } + + inline float getBorderSize(const Settings& settings) + { + return settings.mBorderSize * settings.mCellSize; + } + + inline float getSwimLevel(const Settings& settings, const float agentHalfExtentsZ) + { + return - settings.mSwimHeightScale * agentHalfExtentsZ; + } + + inline btTransform getSwimLevelTransform(const Settings& settings, const btTransform& transform, + const float agentHalfExtentsZ) + { + return btTransform( + transform.getBasis(), + transform.getOrigin() + btVector3(0, 0, getSwimLevel(settings, agentHalfExtentsZ) - agentHalfExtentsZ) + ); + } +} + +#endif diff --git a/components/detournavigator/sharednavmesh.hpp b/components/detournavigator/sharednavmesh.hpp new file mode 100644 index 000000000..beb0a3c41 --- /dev/null +++ b/components/detournavigator/sharednavmesh.hpp @@ -0,0 +1,13 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_SHAREDNAVMESH_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_SHAREDNAVMESH_H + +#include + +class dtNavMesh; + +namespace DetourNavigator +{ + using NavMeshPtr = std::shared_ptr; +} + +#endif diff --git a/components/detournavigator/tilebounds.hpp b/components/detournavigator/tilebounds.hpp new file mode 100644 index 000000000..83fe2b629 --- /dev/null +++ b/components/detournavigator/tilebounds.hpp @@ -0,0 +1,15 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEBOUNDS_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEBOUNDS_H + +#include + +namespace DetourNavigator +{ + struct TileBounds + { + osg::Vec2f mMin; + osg::Vec2f mMax; + }; +} + +#endif diff --git a/components/detournavigator/tilecachedrecastmeshmanager.cpp b/components/detournavigator/tilecachedrecastmeshmanager.cpp new file mode 100644 index 000000000..d624800e9 --- /dev/null +++ b/components/detournavigator/tilecachedrecastmeshmanager.cpp @@ -0,0 +1,175 @@ +#include "tilecachedrecastmeshmanager.hpp" +#include "makenavmesh.hpp" +#include "gettilespositions.hpp" +#include "settingsutils.hpp" + +namespace DetourNavigator +{ + TileCachedRecastMeshManager::TileCachedRecastMeshManager(const Settings& settings) + : mSettings(settings) + {} + + bool TileCachedRecastMeshManager::addObject(const ObjectId id, const btCollisionShape& shape, + const btTransform& transform, const AreaType areaType) + { + bool result = false; + auto& tilesPositions = mObjectsTilesPositions[id]; + const auto border = getBorderSize(mSettings); + getTilesPositions(shape, transform, mSettings, [&] (const TilePosition& tilePosition) + { + const auto tiles = mTiles.lock(); + auto tile = tiles->find(tilePosition); + if (tile == tiles->end()) + { + auto tileBounds = makeTileBounds(mSettings, tilePosition); + tileBounds.mMin -= osg::Vec2f(border, border); + tileBounds.mMax += osg::Vec2f(border, border); + tile = tiles->insert(std::make_pair(tilePosition, + CachedRecastMeshManager(mSettings, tileBounds))).first; + } + if (tile->second.addObject(id, shape, transform, areaType)) + { + tilesPositions.push_back(tilePosition); + result = true; + } + }); + if (result) + ++mRevision; + return result; + } + + bool TileCachedRecastMeshManager::updateObject(const ObjectId id, const btTransform& transform, + const AreaType areaType) + { + const auto object = mObjectsTilesPositions.find(id); + if (object == mObjectsTilesPositions.end()) + return false; + bool result = false; + { + const auto tiles = mTiles.lock(); + for (const auto& tilePosition : object->second) + { + const auto tile = tiles->find(tilePosition); + if (tile != tiles->end()) + result = tile->second.updateObject(id, transform, areaType) || result; + } + } + if (result) + ++mRevision; + return result; + } + + boost::optional TileCachedRecastMeshManager::removeObject(const ObjectId id) + { + const auto object = mObjectsTilesPositions.find(id); + if (object == mObjectsTilesPositions.end()) + return boost::none; + boost::optional result; + for (const auto& tilePosition : object->second) + { + const auto tiles = mTiles.lock(); + const auto tile = tiles->find(tilePosition); + if (tile == tiles->end()) + continue; + const auto tileResult = tile->second.removeObject(id); + if (tile->second.isEmpty()) + tiles->erase(tile); + if (tileResult && !result) + result = tileResult; + } + if (result) + ++mRevision; + return result; + } + + bool TileCachedRecastMeshManager::addWater(const osg::Vec2i& cellPosition, const int cellSize, + const btTransform& transform) + { + const auto border = getBorderSize(mSettings); + + auto& tilesPositions = mWaterTilesPositions[cellPosition]; + + bool result = false; + + if (cellSize == std::numeric_limits::max()) + { + const auto tiles = mTiles.lock(); + for (auto& tile : *tiles) + { + if (tile.second.addWater(cellPosition, cellSize, transform)) + { + tilesPositions.push_back(tile.first); + result = true; + } + } + } + else + { + getTilesPositions(cellSize, transform, mSettings, [&] (const TilePosition& tilePosition) + { + const auto tiles = mTiles.lock(); + auto tile = tiles->find(tilePosition); + if (tile == tiles->end()) + { + auto tileBounds = makeTileBounds(mSettings, tilePosition); + tileBounds.mMin -= osg::Vec2f(border, border); + tileBounds.mMax += osg::Vec2f(border, border); + tile = tiles->insert(std::make_pair(tilePosition, + CachedRecastMeshManager(mSettings, tileBounds))).first; + } + if (tile->second.addWater(cellPosition, cellSize, transform)) + { + tilesPositions.push_back(tilePosition); + result = true; + } + }); + } + + if (result) + ++mRevision; + + return result; + } + + boost::optional TileCachedRecastMeshManager::removeWater(const osg::Vec2i& cellPosition) + { + const auto object = mWaterTilesPositions.find(cellPosition); + if (object == mWaterTilesPositions.end()) + return boost::none; + boost::optional result; + for (const auto& tilePosition : object->second) + { + const auto tiles = mTiles.lock(); + const auto tile = tiles->find(tilePosition); + if (tile == tiles->end()) + continue; + const auto tileResult = tile->second.removeWater(cellPosition); + if (tile->second.isEmpty()) + tiles->erase(tile); + if (tileResult && !result) + result = tileResult; + } + if (result) + ++mRevision; + return result; + } + + std::shared_ptr TileCachedRecastMeshManager::getMesh(const TilePosition& tilePosition) + { + const auto tiles = mTiles.lock(); + const auto it = tiles->find(tilePosition); + if (it == tiles->end()) + return nullptr; + return it->second.getMesh(); + } + + bool TileCachedRecastMeshManager::hasTile(const TilePosition& tilePosition) + { + return mTiles.lockConst()->count(tilePosition); + } + + std::size_t TileCachedRecastMeshManager::getRevision() const + { + return mRevision; + } +} diff --git a/components/detournavigator/tilecachedrecastmeshmanager.hpp b/components/detournavigator/tilecachedrecastmeshmanager.hpp new file mode 100644 index 000000000..f82ef85c5 --- /dev/null +++ b/components/detournavigator/tilecachedrecastmeshmanager.hpp @@ -0,0 +1,52 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_TILECACHEDRECASTMESHMANAGER_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_TILECACHEDRECASTMESHMANAGER_H + +#include "cachedrecastmeshmanager.hpp" +#include "tileposition.hpp" + +#include + +#include +#include + +namespace DetourNavigator +{ + class TileCachedRecastMeshManager + { + public: + TileCachedRecastMeshManager(const Settings& settings); + + bool addObject(const ObjectId id, const btCollisionShape& shape, const btTransform& transform, + const AreaType areaType); + + bool updateObject(const ObjectId id, const btTransform& transform, const AreaType areaType); + + boost::optional removeObject(const ObjectId id); + + bool addWater(const osg::Vec2i& cellPosition, const int cellSize, const btTransform& transform); + + boost::optional removeWater(const osg::Vec2i& cellPosition); + + std::shared_ptr getMesh(const TilePosition& tilePosition); + + bool hasTile(const TilePosition& tilePosition); + + template + void forEachTilePosition(Function&& function) + { + for (const auto& tile : *mTiles.lock()) + function(tile.first); + } + + std::size_t getRevision() const; + + private: + const Settings& mSettings; + Misc::ScopeGuarded> mTiles; + std::unordered_map> mObjectsTilesPositions; + std::map> mWaterTilesPositions; + std::size_t mRevision = 0; + }; +} + +#endif diff --git a/components/detournavigator/tileposition.hpp b/components/detournavigator/tileposition.hpp new file mode 100644 index 000000000..ad7b226b6 --- /dev/null +++ b/components/detournavigator/tileposition.hpp @@ -0,0 +1,11 @@ +#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEPOSITION_H +#define OPENMW_COMPONENTS_DETOURNAVIGATOR_TILEPOSITION_H + +#include + +namespace DetourNavigator +{ + using TilePosition = osg::Vec2i; +} + +#endif diff --git a/components/misc/guarded.hpp b/components/misc/guarded.hpp new file mode 100644 index 000000000..4cb0564b1 --- /dev/null +++ b/components/misc/guarded.hpp @@ -0,0 +1,115 @@ +#ifndef OPENMW_COMPONENTS_MISC_GUARDED_H +#define OPENMW_COMPONENTS_MISC_GUARDED_H + +#include +#include + +namespace Misc +{ + template + class Locked + { + public: + Locked(std::mutex& mutex, T& value) + : mLock(mutex), mValue(value) + {} + + T& get() const + { + return mValue.get(); + } + + T* operator ->() const + { + return std::addressof(get()); + } + + T& operator *() const + { + return get(); + } + + private: + std::unique_lock mLock; + std::reference_wrapper mValue; + }; + + template + class ScopeGuarded + { + public: + ScopeGuarded() + : mMutex() + , mValue() + {} + + ScopeGuarded(const T& value) + : mMutex() + , mValue(value) + {} + + ScopeGuarded(T&& value) + : mMutex() + , mValue(std::move(value)) + {} + + template + ScopeGuarded(Args&& ... args) + : mMutex() + , mValue(std::forward(args) ...) + {} + + ScopeGuarded(const ScopeGuarded& other) + : mMutex() + , mValue(other.lock().get()) + {} + + ScopeGuarded(ScopeGuarded&& other) + : mMutex() + , mValue(std::move(other.lock().get())) + {} + + Locked lock() + { + return Locked(mMutex, mValue); + } + + Locked lockConst() + { + return Locked(mMutex, mValue); + } + + private: + std::mutex mMutex; + T mValue; + }; + + template + class SharedGuarded + { + public: + SharedGuarded() + : mMutex(std::make_shared()), mValue() + {} + + SharedGuarded(std::shared_ptr value) + : mMutex(std::make_shared()), mValue(std::move(value)) + {} + + Locked lock() const + { + return Locked(*mMutex, *mValue); + } + + Locked lockConst() const + { + return Locked(*mMutex, *mValue); + } + + private: + std::shared_ptr mMutex; + std::shared_ptr mValue; + }; +} + +#endif diff --git a/components/nifbullet/bulletnifloader.cpp b/components/nifbullet/bulletnifloader.cpp index 20307f252..1a526c63b 100644 --- a/components/nifbullet/bulletnifloader.cpp +++ b/components/nifbullet/bulletnifloader.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include @@ -42,27 +41,41 @@ bool pathFileNameStartsWithX(const std::string& path) return letterPos < path.size() && (path[letterPos] == 'x' || path[letterPos] == 'X'); } +void fillTriangleMeshWithTransform(btTriangleMesh& mesh, const Nif::NiTriShapeData& data, const osg::Matrixf &transform) +{ + mesh.preallocateVertices(static_cast(data.vertices.size())); + mesh.preallocateIndices(static_cast(data.triangles.size())); + + const std::vector &vertices = data.vertices; + const std::vector &triangles = data.triangles; + + for (std::size_t i = 0; i < triangles.size(); i += 3) + { + mesh.addTriangle( + getbtVector(vertices[triangles[i + 0]] * transform), + getbtVector(vertices[triangles[i + 1]] * transform), + getbtVector(vertices[triangles[i + 2]] * transform) + ); + } } -namespace NifBullet +void fillTriangleMesh(btTriangleMesh& mesh, const Nif::NiTriShapeData& data) { + fillTriangleMeshWithTransform(mesh, data, osg::Matrixf()); +} -BulletNifLoader::BulletNifLoader() - : mCompoundShape() - , mStaticMesh() -{ } -BulletNifLoader::~BulletNifLoader() +namespace NifBullet { -} osg::ref_ptr BulletNifLoader::load(const Nif::File& nif) { mShape = new Resource::BulletShape; - mCompoundShape = nullptr; - mStaticMesh = nullptr; + mCompoundShape.reset(); + mStaticMesh.reset(); + mAvoidStaticMesh.reset(); if (nif.numRoots() < 1) { @@ -111,7 +124,9 @@ osg::ref_ptr BulletNifLoader::load(const Nif::File& nif) { btTransform trans; trans.setIdentity(); - mCompoundShape->addChildShape(trans, new Resource::TriangleMeshShape(mStaticMesh.get(), true)); + std::unique_ptr child(new Resource::TriangleMeshShape(mStaticMesh.get(), true)); + mCompoundShape->addChildShape(trans, child.get()); + child.release(); mStaticMesh.release(); } mShape->mCollisionShape = mCompoundShape.release(); @@ -122,22 +137,26 @@ osg::ref_ptr BulletNifLoader::load(const Nif::File& nif) mStaticMesh.release(); } + if (mAvoidStaticMesh) + { + mShape->mAvoidCollisionShape = new Resource::TriangleMeshShape(mAvoidStaticMesh.get(), false); + mAvoidStaticMesh.release(); + } + return mShape; } } // Find a boundingBox in the node hierarchy. // Return: use bounding box for collision? -bool BulletNifLoader::findBoundingBox(const Nif::Node* node, int flags) +bool BulletNifLoader::findBoundingBox(const Nif::Node* node) { - flags |= node->flags; - if (node->hasBounds) { mShape->mCollisionBoxHalfExtents = node->boundXYZ; mShape->mCollisionBoxTranslate = node->boundPos; - if (flags & Nif::NiNode::Flag_BBoxCollision) + if (node->flags & Nif::NiNode::Flag_BBoxCollision) { return true; } @@ -179,7 +198,7 @@ bool BulletNifLoader::hasAutoGeneratedCollision(const Nif::Node* rootNode) } void BulletNifLoader::handleNode(const std::string& fileName, const Nif::Node *node, int flags, - bool isCollisionNode, bool isAnimated, bool autogenerated) + bool isCollisionNode, bool isAnimated, bool autogenerated, bool avoid) { // Accumulate the flags from all the child nodes. This works for all // the flags we currently use, at least. @@ -192,8 +211,7 @@ void BulletNifLoader::handleNode(const std::string& fileName, const Nif::Node *n isCollisionNode = isCollisionNode || (node->recType == Nif::RC_RootCollisionNode); // Don't collide with AvoidNode shapes - if(node->recType == Nif::RC_AvoidNode) - flags |= 0x800; + avoid = avoid || (node->recType == Nif::RC_AvoidNode); // We encountered a RootCollisionNode inside autogenerated mesh. It is not right. if (node->recType == Nif::RC_RootCollisionNode && autogenerated) @@ -234,7 +252,7 @@ void BulletNifLoader::handleNode(const std::string& fileName, const Nif::Node *n // (occurs in tr_ex_imp_wall_arch_04.nif) if(!node->hasBounds && node->recType == Nif::RC_NiTriShape) { - handleNiTriShape(static_cast(node), flags, getWorldTransform(node), isAnimated); + handleNiTriShape(static_cast(node), flags, getWorldTransform(node), isAnimated, avoid); } } @@ -246,12 +264,13 @@ void BulletNifLoader::handleNode(const std::string& fileName, const Nif::Node *n for(size_t i = 0;i < list.length();i++) { if(!list[i].empty()) - handleNode(fileName, list[i].getPtr(), flags, isCollisionNode, isAnimated, autogenerated); + handleNode(fileName, list[i].getPtr(), flags, isCollisionNode, isAnimated, autogenerated, avoid); } } } -void BulletNifLoader::handleNiTriShape(const Nif::NiTriShape *shape, int flags, const osg::Matrixf &transform, bool isAnimated) +void BulletNifLoader::handleNiTriShape(const Nif::NiTriShape *shape, int flags, const osg::Matrixf &transform, + bool isAnimated, bool avoid) { assert(shape != nullptr); @@ -277,21 +296,7 @@ void BulletNifLoader::handleNiTriShape(const Nif::NiTriShape *shape, int flags, std::unique_ptr childMesh(new btTriangleMesh); - const Nif::NiTriShapeData *data = shape->data.getPtr(); - - childMesh->preallocateVertices(data->vertices.size()); - childMesh->preallocateIndices(data->triangles.size()); - - const std::vector &vertices = data->vertices; - const std::vector &triangles = data->triangles; - - for(size_t i = 0;i < data->triangles.size();i+=3) - { - osg::Vec3f b1 = vertices[triangles[i+0]]; - osg::Vec3f b2 = vertices[triangles[i+1]]; - osg::Vec3f b3 = vertices[triangles[i+2]]; - childMesh->addTriangle(getbtVector(b1), getbtVector(b2), getbtVector(b3)); - } + fillTriangleMesh(*childMesh, shape->data.get()); std::unique_ptr childShape(new Resource::TriangleMeshShape(childMesh.get(), true)); childMesh.release(); @@ -314,27 +319,20 @@ void BulletNifLoader::handleNiTriShape(const Nif::NiTriShape *shape, int flags, mCompoundShape->addChildShape(trans, childShape.get()); childShape.release(); } + else if (avoid) + { + if (!mAvoidStaticMesh) + mAvoidStaticMesh.reset(new btTriangleMesh(false)); + + fillTriangleMeshWithTransform(*mAvoidStaticMesh, shape->data.get(), transform); + } else { if (!mStaticMesh) mStaticMesh.reset(new btTriangleMesh(false)); // Static shape, just transform all vertices into position - const Nif::NiTriShapeData *data = shape->data.getPtr(); - const std::vector &vertices = data->vertices; - const std::vector &triangles = data->triangles; - - mStaticMesh->preallocateVertices(data->vertices.size()); - mStaticMesh->preallocateIndices(data->triangles.size()); - - size_t numtris = data->triangles.size(); - for(size_t i = 0;i < numtris;i+=3) - { - osg::Vec3f b1 = vertices[triangles[i+0]]*transform; - osg::Vec3f b2 = vertices[triangles[i+1]]*transform; - osg::Vec3f b3 = vertices[triangles[i+2]]*transform; - mStaticMesh->addTriangle(getbtVector(b1), getbtVector(b2), getbtVector(b3)); - } + fillTriangleMeshWithTransform(*mStaticMesh, shape->data.get(), transform); } } diff --git a/components/nifbullet/bulletnifloader.hpp b/components/nifbullet/bulletnifloader.hpp index f67ab402e..de7e6bdcd 100644 --- a/components/nifbullet/bulletnifloader.hpp +++ b/components/nifbullet/bulletnifloader.hpp @@ -11,6 +11,8 @@ #include #include +#include + #include #include #include @@ -35,10 +37,6 @@ namespace NifBullet class BulletNifLoader { public: - BulletNifLoader(); - - virtual ~BulletNifLoader(); - void warn(const std::string &msg) { Log(Debug::Warning) << "NIFLoader: Warn:" << msg; @@ -53,18 +51,21 @@ public: osg::ref_ptr load(const Nif::File& file); private: - bool findBoundingBox(const Nif::Node* node, int flags = 0); + bool findBoundingBox(const Nif::Node* node); - void handleNode(const std::string& fileName, Nif::Node const *node, int flags, bool isCollisionNode, bool isAnimated=false, bool autogenerated=false); + void handleNode(const std::string& fileName, Nif::Node const *node, int flags, bool isCollisionNode, + bool isAnimated=false, bool autogenerated=false, bool avoid=false); bool hasAutoGeneratedCollision(const Nif::Node *rootNode); - void handleNiTriShape(const Nif::NiTriShape *shape, int flags, const osg::Matrixf& transform, bool isAnimated); + void handleNiTriShape(const Nif::NiTriShape *shape, int flags, const osg::Matrixf& transform, bool isAnimated, bool avoid); std::unique_ptr mCompoundShape; std::unique_ptr mStaticMesh; + std::unique_ptr mAvoidStaticMesh; + osg::ref_ptr mShape; }; diff --git a/components/resource/bulletshape.cpp b/components/resource/bulletshape.cpp index 15a747dd3..e3f6b22b4 100644 --- a/components/resource/bulletshape.cpp +++ b/components/resource/bulletshape.cpp @@ -13,12 +13,14 @@ namespace Resource BulletShape::BulletShape() : mCollisionShape(nullptr) + , mAvoidCollisionShape(nullptr) { } BulletShape::BulletShape(const BulletShape ©, const osg::CopyOp ©op) : mCollisionShape(duplicateCollisionShape(copy.mCollisionShape)) + , mAvoidCollisionShape(duplicateCollisionShape(copy.mAvoidCollisionShape)) , mCollisionBoxHalfExtents(copy.mCollisionBoxHalfExtents) , mCollisionBoxTranslate(copy.mCollisionBoxTranslate) , mAnimatedShapes(copy.mAnimatedShapes) @@ -27,6 +29,7 @@ BulletShape::BulletShape(const BulletShape ©, const osg::CopyOp ©op) BulletShape::~BulletShape() { + deleteShape(mAvoidCollisionShape); deleteShape(mCollisionShape); } @@ -77,11 +80,23 @@ btCollisionShape* BulletShape::duplicateCollisionShape(const btCollisionShape *s throw std::logic_error(std::string("Unhandled Bullet shape duplication: ")+shape->getName()); } -btCollisionShape *BulletShape::getCollisionShape() +btCollisionShape *BulletShape::getCollisionShape() const { return mCollisionShape; } +btCollisionShape *BulletShape::getAvoidCollisionShape() const +{ + return mAvoidCollisionShape; +} + +void BulletShape::setLocalScaling(const btVector3& scale) +{ + mCollisionShape->setLocalScaling(scale); + if (mAvoidCollisionShape) + mAvoidCollisionShape->setLocalScaling(scale); +} + osg::ref_ptr BulletShape::makeInstance() const { osg::ref_ptr instance (new BulletShapeInstance(this)); @@ -99,6 +114,9 @@ BulletShapeInstance::BulletShapeInstance(osg::ref_ptr source) if (source->mCollisionShape) mCollisionShape = duplicateCollisionShape(source->mCollisionShape); + + if (source->mAvoidCollisionShape) + mAvoidCollisionShape = duplicateCollisionShape(source->mAvoidCollisionShape); } } diff --git a/components/resource/bulletshape.hpp b/components/resource/bulletshape.hpp index b30b5045c..e77c96327 100644 --- a/components/resource/bulletshape.hpp +++ b/components/resource/bulletshape.hpp @@ -25,6 +25,7 @@ namespace Resource META_Object(Resource, BulletShape) btCollisionShape* mCollisionShape; + btCollisionShape* mAvoidCollisionShape; // Used for actors. mCollisionShape is used for actors only when we need to autogenerate collision box for creatures. // For now, use one file <-> one resource for simplicity. @@ -41,7 +42,11 @@ namespace Resource btCollisionShape* duplicateCollisionShape(const btCollisionShape* shape) const; - btCollisionShape* getCollisionShape(); + btCollisionShape* getCollisionShape() const; + + btCollisionShape* getAvoidCollisionShape() const; + + void setLocalScaling(const btVector3& scale); private: diff --git a/components/sceneutil/agentpath.cpp b/components/sceneutil/agentpath.cpp new file mode 100644 index 000000000..aaee4dd1e --- /dev/null +++ b/components/sceneutil/agentpath.cpp @@ -0,0 +1,71 @@ +#include "agentpath.hpp" +#include "detourdebugdraw.hpp" + +#include +#include + +#include + +namespace +{ + void drawAgent(duDebugDraw& debugDraw, const osg::Vec3f& pos, const float radius, const float height, + const float climb, const unsigned color) + { + debugDraw.depthMask(false); + + duDebugDrawCylinderWire(&debugDraw, pos.x() - radius, pos.z() + 0.02f, pos.y() - radius, pos.x() + radius, + pos.z() + height, pos.y() + radius, color, radius * 0.2f); + + duDebugDrawCircle(&debugDraw, pos.x(), pos.z() + climb, pos.y(), radius, duRGBA(0, 0 , 0, 64), radius * 0.1f); + + const auto colb = duRGBA(0, 0, 0, 196); + debugDraw.begin(DU_DRAW_LINES); + debugDraw.vertex(pos.x(), pos.z() - climb, pos.y(), colb); + debugDraw.vertex(pos.x(), pos.z() + climb, pos.y(), colb); + debugDraw.vertex(pos.x() - radius / 2, pos.z() + 0.02f, pos.y(), colb); + debugDraw.vertex(pos.x() + radius / 2, pos.z() + 0.02f, pos.y(), colb); + debugDraw.vertex(pos.x(), pos.z() + 0.02f, pos.y() - radius / 2, colb); + debugDraw.vertex(pos.x(), pos.z() + 0.02f, pos.y() + radius / 2, colb); + debugDraw.end(); + + debugDraw.depthMask(true); + } +} + +namespace SceneUtil +{ + osg::ref_ptr createAgentPathGroup(const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end, + const DetourNavigator::Settings& settings) + { + using namespace DetourNavigator; + + const osg::ref_ptr group(new osg::Group); + + DebugDraw debugDraw(*group, osg::Vec3f(0, 0, 0), 1); + + const auto agentRadius = halfExtents.x(); + const auto agentHeight = 2.0f * halfExtents.z(); + const auto agentClimb = settings.mMaxClimb; + const auto startColor = duRGBA(128, 25, 0, 192); + const auto endColor = duRGBA(51, 102, 0, 129); + + drawAgent(debugDraw, start, agentRadius, agentHeight, agentClimb, startColor); + drawAgent(debugDraw, end, agentRadius, agentHeight, agentClimb, endColor); + + const auto pathColor = duRGBA(0, 0, 0, 220); + + debugDraw.depthMask(false); + + debugDraw.begin(osg::PrimitiveSet::LINE_STRIP, agentRadius * 0.5f); + debugDraw.vertex(osg::Vec3f(start.x(), start.z() + agentClimb, start.y()).ptr(), startColor); + std::for_each(path.begin(), path.end(), + [&] (const osg::Vec3f& v) { debugDraw.vertex(osg::Vec3f(v.x(), v.z() + agentClimb, v.y()).ptr(), pathColor); }); + debugDraw.vertex(osg::Vec3f(end.x(), end.z() + agentClimb, end.y()).ptr(), endColor); + debugDraw.end(); + + debugDraw.depthMask(true); + + return group; + } +} diff --git a/components/sceneutil/agentpath.hpp b/components/sceneutil/agentpath.hpp new file mode 100644 index 000000000..a8965d852 --- /dev/null +++ b/components/sceneutil/agentpath.hpp @@ -0,0 +1,26 @@ +#ifndef OPENMW_COMPONENTS_SCENEUTIL_AGENTPATH_H +#define OPENMW_COMPONENTS_SCENEUTIL_AGENTPATH_H + +#include + +#include + +namespace osg +{ + class Group; + class Vec3f; +} + +namespace DetourNavigator +{ + struct Settings; +} + +namespace SceneUtil +{ + osg::ref_ptr createAgentPathGroup(const std::deque& path, + const osg::Vec3f& halfExtents, const osg::Vec3f& start, const osg::Vec3f& end, + const DetourNavigator::Settings& settings); +} + +#endif diff --git a/components/sceneutil/detourdebugdraw.cpp b/components/sceneutil/detourdebugdraw.cpp new file mode 100644 index 000000000..a0ae67dc2 --- /dev/null +++ b/components/sceneutil/detourdebugdraw.cpp @@ -0,0 +1,128 @@ +#include "detourdebugdraw.hpp" +#include "util.hpp" + +#include + +#include +#include +#include + +#define OPENMW_TO_STRING(X) #X +#define OPENMW_LINE_STRING OPENMW_TO_STRING(__LINE__) + +namespace +{ + using DetourNavigator::operator<<; + + osg::PrimitiveSet::Mode toOsgPrimitiveSetMode(duDebugDrawPrimitives value) + { + switch (value) + { + case DU_DRAW_POINTS: + return osg::PrimitiveSet::POINTS; + case DU_DRAW_LINES: + return osg::PrimitiveSet::LINES; + case DU_DRAW_TRIS: + return osg::PrimitiveSet::TRIANGLES; + case DU_DRAW_QUADS: + return osg::PrimitiveSet::QUADS; + } + throw std::logic_error("Can't convert duDebugDrawPrimitives to osg::PrimitiveSet::Mode, value=" + + std::to_string(value)); + } + +} + +namespace SceneUtil +{ + DebugDraw::DebugDraw(osg::Group& group, const osg::Vec3f& shift, float recastInvertedScaleFactor) + : mGroup(group) + , mShift(shift) + , mRecastInvertedScaleFactor(recastInvertedScaleFactor) + , mDepthMask(false) + , mMode(osg::PrimitiveSet::POINTS) + , mSize(1.0f) + { + } + + void DebugDraw::depthMask(bool state) + { + mDepthMask = state; + } + + void DebugDraw::texture(bool state) + { + if (state) + throw std::logic_error("DebugDraw does not support textures (at " __FILE__ ":" OPENMW_LINE_STRING ")"); + } + + void DebugDraw::begin(osg::PrimitiveSet::Mode mode, float size) + { + mMode = mode; + mVertices = new osg::Vec3Array; + mColors = new osg::Vec4Array; + mSize = size * mRecastInvertedScaleFactor; + } + + void DebugDraw::begin(duDebugDrawPrimitives prim, float size) + { + begin(toOsgPrimitiveSetMode(prim), size); + } + + void DebugDraw::vertex(const float* pos, unsigned color) + { + vertex(pos[0], pos[1], pos[2], color); + } + + void DebugDraw::vertex(const float x, const float y, const float z, unsigned color) + { + addVertex(osg::Vec3f(x, y, z)); + addColor(SceneUtil::colourFromRGBA(color)); + } + + void DebugDraw::vertex(const float* pos, unsigned color, const float* uv) + { + vertex(pos[0], pos[1], pos[2], color, uv[0], uv[1]); + } + + void DebugDraw::vertex(const float, const float, const float, unsigned, const float, const float) + { + throw std::logic_error("Not implemented (at " __FILE__ ":" OPENMW_LINE_STRING ")"); + } + + void DebugDraw::end() + { + osg::ref_ptr stateSet(new osg::StateSet); + stateSet->setMode(GL_BLEND, osg::StateAttribute::ON); + stateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF); + stateSet->setMode(GL_DEPTH, (mDepthMask ? osg::StateAttribute::ON : osg::StateAttribute::OFF)); + stateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); + stateSet->setAttributeAndModes(new osg::LineWidth(mSize)); + stateSet->setAttributeAndModes(new osg::BlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)); + + osg::ref_ptr geometry(new osg::Geometry); + geometry->setStateSet(stateSet); + geometry->setUseDisplayList(false); + geometry->setVertexArray(mVertices); + geometry->setColorArray(mColors, osg::Array::BIND_PER_VERTEX); + geometry->addPrimitiveSet(new osg::DrawArrays(mMode, 0, static_cast(mVertices->size()))); + + mGroup.addChild(geometry); + mColors.release(); + mVertices.release(); + } + + void DebugDraw::addVertex(osg::Vec3f&& position) + { + std::swap(position.y(), position.z()); + mVertices->push_back(position * mRecastInvertedScaleFactor + mShift); + } + + void DebugDraw::addColor(osg::Vec4f&& value) + { + mColors->push_back(value); + } +} + +#undef OPENMW_TO_STRING +#undef OPENMW_LINE_STRING diff --git a/components/sceneutil/detourdebugdraw.hpp b/components/sceneutil/detourdebugdraw.hpp new file mode 100644 index 000000000..bb170e7ba --- /dev/null +++ b/components/sceneutil/detourdebugdraw.hpp @@ -0,0 +1,50 @@ +#include + +#include + +namespace osg +{ + class Group; +} + +namespace SceneUtil +{ + class DebugDraw : public duDebugDraw + { + public: + DebugDraw(osg::Group& group, const osg::Vec3f& shift, float recastInvertedScaleFactor); + + void depthMask(bool state) override; + + void texture(bool state) override; + + void begin(osg::PrimitiveSet::Mode mode, float size); + + void begin(duDebugDrawPrimitives prim, float size) override; + + void vertex(const float* pos, unsigned int color) override; + + void vertex(const float x, const float y, const float z, unsigned int color) override; + + void vertex(const float* pos, unsigned int color, const float* uv) override; + + void vertex(const float x, const float y, const float z, unsigned int color, + const float u, const float v) override; + + void end() override; + + private: + osg::Group& mGroup; + osg::Vec3f mShift; + float mRecastInvertedScaleFactor; + bool mDepthMask; + osg::PrimitiveSet::Mode mMode; + float mSize; + osg::ref_ptr mVertices; + osg::ref_ptr mColors; + + void addVertex(osg::Vec3f&& position); + + void addColor(osg::Vec4f&& value); + }; +} diff --git a/components/sceneutil/navmesh.cpp b/components/sceneutil/navmesh.cpp new file mode 100644 index 000000000..aeb1779bd --- /dev/null +++ b/components/sceneutil/navmesh.cpp @@ -0,0 +1,22 @@ +#include "navmesh.hpp" +#include "detourdebugdraw.hpp" + +#include + +#include + +#include + +namespace SceneUtil +{ + osg::ref_ptr createNavMeshGroup(const dtNavMesh& navMesh, const DetourNavigator::Settings& settings) + { + const osg::ref_ptr group(new osg::Group); + DebugDraw debugDraw(*group, osg::Vec3f(0, 0, 10), 1.0f / settings.mRecastScaleFactor); + dtNavMeshQuery navMeshQuery; + navMeshQuery.init(&navMesh, settings.mMaxNavMeshQueryNodes); + duDebugDrawNavMeshWithClosedList(&debugDraw, navMesh, navMeshQuery, + DU_DRAWNAVMESH_OFFMESHCONS | DU_DRAWNAVMESH_CLOSEDLIST); + return group; + } +} diff --git a/components/sceneutil/navmesh.hpp b/components/sceneutil/navmesh.hpp new file mode 100644 index 000000000..b255b0575 --- /dev/null +++ b/components/sceneutil/navmesh.hpp @@ -0,0 +1,23 @@ +#ifndef OPENMW_COMPONENTS_SCENEUTIL_NAVMESH_H +#define OPENMW_COMPONENTS_SCENEUTIL_NAVMESH_H + +#include + +class dtNavMesh; + +namespace osg +{ + class Group; +} + +namespace DetourNavigator +{ + struct Settings; +} + +namespace SceneUtil +{ + osg::ref_ptr createNavMeshGroup(const dtNavMesh& navMesh, const DetourNavigator::Settings& settings); +} + +#endif diff --git a/components/sceneutil/util.cpp b/components/sceneutil/util.cpp index eec302965..bbeda683a 100644 --- a/components/sceneutil/util.cpp +++ b/components/sceneutil/util.cpp @@ -42,4 +42,15 @@ osg::Vec4f colourFromRGB(unsigned int clr) return colour; } +osg::Vec4f colourFromRGBA(unsigned int value) +{ + return osg::Vec4f(makeOsgColorComponent(value, 0), makeOsgColorComponent(value, 8), + makeOsgColorComponent(value, 16), makeOsgColorComponent(value, 24)); +} + +float makeOsgColorComponent(unsigned int value, unsigned int shift) +{ + return float((value >> shift) & 0xFFu) / 255.0f; +} + } diff --git a/components/sceneutil/util.hpp b/components/sceneutil/util.hpp index 109099740..156d173d2 100644 --- a/components/sceneutil/util.hpp +++ b/components/sceneutil/util.hpp @@ -15,6 +15,10 @@ namespace SceneUtil osg::Vec4f colourFromRGB (unsigned int clr); + osg::Vec4f colourFromRGBA (unsigned int value); + + float makeOsgColorComponent (unsigned int value, unsigned int shift); + } #endif diff --git a/extern/recastnavigation/.editorconfig b/extern/recastnavigation/.editorconfig new file mode 100644 index 000000000..08f28f441 --- /dev/null +++ b/extern/recastnavigation/.editorconfig @@ -0,0 +1,12 @@ +# editorconfig.org + +# top-most EditorConfig file +root = true + +[*] +indent_size = 4 +indent_style = tab + +[*.yml] +indent_size = 2 +indent_style = space diff --git a/extern/recastnavigation/.gitignore b/extern/recastnavigation/.gitignore new file mode 100644 index 000000000..98f17e4b7 --- /dev/null +++ b/extern/recastnavigation/.gitignore @@ -0,0 +1,49 @@ +## Compiled source # +*.com +*.class +*.dll +*.exe +*.ilk +*.o +*.pdb +*.so +*.idb + +## Linux exes have no extension +RecastDemo/Bin/RecastDemo +RecastDemo/Bin/Tests + +# Build directory +RecastDemo/Build + +# Ignore meshes +RecastDemo/Bin/Meshes/* + +## Logs and databases # +*.log +*.sql +*.sqlite + +## OS generated files # +.DS_Store +.DS_Store? +._* +.Spotlight-V100 +.Trashes +ehthumbs.db +Thumbs.db +*.swp +*.swo + +## xcode specific +*xcuserdata* + +## SDL contrib +RecastDemo/Contrib/SDL/* + +## Generated doc files +Docs/html + +## IDE files +.idea/ +cmake-build-*/ diff --git a/extern/recastnavigation/.id b/extern/recastnavigation/.id new file mode 100644 index 000000000..c4937ac23 --- /dev/null +++ b/extern/recastnavigation/.id @@ -0,0 +1 @@ +7bfd9a1d4caccf61e0485b2b05b29966348a8b39 diff --git a/extern/recastnavigation/.travis.yml b/extern/recastnavigation/.travis.yml new file mode 100644 index 000000000..6044cd6b5 --- /dev/null +++ b/extern/recastnavigation/.travis.yml @@ -0,0 +1,36 @@ +sudo: false + +language: cpp + +# Build with gcc and clang. +compiler: + - gcc + - clang + +# Build both debug and release configurations, through use of an environment variable in the build matrix. +env: + - BUILD_TYPE=debug CMAKE_BUILD_TYPE=Debug + - BUILD_TYPE=release CMAKE_BUILD_TYPE=Release + +addons: + apt: + packages: + - libsdl2-dev + +install: + - wget https://github.com/premake/premake-core/releases/download/v5.0.0-alpha12/premake-5.0.0-alpha12-linux.tar.gz -O premake.tar.gz + - tar -xf premake.tar.gz + - rm premake.tar.gz + +# Run premake to generate makefiles. +# Have to cd into directory and back out since premake5 doesn't appear to accept a directory argument. +before_script: + - cd RecastDemo && ../premake5 gmake && cd .. + - mkdir build && cd build && cmake -D CMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} .. && cd .. + +# Run make in the directory containing generated makefiles, on the configuration specified by the environment variable. +script: + - make -C RecastDemo/Build/gmake -j$(nproc) config=${BUILD_TYPE} + - RecastDemo/Bin/Tests + - make -C build -j$(nproc) + - cd build && ctest diff --git a/extern/recastnavigation/.url b/extern/recastnavigation/.url new file mode 100644 index 000000000..b38b1645f --- /dev/null +++ b/extern/recastnavigation/.url @@ -0,0 +1 @@ +https://github.com/recastnavigation/recastnavigation.git diff --git a/extern/recastnavigation/CMakeLists.txt b/extern/recastnavigation/CMakeLists.txt new file mode 100644 index 000000000..d23859dfc --- /dev/null +++ b/extern/recastnavigation/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.0) + +project(RecastNavigation) + +# lib versions +SET(SOVERSION 1) +SET(VERSION 1.0.0) + +option(RECASTNAVIGATION_DEMO "Build demo" ON) +option(RECASTNAVIGATION_TESTS "Build tests" ON) +option(RECASTNAVIGATION_EXAMPLES "Build examples" ON) +option(RECASTNAVIGATION_STATIC "Build static libraries" ON) + +add_subdirectory(DebugUtils) +add_subdirectory(Detour) +add_subdirectory(DetourCrowd) +add_subdirectory(DetourTileCache) +add_subdirectory(Recast) + +if (RECASTNAVIGATION_DEMO) + add_subdirectory(RecastDemo) +endif () + +if (RECASTNAVIGATION_TESTS) + enable_testing() + add_subdirectory(Tests) +endif () diff --git a/extern/recastnavigation/CONTRIBUTING.md b/extern/recastnavigation/CONTRIBUTING.md new file mode 100644 index 000000000..3cfdc2160 --- /dev/null +++ b/extern/recastnavigation/CONTRIBUTING.md @@ -0,0 +1,185 @@ +# Contributing to Recast and Detour + +We'd love for you to contribute to our source code and to make Recast and Detour even better than they are +today! Here are the guidelines we'd like you to follow: + + - [Code of Conduct](#coc) + - [Question or Problem?](#question) + - [Issues and Bugs](#issue) + - [Feature Requests](#feature) + - [Submission Guidelines](#submission-guidelines) + - [Git Commit Guidelines](#git-commit-guidelines) + +## Code of Conduct +This project adheres to the [Open Code of Conduct][code-of-conduct]. +By participating, you are expected to honor this code. + +## Got a Question or Problem? + +If you have questions about how to use Recast or Detour, please direct these to the [Google Group][groups] +discussion list. We are also available on [Gitter][gitter]. + +## Found an Issue? +If you find a bug in the source code or a mistake in the documentation, you can help us by +submitting an issue to our [GitHub Repository][github]. Even better you can submit a Pull Request +with a fix. + +**Please see the Submission Guidelines below**. + +## Want a Feature? +You can request a new feature by submitting an issue to our [GitHub Repository][github]. If you +would like to implement a new feature then consider what kind of change it is: + +* **Major Changes** that you wish to contribute to the project should be discussed first on our +[Google Group][groups] or in [GitHub Issues][github-issues] so that we can better coordinate our efforts, prevent +duplication of work, and help you to craft the change so that it is successfully accepted into the +project. +* **Small Changes** can be crafted and submitted to the [GitHub Repository][github] as a Pull Request. + +## Submission Guidelines + +### Submitting an Issue +Before you submit your issue search the [GitHub Issues][github-issues] archive, +maybe your question was already answered. + +If your issue appears to be a bug, and hasn't been reported, open a new issue. +Help us to maximize the effort we can spend fixing issues and adding new +features, by not reporting duplicate issues. Providing the following information will increase the +chances of your issue being dealt with quickly: + +* **Overview of the Issue** - what type of issue is it, and why is it an issue for you? +* **Callstack** - if it's a crash or other runtime error, a callstack will help diagnosis +* **Screenshots** - for navmesh generation problems, a picture really is worth a thousand words. + Implement `duDebugDraw` and call some methods from DetourDebugDraw.h. Seriously, just do it, we'll definitely ask you to if you haven't! +* **Logs** - stdout and stderr from the console, or log files if there are any. + If integrating into your own codebase, be sure to implement the log callbacks in `rcContext`. +* **Reproduction steps** - a minimal, unambigious set of steps including input, that causes the error for you. + e.g. input geometry and settings you can use to input into RecastDemo to get it to fail. + Note: These can be saved by pressing the 9 key in RecastDemo, and the resulting .gset file can be shared (with the .obj if it is not one of the default ones). +* **Recast version(s) and/or git commit hash** - particularly if you can find the point at which the error first started happening +* **Environment** - operating system, compiler etc. +* **Related issues** - has a similar issue been reported before? +* **Suggest a Fix** - if you can't fix the bug yourself, perhaps you can point to what might be + causing the problem (line of code or commit) + +Here is a great example of a well defined issue: https://github.com/recastnavigation/recastnavigation/issues/12 + +**If you get help, help others. Good karma rulez!** + +### Submitting a Pull Request +Before you submit your pull request consider the following guidelines: + +* Search [GitHub Pull Requests][github-pulls] for an open or closed Pull Request + that relates to your submission. You don't want to duplicate effort. +* Make your changes in a new git branch: + + ```shell + git checkout -b my-fix-branch master + ``` + +* Implement your changes, **including appropriate tests if appropriate/possible**. +* Commit your changes using a descriptive commit message that follows our + [commit message conventions](#commit-message-format). + + ```shell + git commit -a + ``` + Note: the optional commit `-a` command line option will automatically "add" and "rm" edited files. + +* Squash any work-in-progress commits (by rebasing) to form a series of commits that make sense individually. + Ideally the pull request will be small and focused enough that it fits sensibly in one commit. + + ```shell + git rebase -i origin/master + ``` + +* Push your branch to GitHub: + + ```shell + git push origin my-fix-branch + ``` + +* In GitHub, send a pull request to `recastnavigation:master`. +* If we suggest changes then: + * Make the required updates. + * Commit your changes to your branch (e.g. `my-fix-branch`). + * Squash the changes, overwriting history in your fix branch - we don't want history to include incomplete work. + * Push the changes to your GitHub repository (this will update your Pull Request). + +If you have rebased to squash commits together, you will need to force push to update the PR: + + ```shell + git rebase master -i + git push origin my-fix-branch -f + ``` + +That's it! Thank you for your contribution! + +#### After your pull request is merged + +After your pull request is merged, you can safely delete your branch and pull the changes +from the main (upstream) repository: + +* Delete the remote branch on GitHub either through the GitHub web UI or your local shell as follows: + + ```shell + git push origin --delete my-fix-branch + ``` + +* Check out the master branch: + + ```shell + git checkout master -f + ``` + +* Delete the local branch: + + ```shell + git branch -D my-fix-branch + ``` + +* Update your master with the latest upstream version: + + ```shell + git pull --ff upstream master + ``` + +## Git Commit Guidelines + +### Commit content + +Do your best to factor commits appropriately, i.e not too large with unrelated +things in the same commit, and not too small with the same small change applied N +times in N different commits. If there was some accidental reformatting or whitespace +changes during the course of your commits, please rebase them away before submitting +the PR. + +### Commit Message Format +Please format commit messages as follows (based on this [excellent post](http://tbaggery.com/2008/04/19/a-note-about-git-commit-messages.html)): + +``` +Summarize change in 50 characters or less + +Provide more detail after the first line. Leave one blank line below the +summary and wrap all lines at 72 characters or less. + +If the change fixes an issue, leave another blank line after the final +paragraph and indicate which issue is fixed in the specific format +below. + +Fix #42 +``` + +Important things you should try to include in commit messages include: +* Motivation for the change +* Difference from previous behaviour +* Whether the change alters the public API, or affects existing behaviour significantly + + + +[code-of-conduct]: http://todogroup.org/opencodeofconduct/#Recastnavigation/b.hymers@gmail.com +[github]: https://github.com/recastnavigation/recastnavigation +[github-issues]: https://github.com/recastnavigation/recastnavigation/issues +[github-pulls]: https://github.com/recastnavigation/recastnavigation/pulls +[gitter]: https://gitter.im/recastnavigation/chat +[groups]: https://groups.google.com/forum/?fromgroups#!forum/recastnavigation diff --git a/extern/recastnavigation/DebugUtils/CMakeLists.txt b/extern/recastnavigation/DebugUtils/CMakeLists.txt new file mode 100644 index 000000000..8b6a3fcf6 --- /dev/null +++ b/extern/recastnavigation/DebugUtils/CMakeLists.txt @@ -0,0 +1,35 @@ +file(GLOB SOURCES Source/*.cpp) + +if (RECASTNAVIGATION_STATIC) + add_library(DebugUtils STATIC ${SOURCES}) +else() + add_library(DebugUtils SHARED ${SOURCES}) +endif() + +add_library(RecastNavigation::DebugUtils ALIAS DebugUtils) + +set(DebugUtils_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Include") + +target_include_directories(DebugUtils PUBLIC + "$" +) + +target_link_libraries(DebugUtils + Recast + Detour + DetourTileCache +) + +set_target_properties(DebugUtils PROPERTIES + SOVERSION ${SOVERSION} + VERSION ${VERSION} + ) + +install(TARGETS DebugUtils + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + COMPONENT library + ) + +file(GLOB INCLUDES Include/*.h) +install(FILES ${INCLUDES} DESTINATION include) diff --git a/extern/recastnavigation/DebugUtils/Include/DebugDraw.h b/extern/recastnavigation/DebugUtils/Include/DebugDraw.h new file mode 100644 index 000000000..00b544d1c --- /dev/null +++ b/extern/recastnavigation/DebugUtils/Include/DebugDraw.h @@ -0,0 +1,223 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DEBUGDRAW_H +#define DEBUGDRAW_H + +// Some math headers don't have PI defined. +static const float DU_PI = 3.14159265f; + +enum duDebugDrawPrimitives +{ + DU_DRAW_POINTS, + DU_DRAW_LINES, + DU_DRAW_TRIS, + DU_DRAW_QUADS, +}; + +/// Abstract debug draw interface. +struct duDebugDraw +{ + virtual ~duDebugDraw() = 0; + + virtual void depthMask(bool state) = 0; + + virtual void texture(bool state) = 0; + + /// Begin drawing primitives. + /// @param prim [in] primitive type to draw, one of rcDebugDrawPrimitives. + /// @param size [in] size of a primitive, applies to point size and line width only. + virtual void begin(duDebugDrawPrimitives prim, float size = 1.0f) = 0; + + /// Submit a vertex + /// @param pos [in] position of the verts. + /// @param color [in] color of the verts. + virtual void vertex(const float* pos, unsigned int color) = 0; + + /// Submit a vertex + /// @param x,y,z [in] position of the verts. + /// @param color [in] color of the verts. + virtual void vertex(const float x, const float y, const float z, unsigned int color) = 0; + + /// Submit a vertex + /// @param pos [in] position of the verts. + /// @param color [in] color of the verts. + virtual void vertex(const float* pos, unsigned int color, const float* uv) = 0; + + /// Submit a vertex + /// @param x,y,z [in] position of the verts. + /// @param color [in] color of the verts. + virtual void vertex(const float x, const float y, const float z, unsigned int color, const float u, const float v) = 0; + + /// End drawing primitives. + virtual void end() = 0; + + /// Compute a color for given area. + virtual unsigned int areaToCol(unsigned int area); +}; + +inline unsigned int duRGBA(int r, int g, int b, int a) +{ + return ((unsigned int)r) | ((unsigned int)g << 8) | ((unsigned int)b << 16) | ((unsigned int)a << 24); +} + +inline unsigned int duRGBAf(float fr, float fg, float fb, float fa) +{ + unsigned char r = (unsigned char)(fr*255.0f); + unsigned char g = (unsigned char)(fg*255.0f); + unsigned char b = (unsigned char)(fb*255.0f); + unsigned char a = (unsigned char)(fa*255.0f); + return duRGBA(r,g,b,a); +} + +unsigned int duIntToCol(int i, int a); +void duIntToCol(int i, float* col); + +inline unsigned int duMultCol(const unsigned int col, const unsigned int d) +{ + const unsigned int r = col & 0xff; + const unsigned int g = (col >> 8) & 0xff; + const unsigned int b = (col >> 16) & 0xff; + const unsigned int a = (col >> 24) & 0xff; + return duRGBA((r*d) >> 8, (g*d) >> 8, (b*d) >> 8, a); +} + +inline unsigned int duDarkenCol(unsigned int col) +{ + return ((col >> 1) & 0x007f7f7f) | (col & 0xff000000); +} + +inline unsigned int duLerpCol(unsigned int ca, unsigned int cb, unsigned int u) +{ + const unsigned int ra = ca & 0xff; + const unsigned int ga = (ca >> 8) & 0xff; + const unsigned int ba = (ca >> 16) & 0xff; + const unsigned int aa = (ca >> 24) & 0xff; + const unsigned int rb = cb & 0xff; + const unsigned int gb = (cb >> 8) & 0xff; + const unsigned int bb = (cb >> 16) & 0xff; + const unsigned int ab = (cb >> 24) & 0xff; + + unsigned int r = (ra*(255-u) + rb*u)/255; + unsigned int g = (ga*(255-u) + gb*u)/255; + unsigned int b = (ba*(255-u) + bb*u)/255; + unsigned int a = (aa*(255-u) + ab*u)/255; + return duRGBA(r,g,b,a); +} + +inline unsigned int duTransCol(unsigned int c, unsigned int a) +{ + return (a<<24) | (c & 0x00ffffff); +} + + +void duCalcBoxColors(unsigned int* colors, unsigned int colTop, unsigned int colSide); + +void duDebugDrawCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col, const float lineWidth); + +void duDebugDrawBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col, const float lineWidth); + +void duDebugDrawArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, const float h, + const float as0, const float as1, unsigned int col, const float lineWidth); + +void duDebugDrawArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, + const float as0, const float as1, unsigned int col, const float lineWidth); + +void duDebugDrawCircle(struct duDebugDraw* dd, const float x, const float y, const float z, + const float r, unsigned int col, const float lineWidth); + +void duDebugDrawCross(struct duDebugDraw* dd, const float x, const float y, const float z, + const float size, unsigned int col, const float lineWidth); + +void duDebugDrawBox(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, const unsigned int* fcol); + +void duDebugDrawCylinder(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + +void duDebugDrawGridXZ(struct duDebugDraw* dd, const float ox, const float oy, const float oz, + const int w, const int h, const float size, + const unsigned int col, const float lineWidth); + + +// Versions without begin/end, can be used to draw multiple primitives. +void duAppendCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + +void duAppendBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + +void duAppendBoxPoints(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + +void duAppendArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, const float h, + const float as0, const float as1, unsigned int col); + +void duAppendArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, + const float as0, const float as1, unsigned int col); + +void duAppendCircle(struct duDebugDraw* dd, const float x, const float y, const float z, + const float r, unsigned int col); + +void duAppendCross(struct duDebugDraw* dd, const float x, const float y, const float z, + const float size, unsigned int col); + +void duAppendBox(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, const unsigned int* fcol); + +void duAppendCylinder(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + + +class duDisplayList : public duDebugDraw +{ + float* m_pos; + unsigned int* m_color; + int m_size; + int m_cap; + + bool m_depthMask; + duDebugDrawPrimitives m_prim; + float m_primSize; + + void resize(int cap); + +public: + duDisplayList(int cap = 512); + ~duDisplayList(); + virtual void depthMask(bool state); + virtual void begin(duDebugDrawPrimitives prim, float size = 1.0f); + virtual void vertex(const float x, const float y, const float z, unsigned int color); + virtual void vertex(const float* pos, unsigned int color); + virtual void end(); + void clear(); + void draw(struct duDebugDraw* dd); +private: + // Explicitly disabled copy constructor and copy assignment operator. + duDisplayList(const duDisplayList&); + duDisplayList& operator=(const duDisplayList&); +}; + + +#endif // DEBUGDRAW_H diff --git a/extern/recastnavigation/DebugUtils/Include/DetourDebugDraw.h b/extern/recastnavigation/DebugUtils/Include/DetourDebugDraw.h new file mode 100755 index 000000000..ff2ca2f9d --- /dev/null +++ b/extern/recastnavigation/DebugUtils/Include/DetourDebugDraw.h @@ -0,0 +1,48 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURDEBUGDRAW_H +#define DETOURDEBUGDRAW_H + +#include "DetourNavMesh.h" +#include "DetourNavMeshQuery.h" +#include "DetourTileCacheBuilder.h" + +enum DrawNavMeshFlags +{ + DU_DRAWNAVMESH_OFFMESHCONS = 0x01, + DU_DRAWNAVMESH_CLOSEDLIST = 0x02, + DU_DRAWNAVMESH_COLOR_TILES = 0x04, +}; + +void duDebugDrawNavMesh(struct duDebugDraw* dd, const dtNavMesh& mesh, unsigned char flags); +void duDebugDrawNavMeshWithClosedList(struct duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMeshQuery& query, unsigned char flags); +void duDebugDrawNavMeshNodes(struct duDebugDraw* dd, const dtNavMeshQuery& query); +void duDebugDrawNavMeshBVTree(struct duDebugDraw* dd, const dtNavMesh& mesh); +void duDebugDrawNavMeshPortals(struct duDebugDraw* dd, const dtNavMesh& mesh); +void duDebugDrawNavMeshPolysWithFlags(struct duDebugDraw* dd, const dtNavMesh& mesh, const unsigned short polyFlags, const unsigned int col); +void duDebugDrawNavMeshPoly(struct duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef ref, const unsigned int col); + +void duDebugDrawTileCacheLayerAreas(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch); +void duDebugDrawTileCacheLayerRegions(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch); +void duDebugDrawTileCacheContours(duDebugDraw* dd, const struct dtTileCacheContourSet& lcset, + const float* orig, const float cs, const float ch); +void duDebugDrawTileCachePolyMesh(duDebugDraw* dd, const struct dtTileCachePolyMesh& lmesh, + const float* orig, const float cs, const float ch); + +#endif // DETOURDEBUGDRAW_H diff --git a/extern/recastnavigation/DebugUtils/Include/RecastDebugDraw.h b/extern/recastnavigation/DebugUtils/Include/RecastDebugDraw.h new file mode 100644 index 000000000..6a55fa647 --- /dev/null +++ b/extern/recastnavigation/DebugUtils/Include/RecastDebugDraw.h @@ -0,0 +1,42 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef RECAST_DEBUGDRAW_H +#define RECAST_DEBUGDRAW_H + +void duDebugDrawTriMesh(struct duDebugDraw* dd, const float* verts, int nverts, const int* tris, const float* normals, int ntris, const unsigned char* flags, const float texScale); +void duDebugDrawTriMeshSlope(struct duDebugDraw* dd, const float* verts, int nverts, const int* tris, const float* normals, int ntris, const float walkableSlopeAngle, const float texScale); + +void duDebugDrawHeightfieldSolid(struct duDebugDraw* dd, const struct rcHeightfield& hf); +void duDebugDrawHeightfieldWalkable(struct duDebugDraw* dd, const struct rcHeightfield& hf); + +void duDebugDrawCompactHeightfieldSolid(struct duDebugDraw* dd, const struct rcCompactHeightfield& chf); +void duDebugDrawCompactHeightfieldRegions(struct duDebugDraw* dd, const struct rcCompactHeightfield& chf); +void duDebugDrawCompactHeightfieldDistance(struct duDebugDraw* dd, const struct rcCompactHeightfield& chf); + +void duDebugDrawHeightfieldLayer(duDebugDraw* dd, const struct rcHeightfieldLayer& layer, const int idx); +void duDebugDrawHeightfieldLayers(duDebugDraw* dd, const struct rcHeightfieldLayerSet& lset); +void duDebugDrawHeightfieldLayersRegions(duDebugDraw* dd, const struct rcHeightfieldLayerSet& lset); + +void duDebugDrawRegionConnections(struct duDebugDraw* dd, const struct rcContourSet& cset, const float alpha = 1.0f); +void duDebugDrawRawContours(struct duDebugDraw* dd, const struct rcContourSet& cset, const float alpha = 1.0f); +void duDebugDrawContours(struct duDebugDraw* dd, const struct rcContourSet& cset, const float alpha = 1.0f); +void duDebugDrawPolyMesh(struct duDebugDraw* dd, const struct rcPolyMesh& mesh); +void duDebugDrawPolyMeshDetail(struct duDebugDraw* dd, const struct rcPolyMeshDetail& dmesh); + +#endif // RECAST_DEBUGDRAW_H diff --git a/extern/recastnavigation/DebugUtils/Include/RecastDump.h b/extern/recastnavigation/DebugUtils/Include/RecastDump.h new file mode 100644 index 000000000..6a722fdae --- /dev/null +++ b/extern/recastnavigation/DebugUtils/Include/RecastDump.h @@ -0,0 +1,43 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef RECAST_DUMP_H +#define RECAST_DUMP_H + +struct duFileIO +{ + virtual ~duFileIO() = 0; + virtual bool isWriting() const = 0; + virtual bool isReading() const = 0; + virtual bool write(const void* ptr, const size_t size) = 0; + virtual bool read(void* ptr, const size_t size) = 0; +}; + +bool duDumpPolyMeshToObj(struct rcPolyMesh& pmesh, duFileIO* io); +bool duDumpPolyMeshDetailToObj(struct rcPolyMeshDetail& dmesh, duFileIO* io); + +bool duDumpContourSet(struct rcContourSet& cset, duFileIO* io); +bool duReadContourSet(struct rcContourSet& cset, duFileIO* io); + +bool duDumpCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io); +bool duReadCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io); + +void duLogBuildTimes(rcContext& ctx, const int totalTileUsec); + + +#endif // RECAST_DUMP_H diff --git a/extern/recastnavigation/DebugUtils/Source/DebugDraw.cpp b/extern/recastnavigation/DebugUtils/Source/DebugDraw.cpp new file mode 100644 index 000000000..d0179bca2 --- /dev/null +++ b/extern/recastnavigation/DebugUtils/Source/DebugDraw.cpp @@ -0,0 +1,612 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#define _USE_MATH_DEFINES +#include +#include "DebugDraw.h" +#include "DetourMath.h" +#include "DetourNavMesh.h" + + +duDebugDraw::~duDebugDraw() +{ + // Empty +} + +unsigned int duDebugDraw::areaToCol(unsigned int area) +{ + if (area == 0) + { + // Treat zero area type as default. + return duRGBA(0, 192, 255, 255); + } + else + { + return duIntToCol(area, 255); + } +} + +inline int bit(int a, int b) +{ + return (a & (1 << b)) >> b; +} + +unsigned int duIntToCol(int i, int a) +{ + int r = bit(i, 1) + bit(i, 3) * 2 + 1; + int g = bit(i, 2) + bit(i, 4) * 2 + 1; + int b = bit(i, 0) + bit(i, 5) * 2 + 1; + return duRGBA(r*63,g*63,b*63,a); +} + +void duIntToCol(int i, float* col) +{ + int r = bit(i, 0) + bit(i, 3) * 2 + 1; + int g = bit(i, 1) + bit(i, 4) * 2 + 1; + int b = bit(i, 2) + bit(i, 5) * 2 + 1; + col[0] = 1 - r*63.0f/255.0f; + col[1] = 1 - g*63.0f/255.0f; + col[2] = 1 - b*63.0f/255.0f; +} + +void duCalcBoxColors(unsigned int* colors, unsigned int colTop, unsigned int colSide) +{ + if (!colors) return; + + colors[0] = duMultCol(colTop, 250); + colors[1] = duMultCol(colSide, 140); + colors[2] = duMultCol(colSide, 165); + colors[3] = duMultCol(colSide, 217); + colors[4] = duMultCol(colSide, 165); + colors[5] = duMultCol(colSide, 217); +} + +void duDebugDrawCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendCylinderWire(dd, minx,miny,minz, maxx,maxy,maxz, col); + dd->end(); +} + +void duDebugDrawBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendBoxWire(dd, minx,miny,minz, maxx,maxy,maxz, col); + dd->end(); +} + +void duDebugDrawArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, const float h, + const float as0, const float as1, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendArc(dd, x0,y0,z0, x1,y1,z1, h, as0, as1, col); + dd->end(); +} + +void duDebugDrawArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, + const float as0, const float as1, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendArrow(dd, x0,y0,z0, x1,y1,z1, as0, as1, col); + dd->end(); +} + +void duDebugDrawCircle(struct duDebugDraw* dd, const float x, const float y, const float z, + const float r, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendCircle(dd, x,y,z, r, col); + dd->end(); +} + +void duDebugDrawCross(struct duDebugDraw* dd, const float x, const float y, const float z, + const float size, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendCross(dd, x,y,z, size, col); + dd->end(); +} + +void duDebugDrawBox(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, const unsigned int* fcol) +{ + if (!dd) return; + + dd->begin(DU_DRAW_QUADS); + duAppendBox(dd, minx,miny,minz, maxx,maxy,maxz, fcol); + dd->end(); +} + +void duDebugDrawCylinder(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + + dd->begin(DU_DRAW_TRIS); + duAppendCylinder(dd, minx,miny,minz, maxx,maxy,maxz, col); + dd->end(); +} + +void duDebugDrawGridXZ(struct duDebugDraw* dd, const float ox, const float oy, const float oz, + const int w, const int h, const float size, + const unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + for (int i = 0; i <= h; ++i) + { + dd->vertex(ox,oy,oz+i*size, col); + dd->vertex(ox+w*size,oy,oz+i*size, col); + } + for (int i = 0; i <= w; ++i) + { + dd->vertex(ox+i*size,oy,oz, col); + dd->vertex(ox+i*size,oy,oz+h*size, col); + } + dd->end(); +} + + +void duAppendCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + + static const int NUM_SEG = 16; + static float dir[NUM_SEG*2]; + static bool init = false; + if (!init) + { + init = true; + for (int i = 0; i < NUM_SEG; ++i) + { + const float a = (float)i/(float)NUM_SEG*DU_PI*2; + dir[i*2] = dtMathCosf(a); + dir[i*2+1] = dtMathSinf(a); + } + } + + const float cx = (maxx + minx)/2; + const float cz = (maxz + minz)/2; + const float rx = (maxx - minx)/2; + const float rz = (maxz - minz)/2; + + for (int i = 0, j = NUM_SEG-1; i < NUM_SEG; j = i++) + { + dd->vertex(cx+dir[j*2+0]*rx, miny, cz+dir[j*2+1]*rz, col); + dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col); + dd->vertex(cx+dir[j*2+0]*rx, maxy, cz+dir[j*2+1]*rz, col); + dd->vertex(cx+dir[i*2+0]*rx, maxy, cz+dir[i*2+1]*rz, col); + } + for (int i = 0; i < NUM_SEG; i += NUM_SEG/4) + { + dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col); + dd->vertex(cx+dir[i*2+0]*rx, maxy, cz+dir[i*2+1]*rz, col); + } +} + +void duAppendBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + // Top + dd->vertex(minx, miny, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, miny, minz, col); + + // bottom + dd->vertex(minx, maxy, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(minx, maxy, maxz, col); + dd->vertex(minx, maxy, maxz, col); + dd->vertex(minx, maxy, minz, col); + + // Sides + dd->vertex(minx, miny, minz, col); + dd->vertex(minx, maxy, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, maxy, maxz, col); +} + +void duAppendBoxPoints(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + // Top + dd->vertex(minx, miny, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, miny, minz, col); + + // bottom + dd->vertex(minx, maxy, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(minx, maxy, maxz, col); + dd->vertex(minx, maxy, maxz, col); + dd->vertex(minx, maxy, minz, col); +} + +void duAppendBox(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, const unsigned int* fcol) +{ + if (!dd) return; + const float verts[8*3] = + { + minx, miny, minz, + maxx, miny, minz, + maxx, miny, maxz, + minx, miny, maxz, + minx, maxy, minz, + maxx, maxy, minz, + maxx, maxy, maxz, + minx, maxy, maxz, + }; + static const unsigned char inds[6*4] = + { + 7, 6, 5, 4, + 0, 1, 2, 3, + 1, 5, 6, 2, + 3, 7, 4, 0, + 2, 6, 7, 3, + 0, 4, 5, 1, + }; + + const unsigned char* in = inds; + for (int i = 0; i < 6; ++i) + { + dd->vertex(&verts[*in*3], fcol[i]); in++; + dd->vertex(&verts[*in*3], fcol[i]); in++; + dd->vertex(&verts[*in*3], fcol[i]); in++; + dd->vertex(&verts[*in*3], fcol[i]); in++; + } +} + +void duAppendCylinder(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + + static const int NUM_SEG = 16; + static float dir[NUM_SEG*2]; + static bool init = false; + if (!init) + { + init = true; + for (int i = 0; i < NUM_SEG; ++i) + { + const float a = (float)i/(float)NUM_SEG*DU_PI*2; + dir[i*2] = cosf(a); + dir[i*2+1] = sinf(a); + } + } + + unsigned int col2 = duMultCol(col, 160); + + const float cx = (maxx + minx)/2; + const float cz = (maxz + minz)/2; + const float rx = (maxx - minx)/2; + const float rz = (maxz - minz)/2; + + for (int i = 2; i < NUM_SEG; ++i) + { + const int a = 0, b = i-1, c = i; + dd->vertex(cx+dir[a*2+0]*rx, miny, cz+dir[a*2+1]*rz, col2); + dd->vertex(cx+dir[b*2+0]*rx, miny, cz+dir[b*2+1]*rz, col2); + dd->vertex(cx+dir[c*2+0]*rx, miny, cz+dir[c*2+1]*rz, col2); + } + for (int i = 2; i < NUM_SEG; ++i) + { + const int a = 0, b = i, c = i-1; + dd->vertex(cx+dir[a*2+0]*rx, maxy, cz+dir[a*2+1]*rz, col); + dd->vertex(cx+dir[b*2+0]*rx, maxy, cz+dir[b*2+1]*rz, col); + dd->vertex(cx+dir[c*2+0]*rx, maxy, cz+dir[c*2+1]*rz, col); + } + for (int i = 0, j = NUM_SEG-1; i < NUM_SEG; j = i++) + { + dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col2); + dd->vertex(cx+dir[j*2+0]*rx, miny, cz+dir[j*2+1]*rz, col2); + dd->vertex(cx+dir[j*2+0]*rx, maxy, cz+dir[j*2+1]*rz, col); + + dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col2); + dd->vertex(cx+dir[j*2+0]*rx, maxy, cz+dir[j*2+1]*rz, col); + dd->vertex(cx+dir[i*2+0]*rx, maxy, cz+dir[i*2+1]*rz, col); + } +} + + +inline void evalArc(const float x0, const float y0, const float z0, + const float dx, const float dy, const float dz, + const float h, const float u, float* res) +{ + res[0] = x0 + dx * u; + res[1] = y0 + dy * u + h * (1-(u*2-1)*(u*2-1)); + res[2] = z0 + dz * u; +} + + +inline void vcross(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[1]*v2[2] - v1[2]*v2[1]; + dest[1] = v1[2]*v2[0] - v1[0]*v2[2]; + dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; +} + +inline void vnormalize(float* v) +{ + float d = 1.0f / sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); + v[0] *= d; + v[1] *= d; + v[2] *= d; +} + +inline void vsub(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[0]-v2[0]; + dest[1] = v1[1]-v2[1]; + dest[2] = v1[2]-v2[2]; +} + +inline float vdistSqr(const float* v1, const float* v2) +{ + const float x = v1[0]-v2[0]; + const float y = v1[1]-v2[1]; + const float z = v1[2]-v2[2]; + return x*x + y*y + z*z; +} + + +void appendArrowHead(struct duDebugDraw* dd, const float* p, const float* q, + const float s, unsigned int col) +{ + const float eps = 0.001f; + if (!dd) return; + if (vdistSqr(p,q) < eps*eps) return; + float ax[3], ay[3] = {0,1,0}, az[3]; + vsub(az, q, p); + vnormalize(az); + vcross(ax, ay, az); + vcross(ay, az, ax); + vnormalize(ay); + + dd->vertex(p, col); +// dd->vertex(p[0]+az[0]*s+ay[0]*s/2, p[1]+az[1]*s+ay[1]*s/2, p[2]+az[2]*s+ay[2]*s/2, col); + dd->vertex(p[0]+az[0]*s+ax[0]*s/3, p[1]+az[1]*s+ax[1]*s/3, p[2]+az[2]*s+ax[2]*s/3, col); + + dd->vertex(p, col); +// dd->vertex(p[0]+az[0]*s-ay[0]*s/2, p[1]+az[1]*s-ay[1]*s/2, p[2]+az[2]*s-ay[2]*s/2, col); + dd->vertex(p[0]+az[0]*s-ax[0]*s/3, p[1]+az[1]*s-ax[1]*s/3, p[2]+az[2]*s-ax[2]*s/3, col); + +} + +void duAppendArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, const float h, + const float as0, const float as1, unsigned int col) +{ + if (!dd) return; + static const int NUM_ARC_PTS = 8; + static const float PAD = 0.05f; + static const float ARC_PTS_SCALE = (1.0f-PAD*2) / (float)NUM_ARC_PTS; + const float dx = x1 - x0; + const float dy = y1 - y0; + const float dz = z1 - z0; + const float len = sqrtf(dx*dx + dy*dy + dz*dz); + float prev[3]; + evalArc(x0,y0,z0, dx,dy,dz, len*h, PAD, prev); + for (int i = 1; i <= NUM_ARC_PTS; ++i) + { + const float u = PAD + i * ARC_PTS_SCALE; + float pt[3]; + evalArc(x0,y0,z0, dx,dy,dz, len*h, u, pt); + dd->vertex(prev[0],prev[1],prev[2], col); + dd->vertex(pt[0],pt[1],pt[2], col); + prev[0] = pt[0]; prev[1] = pt[1]; prev[2] = pt[2]; + } + + // End arrows + if (as0 > 0.001f) + { + float p[3], q[3]; + evalArc(x0,y0,z0, dx,dy,dz, len*h, PAD, p); + evalArc(x0,y0,z0, dx,dy,dz, len*h, PAD+0.05f, q); + appendArrowHead(dd, p, q, as0, col); + } + + if (as1 > 0.001f) + { + float p[3], q[3]; + evalArc(x0,y0,z0, dx,dy,dz, len*h, 1-PAD, p); + evalArc(x0,y0,z0, dx,dy,dz, len*h, 1-(PAD+0.05f), q); + appendArrowHead(dd, p, q, as1, col); + } +} + +void duAppendArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, + const float as0, const float as1, unsigned int col) +{ + if (!dd) return; + + dd->vertex(x0,y0,z0, col); + dd->vertex(x1,y1,z1, col); + + // End arrows + const float p[3] = {x0,y0,z0}, q[3] = {x1,y1,z1}; + if (as0 > 0.001f) + appendArrowHead(dd, p, q, as0, col); + if (as1 > 0.001f) + appendArrowHead(dd, q, p, as1, col); +} + +void duAppendCircle(struct duDebugDraw* dd, const float x, const float y, const float z, + const float r, unsigned int col) +{ + if (!dd) return; + static const int NUM_SEG = 40; + static float dir[40*2]; + static bool init = false; + if (!init) + { + init = true; + for (int i = 0; i < NUM_SEG; ++i) + { + const float a = (float)i/(float)NUM_SEG*DU_PI*2; + dir[i*2] = cosf(a); + dir[i*2+1] = sinf(a); + } + } + + for (int i = 0, j = NUM_SEG-1; i < NUM_SEG; j = i++) + { + dd->vertex(x+dir[j*2+0]*r, y, z+dir[j*2+1]*r, col); + dd->vertex(x+dir[i*2+0]*r, y, z+dir[i*2+1]*r, col); + } +} + +void duAppendCross(struct duDebugDraw* dd, const float x, const float y, const float z, + const float s, unsigned int col) +{ + if (!dd) return; + dd->vertex(x-s,y,z, col); + dd->vertex(x+s,y,z, col); + dd->vertex(x,y-s,z, col); + dd->vertex(x,y+s,z, col); + dd->vertex(x,y,z-s, col); + dd->vertex(x,y,z+s, col); +} + +duDisplayList::duDisplayList(int cap) : + m_pos(0), + m_color(0), + m_size(0), + m_cap(0), + m_depthMask(true), + m_prim(DU_DRAW_LINES), + m_primSize(1.0f) +{ + if (cap < 8) + cap = 8; + resize(cap); +} + +duDisplayList::~duDisplayList() +{ + delete [] m_pos; + delete [] m_color; +} + +void duDisplayList::resize(int cap) +{ + float* newPos = new float[cap*3]; + if (m_size) + memcpy(newPos, m_pos, sizeof(float)*3*m_size); + delete [] m_pos; + m_pos = newPos; + + unsigned int* newColor = new unsigned int[cap]; + if (m_size) + memcpy(newColor, m_color, sizeof(unsigned int)*m_size); + delete [] m_color; + m_color = newColor; + + m_cap = cap; +} + +void duDisplayList::clear() +{ + m_size = 0; +} + +void duDisplayList::depthMask(bool state) +{ + m_depthMask = state; +} + +void duDisplayList::begin(duDebugDrawPrimitives prim, float size) +{ + clear(); + m_prim = prim; + m_primSize = size; +} + +void duDisplayList::vertex(const float x, const float y, const float z, unsigned int color) +{ + if (m_size+1 >= m_cap) + resize(m_cap*2); + float* p = &m_pos[m_size*3]; + p[0] = x; + p[1] = y; + p[2] = z; + m_color[m_size] = color; + m_size++; +} + +void duDisplayList::vertex(const float* pos, unsigned int color) +{ + vertex(pos[0],pos[1],pos[2],color); +} + +void duDisplayList::end() +{ +} + +void duDisplayList::draw(struct duDebugDraw* dd) +{ + if (!dd) return; + if (!m_size) return; + dd->depthMask(m_depthMask); + dd->begin(m_prim, m_primSize); + for (int i = 0; i < m_size; ++i) + dd->vertex(&m_pos[i*3], m_color[i]); + dd->end(); +} diff --git a/extern/recastnavigation/DebugUtils/Source/DetourDebugDraw.cpp b/extern/recastnavigation/DebugUtils/Source/DetourDebugDraw.cpp new file mode 100644 index 000000000..dd4bad3fd --- /dev/null +++ b/extern/recastnavigation/DebugUtils/Source/DetourDebugDraw.cpp @@ -0,0 +1,862 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DebugDraw.h" +#include "DetourDebugDraw.h" +#include "DetourNavMesh.h" +#include "DetourCommon.h" +#include "DetourNode.h" + + +static float distancePtLine2d(const float* pt, const float* p, const float* q) +{ + float pqx = q[0] - p[0]; + float pqz = q[2] - p[2]; + float dx = pt[0] - p[0]; + float dz = pt[2] - p[2]; + float d = pqx*pqx + pqz*pqz; + float t = pqx*dx + pqz*dz; + if (d != 0) t /= d; + dx = p[0] + t*pqx - pt[0]; + dz = p[2] + t*pqz - pt[2]; + return dx*dx + dz*dz; +} + +static void drawPolyBoundaries(duDebugDraw* dd, const dtMeshTile* tile, + const unsigned int col, const float linew, + bool inner) +{ + static const float thr = 0.01f*0.01f; + + dd->begin(DU_DRAW_LINES, linew); + + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + + if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) continue; + + const dtPolyDetail* pd = &tile->detailMeshes[i]; + + for (int j = 0, nj = (int)p->vertCount; j < nj; ++j) + { + unsigned int c = col; + if (inner) + { + if (p->neis[j] == 0) continue; + if (p->neis[j] & DT_EXT_LINK) + { + bool con = false; + for (unsigned int k = p->firstLink; k != DT_NULL_LINK; k = tile->links[k].next) + { + if (tile->links[k].edge == j) + { + con = true; + break; + } + } + if (con) + c = duRGBA(255,255,255,48); + else + c = duRGBA(0,0,0,48); + } + else + c = duRGBA(0,48,64,32); + } + else + { + if (p->neis[j] != 0) continue; + } + + const float* v0 = &tile->verts[p->verts[j]*3]; + const float* v1 = &tile->verts[p->verts[(j+1) % nj]*3]; + + // Draw detail mesh edges which align with the actual poly edge. + // This is really slow. + for (int k = 0; k < pd->triCount; ++k) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+k)*4]; + const float* tv[3]; + for (int m = 0; m < 3; ++m) + { + if (t[m] < p->vertCount) + tv[m] = &tile->verts[p->verts[t[m]]*3]; + else + tv[m] = &tile->detailVerts[(pd->vertBase+(t[m]-p->vertCount))*3]; + } + for (int m = 0, n = 2; m < 3; n=m++) + { + if (((t[3] >> (n*2)) & 0x3) == 0) continue; // Skip inner detail edges. + if (distancePtLine2d(tv[n],v0,v1) < thr && + distancePtLine2d(tv[m],v0,v1) < thr) + { + dd->vertex(tv[n], c); + dd->vertex(tv[m], c); + } + } + } + } + } + dd->end(); +} + +static void drawMeshTile(duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMeshQuery* query, + const dtMeshTile* tile, unsigned char flags) +{ + dtPolyRef base = mesh.getPolyRefBase(tile); + + int tileNum = mesh.decodePolyIdTile(base); + const unsigned int tileColor = duIntToCol(tileNum, 128); + + dd->depthMask(false); + + dd->begin(DU_DRAW_TRIS); + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) // Skip off-mesh links. + continue; + + const dtPolyDetail* pd = &tile->detailMeshes[i]; + + unsigned int col; + if (query && query->isInClosedList(base | (dtPolyRef)i)) + col = duRGBA(255,196,0,64); + else + { + if (flags & DU_DRAWNAVMESH_COLOR_TILES) + col = tileColor; + else + col = duTransCol(dd->areaToCol(p->getArea()), 64); + } + + for (int j = 0; j < pd->triCount; ++j) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < p->vertCount) + dd->vertex(&tile->verts[p->verts[t[k]]*3], col); + else + dd->vertex(&tile->detailVerts[(pd->vertBase+t[k]-p->vertCount)*3], col); + } + } + } + dd->end(); + + // Draw inter poly boundaries + drawPolyBoundaries(dd, tile, duRGBA(0,48,64,32), 1.5f, true); + + // Draw outer poly boundaries + drawPolyBoundaries(dd, tile, duRGBA(0,48,64,220), 2.5f, false); + + if (flags & DU_DRAWNAVMESH_OFFMESHCONS) + { + dd->begin(DU_DRAW_LINES, 2.0f); + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + if (p->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) // Skip regular polys. + continue; + + unsigned int col, col2; + if (query && query->isInClosedList(base | (dtPolyRef)i)) + col = duRGBA(255,196,0,220); + else + col = duDarkenCol(duTransCol(dd->areaToCol(p->getArea()), 220)); + + const dtOffMeshConnection* con = &tile->offMeshCons[i - tile->header->offMeshBase]; + const float* va = &tile->verts[p->verts[0]*3]; + const float* vb = &tile->verts[p->verts[1]*3]; + + // Check to see if start and end end-points have links. + bool startSet = false; + bool endSet = false; + for (unsigned int k = p->firstLink; k != DT_NULL_LINK; k = tile->links[k].next) + { + if (tile->links[k].edge == 0) + startSet = true; + if (tile->links[k].edge == 1) + endSet = true; + } + + // End points and their on-mesh locations. + dd->vertex(va[0],va[1],va[2], col); + dd->vertex(con->pos[0],con->pos[1],con->pos[2], col); + col2 = startSet ? col : duRGBA(220,32,16,196); + duAppendCircle(dd, con->pos[0],con->pos[1]+0.1f,con->pos[2], con->rad, col2); + + dd->vertex(vb[0],vb[1],vb[2], col); + dd->vertex(con->pos[3],con->pos[4],con->pos[5], col); + col2 = endSet ? col : duRGBA(220,32,16,196); + duAppendCircle(dd, con->pos[3],con->pos[4]+0.1f,con->pos[5], con->rad, col2); + + // End point vertices. + dd->vertex(con->pos[0],con->pos[1],con->pos[2], duRGBA(0,48,64,196)); + dd->vertex(con->pos[0],con->pos[1]+0.2f,con->pos[2], duRGBA(0,48,64,196)); + + dd->vertex(con->pos[3],con->pos[4],con->pos[5], duRGBA(0,48,64,196)); + dd->vertex(con->pos[3],con->pos[4]+0.2f,con->pos[5], duRGBA(0,48,64,196)); + + // Connection arc. + duAppendArc(dd, con->pos[0],con->pos[1],con->pos[2], con->pos[3],con->pos[4],con->pos[5], 0.25f, + (con->flags & 1) ? 0.6f : 0, 0.6f, col); + } + dd->end(); + } + + const unsigned int vcol = duRGBA(0,0,0,196); + dd->begin(DU_DRAW_POINTS, 3.0f); + for (int i = 0; i < tile->header->vertCount; ++i) + { + const float* v = &tile->verts[i*3]; + dd->vertex(v[0], v[1], v[2], vcol); + } + dd->end(); + + dd->depthMask(true); +} + +void duDebugDrawNavMesh(duDebugDraw* dd, const dtNavMesh& mesh, unsigned char flags) +{ + if (!dd) return; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + drawMeshTile(dd, mesh, 0, tile, flags); + } +} + +void duDebugDrawNavMeshWithClosedList(struct duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMeshQuery& query, unsigned char flags) +{ + if (!dd) return; + + const dtNavMeshQuery* q = (flags & DU_DRAWNAVMESH_CLOSEDLIST) ? &query : 0; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + drawMeshTile(dd, mesh, q, tile, flags); + } +} + +void duDebugDrawNavMeshNodes(struct duDebugDraw* dd, const dtNavMeshQuery& query) +{ + if (!dd) return; + + const dtNodePool* pool = query.getNodePool(); + if (pool) + { + const float off = 0.5f; + dd->begin(DU_DRAW_POINTS, 4.0f); + for (int i = 0; i < pool->getHashSize(); ++i) + { + for (dtNodeIndex j = pool->getFirst(i); j != DT_NULL_IDX; j = pool->getNext(j)) + { + const dtNode* node = pool->getNodeAtIdx(j+1); + if (!node) continue; + dd->vertex(node->pos[0],node->pos[1]+off,node->pos[2], duRGBA(255,192,0,255)); + } + } + dd->end(); + + dd->begin(DU_DRAW_LINES, 2.0f); + for (int i = 0; i < pool->getHashSize(); ++i) + { + for (dtNodeIndex j = pool->getFirst(i); j != DT_NULL_IDX; j = pool->getNext(j)) + { + const dtNode* node = pool->getNodeAtIdx(j+1); + if (!node) continue; + if (!node->pidx) continue; + const dtNode* parent = pool->getNodeAtIdx(node->pidx); + if (!parent) continue; + dd->vertex(node->pos[0],node->pos[1]+off,node->pos[2], duRGBA(255,192,0,128)); + dd->vertex(parent->pos[0],parent->pos[1]+off,parent->pos[2], duRGBA(255,192,0,128)); + } + } + dd->end(); + } +} + + +static void drawMeshTileBVTree(duDebugDraw* dd, const dtMeshTile* tile) +{ + // Draw BV nodes. + const float cs = 1.0f / tile->header->bvQuantFactor; + dd->begin(DU_DRAW_LINES, 1.0f); + for (int i = 0; i < tile->header->bvNodeCount; ++i) + { + const dtBVNode* n = &tile->bvTree[i]; + if (n->i < 0) // Leaf indices are positive. + continue; + duAppendBoxWire(dd, tile->header->bmin[0] + n->bmin[0]*cs, + tile->header->bmin[1] + n->bmin[1]*cs, + tile->header->bmin[2] + n->bmin[2]*cs, + tile->header->bmin[0] + n->bmax[0]*cs, + tile->header->bmin[1] + n->bmax[1]*cs, + tile->header->bmin[2] + n->bmax[2]*cs, + duRGBA(255,255,255,128)); + } + dd->end(); +} + +void duDebugDrawNavMeshBVTree(duDebugDraw* dd, const dtNavMesh& mesh) +{ + if (!dd) return; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + drawMeshTileBVTree(dd, tile); + } +} + +static void drawMeshTilePortal(duDebugDraw* dd, const dtMeshTile* tile) +{ + // Draw portals + const float padx = 0.04f; + const float pady = tile->header->walkableClimb; + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int side = 0; side < 8; ++side) + { + unsigned short m = DT_EXT_LINK | (unsigned short)side; + + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + + // Create new links. + const int nv = poly->vertCount; + for (int j = 0; j < nv; ++j) + { + // Skip edges which do not point to the right side. + if (poly->neis[j] != m) + continue; + + // Create new links + const float* va = &tile->verts[poly->verts[j]*3]; + const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3]; + + if (side == 0 || side == 4) + { + unsigned int col = side == 0 ? duRGBA(128,0,0,128) : duRGBA(128,0,128,128); + + const float x = va[0] + ((side == 0) ? -padx : padx); + + dd->vertex(x,va[1]-pady,va[2], col); + dd->vertex(x,va[1]+pady,va[2], col); + + dd->vertex(x,va[1]+pady,va[2], col); + dd->vertex(x,vb[1]+pady,vb[2], col); + + dd->vertex(x,vb[1]+pady,vb[2], col); + dd->vertex(x,vb[1]-pady,vb[2], col); + + dd->vertex(x,vb[1]-pady,vb[2], col); + dd->vertex(x,va[1]-pady,va[2], col); + } + else if (side == 2 || side == 6) + { + unsigned int col = side == 2 ? duRGBA(0,128,0,128) : duRGBA(0,128,128,128); + + const float z = va[2] + ((side == 2) ? -padx : padx); + + dd->vertex(va[0],va[1]-pady,z, col); + dd->vertex(va[0],va[1]+pady,z, col); + + dd->vertex(va[0],va[1]+pady,z, col); + dd->vertex(vb[0],vb[1]+pady,z, col); + + dd->vertex(vb[0],vb[1]+pady,z, col); + dd->vertex(vb[0],vb[1]-pady,z, col); + + dd->vertex(vb[0],vb[1]-pady,z, col); + dd->vertex(va[0],va[1]-pady,z, col); + } + + } + } + } + + dd->end(); +} + +void duDebugDrawNavMeshPortals(duDebugDraw* dd, const dtNavMesh& mesh) +{ + if (!dd) return; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + drawMeshTilePortal(dd, tile); + } +} + +void duDebugDrawNavMeshPolysWithFlags(struct duDebugDraw* dd, const dtNavMesh& mesh, + const unsigned short polyFlags, const unsigned int col) +{ + if (!dd) return; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + dtPolyRef base = mesh.getPolyRefBase(tile); + + for (int j = 0; j < tile->header->polyCount; ++j) + { + const dtPoly* p = &tile->polys[j]; + if ((p->flags & polyFlags) == 0) continue; + duDebugDrawNavMeshPoly(dd, mesh, base|(dtPolyRef)j, col); + } + } +} + +void duDebugDrawNavMeshPoly(duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef ref, const unsigned int col) +{ + if (!dd) return; + + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(mesh.getTileAndPolyByRef(ref, &tile, &poly))) + return; + + dd->depthMask(false); + + const unsigned int c = duTransCol(col, 64); + const unsigned int ip = (unsigned int)(poly - tile->polys); + + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + dtOffMeshConnection* con = &tile->offMeshCons[ip - tile->header->offMeshBase]; + + dd->begin(DU_DRAW_LINES, 2.0f); + + // Connection arc. + duAppendArc(dd, con->pos[0],con->pos[1],con->pos[2], con->pos[3],con->pos[4],con->pos[5], 0.25f, + (con->flags & 1) ? 0.6f : 0.0f, 0.6f, c); + + dd->end(); + } + else + { + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + + dd->begin(DU_DRAW_TRIS); + for (int i = 0; i < pd->triCount; ++i) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+i)*4]; + for (int j = 0; j < 3; ++j) + { + if (t[j] < poly->vertCount) + dd->vertex(&tile->verts[poly->verts[t[j]]*3], c); + else + dd->vertex(&tile->detailVerts[(pd->vertBase+t[j]-poly->vertCount)*3], c); + } + } + dd->end(); + } + + dd->depthMask(true); + +} + +static void debugDrawTileCachePortals(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch) +{ + const int w = (int)layer.header->width; + const int h = (int)layer.header->height; + const float* bmin = layer.header->bmin; + + // Portals + unsigned int pcol = duRGBA(255,255,255,255); + + const int segs[4*4] = {0,0,0,1, 0,1,1,1, 1,1,1,0, 1,0,0,0}; + + // Layer portals + dd->begin(DU_DRAW_LINES, 2.0f); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int idx = x+y*w; + const int lh = (int)layer.heights[idx]; + if (lh == 0xff) continue; + + for (int dir = 0; dir < 4; ++dir) + { + if (layer.cons[idx] & (1<<(dir+4))) + { + const int* seg = &segs[dir*4]; + const float ax = bmin[0] + (x+seg[0])*cs; + const float ay = bmin[1] + (lh+2)*ch; + const float az = bmin[2] + (y+seg[1])*cs; + const float bx = bmin[0] + (x+seg[2])*cs; + const float by = bmin[1] + (lh+2)*ch; + const float bz = bmin[2] + (y+seg[3])*cs; + dd->vertex(ax, ay, az, pcol); + dd->vertex(bx, by, bz, pcol); + } + } + } + } + dd->end(); +} + +void duDebugDrawTileCacheLayerAreas(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch) +{ + const int w = (int)layer.header->width; + const int h = (int)layer.header->height; + const float* bmin = layer.header->bmin; + const float* bmax = layer.header->bmax; + const int idx = layer.header->tlayer; + + unsigned int color = duIntToCol(idx+1, 255); + + // Layer bounds + float lbmin[3], lbmax[3]; + lbmin[0] = bmin[0] + layer.header->minx*cs; + lbmin[1] = bmin[1]; + lbmin[2] = bmin[2] + layer.header->miny*cs; + lbmax[0] = bmin[0] + (layer.header->maxx+1)*cs; + lbmax[1] = bmax[1]; + lbmax[2] = bmin[2] + (layer.header->maxy+1)*cs; + duDebugDrawBoxWire(dd, lbmin[0],lbmin[1],lbmin[2], lbmax[0],lbmax[1],lbmax[2], duTransCol(color,128), 2.0f); + + // Layer height + dd->begin(DU_DRAW_QUADS); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int lidx = x+y*w; + const int lh = (int)layer.heights[lidx]; + if (lh == 0xff) continue; + + const unsigned char area = layer.areas[lidx]; + unsigned int col; + if (area == 63) + col = duLerpCol(color, duRGBA(0,192,255,64), 32); + else if (area == 0) + col = duLerpCol(color, duRGBA(0,0,0,64), 32); + else + col = duLerpCol(color, dd->areaToCol(area), 32); + + const float fx = bmin[0] + x*cs; + const float fy = bmin[1] + (lh+1)*ch; + const float fz = bmin[2] + y*cs; + + dd->vertex(fx, fy, fz, col); + dd->vertex(fx, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz, col); + } + } + dd->end(); + + debugDrawTileCachePortals(dd, layer, cs, ch); +} + +void duDebugDrawTileCacheLayerRegions(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch) +{ + const int w = (int)layer.header->width; + const int h = (int)layer.header->height; + const float* bmin = layer.header->bmin; + const float* bmax = layer.header->bmax; + const int idx = layer.header->tlayer; + + unsigned int color = duIntToCol(idx+1, 255); + + // Layer bounds + float lbmin[3], lbmax[3]; + lbmin[0] = bmin[0] + layer.header->minx*cs; + lbmin[1] = bmin[1]; + lbmin[2] = bmin[2] + layer.header->miny*cs; + lbmax[0] = bmin[0] + (layer.header->maxx+1)*cs; + lbmax[1] = bmax[1]; + lbmax[2] = bmin[2] + (layer.header->maxy+1)*cs; + duDebugDrawBoxWire(dd, lbmin[0],lbmin[1],lbmin[2], lbmax[0],lbmax[1],lbmax[2], duTransCol(color,128), 2.0f); + + // Layer height + dd->begin(DU_DRAW_QUADS); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int lidx = x+y*w; + const int lh = (int)layer.heights[lidx]; + if (lh == 0xff) continue; + const unsigned char reg = layer.regs[lidx]; + + unsigned int col = duLerpCol(color, duIntToCol(reg, 255), 192); + + const float fx = bmin[0] + x*cs; + const float fy = bmin[1] + (lh+1)*ch; + const float fz = bmin[2] + y*cs; + + dd->vertex(fx, fy, fz, col); + dd->vertex(fx, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz, col); + } + } + dd->end(); + + debugDrawTileCachePortals(dd, layer, cs, ch); +} + + + + +/*struct dtTileCacheContour +{ + int nverts; + unsigned char* verts; + unsigned char reg; + unsigned char area; +}; + +struct dtTileCacheContourSet +{ + int nconts; + dtTileCacheContour* conts; +};*/ + +void duDebugDrawTileCacheContours(duDebugDraw* dd, const struct dtTileCacheContourSet& lcset, + const float* orig, const float cs, const float ch) +{ + if (!dd) return; + + const unsigned char a = 255;// (unsigned char)(alpha*255.0f); + + const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1}; + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int i = 0; i < lcset.nconts; ++i) + { + const dtTileCacheContour& c = lcset.conts[i]; + unsigned int color = 0; + + color = duIntToCol(i, a); + + for (int j = 0; j < c.nverts; ++j) + { + const int k = (j+1) % c.nverts; + const unsigned char* va = &c.verts[j*4]; + const unsigned char* vb = &c.verts[k*4]; + const float ax = orig[0] + va[0]*cs; + const float ay = orig[1] + (va[1]+1+(i&1))*ch; + const float az = orig[2] + va[2]*cs; + const float bx = orig[0] + vb[0]*cs; + const float by = orig[1] + (vb[1]+1+(i&1))*ch; + const float bz = orig[2] + vb[2]*cs; + unsigned int col = color; + if ((va[3] & 0xf) != 0xf) + { + // Portal segment + col = duRGBA(255,255,255,128); + int d = va[3] & 0xf; + + const float cx = (ax+bx)*0.5f; + const float cy = (ay+by)*0.5f; + const float cz = (az+bz)*0.5f; + + const float dx = cx + offs[d*2+0]*2*cs; + const float dy = cy; + const float dz = cz + offs[d*2+1]*2*cs; + + dd->vertex(cx,cy,cz,duRGBA(255,0,0,255)); + dd->vertex(dx,dy,dz,duRGBA(255,0,0,255)); + } + + duAppendArrow(dd, ax,ay,az, bx,by,bz, 0.0f, cs*0.5f, col); + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 4.0f); + + for (int i = 0; i < lcset.nconts; ++i) + { + const dtTileCacheContour& c = lcset.conts[i]; + unsigned int color = 0; + + for (int j = 0; j < c.nverts; ++j) + { + const unsigned char* va = &c.verts[j*4]; + + color = duDarkenCol(duIntToCol(i, a)); + if (va[3] & 0x80) + { + // Border vertex + color = duRGBA(255,0,0,255); + } + + float fx = orig[0] + va[0]*cs; + float fy = orig[1] + (va[1]+1+(i&1))*ch; + float fz = orig[2] + va[2]*cs; + dd->vertex(fx,fy,fz, color); + } + } + dd->end(); +} + +void duDebugDrawTileCachePolyMesh(duDebugDraw* dd, const struct dtTileCachePolyMesh& lmesh, + const float* orig, const float cs, const float ch) +{ + if (!dd) return; + + const int nvp = lmesh.nvp; + + const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1}; + + dd->begin(DU_DRAW_TRIS); + + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + const unsigned char area = lmesh.areas[i]; + + unsigned int color; + if (area == DT_TILECACHE_WALKABLE_AREA) + color = duRGBA(0,192,255,64); + else if (area == DT_TILECACHE_NULL_AREA) + color = duRGBA(0,0,0,64); + else + color = dd->areaToCol(area); + + unsigned short vi[3]; + for (int j = 2; j < nvp; ++j) + { + if (p[j] == DT_TILECACHE_NULL_IDX) break; + vi[0] = p[0]; + vi[1] = p[j-1]; + vi[2] = p[j]; + for (int k = 0; k < 3; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, color); + } + } + } + dd->end(); + + // Draw neighbours edges + const unsigned int coln = duRGBA(0,48,64,32); + dd->begin(DU_DRAW_LINES, 1.5f); + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == DT_TILECACHE_NULL_IDX) break; + if (p[nvp+j] & 0x8000) continue; + const int nj = (j+1 >= nvp || p[j+1] == DT_TILECACHE_NULL_IDX) ? 0 : j+1; + int vi[2] = {p[j], p[nj]}; + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, coln); + } + } + } + dd->end(); + + // Draw boundary edges + const unsigned int colb = duRGBA(0,48,64,220); + dd->begin(DU_DRAW_LINES, 2.5f); + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == DT_TILECACHE_NULL_IDX) break; + if ((p[nvp+j] & 0x8000) == 0) continue; + const int nj = (j+1 >= nvp || p[j+1] == DT_TILECACHE_NULL_IDX) ? 0 : j+1; + int vi[2] = {p[j], p[nj]}; + + unsigned int col = colb; + if ((p[nvp+j] & 0xf) != 0xf) + { + const unsigned short* va = &lmesh.verts[vi[0]*3]; + const unsigned short* vb = &lmesh.verts[vi[1]*3]; + + const float ax = orig[0] + va[0]*cs; + const float ay = orig[1] + (va[1]+1+(i&1))*ch; + const float az = orig[2] + va[2]*cs; + const float bx = orig[0] + vb[0]*cs; + const float by = orig[1] + (vb[1]+1+(i&1))*ch; + const float bz = orig[2] + vb[2]*cs; + + const float cx = (ax+bx)*0.5f; + const float cy = (ay+by)*0.5f; + const float cz = (az+bz)*0.5f; + + int d = p[nvp+j] & 0xf; + + const float dx = cx + offs[d*2+0]*2*cs; + const float dy = cy; + const float dz = cz + offs[d*2+1]*2*cs; + + dd->vertex(cx,cy,cz,duRGBA(255,0,0,255)); + dd->vertex(dx,dy,dz,duRGBA(255,0,0,255)); + + col = duRGBA(255,255,255,128); + } + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, col); + } + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + const unsigned int colv = duRGBA(0,0,0,220); + for (int i = 0; i < lmesh.nverts; ++i) + { + const unsigned short* v = &lmesh.verts[i*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, colv); + } + dd->end(); +} + + + diff --git a/extern/recastnavigation/DebugUtils/Source/RecastDebugDraw.cpp b/extern/recastnavigation/DebugUtils/Source/RecastDebugDraw.cpp new file mode 100644 index 000000000..c1a73a168 --- /dev/null +++ b/extern/recastnavigation/DebugUtils/Source/RecastDebugDraw.cpp @@ -0,0 +1,1064 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#define _USE_MATH_DEFINES +#include +#include "DebugDraw.h" +#include "RecastDebugDraw.h" +#include "Recast.h" + +void duDebugDrawTriMesh(duDebugDraw* dd, const float* verts, int /*nverts*/, + const int* tris, const float* normals, int ntris, + const unsigned char* flags, const float texScale) +{ + if (!dd) return; + if (!verts) return; + if (!tris) return; + if (!normals) return; + + float uva[2]; + float uvb[2]; + float uvc[2]; + + const unsigned int unwalkable = duRGBA(192,128,0,255); + + dd->texture(true); + + dd->begin(DU_DRAW_TRIS); + for (int i = 0; i < ntris*3; i += 3) + { + const float* norm = &normals[i]; + unsigned int color; + unsigned char a = (unsigned char)(220*(2+norm[0]+norm[1])/4); + if (flags && !flags[i/3]) + color = duLerpCol(duRGBA(a,a,a,255), unwalkable, 64); + else + color = duRGBA(a,a,a,255); + + const float* va = &verts[tris[i+0]*3]; + const float* vb = &verts[tris[i+1]*3]; + const float* vc = &verts[tris[i+2]*3]; + + int ax = 0, ay = 0; + if (rcAbs(norm[1]) > rcAbs(norm[ax])) + ax = 1; + if (rcAbs(norm[2]) > rcAbs(norm[ax])) + ax = 2; + ax = (1<vertex(va, color, uva); + dd->vertex(vb, color, uvb); + dd->vertex(vc, color, uvc); + } + dd->end(); + dd->texture(false); +} + +void duDebugDrawTriMeshSlope(duDebugDraw* dd, const float* verts, int /*nverts*/, + const int* tris, const float* normals, int ntris, + const float walkableSlopeAngle, const float texScale) +{ + if (!dd) return; + if (!verts) return; + if (!tris) return; + if (!normals) return; + + const float walkableThr = cosf(walkableSlopeAngle/180.0f*DU_PI); + + float uva[2]; + float uvb[2]; + float uvc[2]; + + dd->texture(true); + + const unsigned int unwalkable = duRGBA(192,128,0,255); + + dd->begin(DU_DRAW_TRIS); + for (int i = 0; i < ntris*3; i += 3) + { + const float* norm = &normals[i]; + unsigned int color; + unsigned char a = (unsigned char)(220*(2+norm[0]+norm[1])/4); + if (norm[1] < walkableThr) + color = duLerpCol(duRGBA(a,a,a,255), unwalkable, 64); + else + color = duRGBA(a,a,a,255); + + const float* va = &verts[tris[i+0]*3]; + const float* vb = &verts[tris[i+1]*3]; + const float* vc = &verts[tris[i+2]*3]; + + int ax = 0, ay = 0; + if (rcAbs(norm[1]) > rcAbs(norm[ax])) + ax = 1; + if (rcAbs(norm[2]) > rcAbs(norm[ax])) + ax = 2; + ax = (1<vertex(va, color, uva); + dd->vertex(vb, color, uvb); + dd->vertex(vc, color, uvc); + } + dd->end(); + + dd->texture(false); +} + +void duDebugDrawHeightfieldSolid(duDebugDraw* dd, const rcHeightfield& hf) +{ + if (!dd) return; + + const float* orig = hf.bmin; + const float cs = hf.cs; + const float ch = hf.ch; + + const int w = hf.width; + const int h = hf.height; + + unsigned int fcol[6]; + duCalcBoxColors(fcol, duRGBA(255,255,255,255), duRGBA(255,255,255,255)); + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + float fx = orig[0] + x*cs; + float fz = orig[2] + y*cs; + const rcSpan* s = hf.spans[x + y*w]; + while (s) + { + duAppendBox(dd, fx, orig[1]+s->smin*ch, fz, fx+cs, orig[1] + s->smax*ch, fz+cs, fcol); + s = s->next; + } + } + } + dd->end(); +} + +void duDebugDrawHeightfieldWalkable(duDebugDraw* dd, const rcHeightfield& hf) +{ + if (!dd) return; + + const float* orig = hf.bmin; + const float cs = hf.cs; + const float ch = hf.ch; + + const int w = hf.width; + const int h = hf.height; + + unsigned int fcol[6]; + duCalcBoxColors(fcol, duRGBA(255,255,255,255), duRGBA(217,217,217,255)); + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + float fx = orig[0] + x*cs; + float fz = orig[2] + y*cs; + const rcSpan* s = hf.spans[x + y*w]; + while (s) + { + if (s->area == RC_WALKABLE_AREA) + fcol[0] = duRGBA(64,128,160,255); + else if (s->area == RC_NULL_AREA) + fcol[0] = duRGBA(64,64,64,255); + else + fcol[0] = duMultCol(dd->areaToCol(s->area), 200); + + duAppendBox(dd, fx, orig[1]+s->smin*ch, fz, fx+cs, orig[1] + s->smax*ch, fz+cs, fcol); + s = s->next; + } + } + } + + dd->end(); +} + +void duDebugDrawCompactHeightfieldSolid(duDebugDraw* dd, const rcCompactHeightfield& chf) +{ + if (!dd) return; + + const float cs = chf.cs; + const float ch = chf.ch; + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < chf.height; ++y) + { + for (int x = 0; x < chf.width; ++x) + { + const float fx = chf.bmin[0] + x*cs; + const float fz = chf.bmin[2] + y*cs; + const rcCompactCell& c = chf.cells[x+y*chf.width]; + + for (unsigned i = c.index, ni = c.index+c.count; i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + + const unsigned char area = chf.areas[i]; + unsigned int color; + if (area == RC_WALKABLE_AREA) + color = duRGBA(0,192,255,64); + else if (area == RC_NULL_AREA) + color = duRGBA(0,0,0,64); + else + color = dd->areaToCol(area); + + const float fy = chf.bmin[1] + (s.y+1)*ch; + dd->vertex(fx, fy, fz, color); + dd->vertex(fx, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz, color); + } + } + } + dd->end(); +} + +void duDebugDrawCompactHeightfieldRegions(duDebugDraw* dd, const rcCompactHeightfield& chf) +{ + if (!dd) return; + + const float cs = chf.cs; + const float ch = chf.ch; + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < chf.height; ++y) + { + for (int x = 0; x < chf.width; ++x) + { + const float fx = chf.bmin[0] + x*cs; + const float fz = chf.bmin[2] + y*cs; + const rcCompactCell& c = chf.cells[x+y*chf.width]; + + for (unsigned i = c.index, ni = c.index+c.count; i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + const float fy = chf.bmin[1] + (s.y)*ch; + unsigned int color; + if (s.reg) + color = duIntToCol(s.reg, 192); + else + color = duRGBA(0,0,0,64); + + dd->vertex(fx, fy, fz, color); + dd->vertex(fx, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz, color); + } + } + } + + dd->end(); +} + + +void duDebugDrawCompactHeightfieldDistance(duDebugDraw* dd, const rcCompactHeightfield& chf) +{ + if (!dd) return; + if (!chf.dist) return; + + const float cs = chf.cs; + const float ch = chf.ch; + + float maxd = chf.maxDistance; + if (maxd < 1.0f) maxd = 1; + const float dscale = 255.0f / maxd; + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < chf.height; ++y) + { + for (int x = 0; x < chf.width; ++x) + { + const float fx = chf.bmin[0] + x*cs; + const float fz = chf.bmin[2] + y*cs; + const rcCompactCell& c = chf.cells[x+y*chf.width]; + + for (unsigned i = c.index, ni = c.index+c.count; i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + const float fy = chf.bmin[1] + (s.y+1)*ch; + const unsigned char cd = (unsigned char)(chf.dist[i] * dscale); + const unsigned int color = duRGBA(cd,cd,cd,255); + dd->vertex(fx, fy, fz, color); + dd->vertex(fx, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz, color); + } + } + } + dd->end(); +} + +static void drawLayerPortals(duDebugDraw* dd, const rcHeightfieldLayer* layer) +{ + const float cs = layer->cs; + const float ch = layer->ch; + const int w = layer->width; + const int h = layer->height; + + unsigned int pcol = duRGBA(255,255,255,255); + + const int segs[4*4] = {0,0,0,1, 0,1,1,1, 1,1,1,0, 1,0,0,0}; + + // Layer portals + dd->begin(DU_DRAW_LINES, 2.0f); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int idx = x+y*w; + const int lh = (int)layer->heights[idx]; + if (lh == 255) continue; + + for (int dir = 0; dir < 4; ++dir) + { + if (layer->cons[idx] & (1<<(dir+4))) + { + const int* seg = &segs[dir*4]; + const float ax = layer->bmin[0] + (x+seg[0])*cs; + const float ay = layer->bmin[1] + (lh+2)*ch; + const float az = layer->bmin[2] + (y+seg[1])*cs; + const float bx = layer->bmin[0] + (x+seg[2])*cs; + const float by = layer->bmin[1] + (lh+2)*ch; + const float bz = layer->bmin[2] + (y+seg[3])*cs; + dd->vertex(ax, ay, az, pcol); + dd->vertex(bx, by, bz, pcol); + } + } + } + } + dd->end(); +} + +void duDebugDrawHeightfieldLayer(duDebugDraw* dd, const struct rcHeightfieldLayer& layer, const int idx) +{ + const float cs = layer.cs; + const float ch = layer.ch; + const int w = layer.width; + const int h = layer.height; + + unsigned int color = duIntToCol(idx+1, 255); + + // Layer bounds + float bmin[3], bmax[3]; + bmin[0] = layer.bmin[0] + layer.minx*cs; + bmin[1] = layer.bmin[1]; + bmin[2] = layer.bmin[2] + layer.miny*cs; + bmax[0] = layer.bmin[0] + (layer.maxx+1)*cs; + bmax[1] = layer.bmax[1]; + bmax[2] = layer.bmin[2] + (layer.maxy+1)*cs; + duDebugDrawBoxWire(dd, bmin[0],bmin[1],bmin[2], bmax[0],bmax[1],bmax[2], duTransCol(color,128), 2.0f); + + // Layer height + dd->begin(DU_DRAW_QUADS); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int lidx = x+y*w; + const int lh = (int)layer.heights[lidx]; + if (h == 0xff) continue; + const unsigned char area = layer.areas[lidx]; + + unsigned int col; + if (area == RC_WALKABLE_AREA) + col = duLerpCol(color, duRGBA(0,192,255,64), 32); + else if (area == RC_NULL_AREA) + col = duLerpCol(color, duRGBA(0,0,0,64), 32); + else + col = duLerpCol(color, dd->areaToCol(area), 32); + + const float fx = layer.bmin[0] + x*cs; + const float fy = layer.bmin[1] + (lh+1)*ch; + const float fz = layer.bmin[2] + y*cs; + + dd->vertex(fx, fy, fz, col); + dd->vertex(fx, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz, col); + } + } + dd->end(); + + // Portals + drawLayerPortals(dd, &layer); +} + +void duDebugDrawHeightfieldLayers(duDebugDraw* dd, const struct rcHeightfieldLayerSet& lset) +{ + if (!dd) return; + for (int i = 0; i < lset.nlayers; ++i) + duDebugDrawHeightfieldLayer(dd, lset.layers[i], i); +} + +/* +void duDebugDrawLayerContours(duDebugDraw* dd, const struct rcLayerContourSet& lcset) +{ + if (!dd) return; + + const float* orig = lcset.bmin; + const float cs = lcset.cs; + const float ch = lcset.ch; + + const unsigned char a = 255;// (unsigned char)(alpha*255.0f); + + const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1}; + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int i = 0; i < lcset.nconts; ++i) + { + const rcLayerContour& c = lcset.conts[i]; + unsigned int color = 0; + + color = duIntToCol(i, a); + + for (int j = 0; j < c.nverts; ++j) + { + const int k = (j+1) % c.nverts; + const unsigned char* va = &c.verts[j*4]; + const unsigned char* vb = &c.verts[k*4]; + const float ax = orig[0] + va[0]*cs; + const float ay = orig[1] + (va[1]+1+(i&1))*ch; + const float az = orig[2] + va[2]*cs; + const float bx = orig[0] + vb[0]*cs; + const float by = orig[1] + (vb[1]+1+(i&1))*ch; + const float bz = orig[2] + vb[2]*cs; + unsigned int col = color; + if ((va[3] & 0xf) != 0xf) + { + col = duRGBA(255,255,255,128); + int d = va[3] & 0xf; + + const float cx = (ax+bx)*0.5f; + const float cy = (ay+by)*0.5f; + const float cz = (az+bz)*0.5f; + + const float dx = cx + offs[d*2+0]*2*cs; + const float dy = cy; + const float dz = cz + offs[d*2+1]*2*cs; + + dd->vertex(cx,cy,cz,duRGBA(255,0,0,255)); + dd->vertex(dx,dy,dz,duRGBA(255,0,0,255)); + } + + duAppendArrow(dd, ax,ay,az, bx,by,bz, 0.0f, cs*0.5f, col); + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 4.0f); + + for (int i = 0; i < lcset.nconts; ++i) + { + const rcLayerContour& c = lcset.conts[i]; + unsigned int color = 0; + + for (int j = 0; j < c.nverts; ++j) + { + const unsigned char* va = &c.verts[j*4]; + + color = duDarkenCol(duIntToCol(i, a)); + if (va[3] & 0x80) + color = duRGBA(255,0,0,255); + + float fx = orig[0] + va[0]*cs; + float fy = orig[1] + (va[1]+1+(i&1))*ch; + float fz = orig[2] + va[2]*cs; + dd->vertex(fx,fy,fz, color); + } + } + dd->end(); +} + +void duDebugDrawLayerPolyMesh(duDebugDraw* dd, const struct rcLayerPolyMesh& lmesh) +{ + if (!dd) return; + + const int nvp = lmesh.nvp; + const float cs = lmesh.cs; + const float ch = lmesh.ch; + const float* orig = lmesh.bmin; + + const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1}; + + dd->begin(DU_DRAW_TRIS); + + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + + unsigned int color; + if (lmesh.areas[i] == RC_WALKABLE_AREA) + color = duRGBA(0,192,255,64); + else if (lmesh.areas[i] == RC_NULL_AREA) + color = duRGBA(0,0,0,64); + else + color = duIntToCol(lmesh.areas[i], 255); + + unsigned short vi[3]; + for (int j = 2; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + vi[0] = p[0]; + vi[1] = p[j-1]; + vi[2] = p[j]; + for (int k = 0; k < 3; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, color); + } + } + } + dd->end(); + + // Draw neighbours edges + const unsigned int coln = duRGBA(0,48,64,32); + dd->begin(DU_DRAW_LINES, 1.5f); + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + if (p[nvp+j] & 0x8000) continue; + const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; + int vi[2] = {p[j], p[nj]}; + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, coln); + } + } + } + dd->end(); + + // Draw boundary edges + const unsigned int colb = duRGBA(0,48,64,220); + dd->begin(DU_DRAW_LINES, 2.5f); + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + if ((p[nvp+j] & 0x8000) == 0) continue; + const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; + int vi[2] = {p[j], p[nj]}; + + unsigned int col = colb; + if ((p[nvp+j] & 0xf) != 0xf) + { + const unsigned short* va = &lmesh.verts[vi[0]*3]; + const unsigned short* vb = &lmesh.verts[vi[1]*3]; + + const float ax = orig[0] + va[0]*cs; + const float ay = orig[1] + (va[1]+1+(i&1))*ch; + const float az = orig[2] + va[2]*cs; + const float bx = orig[0] + vb[0]*cs; + const float by = orig[1] + (vb[1]+1+(i&1))*ch; + const float bz = orig[2] + vb[2]*cs; + + const float cx = (ax+bx)*0.5f; + const float cy = (ay+by)*0.5f; + const float cz = (az+bz)*0.5f; + + int d = p[nvp+j] & 0xf; + + const float dx = cx + offs[d*2+0]*2*cs; + const float dy = cy; + const float dz = cz + offs[d*2+1]*2*cs; + + dd->vertex(cx,cy,cz,duRGBA(255,0,0,255)); + dd->vertex(dx,dy,dz,duRGBA(255,0,0,255)); + + col = duRGBA(255,255,255,128); + } + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, col); + } + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + const unsigned int colv = duRGBA(0,0,0,220); + for (int i = 0; i < lmesh.nverts; ++i) + { + const unsigned short* v = &lmesh.verts[i*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, colv); + } + dd->end(); +} +*/ + +static void getContourCenter(const rcContour* cont, const float* orig, float cs, float ch, float* center) +{ + center[0] = 0; + center[1] = 0; + center[2] = 0; + if (!cont->nverts) + return; + for (int i = 0; i < cont->nverts; ++i) + { + const int* v = &cont->verts[i*4]; + center[0] += (float)v[0]; + center[1] += (float)v[1]; + center[2] += (float)v[2]; + } + const float s = 1.0f / cont->nverts; + center[0] *= s * cs; + center[1] *= s * ch; + center[2] *= s * cs; + center[0] += orig[0]; + center[1] += orig[1] + 4*ch; + center[2] += orig[2]; +} + +static const rcContour* findContourFromSet(const rcContourSet& cset, unsigned short reg) +{ + for (int i = 0; i < cset.nconts; ++i) + { + if (cset.conts[i].reg == reg) + return &cset.conts[i]; + } + return 0; +} + +void duDebugDrawRegionConnections(duDebugDraw* dd, const rcContourSet& cset, const float alpha) +{ + if (!dd) return; + + const float* orig = cset.bmin; + const float cs = cset.cs; + const float ch = cset.ch; + + // Draw centers + float pos[3], pos2[3]; + + unsigned int color = duRGBA(0,0,0,196); + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour* cont = &cset.conts[i]; + getContourCenter(cont, orig, cs, ch, pos); + for (int j = 0; j < cont->nverts; ++j) + { + const int* v = &cont->verts[j*4]; + if (v[3] == 0 || (unsigned short)v[3] < cont->reg) continue; + const rcContour* cont2 = findContourFromSet(cset, (unsigned short)v[3]); + if (cont2) + { + getContourCenter(cont2, orig, cs, ch, pos2); + duAppendArc(dd, pos[0],pos[1],pos[2], pos2[0],pos2[1],pos2[2], 0.25f, 0.6f, 0.6f, color); + } + } + } + + dd->end(); + + unsigned char a = (unsigned char)(alpha * 255.0f); + + dd->begin(DU_DRAW_POINTS, 7.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour* cont = &cset.conts[i]; + unsigned int col = duDarkenCol(duIntToCol(cont->reg,a)); + getContourCenter(cont, orig, cs, ch, pos); + dd->vertex(pos, col); + } + dd->end(); +} + +void duDebugDrawRawContours(duDebugDraw* dd, const rcContourSet& cset, const float alpha) +{ + if (!dd) return; + + const float* orig = cset.bmin; + const float cs = cset.cs; + const float ch = cset.ch; + + const unsigned char a = (unsigned char)(alpha*255.0f); + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& c = cset.conts[i]; + unsigned int color = duIntToCol(c.reg, a); + + for (int j = 0; j < c.nrverts; ++j) + { + const int* v = &c.rverts[j*4]; + float fx = orig[0] + v[0]*cs; + float fy = orig[1] + (v[1]+1+(i&1))*ch; + float fz = orig[2] + v[2]*cs; + dd->vertex(fx,fy,fz,color); + if (j > 0) + dd->vertex(fx,fy,fz,color); + } + // Loop last segment. + const int* v = &c.rverts[0]; + float fx = orig[0] + v[0]*cs; + float fy = orig[1] + (v[1]+1+(i&1))*ch; + float fz = orig[2] + v[2]*cs; + dd->vertex(fx,fy,fz,color); + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 2.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& c = cset.conts[i]; + unsigned int color = duDarkenCol(duIntToCol(c.reg, a)); + + for (int j = 0; j < c.nrverts; ++j) + { + const int* v = &c.rverts[j*4]; + float off = 0; + unsigned int colv = color; + if (v[3] & RC_BORDER_VERTEX) + { + colv = duRGBA(255,255,255,a); + off = ch*2; + } + + float fx = orig[0] + v[0]*cs; + float fy = orig[1] + (v[1]+1+(i&1))*ch + off; + float fz = orig[2] + v[2]*cs; + dd->vertex(fx,fy,fz, colv); + } + } + dd->end(); +} + +void duDebugDrawContours(duDebugDraw* dd, const rcContourSet& cset, const float alpha) +{ + if (!dd) return; + + const float* orig = cset.bmin; + const float cs = cset.cs; + const float ch = cset.ch; + + const unsigned char a = (unsigned char)(alpha*255.0f); + + dd->begin(DU_DRAW_LINES, 2.5f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& c = cset.conts[i]; + if (!c.nverts) + continue; + const unsigned int color = duIntToCol(c.reg, a); + const unsigned int bcolor = duLerpCol(color,duRGBA(255,255,255,a),128); + for (int j = 0, k = c.nverts-1; j < c.nverts; k=j++) + { + const int* va = &c.verts[k*4]; + const int* vb = &c.verts[j*4]; + unsigned int col = (va[3] & RC_AREA_BORDER) ? bcolor : color; + float fx,fy,fz; + fx = orig[0] + va[0]*cs; + fy = orig[1] + (va[1]+1+(i&1))*ch; + fz = orig[2] + va[2]*cs; + dd->vertex(fx,fy,fz, col); + fx = orig[0] + vb[0]*cs; + fy = orig[1] + (vb[1]+1+(i&1))*ch; + fz = orig[2] + vb[2]*cs; + dd->vertex(fx,fy,fz, col); + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& c = cset.conts[i]; + unsigned int color = duDarkenCol(duIntToCol(c.reg, a)); + for (int j = 0; j < c.nverts; ++j) + { + const int* v = &c.verts[j*4]; + float off = 0; + unsigned int colv = color; + if (v[3] & RC_BORDER_VERTEX) + { + colv = duRGBA(255,255,255,a); + off = ch*2; + } + + float fx = orig[0] + v[0]*cs; + float fy = orig[1] + (v[1]+1+(i&1))*ch + off; + float fz = orig[2] + v[2]*cs; + dd->vertex(fx,fy,fz, colv); + } + } + dd->end(); +} + +void duDebugDrawPolyMesh(duDebugDraw* dd, const struct rcPolyMesh& mesh) +{ + if (!dd) return; + + const int nvp = mesh.nvp; + const float cs = mesh.cs; + const float ch = mesh.ch; + const float* orig = mesh.bmin; + + dd->begin(DU_DRAW_TRIS); + + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + const unsigned char area = mesh.areas[i]; + + unsigned int color; + if (area == RC_WALKABLE_AREA) + color = duRGBA(0,192,255,64); + else if (area == RC_NULL_AREA) + color = duRGBA(0,0,0,64); + else + color = dd->areaToCol(area); + + unsigned short vi[3]; + for (int j = 2; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + vi[0] = p[0]; + vi[1] = p[j-1]; + vi[2] = p[j]; + for (int k = 0; k < 3; ++k) + { + const unsigned short* v = &mesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, color); + } + } + } + dd->end(); + + // Draw neighbours edges + const unsigned int coln = duRGBA(0,48,64,32); + dd->begin(DU_DRAW_LINES, 1.5f); + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + if (p[nvp+j] & 0x8000) continue; + const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; + const int vi[2] = {p[j], p[nj]}; + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &mesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, coln); + } + } + } + dd->end(); + + // Draw boundary edges + const unsigned int colb = duRGBA(0,48,64,220); + dd->begin(DU_DRAW_LINES, 2.5f); + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + if ((p[nvp+j] & 0x8000) == 0) continue; + const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; + const int vi[2] = {p[j], p[nj]}; + + unsigned int col = colb; + if ((p[nvp+j] & 0xf) != 0xf) + col = duRGBA(255,255,255,128); + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &mesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, col); + } + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + const unsigned int colv = duRGBA(0,0,0,220); + for (int i = 0; i < mesh.nverts; ++i) + { + const unsigned short* v = &mesh.verts[i*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, colv); + } + dd->end(); +} + +void duDebugDrawPolyMeshDetail(duDebugDraw* dd, const struct rcPolyMeshDetail& dmesh) +{ + if (!dd) return; + + dd->begin(DU_DRAW_TRIS); + + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const unsigned int btris = m[2]; + const int ntris = (int)m[3]; + const float* verts = &dmesh.verts[bverts*3]; + const unsigned char* tris = &dmesh.tris[btris*4]; + + unsigned int color = duIntToCol(i, 192); + + for (int j = 0; j < ntris; ++j) + { + dd->vertex(&verts[tris[j*4+0]*3], color); + dd->vertex(&verts[tris[j*4+1]*3], color); + dd->vertex(&verts[tris[j*4+2]*3], color); + } + } + dd->end(); + + // Internal edges. + dd->begin(DU_DRAW_LINES, 1.0f); + const unsigned int coli = duRGBA(0,0,0,64); + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const unsigned int btris = m[2]; + const int ntris = (int)m[3]; + const float* verts = &dmesh.verts[bverts*3]; + const unsigned char* tris = &dmesh.tris[btris*4]; + + for (int j = 0; j < ntris; ++j) + { + const unsigned char* t = &tris[j*4]; + for (int k = 0, kp = 2; k < 3; kp=k++) + { + unsigned char ef = (t[3] >> (kp*2)) & 0x3; + if (ef == 0) + { + // Internal edge + if (t[kp] < t[k]) + { + dd->vertex(&verts[t[kp]*3], coli); + dd->vertex(&verts[t[k]*3], coli); + } + } + } + } + } + dd->end(); + + // External edges. + dd->begin(DU_DRAW_LINES, 2.0f); + const unsigned int cole = duRGBA(0,0,0,64); + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const unsigned int btris = m[2]; + const int ntris = (int)m[3]; + const float* verts = &dmesh.verts[bverts*3]; + const unsigned char* tris = &dmesh.tris[btris*4]; + + for (int j = 0; j < ntris; ++j) + { + const unsigned char* t = &tris[j*4]; + for (int k = 0, kp = 2; k < 3; kp=k++) + { + unsigned char ef = (t[3] >> (kp*2)) & 0x3; + if (ef != 0) + { + // Ext edge + dd->vertex(&verts[t[kp]*3], cole); + dd->vertex(&verts[t[k]*3], cole); + } + } + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + const unsigned int colv = duRGBA(0,0,0,64); + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const int nverts = (int)m[1]; + const float* verts = &dmesh.verts[bverts*3]; + for (int j = 0; j < nverts; ++j) + dd->vertex(&verts[j*3], colv); + } + dd->end(); +} diff --git a/extern/recastnavigation/DebugUtils/Source/RecastDump.cpp b/extern/recastnavigation/DebugUtils/Source/RecastDump.cpp new file mode 100644 index 000000000..209382515 --- /dev/null +++ b/extern/recastnavigation/DebugUtils/Source/RecastDump.cpp @@ -0,0 +1,451 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastDump.h" + + +duFileIO::~duFileIO() +{ + // Empty +} + +static void ioprintf(duFileIO* io, const char* format, ...) +{ + char line[256]; + va_list ap; + va_start(ap, format); + const int n = vsnprintf(line, sizeof(line), format, ap); + va_end(ap); + if (n > 0) + io->write(line, sizeof(char)*n); +} + +bool duDumpPolyMeshToObj(rcPolyMesh& pmesh, duFileIO* io) +{ + if (!io) + { + printf("duDumpPolyMeshToObj: input IO is null.\n"); + return false; + } + if (!io->isWriting()) + { + printf("duDumpPolyMeshToObj: input IO not writing.\n"); + return false; + } + + const int nvp = pmesh.nvp; + const float cs = pmesh.cs; + const float ch = pmesh.ch; + const float* orig = pmesh.bmin; + + ioprintf(io, "# Recast Navmesh\n"); + ioprintf(io, "o NavMesh\n"); + + ioprintf(io, "\n"); + + for (int i = 0; i < pmesh.nverts; ++i) + { + const unsigned short* v = &pmesh.verts[i*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + ioprintf(io, "v %f %f %f\n", x,y,z); + } + + ioprintf(io, "\n"); + + for (int i = 0; i < pmesh.npolys; ++i) + { + const unsigned short* p = &pmesh.polys[i*nvp*2]; + for (int j = 2; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + ioprintf(io, "f %d %d %d\n", p[0]+1, p[j-1]+1, p[j]+1); + } + } + + return true; +} + +bool duDumpPolyMeshDetailToObj(rcPolyMeshDetail& dmesh, duFileIO* io) +{ + if (!io) + { + printf("duDumpPolyMeshDetailToObj: input IO is null.\n"); + return false; + } + if (!io->isWriting()) + { + printf("duDumpPolyMeshDetailToObj: input IO not writing.\n"); + return false; + } + + ioprintf(io, "# Recast Navmesh\n"); + ioprintf(io, "o NavMesh\n"); + + ioprintf(io, "\n"); + + for (int i = 0; i < dmesh.nverts; ++i) + { + const float* v = &dmesh.verts[i*3]; + ioprintf(io, "v %f %f %f\n", v[0],v[1],v[2]); + } + + ioprintf(io, "\n"); + + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const unsigned int btris = m[2]; + const unsigned int ntris = m[3]; + const unsigned char* tris = &dmesh.tris[btris*4]; + for (unsigned int j = 0; j < ntris; ++j) + { + ioprintf(io, "f %d %d %d\n", + (int)(bverts+tris[j*4+0])+1, + (int)(bverts+tris[j*4+1])+1, + (int)(bverts+tris[j*4+2])+1); + } + } + + return true; +} + +static const int CSET_MAGIC = ('c' << 24) | ('s' << 16) | ('e' << 8) | 't'; +static const int CSET_VERSION = 2; + +bool duDumpContourSet(struct rcContourSet& cset, duFileIO* io) +{ + if (!io) + { + printf("duDumpContourSet: input IO is null.\n"); + return false; + } + if (!io->isWriting()) + { + printf("duDumpContourSet: input IO not writing.\n"); + return false; + } + + io->write(&CSET_MAGIC, sizeof(CSET_MAGIC)); + io->write(&CSET_VERSION, sizeof(CSET_VERSION)); + + io->write(&cset.nconts, sizeof(cset.nconts)); + + io->write(cset.bmin, sizeof(cset.bmin)); + io->write(cset.bmax, sizeof(cset.bmax)); + + io->write(&cset.cs, sizeof(cset.cs)); + io->write(&cset.ch, sizeof(cset.ch)); + + io->write(&cset.width, sizeof(cset.width)); + io->write(&cset.height, sizeof(cset.height)); + io->write(&cset.borderSize, sizeof(cset.borderSize)); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& cont = cset.conts[i]; + io->write(&cont.nverts, sizeof(cont.nverts)); + io->write(&cont.nrverts, sizeof(cont.nrverts)); + io->write(&cont.reg, sizeof(cont.reg)); + io->write(&cont.area, sizeof(cont.area)); + io->write(cont.verts, sizeof(int)*4*cont.nverts); + io->write(cont.rverts, sizeof(int)*4*cont.nrverts); + } + + return true; +} + +bool duReadContourSet(struct rcContourSet& cset, duFileIO* io) +{ + if (!io) + { + printf("duReadContourSet: input IO is null.\n"); + return false; + } + if (!io->isReading()) + { + printf("duReadContourSet: input IO not reading.\n"); + return false; + } + + int magic = 0; + int version = 0; + + io->read(&magic, sizeof(magic)); + io->read(&version, sizeof(version)); + + if (magic != CSET_MAGIC) + { + printf("duReadContourSet: Bad voodoo.\n"); + return false; + } + if (version != CSET_VERSION) + { + printf("duReadContourSet: Bad version.\n"); + return false; + } + + io->read(&cset.nconts, sizeof(cset.nconts)); + + cset.conts = (rcContour*)rcAlloc(sizeof(rcContour)*cset.nconts, RC_ALLOC_PERM); + if (!cset.conts) + { + printf("duReadContourSet: Could not alloc contours (%d)\n", cset.nconts); + return false; + } + memset(cset.conts, 0, sizeof(rcContour)*cset.nconts); + + io->read(cset.bmin, sizeof(cset.bmin)); + io->read(cset.bmax, sizeof(cset.bmax)); + + io->read(&cset.cs, sizeof(cset.cs)); + io->read(&cset.ch, sizeof(cset.ch)); + + io->read(&cset.width, sizeof(cset.width)); + io->read(&cset.height, sizeof(cset.height)); + io->read(&cset.borderSize, sizeof(cset.borderSize)); + + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + io->read(&cont.nverts, sizeof(cont.nverts)); + io->read(&cont.nrverts, sizeof(cont.nrverts)); + io->read(&cont.reg, sizeof(cont.reg)); + io->read(&cont.area, sizeof(cont.area)); + + cont.verts = (int*)rcAlloc(sizeof(int)*4*cont.nverts, RC_ALLOC_PERM); + if (!cont.verts) + { + printf("duReadContourSet: Could not alloc contour verts (%d)\n", cont.nverts); + return false; + } + cont.rverts = (int*)rcAlloc(sizeof(int)*4*cont.nrverts, RC_ALLOC_PERM); + if (!cont.rverts) + { + printf("duReadContourSet: Could not alloc contour rverts (%d)\n", cont.nrverts); + return false; + } + + io->read(cont.verts, sizeof(int)*4*cont.nverts); + io->read(cont.rverts, sizeof(int)*4*cont.nrverts); + } + + return true; +} + + +static const int CHF_MAGIC = ('r' << 24) | ('c' << 16) | ('h' << 8) | 'f'; +static const int CHF_VERSION = 3; + +bool duDumpCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io) +{ + if (!io) + { + printf("duDumpCompactHeightfield: input IO is null.\n"); + return false; + } + if (!io->isWriting()) + { + printf("duDumpCompactHeightfield: input IO not writing.\n"); + return false; + } + + io->write(&CHF_MAGIC, sizeof(CHF_MAGIC)); + io->write(&CHF_VERSION, sizeof(CHF_VERSION)); + + io->write(&chf.width, sizeof(chf.width)); + io->write(&chf.height, sizeof(chf.height)); + io->write(&chf.spanCount, sizeof(chf.spanCount)); + + io->write(&chf.walkableHeight, sizeof(chf.walkableHeight)); + io->write(&chf.walkableClimb, sizeof(chf.walkableClimb)); + io->write(&chf.borderSize, sizeof(chf.borderSize)); + + io->write(&chf.maxDistance, sizeof(chf.maxDistance)); + io->write(&chf.maxRegions, sizeof(chf.maxRegions)); + + io->write(chf.bmin, sizeof(chf.bmin)); + io->write(chf.bmax, sizeof(chf.bmax)); + + io->write(&chf.cs, sizeof(chf.cs)); + io->write(&chf.ch, sizeof(chf.ch)); + + int tmp = 0; + if (chf.cells) tmp |= 1; + if (chf.spans) tmp |= 2; + if (chf.dist) tmp |= 4; + if (chf.areas) tmp |= 8; + + io->write(&tmp, sizeof(tmp)); + + if (chf.cells) + io->write(chf.cells, sizeof(rcCompactCell)*chf.width*chf.height); + if (chf.spans) + io->write(chf.spans, sizeof(rcCompactSpan)*chf.spanCount); + if (chf.dist) + io->write(chf.dist, sizeof(unsigned short)*chf.spanCount); + if (chf.areas) + io->write(chf.areas, sizeof(unsigned char)*chf.spanCount); + + return true; +} + +bool duReadCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io) +{ + if (!io) + { + printf("duReadCompactHeightfield: input IO is null.\n"); + return false; + } + if (!io->isReading()) + { + printf("duReadCompactHeightfield: input IO not reading.\n"); + return false; + } + + int magic = 0; + int version = 0; + + io->read(&magic, sizeof(magic)); + io->read(&version, sizeof(version)); + + if (magic != CHF_MAGIC) + { + printf("duReadCompactHeightfield: Bad voodoo.\n"); + return false; + } + if (version != CHF_VERSION) + { + printf("duReadCompactHeightfield: Bad version.\n"); + return false; + } + + io->read(&chf.width, sizeof(chf.width)); + io->read(&chf.height, sizeof(chf.height)); + io->read(&chf.spanCount, sizeof(chf.spanCount)); + + io->read(&chf.walkableHeight, sizeof(chf.walkableHeight)); + io->read(&chf.walkableClimb, sizeof(chf.walkableClimb)); + io->read(&chf.borderSize, sizeof(chf.borderSize)); + + io->read(&chf.maxDistance, sizeof(chf.maxDistance)); + io->read(&chf.maxRegions, sizeof(chf.maxRegions)); + + io->read(chf.bmin, sizeof(chf.bmin)); + io->read(chf.bmax, sizeof(chf.bmax)); + + io->read(&chf.cs, sizeof(chf.cs)); + io->read(&chf.ch, sizeof(chf.ch)); + + int tmp = 0; + io->read(&tmp, sizeof(tmp)); + + if (tmp & 1) + { + chf.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell)*chf.width*chf.height, RC_ALLOC_PERM); + if (!chf.cells) + { + printf("duReadCompactHeightfield: Could not alloc cells (%d)\n", chf.width*chf.height); + return false; + } + io->read(chf.cells, sizeof(rcCompactCell)*chf.width*chf.height); + } + if (tmp & 2) + { + chf.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan)*chf.spanCount, RC_ALLOC_PERM); + if (!chf.spans) + { + printf("duReadCompactHeightfield: Could not alloc spans (%d)\n", chf.spanCount); + return false; + } + io->read(chf.spans, sizeof(rcCompactSpan)*chf.spanCount); + } + if (tmp & 4) + { + chf.dist = (unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_PERM); + if (!chf.dist) + { + printf("duReadCompactHeightfield: Could not alloc dist (%d)\n", chf.spanCount); + return false; + } + io->read(chf.dist, sizeof(unsigned short)*chf.spanCount); + } + if (tmp & 8) + { + chf.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_PERM); + if (!chf.areas) + { + printf("duReadCompactHeightfield: Could not alloc areas (%d)\n", chf.spanCount); + return false; + } + io->read(chf.areas, sizeof(unsigned char)*chf.spanCount); + } + + return true; +} + + +static void logLine(rcContext& ctx, rcTimerLabel label, const char* name, const float pc) +{ + const int t = ctx.getAccumulatedTime(label); + if (t < 0) return; + ctx.log(RC_LOG_PROGRESS, "%s:\t%.2fms\t(%.1f%%)", name, t/1000.0f, t*pc); +} + +void duLogBuildTimes(rcContext& ctx, const int totalTimeUsec) +{ + const float pc = 100.0f / totalTimeUsec; + + ctx.log(RC_LOG_PROGRESS, "Build Times"); + logLine(ctx, RC_TIMER_RASTERIZE_TRIANGLES, "- Rasterize", pc); + logLine(ctx, RC_TIMER_BUILD_COMPACTHEIGHTFIELD, "- Build Compact", pc); + logLine(ctx, RC_TIMER_FILTER_BORDER, "- Filter Border", pc); + logLine(ctx, RC_TIMER_FILTER_WALKABLE, "- Filter Walkable", pc); + logLine(ctx, RC_TIMER_ERODE_AREA, "- Erode Area", pc); + logLine(ctx, RC_TIMER_MEDIAN_AREA, "- Median Area", pc); + logLine(ctx, RC_TIMER_MARK_BOX_AREA, "- Mark Box Area", pc); + logLine(ctx, RC_TIMER_MARK_CONVEXPOLY_AREA, "- Mark Convex Area", pc); + logLine(ctx, RC_TIMER_MARK_CYLINDER_AREA, "- Mark Cylinder Area", pc); + logLine(ctx, RC_TIMER_BUILD_DISTANCEFIELD, "- Build Distance Field", pc); + logLine(ctx, RC_TIMER_BUILD_DISTANCEFIELD_DIST, " - Distance", pc); + logLine(ctx, RC_TIMER_BUILD_DISTANCEFIELD_BLUR, " - Blur", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS, "- Build Regions", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS_WATERSHED, " - Watershed", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS_EXPAND, " - Expand", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS_FLOOD, " - Find Basins", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS_FILTER, " - Filter", pc); + logLine(ctx, RC_TIMER_BUILD_LAYERS, "- Build Layers", pc); + logLine(ctx, RC_TIMER_BUILD_CONTOURS, "- Build Contours", pc); + logLine(ctx, RC_TIMER_BUILD_CONTOURS_TRACE, " - Trace", pc); + logLine(ctx, RC_TIMER_BUILD_CONTOURS_SIMPLIFY, " - Simplify", pc); + logLine(ctx, RC_TIMER_BUILD_POLYMESH, "- Build Polymesh", pc); + logLine(ctx, RC_TIMER_BUILD_POLYMESHDETAIL, "- Build Polymesh Detail", pc); + logLine(ctx, RC_TIMER_MERGE_POLYMESH, "- Merge Polymeshes", pc); + logLine(ctx, RC_TIMER_MERGE_POLYMESHDETAIL, "- Merge Polymesh Details", pc); + ctx.log(RC_LOG_PROGRESS, "=== TOTAL:\t%.2fms", totalTimeUsec/1000.0f); +} + diff --git a/extern/recastnavigation/Detour/CMakeLists.txt b/extern/recastnavigation/Detour/CMakeLists.txt new file mode 100644 index 000000000..de88111d5 --- /dev/null +++ b/extern/recastnavigation/Detour/CMakeLists.txt @@ -0,0 +1,29 @@ +file(GLOB SOURCES Source/*.cpp) + +if(RECASTNAVIGATION_STATIC) + add_library(Detour STATIC ${SOURCES}) +else() + add_library(Detour SHARED ${SOURCES}) +endif() + +add_library(RecastNavigation::Detour ALIAS Detour) + +set(Detour_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Include") + +target_include_directories(Detour PUBLIC + "$" +) + +set_target_properties(Detour PROPERTIES + SOVERSION ${SOVERSION} + VERSION ${VERSION} + ) + +install(TARGETS Detour + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + COMPONENT library + ) + +file(GLOB INCLUDES Include/*.h) +install(FILES ${INCLUDES} DESTINATION include) diff --git a/extern/recastnavigation/Detour/Include/DetourAlloc.h b/extern/recastnavigation/Detour/Include/DetourAlloc.h new file mode 100644 index 000000000..f87b454ac --- /dev/null +++ b/extern/recastnavigation/Detour/Include/DetourAlloc.h @@ -0,0 +1,61 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURALLOCATOR_H +#define DETOURALLOCATOR_H + +#include + +/// Provides hint values to the memory allocator on how long the +/// memory is expected to be used. +enum dtAllocHint +{ + DT_ALLOC_PERM, ///< Memory persist after a function call. + DT_ALLOC_TEMP ///< Memory used temporarily within a function. +}; + +/// A memory allocation function. +// @param[in] size The size, in bytes of memory, to allocate. +// @param[in] rcAllocHint A hint to the allocator on how long the memory is expected to be in use. +// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed. +/// @see dtAllocSetCustom +typedef void* (dtAllocFunc)(size_t size, dtAllocHint hint); + +/// A memory deallocation function. +/// @param[in] ptr A pointer to a memory block previously allocated using #dtAllocFunc. +/// @see dtAllocSetCustom +typedef void (dtFreeFunc)(void* ptr); + +/// Sets the base custom allocation functions to be used by Detour. +/// @param[in] allocFunc The memory allocation function to be used by #dtAlloc +/// @param[in] freeFunc The memory de-allocation function to be used by #dtFree +void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc); + +/// Allocates a memory block. +/// @param[in] size The size, in bytes of memory, to allocate. +/// @param[in] hint A hint to the allocator on how long the memory is expected to be in use. +/// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed. +/// @see dtFree +void* dtAlloc(size_t size, dtAllocHint hint); + +/// Deallocates a memory block. +/// @param[in] ptr A pointer to a memory block previously allocated using #dtAlloc. +/// @see dtAlloc +void dtFree(void* ptr); + +#endif diff --git a/extern/recastnavigation/Detour/Include/DetourAssert.h b/extern/recastnavigation/Detour/Include/DetourAssert.h new file mode 100644 index 000000000..e05fd66fa --- /dev/null +++ b/extern/recastnavigation/Detour/Include/DetourAssert.h @@ -0,0 +1,56 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURASSERT_H +#define DETOURASSERT_H + +// Note: This header file's only purpose is to include define assert. +// Feel free to change the file and include your own implementation instead. + +#ifdef NDEBUG + +// From http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/ +# define dtAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false) + +#else + +/// An assertion failure function. +// @param[in] expression asserted expression. +// @param[in] file Filename of the failed assertion. +// @param[in] line Line number of the failed assertion. +/// @see dtAssertFailSetCustom +typedef void (dtAssertFailFunc)(const char* expression, const char* file, int line); + +/// Sets the base custom assertion failure function to be used by Detour. +/// @param[in] assertFailFunc The function to be invoked in case of failure of #dtAssert +void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc); + +/// Gets the base custom assertion failure function to be used by Detour. +dtAssertFailFunc* dtAssertFailGetCustom(); + +# include +# define dtAssert(expression) \ + { \ + dtAssertFailFunc* failFunc = dtAssertFailGetCustom(); \ + if(failFunc == NULL) { assert(expression); } \ + else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \ + } + +#endif + +#endif // DETOURASSERT_H diff --git a/extern/recastnavigation/Detour/Include/DetourCommon.h b/extern/recastnavigation/Detour/Include/DetourCommon.h new file mode 100644 index 000000000..739858cd9 --- /dev/null +++ b/extern/recastnavigation/Detour/Include/DetourCommon.h @@ -0,0 +1,550 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURCOMMON_H +#define DETOURCOMMON_H + +#include "DetourMath.h" +#include + +/** +@defgroup detour Detour + +Members in this module are used to create, manipulate, and query navigation +meshes. + +@note This is a summary list of members. Use the index or search +feature to find minor members. +*/ + +/// @name General helper functions +/// @{ + +/// Used to ignore a function parameter. VS complains about unused parameters +/// and this silences the warning. +/// @param [in] _ Unused parameter +template void dtIgnoreUnused(const T&) { } + +/// Swaps the values of the two parameters. +/// @param[in,out] a Value A +/// @param[in,out] b Value B +template inline void dtSwap(T& a, T& b) { T t = a; a = b; b = t; } + +/// Returns the minimum of two values. +/// @param[in] a Value A +/// @param[in] b Value B +/// @return The minimum of the two values. +template inline T dtMin(T a, T b) { return a < b ? a : b; } + +/// Returns the maximum of two values. +/// @param[in] a Value A +/// @param[in] b Value B +/// @return The maximum of the two values. +template inline T dtMax(T a, T b) { return a > b ? a : b; } + +/// Returns the absolute value. +/// @param[in] a The value. +/// @return The absolute value of the specified value. +template inline T dtAbs(T a) { return a < 0 ? -a : a; } + +/// Returns the square of the value. +/// @param[in] a The value. +/// @return The square of the value. +template inline T dtSqr(T a) { return a*a; } + +/// Clamps the value to the specified range. +/// @param[in] v The value to clamp. +/// @param[in] mn The minimum permitted return value. +/// @param[in] mx The maximum permitted return value. +/// @return The value, clamped to the specified range. +template inline T dtClamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); } + +/// @} +/// @name Vector helper functions. +/// @{ + +/// Derives the cross product of two vectors. (@p v1 x @p v2) +/// @param[out] dest The cross product. [(x, y, z)] +/// @param[in] v1 A Vector [(x, y, z)] +/// @param[in] v2 A vector [(x, y, z)] +inline void dtVcross(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[1]*v2[2] - v1[2]*v2[1]; + dest[1] = v1[2]*v2[0] - v1[0]*v2[2]; + dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; +} + +/// Derives the dot product of two vectors. (@p v1 . @p v2) +/// @param[in] v1 A Vector [(x, y, z)] +/// @param[in] v2 A vector [(x, y, z)] +/// @return The dot product. +inline float dtVdot(const float* v1, const float* v2) +{ + return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; +} + +/// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s)) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v1 The base vector. [(x, y, z)] +/// @param[in] v2 The vector to scale and add to @p v1. [(x, y, z)] +/// @param[in] s The amount to scale @p v2 by before adding to @p v1. +inline void dtVmad(float* dest, const float* v1, const float* v2, const float s) +{ + dest[0] = v1[0]+v2[0]*s; + dest[1] = v1[1]+v2[1]*s; + dest[2] = v1[2]+v2[2]*s; +} + +/// Performs a linear interpolation between two vectors. (@p v1 toward @p v2) +/// @param[out] dest The result vector. [(x, y, x)] +/// @param[in] v1 The starting vector. +/// @param[in] v2 The destination vector. +/// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0] +inline void dtVlerp(float* dest, const float* v1, const float* v2, const float t) +{ + dest[0] = v1[0]+(v2[0]-v1[0])*t; + dest[1] = v1[1]+(v2[1]-v1[1])*t; + dest[2] = v1[2]+(v2[2]-v1[2])*t; +} + +/// Performs a vector addition. (@p v1 + @p v2) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v1 The base vector. [(x, y, z)] +/// @param[in] v2 The vector to add to @p v1. [(x, y, z)] +inline void dtVadd(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[0]+v2[0]; + dest[1] = v1[1]+v2[1]; + dest[2] = v1[2]+v2[2]; +} + +/// Performs a vector subtraction. (@p v1 - @p v2) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v1 The base vector. [(x, y, z)] +/// @param[in] v2 The vector to subtract from @p v1. [(x, y, z)] +inline void dtVsub(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[0]-v2[0]; + dest[1] = v1[1]-v2[1]; + dest[2] = v1[2]-v2[2]; +} + +/// Scales the vector by the specified value. (@p v * @p t) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v The vector to scale. [(x, y, z)] +/// @param[in] t The scaling factor. +inline void dtVscale(float* dest, const float* v, const float t) +{ + dest[0] = v[0]*t; + dest[1] = v[1]*t; + dest[2] = v[2]*t; +} + +/// Selects the minimum value of each element from the specified vectors. +/// @param[in,out] mn A vector. (Will be updated with the result.) [(x, y, z)] +/// @param[in] v A vector. [(x, y, z)] +inline void dtVmin(float* mn, const float* v) +{ + mn[0] = dtMin(mn[0], v[0]); + mn[1] = dtMin(mn[1], v[1]); + mn[2] = dtMin(mn[2], v[2]); +} + +/// Selects the maximum value of each element from the specified vectors. +/// @param[in,out] mx A vector. (Will be updated with the result.) [(x, y, z)] +/// @param[in] v A vector. [(x, y, z)] +inline void dtVmax(float* mx, const float* v) +{ + mx[0] = dtMax(mx[0], v[0]); + mx[1] = dtMax(mx[1], v[1]); + mx[2] = dtMax(mx[2], v[2]); +} + +/// Sets the vector elements to the specified values. +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] x The x-value of the vector. +/// @param[in] y The y-value of the vector. +/// @param[in] z The z-value of the vector. +inline void dtVset(float* dest, const float x, const float y, const float z) +{ + dest[0] = x; dest[1] = y; dest[2] = z; +} + +/// Performs a vector copy. +/// @param[out] dest The result. [(x, y, z)] +/// @param[in] a The vector to copy. [(x, y, z)] +inline void dtVcopy(float* dest, const float* a) +{ + dest[0] = a[0]; + dest[1] = a[1]; + dest[2] = a[2]; +} + +/// Derives the scalar length of the vector. +/// @param[in] v The vector. [(x, y, z)] +/// @return The scalar length of the vector. +inline float dtVlen(const float* v) +{ + return dtMathSqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); +} + +/// Derives the square of the scalar length of the vector. (len * len) +/// @param[in] v The vector. [(x, y, z)] +/// @return The square of the scalar length of the vector. +inline float dtVlenSqr(const float* v) +{ + return v[0]*v[0] + v[1]*v[1] + v[2]*v[2]; +} + +/// Returns the distance between two points. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @return The distance between the two points. +inline float dtVdist(const float* v1, const float* v2) +{ + const float dx = v2[0] - v1[0]; + const float dy = v2[1] - v1[1]; + const float dz = v2[2] - v1[2]; + return dtMathSqrtf(dx*dx + dy*dy + dz*dz); +} + +/// Returns the square of the distance between two points. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @return The square of the distance between the two points. +inline float dtVdistSqr(const float* v1, const float* v2) +{ + const float dx = v2[0] - v1[0]; + const float dy = v2[1] - v1[1]; + const float dz = v2[2] - v1[2]; + return dx*dx + dy*dy + dz*dz; +} + +/// Derives the distance between the specified points on the xz-plane. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @return The distance between the point on the xz-plane. +/// +/// The vectors are projected onto the xz-plane, so the y-values are ignored. +inline float dtVdist2D(const float* v1, const float* v2) +{ + const float dx = v2[0] - v1[0]; + const float dz = v2[2] - v1[2]; + return dtMathSqrtf(dx*dx + dz*dz); +} + +/// Derives the square of the distance between the specified points on the xz-plane. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @return The square of the distance between the point on the xz-plane. +inline float dtVdist2DSqr(const float* v1, const float* v2) +{ + const float dx = v2[0] - v1[0]; + const float dz = v2[2] - v1[2]; + return dx*dx + dz*dz; +} + +/// Normalizes the vector. +/// @param[in,out] v The vector to normalize. [(x, y, z)] +inline void dtVnormalize(float* v) +{ + float d = 1.0f / dtMathSqrtf(dtSqr(v[0]) + dtSqr(v[1]) + dtSqr(v[2])); + v[0] *= d; + v[1] *= d; + v[2] *= d; +} + +/// Performs a 'sloppy' colocation check of the specified points. +/// @param[in] p0 A point. [(x, y, z)] +/// @param[in] p1 A point. [(x, y, z)] +/// @return True if the points are considered to be at the same location. +/// +/// Basically, this function will return true if the specified points are +/// close enough to eachother to be considered colocated. +inline bool dtVequal(const float* p0, const float* p1) +{ + static const float thr = dtSqr(1.0f/16384.0f); + const float d = dtVdistSqr(p0, p1); + return d < thr; +} + +/// Derives the dot product of two vectors on the xz-plane. (@p u . @p v) +/// @param[in] u A vector [(x, y, z)] +/// @param[in] v A vector [(x, y, z)] +/// @return The dot product on the xz-plane. +/// +/// The vectors are projected onto the xz-plane, so the y-values are ignored. +inline float dtVdot2D(const float* u, const float* v) +{ + return u[0]*v[0] + u[2]*v[2]; +} + +/// Derives the xz-plane 2D perp product of the two vectors. (uz*vx - ux*vz) +/// @param[in] u The LHV vector [(x, y, z)] +/// @param[in] v The RHV vector [(x, y, z)] +/// @return The dot product on the xz-plane. +/// +/// The vectors are projected onto the xz-plane, so the y-values are ignored. +inline float dtVperp2D(const float* u, const float* v) +{ + return u[2]*v[0] - u[0]*v[2]; +} + +/// @} +/// @name Computational geometry helper functions. +/// @{ + +/// Derives the signed xz-plane area of the triangle ABC, or the relationship of line AB to point C. +/// @param[in] a Vertex A. [(x, y, z)] +/// @param[in] b Vertex B. [(x, y, z)] +/// @param[in] c Vertex C. [(x, y, z)] +/// @return The signed xz-plane area of the triangle. +inline float dtTriArea2D(const float* a, const float* b, const float* c) +{ + const float abx = b[0] - a[0]; + const float abz = b[2] - a[2]; + const float acx = c[0] - a[0]; + const float acz = c[2] - a[2]; + return acx*abz - abx*acz; +} + +/// Determines if two axis-aligned bounding boxes overlap. +/// @param[in] amin Minimum bounds of box A. [(x, y, z)] +/// @param[in] amax Maximum bounds of box A. [(x, y, z)] +/// @param[in] bmin Minimum bounds of box B. [(x, y, z)] +/// @param[in] bmax Maximum bounds of box B. [(x, y, z)] +/// @return True if the two AABB's overlap. +/// @see dtOverlapBounds +inline bool dtOverlapQuantBounds(const unsigned short amin[3], const unsigned short amax[3], + const unsigned short bmin[3], const unsigned short bmax[3]) +{ + bool overlap = true; + overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap; + overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap; + overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; + return overlap; +} + +/// Determines if two axis-aligned bounding boxes overlap. +/// @param[in] amin Minimum bounds of box A. [(x, y, z)] +/// @param[in] amax Maximum bounds of box A. [(x, y, z)] +/// @param[in] bmin Minimum bounds of box B. [(x, y, z)] +/// @param[in] bmax Maximum bounds of box B. [(x, y, z)] +/// @return True if the two AABB's overlap. +/// @see dtOverlapQuantBounds +inline bool dtOverlapBounds(const float* amin, const float* amax, + const float* bmin, const float* bmax) +{ + bool overlap = true; + overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap; + overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap; + overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; + return overlap; +} + +/// Derives the closest point on a triangle from the specified reference point. +/// @param[out] closest The closest point on the triangle. +/// @param[in] p The reference point from which to test. [(x, y, z)] +/// @param[in] a Vertex A of triangle ABC. [(x, y, z)] +/// @param[in] b Vertex B of triangle ABC. [(x, y, z)] +/// @param[in] c Vertex C of triangle ABC. [(x, y, z)] +void dtClosestPtPointTriangle(float* closest, const float* p, + const float* a, const float* b, const float* c); + +/// Derives the y-axis height of the closest point on the triangle from the specified reference point. +/// @param[in] p The reference point from which to test. [(x, y, z)] +/// @param[in] a Vertex A of triangle ABC. [(x, y, z)] +/// @param[in] b Vertex B of triangle ABC. [(x, y, z)] +/// @param[in] c Vertex C of triangle ABC. [(x, y, z)] +/// @param[out] h The resulting height. +bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h); + +bool dtIntersectSegmentPoly2D(const float* p0, const float* p1, + const float* verts, int nverts, + float& tmin, float& tmax, + int& segMin, int& segMax); + +bool dtIntersectSegSeg2D(const float* ap, const float* aq, + const float* bp, const float* bq, + float& s, float& t); + +/// Determines if the specified point is inside the convex polygon on the xz-plane. +/// @param[in] pt The point to check. [(x, y, z)] +/// @param[in] verts The polygon vertices. [(x, y, z) * @p nverts] +/// @param[in] nverts The number of vertices. [Limit: >= 3] +/// @return True if the point is inside the polygon. +bool dtPointInPolygon(const float* pt, const float* verts, const int nverts); + +bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, + float* ed, float* et); + +float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t); + +/// Derives the centroid of a convex polygon. +/// @param[out] tc The centroid of the polgyon. [(x, y, z)] +/// @param[in] idx The polygon indices. [(vertIndex) * @p nidx] +/// @param[in] nidx The number of indices in the polygon. [Limit: >= 3] +/// @param[in] verts The polygon vertices. [(x, y, z) * vertCount] +void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts); + +/// Determines if the two convex polygons overlap on the xz-plane. +/// @param[in] polya Polygon A vertices. [(x, y, z) * @p npolya] +/// @param[in] npolya The number of vertices in polygon A. +/// @param[in] polyb Polygon B vertices. [(x, y, z) * @p npolyb] +/// @param[in] npolyb The number of vertices in polygon B. +/// @return True if the two polygons overlap. +bool dtOverlapPolyPoly2D(const float* polya, const int npolya, + const float* polyb, const int npolyb); + +/// @} +/// @name Miscellanious functions. +/// @{ + +inline unsigned int dtNextPow2(unsigned int v) +{ + v--; + v |= v >> 1; + v |= v >> 2; + v |= v >> 4; + v |= v >> 8; + v |= v >> 16; + v++; + return v; +} + +inline unsigned int dtIlog2(unsigned int v) +{ + unsigned int r; + unsigned int shift; + r = (v > 0xffff) << 4; v >>= r; + shift = (v > 0xff) << 3; v >>= shift; r |= shift; + shift = (v > 0xf) << 2; v >>= shift; r |= shift; + shift = (v > 0x3) << 1; v >>= shift; r |= shift; + r |= (v >> 1); + return r; +} + +inline int dtAlign4(int x) { return (x+3) & ~3; } + +inline int dtOppositeTile(int side) { return (side+4) & 0x7; } + +inline void dtSwapByte(unsigned char* a, unsigned char* b) +{ + unsigned char tmp = *a; + *a = *b; + *b = tmp; +} + +inline void dtSwapEndian(unsigned short* v) +{ + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+1); +} + +inline void dtSwapEndian(short* v) +{ + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+1); +} + +inline void dtSwapEndian(unsigned int* v) +{ + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2); +} + +inline void dtSwapEndian(int* v) +{ + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2); +} + +inline void dtSwapEndian(float* v) +{ + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2); +} + +void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, + const float s, const float t, float* out); + +template +TypeToRetrieveAs* dtGetThenAdvanceBufferPointer(const unsigned char*& buffer, const size_t distanceToAdvance) +{ + TypeToRetrieveAs* returnPointer = reinterpret_cast(buffer); + buffer += distanceToAdvance; + return returnPointer; +} + +template +TypeToRetrieveAs* dtGetThenAdvanceBufferPointer(unsigned char*& buffer, const size_t distanceToAdvance) +{ + TypeToRetrieveAs* returnPointer = reinterpret_cast(buffer); + buffer += distanceToAdvance; + return returnPointer; +} + + +/// @} + +#endif // DETOURCOMMON_H + +/////////////////////////////////////////////////////////////////////////// + +// This section contains detailed documentation for members that don't have +// a source file. It reduces clutter in the main section of the header. + +/** + +@fn float dtTriArea2D(const float* a, const float* b, const float* c) +@par + +The vertices are projected onto the xz-plane, so the y-values are ignored. + +This is a low cost function than can be used for various purposes. Its main purpose +is for point/line relationship testing. + +In all cases: A value of zero indicates that all vertices are collinear or represent the same point. +(On the xz-plane.) + +When used for point/line relationship tests, AB usually represents a line against which +the C point is to be tested. In this case: + +A positive value indicates that point C is to the left of line AB, looking from A toward B.
+A negative value indicates that point C is to the right of lineAB, looking from A toward B. + +When used for evaluating a triangle: + +The absolute value of the return value is two times the area of the triangle when it is +projected onto the xz-plane. + +A positive return value indicates: + +
    +
  • The vertices are wrapped in the normal Detour wrap direction.
    • +
  • The triangle's 3D face normal is in the general up direction.
    • +
+ +A negative return value indicates: + +
    +
  • The vertices are reverse wrapped. (Wrapped opposite the normal Detour wrap direction.)
    • +
  • The triangle's 3D face normal is in the general down direction.
    • +
+ +*/ diff --git a/extern/recastnavigation/Detour/Include/DetourMath.h b/extern/recastnavigation/Detour/Include/DetourMath.h new file mode 100644 index 000000000..95e14f884 --- /dev/null +++ b/extern/recastnavigation/Detour/Include/DetourMath.h @@ -0,0 +1,20 @@ +/** +@defgroup detour Detour + +Members in this module are wrappers around the standard math library +*/ + +#ifndef DETOURMATH_H +#define DETOURMATH_H + +#include + +inline float dtMathFabsf(float x) { return fabsf(x); } +inline float dtMathSqrtf(float x) { return sqrtf(x); } +inline float dtMathFloorf(float x) { return floorf(x); } +inline float dtMathCeilf(float x) { return ceilf(x); } +inline float dtMathCosf(float x) { return cosf(x); } +inline float dtMathSinf(float x) { return sinf(x); } +inline float dtMathAtan2f(float y, float x) { return atan2f(y, x); } + +#endif diff --git a/extern/recastnavigation/Detour/Include/DetourNavMesh.h b/extern/recastnavigation/Detour/Include/DetourNavMesh.h new file mode 100644 index 000000000..02ee5e78c --- /dev/null +++ b/extern/recastnavigation/Detour/Include/DetourNavMesh.h @@ -0,0 +1,765 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURNAVMESH_H +#define DETOURNAVMESH_H + +#include "DetourAlloc.h" +#include "DetourStatus.h" + +// Undefine (or define in a build cofnig) the following line to use 64bit polyref. +// Generally not needed, useful for very large worlds. +// Note: tiles build using 32bit refs are not compatible with 64bit refs! +//#define DT_POLYREF64 1 + +#ifdef DT_POLYREF64 +// TODO: figure out a multiplatform version of uint64_t +// - maybe: https://code.google.com/p/msinttypes/ +// - or: http://www.azillionmonkeys.com/qed/pstdint.h +#include +#endif + +// Note: If you want to use 64-bit refs, change the types of both dtPolyRef & dtTileRef. +// It is also recommended that you change dtHashRef() to a proper 64-bit hash. + +/// A handle to a polygon within a navigation mesh tile. +/// @ingroup detour +#ifdef DT_POLYREF64 +static const unsigned int DT_SALT_BITS = 16; +static const unsigned int DT_TILE_BITS = 28; +static const unsigned int DT_POLY_BITS = 20; +typedef uint64_t dtPolyRef; +#else +typedef unsigned int dtPolyRef; +#endif + +/// A handle to a tile within a navigation mesh. +/// @ingroup detour +#ifdef DT_POLYREF64 +typedef uint64_t dtTileRef; +#else +typedef unsigned int dtTileRef; +#endif + +/// The maximum number of vertices per navigation polygon. +/// @ingroup detour +static const int DT_VERTS_PER_POLYGON = 6; + +/// @{ +/// @name Tile Serialization Constants +/// These constants are used to detect whether a navigation tile's data +/// and state format is compatible with the current build. +/// + +/// A magic number used to detect compatibility of navigation tile data. +static const int DT_NAVMESH_MAGIC = 'D'<<24 | 'N'<<16 | 'A'<<8 | 'V'; + +/// A version number used to detect compatibility of navigation tile data. +static const int DT_NAVMESH_VERSION = 7; + +/// A magic number used to detect the compatibility of navigation tile states. +static const int DT_NAVMESH_STATE_MAGIC = 'D'<<24 | 'N'<<16 | 'M'<<8 | 'S'; + +/// A version number used to detect compatibility of navigation tile states. +static const int DT_NAVMESH_STATE_VERSION = 1; + +/// @} + +/// A flag that indicates that an entity links to an external entity. +/// (E.g. A polygon edge is a portal that links to another polygon.) +static const unsigned short DT_EXT_LINK = 0x8000; + +/// A value that indicates the entity does not link to anything. +static const unsigned int DT_NULL_LINK = 0xffffffff; + +/// A flag that indicates that an off-mesh connection can be traversed in both directions. (Is bidirectional.) +static const unsigned int DT_OFFMESH_CON_BIDIR = 1; + +/// The maximum number of user defined area ids. +/// @ingroup detour +static const int DT_MAX_AREAS = 64; + +/// Tile flags used for various functions and fields. +/// For an example, see dtNavMesh::addTile(). +enum dtTileFlags +{ + /// The navigation mesh owns the tile memory and is responsible for freeing it. + DT_TILE_FREE_DATA = 0x01, +}; + +/// Vertex flags returned by dtNavMeshQuery::findStraightPath. +enum dtStraightPathFlags +{ + DT_STRAIGHTPATH_START = 0x01, ///< The vertex is the start position in the path. + DT_STRAIGHTPATH_END = 0x02, ///< The vertex is the end position in the path. + DT_STRAIGHTPATH_OFFMESH_CONNECTION = 0x04, ///< The vertex is the start of an off-mesh connection. +}; + +/// Options for dtNavMeshQuery::findStraightPath. +enum dtStraightPathOptions +{ + DT_STRAIGHTPATH_AREA_CROSSINGS = 0x01, ///< Add a vertex at every polygon edge crossing where area changes. + DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02, ///< Add a vertex at every polygon edge crossing. +}; + + +/// Options for dtNavMeshQuery::initSlicedFindPath and updateSlicedFindPath +enum dtFindPathOptions +{ + DT_FINDPATH_ANY_ANGLE = 0x02, ///< use raycasts during pathfind to "shortcut" (raycast still consider costs) +}; + +/// Options for dtNavMeshQuery::raycast +enum dtRaycastOptions +{ + DT_RAYCAST_USE_COSTS = 0x01, ///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost +}; + + +/// Limit raycasting during any angle pahfinding +/// The limit is given as a multiple of the character radius +static const float DT_RAY_CAST_LIMIT_PROPORTIONS = 50.0f; + +/// Flags representing the type of a navigation mesh polygon. +enum dtPolyTypes +{ + /// The polygon is a standard convex polygon that is part of the surface of the mesh. + DT_POLYTYPE_GROUND = 0, + /// The polygon is an off-mesh connection consisting of two vertices. + DT_POLYTYPE_OFFMESH_CONNECTION = 1, +}; + + +/// Defines a polygon within a dtMeshTile object. +/// @ingroup detour +struct dtPoly +{ + /// Index to first link in linked list. (Or #DT_NULL_LINK if there is no link.) + unsigned int firstLink; + + /// The indices of the polygon's vertices. + /// The actual vertices are located in dtMeshTile::verts. + unsigned short verts[DT_VERTS_PER_POLYGON]; + + /// Packed data representing neighbor polygons references and flags for each edge. + unsigned short neis[DT_VERTS_PER_POLYGON]; + + /// The user defined polygon flags. + unsigned short flags; + + /// The number of vertices in the polygon. + unsigned char vertCount; + + /// The bit packed area id and polygon type. + /// @note Use the structure's set and get methods to acess this value. + unsigned char areaAndtype; + + /// Sets the user defined area id. [Limit: < #DT_MAX_AREAS] + inline void setArea(unsigned char a) { areaAndtype = (areaAndtype & 0xc0) | (a & 0x3f); } + + /// Sets the polygon type. (See: #dtPolyTypes.) + inline void setType(unsigned char t) { areaAndtype = (areaAndtype & 0x3f) | (t << 6); } + + /// Gets the user defined area id. + inline unsigned char getArea() const { return areaAndtype & 0x3f; } + + /// Gets the polygon type. (See: #dtPolyTypes) + inline unsigned char getType() const { return areaAndtype >> 6; } +}; + +/// Defines the location of detail sub-mesh data within a dtMeshTile. +struct dtPolyDetail +{ + unsigned int vertBase; ///< The offset of the vertices in the dtMeshTile::detailVerts array. + unsigned int triBase; ///< The offset of the triangles in the dtMeshTile::detailTris array. + unsigned char vertCount; ///< The number of vertices in the sub-mesh. + unsigned char triCount; ///< The number of triangles in the sub-mesh. +}; + +/// Defines a link between polygons. +/// @note This structure is rarely if ever used by the end user. +/// @see dtMeshTile +struct dtLink +{ + dtPolyRef ref; ///< Neighbour reference. (The neighbor that is linked to.) + unsigned int next; ///< Index of the next link. + unsigned char edge; ///< Index of the polygon edge that owns this link. + unsigned char side; ///< If a boundary link, defines on which side the link is. + unsigned char bmin; ///< If a boundary link, defines the minimum sub-edge area. + unsigned char bmax; ///< If a boundary link, defines the maximum sub-edge area. +}; + +/// Bounding volume node. +/// @note This structure is rarely if ever used by the end user. +/// @see dtMeshTile +struct dtBVNode +{ + unsigned short bmin[3]; ///< Minimum bounds of the node's AABB. [(x, y, z)] + unsigned short bmax[3]; ///< Maximum bounds of the node's AABB. [(x, y, z)] + int i; ///< The node's index. (Negative for escape sequence.) +}; + +/// Defines an navigation mesh off-mesh connection within a dtMeshTile object. +/// An off-mesh connection is a user defined traversable connection made up to two vertices. +struct dtOffMeshConnection +{ + /// The endpoints of the connection. [(ax, ay, az, bx, by, bz)] + float pos[6]; + + /// The radius of the endpoints. [Limit: >= 0] + float rad; + + /// The polygon reference of the connection within the tile. + unsigned short poly; + + /// Link flags. + /// @note These are not the connection's user defined flags. Those are assigned via the + /// connection's dtPoly definition. These are link flags used for internal purposes. + unsigned char flags; + + /// End point side. + unsigned char side; + + /// The id of the offmesh connection. (User assigned when the navigation mesh is built.) + unsigned int userId; +}; + +/// Provides high level information related to a dtMeshTile object. +/// @ingroup detour +struct dtMeshHeader +{ + int magic; ///< Tile magic number. (Used to identify the data format.) + int version; ///< Tile data format version number. + int x; ///< The x-position of the tile within the dtNavMesh tile grid. (x, y, layer) + int y; ///< The y-position of the tile within the dtNavMesh tile grid. (x, y, layer) + int layer; ///< The layer of the tile within the dtNavMesh tile grid. (x, y, layer) + unsigned int userId; ///< The user defined id of the tile. + int polyCount; ///< The number of polygons in the tile. + int vertCount; ///< The number of vertices in the tile. + int maxLinkCount; ///< The number of allocated links. + int detailMeshCount; ///< The number of sub-meshes in the detail mesh. + + /// The number of unique vertices in the detail mesh. (In addition to the polygon vertices.) + int detailVertCount; + + int detailTriCount; ///< The number of triangles in the detail mesh. + int bvNodeCount; ///< The number of bounding volume nodes. (Zero if bounding volumes are disabled.) + int offMeshConCount; ///< The number of off-mesh connections. + int offMeshBase; ///< The index of the first polygon which is an off-mesh connection. + float walkableHeight; ///< The height of the agents using the tile. + float walkableRadius; ///< The radius of the agents using the tile. + float walkableClimb; ///< The maximum climb height of the agents using the tile. + float bmin[3]; ///< The minimum bounds of the tile's AABB. [(x, y, z)] + float bmax[3]; ///< The maximum bounds of the tile's AABB. [(x, y, z)] + + /// The bounding volume quantization factor. + float bvQuantFactor; +}; + +/// Defines a navigation mesh tile. +/// @ingroup detour +struct dtMeshTile +{ + unsigned int salt; ///< Counter describing modifications to the tile. + + unsigned int linksFreeList; ///< Index to the next free link. + dtMeshHeader* header; ///< The tile header. + dtPoly* polys; ///< The tile polygons. [Size: dtMeshHeader::polyCount] + float* verts; ///< The tile vertices. [Size: dtMeshHeader::vertCount] + dtLink* links; ///< The tile links. [Size: dtMeshHeader::maxLinkCount] + dtPolyDetail* detailMeshes; ///< The tile's detail sub-meshes. [Size: dtMeshHeader::detailMeshCount] + + /// The detail mesh's unique vertices. [(x, y, z) * dtMeshHeader::detailVertCount] + float* detailVerts; + + /// The detail mesh's triangles. [(vertA, vertB, vertC) * dtMeshHeader::detailTriCount] + unsigned char* detailTris; + + /// The tile bounding volume nodes. [Size: dtMeshHeader::bvNodeCount] + /// (Will be null if bounding volumes are disabled.) + dtBVNode* bvTree; + + dtOffMeshConnection* offMeshCons; ///< The tile off-mesh connections. [Size: dtMeshHeader::offMeshConCount] + + unsigned char* data; ///< The tile data. (Not directly accessed under normal situations.) + int dataSize; ///< Size of the tile data. + int flags; ///< Tile flags. (See: #dtTileFlags) + dtMeshTile* next; ///< The next free tile, or the next tile in the spatial grid. +private: + dtMeshTile(const dtMeshTile&); + dtMeshTile& operator=(const dtMeshTile&); +}; + +/// Configuration parameters used to define multi-tile navigation meshes. +/// The values are used to allocate space during the initialization of a navigation mesh. +/// @see dtNavMesh::init() +/// @ingroup detour +struct dtNavMeshParams +{ + float orig[3]; ///< The world space origin of the navigation mesh's tile space. [(x, y, z)] + float tileWidth; ///< The width of each tile. (Along the x-axis.) + float tileHeight; ///< The height of each tile. (Along the z-axis.) + int maxTiles; ///< The maximum number of tiles the navigation mesh can contain. + int maxPolys; ///< The maximum number of polygons each tile can contain. +}; + +/// A navigation mesh based on tiles of convex polygons. +/// @ingroup detour +class dtNavMesh +{ +public: + dtNavMesh(); + ~dtNavMesh(); + + /// @{ + /// @name Initialization and Tile Management + + /// Initializes the navigation mesh for tiled use. + /// @param[in] params Initialization parameters. + /// @return The status flags for the operation. + dtStatus init(const dtNavMeshParams* params); + + /// Initializes the navigation mesh for single tile use. + /// @param[in] data Data of the new tile. (See: #dtCreateNavMeshData) + /// @param[in] dataSize The data size of the new tile. + /// @param[in] flags The tile flags. (See: #dtTileFlags) + /// @return The status flags for the operation. + /// @see dtCreateNavMeshData + dtStatus init(unsigned char* data, const int dataSize, const int flags); + + /// The navigation mesh initialization params. + const dtNavMeshParams* getParams() const; + + /// Adds a tile to the navigation mesh. + /// @param[in] data Data for the new tile mesh. (See: #dtCreateNavMeshData) + /// @param[in] dataSize Data size of the new tile mesh. + /// @param[in] flags Tile flags. (See: #dtTileFlags) + /// @param[in] lastRef The desired reference for the tile. (When reloading a tile.) [opt] [Default: 0] + /// @param[out] result The tile reference. (If the tile was succesfully added.) [opt] + /// @return The status flags for the operation. + dtStatus addTile(unsigned char* data, int dataSize, int flags, dtTileRef lastRef, dtTileRef* result); + + /// Removes the specified tile from the navigation mesh. + /// @param[in] ref The reference of the tile to remove. + /// @param[out] data Data associated with deleted tile. + /// @param[out] dataSize Size of the data associated with deleted tile. + /// @return The status flags for the operation. + dtStatus removeTile(dtTileRef ref, unsigned char** data, int* dataSize); + + /// @} + + /// @{ + /// @name Query Functions + + /// Calculates the tile grid location for the specified world position. + /// @param[in] pos The world position for the query. [(x, y, z)] + /// @param[out] tx The tile's x-location. (x, y) + /// @param[out] ty The tile's y-location. (x, y) + void calcTileLoc(const float* pos, int* tx, int* ty) const; + + /// Gets the tile at the specified grid location. + /// @param[in] x The tile's x-location. (x, y, layer) + /// @param[in] y The tile's y-location. (x, y, layer) + /// @param[in] layer The tile's layer. (x, y, layer) + /// @return The tile, or null if the tile does not exist. + const dtMeshTile* getTileAt(const int x, const int y, const int layer) const; + + /// Gets all tiles at the specified grid location. (All layers.) + /// @param[in] x The tile's x-location. (x, y) + /// @param[in] y The tile's y-location. (x, y) + /// @param[out] tiles A pointer to an array of tiles that will hold the result. + /// @param[in] maxTiles The maximum tiles the tiles parameter can hold. + /// @return The number of tiles returned in the tiles array. + int getTilesAt(const int x, const int y, + dtMeshTile const** tiles, const int maxTiles) const; + + /// Gets the tile reference for the tile at specified grid location. + /// @param[in] x The tile's x-location. (x, y, layer) + /// @param[in] y The tile's y-location. (x, y, layer) + /// @param[in] layer The tile's layer. (x, y, layer) + /// @return The tile reference of the tile, or 0 if there is none. + dtTileRef getTileRefAt(int x, int y, int layer) const; + + /// Gets the tile reference for the specified tile. + /// @param[in] tile The tile. + /// @return The tile reference of the tile. + dtTileRef getTileRef(const dtMeshTile* tile) const; + + /// Gets the tile for the specified tile reference. + /// @param[in] ref The tile reference of the tile to retrieve. + /// @return The tile for the specified reference, or null if the + /// reference is invalid. + const dtMeshTile* getTileByRef(dtTileRef ref) const; + + /// The maximum number of tiles supported by the navigation mesh. + /// @return The maximum number of tiles supported by the navigation mesh. + int getMaxTiles() const; + + /// Gets the tile at the specified index. + /// @param[in] i The tile index. [Limit: 0 >= index < #getMaxTiles()] + /// @return The tile at the specified index. + const dtMeshTile* getTile(int i) const; + + /// Gets the tile and polygon for the specified polygon reference. + /// @param[in] ref The reference for the a polygon. + /// @param[out] tile The tile containing the polygon. + /// @param[out] poly The polygon. + /// @return The status flags for the operation. + dtStatus getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; + + /// Returns the tile and polygon for the specified polygon reference. + /// @param[in] ref A known valid reference for a polygon. + /// @param[out] tile The tile containing the polygon. + /// @param[out] poly The polygon. + void getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; + + /// Checks the validity of a polygon reference. + /// @param[in] ref The polygon reference to check. + /// @return True if polygon reference is valid for the navigation mesh. + bool isValidPolyRef(dtPolyRef ref) const; + + /// Gets the polygon reference for the tile's base polygon. + /// @param[in] tile The tile. + /// @return The polygon reference for the base polygon in the specified tile. + dtPolyRef getPolyRefBase(const dtMeshTile* tile) const; + + /// Gets the endpoints for an off-mesh connection, ordered by "direction of travel". + /// @param[in] prevRef The reference of the polygon before the connection. + /// @param[in] polyRef The reference of the off-mesh connection polygon. + /// @param[out] startPos The start position of the off-mesh connection. [(x, y, z)] + /// @param[out] endPos The end position of the off-mesh connection. [(x, y, z)] + /// @return The status flags for the operation. + dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const; + + /// Gets the specified off-mesh connection. + /// @param[in] ref The polygon reference of the off-mesh connection. + /// @return The specified off-mesh connection, or null if the polygon reference is not valid. + const dtOffMeshConnection* getOffMeshConnectionByRef(dtPolyRef ref) const; + + /// @} + + /// @{ + /// @name State Management + /// These functions do not effect #dtTileRef or #dtPolyRef's. + + /// Sets the user defined flags for the specified polygon. + /// @param[in] ref The polygon reference. + /// @param[in] flags The new flags for the polygon. + /// @return The status flags for the operation. + dtStatus setPolyFlags(dtPolyRef ref, unsigned short flags); + + /// Gets the user defined flags for the specified polygon. + /// @param[in] ref The polygon reference. + /// @param[out] resultFlags The polygon flags. + /// @return The status flags for the operation. + dtStatus getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const; + + /// Sets the user defined area for the specified polygon. + /// @param[in] ref The polygon reference. + /// @param[in] area The new area id for the polygon. [Limit: < #DT_MAX_AREAS] + /// @return The status flags for the operation. + dtStatus setPolyArea(dtPolyRef ref, unsigned char area); + + /// Gets the user defined area for the specified polygon. + /// @param[in] ref The polygon reference. + /// @param[out] resultArea The area id for the polygon. + /// @return The status flags for the operation. + dtStatus getPolyArea(dtPolyRef ref, unsigned char* resultArea) const; + + /// Gets the size of the buffer required by #storeTileState to store the specified tile's state. + /// @param[in] tile The tile. + /// @return The size of the buffer required to store the state. + int getTileStateSize(const dtMeshTile* tile) const; + + /// Stores the non-structural state of the tile in the specified buffer. (Flags, area ids, etc.) + /// @param[in] tile The tile. + /// @param[out] data The buffer to store the tile's state in. + /// @param[in] maxDataSize The size of the data buffer. [Limit: >= #getTileStateSize] + /// @return The status flags for the operation. + dtStatus storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const; + + /// Restores the state of the tile. + /// @param[in] tile The tile. + /// @param[in] data The new state. (Obtained from #storeTileState.) + /// @param[in] maxDataSize The size of the state within the data buffer. + /// @return The status flags for the operation. + dtStatus restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize); + + /// @} + + /// @{ + /// @name Encoding and Decoding + /// These functions are generally meant for internal use only. + + /// Derives a standard polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] salt The tile's salt value. + /// @param[in] it The index of the tile. + /// @param[in] ip The index of the polygon within the tile. + inline dtPolyRef encodePolyId(unsigned int salt, unsigned int it, unsigned int ip) const + { +#ifdef DT_POLYREF64 + return ((dtPolyRef)salt << (DT_POLY_BITS+DT_TILE_BITS)) | ((dtPolyRef)it << DT_POLY_BITS) | (dtPolyRef)ip; +#else + return ((dtPolyRef)salt << (m_polyBits+m_tileBits)) | ((dtPolyRef)it << m_polyBits) | (dtPolyRef)ip; +#endif + } + + /// Decodes a standard polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] ref The polygon reference to decode. + /// @param[out] salt The tile's salt value. + /// @param[out] it The index of the tile. + /// @param[out] ip The index of the polygon within the tile. + /// @see #encodePolyId + inline void decodePolyId(dtPolyRef ref, unsigned int& salt, unsigned int& it, unsigned int& ip) const + { +#ifdef DT_POLYREF64 + const dtPolyRef saltMask = ((dtPolyRef)1<> (DT_POLY_BITS+DT_TILE_BITS)) & saltMask); + it = (unsigned int)((ref >> DT_POLY_BITS) & tileMask); + ip = (unsigned int)(ref & polyMask); +#else + const dtPolyRef saltMask = ((dtPolyRef)1<> (m_polyBits+m_tileBits)) & saltMask); + it = (unsigned int)((ref >> m_polyBits) & tileMask); + ip = (unsigned int)(ref & polyMask); +#endif + } + + /// Extracts a tile's salt value from the specified polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] ref The polygon reference. + /// @see #encodePolyId + inline unsigned int decodePolyIdSalt(dtPolyRef ref) const + { +#ifdef DT_POLYREF64 + const dtPolyRef saltMask = ((dtPolyRef)1<> (DT_POLY_BITS+DT_TILE_BITS)) & saltMask); +#else + const dtPolyRef saltMask = ((dtPolyRef)1<> (m_polyBits+m_tileBits)) & saltMask); +#endif + } + + /// Extracts the tile's index from the specified polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] ref The polygon reference. + /// @see #encodePolyId + inline unsigned int decodePolyIdTile(dtPolyRef ref) const + { +#ifdef DT_POLYREF64 + const dtPolyRef tileMask = ((dtPolyRef)1<> DT_POLY_BITS) & tileMask); +#else + const dtPolyRef tileMask = ((dtPolyRef)1<> m_polyBits) & tileMask); +#endif + } + + /// Extracts the polygon's index (within its tile) from the specified polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] ref The polygon reference. + /// @see #encodePolyId + inline unsigned int decodePolyIdPoly(dtPolyRef ref) const + { +#ifdef DT_POLYREF64 + const dtPolyRef polyMask = ((dtPolyRef)1<header->bvQuantFactor; +const dtBVNode* n = &tile->bvTree[i]; +if (n->i >= 0) +{ + // This is a leaf node. + float worldMinX = tile->header->bmin[0] + n->bmin[0]*cs; + float worldMinY = tile->header->bmin[0] + n->bmin[1]*cs; + // Etc... +} +@endcode + +@struct dtMeshTile +@par + +Tiles generally only exist within the context of a dtNavMesh object. + +Some tile content is optional. For example, a tile may not contain any +off-mesh connections. In this case the associated pointer will be null. + +If a detail mesh exists it will share vertices with the base polygon mesh. +Only the vertices unique to the detail mesh will be stored in #detailVerts. + +@warning Tiles returned by a dtNavMesh object are not guarenteed to be populated. +For example: The tile at a location might not have been loaded yet, or may have been removed. +In this case, pointers will be null. So if in doubt, check the polygon count in the +tile's header to determine if a tile has polygons defined. + +@var float dtOffMeshConnection::pos[6] +@par + +For a properly built navigation mesh, vertex A will always be within the bounds of the mesh. +Vertex B is not required to be within the bounds of the mesh. + +*/ diff --git a/extern/recastnavigation/Detour/Include/DetourNavMeshBuilder.h b/extern/recastnavigation/Detour/Include/DetourNavMeshBuilder.h new file mode 100644 index 000000000..9425a7a78 --- /dev/null +++ b/extern/recastnavigation/Detour/Include/DetourNavMeshBuilder.h @@ -0,0 +1,149 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURNAVMESHBUILDER_H +#define DETOURNAVMESHBUILDER_H + +#include "DetourAlloc.h" + +/// Represents the source data used to build an navigation mesh tile. +/// @ingroup detour +struct dtNavMeshCreateParams +{ + + /// @name Polygon Mesh Attributes + /// Used to create the base navigation graph. + /// See #rcPolyMesh for details related to these attributes. + /// @{ + + const unsigned short* verts; ///< The polygon mesh vertices. [(x, y, z) * #vertCount] [Unit: vx] + int vertCount; ///< The number vertices in the polygon mesh. [Limit: >= 3] + const unsigned short* polys; ///< The polygon data. [Size: #polyCount * 2 * #nvp] + const unsigned short* polyFlags; ///< The user defined flags assigned to each polygon. [Size: #polyCount] + const unsigned char* polyAreas; ///< The user defined area ids assigned to each polygon. [Size: #polyCount] + int polyCount; ///< Number of polygons in the mesh. [Limit: >= 1] + int nvp; ///< Number maximum number of vertices per polygon. [Limit: >= 3] + + /// @} + /// @name Height Detail Attributes (Optional) + /// See #rcPolyMeshDetail for details related to these attributes. + /// @{ + + const unsigned int* detailMeshes; ///< The height detail sub-mesh data. [Size: 4 * #polyCount] + const float* detailVerts; ///< The detail mesh vertices. [Size: 3 * #detailVertsCount] [Unit: wu] + int detailVertsCount; ///< The number of vertices in the detail mesh. + const unsigned char* detailTris; ///< The detail mesh triangles. [Size: 4 * #detailTriCount] + int detailTriCount; ///< The number of triangles in the detail mesh. + + /// @} + /// @name Off-Mesh Connections Attributes (Optional) + /// Used to define a custom point-to-point edge within the navigation graph, an + /// off-mesh connection is a user defined traversable connection made up to two vertices, + /// at least one of which resides within a navigation mesh polygon. + /// @{ + + /// Off-mesh connection vertices. [(ax, ay, az, bx, by, bz) * #offMeshConCount] [Unit: wu] + const float* offMeshConVerts; + /// Off-mesh connection radii. [Size: #offMeshConCount] [Unit: wu] + const float* offMeshConRad; + /// User defined flags assigned to the off-mesh connections. [Size: #offMeshConCount] + const unsigned short* offMeshConFlags; + /// User defined area ids assigned to the off-mesh connections. [Size: #offMeshConCount] + const unsigned char* offMeshConAreas; + /// The permitted travel direction of the off-mesh connections. [Size: #offMeshConCount] + /// + /// 0 = Travel only from endpoint A to endpoint B.
+ /// #DT_OFFMESH_CON_BIDIR = Bidirectional travel. + const unsigned char* offMeshConDir; + /// The user defined ids of the off-mesh connection. [Size: #offMeshConCount] + const unsigned int* offMeshConUserID; + /// The number of off-mesh connections. [Limit: >= 0] + int offMeshConCount; + + /// @} + /// @name Tile Attributes + /// @note The tile grid/layer data can be left at zero if the destination is a single tile mesh. + /// @{ + + unsigned int userId; ///< The user defined id of the tile. + int tileX; ///< The tile's x-grid location within the multi-tile destination mesh. (Along the x-axis.) + int tileY; ///< The tile's y-grid location within the multi-tile desitation mesh. (Along the z-axis.) + int tileLayer; ///< The tile's layer within the layered destination mesh. [Limit: >= 0] (Along the y-axis.) + float bmin[3]; ///< The minimum bounds of the tile. [(x, y, z)] [Unit: wu] + float bmax[3]; ///< The maximum bounds of the tile. [(x, y, z)] [Unit: wu] + + /// @} + /// @name General Configuration Attributes + /// @{ + + float walkableHeight; ///< The agent height. [Unit: wu] + float walkableRadius; ///< The agent radius. [Unit: wu] + float walkableClimb; ///< The agent maximum traversable ledge. (Up/Down) [Unit: wu] + float cs; ///< The xz-plane cell size of the polygon mesh. [Limit: > 0] [Unit: wu] + float ch; ///< The y-axis cell height of the polygon mesh. [Limit: > 0] [Unit: wu] + + /// True if a bounding volume tree should be built for the tile. + /// @note The BVTree is not normally needed for layered navigation meshes. + bool buildBvTree; + + /// @} +}; + +/// Builds navigation mesh tile data from the provided tile creation data. +/// @ingroup detour +/// @param[in] params Tile creation data. +/// @param[out] outData The resulting tile data. +/// @param[out] outDataSize The size of the tile data array. +/// @return True if the tile data was successfully created. +bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize); + +/// Swaps the endianess of the tile data's header (#dtMeshHeader). +/// @param[in,out] data The tile data array. +/// @param[in] dataSize The size of the data array. +bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int dataSize); + +/// Swaps endianess of the tile data. +/// @param[in,out] data The tile data array. +/// @param[in] dataSize The size of the data array. +bool dtNavMeshDataSwapEndian(unsigned char* data, const int dataSize); + +#endif // DETOURNAVMESHBUILDER_H + +// This section contains detailed documentation for members that don't have +// a source file. It reduces clutter in the main section of the header. + +/** + +@struct dtNavMeshCreateParams +@par + +This structure is used to marshal data between the Recast mesh generation pipeline and Detour navigation components. + +See the rcPolyMesh and rcPolyMeshDetail documentation for detailed information related to mesh structure. + +Units are usually in voxels (vx) or world units (wu). The units for voxels, grid size, and cell size +are all based on the values of #cs and #ch. + +The standard navigation mesh build process is to create tile data using dtCreateNavMeshData, then add the tile +to a navigation mesh using either the dtNavMesh single tile init() function or the dtNavMesh::addTile() +function. + +@see dtCreateNavMeshData + +*/ + diff --git a/extern/recastnavigation/Detour/Include/DetourNavMeshQuery.h b/extern/recastnavigation/Detour/Include/DetourNavMeshQuery.h new file mode 100644 index 000000000..1c23e4857 --- /dev/null +++ b/extern/recastnavigation/Detour/Include/DetourNavMeshQuery.h @@ -0,0 +1,575 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURNAVMESHQUERY_H +#define DETOURNAVMESHQUERY_H + +#include "DetourNavMesh.h" +#include "DetourStatus.h" + + +// Define DT_VIRTUAL_QUERYFILTER if you wish to derive a custom filter from dtQueryFilter. +// On certain platforms indirect or virtual function call is expensive. The default +// setting is to use non-virtual functions, the actual implementations of the functions +// are declared as inline for maximum speed. + +//#define DT_VIRTUAL_QUERYFILTER 1 + +/// Defines polygon filtering and traversal costs for navigation mesh query operations. +/// @ingroup detour +class dtQueryFilter +{ + float m_areaCost[DT_MAX_AREAS]; ///< Cost per area type. (Used by default implementation.) + unsigned short m_includeFlags; ///< Flags for polygons that can be visited. (Used by default implementation.) + unsigned short m_excludeFlags; ///< Flags for polygons that should not be visted. (Used by default implementation.) + +public: + dtQueryFilter(); + +#ifdef DT_VIRTUAL_QUERYFILTER + virtual ~dtQueryFilter() { } +#endif + + /// Returns true if the polygon can be visited. (I.e. Is traversable.) + /// @param[in] ref The reference id of the polygon test. + /// @param[in] tile The tile containing the polygon. + /// @param[in] poly The polygon to test. +#ifdef DT_VIRTUAL_QUERYFILTER + virtual bool passFilter(const dtPolyRef ref, + const dtMeshTile* tile, + const dtPoly* poly) const; +#else + bool passFilter(const dtPolyRef ref, + const dtMeshTile* tile, + const dtPoly* poly) const; +#endif + + /// Returns cost to move from the beginning to the end of a line segment + /// that is fully contained within a polygon. + /// @param[in] pa The start position on the edge of the previous and current polygon. [(x, y, z)] + /// @param[in] pb The end position on the edge of the current and next polygon. [(x, y, z)] + /// @param[in] prevRef The reference id of the previous polygon. [opt] + /// @param[in] prevTile The tile containing the previous polygon. [opt] + /// @param[in] prevPoly The previous polygon. [opt] + /// @param[in] curRef The reference id of the current polygon. + /// @param[in] curTile The tile containing the current polygon. + /// @param[in] curPoly The current polygon. + /// @param[in] nextRef The refernece id of the next polygon. [opt] + /// @param[in] nextTile The tile containing the next polygon. [opt] + /// @param[in] nextPoly The next polygon. [opt] +#ifdef DT_VIRTUAL_QUERYFILTER + virtual float getCost(const float* pa, const float* pb, + const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly, + const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly, + const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const; +#else + float getCost(const float* pa, const float* pb, + const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly, + const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly, + const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const; +#endif + + /// @name Getters and setters for the default implementation data. + ///@{ + + /// Returns the traversal cost of the area. + /// @param[in] i The id of the area. + /// @returns The traversal cost of the area. + inline float getAreaCost(const int i) const { return m_areaCost[i]; } + + /// Sets the traversal cost of the area. + /// @param[in] i The id of the area. + /// @param[in] cost The new cost of traversing the area. + inline void setAreaCost(const int i, const float cost) { m_areaCost[i] = cost; } + + /// Returns the include flags for the filter. + /// Any polygons that include one or more of these flags will be + /// included in the operation. + inline unsigned short getIncludeFlags() const { return m_includeFlags; } + + /// Sets the include flags for the filter. + /// @param[in] flags The new flags. + inline void setIncludeFlags(const unsigned short flags) { m_includeFlags = flags; } + + /// Returns the exclude flags for the filter. + /// Any polygons that include one ore more of these flags will be + /// excluded from the operation. + inline unsigned short getExcludeFlags() const { return m_excludeFlags; } + + /// Sets the exclude flags for the filter. + /// @param[in] flags The new flags. + inline void setExcludeFlags(const unsigned short flags) { m_excludeFlags = flags; } + + ///@} + +}; + + + +/// Provides information about raycast hit +/// filled by dtNavMeshQuery::raycast +/// @ingroup detour +struct dtRaycastHit +{ + /// The hit parameter. (FLT_MAX if no wall hit.) + float t; + + /// hitNormal The normal of the nearest wall hit. [(x, y, z)] + float hitNormal[3]; + + /// The index of the edge on the final polygon where the wall was hit. + int hitEdgeIndex; + + /// Pointer to an array of reference ids of the visited polygons. [opt] + dtPolyRef* path; + + /// The number of visited polygons. [opt] + int pathCount; + + /// The maximum number of polygons the @p path array can hold. + int maxPath; + + /// The cost of the path until hit. + float pathCost; +}; + +/// Provides custom polygon query behavior. +/// Used by dtNavMeshQuery::queryPolygons. +/// @ingroup detour +class dtPolyQuery +{ +public: + virtual ~dtPolyQuery() { } + + /// Called for each batch of unique polygons touched by the search area in dtNavMeshQuery::queryPolygons. + /// This can be called multiple times for a single query. + virtual void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) = 0; +}; + +/// Provides the ability to perform pathfinding related queries against +/// a navigation mesh. +/// @ingroup detour +class dtNavMeshQuery +{ +public: + dtNavMeshQuery(); + ~dtNavMeshQuery(); + + /// Initializes the query object. + /// @param[in] nav Pointer to the dtNavMesh object to use for all queries. + /// @param[in] maxNodes Maximum number of search nodes. [Limits: 0 < value <= 65535] + /// @returns The status flags for the query. + dtStatus init(const dtNavMesh* nav, const int maxNodes); + + /// @name Standard Pathfinding Functions + // /@{ + + /// Finds a path from the start polygon to the end polygon. + /// @param[in] startRef The refrence id of the start polygon. + /// @param[in] endRef The reference id of the end polygon. + /// @param[in] startPos A position within the start polygon. [(x, y, z)] + /// @param[in] endPos A position within the end polygon. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 1] + dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter, + dtPolyRef* path, int* pathCount, const int maxPath) const; + + /// Finds the straight path from the start to the end position within the polygon corridor. + /// @param[in] startPos Path start position. [(x, y, z)] + /// @param[in] endPos Path end position. [(x, y, z)] + /// @param[in] path An array of polygon references that represent the path corridor. + /// @param[in] pathSize The number of polygons in the @p path array. + /// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount]. + /// @param[out] straightPathFlags Flags describing each point. (See: #dtStraightPathFlags) [opt] + /// @param[out] straightPathRefs The reference id of the polygon that is being entered at each point. [opt] + /// @param[out] straightPathCount The number of points in the straight path. + /// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0] + /// @param[in] options Query options. (see: #dtStraightPathOptions) + /// @returns The status flags for the query. + dtStatus findStraightPath(const float* startPos, const float* endPos, + const dtPolyRef* path, const int pathSize, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options = 0) const; + + ///@} + /// @name Sliced Pathfinding Functions + /// Common use case: + /// -# Call initSlicedFindPath() to initialize the sliced path query. + /// -# Call updateSlicedFindPath() until it returns complete. + /// -# Call finalizeSlicedFindPath() to get the path. + ///@{ + + /// Intializes a sliced path query. + /// @param[in] startRef The refrence id of the start polygon. + /// @param[in] endRef The reference id of the end polygon. + /// @param[in] startPos A position within the start polygon. [(x, y, z)] + /// @param[in] endPos A position within the end polygon. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] options query options (see: #dtFindPathOptions) + /// @returns The status flags for the query. + dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter, const unsigned int options = 0); + + /// Updates an in-progress sliced path query. + /// @param[in] maxIter The maximum number of iterations to perform. + /// @param[out] doneIters The actual number of iterations completed. [opt] + /// @returns The status flags for the query. + dtStatus updateSlicedFindPath(const int maxIter, int* doneIters); + + /// Finalizes and returns the results of a sliced path query. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The max number of polygons the path array can hold. [Limit: >= 1] + /// @returns The status flags for the query. + dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath); + + /// Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest + /// polygon on the existing path that was visited during the search. + /// @param[in] existing An array of polygon references for the existing path. + /// @param[in] existingSize The number of polygon in the @p existing array. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The max number of polygons the @p path array can hold. [Limit: >= 1] + /// @returns The status flags for the query. + dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, + dtPolyRef* path, int* pathCount, const int maxPath); + + ///@} + /// @name Dijkstra Search Functions + /// @{ + + /// Finds the polygons along the navigation graph that touch the specified circle. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] radius The radius of the search circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultRef The reference ids of the polygons touched by the circle. [opt] + /// @param[out] resultParent The reference ids of the parent polygons for each result. + /// Zero if a result polygon has no parent. [opt] + /// @param[out] resultCost The search cost from @p centerPos to the polygon. [opt] + /// @param[out] resultCount The number of polygons found. [opt] + /// @param[in] maxResult The maximum number of polygons the result arrays can hold. + /// @returns The status flags for the query. + dtStatus findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + int* resultCount, const int maxResult) const; + + /// Finds the polygons along the naviation graph that touch the specified convex polygon. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] verts The vertices describing the convex polygon. (CCW) + /// [(x, y, z) * @p nverts] + /// @param[in] nverts The number of vertices in the polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultRef The reference ids of the polygons touched by the search polygon. [opt] + /// @param[out] resultParent The reference ids of the parent polygons for each result. Zero if a + /// result polygon has no parent. [opt] + /// @param[out] resultCost The search cost from the centroid point to the polygon. [opt] + /// @param[out] resultCount The number of polygons found. + /// @param[in] maxResult The maximum number of polygons the result arrays can hold. + /// @returns The status flags for the query. + dtStatus findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + int* resultCount, const int maxResult) const; + + /// Gets a path from the explored nodes in the previous search. + /// @param[in] endRef The reference id of the end polygon. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 0] + /// @returns The status flags. Returns DT_FAILURE | DT_INVALID_PARAM if any parameter is wrong, or if + /// @p endRef was not explored in the previous search. Returns DT_SUCCESS | DT_BUFFER_TOO_SMALL + /// if @p path cannot contain the entire path. In this case it is filled to capacity with a partial path. + /// Otherwise returns DT_SUCCESS. + /// @remarks The result of this function depends on the state of the query object. For that reason it should only + /// be used immediately after one of the two Dijkstra searches, findPolysAroundCircle or findPolysAroundShape. + dtStatus getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const; + + /// @} + /// @name Local Query Functions + ///@{ + + /// Finds the polygon nearest to the specified center point. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] nearestRef The reference id of the nearest polygon. + /// @param[out] nearestPt The nearest point on the polygon. [opt] [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findNearestPoly(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt) const; + + /// Finds polygons that overlap the search box. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] polys The reference ids of the polygons that overlap the query box. + /// @param[out] polyCount The number of polygons in the search result. + /// @param[in] maxPolys The maximum number of polygons the search result can hold. + /// @returns The status flags for the query. + dtStatus queryPolygons(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* polys, int* polyCount, const int maxPolys) const; + + /// Finds polygons that overlap the search box. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] query The query. Polygons found will be batched together and passed to this query. + dtStatus queryPolygons(const float* center, const float* halfExtents, + const dtQueryFilter* filter, dtPolyQuery* query) const; + + /// Finds the non-overlapping navigation polygons in the local neighbourhood around the center position. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the query circle. [(x, y, z)] + /// @param[in] radius The radius of the query circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultRef The reference ids of the polygons touched by the circle. + /// @param[out] resultParent The reference ids of the parent polygons for each result. + /// Zero if a result polygon has no parent. [opt] + /// @param[out] resultCount The number of polygons found. + /// @param[in] maxResult The maximum number of polygons the result arrays can hold. + /// @returns The status flags for the query. + dtStatus findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, + int* resultCount, const int maxResult) const; + + /// Moves from the start to the end position constrained to the navigation mesh. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position of the mover within the start polygon. [(x, y, x)] + /// @param[in] endPos The desired end position of the mover. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultPos The result position of the mover. [(x, y, z)] + /// @param[out] visited The reference ids of the polygons visited during the move. + /// @param[out] visitedCount The number of polygons visited during the move. + /// @param[in] maxVisitedSize The maximum number of polygons the @p visited array can hold. + /// @returns The status flags for the query. + dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, + float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const; + + /// Casts a 'walkability' ray along the surface of the navigation mesh from + /// the start position toward the end position. + /// @note A wrapper around raycast(..., RaycastHit*). Retained for backward compatibility. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position within the start polygon representing + /// the start of the ray. [(x, y, z)] + /// @param[in] endPos The position to cast the ray toward. [(x, y, z)] + /// @param[out] t The hit parameter. (FLT_MAX if no wall hit.) + /// @param[out] hitNormal The normal of the nearest wall hit. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] path The reference ids of the visited polygons. [opt] + /// @param[out] pathCount The number of visited polygons. [opt] + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. + /// @returns The status flags for the query. + dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, + float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const; + + /// Casts a 'walkability' ray along the surface of the navigation mesh from + /// the start position toward the end position. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position within the start polygon representing + /// the start of the ray. [(x, y, z)] + /// @param[in] endPos The position to cast the ray toward. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] flags govern how the raycast behaves. See dtRaycastOptions + /// @param[out] hit Pointer to a raycast hit structure which will be filled by the results. + /// @param[in] prevRef parent of start ref. Used during for cost calculation [opt] + /// @returns The status flags for the query. + dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, const unsigned int options, + dtRaycastHit* hit, dtPolyRef prevRef = 0) const; + + + /// Finds the distance from the specified position to the nearest polygon wall. + /// @param[in] startRef The reference id of the polygon containing @p centerPos. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] maxRadius The radius of the search circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] hitDist The distance to the nearest wall from @p centerPos. + /// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)] + /// @param[out] hitNormal The normalized ray formed from the wall point to the + /// source point. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, + float* hitDist, float* hitPos, float* hitNormal) const; + + /// Returns the segments for the specified polygon, optionally including portals. + /// @param[in] ref The reference id of the polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] segmentVerts The segments. [(ax, ay, az, bx, by, bz) * segmentCount] + /// @param[out] segmentRefs The reference ids of each segment's neighbor polygon. + /// Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount] + /// @param[out] segmentCount The number of segments returned. + /// @param[in] maxSegments The maximum number of segments the result arrays can hold. + /// @returns The status flags for the query. + dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, + float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, + const int maxSegments) const; + + /// Returns random location on navmesh. + /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] frand Function returning a random number [0..1). + /// @param[out] randomRef The reference id of the random location. + /// @param[out] randomPt The random location. + /// @returns The status flags for the query. + dtStatus findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; + + /// Returns random location on navmesh within the reach of specified location. + /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. + /// The location is not exactly constrained by the circle, but it limits the visited polygons. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] frand Function returning a random number [0..1). + /// @param[out] randomRef The reference id of the random location. + /// @param[out] randomPt The random location. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; + + /// Finds the closest point on the specified polygon. + /// @param[in] ref The reference id of the polygon. + /// @param[in] pos The position to check. [(x, y, z)] + /// @param[out] closest The closest point on the polygon. [(x, y, z)] + /// @param[out] posOverPoly True of the position is over the polygon. + /// @returns The status flags for the query. + dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; + + /// Returns a point on the boundary closest to the source point if the source point is outside the + /// polygon's xz-bounds. + /// @param[in] ref The reference id to the polygon. + /// @param[in] pos The position to check. [(x, y, z)] + /// @param[out] closest The closest point. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const; + + /// Gets the height of the polygon at the provided position using the height detail. (Most accurate.) + /// @param[in] ref The reference id of the polygon. + /// @param[in] pos A position within the xz-bounds of the polygon. [(x, y, z)] + /// @param[out] height The height at the surface of the polygon. + /// @returns The status flags for the query. + dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const; + + /// @} + /// @name Miscellaneous Functions + /// @{ + + /// Returns true if the polygon reference is valid and passes the filter restrictions. + /// @param[in] ref The polygon reference to check. + /// @param[in] filter The filter to apply. + bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const; + + /// Returns true if the polygon reference is in the closed list. + /// @param[in] ref The reference id of the polygon to check. + /// @returns True if the polygon is in closed list. + bool isInClosedList(dtPolyRef ref) const; + + /// Gets the node pool. + /// @returns The node pool. + class dtNodePool* getNodePool() const { return m_nodePool; } + + /// Gets the navigation mesh the query object is using. + /// @return The navigation mesh the query object is using. + const dtNavMesh* getAttachedNavMesh() const { return m_nav; } + + /// @} + +private: + // Explicitly disabled copy constructor and copy assignment operator + dtNavMeshQuery(const dtNavMeshQuery&); + dtNavMeshQuery& operator=(const dtNavMeshQuery&); + + /// Queries polygons within a tile. + void queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, + const dtQueryFilter* filter, dtPolyQuery* query) const; + + /// Returns portal points between two polygons. + dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, + unsigned char& fromType, unsigned char& toType) const; + dtStatus getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, + dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, + float* left, float* right) const; + + /// Returns edge mid point between two polygons. + dtStatus getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const; + dtStatus getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, + dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, + float* mid) const; + + // Appends vertex to a straight path + dtStatus appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath) const; + + // Appends intermediate portal points to a straight path. + dtStatus appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options) const; + + // Gets the path leading to the specified end node. + dtStatus getPathToNode(struct dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const; + + const dtNavMesh* m_nav; ///< Pointer to navmesh data. + + struct dtQueryData + { + dtStatus status; + struct dtNode* lastBestNode; + float lastBestNodeCost; + dtPolyRef startRef, endRef; + float startPos[3], endPos[3]; + const dtQueryFilter* filter; + unsigned int options; + float raycastLimitSqr; + }; + dtQueryData m_query; ///< Sliced query state. + + class dtNodePool* m_tinyNodePool; ///< Pointer to small node pool. + class dtNodePool* m_nodePool; ///< Pointer to node pool. + class dtNodeQueue* m_openList; ///< Pointer to open list queue. +}; + +/// Allocates a query object using the Detour allocator. +/// @return An allocated query object, or null on failure. +/// @ingroup detour +dtNavMeshQuery* dtAllocNavMeshQuery(); + +/// Frees the specified query object using the Detour allocator. +/// @param[in] query A query object allocated using #dtAllocNavMeshQuery +/// @ingroup detour +void dtFreeNavMeshQuery(dtNavMeshQuery* query); + +#endif // DETOURNAVMESHQUERY_H diff --git a/extern/recastnavigation/Detour/Include/DetourNode.h b/extern/recastnavigation/Detour/Include/DetourNode.h new file mode 100644 index 000000000..db0974708 --- /dev/null +++ b/extern/recastnavigation/Detour/Include/DetourNode.h @@ -0,0 +1,168 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURNODE_H +#define DETOURNODE_H + +#include "DetourNavMesh.h" + +enum dtNodeFlags +{ + DT_NODE_OPEN = 0x01, + DT_NODE_CLOSED = 0x02, + DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not adjacent. Found using raycast. +}; + +typedef unsigned short dtNodeIndex; +static const dtNodeIndex DT_NULL_IDX = (dtNodeIndex)~0; + +static const int DT_NODE_PARENT_BITS = 24; +static const int DT_NODE_STATE_BITS = 2; +struct dtNode +{ + float pos[3]; ///< Position of the node. + float cost; ///< Cost from previous node to current node. + float total; ///< Cost up to the node. + unsigned int pidx : DT_NODE_PARENT_BITS; ///< Index to parent node. + unsigned int state : DT_NODE_STATE_BITS; ///< extra state information. A polyRef can have multiple nodes with different extra info. see DT_MAX_STATES_PER_NODE + unsigned int flags : 3; ///< Node flags. A combination of dtNodeFlags. + dtPolyRef id; ///< Polygon ref the node corresponds to. +}; + +static const int DT_MAX_STATES_PER_NODE = 1 << DT_NODE_STATE_BITS; // number of extra states per node. See dtNode::state + +class dtNodePool +{ +public: + dtNodePool(int maxNodes, int hashSize); + ~dtNodePool(); + void clear(); + + // Get a dtNode by ref and extra state information. If there is none then - allocate + // There can be more than one node for the same polyRef but with different extra state information + dtNode* getNode(dtPolyRef id, unsigned char state=0); + dtNode* findNode(dtPolyRef id, unsigned char state); + unsigned int findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes); + + inline unsigned int getNodeIdx(const dtNode* node) const + { + if (!node) return 0; + return (unsigned int)(node - m_nodes) + 1; + } + + inline dtNode* getNodeAtIdx(unsigned int idx) + { + if (!idx) return 0; + return &m_nodes[idx - 1]; + } + + inline const dtNode* getNodeAtIdx(unsigned int idx) const + { + if (!idx) return 0; + return &m_nodes[idx - 1]; + } + + inline int getMemUsed() const + { + return sizeof(*this) + + sizeof(dtNode)*m_maxNodes + + sizeof(dtNodeIndex)*m_maxNodes + + sizeof(dtNodeIndex)*m_hashSize; + } + + inline int getMaxNodes() const { return m_maxNodes; } + + inline int getHashSize() const { return m_hashSize; } + inline dtNodeIndex getFirst(int bucket) const { return m_first[bucket]; } + inline dtNodeIndex getNext(int i) const { return m_next[i]; } + inline int getNodeCount() const { return m_nodeCount; } + +private: + // Explicitly disabled copy constructor and copy assignment operator. + dtNodePool(const dtNodePool&); + dtNodePool& operator=(const dtNodePool&); + + dtNode* m_nodes; + dtNodeIndex* m_first; + dtNodeIndex* m_next; + const int m_maxNodes; + const int m_hashSize; + int m_nodeCount; +}; + +class dtNodeQueue +{ +public: + dtNodeQueue(int n); + ~dtNodeQueue(); + + inline void clear() { m_size = 0; } + + inline dtNode* top() { return m_heap[0]; } + + inline dtNode* pop() + { + dtNode* result = m_heap[0]; + m_size--; + trickleDown(0, m_heap[m_size]); + return result; + } + + inline void push(dtNode* node) + { + m_size++; + bubbleUp(m_size-1, node); + } + + inline void modify(dtNode* node) + { + for (int i = 0; i < m_size; ++i) + { + if (m_heap[i] == node) + { + bubbleUp(i, node); + return; + } + } + } + + inline bool empty() const { return m_size == 0; } + + inline int getMemUsed() const + { + return sizeof(*this) + + sizeof(dtNode*) * (m_capacity + 1); + } + + inline int getCapacity() const { return m_capacity; } + +private: + // Explicitly disabled copy constructor and copy assignment operator. + dtNodeQueue(const dtNodeQueue&); + dtNodeQueue& operator=(const dtNodeQueue&); + + void bubbleUp(int i, dtNode* node); + void trickleDown(int i, dtNode* node); + + dtNode** m_heap; + const int m_capacity; + int m_size; +}; + + +#endif // DETOURNODE_H diff --git a/extern/recastnavigation/Detour/Include/DetourStatus.h b/extern/recastnavigation/Detour/Include/DetourStatus.h new file mode 100644 index 000000000..8e1bb44b9 --- /dev/null +++ b/extern/recastnavigation/Detour/Include/DetourStatus.h @@ -0,0 +1,65 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURSTATUS_H +#define DETOURSTATUS_H + +typedef unsigned int dtStatus; + +// High level status. +static const unsigned int DT_FAILURE = 1u << 31; // Operation failed. +static const unsigned int DT_SUCCESS = 1u << 30; // Operation succeed. +static const unsigned int DT_IN_PROGRESS = 1u << 29; // Operation still in progress. + +// Detail information for status. +static const unsigned int DT_STATUS_DETAIL_MASK = 0x0ffffff; +static const unsigned int DT_WRONG_MAGIC = 1 << 0; // Input data is not recognized. +static const unsigned int DT_WRONG_VERSION = 1 << 1; // Input data is in wrong version. +static const unsigned int DT_OUT_OF_MEMORY = 1 << 2; // Operation ran out of memory. +static const unsigned int DT_INVALID_PARAM = 1 << 3; // An input parameter was invalid. +static const unsigned int DT_BUFFER_TOO_SMALL = 1 << 4; // Result buffer for the query was too small to store all results. +static const unsigned int DT_OUT_OF_NODES = 1 << 5; // Query ran out of nodes during search. +static const unsigned int DT_PARTIAL_RESULT = 1 << 6; // Query did not reach the end location, returning best guess. +static const unsigned int DT_ALREADY_OCCUPIED = 1 << 7; // A tile has already been assigned to the given x,y coordinate + + +// Returns true of status is success. +inline bool dtStatusSucceed(dtStatus status) +{ + return (status & DT_SUCCESS) != 0; +} + +// Returns true of status is failure. +inline bool dtStatusFailed(dtStatus status) +{ + return (status & DT_FAILURE) != 0; +} + +// Returns true of status is in progress. +inline bool dtStatusInProgress(dtStatus status) +{ + return (status & DT_IN_PROGRESS) != 0; +} + +// Returns true if specific detail is set. +inline bool dtStatusDetail(dtStatus status, unsigned int detail) +{ + return (status & detail) != 0; +} + +#endif // DETOURSTATUS_H diff --git a/extern/recastnavigation/Detour/Source/DetourAlloc.cpp b/extern/recastnavigation/Detour/Source/DetourAlloc.cpp new file mode 100644 index 000000000..d9ad1fc01 --- /dev/null +++ b/extern/recastnavigation/Detour/Source/DetourAlloc.cpp @@ -0,0 +1,50 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include "DetourAlloc.h" + +static void *dtAllocDefault(size_t size, dtAllocHint) +{ + return malloc(size); +} + +static void dtFreeDefault(void *ptr) +{ + free(ptr); +} + +static dtAllocFunc* sAllocFunc = dtAllocDefault; +static dtFreeFunc* sFreeFunc = dtFreeDefault; + +void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc) +{ + sAllocFunc = allocFunc ? allocFunc : dtAllocDefault; + sFreeFunc = freeFunc ? freeFunc : dtFreeDefault; +} + +void* dtAlloc(size_t size, dtAllocHint hint) +{ + return sAllocFunc(size, hint); +} + +void dtFree(void* ptr) +{ + if (ptr) + sFreeFunc(ptr); +} diff --git a/extern/recastnavigation/Detour/Source/DetourAssert.cpp b/extern/recastnavigation/Detour/Source/DetourAssert.cpp new file mode 100644 index 000000000..5e019e0cf --- /dev/null +++ b/extern/recastnavigation/Detour/Source/DetourAssert.cpp @@ -0,0 +1,35 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DetourAssert.h" + +#ifndef NDEBUG + +static dtAssertFailFunc* sAssertFailFunc = 0; + +void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc) +{ + sAssertFailFunc = assertFailFunc; +} + +dtAssertFailFunc* dtAssertFailGetCustom() +{ + return sAssertFailFunc; +} + +#endif diff --git a/extern/recastnavigation/Detour/Source/DetourCommon.cpp b/extern/recastnavigation/Detour/Source/DetourCommon.cpp new file mode 100644 index 000000000..41d0d7bd3 --- /dev/null +++ b/extern/recastnavigation/Detour/Source/DetourCommon.cpp @@ -0,0 +1,388 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DetourCommon.h" +#include "DetourMath.h" + +////////////////////////////////////////////////////////////////////////////////////////// + +void dtClosestPtPointTriangle(float* closest, const float* p, + const float* a, const float* b, const float* c) +{ + // Check if P in vertex region outside A + float ab[3], ac[3], ap[3]; + dtVsub(ab, b, a); + dtVsub(ac, c, a); + dtVsub(ap, p, a); + float d1 = dtVdot(ab, ap); + float d2 = dtVdot(ac, ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + // barycentric coordinates (1,0,0) + dtVcopy(closest, a); + return; + } + + // Check if P in vertex region outside B + float bp[3]; + dtVsub(bp, p, b); + float d3 = dtVdot(ab, bp); + float d4 = dtVdot(ac, bp); + if (d3 >= 0.0f && d4 <= d3) + { + // barycentric coordinates (0,1,0) + dtVcopy(closest, b); + return; + } + + // Check if P in edge region of AB, if so return projection of P onto AB + float vc = d1*d4 - d3*d2; + if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) + { + // barycentric coordinates (1-v,v,0) + float v = d1 / (d1 - d3); + closest[0] = a[0] + v * ab[0]; + closest[1] = a[1] + v * ab[1]; + closest[2] = a[2] + v * ab[2]; + return; + } + + // Check if P in vertex region outside C + float cp[3]; + dtVsub(cp, p, c); + float d5 = dtVdot(ab, cp); + float d6 = dtVdot(ac, cp); + if (d6 >= 0.0f && d5 <= d6) + { + // barycentric coordinates (0,0,1) + dtVcopy(closest, c); + return; + } + + // Check if P in edge region of AC, if so return projection of P onto AC + float vb = d5*d2 - d1*d6; + if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) + { + // barycentric coordinates (1-w,0,w) + float w = d2 / (d2 - d6); + closest[0] = a[0] + w * ac[0]; + closest[1] = a[1] + w * ac[1]; + closest[2] = a[2] + w * ac[2]; + return; + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float va = d3*d6 - d5*d4; + if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) + { + // barycentric coordinates (0,1-w,w) + float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + closest[0] = b[0] + w * (c[0] - b[0]); + closest[1] = b[1] + w * (c[1] - b[1]); + closest[2] = b[2] + w * (c[2] - b[2]); + return; + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + float denom = 1.0f / (va + vb + vc); + float v = vb * denom; + float w = vc * denom; + closest[0] = a[0] + ab[0] * v + ac[0] * w; + closest[1] = a[1] + ab[1] * v + ac[1] * w; + closest[2] = a[2] + ab[2] * v + ac[2] * w; +} + +bool dtIntersectSegmentPoly2D(const float* p0, const float* p1, + const float* verts, int nverts, + float& tmin, float& tmax, + int& segMin, int& segMax) +{ + static const float EPS = 0.00000001f; + + tmin = 0; + tmax = 1; + segMin = -1; + segMax = -1; + + float dir[3]; + dtVsub(dir, p1, p0); + + for (int i = 0, j = nverts-1; i < nverts; j=i++) + { + float edge[3], diff[3]; + dtVsub(edge, &verts[i*3], &verts[j*3]); + dtVsub(diff, p0, &verts[j*3]); + const float n = dtVperp2D(edge, diff); + const float d = dtVperp2D(dir, edge); + if (fabsf(d) < EPS) + { + // S is nearly parallel to this edge + if (n < 0) + return false; + else + continue; + } + const float t = n / d; + if (d < 0) + { + // segment S is entering across this edge + if (t > tmin) + { + tmin = t; + segMin = j; + // S enters after leaving polygon + if (tmin > tmax) + return false; + } + } + else + { + // segment S is leaving across this edge + if (t < tmax) + { + tmax = t; + segMax = j; + // S leaves before entering polygon + if (tmax < tmin) + return false; + } + } + } + + return true; +} + +float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t) +{ + float pqx = q[0] - p[0]; + float pqz = q[2] - p[2]; + float dx = pt[0] - p[0]; + float dz = pt[2] - p[2]; + float d = pqx*pqx + pqz*pqz; + t = pqx*dx + pqz*dz; + if (d > 0) t /= d; + if (t < 0) t = 0; + else if (t > 1) t = 1; + dx = p[0] + t*pqx - pt[0]; + dz = p[2] + t*pqz - pt[2]; + return dx*dx + dz*dz; +} + +void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts) +{ + tc[0] = 0.0f; + tc[1] = 0.0f; + tc[2] = 0.0f; + for (int j = 0; j < nidx; ++j) + { + const float* v = &verts[idx[j]*3]; + tc[0] += v[0]; + tc[1] += v[1]; + tc[2] += v[2]; + } + const float s = 1.0f / nidx; + tc[0] *= s; + tc[1] *= s; + tc[2] *= s; +} + +bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h) +{ + float v0[3], v1[3], v2[3]; + dtVsub(v0, c,a); + dtVsub(v1, b,a); + dtVsub(v2, p,a); + + const float dot00 = dtVdot2D(v0, v0); + const float dot01 = dtVdot2D(v0, v1); + const float dot02 = dtVdot2D(v0, v2); + const float dot11 = dtVdot2D(v1, v1); + const float dot12 = dtVdot2D(v1, v2); + + // Compute barycentric coordinates + const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + const float u = (dot11 * dot02 - dot01 * dot12) * invDenom; + const float v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + // The (sloppy) epsilon is needed to allow to get height of points which + // are interpolated along the edges of the triangles. + static const float EPS = 1e-4f; + + // If point lies inside the triangle, return interpolated ycoord. + if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS) + { + h = a[1] + v0[1]*u + v1[1]*v; + return true; + } + + return false; +} + +/// @par +/// +/// All points are projected onto the xz-plane, so the y-values are ignored. +bool dtPointInPolygon(const float* pt, const float* verts, const int nverts) +{ + // TODO: Replace pnpoly with triArea2D tests? + int i, j; + bool c = false; + for (i = 0, j = nverts-1; i < nverts; j = i++) + { + const float* vi = &verts[i*3]; + const float* vj = &verts[j*3]; + if (((vi[2] > pt[2]) != (vj[2] > pt[2])) && + (pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) + c = !c; + } + return c; +} + +bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, + float* ed, float* et) +{ + // TODO: Replace pnpoly with triArea2D tests? + int i, j; + bool c = false; + for (i = 0, j = nverts-1; i < nverts; j = i++) + { + const float* vi = &verts[i*3]; + const float* vj = &verts[j*3]; + if (((vi[2] > pt[2]) != (vj[2] > pt[2])) && + (pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) + c = !c; + ed[j] = dtDistancePtSegSqr2D(pt, vj, vi, et[j]); + } + return c; +} + +static void projectPoly(const float* axis, const float* poly, const int npoly, + float& rmin, float& rmax) +{ + rmin = rmax = dtVdot2D(axis, &poly[0]); + for (int i = 1; i < npoly; ++i) + { + const float d = dtVdot2D(axis, &poly[i*3]); + rmin = dtMin(rmin, d); + rmax = dtMax(rmax, d); + } +} + +inline bool overlapRange(const float amin, const float amax, + const float bmin, const float bmax, + const float eps) +{ + return ((amin+eps) > bmax || (amax-eps) < bmin) ? false : true; +} + +/// @par +/// +/// All vertices are projected onto the xz-plane, so the y-values are ignored. +bool dtOverlapPolyPoly2D(const float* polya, const int npolya, + const float* polyb, const int npolyb) +{ + const float eps = 1e-4f; + + for (int i = 0, j = npolya-1; i < npolya; j=i++) + { + const float* va = &polya[j*3]; + const float* vb = &polya[i*3]; + const float n[3] = { vb[2]-va[2], 0, -(vb[0]-va[0]) }; + float amin,amax,bmin,bmax; + projectPoly(n, polya, npolya, amin,amax); + projectPoly(n, polyb, npolyb, bmin,bmax); + if (!overlapRange(amin,amax, bmin,bmax, eps)) + { + // Found separating axis + return false; + } + } + for (int i = 0, j = npolyb-1; i < npolyb; j=i++) + { + const float* va = &polyb[j*3]; + const float* vb = &polyb[i*3]; + const float n[3] = { vb[2]-va[2], 0, -(vb[0]-va[0]) }; + float amin,amax,bmin,bmax; + projectPoly(n, polya, npolya, amin,amax); + projectPoly(n, polyb, npolyb, bmin,bmax); + if (!overlapRange(amin,amax, bmin,bmax, eps)) + { + // Found separating axis + return false; + } + } + return true; +} + +// Returns a random point in a convex polygon. +// Adapted from Graphics Gems article. +void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, + const float s, const float t, float* out) +{ + // Calc triangle araes + float areasum = 0.0f; + for (int i = 2; i < npts; i++) { + areas[i] = dtTriArea2D(&pts[0], &pts[(i-1)*3], &pts[i*3]); + areasum += dtMax(0.001f, areas[i]); + } + // Find sub triangle weighted by area. + const float thr = s*areasum; + float acc = 0.0f; + float u = 1.0f; + int tri = npts - 1; + for (int i = 2; i < npts; i++) { + const float dacc = areas[i]; + if (thr >= acc && thr < (acc+dacc)) + { + u = (thr - acc) / dacc; + tri = i; + break; + } + acc += dacc; + } + + float v = dtMathSqrtf(t); + + const float a = 1 - v; + const float b = (1 - u) * v; + const float c = u * v; + const float* pa = &pts[0]; + const float* pb = &pts[(tri-1)*3]; + const float* pc = &pts[tri*3]; + + out[0] = a*pa[0] + b*pb[0] + c*pc[0]; + out[1] = a*pa[1] + b*pb[1] + c*pc[1]; + out[2] = a*pa[2] + b*pb[2] + c*pc[2]; +} + +inline float vperpXZ(const float* a, const float* b) { return a[0]*b[2] - a[2]*b[0]; } + +bool dtIntersectSegSeg2D(const float* ap, const float* aq, + const float* bp, const float* bq, + float& s, float& t) +{ + float u[3], v[3], w[3]; + dtVsub(u,aq,ap); + dtVsub(v,bq,bp); + dtVsub(w,ap,bp); + float d = vperpXZ(u,v); + if (fabsf(d) < 1e-6f) return false; + s = vperpXZ(v,w) / d; + t = vperpXZ(u,w) / d; + return true; +} + diff --git a/extern/recastnavigation/Detour/Source/DetourNavMesh.cpp b/extern/recastnavigation/Detour/Source/DetourNavMesh.cpp new file mode 100644 index 000000000..17df26d8c --- /dev/null +++ b/extern/recastnavigation/Detour/Source/DetourNavMesh.cpp @@ -0,0 +1,1522 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include +#include +#include "DetourNavMesh.h" +#include "DetourNode.h" +#include "DetourCommon.h" +#include "DetourMath.h" +#include "DetourAlloc.h" +#include "DetourAssert.h" +#include + + +inline bool overlapSlabs(const float* amin, const float* amax, + const float* bmin, const float* bmax, + const float px, const float py) +{ + // Check for horizontal overlap. + // The segment is shrunken a little so that slabs which touch + // at end points are not connected. + const float minx = dtMax(amin[0]+px,bmin[0]+px); + const float maxx = dtMin(amax[0]-px,bmax[0]-px); + if (minx > maxx) + return false; + + // Check vertical overlap. + const float ad = (amax[1]-amin[1]) / (amax[0]-amin[0]); + const float ak = amin[1] - ad*amin[0]; + const float bd = (bmax[1]-bmin[1]) / (bmax[0]-bmin[0]); + const float bk = bmin[1] - bd*bmin[0]; + const float aminy = ad*minx + ak; + const float amaxy = ad*maxx + ak; + const float bminy = bd*minx + bk; + const float bmaxy = bd*maxx + bk; + const float dmin = bminy - aminy; + const float dmax = bmaxy - amaxy; + + // Crossing segments always overlap. + if (dmin*dmax < 0) + return true; + + // Check for overlap at endpoints. + const float thr = dtSqr(py*2); + if (dmin*dmin <= thr || dmax*dmax <= thr) + return true; + + return false; +} + +static float getSlabCoord(const float* va, const int side) +{ + if (side == 0 || side == 4) + return va[0]; + else if (side == 2 || side == 6) + return va[2]; + return 0; +} + +static void calcSlabEndPoints(const float* va, const float* vb, float* bmin, float* bmax, const int side) +{ + if (side == 0 || side == 4) + { + if (va[2] < vb[2]) + { + bmin[0] = va[2]; + bmin[1] = va[1]; + bmax[0] = vb[2]; + bmax[1] = vb[1]; + } + else + { + bmin[0] = vb[2]; + bmin[1] = vb[1]; + bmax[0] = va[2]; + bmax[1] = va[1]; + } + } + else if (side == 2 || side == 6) + { + if (va[0] < vb[0]) + { + bmin[0] = va[0]; + bmin[1] = va[1]; + bmax[0] = vb[0]; + bmax[1] = vb[1]; + } + else + { + bmin[0] = vb[0]; + bmin[1] = vb[1]; + bmax[0] = va[0]; + bmax[1] = va[1]; + } + } +} + +inline int computeTileHash(int x, int y, const int mask) +{ + const unsigned int h1 = 0x8da6b343; // Large multiplicative constants; + const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes + unsigned int n = h1 * x + h2 * y; + return (int)(n & mask); +} + +inline unsigned int allocLink(dtMeshTile* tile) +{ + if (tile->linksFreeList == DT_NULL_LINK) + return DT_NULL_LINK; + unsigned int link = tile->linksFreeList; + tile->linksFreeList = tile->links[link].next; + return link; +} + +inline void freeLink(dtMeshTile* tile, unsigned int link) +{ + tile->links[link].next = tile->linksFreeList; + tile->linksFreeList = link; +} + + +dtNavMesh* dtAllocNavMesh() +{ + void* mem = dtAlloc(sizeof(dtNavMesh), DT_ALLOC_PERM); + if (!mem) return 0; + return new(mem) dtNavMesh; +} + +/// @par +/// +/// This function will only free the memory for tiles with the #DT_TILE_FREE_DATA +/// flag set. +void dtFreeNavMesh(dtNavMesh* navmesh) +{ + if (!navmesh) return; + navmesh->~dtNavMesh(); + dtFree(navmesh); +} + +////////////////////////////////////////////////////////////////////////////////////////// + +/** +@class dtNavMesh + +The navigation mesh consists of one or more tiles defining three primary types of structural data: + +A polygon mesh which defines most of the navigation graph. (See rcPolyMesh for its structure.) +A detail mesh used for determining surface height on the polygon mesh. (See rcPolyMeshDetail for its structure.) +Off-mesh connections, which define custom point-to-point edges within the navigation graph. + +The general build process is as follows: + +-# Create rcPolyMesh and rcPolyMeshDetail data using the Recast build pipeline. +-# Optionally, create off-mesh connection data. +-# Combine the source data into a dtNavMeshCreateParams structure. +-# Create a tile data array using dtCreateNavMeshData(). +-# Allocate at dtNavMesh object and initialize it. (For single tile navigation meshes, + the tile data is loaded during this step.) +-# For multi-tile navigation meshes, load the tile data using dtNavMesh::addTile(). + +Notes: + +- This class is usually used in conjunction with the dtNavMeshQuery class for pathfinding. +- Technically, all navigation meshes are tiled. A 'solo' mesh is simply a navigation mesh initialized + to have only a single tile. +- This class does not implement any asynchronous methods. So the ::dtStatus result of all methods will + always contain either a success or failure flag. + +@see dtNavMeshQuery, dtCreateNavMeshData, dtNavMeshCreateParams, #dtAllocNavMesh, #dtFreeNavMesh +*/ + +dtNavMesh::dtNavMesh() : + m_tileWidth(0), + m_tileHeight(0), + m_maxTiles(0), + m_tileLutSize(0), + m_tileLutMask(0), + m_posLookup(0), + m_nextFree(0), + m_tiles(0) +{ +#ifndef DT_POLYREF64 + m_saltBits = 0; + m_tileBits = 0; + m_polyBits = 0; +#endif + memset(&m_params, 0, sizeof(dtNavMeshParams)); + m_orig[0] = 0; + m_orig[1] = 0; + m_orig[2] = 0; +} + +dtNavMesh::~dtNavMesh() +{ + for (int i = 0; i < m_maxTiles; ++i) + { + if (m_tiles[i].flags & DT_TILE_FREE_DATA) + { + dtFree(m_tiles[i].data); + m_tiles[i].data = 0; + m_tiles[i].dataSize = 0; + } + } + dtFree(m_posLookup); + dtFree(m_tiles); +} + +dtStatus dtNavMesh::init(const dtNavMeshParams* params) +{ + memcpy(&m_params, params, sizeof(dtNavMeshParams)); + dtVcopy(m_orig, params->orig); + m_tileWidth = params->tileWidth; + m_tileHeight = params->tileHeight; + + // Init tiles + m_maxTiles = params->maxTiles; + m_tileLutSize = dtNextPow2(params->maxTiles/4); + if (!m_tileLutSize) m_tileLutSize = 1; + m_tileLutMask = m_tileLutSize-1; + + m_tiles = (dtMeshTile*)dtAlloc(sizeof(dtMeshTile)*m_maxTiles, DT_ALLOC_PERM); + if (!m_tiles) + return DT_FAILURE | DT_OUT_OF_MEMORY; + m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*)*m_tileLutSize, DT_ALLOC_PERM); + if (!m_posLookup) + return DT_FAILURE | DT_OUT_OF_MEMORY; + memset(m_tiles, 0, sizeof(dtMeshTile)*m_maxTiles); + memset(m_posLookup, 0, sizeof(dtMeshTile*)*m_tileLutSize); + m_nextFree = 0; + for (int i = m_maxTiles-1; i >= 0; --i) + { + m_tiles[i].salt = 1; + m_tiles[i].next = m_nextFree; + m_nextFree = &m_tiles[i]; + } + + // Init ID generator values. +#ifndef DT_POLYREF64 + m_tileBits = dtIlog2(dtNextPow2((unsigned int)params->maxTiles)); + m_polyBits = dtIlog2(dtNextPow2((unsigned int)params->maxPolys)); + // Only allow 31 salt bits, since the salt mask is calculated using 32bit uint and it will overflow. + m_saltBits = dtMin((unsigned int)31, 32 - m_tileBits - m_polyBits); + + if (m_saltBits < 10) + return DT_FAILURE | DT_INVALID_PARAM; +#endif + + return DT_SUCCESS; +} + +dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flags) +{ + // Make sure the data is in right format. + dtMeshHeader* header = (dtMeshHeader*)data; + if (header->magic != DT_NAVMESH_MAGIC) + return DT_FAILURE | DT_WRONG_MAGIC; + if (header->version != DT_NAVMESH_VERSION) + return DT_FAILURE | DT_WRONG_VERSION; + + dtNavMeshParams params; + dtVcopy(params.orig, header->bmin); + params.tileWidth = header->bmax[0] - header->bmin[0]; + params.tileHeight = header->bmax[2] - header->bmin[2]; + params.maxTiles = 1; + params.maxPolys = header->polyCount; + + dtStatus status = init(¶ms); + if (dtStatusFailed(status)) + return status; + + return addTile(data, dataSize, flags, 0, 0); +} + +/// @par +/// +/// @note The parameters are created automatically when the single tile +/// initialization is performed. +const dtNavMeshParams* dtNavMesh::getParams() const +{ + return &m_params; +} + +////////////////////////////////////////////////////////////////////////////////////////// +int dtNavMesh::findConnectingPolys(const float* va, const float* vb, + const dtMeshTile* tile, int side, + dtPolyRef* con, float* conarea, int maxcon) const +{ + if (!tile) return 0; + + float amin[2], amax[2]; + calcSlabEndPoints(va, vb, amin, amax, side); + const float apos = getSlabCoord(va, side); + + // Remove links pointing to 'side' and compact the links array. + float bmin[2], bmax[2]; + unsigned short m = DT_EXT_LINK | (unsigned short)side; + int n = 0; + + dtPolyRef base = getPolyRefBase(tile); + + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + const int nv = poly->vertCount; + for (int j = 0; j < nv; ++j) + { + // Skip edges which do not point to the right side. + if (poly->neis[j] != m) continue; + + const float* vc = &tile->verts[poly->verts[j]*3]; + const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3]; + const float bpos = getSlabCoord(vc, side); + + // Segments are not close enough. + if (dtAbs(apos-bpos) > 0.01f) + continue; + + // Check if the segments touch. + calcSlabEndPoints(vc,vd, bmin,bmax, side); + + if (!overlapSlabs(amin,amax, bmin,bmax, 0.01f, tile->header->walkableClimb)) continue; + + // Add return value. + if (n < maxcon) + { + conarea[n*2+0] = dtMax(amin[0], bmin[0]); + conarea[n*2+1] = dtMin(amax[0], bmax[0]); + con[n] = base | (dtPolyRef)i; + n++; + } + break; + } + } + return n; +} + +void dtNavMesh::unconnectLinks(dtMeshTile* tile, dtMeshTile* target) +{ + if (!tile || !target) return; + + const unsigned int targetNum = decodePolyIdTile(getTileRef(target)); + + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + unsigned int j = poly->firstLink; + unsigned int pj = DT_NULL_LINK; + while (j != DT_NULL_LINK) + { + if (decodePolyIdTile(tile->links[j].ref) == targetNum) + { + // Remove link. + unsigned int nj = tile->links[j].next; + if (pj == DT_NULL_LINK) + poly->firstLink = nj; + else + tile->links[pj].next = nj; + freeLink(tile, j); + j = nj; + } + else + { + // Advance + pj = j; + j = tile->links[j].next; + } + } + } +} + +void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) +{ + if (!tile) return; + + // Connect border links. + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + + // Create new links. +// unsigned short m = DT_EXT_LINK | (unsigned short)side; + + const int nv = poly->vertCount; + for (int j = 0; j < nv; ++j) + { + // Skip non-portal edges. + if ((poly->neis[j] & DT_EXT_LINK) == 0) + continue; + + const int dir = (int)(poly->neis[j] & 0xff); + if (side != -1 && dir != side) + continue; + + // Create new links + const float* va = &tile->verts[poly->verts[j]*3]; + const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3]; + dtPolyRef nei[4]; + float neia[4*2]; + int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(dir), nei,neia,4); + for (int k = 0; k < nnei; ++k) + { + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = nei[k]; + link->edge = (unsigned char)j; + link->side = (unsigned char)dir; + + link->next = poly->firstLink; + poly->firstLink = idx; + + // Compress portal limits to a byte value. + if (dir == 0 || dir == 4) + { + float tmin = (neia[k*2+0]-va[2]) / (vb[2]-va[2]); + float tmax = (neia[k*2+1]-va[2]) / (vb[2]-va[2]); + if (tmin > tmax) + dtSwap(tmin,tmax); + link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f); + link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f); + } + else if (dir == 2 || dir == 6) + { + float tmin = (neia[k*2+0]-va[0]) / (vb[0]-va[0]); + float tmax = (neia[k*2+1]-va[0]) / (vb[0]-va[0]); + if (tmin > tmax) + dtSwap(tmin,tmax); + link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f); + link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f); + } + } + } + } + } +} + +void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side) +{ + if (!tile) return; + + // Connect off-mesh links. + // We are interested on links which land from target tile to this tile. + const unsigned char oppositeSide = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side); + + for (int i = 0; i < target->header->offMeshConCount; ++i) + { + dtOffMeshConnection* targetCon = &target->offMeshCons[i]; + if (targetCon->side != oppositeSide) + continue; + + dtPoly* targetPoly = &target->polys[targetCon->poly]; + // Skip off-mesh connections which start location could not be connected at all. + if (targetPoly->firstLink == DT_NULL_LINK) + continue; + + const float halfExtents[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad }; + + // Find polygon to connect to. + const float* p = &targetCon->pos[3]; + float nearestPt[3]; + dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt); + if (!ref) + continue; + // findNearestPoly may return too optimistic results, further check to make sure. + if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(targetCon->rad)) + continue; + // Make sure the location is on current mesh. + float* v = &target->verts[targetPoly->verts[1]*3]; + dtVcopy(v, nearestPt); + + // Link off-mesh connection to target poly. + unsigned int idx = allocLink(target); + if (idx != DT_NULL_LINK) + { + dtLink* link = &target->links[idx]; + link->ref = ref; + link->edge = (unsigned char)1; + link->side = oppositeSide; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = targetPoly->firstLink; + targetPoly->firstLink = idx; + } + + // Link target poly to off-mesh connection. + if (targetCon->flags & DT_OFFMESH_CON_BIDIR) + { + unsigned int tidx = allocLink(tile); + if (tidx != DT_NULL_LINK) + { + const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); + dtPoly* landPoly = &tile->polys[landPolyIdx]; + dtLink* link = &tile->links[tidx]; + link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly); + link->edge = 0xff; + link->side = (unsigned char)(side == -1 ? 0xff : side); + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = landPoly->firstLink; + landPoly->firstLink = tidx; + } + } + } + +} + +void dtNavMesh::connectIntLinks(dtMeshTile* tile) +{ + if (!tile) return; + + dtPolyRef base = getPolyRefBase(tile); + + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + poly->firstLink = DT_NULL_LINK; + + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + + // Build edge links backwards so that the links will be + // in the linked list from lowest index to highest. + for (int j = poly->vertCount-1; j >= 0; --j) + { + // Skip hard and non-internal edges. + if (poly->neis[j] == 0 || (poly->neis[j] & DT_EXT_LINK)) continue; + + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = base | (dtPolyRef)(poly->neis[j]-1); + link->edge = (unsigned char)j; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = poly->firstLink; + poly->firstLink = idx; + } + } + } +} + +void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) +{ + if (!tile) return; + + dtPolyRef base = getPolyRefBase(tile); + + // Base off-mesh connection start points. + for (int i = 0; i < tile->header->offMeshConCount; ++i) + { + dtOffMeshConnection* con = &tile->offMeshCons[i]; + dtPoly* poly = &tile->polys[con->poly]; + + const float halfExtents[3] = { con->rad, tile->header->walkableClimb, con->rad }; + + // Find polygon to connect to. + const float* p = &con->pos[0]; // First vertex + float nearestPt[3]; + dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt); + if (!ref) continue; + // findNearestPoly may return too optimistic results, further check to make sure. + if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) + continue; + // Make sure the location is on current mesh. + float* v = &tile->verts[poly->verts[0]*3]; + dtVcopy(v, nearestPt); + + // Link off-mesh connection to target poly. + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = ref; + link->edge = (unsigned char)0; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = poly->firstLink; + poly->firstLink = idx; + } + + // Start end-point is always connect back to off-mesh connection. + unsigned int tidx = allocLink(tile); + if (tidx != DT_NULL_LINK) + { + const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); + dtPoly* landPoly = &tile->polys[landPolyIdx]; + dtLink* link = &tile->links[tidx]; + link->ref = base | (dtPolyRef)(con->poly); + link->edge = 0xff; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = landPoly->firstLink; + landPoly->firstLink = tidx; + } + } +} + +void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const +{ + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + getTileAndPolyByRefUnsafe(ref, &tile, &poly); + + // Off-mesh connections don't have detail polygons. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const float* v0 = &tile->verts[poly->verts[0]*3]; + const float* v1 = &tile->verts[poly->verts[1]*3]; + const float d0 = dtVdist(pos, v0); + const float d1 = dtVdist(pos, v1); + const float u = d0 / (d0+d1); + dtVlerp(closest, v0, v1, u); + if (posOverPoly) + *posOverPoly = false; + return; + } + + const unsigned int ip = (unsigned int)(poly - tile->polys); + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + + // Clamp point to be inside the polygon. + float verts[DT_VERTS_PER_POLYGON*3]; + float edged[DT_VERTS_PER_POLYGON]; + float edget[DT_VERTS_PER_POLYGON]; + const int nv = poly->vertCount; + for (int i = 0; i < nv; ++i) + dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); + + dtVcopy(closest, pos); + if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) + { + // Point is outside the polygon, dtClamp to nearest edge. + float dmin = edged[0]; + int imin = 0; + for (int i = 1; i < nv; ++i) + { + if (edged[i] < dmin) + { + dmin = edged[i]; + imin = i; + } + } + const float* va = &verts[imin*3]; + const float* vb = &verts[((imin+1)%nv)*3]; + dtVlerp(closest, va, vb, edget[imin]); + + if (posOverPoly) + *posOverPoly = false; + } + else + { + if (posOverPoly) + *posOverPoly = true; + } + + // Find height at the location. + for (int j = 0; j < pd->triCount; ++j) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; + const float* v[3]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < poly->vertCount) + v[k] = &tile->verts[poly->verts[t[k]]*3]; + else + v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; + } + float h; + if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h)) + { + closest[1] = h; + break; + } + } +} + +dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, + const float* center, const float* halfExtents, + float* nearestPt) const +{ + float bmin[3], bmax[3]; + dtVsub(bmin, center, halfExtents); + dtVadd(bmax, center, halfExtents); + + // Get nearby polygons from proximity grid. + dtPolyRef polys[128]; + int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128); + + // Find nearest polygon amongst the nearby polygons. + dtPolyRef nearest = 0; + float nearestDistanceSqr = FLT_MAX; + for (int i = 0; i < polyCount; ++i) + { + dtPolyRef ref = polys[i]; + float closestPtPoly[3]; + float diff[3]; + bool posOverPoly = false; + float d; + closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly); + + // If a point is directly over a polygon and closer than + // climb height, favor that instead of straight line nearest point. + dtVsub(diff, center, closestPtPoly); + if (posOverPoly) + { + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); + } + + if (d < nearestDistanceSqr) + { + dtVcopy(nearestPt, closestPtPoly); + nearestDistanceSqr = d; + nearest = ref; + } + } + + return nearest; +} + +int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, + dtPolyRef* polys, const int maxPolys) const +{ + if (tile->bvTree) + { + const dtBVNode* node = &tile->bvTree[0]; + const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount]; + const float* tbmin = tile->header->bmin; + const float* tbmax = tile->header->bmax; + const float qfac = tile->header->bvQuantFactor; + + // Calculate quantized box + unsigned short bmin[3], bmax[3]; + // dtClamp query box to world box. + float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; + float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]; + float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]; + float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]; + float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]; + float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]; + // Quantize + bmin[0] = (unsigned short)(qfac * minx) & 0xfffe; + bmin[1] = (unsigned short)(qfac * miny) & 0xfffe; + bmin[2] = (unsigned short)(qfac * minz) & 0xfffe; + bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; + bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; + bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; + + // Traverse tree + dtPolyRef base = getPolyRefBase(tile); + int n = 0; + while (node < end) + { + const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax); + const bool isLeafNode = node->i >= 0; + + if (isLeafNode && overlap) + { + if (n < maxPolys) + polys[n++] = base | (dtPolyRef)node->i; + } + + if (overlap || isLeafNode) + node++; + else + { + const int escapeIndex = -node->i; + node += escapeIndex; + } + } + + return n; + } + else + { + float bmin[3], bmax[3]; + int n = 0; + dtPolyRef base = getPolyRefBase(tile); + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + // Calc polygon bounds. + const float* v = &tile->verts[p->verts[0]*3]; + dtVcopy(bmin, v); + dtVcopy(bmax, v); + for (int j = 1; j < p->vertCount; ++j) + { + v = &tile->verts[p->verts[j]*3]; + dtVmin(bmin, v); + dtVmax(bmax, v); + } + if (dtOverlapBounds(qmin,qmax, bmin,bmax)) + { + if (n < maxPolys) + polys[n++] = base | (dtPolyRef)i; + } + } + return n; + } +} + +/// @par +/// +/// The add operation will fail if the data is in the wrong format, the allocated tile +/// space is full, or there is a tile already at the specified reference. +/// +/// The lastRef parameter is used to restore a tile with the same tile +/// reference it had previously used. In this case the #dtPolyRef's for the +/// tile will be restored to the same values they were before the tile was +/// removed. +/// +/// The nav mesh assumes exclusive access to the data passed and will make +/// changes to the dynamic portion of the data. For that reason the data +/// should not be reused in other nav meshes until the tile has been successfully +/// removed from this nav mesh. +/// +/// @see dtCreateNavMeshData, #removeTile +dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, + dtTileRef lastRef, dtTileRef* result) +{ + // Make sure the data is in right format. + dtMeshHeader* header = (dtMeshHeader*)data; + if (header->magic != DT_NAVMESH_MAGIC) + return DT_FAILURE | DT_WRONG_MAGIC; + if (header->version != DT_NAVMESH_VERSION) + return DT_FAILURE | DT_WRONG_VERSION; + + // Make sure the location is free. + if (getTileAt(header->x, header->y, header->layer)) + return DT_FAILURE | DT_ALREADY_OCCUPIED; + + // Allocate a tile. + dtMeshTile* tile = 0; + if (!lastRef) + { + if (m_nextFree) + { + tile = m_nextFree; + m_nextFree = tile->next; + tile->next = 0; + } + } + else + { + // Try to relocate the tile to specific index with same salt. + int tileIndex = (int)decodePolyIdTile((dtPolyRef)lastRef); + if (tileIndex >= m_maxTiles) + return DT_FAILURE | DT_OUT_OF_MEMORY; + // Try to find the specific tile id from the free list. + dtMeshTile* target = &m_tiles[tileIndex]; + dtMeshTile* prev = 0; + tile = m_nextFree; + while (tile && tile != target) + { + prev = tile; + tile = tile->next; + } + // Could not find the correct location. + if (tile != target) + return DT_FAILURE | DT_OUT_OF_MEMORY; + // Remove from freelist + if (!prev) + m_nextFree = tile->next; + else + prev->next = tile->next; + + // Restore salt. + tile->salt = decodePolyIdSalt((dtPolyRef)lastRef); + } + + // Make sure we could allocate a tile. + if (!tile) + return DT_FAILURE | DT_OUT_OF_MEMORY; + + // Insert tile into the position lut. + int h = computeTileHash(header->x, header->y, m_tileLutMask); + tile->next = m_posLookup[h]; + m_posLookup[h] = tile; + + // Patch header pointers. + const int headerSize = dtAlign4(sizeof(dtMeshHeader)); + const int vertsSize = dtAlign4(sizeof(float)*3*header->vertCount); + const int polysSize = dtAlign4(sizeof(dtPoly)*header->polyCount); + const int linksSize = dtAlign4(sizeof(dtLink)*(header->maxLinkCount)); + const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*header->detailMeshCount); + const int detailVertsSize = dtAlign4(sizeof(float)*3*header->detailVertCount); + const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*header->detailTriCount); + const int bvtreeSize = dtAlign4(sizeof(dtBVNode)*header->bvNodeCount); + const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount); + + unsigned char* d = data + headerSize; + tile->verts = dtGetThenAdvanceBufferPointer(d, vertsSize); + tile->polys = dtGetThenAdvanceBufferPointer(d, polysSize); + tile->links = dtGetThenAdvanceBufferPointer(d, linksSize); + tile->detailMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); + tile->detailVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); + tile->detailTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); + tile->bvTree = dtGetThenAdvanceBufferPointer(d, bvtreeSize); + tile->offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshLinksSize); + + // If there are no items in the bvtree, reset the tree pointer. + if (!bvtreeSize) + tile->bvTree = 0; + + // Build links freelist + tile->linksFreeList = 0; + tile->links[header->maxLinkCount-1].next = DT_NULL_LINK; + for (int i = 0; i < header->maxLinkCount-1; ++i) + tile->links[i].next = i+1; + + // Init tile. + tile->header = header; + tile->data = data; + tile->dataSize = dataSize; + tile->flags = flags; + + connectIntLinks(tile); + + // Base off-mesh connections to their starting polygons and connect connections inside the tile. + baseOffMeshLinks(tile); + connectExtOffMeshLinks(tile, tile, -1); + + // Create connections with neighbour tiles. + static const int MAX_NEIS = 32; + dtMeshTile* neis[MAX_NEIS]; + int nneis; + + // Connect with layers in current tile. + nneis = getTilesAt(header->x, header->y, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + { + if (neis[j] == tile) + continue; + + connectExtLinks(tile, neis[j], -1); + connectExtLinks(neis[j], tile, -1); + connectExtOffMeshLinks(tile, neis[j], -1); + connectExtOffMeshLinks(neis[j], tile, -1); + } + + // Connect with neighbour tiles. + for (int i = 0; i < 8; ++i) + { + nneis = getNeighbourTilesAt(header->x, header->y, i, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + { + connectExtLinks(tile, neis[j], i); + connectExtLinks(neis[j], tile, dtOppositeTile(i)); + connectExtOffMeshLinks(tile, neis[j], i); + connectExtOffMeshLinks(neis[j], tile, dtOppositeTile(i)); + } + } + + if (result) + *result = getTileRef(tile); + + return DT_SUCCESS; +} + +const dtMeshTile* dtNavMesh::getTileAt(const int x, const int y, const int layer) const +{ + // Find tile based on hash. + int h = computeTileHash(x,y,m_tileLutMask); + dtMeshTile* tile = m_posLookup[h]; + while (tile) + { + if (tile->header && + tile->header->x == x && + tile->header->y == y && + tile->header->layer == layer) + { + return tile; + } + tile = tile->next; + } + return 0; +} + +int dtNavMesh::getNeighbourTilesAt(const int x, const int y, const int side, dtMeshTile** tiles, const int maxTiles) const +{ + int nx = x, ny = y; + switch (side) + { + case 0: nx++; break; + case 1: nx++; ny++; break; + case 2: ny++; break; + case 3: nx--; ny++; break; + case 4: nx--; break; + case 5: nx--; ny--; break; + case 6: ny--; break; + case 7: nx++; ny--; break; + }; + + return getTilesAt(nx, ny, tiles, maxTiles); +} + +int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile** tiles, const int maxTiles) const +{ + int n = 0; + + // Find tile based on hash. + int h = computeTileHash(x,y,m_tileLutMask); + dtMeshTile* tile = m_posLookup[h]; + while (tile) + { + if (tile->header && + tile->header->x == x && + tile->header->y == y) + { + if (n < maxTiles) + tiles[n++] = tile; + } + tile = tile->next; + } + + return n; +} + +/// @par +/// +/// This function will not fail if the tiles array is too small to hold the +/// entire result set. It will simply fill the array to capacity. +int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile const** tiles, const int maxTiles) const +{ + int n = 0; + + // Find tile based on hash. + int h = computeTileHash(x,y,m_tileLutMask); + dtMeshTile* tile = m_posLookup[h]; + while (tile) + { + if (tile->header && + tile->header->x == x && + tile->header->y == y) + { + if (n < maxTiles) + tiles[n++] = tile; + } + tile = tile->next; + } + + return n; +} + + +dtTileRef dtNavMesh::getTileRefAt(const int x, const int y, const int layer) const +{ + // Find tile based on hash. + int h = computeTileHash(x,y,m_tileLutMask); + dtMeshTile* tile = m_posLookup[h]; + while (tile) + { + if (tile->header && + tile->header->x == x && + tile->header->y == y && + tile->header->layer == layer) + { + return getTileRef(tile); + } + tile = tile->next; + } + return 0; +} + +const dtMeshTile* dtNavMesh::getTileByRef(dtTileRef ref) const +{ + if (!ref) + return 0; + unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref); + unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref); + if ((int)tileIndex >= m_maxTiles) + return 0; + const dtMeshTile* tile = &m_tiles[tileIndex]; + if (tile->salt != tileSalt) + return 0; + return tile; +} + +int dtNavMesh::getMaxTiles() const +{ + return m_maxTiles; +} + +dtMeshTile* dtNavMesh::getTile(int i) +{ + return &m_tiles[i]; +} + +const dtMeshTile* dtNavMesh::getTile(int i) const +{ + return &m_tiles[i]; +} + +void dtNavMesh::calcTileLoc(const float* pos, int* tx, int* ty) const +{ + *tx = (int)floorf((pos[0]-m_orig[0]) / m_tileWidth); + *ty = (int)floorf((pos[2]-m_orig[2]) / m_tileHeight); +} + +dtStatus dtNavMesh::getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const +{ + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + if (ip >= (unsigned int)m_tiles[it].header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + *tile = &m_tiles[it]; + *poly = &m_tiles[it].polys[ip]; + return DT_SUCCESS; +} + +/// @par +/// +/// @warning Only use this function if it is known that the provided polygon +/// reference is valid. This function is faster than #getTileAndPolyByRef, but +/// it does not validate the reference. +void dtNavMesh::getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + *tile = &m_tiles[it]; + *poly = &m_tiles[it].polys[ip]; +} + +bool dtNavMesh::isValidPolyRef(dtPolyRef ref) const +{ + if (!ref) return false; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + if (ip >= (unsigned int)m_tiles[it].header->polyCount) return false; + return true; +} + +/// @par +/// +/// This function returns the data for the tile so that, if desired, +/// it can be added back to the navigation mesh at a later point. +/// +/// @see #addTile +dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSize) +{ + if (!ref) + return DT_FAILURE | DT_INVALID_PARAM; + unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref); + unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref); + if ((int)tileIndex >= m_maxTiles) + return DT_FAILURE | DT_INVALID_PARAM; + dtMeshTile* tile = &m_tiles[tileIndex]; + if (tile->salt != tileSalt) + return DT_FAILURE | DT_INVALID_PARAM; + + // Remove tile from hash lookup. + int h = computeTileHash(tile->header->x,tile->header->y,m_tileLutMask); + dtMeshTile* prev = 0; + dtMeshTile* cur = m_posLookup[h]; + while (cur) + { + if (cur == tile) + { + if (prev) + prev->next = cur->next; + else + m_posLookup[h] = cur->next; + break; + } + prev = cur; + cur = cur->next; + } + + // Remove connections to neighbour tiles. + static const int MAX_NEIS = 32; + dtMeshTile* neis[MAX_NEIS]; + int nneis; + + // Disconnect from other layers in current tile. + nneis = getTilesAt(tile->header->x, tile->header->y, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + { + if (neis[j] == tile) continue; + unconnectLinks(neis[j], tile); + } + + // Disconnect from neighbour tiles. + for (int i = 0; i < 8; ++i) + { + nneis = getNeighbourTilesAt(tile->header->x, tile->header->y, i, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + unconnectLinks(neis[j], tile); + } + + // Reset tile. + if (tile->flags & DT_TILE_FREE_DATA) + { + // Owns data + dtFree(tile->data); + tile->data = 0; + tile->dataSize = 0; + if (data) *data = 0; + if (dataSize) *dataSize = 0; + } + else + { + if (data) *data = tile->data; + if (dataSize) *dataSize = tile->dataSize; + } + + tile->header = 0; + tile->flags = 0; + tile->linksFreeList = 0; + tile->polys = 0; + tile->verts = 0; + tile->links = 0; + tile->detailMeshes = 0; + tile->detailVerts = 0; + tile->detailTris = 0; + tile->bvTree = 0; + tile->offMeshCons = 0; + + // Update salt, salt should never be zero. +#ifdef DT_POLYREF64 + tile->salt = (tile->salt+1) & ((1<salt = (tile->salt+1) & ((1<salt == 0) + tile->salt++; + + // Add to free list. + tile->next = m_nextFree; + m_nextFree = tile; + + return DT_SUCCESS; +} + +dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const +{ + if (!tile) return 0; + const unsigned int it = (unsigned int)(tile - m_tiles); + return (dtTileRef)encodePolyId(tile->salt, it, 0); +} + +/// @par +/// +/// Example use case: +/// @code +/// +/// const dtPolyRef base = navmesh->getPolyRefBase(tile); +/// for (int i = 0; i < tile->header->polyCount; ++i) +/// { +/// const dtPoly* p = &tile->polys[i]; +/// const dtPolyRef ref = base | (dtPolyRef)i; +/// +/// // Use the reference to access the polygon data. +/// } +/// @endcode +dtPolyRef dtNavMesh::getPolyRefBase(const dtMeshTile* tile) const +{ + if (!tile) return 0; + const unsigned int it = (unsigned int)(tile - m_tiles); + return encodePolyId(tile->salt, it, 0); +} + +struct dtTileState +{ + int magic; // Magic number, used to identify the data. + int version; // Data version number. + dtTileRef ref; // Tile ref at the time of storing the data. +}; + +struct dtPolyState +{ + unsigned short flags; // Flags (see dtPolyFlags). + unsigned char area; // Area ID of the polygon. +}; + +/// @see #storeTileState +int dtNavMesh::getTileStateSize(const dtMeshTile* tile) const +{ + if (!tile) return 0; + const int headerSize = dtAlign4(sizeof(dtTileState)); + const int polyStateSize = dtAlign4(sizeof(dtPolyState) * tile->header->polyCount); + return headerSize + polyStateSize; +} + +/// @par +/// +/// Tile state includes non-structural data such as polygon flags, area ids, etc. +/// @note The state data is only valid until the tile reference changes. +/// @see #getTileStateSize, #restoreTileState +dtStatus dtNavMesh::storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const +{ + // Make sure there is enough space to store the state. + const int sizeReq = getTileStateSize(tile); + if (maxDataSize < sizeReq) + return DT_FAILURE | DT_BUFFER_TOO_SMALL; + + dtTileState* tileState = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtTileState))); + dtPolyState* polyStates = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount)); + + // Store tile state. + tileState->magic = DT_NAVMESH_STATE_MAGIC; + tileState->version = DT_NAVMESH_STATE_VERSION; + tileState->ref = getTileRef(tile); + + // Store per poly state. + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + dtPolyState* s = &polyStates[i]; + s->flags = p->flags; + s->area = p->getArea(); + } + + return DT_SUCCESS; +} + +/// @par +/// +/// Tile state includes non-structural data such as polygon flags, area ids, etc. +/// @note This function does not impact the tile's #dtTileRef and #dtPolyRef's. +/// @see #storeTileState +dtStatus dtNavMesh::restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize) +{ + // Make sure there is enough space to store the state. + const int sizeReq = getTileStateSize(tile); + if (maxDataSize < sizeReq) + return DT_FAILURE | DT_INVALID_PARAM; + + const dtTileState* tileState = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtTileState))); + const dtPolyState* polyStates = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount)); + + // Check that the restore is possible. + if (tileState->magic != DT_NAVMESH_STATE_MAGIC) + return DT_FAILURE | DT_WRONG_MAGIC; + if (tileState->version != DT_NAVMESH_STATE_VERSION) + return DT_FAILURE | DT_WRONG_VERSION; + if (tileState->ref != getTileRef(tile)) + return DT_FAILURE | DT_INVALID_PARAM; + + // Restore per poly state. + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* p = &tile->polys[i]; + const dtPolyState* s = &polyStates[i]; + p->flags = s->flags; + p->setArea(s->area); + } + + return DT_SUCCESS; +} + +/// @par +/// +/// Off-mesh connections are stored in the navigation mesh as special 2-vertex +/// polygons with a single edge. At least one of the vertices is expected to be +/// inside a normal polygon. So an off-mesh connection is "entered" from a +/// normal polygon at one of its endpoints. This is the polygon identified by +/// the prevRef parameter. +dtStatus dtNavMesh::getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const +{ + unsigned int salt, it, ip; + + if (!polyRef) + return DT_FAILURE; + + // Get current polygon + decodePolyId(polyRef, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + const dtPoly* poly = &tile->polys[ip]; + + // Make sure that the current poly is indeed off-mesh link. + if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) + return DT_FAILURE; + + // Figure out which way to hand out the vertices. + int idx0 = 0, idx1 = 1; + + // Find link that points to first vertex. + for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) + { + if (tile->links[i].edge == 0) + { + if (tile->links[i].ref != prevRef) + { + idx0 = 1; + idx1 = 0; + } + break; + } + } + + dtVcopy(startPos, &tile->verts[poly->verts[idx0]*3]); + dtVcopy(endPos, &tile->verts[poly->verts[idx1]*3]); + + return DT_SUCCESS; +} + + +const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) const +{ + unsigned int salt, it, ip; + + if (!ref) + return 0; + + // Get current polygon + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return 0; + const dtPoly* poly = &tile->polys[ip]; + + // Make sure that the current poly is indeed off-mesh link. + if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) + return 0; + + const unsigned int idx = ip - tile->header->offMeshBase; + dtAssert(idx < (unsigned int)tile->header->offMeshConCount); + return &tile->offMeshCons[idx]; +} + + +dtStatus dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags) +{ + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + dtPoly* poly = &tile->polys[ip]; + + // Change flags. + poly->flags = flags; + + return DT_SUCCESS; +} + +dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const +{ + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + const dtPoly* poly = &tile->polys[ip]; + + *resultFlags = poly->flags; + + return DT_SUCCESS; +} + +dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area) +{ + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + dtPoly* poly = &tile->polys[ip]; + + poly->setArea(area); + + return DT_SUCCESS; +} + +dtStatus dtNavMesh::getPolyArea(dtPolyRef ref, unsigned char* resultArea) const +{ + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + const dtPoly* poly = &tile->polys[ip]; + + *resultArea = poly->getArea(); + + return DT_SUCCESS; +} + diff --git a/extern/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp b/extern/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp new file mode 100644 index 000000000..e93a97629 --- /dev/null +++ b/extern/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp @@ -0,0 +1,802 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include +#include +#include +#include "DetourNavMesh.h" +#include "DetourCommon.h" +#include "DetourMath.h" +#include "DetourNavMeshBuilder.h" +#include "DetourAlloc.h" +#include "DetourAssert.h" + +static unsigned short MESH_NULL_IDX = 0xffff; + + +struct BVItem +{ + unsigned short bmin[3]; + unsigned short bmax[3]; + int i; +}; + +static int compareItemX(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[0] < b->bmin[0]) + return -1; + if (a->bmin[0] > b->bmin[0]) + return 1; + return 0; +} + +static int compareItemY(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[1] < b->bmin[1]) + return -1; + if (a->bmin[1] > b->bmin[1]) + return 1; + return 0; +} + +static int compareItemZ(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[2] < b->bmin[2]) + return -1; + if (a->bmin[2] > b->bmin[2]) + return 1; + return 0; +} + +static void calcExtends(BVItem* items, const int /*nitems*/, const int imin, const int imax, + unsigned short* bmin, unsigned short* bmax) +{ + bmin[0] = items[imin].bmin[0]; + bmin[1] = items[imin].bmin[1]; + bmin[2] = items[imin].bmin[2]; + + bmax[0] = items[imin].bmax[0]; + bmax[1] = items[imin].bmax[1]; + bmax[2] = items[imin].bmax[2]; + + for (int i = imin+1; i < imax; ++i) + { + const BVItem& it = items[i]; + if (it.bmin[0] < bmin[0]) bmin[0] = it.bmin[0]; + if (it.bmin[1] < bmin[1]) bmin[1] = it.bmin[1]; + if (it.bmin[2] < bmin[2]) bmin[2] = it.bmin[2]; + + if (it.bmax[0] > bmax[0]) bmax[0] = it.bmax[0]; + if (it.bmax[1] > bmax[1]) bmax[1] = it.bmax[1]; + if (it.bmax[2] > bmax[2]) bmax[2] = it.bmax[2]; + } +} + +inline int longestAxis(unsigned short x, unsigned short y, unsigned short z) +{ + int axis = 0; + unsigned short maxVal = x; + if (y > maxVal) + { + axis = 1; + maxVal = y; + } + if (z > maxVal) + { + axis = 2; + } + return axis; +} + +static void subdivide(BVItem* items, int nitems, int imin, int imax, int& curNode, dtBVNode* nodes) +{ + int inum = imax - imin; + int icur = curNode; + + dtBVNode& node = nodes[curNode++]; + + if (inum == 1) + { + // Leaf + node.bmin[0] = items[imin].bmin[0]; + node.bmin[1] = items[imin].bmin[1]; + node.bmin[2] = items[imin].bmin[2]; + + node.bmax[0] = items[imin].bmax[0]; + node.bmax[1] = items[imin].bmax[1]; + node.bmax[2] = items[imin].bmax[2]; + + node.i = items[imin].i; + } + else + { + // Split + calcExtends(items, nitems, imin, imax, node.bmin, node.bmax); + + int axis = longestAxis(node.bmax[0] - node.bmin[0], + node.bmax[1] - node.bmin[1], + node.bmax[2] - node.bmin[2]); + + if (axis == 0) + { + // Sort along x-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemX); + } + else if (axis == 1) + { + // Sort along y-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemY); + } + else + { + // Sort along z-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemZ); + } + + int isplit = imin+inum/2; + + // Left + subdivide(items, nitems, imin, isplit, curNode, nodes); + // Right + subdivide(items, nitems, isplit, imax, curNode, nodes); + + int iescape = curNode - icur; + // Negative index means escape. + node.i = -iescape; + } +} + +static int createBVTree(dtNavMeshCreateParams* params, dtBVNode* nodes, int /*nnodes*/) +{ + // Build tree + float quantFactor = 1 / params->cs; + BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*params->polyCount, DT_ALLOC_TEMP); + for (int i = 0; i < params->polyCount; i++) + { + BVItem& it = items[i]; + it.i = i; + // Calc polygon bounds. Use detail meshes if available. + if (params->detailMeshes) + { + int vb = (int)params->detailMeshes[i*4+0]; + int ndv = (int)params->detailMeshes[i*4+1]; + float bmin[3]; + float bmax[3]; + + const float* dv = ¶ms->detailVerts[vb*3]; + dtVcopy(bmin, dv); + dtVcopy(bmax, dv); + + for (int j = 1; j < ndv; j++) + { + dtVmin(bmin, &dv[j * 3]); + dtVmax(bmax, &dv[j * 3]); + } + + // BV-tree uses cs for all dimensions + it.bmin[0] = (unsigned short)dtClamp((int)((bmin[0] - params->bmin[0])*quantFactor), 0, 0xffff); + it.bmin[1] = (unsigned short)dtClamp((int)((bmin[1] - params->bmin[1])*quantFactor), 0, 0xffff); + it.bmin[2] = (unsigned short)dtClamp((int)((bmin[2] - params->bmin[2])*quantFactor), 0, 0xffff); + + it.bmax[0] = (unsigned short)dtClamp((int)((bmax[0] - params->bmin[0])*quantFactor), 0, 0xffff); + it.bmax[1] = (unsigned short)dtClamp((int)((bmax[1] - params->bmin[1])*quantFactor), 0, 0xffff); + it.bmax[2] = (unsigned short)dtClamp((int)((bmax[2] - params->bmin[2])*quantFactor), 0, 0xffff); + } + else + { + const unsigned short* p = ¶ms->polys[i*params->nvp * 2]; + it.bmin[0] = it.bmax[0] = params->verts[p[0] * 3 + 0]; + it.bmin[1] = it.bmax[1] = params->verts[p[0] * 3 + 1]; + it.bmin[2] = it.bmax[2] = params->verts[p[0] * 3 + 2]; + + for (int j = 1; j < params->nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + unsigned short x = params->verts[p[j] * 3 + 0]; + unsigned short y = params->verts[p[j] * 3 + 1]; + unsigned short z = params->verts[p[j] * 3 + 2]; + + if (x < it.bmin[0]) it.bmin[0] = x; + if (y < it.bmin[1]) it.bmin[1] = y; + if (z < it.bmin[2]) it.bmin[2] = z; + + if (x > it.bmax[0]) it.bmax[0] = x; + if (y > it.bmax[1]) it.bmax[1] = y; + if (z > it.bmax[2]) it.bmax[2] = z; + } + // Remap y + it.bmin[1] = (unsigned short)dtMathFloorf((float)it.bmin[1] * params->ch / params->cs); + it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1] * params->ch / params->cs); + } + } + + int curNode = 0; + subdivide(items, params->polyCount, 0, params->polyCount, curNode, nodes); + + dtFree(items); + + return curNode; +} + +static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, const float* bmax) +{ + static const unsigned char XP = 1<<0; + static const unsigned char ZP = 1<<1; + static const unsigned char XM = 1<<2; + static const unsigned char ZM = 1<<3; + + unsigned char outcode = 0; + outcode |= (pt[0] >= bmax[0]) ? XP : 0; + outcode |= (pt[2] >= bmax[2]) ? ZP : 0; + outcode |= (pt[0] < bmin[0]) ? XM : 0; + outcode |= (pt[2] < bmin[2]) ? ZM : 0; + + switch (outcode) + { + case XP: return 0; + case XP|ZP: return 1; + case ZP: return 2; + case XM|ZP: return 3; + case XM: return 4; + case XM|ZM: return 5; + case ZM: return 6; + case XP|ZM: return 7; + }; + + return 0xff; +} + +// TODO: Better error handling. + +/// @par +/// +/// The output data array is allocated using the detour allocator (dtAlloc()). The method +/// used to free the memory will be determined by how the tile is added to the navigation +/// mesh. +/// +/// @see dtNavMesh, dtNavMesh::addTile() +bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize) +{ + if (params->nvp > DT_VERTS_PER_POLYGON) + return false; + if (params->vertCount >= 0xffff) + return false; + if (!params->vertCount || !params->verts) + return false; + if (!params->polyCount || !params->polys) + return false; + + const int nvp = params->nvp; + + // Classify off-mesh connection points. We store only the connections + // whose start point is inside the tile. + unsigned char* offMeshConClass = 0; + int storedOffMeshConCount = 0; + int offMeshConLinkCount = 0; + + if (params->offMeshConCount > 0) + { + offMeshConClass = (unsigned char*)dtAlloc(sizeof(unsigned char)*params->offMeshConCount*2, DT_ALLOC_TEMP); + if (!offMeshConClass) + return false; + + // Find tight heigh bounds, used for culling out off-mesh start locations. + float hmin = FLT_MAX; + float hmax = -FLT_MAX; + + if (params->detailVerts && params->detailVertsCount) + { + for (int i = 0; i < params->detailVertsCount; ++i) + { + const float h = params->detailVerts[i*3+1]; + hmin = dtMin(hmin,h); + hmax = dtMax(hmax,h); + } + } + else + { + for (int i = 0; i < params->vertCount; ++i) + { + const unsigned short* iv = ¶ms->verts[i*3]; + const float h = params->bmin[1] + iv[1] * params->ch; + hmin = dtMin(hmin,h); + hmax = dtMax(hmax,h); + } + } + hmin -= params->walkableClimb; + hmax += params->walkableClimb; + float bmin[3], bmax[3]; + dtVcopy(bmin, params->bmin); + dtVcopy(bmax, params->bmax); + bmin[1] = hmin; + bmax[1] = hmax; + + for (int i = 0; i < params->offMeshConCount; ++i) + { + const float* p0 = ¶ms->offMeshConVerts[(i*2+0)*3]; + const float* p1 = ¶ms->offMeshConVerts[(i*2+1)*3]; + offMeshConClass[i*2+0] = classifyOffMeshPoint(p0, bmin, bmax); + offMeshConClass[i*2+1] = classifyOffMeshPoint(p1, bmin, bmax); + + // Zero out off-mesh start positions which are not even potentially touching the mesh. + if (offMeshConClass[i*2+0] == 0xff) + { + if (p0[1] < bmin[1] || p0[1] > bmax[1]) + offMeshConClass[i*2+0] = 0; + } + + // Cound how many links should be allocated for off-mesh connections. + if (offMeshConClass[i*2+0] == 0xff) + offMeshConLinkCount++; + if (offMeshConClass[i*2+1] == 0xff) + offMeshConLinkCount++; + + if (offMeshConClass[i*2+0] == 0xff) + storedOffMeshConCount++; + } + } + + // Off-mesh connectionss are stored as polygons, adjust values. + const int totPolyCount = params->polyCount + storedOffMeshConCount; + const int totVertCount = params->vertCount + storedOffMeshConCount*2; + + // Find portal edges which are at tile borders. + int edgeCount = 0; + int portalCount = 0; + for (int i = 0; i < params->polyCount; ++i) + { + const unsigned short* p = ¶ms->polys[i*2*nvp]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + edgeCount++; + + if (p[nvp+j] & 0x8000) + { + unsigned short dir = p[nvp+j] & 0xf; + if (dir != 0xf) + portalCount++; + } + } + } + + const int maxLinkCount = edgeCount + portalCount*2 + offMeshConLinkCount*2; + + // Find unique detail vertices. + int uniqueDetailVertCount = 0; + int detailTriCount = 0; + if (params->detailMeshes) + { + // Has detail mesh, count unique detail vertex count and use input detail tri count. + detailTriCount = params->detailTriCount; + for (int i = 0; i < params->polyCount; ++i) + { + const unsigned short* p = ¶ms->polys[i*nvp*2]; + int ndv = params->detailMeshes[i*4+1]; + int nv = 0; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + nv++; + } + ndv -= nv; + uniqueDetailVertCount += ndv; + } + } + else + { + // No input detail mesh, build detail mesh from nav polys. + uniqueDetailVertCount = 0; // No extra detail verts. + detailTriCount = 0; + for (int i = 0; i < params->polyCount; ++i) + { + const unsigned short* p = ¶ms->polys[i*nvp*2]; + int nv = 0; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + nv++; + } + detailTriCount += nv-2; + } + } + + // Calculate data size + const int headerSize = dtAlign4(sizeof(dtMeshHeader)); + const int vertsSize = dtAlign4(sizeof(float)*3*totVertCount); + const int polysSize = dtAlign4(sizeof(dtPoly)*totPolyCount); + const int linksSize = dtAlign4(sizeof(dtLink)*maxLinkCount); + const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount); + const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount); + const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount); + const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode)*params->polyCount*2) : 0; + const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount); + + const int dataSize = headerSize + vertsSize + polysSize + linksSize + + detailMeshesSize + detailVertsSize + detailTrisSize + + bvTreeSize + offMeshConsSize; + + unsigned char* data = (unsigned char*)dtAlloc(sizeof(unsigned char)*dataSize, DT_ALLOC_PERM); + if (!data) + { + dtFree(offMeshConClass); + return false; + } + memset(data, 0, dataSize); + + unsigned char* d = data; + + dtMeshHeader* header = dtGetThenAdvanceBufferPointer(d, headerSize); + float* navVerts = dtGetThenAdvanceBufferPointer(d, vertsSize); + dtPoly* navPolys = dtGetThenAdvanceBufferPointer(d, polysSize); + d += linksSize; // Ignore links; just leave enough space for them. They'll be created on load. + dtPolyDetail* navDMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); + float* navDVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); + unsigned char* navDTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); + dtBVNode* navBvtree = dtGetThenAdvanceBufferPointer(d, bvTreeSize); + dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshConsSize); + + + // Store header + header->magic = DT_NAVMESH_MAGIC; + header->version = DT_NAVMESH_VERSION; + header->x = params->tileX; + header->y = params->tileY; + header->layer = params->tileLayer; + header->userId = params->userId; + header->polyCount = totPolyCount; + header->vertCount = totVertCount; + header->maxLinkCount = maxLinkCount; + dtVcopy(header->bmin, params->bmin); + dtVcopy(header->bmax, params->bmax); + header->detailMeshCount = params->polyCount; + header->detailVertCount = uniqueDetailVertCount; + header->detailTriCount = detailTriCount; + header->bvQuantFactor = 1.0f / params->cs; + header->offMeshBase = params->polyCount; + header->walkableHeight = params->walkableHeight; + header->walkableRadius = params->walkableRadius; + header->walkableClimb = params->walkableClimb; + header->offMeshConCount = storedOffMeshConCount; + header->bvNodeCount = params->buildBvTree ? params->polyCount*2 : 0; + + const int offMeshVertsBase = params->vertCount; + const int offMeshPolyBase = params->polyCount; + + // Store vertices + // Mesh vertices + for (int i = 0; i < params->vertCount; ++i) + { + const unsigned short* iv = ¶ms->verts[i*3]; + float* v = &navVerts[i*3]; + v[0] = params->bmin[0] + iv[0] * params->cs; + v[1] = params->bmin[1] + iv[1] * params->ch; + v[2] = params->bmin[2] + iv[2] * params->cs; + } + // Off-mesh link vertices. + int n = 0; + for (int i = 0; i < params->offMeshConCount; ++i) + { + // Only store connections which start from this tile. + if (offMeshConClass[i*2+0] == 0xff) + { + const float* linkv = ¶ms->offMeshConVerts[i*2*3]; + float* v = &navVerts[(offMeshVertsBase + n*2)*3]; + dtVcopy(&v[0], &linkv[0]); + dtVcopy(&v[3], &linkv[3]); + n++; + } + } + + // Store polygons + // Mesh polys + const unsigned short* src = params->polys; + for (int i = 0; i < params->polyCount; ++i) + { + dtPoly* p = &navPolys[i]; + p->vertCount = 0; + p->flags = params->polyFlags[i]; + p->setArea(params->polyAreas[i]); + p->setType(DT_POLYTYPE_GROUND); + for (int j = 0; j < nvp; ++j) + { + if (src[j] == MESH_NULL_IDX) break; + p->verts[j] = src[j]; + if (src[nvp+j] & 0x8000) + { + // Border or portal edge. + unsigned short dir = src[nvp+j] & 0xf; + if (dir == 0xf) // Border + p->neis[j] = 0; + else if (dir == 0) // Portal x- + p->neis[j] = DT_EXT_LINK | 4; + else if (dir == 1) // Portal z+ + p->neis[j] = DT_EXT_LINK | 2; + else if (dir == 2) // Portal x+ + p->neis[j] = DT_EXT_LINK | 0; + else if (dir == 3) // Portal z- + p->neis[j] = DT_EXT_LINK | 6; + } + else + { + // Normal connection + p->neis[j] = src[nvp+j]+1; + } + + p->vertCount++; + } + src += nvp*2; + } + // Off-mesh connection vertices. + n = 0; + for (int i = 0; i < params->offMeshConCount; ++i) + { + // Only store connections which start from this tile. + if (offMeshConClass[i*2+0] == 0xff) + { + dtPoly* p = &navPolys[offMeshPolyBase+n]; + p->vertCount = 2; + p->verts[0] = (unsigned short)(offMeshVertsBase + n*2+0); + p->verts[1] = (unsigned short)(offMeshVertsBase + n*2+1); + p->flags = params->offMeshConFlags[i]; + p->setArea(params->offMeshConAreas[i]); + p->setType(DT_POLYTYPE_OFFMESH_CONNECTION); + n++; + } + } + + // Store detail meshes and vertices. + // The nav polygon vertices are stored as the first vertices on each mesh. + // We compress the mesh data by skipping them and using the navmesh coordinates. + if (params->detailMeshes) + { + unsigned short vbase = 0; + for (int i = 0; i < params->polyCount; ++i) + { + dtPolyDetail& dtl = navDMeshes[i]; + const int vb = (int)params->detailMeshes[i*4+0]; + const int ndv = (int)params->detailMeshes[i*4+1]; + const int nv = navPolys[i].vertCount; + dtl.vertBase = (unsigned int)vbase; + dtl.vertCount = (unsigned char)(ndv-nv); + dtl.triBase = (unsigned int)params->detailMeshes[i*4+2]; + dtl.triCount = (unsigned char)params->detailMeshes[i*4+3]; + // Copy vertices except the first 'nv' verts which are equal to nav poly verts. + if (ndv-nv) + { + memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv)); + vbase += (unsigned short)(ndv-nv); + } + } + // Store triangles. + memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount); + } + else + { + // Create dummy detail mesh by triangulating polys. + int tbase = 0; + for (int i = 0; i < params->polyCount; ++i) + { + dtPolyDetail& dtl = navDMeshes[i]; + const int nv = navPolys[i].vertCount; + dtl.vertBase = 0; + dtl.vertCount = 0; + dtl.triBase = (unsigned int)tbase; + dtl.triCount = (unsigned char)(nv-2); + // Triangulate polygon (local indices). + for (int j = 2; j < nv; ++j) + { + unsigned char* t = &navDTris[tbase*4]; + t[0] = 0; + t[1] = (unsigned char)(j-1); + t[2] = (unsigned char)j; + // Bit for each edge that belongs to poly boundary. + t[3] = (1<<2); + if (j == 2) t[3] |= (1<<0); + if (j == nv-1) t[3] |= (1<<4); + tbase++; + } + } + } + + // Store and create BVtree. + if (params->buildBvTree) + { + createBVTree(params, navBvtree, 2*params->polyCount); + } + + // Store Off-Mesh connections. + n = 0; + for (int i = 0; i < params->offMeshConCount; ++i) + { + // Only store connections which start from this tile. + if (offMeshConClass[i*2+0] == 0xff) + { + dtOffMeshConnection* con = &offMeshCons[n]; + con->poly = (unsigned short)(offMeshPolyBase + n); + // Copy connection end-points. + const float* endPts = ¶ms->offMeshConVerts[i*2*3]; + dtVcopy(&con->pos[0], &endPts[0]); + dtVcopy(&con->pos[3], &endPts[3]); + con->rad = params->offMeshConRad[i]; + con->flags = params->offMeshConDir[i] ? DT_OFFMESH_CON_BIDIR : 0; + con->side = offMeshConClass[i*2+1]; + if (params->offMeshConUserID) + con->userId = params->offMeshConUserID[i]; + n++; + } + } + + dtFree(offMeshConClass); + + *outData = data; + *outDataSize = dataSize; + + return true; +} + +bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/) +{ + dtMeshHeader* header = (dtMeshHeader*)data; + + int swappedMagic = DT_NAVMESH_MAGIC; + int swappedVersion = DT_NAVMESH_VERSION; + dtSwapEndian(&swappedMagic); + dtSwapEndian(&swappedVersion); + + if ((header->magic != DT_NAVMESH_MAGIC || header->version != DT_NAVMESH_VERSION) && + (header->magic != swappedMagic || header->version != swappedVersion)) + { + return false; + } + + dtSwapEndian(&header->magic); + dtSwapEndian(&header->version); + dtSwapEndian(&header->x); + dtSwapEndian(&header->y); + dtSwapEndian(&header->layer); + dtSwapEndian(&header->userId); + dtSwapEndian(&header->polyCount); + dtSwapEndian(&header->vertCount); + dtSwapEndian(&header->maxLinkCount); + dtSwapEndian(&header->detailMeshCount); + dtSwapEndian(&header->detailVertCount); + dtSwapEndian(&header->detailTriCount); + dtSwapEndian(&header->bvNodeCount); + dtSwapEndian(&header->offMeshConCount); + dtSwapEndian(&header->offMeshBase); + dtSwapEndian(&header->walkableHeight); + dtSwapEndian(&header->walkableRadius); + dtSwapEndian(&header->walkableClimb); + dtSwapEndian(&header->bmin[0]); + dtSwapEndian(&header->bmin[1]); + dtSwapEndian(&header->bmin[2]); + dtSwapEndian(&header->bmax[0]); + dtSwapEndian(&header->bmax[1]); + dtSwapEndian(&header->bmax[2]); + dtSwapEndian(&header->bvQuantFactor); + + // Freelist index and pointers are updated when tile is added, no need to swap. + + return true; +} + +/// @par +/// +/// @warning This function assumes that the header is in the correct endianess already. +/// Call #dtNavMeshHeaderSwapEndian() first on the data if the data is expected to be in wrong endianess +/// to start with. Call #dtNavMeshHeaderSwapEndian() after the data has been swapped if converting from +/// native to foreign endianess. +bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) +{ + // Make sure the data is in right format. + dtMeshHeader* header = (dtMeshHeader*)data; + if (header->magic != DT_NAVMESH_MAGIC) + return false; + if (header->version != DT_NAVMESH_VERSION) + return false; + + // Patch header pointers. + const int headerSize = dtAlign4(sizeof(dtMeshHeader)); + const int vertsSize = dtAlign4(sizeof(float)*3*header->vertCount); + const int polysSize = dtAlign4(sizeof(dtPoly)*header->polyCount); + const int linksSize = dtAlign4(sizeof(dtLink)*(header->maxLinkCount)); + const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*header->detailMeshCount); + const int detailVertsSize = dtAlign4(sizeof(float)*3*header->detailVertCount); + const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*header->detailTriCount); + const int bvtreeSize = dtAlign4(sizeof(dtBVNode)*header->bvNodeCount); + const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount); + + unsigned char* d = data + headerSize; + float* verts = dtGetThenAdvanceBufferPointer(d, vertsSize); + dtPoly* polys = dtGetThenAdvanceBufferPointer(d, polysSize); + d += linksSize; // Ignore links; they technically should be endian-swapped but all their data is overwritten on load anyway. + //dtLink* links = dtGetThenAdvanceBufferPointer(d, linksSize); + dtPolyDetail* detailMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); + float* detailVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); + d += detailTrisSize; // Ignore detail tris; single bytes can't be endian-swapped. + //unsigned char* detailTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); + dtBVNode* bvTree = dtGetThenAdvanceBufferPointer(d, bvtreeSize); + dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshLinksSize); + + // Vertices + for (int i = 0; i < header->vertCount*3; ++i) + { + dtSwapEndian(&verts[i]); + } + + // Polys + for (int i = 0; i < header->polyCount; ++i) + { + dtPoly* p = &polys[i]; + // poly->firstLink is update when tile is added, no need to swap. + for (int j = 0; j < DT_VERTS_PER_POLYGON; ++j) + { + dtSwapEndian(&p->verts[j]); + dtSwapEndian(&p->neis[j]); + } + dtSwapEndian(&p->flags); + } + + // Links are rebuild when tile is added, no need to swap. + + // Detail meshes + for (int i = 0; i < header->detailMeshCount; ++i) + { + dtPolyDetail* pd = &detailMeshes[i]; + dtSwapEndian(&pd->vertBase); + dtSwapEndian(&pd->triBase); + } + + // Detail verts + for (int i = 0; i < header->detailVertCount*3; ++i) + { + dtSwapEndian(&detailVerts[i]); + } + + // BV-tree + for (int i = 0; i < header->bvNodeCount; ++i) + { + dtBVNode* node = &bvTree[i]; + for (int j = 0; j < 3; ++j) + { + dtSwapEndian(&node->bmin[j]); + dtSwapEndian(&node->bmax[j]); + } + dtSwapEndian(&node->i); + } + + // Off-mesh Connections. + for (int i = 0; i < header->offMeshConCount; ++i) + { + dtOffMeshConnection* con = &offMeshCons[i]; + for (int j = 0; j < 6; ++j) + dtSwapEndian(&con->pos[j]); + dtSwapEndian(&con->rad); + dtSwapEndian(&con->poly); + } + + return true; +} diff --git a/extern/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp b/extern/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp new file mode 100644 index 000000000..90999f2f6 --- /dev/null +++ b/extern/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp @@ -0,0 +1,3664 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include +#include "DetourNavMeshQuery.h" +#include "DetourNavMesh.h" +#include "DetourNode.h" +#include "DetourCommon.h" +#include "DetourMath.h" +#include "DetourAlloc.h" +#include "DetourAssert.h" +#include + +/// @class dtQueryFilter +/// +/// The Default Implementation +/// +/// At construction: All area costs default to 1.0. All flags are included +/// and none are excluded. +/// +/// If a polygon has both an include and an exclude flag, it will be excluded. +/// +/// The way filtering works, a navigation mesh polygon must have at least one flag +/// set to ever be considered by a query. So a polygon with no flags will never +/// be considered. +/// +/// Setting the include flags to 0 will result in all polygons being excluded. +/// +/// Custom Implementations +/// +/// DT_VIRTUAL_QUERYFILTER must be defined in order to extend this class. +/// +/// Implement a custom query filter by overriding the virtual passFilter() +/// and getCost() functions. If this is done, both functions should be as +/// fast as possible. Use cached local copies of data rather than accessing +/// your own objects where possible. +/// +/// Custom implementations do not need to adhere to the flags or cost logic +/// used by the default implementation. +/// +/// In order for A* searches to work properly, the cost should be proportional to +/// the travel distance. Implementing a cost modifier less than 1.0 is likely +/// to lead to problems during pathfinding. +/// +/// @see dtNavMeshQuery + +dtQueryFilter::dtQueryFilter() : + m_includeFlags(0xffff), + m_excludeFlags(0) +{ + for (int i = 0; i < DT_MAX_AREAS; ++i) + m_areaCost[i] = 1.0f; +} + +#ifdef DT_VIRTUAL_QUERYFILTER +bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, + const dtMeshTile* /*tile*/, + const dtPoly* poly) const +{ + return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; +} + +float dtQueryFilter::getCost(const float* pa, const float* pb, + const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, + const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, + const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const +{ + return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; +} +#else +inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, + const dtMeshTile* /*tile*/, + const dtPoly* poly) const +{ + return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; +} + +inline float dtQueryFilter::getCost(const float* pa, const float* pb, + const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, + const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, + const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const +{ + return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; +} +#endif + +static const float H_SCALE = 0.999f; // Search heuristic scale. + + +dtNavMeshQuery* dtAllocNavMeshQuery() +{ + void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM); + if (!mem) return 0; + return new(mem) dtNavMeshQuery; +} + +void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh) +{ + if (!navmesh) return; + navmesh->~dtNavMeshQuery(); + dtFree(navmesh); +} + +////////////////////////////////////////////////////////////////////////////////////////// + +/// @class dtNavMeshQuery +/// +/// For methods that support undersized buffers, if the buffer is too small +/// to hold the entire result set the return status of the method will include +/// the #DT_BUFFER_TOO_SMALL flag. +/// +/// Constant member functions can be used by multiple clients without side +/// effects. (E.g. No change to the closed list. No impact on an in-progress +/// sliced path query. Etc.) +/// +/// Walls and portals: A @e wall is a polygon segment that is +/// considered impassable. A @e portal is a passable segment between polygons. +/// A portal may be treated as a wall based on the dtQueryFilter used for a query. +/// +/// @see dtNavMesh, dtQueryFilter, #dtAllocNavMeshQuery(), #dtAllocNavMeshQuery() + +dtNavMeshQuery::dtNavMeshQuery() : + m_nav(0), + m_tinyNodePool(0), + m_nodePool(0), + m_openList(0) +{ + memset(&m_query, 0, sizeof(dtQueryData)); +} + +dtNavMeshQuery::~dtNavMeshQuery() +{ + if (m_tinyNodePool) + m_tinyNodePool->~dtNodePool(); + if (m_nodePool) + m_nodePool->~dtNodePool(); + if (m_openList) + m_openList->~dtNodeQueue(); + dtFree(m_tinyNodePool); + dtFree(m_nodePool); + dtFree(m_openList); +} + +/// @par +/// +/// Must be the first function called after construction, before other +/// functions are used. +/// +/// This function can be used multiple times. +dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) +{ + if (maxNodes > DT_NULL_IDX || maxNodes > (1 << DT_NODE_PARENT_BITS) - 1) + return DT_FAILURE | DT_INVALID_PARAM; + + m_nav = nav; + + if (!m_nodePool || m_nodePool->getMaxNodes() < maxNodes) + { + if (m_nodePool) + { + m_nodePool->~dtNodePool(); + dtFree(m_nodePool); + m_nodePool = 0; + } + m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4)); + if (!m_nodePool) + return DT_FAILURE | DT_OUT_OF_MEMORY; + } + else + { + m_nodePool->clear(); + } + + if (!m_tinyNodePool) + { + m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32); + if (!m_tinyNodePool) + return DT_FAILURE | DT_OUT_OF_MEMORY; + } + else + { + m_tinyNodePool->clear(); + } + + if (!m_openList || m_openList->getCapacity() < maxNodes) + { + if (m_openList) + { + m_openList->~dtNodeQueue(); + dtFree(m_openList); + m_openList = 0; + } + m_openList = new (dtAlloc(sizeof(dtNodeQueue), DT_ALLOC_PERM)) dtNodeQueue(maxNodes); + if (!m_openList) + return DT_FAILURE | DT_OUT_OF_MEMORY; + } + else + { + m_openList->clear(); + } + + return DT_SUCCESS; +} + +dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const +{ + dtAssert(m_nav); + + // Randomly pick one tile. Assume that all tiles cover roughly the same area. + const dtMeshTile* tile = 0; + float tsum = 0.0f; + for (int i = 0; i < m_nav->getMaxTiles(); i++) + { + const dtMeshTile* t = m_nav->getTile(i); + if (!t || !t->header) continue; + + // Choose random tile using reservoi sampling. + const float area = 1.0f; // Could be tile area too. + tsum += area; + const float u = frand(); + if (u*tsum <= area) + tile = t; + } + if (!tile) + return DT_FAILURE; + + // Randomly pick one polygon weighted by polygon area. + const dtPoly* poly = 0; + dtPolyRef polyRef = 0; + const dtPolyRef base = m_nav->getPolyRefBase(tile); + + float areaSum = 0.0f; + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() != DT_POLYTYPE_GROUND) + continue; + // Must pass filter + const dtPolyRef ref = base | (dtPolyRef)i; + if (!filter->passFilter(ref, tile, p)) + continue; + + // Calc area of the polygon. + float polyArea = 0.0f; + for (int j = 2; j < p->vertCount; ++j) + { + const float* va = &tile->verts[p->verts[0]*3]; + const float* vb = &tile->verts[p->verts[j-1]*3]; + const float* vc = &tile->verts[p->verts[j]*3]; + polyArea += dtTriArea2D(va,vb,vc); + } + + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + poly = p; + polyRef = ref; + } + } + + if (!poly) + return DT_FAILURE; + + // Randomly pick point on polygon. + const float* v = &tile->verts[poly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < poly->vertCount; ++j) + { + v = &tile->verts[poly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt); + + float h = 0.0f; + dtStatus status = getPolyHeight(polyRef, pt, &h); + if (dtStatusFailed(status)) + return status; + pt[1] = h; + + dtVcopy(randomPt, pt); + *randomRef = polyRef; + + return DT_SUCCESS; +} + +dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const +{ + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + // Validate input + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + const dtMeshTile* startTile = 0; + const dtPoly* startPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly); + if (!filter->passFilter(startRef, startTile, startPoly)) + return DT_FAILURE | DT_INVALID_PARAM; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(startRef); + dtVcopy(startNode->pos, centerPos); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = startRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + dtStatus status = DT_SUCCESS; + + const float radiusSqr = dtSqr(maxRadius); + float areaSum = 0.0f; + + const dtMeshTile* randomTile = 0; + const dtPoly* randomPoly = 0; + dtPolyRef randomPolyRef = 0; + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + const dtMeshTile* bestTile = 0; + const dtPoly* bestPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); + + // Place random locations on on ground. + if (bestPoly->getType() == DT_POLYTYPE_GROUND) + { + // Calc area of the polygon. + float polyArea = 0.0f; + for (int j = 2; j < bestPoly->vertCount; ++j) + { + const float* va = &bestTile->verts[bestPoly->verts[0]*3]; + const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3]; + const float* vc = &bestTile->verts[bestPoly->verts[j]*3]; + polyArea += dtTriArea2D(va,vb,vc); + } + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + randomTile = bestTile; + randomPoly = bestPoly; + randomPolyRef = bestRef; + } + } + + + // Get parent poly and tile. + dtPolyRef parentRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + if (bestNode->pidx) + parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + if (parentRef) + m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + const dtLink* link = &bestTile->links[i]; + dtPolyRef neighbourRef = link->ref; + // Skip invalid neighbours and do not follow back to parent. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Expand to neighbour + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); + + // Do not advance if the polygon is excluded by the filter. + if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) + continue; + + // Find edge and calc distance to the edge. + float va[3], vb[3]; + if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) + continue; + + // If the circle is not touching the next polygon, skip it. + float tseg; + float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); + if (distSqr > radiusSqr) + continue; + + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; + continue; + } + + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Cost + if (neighbourNode->flags == 0) + dtVlerp(neighbourNode->pos, va, vb, 0.5f); + + const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); + + // The node is already in open list and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) + continue; + + neighbourNode->id = neighbourRef; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); + neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->total = total; + + if (neighbourNode->flags & DT_NODE_OPEN) + { + m_openList->modify(neighbourNode); + } + else + { + neighbourNode->flags = DT_NODE_OPEN; + m_openList->push(neighbourNode); + } + } + } + + if (!randomPoly) + return DT_FAILURE; + + // Randomly pick point on polygon. + const float* v = &randomTile->verts[randomPoly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < randomPoly->vertCount; ++j) + { + v = &randomTile->verts[randomPoly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt); + + float h = 0.0f; + dtStatus stat = getPolyHeight(randomPolyRef, pt, &h); + if (dtStatusFailed(status)) + return stat; + pt[1] = h; + + dtVcopy(randomPt, pt); + *randomRef = randomPolyRef; + + return DT_SUCCESS; +} + + +////////////////////////////////////////////////////////////////////////////////////////// + +/// @par +/// +/// Uses the detail polygons to find the surface height. (Most accurate.) +/// +/// @p pos does not have to be within the bounds of the polygon or navigation mesh. +/// +/// See closestPointOnPolyBoundary() for a limited but faster option. +/// +dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const +{ + dtAssert(m_nav); + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; + if (!tile) + return DT_FAILURE | DT_INVALID_PARAM; + + // Off-mesh connections don't have detail polygons. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const float* v0 = &tile->verts[poly->verts[0]*3]; + const float* v1 = &tile->verts[poly->verts[1]*3]; + const float d0 = dtVdist(pos, v0); + const float d1 = dtVdist(pos, v1); + const float u = d0 / (d0+d1); + dtVlerp(closest, v0, v1, u); + if (posOverPoly) + *posOverPoly = false; + return DT_SUCCESS; + } + + const unsigned int ip = (unsigned int)(poly - tile->polys); + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + + // Clamp point to be inside the polygon. + float verts[DT_VERTS_PER_POLYGON*3]; + float edged[DT_VERTS_PER_POLYGON]; + float edget[DT_VERTS_PER_POLYGON]; + const int nv = poly->vertCount; + for (int i = 0; i < nv; ++i) + dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); + + dtVcopy(closest, pos); + if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) + { + // Point is outside the polygon, dtClamp to nearest edge. + float dmin = edged[0]; + int imin = 0; + for (int i = 1; i < nv; ++i) + { + if (edged[i] < dmin) + { + dmin = edged[i]; + imin = i; + } + } + const float* va = &verts[imin*3]; + const float* vb = &verts[((imin+1)%nv)*3]; + dtVlerp(closest, va, vb, edget[imin]); + + if (posOverPoly) + *posOverPoly = false; + } + else + { + if (posOverPoly) + *posOverPoly = true; + } + + // Find height at the location. + for (int j = 0; j < pd->triCount; ++j) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; + const float* v[3]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < poly->vertCount) + v[k] = &tile->verts[poly->verts[t[k]]*3]; + else + v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; + } + float h; + if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h)) + { + closest[1] = h; + break; + } + } + + return DT_SUCCESS; +} + +/// @par +/// +/// Much faster than closestPointOnPoly(). +/// +/// If the provided position lies within the polygon's xz-bounds (above or below), +/// then @p pos and @p closest will be equal. +/// +/// The height of @p closest will be the polygon boundary. The height detail is not used. +/// +/// @p pos does not have to be within the bounds of the polybon or the navigation mesh. +/// +dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const +{ + dtAssert(m_nav); + + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; + + // Collect vertices. + float verts[DT_VERTS_PER_POLYGON*3]; + float edged[DT_VERTS_PER_POLYGON]; + float edget[DT_VERTS_PER_POLYGON]; + int nv = 0; + for (int i = 0; i < (int)poly->vertCount; ++i) + { + dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); + nv++; + } + + bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget); + if (inside) + { + // Point is inside the polygon, return the point. + dtVcopy(closest, pos); + } + else + { + // Point is outside the polygon, dtClamp to nearest edge. + float dmin = edged[0]; + int imin = 0; + for (int i = 1; i < nv; ++i) + { + if (edged[i] < dmin) + { + dmin = edged[i]; + imin = i; + } + } + const float* va = &verts[imin*3]; + const float* vb = &verts[((imin+1)%nv)*3]; + dtVlerp(closest, va, vb, edget[imin]); + } + + return DT_SUCCESS; +} + +/// @par +/// +/// Will return #DT_FAILURE if the provided position is outside the xz-bounds +/// of the polygon. +/// +dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const +{ + dtAssert(m_nav); + + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; + + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const float* v0 = &tile->verts[poly->verts[0]*3]; + const float* v1 = &tile->verts[poly->verts[1]*3]; + const float d0 = dtVdist2D(pos, v0); + const float d1 = dtVdist2D(pos, v1); + const float u = d0 / (d0+d1); + if (height) + *height = v0[1] + (v1[1] - v0[1]) * u; + return DT_SUCCESS; + } + else + { + const unsigned int ip = (unsigned int)(poly - tile->polys); + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + for (int j = 0; j < pd->triCount; ++j) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; + const float* v[3]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < poly->vertCount) + v[k] = &tile->verts[poly->verts[t[k]]*3]; + else + v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; + } + float h; + if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) + { + if (height) + *height = h; + return DT_SUCCESS; + } + } + } + + return DT_FAILURE | DT_INVALID_PARAM; +} + +class dtFindNearestPolyQuery : public dtPolyQuery +{ + const dtNavMeshQuery* m_query; + const float* m_center; + float m_nearestDistanceSqr; + dtPolyRef m_nearestRef; + float m_nearestPoint[3]; + +public: + dtFindNearestPolyQuery(const dtNavMeshQuery* query, const float* center) + : m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint() + { + } + + dtPolyRef nearestRef() const { return m_nearestRef; } + const float* nearestPoint() const { return m_nearestPoint; } + + void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) + { + dtIgnoreUnused(polys); + + for (int i = 0; i < count; ++i) + { + dtPolyRef ref = refs[i]; + float closestPtPoly[3]; + float diff[3]; + bool posOverPoly = false; + float d; + m_query->closestPointOnPoly(ref, m_center, closestPtPoly, &posOverPoly); + + // If a point is directly over a polygon and closer than + // climb height, favor that instead of straight line nearest point. + dtVsub(diff, m_center, closestPtPoly); + if (posOverPoly) + { + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); + } + + if (d < m_nearestDistanceSqr) + { + dtVcopy(m_nearestPoint, closestPtPoly); + + m_nearestDistanceSqr = d; + m_nearestRef = ref; + } + } + } +}; + +/// @par +/// +/// @note If the search box does not intersect any polygons the search will +/// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check +/// @p nearestRef before using @p nearestPt. +/// +dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt) const +{ + dtAssert(m_nav); + + if (!nearestRef) + return DT_FAILURE | DT_INVALID_PARAM; + + dtFindNearestPolyQuery query(this, center); + + dtStatus status = queryPolygons(center, halfExtents, filter, &query); + if (dtStatusFailed(status)) + return status; + + *nearestRef = query.nearestRef(); + // Only override nearestPt if we actually found a poly so the nearest point + // is valid. + if (nearestPt && *nearestRef) + dtVcopy(nearestPt, query.nearestPoint()); + + return DT_SUCCESS; +} + +void dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, + const dtQueryFilter* filter, dtPolyQuery* query) const +{ + dtAssert(m_nav); + static const int batchSize = 32; + dtPolyRef polyRefs[batchSize]; + dtPoly* polys[batchSize]; + int n = 0; + + if (tile->bvTree) + { + const dtBVNode* node = &tile->bvTree[0]; + const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount]; + const float* tbmin = tile->header->bmin; + const float* tbmax = tile->header->bmax; + const float qfac = tile->header->bvQuantFactor; + + // Calculate quantized box + unsigned short bmin[3], bmax[3]; + // dtClamp query box to world box. + float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; + float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]; + float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]; + float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]; + float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]; + float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]; + // Quantize + bmin[0] = (unsigned short)(qfac * minx) & 0xfffe; + bmin[1] = (unsigned short)(qfac * miny) & 0xfffe; + bmin[2] = (unsigned short)(qfac * minz) & 0xfffe; + bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; + bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; + bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; + + // Traverse tree + const dtPolyRef base = m_nav->getPolyRefBase(tile); + while (node < end) + { + const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax); + const bool isLeafNode = node->i >= 0; + + if (isLeafNode && overlap) + { + dtPolyRef ref = base | (dtPolyRef)node->i; + if (filter->passFilter(ref, tile, &tile->polys[node->i])) + { + polyRefs[n] = ref; + polys[n] = &tile->polys[node->i]; + + if (n == batchSize - 1) + { + query->process(tile, polys, polyRefs, batchSize); + n = 0; + } + else + { + n++; + } + } + } + + if (overlap || isLeafNode) + node++; + else + { + const int escapeIndex = -node->i; + node += escapeIndex; + } + } + } + else + { + float bmin[3], bmax[3]; + const dtPolyRef base = m_nav->getPolyRefBase(tile); + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + // Must pass filter + const dtPolyRef ref = base | (dtPolyRef)i; + if (!filter->passFilter(ref, tile, p)) + continue; + // Calc polygon bounds. + const float* v = &tile->verts[p->verts[0]*3]; + dtVcopy(bmin, v); + dtVcopy(bmax, v); + for (int j = 1; j < p->vertCount; ++j) + { + v = &tile->verts[p->verts[j]*3]; + dtVmin(bmin, v); + dtVmax(bmax, v); + } + if (dtOverlapBounds(qmin, qmax, bmin, bmax)) + { + polyRefs[n] = ref; + polys[n] = p; + + if (n == batchSize - 1) + { + query->process(tile, polys, polyRefs, batchSize); + n = 0; + } + else + { + n++; + } + } + } + } + + // Process the last polygons that didn't make a full batch. + if (n > 0) + query->process(tile, polys, polyRefs, n); +} + +class dtCollectPolysQuery : public dtPolyQuery +{ + dtPolyRef* m_polys; + const int m_maxPolys; + int m_numCollected; + bool m_overflow; + +public: + dtCollectPolysQuery(dtPolyRef* polys, const int maxPolys) + : m_polys(polys), m_maxPolys(maxPolys), m_numCollected(0), m_overflow(false) + { + } + + int numCollected() const { return m_numCollected; } + bool overflowed() const { return m_overflow; } + + void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) + { + dtIgnoreUnused(tile); + dtIgnoreUnused(polys); + + int numLeft = m_maxPolys - m_numCollected; + int toCopy = count; + if (toCopy > numLeft) + { + m_overflow = true; + toCopy = numLeft; + } + + memcpy(m_polys + m_numCollected, refs, (size_t)toCopy * sizeof(dtPolyRef)); + m_numCollected += toCopy; + } +}; + +/// @par +/// +/// If no polygons are found, the function will return #DT_SUCCESS with a +/// @p polyCount of zero. +/// +/// If @p polys is too small to hold the entire result set, then the array will +/// be filled to capacity. The method of choosing which polygons from the +/// full set are included in the partial result set is undefined. +/// +dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* polys, int* polyCount, const int maxPolys) const +{ + if (!polys || !polyCount || maxPolys < 0) + return DT_FAILURE | DT_INVALID_PARAM; + + dtCollectPolysQuery collector(polys, maxPolys); + + dtStatus status = queryPolygons(center, halfExtents, filter, &collector); + if (dtStatusFailed(status)) + return status; + + *polyCount = collector.numCollected(); + return collector.overflowed() ? DT_SUCCESS | DT_BUFFER_TOO_SMALL : DT_SUCCESS; +} + +/// @par +/// +/// The query will be invoked with batches of polygons. Polygons passed +/// to the query have bounding boxes that overlap with the center and halfExtents +/// passed to this function. The dtPolyQuery::process function is invoked multiple +/// times until all overlapping polygons have been processed. +/// +dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents, + const dtQueryFilter* filter, dtPolyQuery* query) const +{ + dtAssert(m_nav); + + if (!center || !halfExtents || !filter || !query) + return DT_FAILURE | DT_INVALID_PARAM; + + float bmin[3], bmax[3]; + dtVsub(bmin, center, halfExtents); + dtVadd(bmax, center, halfExtents); + + // Find tiles the query touches. + int minx, miny, maxx, maxy; + m_nav->calcTileLoc(bmin, &minx, &miny); + m_nav->calcTileLoc(bmax, &maxx, &maxy); + + static const int MAX_NEIS = 32; + const dtMeshTile* neis[MAX_NEIS]; + + for (int y = miny; y <= maxy; ++y) + { + for (int x = minx; x <= maxx; ++x) + { + const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS); + for (int j = 0; j < nneis; ++j) + { + queryPolygonsInTile(neis[j], bmin, bmax, filter, query); + } + } + } + + return DT_SUCCESS; +} + +/// @par +/// +/// If the end polygon cannot be reached through the navigation graph, +/// the last polygon in the path will be the nearest the end polygon. +/// +/// If the path array is to small to hold the full result, it will be filled as +/// far as possible from the start polygon toward the end polygon. +/// +/// The start and end positions are used to calculate traversal costs. +/// (The y-values impact the result.) +/// +dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter, + dtPolyRef* path, int* pathCount, const int maxPath) const +{ + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + if (pathCount) + *pathCount = 0; + + // Validate input + if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || + !startPos || !endPos || !filter || maxPath <= 0 || !path || !pathCount) + return DT_FAILURE | DT_INVALID_PARAM; + + if (startRef == endRef) + { + path[0] = startRef; + *pathCount = 1; + return DT_SUCCESS; + } + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(startRef); + dtVcopy(startNode->pos, startPos); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = dtVdist(startPos, endPos) * H_SCALE; + startNode->id = startRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + dtNode* lastBestNode = startNode; + float lastBestNodeCost = startNode->total; + + bool outOfNodes = false; + + while (!m_openList->empty()) + { + // Remove node from open list and put it in closed list. + dtNode* bestNode = m_openList->pop(); + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Reached the goal, stop searching. + if (bestNode->id == endRef) + { + lastBestNode = bestNode; + break; + } + + // Get current poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + const dtMeshTile* bestTile = 0; + const dtPoly* bestPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); + + // Get parent poly and tile. + dtPolyRef parentRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + if (bestNode->pidx) + parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + if (parentRef) + m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + dtPolyRef neighbourRef = bestTile->links[i].ref; + + // Skip invalid ids and do not expand back to where we came from. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Get neighbour poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); + + if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) + continue; + + // deal explicitly with crossing tile boundaries + unsigned char crossSide = 0; + if (bestTile->links[i].side != 0xff) + crossSide = bestTile->links[i].side >> 1; + + // get the node + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide); + if (!neighbourNode) + { + outOfNodes = true; + continue; + } + + // If the node is visited the first time, calculate node position. + if (neighbourNode->flags == 0) + { + getEdgeMidPoint(bestRef, bestPoly, bestTile, + neighbourRef, neighbourPoly, neighbourTile, + neighbourNode->pos); + } + + // Calculate cost and heuristic. + float cost = 0; + float heuristic = 0; + + // Special case for last node. + if (neighbourRef == endRef) + { + // Cost + const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + const float endCost = filter->getCost(neighbourNode->pos, endPos, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly, + 0, 0, 0); + + cost = bestNode->cost + curCost + endCost; + heuristic = 0; + } + else + { + // Cost + const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + cost = bestNode->cost + curCost; + heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE; + } + + const float total = cost + heuristic; + + // The node is already in open list and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) + continue; + // The node is already visited and process, and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total) + continue; + + // Add or update the node. + neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->id = neighbourRef; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); + neighbourNode->cost = cost; + neighbourNode->total = total; + + if (neighbourNode->flags & DT_NODE_OPEN) + { + // Already in open, update node location. + m_openList->modify(neighbourNode); + } + else + { + // Put the node in open list. + neighbourNode->flags |= DT_NODE_OPEN; + m_openList->push(neighbourNode); + } + + // Update nearest node to target so far. + if (heuristic < lastBestNodeCost) + { + lastBestNodeCost = heuristic; + lastBestNode = neighbourNode; + } + } + } + + dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath); + + if (lastBestNode->id != endRef) + status |= DT_PARTIAL_RESULT; + + if (outOfNodes) + status |= DT_OUT_OF_NODES; + + return status; +} + +dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const +{ + // Find the length of the entire path. + dtNode* curNode = endNode; + int length = 0; + do + { + length++; + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); + } while (curNode); + + // If the path cannot be fully stored then advance to the last node we will be able to store. + curNode = endNode; + int writeCount; + for (writeCount = length; writeCount > maxPath; writeCount--) + { + dtAssert(curNode); + + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); + } + + // Write path + for (int i = writeCount - 1; i >= 0; i--) + { + dtAssert(curNode); + + path[i] = curNode->id; + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); + } + + dtAssert(!curNode); + + *pathCount = dtMin(length, maxPath); + + if (length > maxPath) + return DT_SUCCESS | DT_BUFFER_TOO_SMALL; + + return DT_SUCCESS; +} + + +/// @par +/// +/// @warning Calling any non-slice methods before calling finalizeSlicedFindPath() +/// or finalizeSlicedFindPathPartial() may result in corrupted data! +/// +/// The @p filter pointer is stored and used for the duration of the sliced +/// path query. +/// +dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter, const unsigned int options) +{ + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + // Init path state. + memset(&m_query, 0, sizeof(dtQueryData)); + m_query.status = DT_FAILURE; + m_query.startRef = startRef; + m_query.endRef = endRef; + dtVcopy(m_query.startPos, startPos); + dtVcopy(m_query.endPos, endPos); + m_query.filter = filter; + m_query.options = options; + m_query.raycastLimitSqr = FLT_MAX; + + if (!startRef || !endRef) + return DT_FAILURE | DT_INVALID_PARAM; + + // Validate input + if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + // trade quality with performance? + if (options & DT_FINDPATH_ANY_ANGLE) + { + // limiting to several times the character radius yields nice results. It is not sensitive + // so it is enough to compute it from the first tile. + const dtMeshTile* tile = m_nav->getTileByRef(startRef); + float agentRadius = tile->header->walkableRadius; + m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS); + } + + if (startRef == endRef) + { + m_query.status = DT_SUCCESS; + return DT_SUCCESS; + } + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(startRef); + dtVcopy(startNode->pos, startPos); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = dtVdist(startPos, endPos) * H_SCALE; + startNode->id = startRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + m_query.status = DT_IN_PROGRESS; + m_query.lastBestNode = startNode; + m_query.lastBestNodeCost = startNode->total; + + return m_query.status; +} + +dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) +{ + if (!dtStatusInProgress(m_query.status)) + return m_query.status; + + // Make sure the request is still valid. + if (!m_nav->isValidPolyRef(m_query.startRef) || !m_nav->isValidPolyRef(m_query.endRef)) + { + m_query.status = DT_FAILURE; + return DT_FAILURE; + } + + dtRaycastHit rayHit; + rayHit.maxPath = 0; + + int iter = 0; + while (iter < maxIter && !m_openList->empty()) + { + iter++; + + // Remove node from open list and put it in closed list. + dtNode* bestNode = m_openList->pop(); + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Reached the goal, stop searching. + if (bestNode->id == m_query.endRef) + { + m_query.lastBestNode = bestNode; + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + m_query.status = DT_SUCCESS | details; + if (doneIters) + *doneIters = iter; + return m_query.status; + } + + // Get current poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + const dtMeshTile* bestTile = 0; + const dtPoly* bestPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly))) + { + // The polygon has disappeared during the sliced query, fail. + m_query.status = DT_FAILURE; + if (doneIters) + *doneIters = iter; + return m_query.status; + } + + // Get parent and grand parent poly and tile. + dtPolyRef parentRef = 0, grandpaRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + dtNode* parentNode = 0; + if (bestNode->pidx) + { + parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx); + parentRef = parentNode->id; + if (parentNode->pidx) + grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id; + } + if (parentRef) + { + bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)); + if (invalidParent || (grandpaRef && !m_nav->isValidPolyRef(grandpaRef)) ) + { + // The polygon has disappeared during the sliced query, fail. + m_query.status = DT_FAILURE; + if (doneIters) + *doneIters = iter; + return m_query.status; + } + } + + // decide whether to test raycast to previous nodes + bool tryLOS = false; + if (m_query.options & DT_FINDPATH_ANY_ANGLE) + { + if ((parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < m_query.raycastLimitSqr)) + tryLOS = true; + } + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + dtPolyRef neighbourRef = bestTile->links[i].ref; + + // Skip invalid ids and do not expand back to where we came from. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Get neighbour poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); + + if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) + continue; + + // get the neighbor node + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, 0); + if (!neighbourNode) + { + m_query.status |= DT_OUT_OF_NODES; + continue; + } + + // do not expand to nodes that were already visited from the same parent + if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx) + continue; + + // If the node is visited the first time, calculate node position. + if (neighbourNode->flags == 0) + { + getEdgeMidPoint(bestRef, bestPoly, bestTile, + neighbourRef, neighbourPoly, neighbourTile, + neighbourNode->pos); + } + + // Calculate cost and heuristic. + float cost = 0; + float heuristic = 0; + + // raycast parent + bool foundShortCut = false; + rayHit.pathCost = rayHit.t = 0; + if (tryLOS) + { + raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef); + foundShortCut = rayHit.t >= 1.0f; + } + + // update move cost + if (foundShortCut) + { + // shortcut found using raycast. Using shorter cost instead + cost = parentNode->cost + rayHit.pathCost; + } + else + { + // No shortcut found. + const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + cost = bestNode->cost + curCost; + } + + // Special case for last node. + if (neighbourRef == m_query.endRef) + { + const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly, + 0, 0, 0); + + cost = cost + endCost; + heuristic = 0; + } + else + { + heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE; + } + + const float total = cost + heuristic; + + // The node is already in open list and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) + continue; + // The node is already visited and process, and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total) + continue; + + // Add or update the node. + neighbourNode->pidx = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode); + neighbourNode->id = neighbourRef; + neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED)); + neighbourNode->cost = cost; + neighbourNode->total = total; + if (foundShortCut) + neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); + + if (neighbourNode->flags & DT_NODE_OPEN) + { + // Already in open, update node location. + m_openList->modify(neighbourNode); + } + else + { + // Put the node in open list. + neighbourNode->flags |= DT_NODE_OPEN; + m_openList->push(neighbourNode); + } + + // Update nearest node to target so far. + if (heuristic < m_query.lastBestNodeCost) + { + m_query.lastBestNodeCost = heuristic; + m_query.lastBestNode = neighbourNode; + } + } + } + + // Exhausted all nodes, but could not find path. + if (m_openList->empty()) + { + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + m_query.status = DT_SUCCESS | details; + } + + if (doneIters) + *doneIters = iter; + + return m_query.status; +} + +dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath) +{ + *pathCount = 0; + + if (dtStatusFailed(m_query.status)) + { + // Reset query. + memset(&m_query, 0, sizeof(dtQueryData)); + return DT_FAILURE; + } + + int n = 0; + + if (m_query.startRef == m_query.endRef) + { + // Special case: the search starts and ends at same poly. + path[n++] = m_query.startRef; + } + else + { + // Reverse the path. + dtAssert(m_query.lastBestNode); + + if (m_query.lastBestNode->id != m_query.endRef) + m_query.status |= DT_PARTIAL_RESULT; + + dtNode* prev = 0; + dtNode* node = m_query.lastBestNode; + int prevRay = 0; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + node->pidx = m_nodePool->getNodeIdx(prev); + prev = node; + int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut) + node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node + prevRay = nextRay; + node = next; + } + while (node); + + // Store path + node = prev; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + dtStatus status = 0; + if (node->flags & DT_NODE_PARENT_DETACHED) + { + float t, normal[3]; + int m; + status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n); + n += m; + // raycast ends on poly boundary and the path might include the next poly boundary. + if (path[n-1] == next->id) + n--; // remove to avoid duplicates + } + else + { + path[n++] = node->id; + if (n >= maxPath) + status = DT_BUFFER_TOO_SMALL; + } + + if (status & DT_STATUS_DETAIL_MASK) + { + m_query.status |= status & DT_STATUS_DETAIL_MASK; + break; + } + node = next; + } + while (node); + } + + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + + // Reset query. + memset(&m_query, 0, sizeof(dtQueryData)); + + *pathCount = n; + + return DT_SUCCESS | details; +} + +dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, + dtPolyRef* path, int* pathCount, const int maxPath) +{ + *pathCount = 0; + + if (existingSize == 0) + { + return DT_FAILURE; + } + + if (dtStatusFailed(m_query.status)) + { + // Reset query. + memset(&m_query, 0, sizeof(dtQueryData)); + return DT_FAILURE; + } + + int n = 0; + + if (m_query.startRef == m_query.endRef) + { + // Special case: the search starts and ends at same poly. + path[n++] = m_query.startRef; + } + else + { + // Find furthest existing node that was visited. + dtNode* prev = 0; + dtNode* node = 0; + for (int i = existingSize-1; i >= 0; --i) + { + m_nodePool->findNodes(existing[i], &node, 1); + if (node) + break; + } + + if (!node) + { + m_query.status |= DT_PARTIAL_RESULT; + dtAssert(m_query.lastBestNode); + node = m_query.lastBestNode; + } + + // Reverse the path. + int prevRay = 0; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + node->pidx = m_nodePool->getNodeIdx(prev); + prev = node; + int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut) + node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node + prevRay = nextRay; + node = next; + } + while (node); + + // Store path + node = prev; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + dtStatus status = 0; + if (node->flags & DT_NODE_PARENT_DETACHED) + { + float t, normal[3]; + int m; + status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n); + n += m; + // raycast ends on poly boundary and the path might include the next poly boundary. + if (path[n-1] == next->id) + n--; // remove to avoid duplicates + } + else + { + path[n++] = node->id; + if (n >= maxPath) + status = DT_BUFFER_TOO_SMALL; + } + + if (status & DT_STATUS_DETAIL_MASK) + { + m_query.status |= status & DT_STATUS_DETAIL_MASK; + break; + } + node = next; + } + while (node); + } + + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + + // Reset query. + memset(&m_query, 0, sizeof(dtQueryData)); + + *pathCount = n; + + return DT_SUCCESS | details; +} + + +dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath) const +{ + if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos)) + { + // The vertices are equal, update flags and poly. + if (straightPathFlags) + straightPathFlags[(*straightPathCount)-1] = flags; + if (straightPathRefs) + straightPathRefs[(*straightPathCount)-1] = ref; + } + else + { + // Append new vertex. + dtVcopy(&straightPath[(*straightPathCount)*3], pos); + if (straightPathFlags) + straightPathFlags[(*straightPathCount)] = flags; + if (straightPathRefs) + straightPathRefs[(*straightPathCount)] = ref; + (*straightPathCount)++; + + // If there is no space to append more vertices, return. + if ((*straightPathCount) >= maxStraightPath) + { + return DT_SUCCESS | DT_BUFFER_TOO_SMALL; + } + + // If reached end of path, return. + if (flags == DT_STRAIGHTPATH_END) + { + return DT_SUCCESS; + } + } + return DT_IN_PROGRESS; +} + +dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options) const +{ + const float* startPos = &straightPath[(*straightPathCount-1)*3]; + // Append or update last vertex + dtStatus stat = 0; + for (int i = startIdx; i < endIdx; i++) + { + // Calculate portal + const dtPolyRef from = path[i]; + const dtMeshTile* fromTile = 0; + const dtPoly* fromPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + + const dtPolyRef to = path[i+1]; + const dtMeshTile* toTile = 0; + const dtPoly* toPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + + float left[3], right[3]; + if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right))) + break; + + if (options & DT_STRAIGHTPATH_AREA_CROSSINGS) + { + // Skip intersection if only area crossings are requested. + if (fromPoly->getArea() == toPoly->getArea()) + continue; + } + + // Append intersection + float s,t; + if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t)) + { + float pt[3]; + dtVlerp(pt, left,right, t); + + stat = appendVertex(pt, 0, path[i+1], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; + } + } + return DT_IN_PROGRESS; +} + +/// @par +/// +/// This method peforms what is often called 'string pulling'. +/// +/// The start position is clamped to the first polygon in the path, and the +/// end position is clamped to the last. So the start and end positions should +/// normally be within or very near the first and last polygons respectively. +/// +/// The returned polygon references represent the reference id of the polygon +/// that is entered at the associated path position. The reference id associated +/// with the end point will always be zero. This allows, for example, matching +/// off-mesh link points to their representative polygons. +/// +/// If the provided result buffers are too small for the entire result set, +/// they will be filled as far as possible from the start toward the end +/// position. +/// +dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos, + const dtPolyRef* path, const int pathSize, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options) const +{ + dtAssert(m_nav); + + *straightPathCount = 0; + + if (!maxStraightPath) + return DT_FAILURE | DT_INVALID_PARAM; + + if (!path[0]) + return DT_FAILURE | DT_INVALID_PARAM; + + dtStatus stat = 0; + + // TODO: Should this be callers responsibility? + float closestStartPos[3]; + if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos))) + return DT_FAILURE | DT_INVALID_PARAM; + + float closestEndPos[3]; + if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos))) + return DT_FAILURE | DT_INVALID_PARAM; + + // Add start point. + stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; + + if (pathSize > 1) + { + float portalApex[3], portalLeft[3], portalRight[3]; + dtVcopy(portalApex, closestStartPos); + dtVcopy(portalLeft, portalApex); + dtVcopy(portalRight, portalApex); + int apexIndex = 0; + int leftIndex = 0; + int rightIndex = 0; + + unsigned char leftPolyType = 0; + unsigned char rightPolyType = 0; + + dtPolyRef leftPolyRef = path[0]; + dtPolyRef rightPolyRef = path[0]; + + for (int i = 0; i < pathSize; ++i) + { + float left[3], right[3]; + unsigned char toType; + + if (i+1 < pathSize) + { + unsigned char fromType; // fromType is ignored. + + // Next portal. + if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType))) + { + // Failed to get portal points, in practice this means that path[i+1] is invalid polygon. + // Clamp the end point to path[i], and return the path so far. + + if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos))) + { + // This should only happen when the first polygon is invalid. + return DT_FAILURE | DT_INVALID_PARAM; + } + + // Apeend portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + // Ignore status return value as we're just about to return anyway. + appendPortals(apexIndex, i, closestEndPos, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + } + + // Ignore status return value as we're just about to return anyway. + appendVertex(closestEndPos, 0, path[i], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + + return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); + } + + // If starting really close the portal, advance. + if (i == 0) + { + float t; + if (dtDistancePtSegSqr2D(portalApex, left, right, t) < dtSqr(0.001f)) + continue; + } + } + else + { + // End of the path. + dtVcopy(left, closestEndPos); + dtVcopy(right, closestEndPos); + + toType = DT_POLYTYPE_GROUND; + } + + // Right vertex. + if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f) + { + if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f) + { + dtVcopy(portalRight, right); + rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0; + rightPolyType = toType; + rightIndex = i; + } + else + { + // Append portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, leftIndex, portalLeft, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + if (stat != DT_IN_PROGRESS) + return stat; + } + + dtVcopy(portalApex, portalLeft); + apexIndex = leftIndex; + + unsigned char flags = 0; + if (!leftPolyRef) + flags = DT_STRAIGHTPATH_END; + else if (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) + flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; + dtPolyRef ref = leftPolyRef; + + // Append or update vertex + stat = appendVertex(portalApex, flags, ref, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; + + dtVcopy(portalLeft, portalApex); + dtVcopy(portalRight, portalApex); + leftIndex = apexIndex; + rightIndex = apexIndex; + + // Restart + i = apexIndex; + + continue; + } + } + + // Left vertex. + if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f) + { + if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f) + { + dtVcopy(portalLeft, left); + leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0; + leftPolyType = toType; + leftIndex = i; + } + else + { + // Append portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, rightIndex, portalRight, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + if (stat != DT_IN_PROGRESS) + return stat; + } + + dtVcopy(portalApex, portalRight); + apexIndex = rightIndex; + + unsigned char flags = 0; + if (!rightPolyRef) + flags = DT_STRAIGHTPATH_END; + else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) + flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; + dtPolyRef ref = rightPolyRef; + + // Append or update vertex + stat = appendVertex(portalApex, flags, ref, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; + + dtVcopy(portalLeft, portalApex); + dtVcopy(portalRight, portalApex); + leftIndex = apexIndex; + rightIndex = apexIndex; + + // Restart + i = apexIndex; + + continue; + } + } + } + + // Append portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + if (stat != DT_IN_PROGRESS) + return stat; + } + } + + // Ignore status return value as we're just about to return anyway. + appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + + return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); +} + +/// @par +/// +/// This method is optimized for small delta movement and a small number of +/// polygons. If used for too great a distance, the result set will form an +/// incomplete path. +/// +/// @p resultPos will equal the @p endPos if the end is reached. +/// Otherwise the closest reachable position will be returned. +/// +/// @p resultPos is not projected onto the surface of the navigation +/// mesh. Use #getPolyHeight if this is needed. +/// +/// This method treats the end position in the same manner as +/// the #raycast method. (As a 2D point.) See that method's documentation +/// for details. +/// +/// If the @p visited array is too small to hold the entire result set, it will +/// be filled as far as possible from the start position toward the end +/// position. +/// +dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, + float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const +{ + dtAssert(m_nav); + dtAssert(m_tinyNodePool); + + *visitedCount = 0; + + // Validate input + if (!startRef) + return DT_FAILURE | DT_INVALID_PARAM; + if (!m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + dtStatus status = DT_SUCCESS; + + static const int MAX_STACK = 48; + dtNode* stack[MAX_STACK]; + int nstack = 0; + + m_tinyNodePool->clear(); + + dtNode* startNode = m_tinyNodePool->getNode(startRef); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = startRef; + startNode->flags = DT_NODE_CLOSED; + stack[nstack++] = startNode; + + float bestPos[3]; + float bestDist = FLT_MAX; + dtNode* bestNode = 0; + dtVcopy(bestPos, startPos); + + // Search constraints + float searchPos[3], searchRadSqr; + dtVlerp(searchPos, startPos, endPos, 0.5f); + searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f); + + float verts[DT_VERTS_PER_POLYGON*3]; + + while (nstack) + { + // Pop front. + dtNode* curNode = stack[0]; + for (int i = 0; i < nstack-1; ++i) + stack[i] = stack[i+1]; + nstack--; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef curRef = curNode->id; + const dtMeshTile* curTile = 0; + const dtPoly* curPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly); + + // Collect vertices. + const int nverts = curPoly->vertCount; + for (int i = 0; i < nverts; ++i) + dtVcopy(&verts[i*3], &curTile->verts[curPoly->verts[i]*3]); + + // If target is inside the poly, stop search. + if (dtPointInPolygon(endPos, verts, nverts)) + { + bestNode = curNode; + dtVcopy(bestPos, endPos); + break; + } + + // Find wall edges and find nearest point inside the walls. + for (int i = 0, j = (int)curPoly->vertCount-1; i < (int)curPoly->vertCount; j = i++) + { + // Find links to neighbours. + static const int MAX_NEIS = 8; + int nneis = 0; + dtPolyRef neis[MAX_NEIS]; + + if (curPoly->neis[j] & DT_EXT_LINK) + { + // Tile border. + for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next) + { + const dtLink* link = &curTile->links[k]; + if (link->edge == j) + { + if (link->ref != 0) + { + const dtMeshTile* neiTile = 0; + const dtPoly* neiPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); + if (filter->passFilter(link->ref, neiTile, neiPoly)) + { + if (nneis < MAX_NEIS) + neis[nneis++] = link->ref; + } + } + } + } + } + else if (curPoly->neis[j]) + { + const unsigned int idx = (unsigned int)(curPoly->neis[j]-1); + const dtPolyRef ref = m_nav->getPolyRefBase(curTile) | idx; + if (filter->passFilter(ref, curTile, &curTile->polys[idx])) + { + // Internal edge, encode id. + neis[nneis++] = ref; + } + } + + if (!nneis) + { + // Wall edge, calc distance. + const float* vj = &verts[j*3]; + const float* vi = &verts[i*3]; + float tseg; + const float distSqr = dtDistancePtSegSqr2D(endPos, vj, vi, tseg); + if (distSqr < bestDist) + { + // Update nearest distance. + dtVlerp(bestPos, vj,vi, tseg); + bestDist = distSqr; + bestNode = curNode; + } + } + else + { + for (int k = 0; k < nneis; ++k) + { + // Skip if no node can be allocated. + dtNode* neighbourNode = m_tinyNodePool->getNode(neis[k]); + if (!neighbourNode) + continue; + // Skip if already visited. + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Skip the link if it is too far from search constraint. + // TODO: Maybe should use getPortalPoints(), but this one is way faster. + const float* vj = &verts[j*3]; + const float* vi = &verts[i*3]; + float tseg; + float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, tseg); + if (distSqr > searchRadSqr) + continue; + + // Mark as the node as visited and push to queue. + if (nstack < MAX_STACK) + { + neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode); + neighbourNode->flags |= DT_NODE_CLOSED; + stack[nstack++] = neighbourNode; + } + } + } + } + } + + int n = 0; + if (bestNode) + { + // Reverse the path. + dtNode* prev = 0; + dtNode* node = bestNode; + do + { + dtNode* next = m_tinyNodePool->getNodeAtIdx(node->pidx); + node->pidx = m_tinyNodePool->getNodeIdx(prev); + prev = node; + node = next; + } + while (node); + + // Store result + node = prev; + do + { + visited[n++] = node->id; + if (n >= maxVisitedSize) + { + status |= DT_BUFFER_TOO_SMALL; + break; + } + node = m_tinyNodePool->getNodeAtIdx(node->pidx); + } + while (node); + } + + dtVcopy(resultPos, bestPos); + + *visitedCount = n; + + return status; +} + + +dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, + unsigned char& fromType, unsigned char& toType) const +{ + dtAssert(m_nav); + + const dtMeshTile* fromTile = 0; + const dtPoly* fromPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + fromType = fromPoly->getType(); + + const dtMeshTile* toTile = 0; + const dtPoly* toPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + toType = toPoly->getType(); + + return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right); +} + +// Returns portal points between two polygons. +dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, + dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, + float* left, float* right) const +{ + // Find the link that points to the 'to' polygon. + const dtLink* link = 0; + for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next) + { + if (fromTile->links[i].ref == to) + { + link = &fromTile->links[i]; + break; + } + } + if (!link) + return DT_FAILURE | DT_INVALID_PARAM; + + // Handle off-mesh connections. + if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + // Find link that points to first vertex. + for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next) + { + if (fromTile->links[i].ref == to) + { + const int v = fromTile->links[i].edge; + dtVcopy(left, &fromTile->verts[fromPoly->verts[v]*3]); + dtVcopy(right, &fromTile->verts[fromPoly->verts[v]*3]); + return DT_SUCCESS; + } + } + return DT_FAILURE | DT_INVALID_PARAM; + } + + if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next) + { + if (toTile->links[i].ref == from) + { + const int v = toTile->links[i].edge; + dtVcopy(left, &toTile->verts[toPoly->verts[v]*3]); + dtVcopy(right, &toTile->verts[toPoly->verts[v]*3]); + return DT_SUCCESS; + } + } + return DT_FAILURE | DT_INVALID_PARAM; + } + + // Find portal vertices. + const int v0 = fromPoly->verts[link->edge]; + const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount]; + dtVcopy(left, &fromTile->verts[v0*3]); + dtVcopy(right, &fromTile->verts[v1*3]); + + // If the link is at tile boundary, dtClamp the vertices to + // the link width. + if (link->side != 0xff) + { + // Unpack portal limits. + if (link->bmin != 0 || link->bmax != 255) + { + const float s = 1.0f/255.0f; + const float tmin = link->bmin*s; + const float tmax = link->bmax*s; + dtVlerp(left, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmin); + dtVlerp(right, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmax); + } + } + + return DT_SUCCESS; +} + +// Returns edge mid point between two polygons. +dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const +{ + float left[3], right[3]; + unsigned char fromType, toType; + if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType))) + return DT_FAILURE | DT_INVALID_PARAM; + mid[0] = (left[0]+right[0])*0.5f; + mid[1] = (left[1]+right[1])*0.5f; + mid[2] = (left[2]+right[2])*0.5f; + return DT_SUCCESS; +} + +dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, + dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, + float* mid) const +{ + float left[3], right[3]; + if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right))) + return DT_FAILURE | DT_INVALID_PARAM; + mid[0] = (left[0]+right[0])*0.5f; + mid[1] = (left[1]+right[1])*0.5f; + mid[2] = (left[2]+right[2])*0.5f; + return DT_SUCCESS; +} + + + +/// @par +/// +/// This method is meant to be used for quick, short distance checks. +/// +/// If the path array is too small to hold the result, it will be filled as +/// far as possible from the start postion toward the end position. +/// +/// Using the Hit Parameter (t) +/// +/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit +/// the end position. In this case the path represents a valid corridor to the +/// end position and the value of @p hitNormal is undefined. +/// +/// If the hit parameter is zero, then the start position is on the wall that +/// was hit and the value of @p hitNormal is undefined. +/// +/// If 0 < t < 1.0 then the following applies: +/// +/// @code +/// distanceToHitBorder = distanceToEndPosition * t +/// hitPoint = startPos + (endPos - startPos) * t +/// @endcode +/// +/// Use Case Restriction +/// +/// The raycast ignores the y-value of the end position. (2D check.) This +/// places significant limits on how it can be used. For example: +/// +/// Consider a scene where there is a main floor with a second floor balcony +/// that hangs over the main floor. So the first floor mesh extends below the +/// balcony mesh. The start position is somewhere on the first floor. The end +/// position is on the balcony. +/// +/// The raycast will search toward the end position along the first floor mesh. +/// If it reaches the end position's xz-coordinates it will indicate FLT_MAX +/// (no wall hit), meaning it reached the end position. This is one example of why +/// this method is meant for short distance checks. +/// +dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, + float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const +{ + dtRaycastHit hit; + hit.path = path; + hit.maxPath = maxPath; + + dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit); + + *t = hit.t; + if (hitNormal) + dtVcopy(hitNormal, hit.hitNormal); + if (pathCount) + *pathCount = hit.pathCount; + + return status; +} + + +/// @par +/// +/// This method is meant to be used for quick, short distance checks. +/// +/// If the path array is too small to hold the result, it will be filled as +/// far as possible from the start postion toward the end position. +/// +/// Using the Hit Parameter t of RaycastHit +/// +/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit +/// the end position. In this case the path represents a valid corridor to the +/// end position and the value of @p hitNormal is undefined. +/// +/// If the hit parameter is zero, then the start position is on the wall that +/// was hit and the value of @p hitNormal is undefined. +/// +/// If 0 < t < 1.0 then the following applies: +/// +/// @code +/// distanceToHitBorder = distanceToEndPosition * t +/// hitPoint = startPos + (endPos - startPos) * t +/// @endcode +/// +/// Use Case Restriction +/// +/// The raycast ignores the y-value of the end position. (2D check.) This +/// places significant limits on how it can be used. For example: +/// +/// Consider a scene where there is a main floor with a second floor balcony +/// that hangs over the main floor. So the first floor mesh extends below the +/// balcony mesh. The start position is somewhere on the first floor. The end +/// position is on the balcony. +/// +/// The raycast will search toward the end position along the first floor mesh. +/// If it reaches the end position's xz-coordinates it will indicate FLT_MAX +/// (no wall hit), meaning it reached the end position. This is one example of why +/// this method is meant for short distance checks. +/// +dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, const unsigned int options, + dtRaycastHit* hit, dtPolyRef prevRef) const +{ + dtAssert(m_nav); + + hit->t = 0; + hit->pathCount = 0; + hit->pathCost = 0; + + // Validate input + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + if (prevRef && !m_nav->isValidPolyRef(prevRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + float dir[3], curPos[3], lastPos[3]; + float verts[DT_VERTS_PER_POLYGON*3+3]; + int n = 0; + + dtVcopy(curPos, startPos); + dtVsub(dir, endPos, startPos); + dtVset(hit->hitNormal, 0, 0, 0); + + dtStatus status = DT_SUCCESS; + + const dtMeshTile* prevTile, *tile, *nextTile; + const dtPoly* prevPoly, *poly, *nextPoly; + dtPolyRef curRef; + + // The API input has been checked already, skip checking internal data. + curRef = startRef; + tile = 0; + poly = 0; + m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly); + nextTile = prevTile = tile; + nextPoly = prevPoly = poly; + if (prevRef) + m_nav->getTileAndPolyByRefUnsafe(prevRef, &prevTile, &prevPoly); + + while (curRef) + { + // Cast ray against current polygon. + + // Collect vertices. + int nv = 0; + for (int i = 0; i < (int)poly->vertCount; ++i) + { + dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); + nv++; + } + + float tmin, tmax; + int segMin, segMax; + if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax)) + { + // Could not hit the polygon, keep the old t and report hit. + hit->pathCount = n; + return status; + } + + hit->hitEdgeIndex = segMax; + + // Keep track of furthest t so far. + if (tmax > hit->t) + hit->t = tmax; + + // Store visited polygons. + if (n < hit->maxPath) + hit->path[n++] = curRef; + else + status |= DT_BUFFER_TOO_SMALL; + + // Ray end is completely inside the polygon. + if (segMax == -1) + { + hit->t = FLT_MAX; + hit->pathCount = n; + + // add the cost + if (options & DT_RAYCAST_USE_COSTS) + hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly); + return status; + } + + // Follow neighbours. + dtPolyRef nextRef = 0; + + for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) + { + const dtLink* link = &tile->links[i]; + + // Find link which contains this edge. + if ((int)link->edge != segMax) + continue; + + // Get pointer to the next polygon. + nextTile = 0; + nextPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly); + + // Skip off-mesh connections. + if (nextPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + + // Skip links based on filter. + if (!filter->passFilter(link->ref, nextTile, nextPoly)) + continue; + + // If the link is internal, just return the ref. + if (link->side == 0xff) + { + nextRef = link->ref; + break; + } + + // If the link is at tile boundary, + + // Check if the link spans the whole edge, and accept. + if (link->bmin == 0 && link->bmax == 255) + { + nextRef = link->ref; + break; + } + + // Check for partial edge links. + const int v0 = poly->verts[link->edge]; + const int v1 = poly->verts[(link->edge+1) % poly->vertCount]; + const float* left = &tile->verts[v0*3]; + const float* right = &tile->verts[v1*3]; + + // Check that the intersection lies inside the link portal. + if (link->side == 0 || link->side == 4) + { + // Calculate link size. + const float s = 1.0f/255.0f; + float lmin = left[2] + (right[2] - left[2])*(link->bmin*s); + float lmax = left[2] + (right[2] - left[2])*(link->bmax*s); + if (lmin > lmax) dtSwap(lmin, lmax); + + // Find Z intersection. + float z = startPos[2] + (endPos[2]-startPos[2])*tmax; + if (z >= lmin && z <= lmax) + { + nextRef = link->ref; + break; + } + } + else if (link->side == 2 || link->side == 6) + { + // Calculate link size. + const float s = 1.0f/255.0f; + float lmin = left[0] + (right[0] - left[0])*(link->bmin*s); + float lmax = left[0] + (right[0] - left[0])*(link->bmax*s); + if (lmin > lmax) dtSwap(lmin, lmax); + + // Find X intersection. + float x = startPos[0] + (endPos[0]-startPos[0])*tmax; + if (x >= lmin && x <= lmax) + { + nextRef = link->ref; + break; + } + } + } + + // add the cost + if (options & DT_RAYCAST_USE_COSTS) + { + // compute the intersection point at the furthest end of the polygon + // and correct the height (since the raycast moves in 2d) + dtVcopy(lastPos, curPos); + dtVmad(curPos, startPos, dir, hit->t); + float* e1 = &verts[segMax*3]; + float* e2 = &verts[((segMax+1)%nv)*3]; + float eDir[3], diff[3]; + dtVsub(eDir, e2, e1); + dtVsub(diff, curPos, e1); + float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2]; + curPos[1] = e1[1] + eDir[1] * s; + + hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly); + } + + if (!nextRef) + { + // No neighbour, we hit a wall. + + // Calculate hit normal. + const int a = segMax; + const int b = segMax+1 < nv ? segMax+1 : 0; + const float* va = &verts[a*3]; + const float* vb = &verts[b*3]; + const float dx = vb[0] - va[0]; + const float dz = vb[2] - va[2]; + hit->hitNormal[0] = dz; + hit->hitNormal[1] = 0; + hit->hitNormal[2] = -dx; + dtVnormalize(hit->hitNormal); + + hit->pathCount = n; + return status; + } + + // No hit, advance to neighbour polygon. + prevRef = curRef; + curRef = nextRef; + prevTile = tile; + tile = nextTile; + prevPoly = poly; + poly = nextPoly; + } + + hit->pathCount = n; + + return status; +} + +/// @par +/// +/// At least one result array must be provided. +/// +/// The order of the result set is from least to highest cost to reach the polygon. +/// +/// A common use case for this method is to perform Dijkstra searches. +/// Candidate polygons are found by searching the graph beginning at the start polygon. +/// +/// If a polygon is not found via the graph search, even if it intersects the +/// search circle, it will not be included in the result set. For example: +/// +/// polyA is the start polygon. +/// polyB shares an edge with polyA. (Is adjacent.) +/// polyC shares an edge with polyB, but not with polyA +/// Even if the search circle overlaps polyC, it will not be included in the +/// result set unless polyB is also in the set. +/// +/// The value of the center point is used as the start position for cost +/// calculations. It is not projected onto the surface of the mesh, so its +/// y-value will effect the costs. +/// +/// Intersection tests occur in 2D. All polygons and the search circle are +/// projected onto the xz-plane. So the y-value of the center point does not +/// effect intersection tests. +/// +/// If the result arrays are to small to hold the entire result set, they will be +/// filled to capacity. +/// +dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + int* resultCount, const int maxResult) const +{ + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + *resultCount = 0; + + // Validate input + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(startRef); + dtVcopy(startNode->pos, centerPos); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = startRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + dtStatus status = DT_SUCCESS; + + int n = 0; + + const float radiusSqr = dtSqr(radius); + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + const dtMeshTile* bestTile = 0; + const dtPoly* bestPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); + + // Get parent poly and tile. + dtPolyRef parentRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + if (bestNode->pidx) + parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + if (parentRef) + m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + if (n < maxResult) + { + if (resultRef) + resultRef[n] = bestRef; + if (resultParent) + resultParent[n] = parentRef; + if (resultCost) + resultCost[n] = bestNode->total; + ++n; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + const dtLink* link = &bestTile->links[i]; + dtPolyRef neighbourRef = link->ref; + // Skip invalid neighbours and do not follow back to parent. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Expand to neighbour + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); + + // Do not advance if the polygon is excluded by the filter. + if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) + continue; + + // Find edge and calc distance to the edge. + float va[3], vb[3]; + if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) + continue; + + // If the circle is not touching the next polygon, skip it. + float tseg; + float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); + if (distSqr > radiusSqr) + continue; + + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; + continue; + } + + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Cost + if (neighbourNode->flags == 0) + dtVlerp(neighbourNode->pos, va, vb, 0.5f); + + float cost = filter->getCost( + bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + + const float total = bestNode->total + cost; + + // The node is already in open list and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) + continue; + + neighbourNode->id = neighbourRef; + neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->total = total; + + if (neighbourNode->flags & DT_NODE_OPEN) + { + m_openList->modify(neighbourNode); + } + else + { + neighbourNode->flags = DT_NODE_OPEN; + m_openList->push(neighbourNode); + } + } + } + + *resultCount = n; + + return status; +} + +/// @par +/// +/// The order of the result set is from least to highest cost. +/// +/// At least one result array must be provided. +/// +/// A common use case for this method is to perform Dijkstra searches. +/// Candidate polygons are found by searching the graph beginning at the start +/// polygon. +/// +/// The same intersection test restrictions that apply to findPolysAroundCircle() +/// method apply to this method. +/// +/// The 3D centroid of the search polygon is used as the start position for cost +/// calculations. +/// +/// Intersection tests occur in 2D. All polygons are projected onto the +/// xz-plane. So the y-values of the vertices do not effect intersection tests. +/// +/// If the result arrays are is too small to hold the entire result set, they will +/// be filled to capacity. +/// +dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + int* resultCount, const int maxResult) const +{ + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + *resultCount = 0; + + // Validate input + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + m_nodePool->clear(); + m_openList->clear(); + + float centerPos[3] = {0,0,0}; + for (int i = 0; i < nverts; ++i) + dtVadd(centerPos,centerPos,&verts[i*3]); + dtVscale(centerPos,centerPos,1.0f/nverts); + + dtNode* startNode = m_nodePool->getNode(startRef); + dtVcopy(startNode->pos, centerPos); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = startRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + dtStatus status = DT_SUCCESS; + + int n = 0; + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + const dtMeshTile* bestTile = 0; + const dtPoly* bestPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); + + // Get parent poly and tile. + dtPolyRef parentRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + if (bestNode->pidx) + parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + if (parentRef) + m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + if (n < maxResult) + { + if (resultRef) + resultRef[n] = bestRef; + if (resultParent) + resultParent[n] = parentRef; + if (resultCost) + resultCost[n] = bestNode->total; + + ++n; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + const dtLink* link = &bestTile->links[i]; + dtPolyRef neighbourRef = link->ref; + // Skip invalid neighbours and do not follow back to parent. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Expand to neighbour + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); + + // Do not advance if the polygon is excluded by the filter. + if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) + continue; + + // Find edge and calc distance to the edge. + float va[3], vb[3]; + if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) + continue; + + // If the poly is not touching the edge to the next polygon, skip the connection it. + float tmin, tmax; + int segMin, segMax; + if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, tmin, tmax, segMin, segMax)) + continue; + if (tmin > 1.0f || tmax < 0.0f) + continue; + + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; + continue; + } + + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Cost + if (neighbourNode->flags == 0) + dtVlerp(neighbourNode->pos, va, vb, 0.5f); + + float cost = filter->getCost( + bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + + const float total = bestNode->total + cost; + + // The node is already in open list and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) + continue; + + neighbourNode->id = neighbourRef; + neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->total = total; + + if (neighbourNode->flags & DT_NODE_OPEN) + { + m_openList->modify(neighbourNode); + } + else + { + neighbourNode->flags = DT_NODE_OPEN; + m_openList->push(neighbourNode); + } + } + } + + *resultCount = n; + + return status; +} + +dtStatus dtNavMeshQuery::getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const +{ + if (!m_nav->isValidPolyRef(endRef) || !path || !pathCount || maxPath < 0) + return DT_FAILURE | DT_INVALID_PARAM; + + *pathCount = 0; + + dtNode* endNode; + if (m_nodePool->findNodes(endRef, &endNode, 1) != 1 || + (endNode->flags & DT_NODE_CLOSED) == 0) + return DT_FAILURE | DT_INVALID_PARAM; + + return getPathToNode(endNode, path, pathCount, maxPath); +} + +/// @par +/// +/// This method is optimized for a small search radius and small number of result +/// polygons. +/// +/// Candidate polygons are found by searching the navigation graph beginning at +/// the start polygon. +/// +/// The same intersection test restrictions that apply to the findPolysAroundCircle +/// mehtod applies to this method. +/// +/// The value of the center point is used as the start point for cost calculations. +/// It is not projected onto the surface of the mesh, so its y-value will effect +/// the costs. +/// +/// Intersection tests occur in 2D. All polygons and the search circle are +/// projected onto the xz-plane. So the y-value of the center point does not +/// effect intersection tests. +/// +/// If the result arrays are is too small to hold the entire result set, they will +/// be filled to capacity. +/// +dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, + int* resultCount, const int maxResult) const +{ + dtAssert(m_nav); + dtAssert(m_tinyNodePool); + + *resultCount = 0; + + // Validate input + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + static const int MAX_STACK = 48; + dtNode* stack[MAX_STACK]; + int nstack = 0; + + m_tinyNodePool->clear(); + + dtNode* startNode = m_tinyNodePool->getNode(startRef); + startNode->pidx = 0; + startNode->id = startRef; + startNode->flags = DT_NODE_CLOSED; + stack[nstack++] = startNode; + + const float radiusSqr = dtSqr(radius); + + float pa[DT_VERTS_PER_POLYGON*3]; + float pb[DT_VERTS_PER_POLYGON*3]; + + dtStatus status = DT_SUCCESS; + + int n = 0; + if (n < maxResult) + { + resultRef[n] = startNode->id; + if (resultParent) + resultParent[n] = 0; + ++n; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + while (nstack) + { + // Pop front. + dtNode* curNode = stack[0]; + for (int i = 0; i < nstack-1; ++i) + stack[i] = stack[i+1]; + nstack--; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef curRef = curNode->id; + const dtMeshTile* curTile = 0; + const dtPoly* curPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly); + + for (unsigned int i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next) + { + const dtLink* link = &curTile->links[i]; + dtPolyRef neighbourRef = link->ref; + // Skip invalid neighbours. + if (!neighbourRef) + continue; + + // Skip if cannot alloca more nodes. + dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef); + if (!neighbourNode) + continue; + // Skip visited. + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Expand to neighbour + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); + + // Skip off-mesh connections. + if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + + // Do not advance if the polygon is excluded by the filter. + if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) + continue; + + // Find edge and calc distance to the edge. + float va[3], vb[3]; + if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) + continue; + + // If the circle is not touching the next polygon, skip it. + float tseg; + float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); + if (distSqr > radiusSqr) + continue; + + // Mark node visited, this is done before the overlap test so that + // we will not visit the poly again if the test fails. + neighbourNode->flags |= DT_NODE_CLOSED; + neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode); + + // Check that the polygon does not collide with existing polygons. + + // Collect vertices of the neighbour poly. + const int npa = neighbourPoly->vertCount; + for (int k = 0; k < npa; ++k) + dtVcopy(&pa[k*3], &neighbourTile->verts[neighbourPoly->verts[k]*3]); + + bool overlap = false; + for (int j = 0; j < n; ++j) + { + dtPolyRef pastRef = resultRef[j]; + + // Connected polys do not overlap. + bool connected = false; + for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next) + { + if (curTile->links[k].ref == pastRef) + { + connected = true; + break; + } + } + if (connected) + continue; + + // Potentially overlapping. + const dtMeshTile* pastTile = 0; + const dtPoly* pastPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly); + + // Get vertices and test overlap + const int npb = pastPoly->vertCount; + for (int k = 0; k < npb; ++k) + dtVcopy(&pb[k*3], &pastTile->verts[pastPoly->verts[k]*3]); + + if (dtOverlapPolyPoly2D(pa,npa, pb,npb)) + { + overlap = true; + break; + } + } + if (overlap) + continue; + + // This poly is fine, store and advance to the poly. + if (n < maxResult) + { + resultRef[n] = neighbourRef; + if (resultParent) + resultParent[n] = curRef; + ++n; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + if (nstack < MAX_STACK) + { + stack[nstack++] = neighbourNode; + } + } + } + + *resultCount = n; + + return status; +} + + +struct dtSegInterval +{ + dtPolyRef ref; + short tmin, tmax; +}; + +static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, + const short tmin, const short tmax, const dtPolyRef ref) +{ + if (nints+1 > maxInts) return; + // Find insertion point. + int idx = 0; + while (idx < nints) + { + if (tmax <= ints[idx].tmin) + break; + idx++; + } + // Move current results. + if (nints-idx) + memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx)); + // Store + ints[idx].ref = ref; + ints[idx].tmin = tmin; + ints[idx].tmax = tmax; + nints++; +} + +/// @par +/// +/// If the @p segmentRefs parameter is provided, then all polygon segments will be returned. +/// Otherwise only the wall segments are returned. +/// +/// A segment that is normally a portal will be included in the result set as a +/// wall if the @p filter results in the neighbor polygon becoomming impassable. +/// +/// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the +/// maximum segments per polygon of the source navigation mesh. +/// +dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, + float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, + const int maxSegments) const +{ + dtAssert(m_nav); + + *segmentCount = 0; + + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; + + int n = 0; + static const int MAX_INTERVAL = 16; + dtSegInterval ints[MAX_INTERVAL]; + int nints; + + const bool storePortals = segmentRefs != 0; + + dtStatus status = DT_SUCCESS; + + for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++) + { + // Skip non-solid edges. + nints = 0; + if (poly->neis[j] & DT_EXT_LINK) + { + // Tile border. + for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next) + { + const dtLink* link = &tile->links[k]; + if (link->edge == j) + { + if (link->ref != 0) + { + const dtMeshTile* neiTile = 0; + const dtPoly* neiPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); + if (filter->passFilter(link->ref, neiTile, neiPoly)) + { + insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax, link->ref); + } + } + } + } + } + else + { + // Internal edge + dtPolyRef neiRef = 0; + if (poly->neis[j]) + { + const unsigned int idx = (unsigned int)(poly->neis[j]-1); + neiRef = m_nav->getPolyRefBase(tile) | idx; + if (!filter->passFilter(neiRef, tile, &tile->polys[idx])) + neiRef = 0; + } + + // If the edge leads to another polygon and portals are not stored, skip. + if (neiRef != 0 && !storePortals) + continue; + + if (n < maxSegments) + { + const float* vj = &tile->verts[poly->verts[j]*3]; + const float* vi = &tile->verts[poly->verts[i]*3]; + float* seg = &segmentVerts[n*6]; + dtVcopy(seg+0, vj); + dtVcopy(seg+3, vi); + if (segmentRefs) + segmentRefs[n] = neiRef; + n++; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + continue; + } + + // Add sentinels + insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0); + insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0); + + // Store segments. + const float* vj = &tile->verts[poly->verts[j]*3]; + const float* vi = &tile->verts[poly->verts[i]*3]; + for (int k = 1; k < nints; ++k) + { + // Portal segment. + if (storePortals && ints[k].ref) + { + const float tmin = ints[k].tmin/255.0f; + const float tmax = ints[k].tmax/255.0f; + if (n < maxSegments) + { + float* seg = &segmentVerts[n*6]; + dtVlerp(seg+0, vj,vi, tmin); + dtVlerp(seg+3, vj,vi, tmax); + if (segmentRefs) + segmentRefs[n] = ints[k].ref; + n++; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + } + + // Wall segment. + const int imin = ints[k-1].tmax; + const int imax = ints[k].tmin; + if (imin != imax) + { + const float tmin = imin/255.0f; + const float tmax = imax/255.0f; + if (n < maxSegments) + { + float* seg = &segmentVerts[n*6]; + dtVlerp(seg+0, vj,vi, tmin); + dtVlerp(seg+3, vj,vi, tmax); + if (segmentRefs) + segmentRefs[n] = 0; + n++; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + } + } + } + + *segmentCount = n; + + return status; +} + +/// @par +/// +/// @p hitPos is not adjusted using the height detail data. +/// +/// @p hitDist will equal the search radius if there is no wall within the +/// radius. In this case the values of @p hitPos and @p hitNormal are +/// undefined. +/// +/// The normal will become unpredicable if @p hitDist is a very small number. +/// +dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, + float* hitDist, float* hitPos, float* hitNormal) const +{ + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + // Validate input + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(startRef); + dtVcopy(startNode->pos, centerPos); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = startRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + float radiusSqr = dtSqr(maxRadius); + + dtStatus status = DT_SUCCESS; + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + const dtMeshTile* bestTile = 0; + const dtPoly* bestPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); + + // Get parent poly and tile. + dtPolyRef parentRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + if (bestNode->pidx) + parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + if (parentRef) + m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + // Hit test walls. + for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++) + { + // Skip non-solid edges. + if (bestPoly->neis[j] & DT_EXT_LINK) + { + // Tile border. + bool solid = true; + for (unsigned int k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next) + { + const dtLink* link = &bestTile->links[k]; + if (link->edge == j) + { + if (link->ref != 0) + { + const dtMeshTile* neiTile = 0; + const dtPoly* neiPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); + if (filter->passFilter(link->ref, neiTile, neiPoly)) + solid = false; + } + break; + } + } + if (!solid) continue; + } + else if (bestPoly->neis[j]) + { + // Internal edge + const unsigned int idx = (unsigned int)(bestPoly->neis[j]-1); + const dtPolyRef ref = m_nav->getPolyRefBase(bestTile) | idx; + if (filter->passFilter(ref, bestTile, &bestTile->polys[idx])) + continue; + } + + // Calc distance to the edge. + const float* vj = &bestTile->verts[bestPoly->verts[j]*3]; + const float* vi = &bestTile->verts[bestPoly->verts[i]*3]; + float tseg; + float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, tseg); + + // Edge is too far, skip. + if (distSqr > radiusSqr) + continue; + + // Hit wall, update radius. + radiusSqr = distSqr; + // Calculate hit pos. + hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg; + hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg; + hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg; + } + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + const dtLink* link = &bestTile->links[i]; + dtPolyRef neighbourRef = link->ref; + // Skip invalid neighbours and do not follow back to parent. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Expand to neighbour. + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); + + // Skip off-mesh connections. + if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + + // Calc distance to the edge. + const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3]; + const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3]; + float tseg; + float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); + + // If the circle is not touching the next polygon, skip it. + if (distSqr > radiusSqr) + continue; + + if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) + continue; + + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; + continue; + } + + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Cost + if (neighbourNode->flags == 0) + { + getEdgeMidPoint(bestRef, bestPoly, bestTile, + neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos); + } + + const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); + + // The node is already in open list and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) + continue; + + neighbourNode->id = neighbourRef; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); + neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->total = total; + + if (neighbourNode->flags & DT_NODE_OPEN) + { + m_openList->modify(neighbourNode); + } + else + { + neighbourNode->flags |= DT_NODE_OPEN; + m_openList->push(neighbourNode); + } + } + } + + // Calc hit normal. + dtVsub(hitNormal, centerPos, hitPos); + dtVnormalize(hitNormal); + + *hitDist = dtMathSqrtf(radiusSqr); + + return status; +} + +bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const +{ + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly); + // If cannot get polygon, assume it does not exists and boundary is invalid. + if (dtStatusFailed(status)) + return false; + // If cannot pass filter, assume flags has changed and boundary is invalid. + if (!filter->passFilter(ref, tile, poly)) + return false; + return true; +} + +/// @par +/// +/// The closed list is the list of polygons that were fully evaluated during +/// the last navigation graph search. (A* or Dijkstra) +/// +bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const +{ + if (!m_nodePool) return false; + + dtNode* nodes[DT_MAX_STATES_PER_NODE]; + int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE); + + for (int i=0; iflags & DT_NODE_CLOSED) + return true; + } + + return false; +} diff --git a/extern/recastnavigation/Detour/Source/DetourNode.cpp b/extern/recastnavigation/Detour/Source/DetourNode.cpp new file mode 100644 index 000000000..48abbba6b --- /dev/null +++ b/extern/recastnavigation/Detour/Source/DetourNode.cpp @@ -0,0 +1,200 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DetourNode.h" +#include "DetourAlloc.h" +#include "DetourAssert.h" +#include "DetourCommon.h" +#include + +#ifdef DT_POLYREF64 +// From Thomas Wang, https://gist.github.com/badboy/6267743 +inline unsigned int dtHashRef(dtPolyRef a) +{ + a = (~a) + (a << 18); // a = (a << 18) - a - 1; + a = a ^ (a >> 31); + a = a * 21; // a = (a + (a << 2)) + (a << 4); + a = a ^ (a >> 11); + a = a + (a << 6); + a = a ^ (a >> 22); + return (unsigned int)a; +} +#else +inline unsigned int dtHashRef(dtPolyRef a) +{ + a += ~(a<<15); + a ^= (a>>10); + a += (a<<3); + a ^= (a>>6); + a += ~(a<<11); + a ^= (a>>16); + return (unsigned int)a; +} +#endif + +////////////////////////////////////////////////////////////////////////////////////////// +dtNodePool::dtNodePool(int maxNodes, int hashSize) : + m_nodes(0), + m_first(0), + m_next(0), + m_maxNodes(maxNodes), + m_hashSize(hashSize), + m_nodeCount(0) +{ + dtAssert(dtNextPow2(m_hashSize) == (unsigned int)m_hashSize); + // pidx is special as 0 means "none" and 1 is the first node. For that reason + // we have 1 fewer nodes available than the number of values it can contain. + dtAssert(m_maxNodes > 0 && m_maxNodes <= DT_NULL_IDX && m_maxNodes <= (1 << DT_NODE_PARENT_BITS) - 1); + + m_nodes = (dtNode*)dtAlloc(sizeof(dtNode)*m_maxNodes, DT_ALLOC_PERM); + m_next = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*m_maxNodes, DT_ALLOC_PERM); + m_first = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*hashSize, DT_ALLOC_PERM); + + dtAssert(m_nodes); + dtAssert(m_next); + dtAssert(m_first); + + memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize); + memset(m_next, 0xff, sizeof(dtNodeIndex)*m_maxNodes); +} + +dtNodePool::~dtNodePool() +{ + dtFree(m_nodes); + dtFree(m_next); + dtFree(m_first); +} + +void dtNodePool::clear() +{ + memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize); + m_nodeCount = 0; +} + +unsigned int dtNodePool::findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes) +{ + int n = 0; + unsigned int bucket = dtHashRef(id) & (m_hashSize-1); + dtNodeIndex i = m_first[bucket]; + while (i != DT_NULL_IDX) + { + if (m_nodes[i].id == id) + { + if (n >= maxNodes) + return n; + nodes[n++] = &m_nodes[i]; + } + i = m_next[i]; + } + + return n; +} + +dtNode* dtNodePool::findNode(dtPolyRef id, unsigned char state) +{ + unsigned int bucket = dtHashRef(id) & (m_hashSize-1); + dtNodeIndex i = m_first[bucket]; + while (i != DT_NULL_IDX) + { + if (m_nodes[i].id == id && m_nodes[i].state == state) + return &m_nodes[i]; + i = m_next[i]; + } + return 0; +} + +dtNode* dtNodePool::getNode(dtPolyRef id, unsigned char state) +{ + unsigned int bucket = dtHashRef(id) & (m_hashSize-1); + dtNodeIndex i = m_first[bucket]; + dtNode* node = 0; + while (i != DT_NULL_IDX) + { + if (m_nodes[i].id == id && m_nodes[i].state == state) + return &m_nodes[i]; + i = m_next[i]; + } + + if (m_nodeCount >= m_maxNodes) + return 0; + + i = (dtNodeIndex)m_nodeCount; + m_nodeCount++; + + // Init node + node = &m_nodes[i]; + node->pidx = 0; + node->cost = 0; + node->total = 0; + node->id = id; + node->state = state; + node->flags = 0; + + m_next[i] = m_first[bucket]; + m_first[bucket] = i; + + return node; +} + + +////////////////////////////////////////////////////////////////////////////////////////// +dtNodeQueue::dtNodeQueue(int n) : + m_heap(0), + m_capacity(n), + m_size(0) +{ + dtAssert(m_capacity > 0); + + m_heap = (dtNode**)dtAlloc(sizeof(dtNode*)*(m_capacity+1), DT_ALLOC_PERM); + dtAssert(m_heap); +} + +dtNodeQueue::~dtNodeQueue() +{ + dtFree(m_heap); +} + +void dtNodeQueue::bubbleUp(int i, dtNode* node) +{ + int parent = (i-1)/2; + // note: (index > 0) means there is a parent + while ((i > 0) && (m_heap[parent]->total > node->total)) + { + m_heap[i] = m_heap[parent]; + i = parent; + parent = (i-1)/2; + } + m_heap[i] = node; +} + +void dtNodeQueue::trickleDown(int i, dtNode* node) +{ + int child = (i*2)+1; + while (child < m_size) + { + if (((child+1) < m_size) && + (m_heap[child]->total > m_heap[child+1]->total)) + { + child++; + } + m_heap[i] = m_heap[child]; + i = child; + child = (i*2)+1; + } + bubbleUp(i, node); +} diff --git a/extern/recastnavigation/DetourCrowd/CMakeLists.txt b/extern/recastnavigation/DetourCrowd/CMakeLists.txt new file mode 100644 index 000000000..73cdf7ce8 --- /dev/null +++ b/extern/recastnavigation/DetourCrowd/CMakeLists.txt @@ -0,0 +1,33 @@ +file(GLOB SOURCES Source/*.cpp) + +if (RECASTNAVIGATION_STATIC) + add_library(DetourCrowd STATIC ${SOURCES}) +else () + add_library(DetourCrowd SHARED ${SOURCES}) +endif () + +add_library(RecastNavigation::DetourCrowd ALIAS DetourCrowd) + +set(DetourCrowd_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Include") + +target_include_directories(DetourCrowd PUBLIC + "$" +) + +target_link_libraries(DetourCrowd + Detour +) + +set_target_properties(DetourCrowd PROPERTIES + SOVERSION ${SOVERSION} + VERSION ${VERSION} + ) + +install(TARGETS DetourCrowd + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + COMPONENT library + ) + +file(GLOB INCLUDES Include/*.h) +install(FILES ${INCLUDES} DESTINATION include) diff --git a/extern/recastnavigation/DetourCrowd/Include/DetourCrowd.h b/extern/recastnavigation/DetourCrowd/Include/DetourCrowd.h new file mode 100644 index 000000000..952050878 --- /dev/null +++ b/extern/recastnavigation/DetourCrowd/Include/DetourCrowd.h @@ -0,0 +1,460 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURCROWD_H +#define DETOURCROWD_H + +#include "DetourNavMeshQuery.h" +#include "DetourObstacleAvoidance.h" +#include "DetourLocalBoundary.h" +#include "DetourPathCorridor.h" +#include "DetourProximityGrid.h" +#include "DetourPathQueue.h" + +/// The maximum number of neighbors that a crowd agent can take into account +/// for steering decisions. +/// @ingroup crowd +static const int DT_CROWDAGENT_MAX_NEIGHBOURS = 6; + +/// The maximum number of corners a crowd agent will look ahead in the path. +/// This value is used for sizing the crowd agent corner buffers. +/// Due to the behavior of the crowd manager, the actual number of useful +/// corners will be one less than this number. +/// @ingroup crowd +static const int DT_CROWDAGENT_MAX_CORNERS = 4; + +/// The maximum number of crowd avoidance configurations supported by the +/// crowd manager. +/// @ingroup crowd +/// @see dtObstacleAvoidanceParams, dtCrowd::setObstacleAvoidanceParams(), dtCrowd::getObstacleAvoidanceParams(), +/// dtCrowdAgentParams::obstacleAvoidanceType +static const int DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS = 8; + +/// The maximum number of query filter types supported by the crowd manager. +/// @ingroup crowd +/// @see dtQueryFilter, dtCrowd::getFilter() dtCrowd::getEditableFilter(), +/// dtCrowdAgentParams::queryFilterType +static const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16; + +/// Provides neighbor data for agents managed by the crowd. +/// @ingroup crowd +/// @see dtCrowdAgent::neis, dtCrowd +struct dtCrowdNeighbour +{ + int idx; ///< The index of the neighbor in the crowd. + float dist; ///< The distance between the current agent and the neighbor. +}; + +/// The type of navigation mesh polygon the agent is currently traversing. +/// @ingroup crowd +enum CrowdAgentState +{ + DT_CROWDAGENT_STATE_INVALID, ///< The agent is not in a valid state. + DT_CROWDAGENT_STATE_WALKING, ///< The agent is traversing a normal navigation mesh polygon. + DT_CROWDAGENT_STATE_OFFMESH, ///< The agent is traversing an off-mesh connection. +}; + +/// Configuration parameters for a crowd agent. +/// @ingroup crowd +struct dtCrowdAgentParams +{ + float radius; ///< Agent radius. [Limit: >= 0] + float height; ///< Agent height. [Limit: > 0] + float maxAcceleration; ///< Maximum allowed acceleration. [Limit: >= 0] + float maxSpeed; ///< Maximum allowed speed. [Limit: >= 0] + + /// Defines how close a collision element must be before it is considered for steering behaviors. [Limits: > 0] + float collisionQueryRange; + + float pathOptimizationRange; ///< The path visibility optimization range. [Limit: > 0] + + /// How aggresive the agent manager should be at avoiding collisions with this agent. [Limit: >= 0] + float separationWeight; + + /// Flags that impact steering behavior. (See: #UpdateFlags) + unsigned char updateFlags; + + /// The index of the avoidance configuration to use for the agent. + /// [Limits: 0 <= value <= #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS] + unsigned char obstacleAvoidanceType; + + /// The index of the query filter used by this agent. + unsigned char queryFilterType; + + /// User defined data attached to the agent. + void* userData; +}; + +enum MoveRequestState +{ + DT_CROWDAGENT_TARGET_NONE = 0, + DT_CROWDAGENT_TARGET_FAILED, + DT_CROWDAGENT_TARGET_VALID, + DT_CROWDAGENT_TARGET_REQUESTING, + DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE, + DT_CROWDAGENT_TARGET_WAITING_FOR_PATH, + DT_CROWDAGENT_TARGET_VELOCITY, +}; + +/// Represents an agent managed by a #dtCrowd object. +/// @ingroup crowd +struct dtCrowdAgent +{ + /// True if the agent is active, false if the agent is in an unused slot in the agent pool. + bool active; + + /// The type of mesh polygon the agent is traversing. (See: #CrowdAgentState) + unsigned char state; + + /// True if the agent has valid path (targetState == DT_CROWDAGENT_TARGET_VALID) and the path does not lead to the requested position, else false. + bool partial; + + /// The path corridor the agent is using. + dtPathCorridor corridor; + + /// The local boundary data for the agent. + dtLocalBoundary boundary; + + /// Time since the agent's path corridor was optimized. + float topologyOptTime; + + /// The known neighbors of the agent. + dtCrowdNeighbour neis[DT_CROWDAGENT_MAX_NEIGHBOURS]; + + /// The number of neighbors. + int nneis; + + /// The desired speed. + float desiredSpeed; + + float npos[3]; ///< The current agent position. [(x, y, z)] + float disp[3]; ///< A temporary value used to accumulate agent displacement during iterative collision resolution. [(x, y, z)] + float dvel[3]; ///< The desired velocity of the agent. Based on the current path, calculated from scratch each frame. [(x, y, z)] + float nvel[3]; ///< The desired velocity adjusted by obstacle avoidance, calculated from scratch each frame. [(x, y, z)] + float vel[3]; ///< The actual velocity of the agent. The change from nvel -> vel is constrained by max acceleration. [(x, y, z)] + + /// The agent's configuration parameters. + dtCrowdAgentParams params; + + /// The local path corridor corners for the agent. (Staight path.) [(x, y, z) * #ncorners] + float cornerVerts[DT_CROWDAGENT_MAX_CORNERS*3]; + + /// The local path corridor corner flags. (See: #dtStraightPathFlags) [(flags) * #ncorners] + unsigned char cornerFlags[DT_CROWDAGENT_MAX_CORNERS]; + + /// The reference id of the polygon being entered at the corner. [(polyRef) * #ncorners] + dtPolyRef cornerPolys[DT_CROWDAGENT_MAX_CORNERS]; + + /// The number of corners. + int ncorners; + + unsigned char targetState; ///< State of the movement request. + dtPolyRef targetRef; ///< Target polyref of the movement request. + float targetPos[3]; ///< Target position of the movement request (or velocity in case of DT_CROWDAGENT_TARGET_VELOCITY). + dtPathQueueRef targetPathqRef; ///< Path finder ref. + bool targetReplan; ///< Flag indicating that the current path is being replanned. + float targetReplanTime; ///