Merge remote-tracking branch 'gus/AIFix2'

Conflicts:
	apps/openmw/mwbase/mechanicsmanager.hpp
	apps/openmw/mwmechanics/actors.cpp
	apps/openmw/mwmechanics/actors.hpp
	apps/openmw/mwmechanics/mechanicsmanagerimp.cpp
	apps/openmw/mwmechanics/mechanicsmanagerimp.hpp

apps/openmw/mwbase/mechanicsmanager.hpp

@@ -3,6 +3,7 @@
 
 #include <string>
 #include <vector>
+#include <list>
 
 namespace Ogre
 {
@@ -159,6 +160,9 @@ namespace MWBase
         virtual bool isAIActive() = 0;
 
             virtual void getObjectsInRange (const Ogre::Vector3& position, float radius, std::vector<MWWorld::Ptr>& objects) = 0;
+
+            ///return the list of actors which are following the given actor (ie AiFollow is active and the target is the actor)
+            virtual std::list<MWWorld::Ptr> getActorsFollowing(const MWWorld::Ptr& actor) = 0;
     };
 }
 

apps/openmw/mwmechanics/actors.cpp

@@ -27,6 +27,7 @@
 #include "../mwbase/mechanicsmanager.hpp"
 
 #include "aicombat.hpp"
+#include "aifollow.hpp"
 
 namespace
 {
@@ -927,4 +928,22 @@ namespace MWMechanics
                 out.push_back(iter->first);
         }
     }
+
+    std::list<MWWorld::Ptr> Actors::getActorsFollowing(const MWWorld::Ptr& actor)
+    {
+        std::list<MWWorld::Ptr> list;
+        for(PtrControllerMap::iterator iter(mActors.begin());iter != mActors.end();iter++)
+        {
+            const MWWorld::Class &cls = MWWorld::Class::get(iter->first);
+            CreatureStats &stats = cls.getCreatureStats(iter->first);
+
+            if(stats.getAiSequence().getTypeId() == AiPackage::TypeIdFollow)
+            {
+                MWMechanics::AiFollow* package = static_cast<MWMechanics::AiFollow*>(stats.getAiSequence().getActivePackage());
+                if(package->getFollowedActor() == actor.getCellRef().mRefID)
+                    list.push_front(iter->first);
+            }
+        }
+        return list;
+    }
 }

apps/openmw/mwmechanics/actors.hpp

@@ -96,6 +96,9 @@ namespace MWMechanics
 
             void getObjectsInRange(const Ogre::Vector3& position, float radius, std::vector<MWWorld::Ptr>& out);
 
+            std::list<MWWorld::Ptr> getActorsFollowing(const MWWorld::Ptr& actor);
+            ///<return the list of actors which are following the given actor (ie AiFollow is active and the target is the actor)
+
     private:
         PtrControllerMap mActors;
 

apps/openmw/mwmechanics/aicombat.cpp

@@ -3,7 +3,7 @@
 #include "movement.hpp"
 
 #include "../mwworld/class.hpp"
-#include "../mwworld/timestamp.hpp"
+
 #include "../mwbase/world.hpp"
 #include "../mwbase/environment.hpp"
 #include "../mwbase/mechanicsmanager.hpp"
@@ -54,17 +54,6 @@ namespace MWMechanics
         }
 
         ESM::Position pos = actor.getRefData().getPosition();
-        const ESM::Pathgrid *pathgrid =
-            MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*actor.getCell()->mCell);
-
-        float xCell = 0;
-        float yCell = 0;
-
-        if (actor.getCell()->mCell->isExterior())
-        {
-            xCell = actor.getCell()->mCell->mData.mX * ESM::Land::REAL_SIZE;
-            yCell = actor.getCell()->mCell->mData.mY * ESM::Land::REAL_SIZE;
-        }
 
         ESM::Pathgrid::Point dest;
         dest.mX = target.getRefData().getPosition().pos[0];
@@ -79,15 +68,16 @@ namespace MWMechanics
         mTimer2 = mTimer2 + duration;
 
         if(!mPathFinder.isPathConstructed())
