diff --git a/apps/openmw/mwmechanics/aitravel.cpp b/apps/openmw/mwmechanics/aitravel.cpp index 897dd17480..b5e67e449b 100644 --- a/apps/openmw/mwmechanics/aitravel.cpp +++ b/apps/openmw/mwmechanics/aitravel.cpp @@ -1,8 +1,19 @@ #include "aitravel.hpp" #include +#include "character.hpp" + +#include "../mwworld/class.hpp" +#include "../mwbase/world.hpp" +#include "../mwbase/environment.hpp" +#include "movement.hpp" + +#include +#include +#include "boost/tuple/tuple.hpp" + MWMechanics::AiTravel::AiTravel(float x, float y, float z) -: mX(x),mY(y),mZ(z) +: mX(x),mY(y),mZ(z),isPathConstructed(false) { } @@ -11,10 +22,186 @@ MWMechanics::AiTravel * MWMechanics::AiTravel::clone() const return new AiTravel(*this); } +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)); +} + +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)); +} + +int getClosestPoint(const ESM::Pathgrid* grid,float x,float y,float z) +{ + if(!grid) throw std::exception("NULL PathGrid!"); + if(grid->mPoints.empty()) throw std::exception("empty PathGrid!"); + + float m = distance(grid->mPoints[0],x,y,z); + int i0 = 0; + + for(int i=1; imPoints.size();i++) + { + if(distance(grid->mPoints[i],x,y,z)mPoints[i],x,y,z); + i0 = i; + } + } + + return i0; +} + +float sgn(float a) +{ + if(a>0) return 1.; + else return -1.; +} + +float getZAngle(float dX,float dY) +{ + float h = sqrt(dX*dX+dY*dY); + return Ogre::Radian(acos(dY/h)*sgn(asin(dX/h))).valueDegrees(); +} + +struct Edge +{ + float distance; +}; + +typedef boost::adjacency_list,boost::property > PathGridGraph; +typedef boost::property_map::type WeightMap; +typedef PathGridGraph::vertex_descriptor PointID; +typedef PathGridGraph::edge_descriptor PointConnectionID; + +struct found_path {}; + +class goalVisited : public boost::default_astar_visitor +{ +public: + goalVisited(PointID goal) : mGoal(goal) {} + + void examine_vertex(PointID u, const PathGridGraph g) + { + if(u == mGoal) + throw found_path(); + } +private: + PointID mGoal; +}; + +class DistanceHeuristic : public boost::astar_heuristic +{ +public: + DistanceHeuristic(const PathGridGraph & l, PointID goal) + : mGraph(l), mGoal(goal) {} + + float operator()(PointID u) + { + const ESM::Pathgrid::Point & U = mGraph[u]; + const ESM::Pathgrid::Point & V = mGraph[mGoal]; + float dx = U.mX - V.mX; + float dy = U.mY - V.mY; + float dz = U.mZ - V.mZ; + return sqrt(dx * dx + dy * dy + dz * dz); + } +private: + const PathGridGraph & mGraph; + PointID mGoal; +}; + +std::list getPath(PointID start,PointID end,PathGridGraph graph){ + std::vector p(boost::num_vertices(graph)); + std::vector d(boost::num_vertices(graph)); + std::list shortest_path; + + try { + boost::astar_search + ( + graph, + start, + DistanceHeuristic(graph,end), + boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(goalVisited(end))//.weight_map(boost::get(&Edge::distance, graph)) + ); + + } catch(found_path fg) { + for(PointID v = end;; v = p[v]) { + shortest_path.push_front(graph[v]); + if(p[v] == v) + break; + } + } + return shortest_path; +} + +PathGridGraph buildGraph(const ESM::Pathgrid* pathgrid) +{ + PathGridGraph graph; + + for(int i = 0;imPoints.size();i++) + { + PointID pID = boost::add_vertex(graph); + graph[pID] = pathgrid->mPoints[i]; + } + + for(int i = 0;imEdges.size();i++) + { + PointID u = pathgrid->mEdges[i].mV0; + PointID v = pathgrid->mEdges[i].mV1; + + PointConnectionID edge; + bool done; + 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; +} + bool MWMechanics::AiTravel::execute (const MWWorld::Ptr& actor) { - std::cout << "AiTravel completed.\n"; - return true; + const ESM::Pathgrid *pathgrid = + MWBase::Environment::get().getWorld()->getStore().get().search(*actor.getCell()->mCell); + + ESM::Position pos = actor.getRefData().getPosition(); + + if(!isPathConstructed) + { + int start = getClosestPoint(pathgrid,pos.pos[0],pos.pos[1],pos.pos[2]); + int end = getClosestPoint(pathgrid,mX,mY,mZ); + + PathGridGraph graph = buildGraph(pathgrid); + + mPath = getPath(start,end,graph); + isPathConstructed = true; + } + if(mPath.empty()) + { + MWWorld::Class::get(actor).getMovementSettings(actor).mForwardBackward = 0; + return true; + } + ESM::Pathgrid::Point nextPoint = *mPath.begin(); + if(distance(nextPoint,pos.pos[0],pos.pos[1],pos.pos[2]) < 20) + { + mPath.pop_front(); + if(mPath.empty()) + { + MWWorld::Class::get(actor).getMovementSettings(actor).mForwardBackward = 0; + return true; + } + nextPoint = *mPath.begin(); + } + + float dX = nextPoint.mX - pos.pos[0]; + float dY = nextPoint.mY - pos.pos[1]; + + MWBase::Environment::get().getWorld()->rotateObject(actor,0,0,getZAngle(dX,dY),false); + MWWorld::Class::get(actor).getMovementSettings(actor).mForwardBackward = 1; + return false; + //return true; } int MWMechanics::AiTravel::getTypeId() const diff --git a/apps/openmw/mwmechanics/aitravel.hpp b/apps/openmw/mwmechanics/aitravel.hpp index 1c6abbf279..a596f4c85d 100644 --- a/apps/openmw/mwmechanics/aitravel.hpp +++ b/apps/openmw/mwmechanics/aitravel.hpp @@ -1,7 +1,9 @@ #ifndef GAME_MWMECHANICS_AITRAVEL_H #define GAME_MWMECHANICS_AITRAVEL_H -#include "aipackage.hpp" +#include "aipackage.hpp" +#include "components\esm\loadpgrd.hpp" +#include namespace MWMechanics { @@ -21,6 +23,9 @@ namespace MWMechanics float mY; float mZ; + bool isPathConstructed; + std::list mPath; + }; }