From 7b465ae4f1703f19182538658a58d74462d71b96 Mon Sep 17 00:00:00 2001 From: Torben Carrington Date: Wed, 29 May 2013 17:33:33 -0700 Subject: [PATCH] Pathfinding Overhaul - Even more cleanup and spacing corrections, small renaming (more to come), removed a few unnecessary actions that wasted CPU time and tmp RAM. --- apps/openmw/mwmechanics/pathfinding.cpp | 169 +++++++++--------------- apps/openmw/mwmechanics/pathfinding.hpp | 3 +- 2 files changed, 65 insertions(+), 107 deletions(-) diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index b13bfa7af9..e319116f18 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -10,18 +10,36 @@ namespace { - // helpers functions - float distanceZCorrected(ESM::Pathgrid::Point point,float x,float y,float z) + struct found_path {}; + + typedef boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, + boost::property, boost::property > + PathGridGraph; + typedef boost::property_map::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) { 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) + 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) + 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)); } @@ -30,90 +48,30 @@ namespace { if(a > 0) return 1.0; - else - return -1.0; + return -1.0; } - int getClosestPoint(const ESM::Pathgrid* grid,float x,float y,float z) + int getClosestPoint(const ESM::Pathgrid* grid, float x, float y, float z) { - if(!grid) - return -1; - if(grid->mPoints.empty()) + if(!grid || grid->mPoints.empty()) return -1; - float m = distance(grid->mPoints[0],x,y,z); - int i0 = 0; + float distanceBetween = distance(grid->mPoints[0], x, y, z); + int closestIndex = 0; - for(unsigned int i = 1; i < grid->mPoints.size(); ++i) + for(unsigned int counter = 1; counter < grid->mPoints.size(); counter++) { - if(distance(grid->mPoints[i],x,y,z) < m) + if(distance(grid->mPoints[counter], x, y, z) < distanceBetween) { - m = distance(grid->mPoints[i],x,y,z); - i0 = i; + distanceBetween = distance(grid->mPoints[counter], x, y, z); + closestIndex = counter; } } - return i0; + + return closestIndex; } - 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::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; -};*/ - - 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; - }; - - - PathGridGraph buildGraph(const ESM::Pathgrid* pathgrid,float xCell = 0,float yCell = 0) + PathGridGraph buildGraph(const ESM::Pathgrid* pathgrid, float xCell = 0, float yCell = 0) { PathGridGraph graph; @@ -132,39 +90,38 @@ PointID mGoal; PointConnectionID edge; bool done; - boost::tie(edge,done) = boost::add_edge(u,v,graph); + 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]); + weightmap[edge] = distance(graph[u], graph[v]); } return graph; } - std::list findPath(PointID start,PointID end,PathGridGraph graph){ + std::list findPath(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::dijkstra_shortest_paths - ( - graph, - start, - boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(goalVisited(end))//.weight_map(boost::get(&Edge::distance, graph)) -); + try + { + boost::dijkstra_shortest_paths(graph, start, + boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(goalVisited(end))); + } - } catch(found_path fg) { - for(PointID v = end; ; v = p[v]) { + 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 - } namespace MWMechanics @@ -181,53 +138,53 @@ namespace MWMechanics mIsPathConstructed = false; } - void PathFinder::buildPath(ESM::Pathgrid::Point startPoint,ESM::Pathgrid::Point endPoint, - const ESM::Pathgrid* pathGrid,float xCell,float yCell) + void PathFinder::buildPath(ESM::Pathgrid::Point startPoint, ESM::Pathgrid::Point endPoint, + const ESM::Pathgrid* pathGrid, float xCell, float yCell) { //first check if there is an obstacle - if(MWBase::Environment::get().getWorld()->castRay(startPoint.mX,startPoint.mY,startPoint.mZ, - endPoint.mX,endPoint.mY,endPoint.mZ) ) + if(MWBase::Environment::get().getWorld()->castRay(startPoint.mX, startPoint.mY, startPoint.mZ, + endPoint.mX, endPoint.mY, endPoint.mZ) ) { - int start = getClosestPoint(pathGrid,startPoint.mX - xCell,startPoint.mY - yCell,startPoint.mZ); - int end = getClosestPoint(pathGrid,endPoint.mX - xCell,endPoint.mY - yCell,endPoint.mZ); + 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 = findPath(start,end,graph); + PathGridGraph graph = buildGraph(pathGrid, xCell, yCell); + mPath = findPath(start, end, graph); } } + mPath.push_back(endPoint); mIsPathConstructed = true; } - float PathFinder::getZAngleToNext(float x,float y,float z) + 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; 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) + 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) + if(distanceZCorrected(nextPoint, x, y, z) < 20) { mPath.pop_front(); - if(mPath.empty()) return true; - - nextPoint = *mPath.begin(); } + return false; } diff --git a/apps/openmw/mwmechanics/pathfinding.hpp b/apps/openmw/mwmechanics/pathfinding.hpp index dc380afb41..90c72ebcfe 100644 --- a/apps/openmw/mwmechanics/pathfinding.hpp +++ b/apps/openmw/mwmechanics/pathfinding.hpp @@ -15,7 +15,8 @@ namespace MWMechanics 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. + 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();