Use navmesh raycast to find reachable position around target

apps/openmw/mwmechanics/aicombat.cpp

@@ -8,6 +8,7 @@
 #include <components/misc/mathutil.hpp>
 
 #include <components/sceneutil/positionattitudetransform.hpp>
+#include <components/detournavigator/navigator.hpp>
 
 #include "../mwphysics/collisiontype.hpp"
 
@@ -127,10 +128,11 @@ namespace MWMechanics
             {
                 //Update every frame. UpdateLOS uses a timer, so the LOS check does not happen every frame.
                 updateLOS(actor, target, duration, storage);
-                float targetReachedTolerance = 0.0f;
-                if (storage.mLOS)
-                    targetReachedTolerance = storage.mAttackRange;
-                const bool is_target_reached = pathTo(actor, target.getRefData().getPosition().asVec3(), duration, targetReachedTolerance);
+                const float targetReachedTolerance = storage.mLOS && !storage.mUseCustomDestination
+                        ? storage.mAttackRange : 0.0f;
+                const osg::Vec3f destination = storage.mUseCustomDestination
+                        ? storage.mCustomDestination : target.getRefData().getPosition().asVec3();
+                const bool is_target_reached = pathTo(actor, destination, duration, targetReachedTolerance);
                 if (is_target_reached) storage.mReadyToAttack = true;
             }
 
@@ -232,8 +234,8 @@ namespace MWMechanics
         const ESM::Weapon* weapon = currentAction->getWeapon();
 
         ESM::Position pos = actor.getRefData().getPosition();
-        osg::Vec3f vActorPos(pos.asVec3());
-        osg::Vec3f vTargetPos(target.getRefData().getPosition().asVec3());
+        const osg::Vec3f vActorPos(pos.asVec3());
+        const osg::Vec3f vTargetPos(target.getRefData().getPosition().asVec3());
 
         osg::Vec3f vAimDir = MWBase::Environment::get().getWorld()->aimToTarget(actor, target);
         float distToTarget = MWBase::Environment::get().getWorld()->getHitDistance(actor, target);
@@ -243,9 +245,7 @@ namespace MWMechanics
         if (isRangedCombat)
         {
             // rotate actor taking into account target movement direction and projectile speed
-            osg::Vec3f& lastTargetPos = storage.mLastTargetPos;
-            vAimDir = AimDirToMovingTarget(actor, target, lastTargetPos, AI_REACTION_TIME, (weapon ? weapon->mData.mType : 0), storage.mStrength);
-            lastTargetPos = vTargetPos;
+            vAimDir = AimDirToMovingTarget(actor, target, storage.mLastTargetPos, AI_REACTION_TIME, (weapon ? weapon->mData.mType : 0), storage.mStrength);
 
             storage.mMovement.mRotation[0] = getXAngleToDir(vAimDir);
             storage.mMovement.mRotation[2] = getZAngleToDir(vAimDir);
@@ -256,28 +256,66 @@ namespace MWMechanics
             storage.mMovement.mRotation[2] = getZAngleToDir((vTargetPos-vActorPos)); // using vAimDir results in spastic movements since the head is animated
         }
 
+        storage.mLastTargetPos = vTargetPos;
+
         if (storage.mReadyToAttack)
         {
             storage.startCombatMove(isRangedCombat, distToTarget, rangeAttack, actor, target);
             // start new attack
             storage.startAttackIfReady(actor, characterController, weapon, isRangedCombat);
         }
