diff --git a/apps/openmw/mwmechanics/aiescort.cpp b/apps/openmw/mwmechanics/aiescort.cpp
index 5b94c49388..b873844a24 100644
--- a/apps/openmw/mwmechanics/aiescort.cpp
+++ b/apps/openmw/mwmechanics/aiescort.cpp
@@ -151,7 +151,7 @@ bool MWMechanics::AiEscort::execute (const MWWorld::Ptr& actor)
 
     if(distanceBetweenResult <= mMaxDist * mMaxDist)
     {
-        float zAngle = mPathFinder.getZAngleToNext(pos.pos[0],pos.pos[1],pos.pos[2]);
+        float zAngle = mPathFinder.getZAngleToNext(pos.pos[0],pos.pos[1]);
         MWBase::Environment::get().getWorld()->rotateObject(actor,0,0,zAngle,false);
         MWWorld::Class::get(actor).getMovementSettings(actor).mPosition[1] = 1;
         mMaxDist = 470;
diff --git a/apps/openmw/mwmechanics/aitravel.cpp b/apps/openmw/mwmechanics/aitravel.cpp
index fbae5c1d24..99df68a916 100644
--- a/apps/openmw/mwmechanics/aitravel.cpp
+++ b/apps/openmw/mwmechanics/aitravel.cpp
@@ -87,7 +87,7 @@ bool MWMechanics::AiTravel::execute (const MWWorld::Ptr& actor)
         return true;
     }
 
-    float zAngle = mPathFinder.getZAngleToNext(pos.pos[0],pos.pos[1],pos.pos[2]);
+    float zAngle = mPathFinder.getZAngleToNext(pos.pos[0],pos.pos[1]);
     MWBase::Environment::get().getWorld()->rotateObject(actor,0,0,zAngle,false);
     MWWorld::Class::get(actor).getMovementSettings(actor).mPosition[1] = 1;
 
diff --git a/apps/openmw/mwmechanics/aiwander.cpp b/apps/openmw/mwmechanics/aiwander.cpp
index 46c5598cce..310cc3274c 100644
--- a/apps/openmw/mwmechanics/aiwander.cpp
+++ b/apps/openmw/mwmechanics/aiwander.cpp
@@ -64,7 +64,7 @@ bool MWMechanics::AiWander::execute (const MWWorld::Ptr& actor)
         {
             if(!mRepeat)
             {
-                stopWalking(actor, mPathFinder);
+                stopWalking(actor);
                 return true;
             }
             else
@@ -74,7 +74,7 @@ bool MWMechanics::AiWander::execute (const MWWorld::Ptr& actor)
         {
             if(!mRepeat)
             {
-                stopWalking(actor, mPathFinder);
+                stopWalking(actor);
                 return true;
             }
             else
@@ -149,7 +149,7 @@ bool MWMechanics::AiWander::execute (const MWWorld::Ptr& actor)
         // FIXME: This *should* pause the AiWander package instead of terminating it.
         if(sideX*(pos.pos[0] - actor.getCell()->mCell->mData.mX * ESM::Land::REAL_SIZE) > sideX * (ESM::Land::REAL_SIZE / 2.0 - 200)) 
         {
-            MWWorld::Class::get(actor).getMovementSettings(actor).mPosition[1] = 0;
+            stopWalking(actor);
             return true;
         }
     }
@@ -161,7 +161,7 @@ bool MWMechanics::AiWander::execute (const MWWorld::Ptr& actor)
         // FIXME: This *should* pause the AiWander package instead of terminating it.
         if(sideY*(pos.pos[1] - actor.getCell()->mCell->mData.mY * ESM::Land::REAL_SIZE) > sideY * (ESM::Land::REAL_SIZE / 2.0 - 200)) 
         {
-            MWWorld::Class::get(actor).getMovementSettings(actor).mPosition[1] = 0;
+            stopWalking(actor);
             return true;
         }
     }