-            mPathFinder.buildPath(start, dest, pathgrid, xCell, yCell, true);
+            mPathFinder.buildPath(start, dest, actor.getCell(), true);
         else
         {
-            mPathFinder2.buildPath(start, dest, pathgrid, xCell, yCell, true);
+            if(mTimer2 > 0.25)
-            ESM::Pathgrid::Point lastPt = mPathFinder.getPath().back();
-            if((mTimer2 > 0.25)&&(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))
             {
                 mTimer2 = 0;
+                mPathFinder2.buildPath(start, dest, actor.getCell(), 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;
             }
         }
@@ -112,7 +102,8 @@ namespace MWMechanics
 
             zAngle = Ogre::Radian( Ogre::Math::ACos(directionY / directionResult) * sgn(Ogre::Math::ASin(directionX / directionResult)) ).valueDegrees();
             // TODO: use movement settings instead of rotating directly
-            MWBase::Environment::get().getWorld()->rotateObject(actor, 0, 0, zAngle, false);
+            //MWBase::Environment::get().getWorld()->rotateObject(actor, 0, 0, zAngle, false);
+            MWWorld::Class::get(actor).getMovementSettings(actor).mRotation[2] = 10*(Ogre::Degree(zAngle).valueRadians()-pos.rot[2]);
 
             mPathFinder.clearPath();
 

apps/openmw/mwmechanics/aiescort.cpp

@@ -28,8 +28,8 @@ namespace MWMechanics
 {
     AiEscort::AiEscort(const std::string &actorId, int duration, float x, float y, float z)
     : mActorId(actorId), mX(x), mY(y), mZ(z), mDuration(duration)
-    , cellX(std::numeric_limits<int>::max())
+    , mCellX(std::numeric_limits<int>::max())
-    , cellY(std::numeric_limits<int>::max())
+    , mCellY(std::numeric_limits<int>::max())
     {
         mMaxDist = 470;
 
@@ -47,8 +47,8 @@ namespace MWMechanics
 
     AiEscort::AiEscort(const std::string &actorId, const std::string &cellId,int duration, float x, float y, float z)
     : mActorId(actorId), mCellId(cellId), mX(x), mY(y), mZ(z), mDuration(duration)
-    , cellX(std::numeric_limits<int>::max())
+    , mCellX(std::numeric_limits<int>::max())
-    , cellY(std::numeric_limits<int>::max())
+    , mCellY(std::numeric_limits<int>::max())
     {
         mMaxDist = 470;
 
@@ -84,9 +84,7 @@ namespace MWMechanics
 
         MWWorld::Ptr player = MWBase::Environment::get().getWorld()->getPlayerPtr();
         ESM::Position pos = actor.getRefData().getPosition();
-        bool cellChange = actor.getCell()->mCell->mData.mX != cellX || actor.getCell()->mCell->mData.mY != cellY;
+        bool cellChange = actor.getCell()->mCell->mData.mX != mCellX || actor.getCell()->mCell->mData.mY != mCellY;
-        const ESM::Pathgrid *pathgrid =
-            MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*actor.getCell()->mCell);
 
         if(actor.getCell()->mCell->mData.mX != player.getCell()->mCell->mData.mX)
         {
@@ -116,15 +114,8 @@ namespace MWMechanics
 
         if(!mPathFinder.isPathConstructed() || cellChange)
         {
-            cellX = actor.getCell()->mCell->mData.mX;
+            mCellX = actor.getCell()->mCell->mData.mX;
-            cellY = actor.getCell()->mCell->mData.mY;
+            mCellY = actor.getCell()->mCell->mData.mY;
-            float xCell = 0;
-            float yCell = 0;
-            if (actor.getCell()->mCell->isExterior())
-            {
-                xCell = actor.getCell()->mCell->mData.mX * ESM::Land::REAL_SIZE;
-                yCell = actor.getCell()->mCell->mData.mY * ESM::Land::REAL_SIZE;
-            }
 
             ESM::Pathgrid::Point dest;
             dest.mX = mX;
@@ -136,7 +127,7 @@ namespace MWMechanics
             start.mY = pos.pos[1];
             start.mZ = pos.pos[2];
 
-            mPathFinder.buildPath(start, dest, pathgrid, xCell, yCell, true);
+            mPathFinder.buildPath(start, dest, actor.getCell(), true);
         }
 
         if(mPathFinder.checkPathCompleted(pos.pos[0],pos.pos[1],pos.pos[2]))

apps/openmw/mwmechanics/aiescort.hpp

@@ -34,8 +34,8 @@ namespace MWMechanics
             unsigned int mDuration;
 
             PathFinder mPathFinder;
-            int cellX;
+            int mCellX;
-            int cellY;
+            int mCellY;
     };
 }
 #endif

apps/openmw/mwmechanics/aifollow.cpp

@@ -1,26 +1,119 @@
 #include "aifollow.hpp"
 #include <iostream>
+#include "../mwbase/world.hpp"
+#include "../mwbase/environment.hpp"
+#include "../mwworld/class.hpp"
+#include "movement.hpp"
+
+#include <OgreMath.h>
 
 MWMechanics::AiFollow::AiFollow(const std::string &actorId,float duration, float x, float y, float z)
-: mDuration(duration), mX(x), mY(y), mZ(z), mActorId(actorId)
+: mDuration(duration), mX(x), mY(y), mZ(z), mActorId(actorId), mCellId(""), mTimer(0), mStuckTimer(0)
 {
 }
 MWMechanics::AiFollow::AiFollow(const std::string &actorId,const std::string &cellId,float duration, float x, float y, float z)
-: mDuration(duration), mX(x), mY(y), mZ(z), mActorId(actorId), mCellId(cellId)
+: mDuration(duration), mX(x), mY(y), mZ(z), mActorId(actorId), mCellId(cellId), mTimer(0), mStuckTimer(0)
 {
 }
 
+bool MWMechanics::AiFollow::execute (const MWWorld::Ptr& actor,float duration)
+{
+    const MWWorld::Ptr target = MWBase::Environment::get().getWorld()->getPtr(mActorId, false);
+
+    mTimer = mTimer + duration;
+    mStuckTimer = mStuckTimer + duration;
+    mTotalTime = mTotalTime + duration;
+
+    ESM::Position pos = actor.getRefData().getPosition();
+
+    if(mTotalTime > mDuration && mDuration != 0)
+        return true;
+
+    if((pos.pos[0]-mX)*(pos.pos[0]-mX) +
+        (pos.pos[1]-mY)*(pos.pos[1]-mY) +
+        (pos.pos[2]-mZ)*(pos.pos[2]-mZ) < 100*100) 
+    {
+        if(actor.getCell()->isExterior())
+        {
+            if(mCellId == "") 
+                return true;
+        }
+        else
+        {
+            if(mCellId == actor.getCell()->mCell->mName)
+                return true;
+        }
+    }
+
+    ESM::Pathgrid::Point dest;
+    dest.mX = target.getRefData().getPosition().pos[0];
+    dest.mY = target.getRefData().getPosition().pos[1];
+    dest.mZ = target.getRefData().getPosition().pos[2];
+
+    ESM::Pathgrid::Point start;
+    start.mX = pos.pos[0];
+    start.mY = pos.pos[1];
+    start.mZ = pos.pos[2];
+
+    if(mPathFinder.getPath().empty())
+        mPathFinder.buildPath(start, dest, actor.getCell(), true);
+
+
+    if(mTimer > 0.25)
+    {
+        if(!mPathFinder.getPath().empty())
+        {
+            ESM::Pathgrid::Point lastPos = mPathFinder.getPath().back();
+
+            if((dest.mX - lastPos.mX)*(dest.mX - lastPos.mX) 
+                +(dest.mY - lastPos.mY)*(dest.mY - lastPos.mY)
+                +(dest.mZ - lastPos.mZ)*(dest.mZ - lastPos.mZ)
+            > 100*100)
+                mPathFinder.addPointToPath(dest);
+        }
+
+        mTimer = 0;
+    }
+
+    if(mStuckTimer>0.5)
+    {
+        if((mStuckPos.pos[0] - pos.pos[0])*(mStuckPos.pos[0] - pos.pos[0])
+            +(mStuckPos.pos[1] - pos.pos[1])*(mStuckPos.pos[1] - pos.pos[1])
+            +(mStuckPos.pos[2] - pos.pos[2])*(mStuckPos.pos[2] - pos.pos[2]) < 100) //NPC is stuck
+            mPathFinder.buildPath(start, dest, actor.getCell(), true);
+
+        mStuckTimer = 0;
+        mStuckPos = pos;
+    }
+
+    if(!mPathFinder.checkPathCompleted(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).mRotation[2] = 10*(Ogre::Degree(zAngle).valueRadians()-pos.rot[2]);
+        //std::cout << Ogre::Degree(zAngle).valueDegrees()-Ogre::Radian(actor.getRefData().getPosition().rot[2]).valueDegrees() << " "<< pos.rot[2] << " " << zAngle << "\n";
+        //MWWorld::Class::get(actor).get
+    }
+
+    if((dest.mX - pos.pos[0])*(dest.mX - pos.pos[0])+(dest.mY - pos.pos[1])*(dest.mY - pos.pos[1])+(dest.mZ - pos.pos[2])*(dest.mZ - pos.pos[2])
+        < 100*100)
+        MWWorld::Class::get(actor).getMovementSettings(actor).mPosition[1] = 0;
+    else
+        MWWorld::Class::get(actor).getMovementSettings(actor).mPosition[1] = 1;
+
+    return false;
+}
+
+std::string MWMechanics::AiFollow::getFollowedActor()
+{
+    return mActorId;
+}
+
 MWMechanics::AiFollow *MWMechanics::AiFollow::clone() const
 {
     return new AiFollow(*this);
 }
 
- bool MWMechanics::AiFollow::execute (const MWWorld::Ptr& actor,float duration)
-{
-    std::cout << "AiFollow completed.\n";
-    return true;
-}
-
  int MWMechanics::AiFollow::getTypeId() const
 {
     return TypeIdFollow;

apps/openmw/mwmechanics/aifollow.hpp

@@ -3,6 +3,8 @@
 
 #include "aipackage.hpp"
 #include <string>
+#include "pathfinding.hpp"
+#include "../../../components/esm/defs.hpp"
 
 namespace MWMechanics
 {
@@ -17,6 +19,8 @@ namespace MWMechanics
                     ///< \return Package completed?
             virtual int getTypeId() const;
 
+            std::string getFollowedActor();
+
         private:
             float mDuration;
             float mX;
@@ -24,6 +28,14 @@ namespace MWMechanics
             float mZ;
             std::string mActorId;
             std::string mCellId;
+
+            float mTimer;
+            float mStuckTimer;
+            float mTotalTime;
+
+            ESM::Position mStuckPos;
+
+            PathFinder mPathFinder;
     };
 }
 #endif

apps/openmw/mwmechanics/aisequence.cpp

@@ -124,6 +124,14 @@ void MWMechanics::AiSequence::queue (const AiPackage& package)
     mPackages.push_back (package.clone());
 }
 
+MWMechanics::AiPackage* MWMechanics::AiSequence::getActivePackage()
+{
+    if(mPackages.empty())
+        throw std::runtime_error(std::string("No AI Package!"));
+    else
+        return mPackages.front();
+}
+
 void MWMechanics::AiSequence::fill(const ESM::AIPackageList &list)
 {
     for (std::vector<ESM::AIPackage>::const_iterator it = list.mList.begin(); it != list.mList.end(); ++it)

apps/openmw/mwmechanics/aisequence.hpp

@@ -59,6 +59,9 @@ namespace MWMechanics
             ///< Add \a package to the end of the sequence (executed after all other packages have been
             /// completed)
 
+            AiPackage* getActivePackage();
+            ///< return the current active package. If there is no active package, throw an exeption
+
             void fill (const ESM::AIPackageList& list);
     };
 }