-        else if (!isRangedCombat && !mPathFinder.isPathConstructed() && storage.mCurrentAction->isAttackingOrSpell())
+
+        // If actor uses custom destination it has to try to rebuild path because environment can change
+        // (door is opened between actor and target) or target position has changed and current custom destination
+        // is not good enough to attack target.
+        if (storage.mCurrentAction->isAttackingOrSpell()
+            && ((!storage.mReadyToAttack && !mPathFinder.isPathConstructed())
+                || (storage.mUseCustomDestination && (storage.mCustomDestination - vTargetPos).length() > rangeAttack)))
         {
-            const osg::Vec3f position = actor.getRefData().getPosition().asVec3();
-            const osg::Vec3f destination = target.getRefData().getPosition().asVec3();
-            const osg::Vec3f halfExtents = MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(actor);
-            mPathFinder.buildPath(actor, position, destination, actor.getCell(), getPathGridGraph(actor.getCell()),
-                                  halfExtents, getNavigatorFlags(actor), getAreaCosts(actor));
+            // Try to build path to the target.
+            const auto halfExtents = MWBase::Environment::get().getWorld()->getPathfindingHalfExtents(actor);
+            const auto navigatorFlags = getNavigatorFlags(actor);
+            const auto areaCosts = getAreaCosts(actor);
+            const auto pathGridGraph = getPathGridGraph(actor.getCell());
+            mPathFinder.buildPath(actor, vActorPos, vTargetPos, actor.getCell(), pathGridGraph, halfExtents, navigatorFlags, areaCosts);
 
             if (!mPathFinder.isPathConstructed())
             {
-                storage.stopAttack();
-                characterController.setAttackingOrSpell(false);
-                currentAction.reset(new ActionFlee());
-                actionCooldown = currentAction->getActionCooldown();
-                storage.startFleeing();
-                MWBase::Environment::get().getDialogueManager()->say(actor, "flee");
+                // If there is no path, try to find a point on a line from the actor position to target projected
+                // on navmesh to attack the target from there.
+                const MWBase::World* world = MWBase::Environment::get().getWorld();
+                const auto halfExtents = world->getPathfindingHalfExtents(actor);
+                const auto navigator = world->getNavigator();
+                const auto navigatorFlags = getNavigatorFlags(actor);
+                const auto areaCosts = getAreaCosts(actor);
+                const auto hit = navigator->raycast(halfExtents, vActorPos, vTargetPos, navigatorFlags);
+
+                if (hit.has_value() && (*hit - vTargetPos).length() <= rangeAttack)
+                {
+                    // If the point is close enough, try to find a path to that point.
+                    mPathFinder.buildPath(actor, vActorPos, *hit, actor.getCell(), pathGridGraph, halfExtents, navigatorFlags, areaCosts);
+                    if (mPathFinder.isPathConstructed())
+                    {
+                        // If path to that point is found use it as custom destination.
+                        storage.mCustomDestination = *hit;
+                        storage.mUseCustomDestination = true;
+                    }
+                }
+
+                if (!mPathFinder.isPathConstructed())
+                {
+                    storage.mUseCustomDestination = false;
+                    storage.stopAttack();
+                    characterController.setAttackingOrSpell(false);
+                    currentAction.reset(new ActionFlee());
+                    actionCooldown = currentAction->getActionCooldown();
+                    storage.startFleeing();
+                    MWBase::Environment::get().getDialogueManager()->say(actor, "flee");
+                }
+            }
+            else
+            {
+                storage.mUseCustomDestination = false;
             }
         }
 

apps/openmw/mwmechanics/aicombat.hpp

@@ -55,6 +55,9 @@ namespace MWMechanics
         float mFleeBlindRunTimer;
         ESM::Pathgrid::Point mFleeDest;
 
+        bool mUseCustomDestination;
+        osg::Vec3f mCustomDestination;
+
         AiCombatStorage():
         mAttackCooldown(0.0f),
         mTimerReact(AI_REACTION_TIME),
@@ -74,7 +77,9 @@ namespace MWMechanics
         mFleeState(FleeState_None),
         mLOS(false),
         mUpdateLOSTimer(0.0f),
-        mFleeBlindRunTimer(0.0f)
+        mFleeBlindRunTimer(0.0f),
+        mUseCustomDestination(false),
+        mCustomDestination()
         {}
 
         void startCombatMove(bool isDistantCombat, float distToTarget, float rangeAttack, const MWWorld::Ptr& actor, const MWWorld::Ptr& target);

apps/openmw_test_suite/detournavigator/navigator.cpp

@@ -802,4 +802,26 @@ namespace
         EXPECT_GT(duration, mSettings.mMinUpdateInterval)
             << std::chrono::duration_cast<std::chrono::duration<float, std::milli>>(duration).count() << " ms";
     }
+
+    TEST_F(DetourNavigatorNavigatorTest, update_then_raycast_should_return_position)
+    {
+        const std::array<btScalar, 5 * 5> heightfieldData {{
+            0,   0,    0,    0,    0,
+            0, -25,  -25,  -25,  -25,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+            0, -25, -100, -100, -100,
+        }};
+        btHeightfieldTerrainShape shape(5, 5, heightfieldData.data(), 1, 0, 0, 2, PHY_FLOAT, false);
+        shape.setLocalScaling(btVector3(128, 128, 1));
+
+        mNavigator->addAgent(mAgentHalfExtents);
+        mNavigator->addObject(ObjectId(&shape), shape, btTransform::getIdentity());
+        mNavigator->update(mPlayerPosition);
+        mNavigator->wait();
+
+        const auto result = mNavigator->raycast(mAgentHalfExtents, mStart, mEnd, Flag_walk);
+
+        ASSERT_THAT(result, Optional(Vec3fEq(mEnd.x(), mEnd.y(), 1.87719)));
+    }
 }

components/CMakeLists.txt