@@ -242,7 +242,7 @@ bool MWMechanics::AiWander::execute (const MWWorld::Ptr& actor)
 
     if(mWalking)
     {
-        float zAngle = mPathFinder.getZAngleToNext(pos.pos[0],pos.pos[1],pos.pos[2]);
+        float zAngle = mPathFinder.getZAngleToNext(pos.pos[0],pos.pos[1]);
         MWBase::Environment::get().getWorld()->rotateObject(actor,0,0,zAngle,false);
         MWWorld::Class::get(actor).getMovementSettings(actor).mPosition[1] = 1;
 
@@ -256,7 +256,7 @@ bool MWMechanics::AiWander::execute (const MWWorld::Ptr& actor)
 
         if(distance < 1200 || mPathFinder.checkIfNextPointReached(pos.pos[0],pos.pos[1],pos.pos[2]))
         {
-            stopWalking(actor, mPathFinder);
+            stopWalking(actor);
             mMoveNow = false;
             mWalking = false;
             mChooseAction = true;
@@ -272,10 +272,9 @@ int MWMechanics::AiWander::getTypeId() const
     return 0;
 }
 
-void MWMechanics::AiWander::stopWalking(const MWWorld::Ptr& actor, PathFinder& path)
+void MWMechanics::AiWander::stopWalking(const MWWorld::Ptr& actor)
 {
-    PathFinder pathClearer;
-    path = pathClearer;
+    mPathFinder.clearPath();
     MWWorld::Class::get(actor).getMovementSettings(actor).mPosition[1] = 0;
 }
 
diff --git a/apps/openmw/mwmechanics/aiwander.hpp b/apps/openmw/mwmechanics/aiwander.hpp
index cf3820527c..c82ccc2155 100644
--- a/apps/openmw/mwmechanics/aiwander.hpp
+++ b/apps/openmw/mwmechanics/aiwander.hpp
@@ -22,7 +22,7 @@ namespace MWMechanics
             ///< 0: Wander
 
         private:
-            void stopWalking(const MWWorld::Ptr& actor, PathFinder& path);
+            void stopWalking(const MWWorld::Ptr& actor);
             void playIdle(const MWWorld::Ptr& actor, unsigned short idleSelect);
             bool checkIdle(const MWWorld::Ptr& actor, unsigned short idleSelect);
 
diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp
index e319116f18..3c15b050e9 100644
--- a/apps/openmw/mwmechanics/pathfinding.cpp
+++ b/apps/openmw/mwmechanics/pathfinding.cpp
@@ -31,17 +31,26 @@ namespace
 
     float distanceZCorrected(ESM::Pathgrid::Point point, float x, float y, float z)
     {
-        return sqrt((point.mX - x) * (point.mX - x) + (point.mY - y) * (point.mY - y) + 0.1 * (point.mZ - z) * (point.mZ - z));
+        x -= point.mX;
+        y -= point.mY;
+        z -= point.mZ;
+        return sqrt(x * x + y * y + 0.1 * z * z);
     }
 
     float distance(ESM::Pathgrid::Point point, float x, float y, float z)
     {
-        return sqrt((point.mX - x) * (point.mX - x) + (point.mY - y) * (point.mY - y) + (point.mZ - z) * (point.mZ - z));
+        x -= point.mX;
+        y -= point.mY;
+        z -= point.mZ;
+        return sqrt(x * x + y * y + z * z);
     }
 
     float distance(ESM::Pathgrid::Point a, ESM::Pathgrid::Point b)
     {
-        return sqrt(float(a.mX - b.mX) * (a.mX - b.mX) + (a.mY - b.mY) * (a.mY - b.mY) + (a.mZ - b.mZ) * (a.mZ - b.mZ));
+        float x = a.mX - b.mX;
+        float y = a.mY - b.mY;
+        float z = a.mZ - b.mZ;
+        return sqrt(x * x + y * y + z * z);
     }
 
     static float sgn(float a)
@@ -75,18 +84,18 @@ namespace
     {
         PathGridGraph graph;
 
-        for(unsigned int i = 0; i < pathgrid->mPoints.size(); ++i)
+        for(unsigned int counter = 0; counter < pathgrid->mPoints.size(); counter++)
         {
             PointID pID = boost::add_vertex(graph);
-            graph[pID].mX = pathgrid->mPoints[i].mX + xCell;
-            graph[pID].mY = pathgrid->mPoints[i].mY + yCell;
-            graph[pID].mZ = pathgrid->mPoints[i].mZ;
+            graph[pID].mX = pathgrid->mPoints[counter].mX + xCell;
+            graph[pID].mY = pathgrid->mPoints[counter].mY + yCell;
+            graph[pID].mZ = pathgrid->mPoints[counter].mZ;
         }
 
-        for(unsigned int i = 0;i<pathgrid->mEdges.size();++i)
+        for(unsigned int counterTwo = 0; counterTwo < pathgrid->mEdges.size(); counterTwo++)
         {
-            PointID u = pathgrid->mEdges[i].mV0;
-            PointID v = pathgrid->mEdges[i].mV1;
+            PointID u = pathgrid->mEdges[counterTwo].mV0;
+            PointID v = pathgrid->mEdges[counterTwo].mV1;
 
             PointConnectionID edge;
             bool done;
@@ -145,13 +154,13 @@ namespace MWMechanics
         if(MWBase::Environment::get().getWorld()->castRay(startPoint.mX, startPoint.mY, startPoint.mZ,
             endPoint.mX, endPoint.mY, endPoint.mZ) )
         {
-            int start = getClosestPoint(pathGrid, startPoint.mX - xCell, startPoint.mY - yCell,startPoint.mZ);
-            int end = getClosestPoint(pathGrid, endPoint.mX - xCell, endPoint.mY - yCell, endPoint.mZ);
+            int startNode = getClosestPoint(pathGrid, startPoint.mX - xCell, startPoint.mY - yCell,startPoint.mZ);
+            int endNode = getClosestPoint(pathGrid, endPoint.mX - xCell, endPoint.mY - yCell, endPoint.mZ);
 
-            if(start != -1 && end != -1)
+            if(startNode != -1 && endNode != -1)
             {
                 PathGridGraph graph = buildGraph(pathGrid, xCell, yCell);
-                mPath = findPath(start, end, graph);
+                mPath = findPath(startNode, endNode, graph);
             }
         }
 
@@ -159,10 +168,11 @@ namespace MWMechanics
         mIsPathConstructed = true;
     }
 
-    float PathFinder::getZAngleToNext(float x, float y, float z)
+    float PathFinder::getZAngleToNext(float x, float y)
     {
+        // This if should never be true:
         if(mPath.empty())
-            return 0; // shouldn't happen!
+            return 0;
 
         ESM::Pathgrid::Point nextPoint = *mPath.begin();
         float dX = nextPoint.mX - x;
diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp
index 90c72ebcfe..fcd609c8e4 100644
--- a/apps/openmw/mwmechanics/pathfinding.hpp
+++ b/apps/openmw/mwmechanics/pathfinding.hpp
@@ -12,12 +12,12 @@ namespace MWMechanics
             PathFinder();
 
             void clearPath();
-            void buildPath(ESM::Pathgrid::Point startPoint,ESM::Pathgrid::Point endPoint,
-                const ESM::Pathgrid* pathGrid,float xCell = 0,float yCell = 0);
+            void buildPath(ESM::Pathgrid::Point startPoint, ESM::Pathgrid::Point endPoint,
+                const ESM::Pathgrid* pathGrid, float xCell = 0, float yCell = 0);
 
-            bool checkIfNextPointReached(float x,float y,float z);
+            bool checkIfNextPointReached(float x, float y, float z);
             ///< \Returns true if the last point of the path has been reached.
-            float getZAngleToNext(float x,float y,float z);
+            float getZAngleToNext(float x, float y);
 
             std::list<ESM::Pathgrid::Point> getPath();
             bool isPathConstructed();