diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp
index ce5673909f..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
@@ -150,7 +150,9 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
             + 1.2 * std::max(halfExtents.x(), halfExtents.y());
     const float pointTolerance = std::max(MIN_TOLERANCE, actorTolerance);
 
-    mPathFinder.update(position, pointTolerance, DEFAULT_TOLERANCE);
+    static const bool smoothMovement = Settings::Manager::getBool("smooth movement", "Game");
+    mPathFinder.update(position, pointTolerance, DEFAULT_TOLERANCE,
+                       /*shortenIfAlmostStraight=*/smoothMovement, actorCanMoveByZ);
 
     if (isDestReached || mPathFinder.checkPathCompleted()) // if path is finished
     {
@@ -180,7 +182,6 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
     const auto destination = mPathFinder.getPath().empty() ? dest : mPathFinder.getPath().front();
     mObstacleCheck.update(actor, destination, duration);
 
-    static const bool smoothMovement = Settings::Manager::getBool("smooth movement", "Game");
     if (smoothMovement)
     {
         const float smoothTurnReservedDist = 150;
diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp
index 276321b81a..93ae90547c 100644
--- a/apps/openmw/mwmechanics/pathfinding.cpp
+++ b/apps/openmw/mwmechanics/pathfinding.cpp
@@ -97,12 +97,12 @@ namespace
         float dotProduct = v1.x() * v3.x() + v1.y() * v3.y();
         float crossProduct = v1.x() * v3.y() - v1.y() * v3.x();
 
-        // Check that the angle between v1 and v3 is less or equal than 10 degrees.
-        static const float cos170 = std::cos(osg::PI / 180 * 170);
-        bool checkAngle = dotProduct <= cos170 * v1.length() * v3.length();
+        // Check that the angle between v1 and v3 is less or equal than 5 degrees.
+        static const float cos175 = std::cos(osg::PI * (175.0 / 180));
+        bool checkAngle = dotProduct <= cos175 * v1.length() * v3.length();
 
         // Check that distance from p2 to the line (p1, p3) is less or equal than `pointTolerance`.
-        bool checkDist = std::abs(crossProduct) <= pointTolerance * (p3 - p1).length() * 2;
+        bool checkDist = std::abs(crossProduct) <= pointTolerance * (p3 - p1).length();
 
         return checkAngle && checkDist;
     }
@@ -296,7 +296,8 @@ namespace MWMechanics
         return getXAngleToDir(dir);
     }
 
-    void PathFinder::update(const osg::Vec3f& position, const float pointTolerance, const float destinationTolerance)
+    void PathFinder::update(const osg::Vec3f& position, float pointTolerance, float destinationTolerance,
+                            bool shortenIfAlmostStraight, bool canMoveByZ)
     {
         if (mPath.empty())
             return;
@@ -304,13 +305,24 @@ namespace MWMechanics
         while (mPath.size() > 1 && sqrDistanceIgnoreZ(mPath.front(), position) < pointTolerance * pointTolerance)
             mPath.pop_front();
 
-        while (mPath.size() > 2 && isAlmostStraight(mPath[0], mPath[1], mPath[2], pointTolerance))
-            mPath.erase(mPath.begin() + 1);
-        if (mPath.size() > 1 && isAlmostStraight(position, mPath[0], mPath[1], pointTolerance))
-            mPath.pop_front();
+        if (shortenIfAlmostStraight)
+        {
+            while (mPath.size() > 2 && isAlmostStraight(mPath[0], mPath[1], mPath[2], pointTolerance))
+                mPath.erase(mPath.begin() + 1);
+            if (mPath.size() > 1 && isAlmostStraight(position, mPath[0], mPath[1], pointTolerance))
+                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 5af822fee7..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, const float pointTolerance, const float destinationTolerance);
+            void update(const osg::Vec3f& position, float pointTolerance, float destinationTolerance,
+                        bool shortenIfAlmostStraight, bool canMoveByZ);
 
             bool checkPathCompleted() const
             {