diff --git a/apps/openmw/mwmechanics/aipackage.cpp b/apps/openmw/mwmechanics/aipackage.cpp
index 9bc3ed103e..c7cc649d46 100644
--- a/apps/openmw/mwmechanics/aipackage.cpp
+++ b/apps/openmw/mwmechanics/aipackage.cpp
@@ -171,7 +171,7 @@ bool MWMechanics::AiPackage::pathTo(const MWWorld::Ptr& actor, const osg::Vec3f&
         mTimer = 0;
     }
 
-    if (isDestReached || mPathFinder.checkPathCompleted(pos.pos[0], pos.pos[1])) // if path is finished
+    if (isDestReached || mPathFinder.checkPathCompleted(pos.asVec3())) // if path is finished
     {
         // turn to destination point
         zTurn(actor, getZAngleToPoint(start, dest));
diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp
index 54536a93b1..8e78e10bd5 100644
--- a/apps/openmw/mwmechanics/aiwander.cpp
+++ b/apps/openmw/mwmechanics/aiwander.cpp
@@ -249,7 +249,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(pos.asVec3(), DESTINATION_TOLERANCE))
+        {
             completeManualWalking(actor, storage);
         }
 
diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp
index b2b96539ac..2efd06e7b2 100644
--- a/apps/openmw/mwmechanics/pathfinding.cpp
+++ b/apps/openmw/mwmechanics/pathfinding.cpp
@@ -55,11 +55,14 @@ namespace
             (closestReachableIndex, closestReachableIndex == closestIndex);
     }
 
-    float sqrDistanceIgnoreZ(const osg::Vec3f& point, float x, float y)
+    float sqrDistance(const osg::Vec2f& lhs, const osg::Vec2f& rhs)
     {
-        x -= point.x();
-        y -= point.y();
-        return (x * x + y * y);
+        return (lhs - rhs).length2();
+    }
+
+    float sqrDistanceIgnoreZ(const osg::Vec3f& lhs, const osg::Vec3f& rhs)
+    {
+        return sqrDistance(osg::Vec2f(lhs.x(), lhs.y()), osg::Vec2f(rhs.x(), rhs.y()));
     }
 }
 
@@ -263,9 +266,9 @@ namespace MWMechanics
         return getXAngleToDir(dir);
     }
 
-    bool PathFinder::checkPathCompleted(float x, float y, float tolerance)
+    bool PathFinder::checkPathCompleted(const osg::Vec3f& position, const float tolerance)
     {
-        if (!mPath.empty() && sqrDistanceIgnoreZ(mPath.front(), x, y) < tolerance*tolerance)
+        if (!mPath.empty() && sqrDistanceIgnoreZ(mPath.front(), position) < tolerance*tolerance)
             mPath.pop_front();
 
         return mPath.empty();
diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp
index fddd62c29a..596178655a 100644
--- a/apps/openmw/mwmechanics/pathfinding.hpp
+++ b/apps/openmw/mwmechanics/pathfinding.hpp
@@ -63,7 +63,7 @@ namespace MWMechanics
             void buildPath(const osg::Vec3f& startPoint, const osg::Vec3f& endPoint,
                            const MWWorld::CellStore* cell, const PathgridGraph& pathgridGraph);
 
-            bool checkPathCompleted(float x, float y, float tolerance = PathTolerance);
+            bool checkPathCompleted(const osg::Vec3f& position, const float tolerance = PathTolerance);
             ///< \Returns true if we are within \a tolerance units of the last path point.
 
             /// In radians