From 63424ade56fd9c7da96446d666e4c4e774f1c4c0 Mon Sep 17 00:00:00 2001 From: gus Date: Sun, 31 Mar 2013 17:30:03 +0000 Subject: [PATCH] refactoring --- apps/openmw/CMakeLists.txt | 2 +- apps/openmw/mwmechanics/aitravel.cpp | 177 ++------------------- apps/openmw/mwmechanics/aitravel.hpp | 8 +- apps/openmw/mwmechanics/pathfinding.cpp | 201 ++++++++++++++++++++++++ apps/openmw/mwmechanics/pathfinding.hpp | 26 +++ 5 files changed, 245 insertions(+), 169 deletions(-) create mode 100644 apps/openmw/mwmechanics/pathfinding.cpp create mode 100644 apps/openmw/mwmechanics/pathfinding.hpp diff --git a/apps/openmw/CMakeLists.txt b/apps/openmw/CMakeLists.txt index 41599b35d..d29e0ff2e 100644 --- a/apps/openmw/CMakeLists.txt +++ b/apps/openmw/CMakeLists.txt @@ -65,7 +65,7 @@ add_openmw_dir (mwclass add_openmw_dir (mwmechanics mechanicsmanagerimp stat character creaturestats magiceffects movement actors activators drawstate spells activespells npcstats aipackage aisequence alchemy aiwander aitravel aifollow - aiescort aiactivate repair + aiescort aiactivate repair pathfinding ) add_openmw_dir (mwbase diff --git a/apps/openmw/mwmechanics/aitravel.cpp b/apps/openmw/mwmechanics/aitravel.cpp index 2c5260a62..df6a38bb0 100644 --- a/apps/openmw/mwmechanics/aitravel.cpp +++ b/apps/openmw/mwmechanics/aitravel.cpp @@ -14,7 +14,7 @@ #include "boost/tuple/tuple.hpp" MWMechanics::AiTravel::AiTravel(float x, float y, float z) -: mX(x),mY(y),mZ(z),isPathConstructed(false) + : mX(x),mY(y),mZ(z),mPathFinder() { } @@ -23,151 +23,17 @@ MWMechanics::AiTravel * MWMechanics::AiTravel::clone() const return new AiTravel(*this); } -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)); -} - -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) return -1; - if(grid->mPoints.empty()) return -1; - - float m = distance(grid->mPoints[0],x,y,z); - int i0 = 0; - - for(unsigned 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(); -} - -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,float xCell = 0,float yCell = 0) -{ - PathGridGraph graph; - - for(unsigned int i = 0;imPoints.size();++i) - { - 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; - } - - for(unsigned 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) { const ESM::Pathgrid *pathgrid = MWBase::Environment::get().getWorld()->getStore().get().search(*actor.getCell()->mCell); - + ESM::Position pos = actor.getRefData().getPosition(); bool cellChange = actor.getCell()->mCell->mData.mX != cellX || actor.getCell()->mCell->mData.mY != cellY; @@ -193,7 +59,7 @@ bool MWMechanics::AiTravel::execute (const MWWorld::Ptr& actor) } } - if(!isPathConstructed ||cellChange) + if(!mPathFinder.mIsPathConstructed ||cellChange) { cellX = actor.getCell()->mCell->mData.mX; cellY = actor.getCell()->mCell->mData.mY; @@ -205,43 +71,26 @@ bool MWMechanics::AiTravel::execute (const MWWorld::Ptr& actor) yCell = actor.getCell()->mCell->mData.mY * ESM::Land::REAL_SIZE; } - int start = getClosestPoint(pathgrid,pos.pos[0] - xCell,pos.pos[1] - yCell,pos.pos[2]); - int end = getClosestPoint(pathgrid,mX - xCell,mY - yCell,mZ); - - if(start != -1 && end != -1) - { - PathGridGraph graph = buildGraph(pathgrid,xCell,yCell); - mPath = getPath(start,end,graph); - } - ESM::Pathgrid::Point dest; dest.mX = mX; dest.mY = mY; dest.mZ = mZ; - mPath.push_back(dest); - isPathConstructed = true; + + ESM::Pathgrid::Point start; + dest.mX = pos.pos[0]; + dest.mY = pos.pos[1]; + dest.mZ = pos.pos[2]; + + mPathFinder.findPath(start,dest,pathgrid,xCell,yCell); } - if(mPath.empty()) + if(mPathFinder.checkIfNextPointReached(pos.pos[0],pos.pos[1],pos.pos[2])) { MWWorld::Class::get(actor).getMovementSettings(actor).mForwardBackward = 0; return true; } - ESM::Pathgrid::Point nextPoint = *mPath.begin(); - if(distanceZCorrected(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); + float zAngle = mPathFinder.getZAngleToNext(pos.pos[0],pos.pos[1],pos.pos[2]); + MWBase::Environment::get().getWorld()->rotateObject(actor,0,0,zAngle,false); MWWorld::Class::get(actor).getMovementSettings(actor).mForwardBackward = 1; return false; diff --git a/apps/openmw/mwmechanics/aitravel.hpp b/apps/openmw/mwmechanics/aitravel.hpp index ef2359ba9..52b41850f 100644 --- a/apps/openmw/mwmechanics/aitravel.hpp +++ b/apps/openmw/mwmechanics/aitravel.hpp @@ -2,8 +2,7 @@ #define GAME_MWMECHANICS_AITRAVEL_H #include "aipackage.hpp" -#include -#include +#include "pathfinding.hpp" namespace MWMechanics { @@ -26,8 +25,9 @@ namespace MWMechanics int cellX; int cellY; - bool isPathConstructed; - std::list mPath; + //bool isPathConstructed; + //std::list mPath; + PathFinder mPathFinder; }; } diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp new file mode 100644 index 000000000..9b76082b4 --- /dev/null +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -0,0 +1,201 @@ +#include "pathfinding.hpp" +#include +#include +#include "boost/tuple/tuple.hpp" +#include "OgreMath.h" + +namespace MWMechanics +{ + + //helpers functions + 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)); + } + + 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)); + } + + float sgn(float a) + { + if(a>0) return 1.; + else return -1.; + } + + int getClosestPoint(const ESM::Pathgrid* grid,float x,float y,float z) + { + if(!grid) return -1; + if(grid->mPoints.empty()) return -1; + + float m = distance(grid->mPoints[0],x,y,z); + int i0 = 0; + + for(unsigned int i=1; imPoints.size();++i) + { + if(distance(grid->mPoints[i],x,y,z)mPoints[i],x,y,z); + i0 = i; + } + } + return i0; + } + + 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; + }; + + PathGridGraph buildGraph(const ESM::Pathgrid* pathgrid,float xCell = 0,float yCell = 0) + { + PathGridGraph graph; + + for(unsigned int i = 0;imPoints.size();++i) + { + 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; + } + + for(unsigned 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; + } + + 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; + } + + //end of helpers functions + + PathFinder::PathFinder() + { + mIsPathConstructed = false; + } + + std::list PathFinder::findPath(ESM::Pathgrid::Point startPoint,ESM::Pathgrid::Point endPoint, + const ESM::Pathgrid* pathGrid,float xCell,float yCell) + { + int start = getClosestPoint(pathGrid,startPoint.mX - xCell,startPoint.mY - yCell,startPoint.mZ); + int end = getClosestPoint(pathGrid,endPoint.mX - xCell,endPoint.mY - yCell,endPoint.mZ); + + if(start != -1 && end != -1) + { + PathGridGraph graph = buildGraph(pathGrid,xCell,yCell); + mPath = getPath(start,end,graph); + } + + mPath.push_back(endPoint); + mIsPathConstructed = true; + + return mPath; + } + + float PathFinder::getZAngleToNext(float x,float y,float z) + { + if(mPath.empty()) + { + return 0;/// shouldn't happen! + } + ESM::Pathgrid::Point nextPoint = *mPath.begin(); + float dX = nextPoint.mX - x; + float dY = nextPoint.mY - y; + float h = sqrt(dX*dX+dY*dY); + return Ogre::Radian(acos(dY/h)*sgn(asin(dX/h))).valueDegrees(); + } + + bool PathFinder::checkIfNextPointReached(float x,float y,float z) + { + if(mPath.empty()) + { + return true; + } + ESM::Pathgrid::Point nextPoint = *mPath.begin(); + if(distanceZCorrected(nextPoint,x,y,z) < 20) + { + mPath.pop_front(); + if(mPath.empty()) + { + return true; + } + nextPoint = *mPath.begin(); + } + return false; + } +} \ No newline at end of file diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp new file mode 100644 index 000000000..200b19125 --- /dev/null +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -0,0 +1,26 @@ +#ifndef GAME_MWMECHANICS_PATHFINDING_H +#define GAME_MWMECHANICS_PATHFINDING_H + +#include +#include + +namespace MWMechanics +{ + class PathFinder + { + public: + PathFinder(); + + std::list findPath(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);//returns true if the last point of the path has been reached. + float getZAngleToNext(float x,float y,float z); + + + std::list mPath; + bool mIsPathConstructed; + }; +} + +#endif \ No newline at end of file