diff --git a/apps/openmw/mwmechanics/aicombat.cpp b/apps/openmw/mwmechanics/aicombat.cpp
index bf5968e8c7..529855b2ff 100644
--- a/apps/openmw/mwmechanics/aicombat.cpp
+++ b/apps/openmw/mwmechanics/aicombat.cpp
@@ -55,14 +55,20 @@ namespace MWMechanics
             start.mY = pos.pos[1];
             start.mZ = pos.pos[2];
 
-            std::cout << start.mX << " " << dest.mX << "\n";
-
-            mPathFinder.buildPath(start, dest, pathgrid, xCell, yCell, true);
-
+            if(!mPathFinder.isPathConstructed())
+                mPathFinder.buildPath(start, dest, pathgrid, xCell, yCell, true);
+            else
+            {
+                mPathFinder2.buildPath(start, dest, pathgrid, xCell, yCell, true);
+                ESM::Pathgrid::Point lastPt = mPathFinder.getPath().back();
+                if(mPathFinder2.getPathSize() < mPathFinder.getPathSize() ||
+                   (dest.mX - lastPt.mX)*(dest.mX - lastPt.mX)+(dest.mY - lastPt.mY)*(dest.mY - lastPt.mY)+(dest.mZ - lastPt.mZ)*(dest.mZ - lastPt.mZ) > 200*200)
+                    mPathFinder = mPathFinder2;
+            }
+            
             mPathFinder.checkPathCompleted(pos.pos[0],pos.pos[1],pos.pos[2]);
 
             float zAngle = mPathFinder.getZAngleToNext(pos.pos[0], pos.pos[1]);
-            std::cout << zAngle;
             MWBase::Environment::get().getWorld()->rotateObject(actor, 0, 0, zAngle, false);
             MWWorld::Class::get(actor).getMovementSettings(actor).mPosition[1] = 1;
             
@@ -71,7 +77,9 @@ namespace MWMechanics
             if((dest.mX - start.mX)*(dest.mX - start.mX)+(dest.mY - start.mY)*(dest.mY - start.mY)+(dest.mZ - start.mZ)*(dest.mZ - start.mZ)
                 < range*range)
             {
-                 MWWorld::TimeStamp time = MWBase::Environment::get().getWorld()->getTimeStamp();
+                mPathFinder.clearPath();
+
+                MWWorld::TimeStamp time = MWBase::Environment::get().getWorld()->getTimeStamp();
                 if(mStartingSecond == 0)
                 {
                     MWWorld::Class::get(actor).getCreatureStats(actor).setAttackingOrSpell(false);
diff --git a/apps/openmw/mwmechanics/aicombat.hpp b/apps/openmw/mwmechanics/aicombat.hpp
index bde66a46bc..f61582adc8 100644
--- a/apps/openmw/mwmechanics/aicombat.hpp
+++ b/apps/openmw/mwmechanics/aicombat.hpp
@@ -33,6 +33,7 @@ namespace MWMechanics
             std::string mTargetId;
 
             PathFinder mPathFinder;
+            PathFinder mPathFinder2;
             unsigned int mStartingSecond;
     };
 }
diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp
index 8ef0edab83..ff266f9ae7 100644
--- a/apps/openmw/mwmechanics/pathfinding.cpp
+++ b/apps/openmw/mwmechanics/pathfinding.cpp
@@ -189,7 +189,7 @@ namespace MWMechanics
         // This should never happen (programmers should have an if statement checking mIsPathConstructed that prevents this call
         // if otherwise).
         if(mPath.empty())
-            return 0;
+            return 0.;
 
         const ESM::Pathgrid::Point &nextPoint = *mPath.begin();
         float directionX = nextPoint.mX - x;
@@ -199,6 +199,21 @@ namespace MWMechanics
         return Ogre::Radian(acos(directionY / directionResult) * sgn(asin(directionX / directionResult))).valueDegrees();
     }
 
+    bool PathFinder::checkWaypoint(float x, float y, float z)
+    {
+        if(mPath.empty())
+            return true;
+
+        ESM::Pathgrid::Point nextPoint = *mPath.begin();
+        if(distanceZCorrected(nextPoint, x, y, z) < 64)
+        {
+            mPath.pop_front();
+            if(mPath.empty()) mIsPathConstructed = false;
+            return true;
+        }
+        return false;
+    }
+
     bool PathFinder::checkPathCompleted(float x, float y, float z)
     {
         if(mPath.empty())
diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp
index 35e0fa9087..916df850b5 100644
--- a/apps/openmw/mwmechanics/pathfinding.hpp
+++ b/apps/openmw/mwmechanics/pathfinding.hpp
@@ -18,6 +18,8 @@ namespace MWMechanics
 
             bool checkPathCompleted(float x, float y, float z);
             ///< \Returns true if the last point of the path has been reached.
+            bool checkWaypoint(float x, float y, float z);
+            ///< \Returns true if a way point was reached
             float getZAngleToNext(float x, float y) const;
 
             bool isPathConstructed() const
@@ -25,6 +27,16 @@ namespace MWMechanics
                 return mIsPathConstructed;
             }
 
+            int getPathSize() const
+            {
+                return mPath.size();
+            }
+
+            std::list<ESM::Pathgrid::Point> getPath() const
+            {
+                return mPath;
+            }
+
         private:
             std::list<ESM::Pathgrid::Point> mPath;
             bool mIsPathConstructed;