apps/openmw/mwmechanics/aitravel.cpp

@@ -61,7 +61,6 @@ namespace MWMechanics
             }
         }
 
-        const ESM::Pathgrid *pathgrid = world->getStore().get<ESM::Pathgrid>().search(*cell);
         bool cellChange = cell->mData.mX != cellX || cell->mData.mY != cellY;
         if(!mPathFinder.isPathConstructed() || cellChange)
         {
@@ -86,7 +85,7 @@ namespace MWMechanics
             start.mY = pos.pos[1];
             start.mZ = pos.pos[2];
 
-            mPathFinder.buildPath(start, dest, pathgrid, xCell, yCell, true);
+            mPathFinder.buildPath(start, dest, actor.getCell(), true);
         }
 
         if(mPathFinder.checkPathCompleted(pos.pos[0], pos.pos[1], pos.pos[2]))

apps/openmw/mwmechanics/aiwander.cpp

@@ -260,7 +260,7 @@ namespace MWMechanics
                 start.mY = pos.pos[1];
                 start.mZ = pos.pos[2];
 
-                mPathFinder.buildPath(start, dest, mPathgrid, mXCell, mYCell, false);
+                mPathFinder.buildPath(start, dest, actor.getCell(), false);
 
                 if(mPathFinder.isPathConstructed())
                 {

apps/openmw/mwmechanics/mechanicsmanagerimp.cpp

@@ -956,4 +956,9 @@ namespace MWMechanics
         mActors.getObjectsInRange(position, radius, objects);
         mObjects.getObjectsInRange(position, radius, objects);
     }
