diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index 62c825be7..6d9089455 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -3,46 +3,48 @@ #include "../mwbase/world.hpp" #include "../mwbase/environment.hpp" +#include "OgreMath.h" + #include #include -#include "boost/tuple/tuple.hpp" -#include "OgreMath.h" namespace { - //helpers functions +//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)); + 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)); + 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)); + 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)); } static float sgn(float a) { - if(a>0) return 1.; - else return -1.; + if(a > 0) return 1.0; + else return -1.0; } int getClosestPoint(const ESM::Pathgrid* grid,float x,float y,float z) { - if(!grid) return -1; - if(grid->mPoints.empty()) return -1; + 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) + for(unsigned int i = 1; i < grid->mPoints.size(); ++i) { - if(distance(grid->mPoints[i],x,y,z)mPoints[i],x,y,z) < m) { m = distance(grid->mPoints[i],x,y,z); i0 = i; @@ -55,64 +57,64 @@ namespace boost::property,boost::property > PathGridGraph; typedef boost::property_map::type WeightMap; typedef PathGridGraph::vertex_descriptor PointID; - typedef PathGridGraph::edge_descriptor PointConnectionID; + typedef PathGridGraph::edge_descriptor PointConnectionID; struct found_path {}; /*class goalVisited : public boost::default_astar_visitor - { - public: - goalVisited(PointID goal) : mGoal(goal) {} +{ +public: +goalVisited(PointID goal) : mGoal(goal) {} - void examine_vertex(PointID u, const PathGridGraph g) - { - if(u == mGoal) - throw found_path(); - } - private: - PointID mGoal; - }; +void examine_vertex(PointID u, const PathGridGraph g) +{ +if(u == mGoal) +throw found_path(); +} +private: +PointID mGoal; +}; - class DistanceHeuristic : public boost::atasr_heuristic - { - public: - DistanceHeuristic(const PathGridGraph & l, PointID goal) - : mGraph(l), mGoal(goal) {} +class DistanceHeuristic : public boost::atasr_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; - };*/ +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; +};*/ - class goalVisited : public boost::default_dijkstra_visitor - { - public: - goalVisited(PointID goal) : mGoal(goal) {} +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; - }; +void examine_vertex(PointID u, const PathGridGraph g) +{ +if(u == mGoal) +throw found_path(); +} +private: +PointID mGoal; +}; PathGridGraph buildGraph(const ESM::Pathgrid* pathgrid,float xCell = 0,float yCell = 0) { PathGridGraph graph; - for(unsigned int i = 0;imPoints.size();++i) + for(unsigned int i = 0; i < pathgrid->mPoints.size(); ++i) { PointID pID = boost::add_vertex(graph); graph[pID].mX = pathgrid->mPoints[i].mX + xCell; @@ -130,7 +132,6 @@ namespace 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; @@ -147,10 +148,10 @@ namespace graph, start, 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]) { + for(PointID v = end; ; v = p[v]) { shortest_path.push_front(graph[v]); if(p[v] == v) break; @@ -170,6 +171,13 @@ namespace MWMechanics mIsPathConstructed = false; } + void PathFinder::clearPath() + { + if(!mPath.empty()) + mPath.clear(); + mIsPathConstructed = false; + } + void PathFinder::buildPath(ESM::Pathgrid::Point startPoint,ESM::Pathgrid::Point endPoint, const ESM::Pathgrid* pathGrid,float xCell,float yCell) { @@ -193,9 +201,8 @@ namespace MWMechanics float PathFinder::getZAngleToNext(float x,float y,float z) { if(mPath.empty()) - { - return 0;/// shouldn't happen! - } + return 0; /// shouldn't happen! + ESM::Pathgrid::Point nextPoint = *mPath.begin(); float dX = nextPoint.mX - x; float dY = nextPoint.mY - y; @@ -206,17 +213,16 @@ namespace MWMechanics 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; @@ -226,8 +232,10 @@ namespace MWMechanics { return mPath; } + bool PathFinder::isPathConstructed() { return mIsPathConstructed; } -} \ No newline at end of file +} + diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index b1bbab37a..dc380afb4 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -8,22 +8,23 @@ namespace MWMechanics { class PathFinder { - public: - PathFinder(); + public: + PathFinder(); - void buildPath(ESM::Pathgrid::Point startPoint,ESM::Pathgrid::Point endPoint, - const ESM::Pathgrid* pathGrid,float xCell = 0,float yCell = 0); + void clearPath(); + 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);//returns true if the last point of the path has been reached. - float getZAngleToNext(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); - std::list getPath(); - bool isPathConstructed(); + std::list getPath(); + bool isPathConstructed(); - private: - std::list mPath; - bool mIsPathConstructed; + private: + std::list mPath; + bool mIsPathConstructed; }; } -#endif \ No newline at end of file +#endif