diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp
index da1766f4d9..99132b711a 100644
--- a/apps/openmw/mwmechanics/aipackage.cpp
+++ b/apps/openmw/mwmechanics/aipackage.cpp
@@ -96,6 +96,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
 
     const float distToTarget = distance(position, dest);
     const bool isDestReached = (distToTarget <= destTolerance);
+    const bool actorCanMoveByZ = canActorMoveByZAxis(actor);
 
     if (!isDestReached && mTimer > AI_REACTION_TIME)
     {
@@ -104,7 +105,6 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
 
         const bool wasShortcutting = mIsShortcutting;
         bool destInLOS = false;
-        const bool actorCanMoveByZ = canActorMoveByZAxis(actor);
 
         // Prohibit shortcuts for AiWander, if the actor can not move in 3 dimensions.
         mIsShortcutting = actorCanMoveByZ
@@ -151,7 +151,8 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
     const float pointTolerance = std::max(MIN_TOLERANCE, actorTolerance);
 
     static const bool smoothMovement = Settings::Manager::getBool("smooth movement", "Game");
-    mPathFinder.update(position, pointTolerance, DEFAULT_TOLERANCE, /*shortenIfAlmostStraight=*/smoothMovement);
+    mPathFinder.update(position, pointTolerance, DEFAULT_TOLERANCE,
+                       /*shortenIfAlmostStraight=*/smoothMovement, actorCanMoveByZ);
 
     if (isDestReached || mPathFinder.checkPathCompleted()) // if path is finished
     {
diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp
index cc28b7995e..93ae90547c 100644
--- a/apps/openmw/mwmechanics/pathfinding.cpp
+++ b/apps/openmw/mwmechanics/pathfinding.cpp
@@ -296,7 +296,8 @@ namespace MWMechanics
         return getXAngleToDir(dir);
     }
 
-    void PathFinder::update(const osg::Vec3f& position, float pointTolerance, float destinationTolerance, bool shortenIfAlmostStraight)
+    void PathFinder::update(const osg::Vec3f& position, float pointTolerance, float destinationTolerance,
+                            bool shortenIfAlmostStraight, bool canMoveByZ)
     {
         if (mPath.empty())
             return;
@@ -312,8 +313,16 @@ namespace MWMechanics
                 mPath.pop_front();
         }
 
-        if (mPath.size() == 1 && sqrDistanceIgnoreZ(mPath.front(), position) < destinationTolerance * destinationTolerance)
-            mPath.pop_front();
+        if (mPath.size() == 1)
+        {
+            float distSqr;
+            if (canMoveByZ)
+                distSqr = (mPath.front() - position).length2();
+            else
+                distSqr = sqrDistanceIgnoreZ(mPath.front(), position);
+            if (distSqr < destinationTolerance * destinationTolerance)
+                mPath.pop_front();
+        }
     }
 
     void PathFinder::buildStraightPath(const osg::Vec3f& endPoint)
diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp
index bd81bbfe1f..b5c376b8ca 100644
--- a/apps/openmw/mwmechanics/pathfinding.hpp
+++ b/apps/openmw/mwmechanics/pathfinding.hpp
@@ -102,7 +102,8 @@ namespace MWMechanics
                 const DetourNavigator::Flags flags, const DetourNavigator::AreaCosts& areaCosts);
 
             /// Remove front point if exist and within tolerance
-            void update(const osg::Vec3f& position, float pointTolerance, float destinationTolerance, bool shortenIfAlmostStraight);
+            void update(const osg::Vec3f& position, float pointTolerance, float destinationTolerance,
+                        bool shortenIfAlmostStraight, bool canMoveByZ);
 
             bool checkPathCompleted() const
             {