+
+    std::list<MWWorld::Ptr> MechanicsManager::getActorsFollowing(const MWWorld::Ptr& actor)
+    {
+        return mActors.getActorsFollowing(actor);
+    }
 }

apps/openmw/mwmechanics/mechanicsmanagerimp.hpp

@@ -137,6 +137,8 @@ namespace MWMechanics
         virtual bool isAIActive();
 
         virtual void getObjectsInRange (const Ogre::Vector3& position, float radius, std::vector<MWWorld::Ptr>& objects);
+
+        virtual std::list<MWWorld::Ptr> getActorsFollowing(const MWWorld::Ptr& actor);
     };
 }
 

apps/openmw/mwmechanics/pathfinding.cpp

@@ -5,30 +5,10 @@
 
 #include "OgreMath.h"
 
-#include <boost/graph/dijkstra_shortest_paths.hpp>
+#include <map>
-#include <boost/graph/adjacency_list.hpp>
 
 namespace
 {
-    struct found_path {};
-
-    typedef boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS,
-        boost::property<boost::vertex_index_t, int, ESM::Pathgrid::Point>, boost::property<boost::edge_weight_t, float> >
-        PathGridGraph;
-    typedef boost::property_map<PathGridGraph, boost::edge_weight_t>::type WeightMap;
-    typedef PathGridGraph::vertex_descriptor PointID;
-    typedef PathGridGraph::edge_descriptor PointConnectionID;
-
-    class goalVisited : public boost::default_dijkstra_visitor
-    {
-        public:
-            goalVisited(PointID goal) {mGoal = goal;};
-            void examine_vertex(PointID u, const PathGridGraph g) {if(u == mGoal) throw found_path();};
-
-        private:
-            PointID mGoal;
-    };
-
     float distanceZCorrected(ESM::Pathgrid::Point point, float x, float y, float z)
     {
         x -= point.mX;
@@ -80,64 +60,104 @@ namespace
         return closestIndex;
     }
 
-    PathGridGraph buildGraph(const ESM::Pathgrid* pathgrid, float xCell = 0, float yCell = 0)
+    /*std::list<ESM::Pathgrid::Point> reconstructPath(const std::vector<MWMechanics::PathFinder::Node>& graph,const ESM::Pathgrid* pathgrid, int lastNode,float xCell, float yCell)
     {
-        PathGridGraph graph;
+        std::list<ESM::Pathgrid::Point> path;
+        while(graph[lastNode].parent != -1)
+        {
+            //std::cout << "not empty" << xCell;
+            ESM::Pathgrid::Point pt = pathgrid->mPoints[lastNode];
+            pt.mX += xCell;
+            pt.mY += yCell;
+            path.push_front(pt);
+            lastNode = graph[lastNode].parent;
+        }
+        return path;
+    }*/
 
-        for(unsigned int counter = 0; counter < pathgrid->mPoints.size(); counter++)
+
+
+    /*std::list<ESM::Pathgrid::Point> buildPath2(const ESM::Pathgrid* pathgrid,int start,int goal,float xCell = 0, float yCell = 0)
     {
-            PointID pID = boost::add_vertex(graph);
+        std::vector<Node> graph;
-            graph[pID].mX = pathgrid->mPoints[counter].mX + xCell;
+        for(unsigned int i = 0; i < pathgrid->mPoints.size(); i++)
-            graph[pID].mY = pathgrid->mPoints[counter].mY + yCell;
+        {
-            graph[pID].mZ = pathgrid->mPoints[counter].mZ;
+            Node node;
+            node.label = i;
+            node.parent = -1;
+            graph.push_back(node);
+        }
+        for(unsigned int i = 0; i < pathgrid->mEdges.size(); i++)
+        {
+            Edge edge;
+            edge.destination = pathgrid->mEdges[i].mV1;
+            edge.cost = distance(pathgrid->mPoints[pathgrid->mEdges[i].mV0],pathgrid->mPoints[pathgrid->mEdges[i].mV1]);
+            graph[pathgrid->mEdges[i].mV0].edges.push_back(edge);
+            edge.destination = pathgrid->mEdges[i].mV0;
+            graph[pathgrid->mEdges[i].mV1].edges.push_back(edge);
         }
 
-        for(unsigned int counterTwo = 0; counterTwo < pathgrid->mEdges.size(); counterTwo++)
+        std::vector<float> g_score(pathgrid->mPoints.size(),-1.);
-        {
+        std::vector<float> f_score(pathgrid->mPoints.size(),-1.);
-            PointID u = pathgrid->mEdges[counterTwo].mV0;
-            PointID v = pathgrid->mEdges[counterTwo].mV1;
 
-            PointConnectionID edge;
+        g_score[start] = 0;
-            bool done;
+        f_score[start] = distance(pathgrid->mPoints[start],pathgrid->mPoints[goal]);
-            boost::tie(edge, done) = boost::add_edge(u, v, graph);
-            WeightMap weightmap = boost::get(boost::edge_weight, graph);
-            weightmap[edge] = distance(graph[u], graph[v]);
-        }
 
-        return graph;
+        std::list<int> openset;
-    }
+        std::list<int> closedset;
+        openset.push_back(start);
 
-    std::list<ESM::Pathgrid::Point> findPath(PointID start, PointID end, PathGridGraph graph)
+        int current = -1;
-    {
-        std::vector<PointID> p(boost::num_vertices(graph));
-        std::vector<float> d(boost::num_vertices(graph));
-        std::list<ESM::Pathgrid::Point> shortest_path;
 
-        try
+        while(!openset.empty())
         {
-            boost::dijkstra_shortest_paths(graph, start,
+            current = openset.front();
-                boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(goalVisited(end)));
+            openset.pop_front();
-        }
 
-        catch(found_path& fg)
+            if(current == goal) break;
+
+            closedset.push_back(current);
+            
+            for(int j = 0;j<graph[current].edges.size();j++)
             {
-            for(PointID v = end; ; v = p[v])
+                //int next = graph[current].edges[j].destination
+                if(std::find(closedset.begin(),closedset.end(),graph[current].edges[j].destination) == closedset.end())
                 {
-                shortest_path.push_front(graph[v]);
+                    int dest = graph[current].edges[j].destination;
-                if(p[v] == v)
+                    float tentative_g = g_score[current] + graph[current].edges[j].cost;
+                    bool isInOpenSet = std::find(openset.begin(),openset.end(),dest) != openset.end();
+                    if(!isInOpenSet
+                        || tentative_g < g_score[dest] )
+                    {
+                        graph[dest].parent = current;
+                        g_score[dest] = tentative_g;
+                        f_score[dest] = tentative_g + distance(pathgrid->mPoints[dest],pathgrid->mPoints[goal]);
+                        if(!isInOpenSet)
+                        {
+                            std::list<int>::iterator it = openset.begin();
+                            for(it = openset.begin();it!= openset.end();it++)
+                            {
+                                if(g_score[*it]>g_score[dest])
                                     break;
                             }
+                            openset.insert(it,dest);
+                        }
+                    }
+                }
             }
 
-        return shortest_path;
         }
+        return reconstructPath(graph,pathgrid,current,xCell,yCell);
+
+    }*/
+
 }
 
 namespace MWMechanics
 {
     PathFinder::PathFinder()
+        :mIsPathConstructed(false),mIsGraphConstructed(false)
     {
-        mIsPathConstructed = false;
     }
 
     void PathFinder::clearPath()
@@ -147,10 +167,124 @@ namespace MWMechanics
         mIsPathConstructed = false;
     }
 