@@ -181,6 +181,7 @@ add_component_dir(detournavigator
     settings
     navigator
     findrandompointaroundcircle
+    raycast
     )
 
 set (ESM_UI ${CMAKE_SOURCE_DIR}/files/ui/contentselector.ui

components/detournavigator/navigator.cpp

@@ -1,5 +1,6 @@
 #include "findrandompointaroundcircle.hpp"
 #include "navigator.hpp"
+#include "raycast.hpp"
 
 namespace DetourNavigator
 {
@@ -17,4 +18,19 @@ namespace DetourNavigator
             return std::optional<osg::Vec3f>();
         return std::optional<osg::Vec3f>(fromNavMeshCoordinates(settings, *result));
     }
+
+    std::optional<osg::Vec3f> Navigator::raycast(const osg::Vec3f& agentHalfExtents, const osg::Vec3f& start,
+        const osg::Vec3f& end, const Flags includeFlags) const
+    {
+        const auto navMesh = getNavMesh(agentHalfExtents);
+        if (navMesh == nullptr)
+            return {};
+        const auto settings = getSettings();
+        const auto result = DetourNavigator::raycast(navMesh->lockConst()->getImpl(),
+            toNavMeshCoordinates(settings, agentHalfExtents), toNavMeshCoordinates(settings, start),
+            toNavMeshCoordinates(settings, end), includeFlags, settings);
+        if (!result)
+            return {};
+        return fromNavMeshCoordinates(settings, *result);
+    }
 }

components/detournavigator/navigator.hpp

@@ -223,6 +223,17 @@ namespace DetourNavigator
         std::optional<osg::Vec3f> findRandomPointAroundCircle(const osg::Vec3f& agentHalfExtents,
             const osg::Vec3f& start, const float maxRadius, const Flags includeFlags) const;
 
+        /**
+         * @brief raycast finds farest navmesh point from start on a line from start to end that has path from start.
+         * @param agentHalfExtents allows to find navmesh for given actor.
+         * @param start of the line
+         * @param end of the line
+         * @param includeFlags setup allowed surfaces for actor to walk.
+         * @return not empty optional with position if point is found and empty optional if point is not found.
+         */
+        std::optional<osg::Vec3f> raycast(const osg::Vec3f& agentHalfExtents, const osg::Vec3f& start,
+            const osg::Vec3f& end, const Flags includeFlags) const;
+
         virtual RecastMeshTiles getRecastMeshTiles() = 0;
     };
 }

components/detournavigator/raycast.cpp

@@ -0,0 +1,44 @@
+#include "raycast.hpp"
+#include "settings.hpp"
+#include "findsmoothpath.hpp"
+
+#include <DetourCommon.h>
+#include <DetourNavMesh.h>
+#include <DetourNavMeshQuery.h>
+
+#include <array>
+
+namespace DetourNavigator
+{
+    std::optional<osg::Vec3f> raycast(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents,
+        const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const Settings& settings)
+    {
+        dtNavMeshQuery navMeshQuery;
+        if (!initNavMeshQuery(navMeshQuery, navMesh, settings.mMaxNavMeshQueryNodes))
+            return {};
+
+        dtQueryFilter queryFilter;
+        queryFilter.setIncludeFlags(includeFlags);
+
+        dtPolyRef ref = 0;
+        if (dtStatus status = navMeshQuery.findNearestPoly(start.ptr(), halfExtents.ptr(), &queryFilter, &ref, nullptr);
+            dtStatusFailed(status) || ref == 0)
+            return {};
+
+        const unsigned options = 0;
+        std::array<dtPolyRef, 16> path;
+        dtRaycastHit hit;
+        hit.path = path.data();
+        hit.maxPath = path.size();
+        if (dtStatus status = navMeshQuery.raycast(ref, start.ptr(), end.ptr(), &queryFilter, options, &hit);
+            dtStatusFailed(status) || hit.pathCount == 0)
+            return {};
+
+        osg::Vec3f hitPosition;
+        if (dtStatus status = navMeshQuery.closestPointOnPoly(path[hit.pathCount - 1], end.ptr(), hitPosition.ptr(), nullptr);
+            dtStatusFailed(status))
+            return {};
+
+        return hitPosition;
+    }
+}

components/detournavigator/raycast.hpp

@@ -0,0 +1,19 @@
+#ifndef OPENMW_COMPONENTS_DETOURNAVIGATOR_RAYCAST_H
+#define OPENMW_COMPONENTS_DETOURNAVIGATOR_RAYCAST_H
+
+#include "flags.hpp"
+
+#include <optional>
+#include <osg/Vec3f>
+
+class dtNavMesh;
+
+namespace DetourNavigator
+{
+    struct Settings;
+
+    std::optional<osg::Vec3f> raycast(const dtNavMesh& navMesh, const osg::Vec3f& halfExtents,
+        const osg::Vec3f& start, const osg::Vec3f& end, const Flags includeFlags, const Settings& settings);
+}
+
+#endif