+    void PathFinder::buildPathgridGraph(const ESM::Pathgrid* pathGrid)
+    {
+        mGraph.clear();
+        mGScore.resize(pathGrid->mPoints.size(),-1);
+        mFScore.resize(pathGrid->mPoints.size(),-1);
+        Node defaultNode;
+        defaultNode.label = -1;
+        defaultNode.parent = -1;
+        mGraph.resize(pathGrid->mPoints.size(),defaultNode);
+        for(unsigned int i = 0; i < pathGrid->mPoints.size(); i++)
+        {
+            Node node;
+            node.label = i;
+            node.parent = -1;
+            mGraph[i] = node;
+        }
+        for(unsigned int i = 0; i < pathGrid->mEdges.size(); i++)
+        {
+            Edge edge;
+            edge.destination = pathGrid->mEdges[i].mV1;
+            edge.cost = distance(pathGrid->mPoints[pathGrid->mEdges[i].mV0],pathGrid->mPoints[pathGrid->mEdges[i].mV1]);
+            mGraph[pathGrid->mEdges[i].mV0].edges.push_back(edge);
+            edge.destination = pathGrid->mEdges[i].mV0;
+            mGraph[pathGrid->mEdges[i].mV1].edges.push_back(edge);
+        }
+        mIsGraphConstructed = true;
+    }
+
+    void PathFinder::cleanUpAStar()
+    {
+        for(int i=0;i<mGraph.size();i++)
+        {
+            mGraph[i].parent = -1;
+            mGScore[i] = -1;
+            mFScore[i] = -1;
+        }
+    }
+
+    std::list<ESM::Pathgrid::Point> PathFinder::aStarSearch(const ESM::Pathgrid* pathGrid,int start,int goal,float xCell, float yCell)
+    {
+        cleanUpAStar();
+        mGScore[start] = 0;
+        mFScore[start] = distance(pathGrid->mPoints[start],pathGrid->mPoints[goal]);
+
+        std::list<int> openset;
+        std::list<int> closedset;
+        openset.push_back(start);
+
+        int current = -1;
+
+        while(!openset.empty())
+        {
+            current = openset.front();
+            openset.pop_front();
+
+            if(current == goal) break;
+
+            closedset.push_back(current);
+            
+            for(int j = 0;j<mGraph[current].edges.size();j++)
+            {
+                //int next = mGraph[current].edges[j].destination
+                if(std::find(closedset.begin(),closedset.end(),mGraph[current].edges[j].destination) == closedset.end())
+                {
+                    int dest = mGraph[current].edges[j].destination;
+                    float tentative_g = mGScore[current] + mGraph[current].edges[j].cost;
+                    bool isInOpenSet = std::find(openset.begin(),openset.end(),dest) != openset.end();
+                    if(!isInOpenSet
+                        || tentative_g < mGScore[dest] )
+                    {
+                        mGraph[dest].parent = current;
+                        mGScore[dest] = tentative_g;
+                        mFScore[dest] = tentative_g + distance(pathGrid->mPoints[dest],pathGrid->mPoints[goal]);
+                        if(!isInOpenSet)
+                        {
+                            std::list<int>::iterator it = openset.begin();
+                            for(it = openset.begin();it!= openset.end();it++)
+                            {
+                                if(mGScore[*it]>mGScore[dest])
+                                    break;
+                            }
+                            openset.insert(it,dest);
+                        }
+                    }
+                }
+            }
+
+        }
+
+        std::list<ESM::Pathgrid::Point> path;
+        while(mGraph[current].parent != -1)
+        {
+            //std::cout << "not empty" << xCell;
+            ESM::Pathgrid::Point pt = pathGrid->mPoints[current];
+            pt.mX += xCell;
+            pt.mY += yCell;
+            path.push_front(pt);
+            current = mGraph[current].parent;
+        }
+
+        if(path.empty())
+        {
+            ESM::Pathgrid::Point pt = pathGrid->mPoints[goal];
+            pt.mX += xCell;
+            pt.mY += yCell;
+            path.push_front(pt);
+        }
+
+        return path;
+    }
+
     void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint,
-                               const ESM::Pathgrid *pathGrid, float xCell, float yCell, bool allowShortcuts)
+                               const MWWorld::CellStore* cell, bool allowShortcuts)
     {
         mPath.clear();
+        if(mCell != cell) mIsGraphConstructed = false;
+        mCell = cell;
+
         if(allowShortcuts)
         {
             if(MWBase::Environment::get().getWorld()->castRay(startPoint.mX, startPoint.mY, startPoint.mZ,
@@ -160,13 +294,24 @@ namespace MWMechanics
 
         if(!allowShortcuts)
         {
+            const ESM::Pathgrid *pathGrid =
+                MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*mCell->mCell);
+            float xCell = 0;
+            float yCell = 0;
+
+            if (mCell->isExterior())
+            {
+                xCell = mCell->mCell->mData.mX * ESM::Land::REAL_SIZE;
+                yCell = mCell->mCell->mData.mY * ESM::Land::REAL_SIZE;
+            }
             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(startNode != -1 && endNode != -1)
             {
-                PathGridGraph graph = buildGraph(pathGrid, xCell, yCell);
+                if(!mIsGraphConstructed) buildPathgridGraph(pathGrid);
-                mPath = findPath(startNode, endNode, graph);
+
+                mPath = aStarSearch(pathGrid,startNode,endNode,xCell,yCell);//findPath(startNode, endNode, mGraph);
 
                 if(!mPath.empty())
                 {

apps/openmw/mwmechanics/pathfinding.hpp

@@ -3,6 +3,12 @@
 
 #include <components/esm/loadpgrd.hpp>
 #include <list>
+#include <boost/graph/adjacency_list.hpp>
+
+namespace MWWorld
+{
+    class CellStore;
+}
 
 namespace MWMechanics
 {
@@ -12,9 +18,11 @@ namespace MWMechanics
             PathFinder();
 
             void clearPath();
+
+            void buildPathgridGraph(const ESM::Pathgrid* pathGrid);
+
             void buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint,
-                           const ESM::Pathgrid* pathGrid, float xCell = 0, float yCell = 0,
+                           const MWWorld::CellStore* cell, bool allowShortcuts = true);
-                           bool allowShortcuts = true);
 
             bool checkPathCompleted(float x, float y, float z);
             ///< \Returns true if the last point of the path has been reached.
@@ -37,9 +45,38 @@ namespace MWMechanics
                 return mPath;
             }
 
+            void addPointToPath(ESM::Pathgrid::Point &point)
+            {
+                mPath.push_back(point);
+            }
+
         private:
-            std::list<ESM::Pathgrid::Point> mPath;
+
+            struct Edge
+            {
+                int destination;
+                float cost;
+            };
+            struct Node
+            {
+                int label;
+                std::vector<Edge> edges;
+                int parent;//used in pathfinding
+            };
+
+            std::vector<float> mGScore;
+            std::vector<float> mFScore;
+
+            std::list<ESM::Pathgrid::Point> aStarSearch(const ESM::Pathgrid* pathGrid,int start,int goal,float xCell = 0, float yCell = 0);
+            void cleanUpAStar();
+
+            std::vector<Node> mGraph;
             bool mIsPathConstructed;
+
+
+            std::list<ESM::Pathgrid::Point> mPath;
+            bool mIsGraphConstructed;
+            const MWWorld::CellStore* mCell;
     };
 }
 

apps/openmw/mwworld/actionteleport.cpp

@@ -3,6 +3,7 @@
 
 #include "../mwbase/environment.hpp"
 #include "../mwbase/world.hpp"
+#include "../mwbase/mechanicsmanager.hpp"
 #include "player.hpp"
 
 namespace MWWorld
@@ -16,11 +17,35 @@ namespace MWWorld
     void ActionTeleport::executeImp (const Ptr& actor)
     {
         MWBase::World* world = MWBase::Environment::get().getWorld();
-        world->getPlayer().setTeleported(true);
 
+        //find any NPC that is following the actor and teleport him too
+        std::list<MWWorld::Ptr> followers = MWBase::Environment::get().getMechanicsManager()->getActorsFollowing(actor);
+        for(std::list<MWWorld::Ptr>::iterator it = followers.begin();it != followers.end();it++)
+        {
+            std::cout << "teleporting someone!" << (*it).getCellRef().mRefID;
+            executeImp(*it);
+        }
+
+        if(actor == world->getPlayerPtr())
+        {
+            world->getPlayer().setTeleported(true);
             if (mCellName.empty())
                 world->changeToExteriorCell (mPosition);
             else
                 world->changeToInteriorCell (mCellName, mPosition);
         }
+        else
+        {
+            if (mCellName.empty())
+            {
+                int cellX;
+                int cellY;
+                world->positionToIndex(mPosition.pos[0],mPosition.pos[1],cellX,cellY);
+                world->moveObject(actor,*world->getExterior(cellX,cellY),
+                    mPosition.pos[0],mPosition.pos[1],mPosition.pos[2]);
+            }
+            else
+                world->moveObject(actor,*world->getInterior(mCellName),mPosition.pos[0],mPosition.pos[1],mPosition.pos[2]);
+        }
+    }
 }