X-Git-Url: https://git.lizzy.rs/?a=blobdiff_plain;f=src%2Fpathfinder.cpp;h=3f0b98c1074ce35dbf54eb5719088318dd4a7726;hb=b6b80f55c8a2bf4eae440108b3274f2f921e3a94;hp=90fc4ecdaab41690ecb0082a58bd96d81f5e54aa;hpb=46e5ef4e9a19a162c8baee15d314233c0bb0e331;p=minetest.git diff --git a/src/pathfinder.cpp b/src/pathfinder.cpp index 90fc4ecda..3f0b98c10 100644 --- a/src/pathfinder.cpp +++ b/src/pathfinder.cpp @@ -1,6 +1,7 @@ /* Minetest Copyright (C) 2013 sapier, sapier at gmx dot net +Copyright (C) 2016 est31, This program is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by @@ -22,9 +23,8 @@ with this program; if not, write to the Free Software Foundation, Inc., /******************************************************************************/ #include "pathfinder.h" -#include "environment.h" #include "map.h" -#include "log.h" +#include "nodedef.h" //#define PATHFINDER_DEBUG //#define PATHFINDER_CALC_TIME @@ -43,9 +43,6 @@ with this program; if not, write to the Free Software Foundation, Inc., /* Typedefs and macros */ /******************************************************************************/ -/** shortcut to print a 3d pos */ -#define PPOS(pos) "(" << pos.X << "," << pos.Y << "," << pos.Z << ")" - #define LVL "(" << level << ")" << #ifdef PATHFINDER_DEBUG @@ -55,88 +52,137 @@ with this program; if not, write to the Free Software Foundation, Inc., #define ERROR_TARGET std::cout #else #define DEBUG_OUT(a) while(0) -#define INFO_TARGET infostream << "pathfinder: " -#define VERBOSE_TARGET verbosestream << "pathfinder: " -#define ERROR_TARGET errorstream << "pathfinder: " +#define INFO_TARGET infostream << "Pathfinder: " +#define VERBOSE_TARGET verbosestream << "Pathfinder: " +#define ERROR_TARGET warningstream << "Pathfinder: " #endif +#define PATHFINDER_MAX_WAYPOINTS 700 + /******************************************************************************/ /* Class definitions */ /******************************************************************************/ /** representation of cost in specific direction */ -class path_cost { +class PathCost { public: /** default constructor */ - path_cost(); + PathCost() = default; /** copy constructor */ - path_cost(const path_cost& b); + PathCost(const PathCost &b); /** assignment operator */ - path_cost& operator= (const path_cost& b); + PathCost &operator= (const PathCost &b); - bool valid; /**< movement is possible */ - int value; /**< cost of movement */ - int direction; /**< y-direction of movement */ - bool updated; /**< this cost has ben calculated */ + bool valid = false; /**< movement is possible */ + int value = 0; /**< cost of movement */ + int y_change = 0; /**< change of y position of movement */ + bool updated = false; /**< this cost has ben calculated */ }; /** representation of a mapnode to be used for pathfinding */ -class path_gridnode { +class PathGridnode { public: /** default constructor */ - path_gridnode(); + PathGridnode() = default; /** copy constructor */ - path_gridnode(const path_gridnode& b); + PathGridnode(const PathGridnode &b); /** * assignment operator * @param b node to copy */ - path_gridnode& operator= (const path_gridnode& b); + PathGridnode &operator= (const PathGridnode &b); /** * read cost in a specific direction * @param dir direction of cost to fetch */ - path_cost get_cost(v3s16 dir); + PathCost getCost(v3s16 dir); /** * set cost value for movement * @param dir direction to set cost for * @cost cost to set */ - void set_cost(v3s16 dir,path_cost cost); - - bool valid; /**< node is on surface */ - bool target; /**< node is target position */ - bool source; /**< node is stating position */ - int totalcost; /**< cost to move here from starting point */ - v3s16 sourcedir; /**< origin of movement for current cost */ - int surfaces; /**< number of surfaces with same x,z value*/ - v3s16 pos; /**< real position of node */ - path_cost directions[4]; /**< cost in different directions */ + void setCost(v3s16 dir, const PathCost &cost); + + bool valid = false; /**< node is on surface */ + bool target = false; /**< node is target position */ + bool source = false; /**< node is stating position */ + int totalcost = -1; /**< cost to move here from starting point */ + int estimated_cost = -1; /**< totalcost + heuristic cost to end */ + v3s16 sourcedir; /**< origin of movement for current cost */ + v3s16 pos; /**< real position of node */ + PathCost directions[4]; /**< cost in different directions */ + bool is_closed = false; /**< for A* search: if true, is in closed list */ + bool is_open = false; /**< for A* search: if true, is in open list */ /* debug values */ - bool is_element; /**< node is element of path detected */ - char type; /**< type of node */ + bool is_element = false; /**< node is element of path detected */ + char type = 'u'; /**< Type of pathfinding node. + * u = unknown + * i = invalid + * s = surface (walkable node) + * - = non-walkable node (e.g. air) above surface + * g = other non-walkable node + */ +}; + +class Pathfinder; +class PathfinderCompareHeuristic; + +/** Abstract class to manage the map data */ +class GridNodeContainer { +public: + virtual PathGridnode &access(v3s16 p)=0; + virtual ~GridNodeContainer() = default; + +protected: + Pathfinder *m_pathf; + + void initNode(v3s16 ipos, PathGridnode *p_node); +}; + +class ArrayGridNodeContainer : public GridNodeContainer { +public: + virtual ~ArrayGridNodeContainer() = default; + + ArrayGridNodeContainer(Pathfinder *pathf, v3s16 dimensions); + virtual PathGridnode &access(v3s16 p); +private: + v3s16 m_dimensions; + + int m_x_stride; + int m_y_stride; + std::vector m_nodes_array; +}; + +class MapGridNodeContainer : public GridNodeContainer { +public: + virtual ~MapGridNodeContainer() = default; + + MapGridNodeContainer(Pathfinder *pathf); + virtual PathGridnode &access(v3s16 p); +private: + std::map m_nodes; }; /** class doing pathfinding */ -class pathfinder { +class Pathfinder { public: - /** - * default constructor - */ - pathfinder(); + Pathfinder() = delete; + Pathfinder(Map *map, const NodeDefManager *ndef) : m_map(map), m_ndef(ndef) {} + + ~Pathfinder(); /** * path evaluation function @@ -146,29 +192,16 @@ class pathfinder { * @param searchdistance maximum number of nodes to look in each direction * @param max_jump maximum number of blocks a path may jump up * @param max_drop maximum number of blocks a path may drop - * @param algo algorithm to use for finding a path + * @param algo Algorithm to use for finding a path */ - std::vector get_Path(ServerEnvironment* env, - v3s16 source, + std::vector getPath(v3s16 source, v3s16 destination, unsigned int searchdistance, unsigned int max_jump, unsigned int max_drop, - algorithm algo); + PathAlgorithm algo); private: - /** data struct for storing internal information */ - struct limits { - struct limit { - int min; - int max; - }; - - limit X; - limit Y; - limit Z; - }; - /* helper functions */ /** @@ -190,11 +223,17 @@ class pathfinder { * @param ipos index position * @return gridnode for index */ - path_gridnode& getIndexElement(v3s16 ipos); + PathGridnode &getIndexElement(v3s16 ipos); + + /** + * Get gridnode at a specific index position + * @return gridnode for index + */ + PathGridnode &getIdxElem(s16 x, s16 y, s16 z); /** - * invert a 3d position - * @param pos 3d position + * invert a 3D position (change sign of coordinates) + * @param pos 3D position * @return pos *-1 */ v3s16 invert(v3s16 pos); @@ -204,38 +243,17 @@ class pathfinder { * @param index position to validate * @return true/false */ - bool valid_index(v3s16 index); - - /** - * translate position to float position - * @param pos integer position - * @return float position - */ - v3f tov3f(v3s16 pos); + bool isValidIndex(v3s16 index); /* algorithm functions */ /** - * calculate 2d manahttan distance to target + * calculate 2D Manhattan distance to target * @param pos position to calc distance * @return integer distance */ - int get_manhattandistance(v3s16 pos); - - /** - * get best direction based uppon heuristics - * @param directions list of unchecked directions - * @param g_pos mapnode to start from - * @return direction to check - */ - v3s16 get_dir_heuristic(std::vector& directions,path_gridnode& g_pos); - - /** - * build internal data representation of search area - * @return true/false if costmap creation was successfull - */ - bool build_costmap(); + int getXZManhattanDist(v3s16 pos); /** * calculate cost of movement @@ -243,7 +261,7 @@ class pathfinder { * @param dir direction to move to * @return cost information */ - path_cost calc_cost(v3s16 pos,v3s16 dir); + PathCost calcCost(v3s16 pos, v3s16 dir); /** * recursive update whole search areas total cost information @@ -253,139 +271,169 @@ class pathfinder { * @param level current recursion depth * @return true/false path to destination has been found */ - bool update_all_costs(v3s16 ipos,v3s16 srcdir,int total_cost,int level); + bool updateAllCosts(v3s16 ipos, v3s16 srcdir, int current_cost, int level); /** - * recursive try to find a patrh to destionation - * @param ipos position to check next - * @param srcdir positionc checked last time - * @param total_cost cost of moving to ipos - * @param level current recursion depth + * try to find a path to destination using a heuristic function + * to estimate distance to target (A* search algorithm) + * @param isource start position (index pos) + * @param idestination end position (index pos) * @return true/false path to destination has been found */ - bool update_cost_heuristic(v3s16 ipos,v3s16 srcdir,int current_cost,int level); + bool updateCostHeuristic(v3s16 isource, v3s16 idestination); /** - * recursive build a vector containing all nodes from source to destination + * build a vector containing all nodes from destination to source; + * to be called after the node costs have been processed * @param path vector to add nodes to - * @param pos pos to check next - * @param level recursion depth + * @param ipos initial pos to check (index pos) + * @return true/false path has been fully built + */ + bool buildPath(std::vector &path, v3s16 ipos); + + /** + * go downwards from a position until some barrier + * is hit. + * @param pos position from which to go downwards + * @param max_down maximum distance to go downwards + * @return new position after movement; if too far down, + * pos is returned */ - void build_path(std::vector& path,v3s16 pos, int level); + v3s16 walkDownwards(v3s16 pos, unsigned int max_down); /* variables */ - int m_max_index_x; /**< max index of search area in x direction */ - int m_max_index_y; /**< max index of search area in y direction */ - int m_max_index_z; /**< max index of search area in z direction */ + int m_max_index_x = 0; /**< max index of search area in x direction */ + int m_max_index_y = 0; /**< max index of search area in y direction */ + int m_max_index_z = 0; /**< max index of search area in z direction */ - int m_searchdistance; /**< max distance to search in each direction */ - int m_maxdrop; /**< maximum number of blocks a path may drop */ - int m_maxjump; /**< maximum number of blocks a path may jump */ - int m_min_target_distance; /**< current smalest path to target */ + int m_searchdistance = 0; /**< max distance to search in each direction */ + int m_maxdrop = 0; /**< maximum number of blocks a path may drop */ + int m_maxjump = 0; /**< maximum number of blocks a path may jump */ + int m_min_target_distance = 0; /**< current smalest path to target */ - bool m_prefetch; /**< prefetch cost data */ + bool m_prefetch = true; /**< prefetch cost data */ - v3s16 m_start; /**< source position */ - v3s16 m_destination; /**< destination position */ + v3s16 m_start; /**< source position */ + v3s16 m_destination; /**< destination position */ - limits m_limits; /**< position limits in real map coordinates */ + core::aabbox3d m_limits; /**< position limits in real map coordinates */ - /** 3d grid containing all map data already collected and analyzed */ - std::vector > > m_data; + /** contains all map data already collected and analyzed. + Access it via the getIndexElement/getIdxElem methods. */ + friend class GridNodeContainer; + GridNodeContainer *m_nodes_container = nullptr; - ServerEnvironment* m_env; /**< minetest environment pointer */ + Map *m_map = nullptr; + + const NodeDefManager *m_ndef = nullptr; + + friend class PathfinderCompareHeuristic; #ifdef PATHFINDER_DEBUG /** * print collected cost information */ - void print_cost(); + void printCost(); /** * print collected cost information in a specific direction * @param dir direction to print */ - void print_cost(path_directions dir); + void printCost(PathDirections dir); /** * print type of node as evaluated */ - void print_type(); + void printType(); /** * print pathlenght for all nodes in search area */ - void print_pathlen(); + void printPathLen(); /** * print a path * @param path path to show */ - void print_path(std::vector path); + void printPath(std::vector path); /** * print y direction for all movements */ - void print_ydir(); + void printYdir(); /** * print y direction for moving in a specific direction * @param dir direction to show data */ - void print_ydir(path_directions dir); + void printYdir(PathDirections dir); /** * helper function to translate a direction to speaking text * @param dir direction to translate * @return textual name of direction */ - std::string dir_to_name(path_directions dir); + std::string dirToName(PathDirections dir); #endif }; +/** Helper class for the open list priority queue in the A* pathfinder + * to sort the pathfinder nodes by cost. + */ +class PathfinderCompareHeuristic +{ + private: + Pathfinder *myPathfinder; + public: + PathfinderCompareHeuristic(Pathfinder *pf) + { + myPathfinder = pf; + } + bool operator() (v3s16 pos1, v3s16 pos2) { + v3s16 ipos1 = myPathfinder->getIndexPos(pos1); + v3s16 ipos2 = myPathfinder->getIndexPos(pos2); + PathGridnode &g_pos1 = myPathfinder->getIndexElement(ipos1); + PathGridnode &g_pos2 = myPathfinder->getIndexElement(ipos2); + if (!g_pos1.valid) + return false; + if (!g_pos2.valid) + return false; + return g_pos1.estimated_cost > g_pos2.estimated_cost; + } +}; + /******************************************************************************/ /* implementation */ /******************************************************************************/ -std::vector get_Path(ServerEnvironment* env, - v3s16 source, - v3s16 destination, - unsigned int searchdistance, - unsigned int max_jump, - unsigned int max_drop, - algorithm algo) { - - pathfinder searchclass; - - return searchclass.get_Path(env, - source,destination, - searchdistance,max_jump,max_drop,algo); -} - -/******************************************************************************/ -path_cost::path_cost() -: valid(false), - value(0), - direction(0), - updated(false) +std::vector get_path(Map* map, const NodeDefManager *ndef, + v3s16 source, + v3s16 destination, + unsigned int searchdistance, + unsigned int max_jump, + unsigned int max_drop, + PathAlgorithm algo) { - //intentionaly empty + return Pathfinder(map, ndef).getPath(source, destination, + searchdistance, max_jump, max_drop, algo); } /******************************************************************************/ -path_cost::path_cost(const path_cost& b) { +PathCost::PathCost(const PathCost &b) +{ valid = b.valid; - direction = b.direction; + y_change = b.y_change; value = b.value; updated = b.updated; } /******************************************************************************/ -path_cost& path_cost::operator= (const path_cost& b) { +PathCost &PathCost::operator= (const PathCost &b) +{ valid = b.valid; - direction = b.direction; + y_change = b.y_change; value = b.value; updated = b.updated; @@ -393,32 +441,16 @@ path_cost& path_cost::operator= (const path_cost& b) { } /******************************************************************************/ -path_gridnode::path_gridnode() -: valid(false), - target(false), - source(false), - totalcost(-1), - sourcedir(v3s16(0,0,0)), - surfaces(0), - pos(v3s16(0,0,0)), - is_element(false), - type('u') -{ - //intentionaly empty -} - -/******************************************************************************/ -path_gridnode::path_gridnode(const path_gridnode& b) +PathGridnode::PathGridnode(const PathGridnode &b) : valid(b.valid), target(b.target), source(b.source), totalcost(b.totalcost), sourcedir(b.sourcedir), - surfaces(b.surfaces), pos(b.pos), is_element(b.is_element), type(b.type) - { +{ directions[DIR_XP] = b.directions[DIR_XP]; directions[DIR_XM] = b.directions[DIR_XM]; @@ -427,14 +459,14 @@ path_gridnode::path_gridnode(const path_gridnode& b) } /******************************************************************************/ -path_gridnode& path_gridnode::operator= (const path_gridnode& b) { +PathGridnode &PathGridnode::operator= (const PathGridnode &b) +{ valid = b.valid; target = b.target; source = b.source; is_element = b.is_element; totalcost = b.totalcost; sourcedir = b.sourcedir; - surfaces = b.surfaces; pos = b.pos; type = b.type; @@ -447,7 +479,8 @@ path_gridnode& path_gridnode::operator= (const path_gridnode& b) { } /******************************************************************************/ -path_cost path_gridnode::get_cost(v3s16 dir) { +PathCost PathGridnode::getCost(v3s16 dir) +{ if (dir.X > 0) { return directions[DIR_XP]; } @@ -460,12 +493,13 @@ path_cost path_gridnode::get_cost(v3s16 dir) { if (dir.Z < 0) { return directions[DIR_ZM]; } - path_cost retval; + PathCost retval; return retval; } /******************************************************************************/ -void path_gridnode::set_cost(v3s16 dir,path_cost cost) { +void PathGridnode::setCost(v3s16 dir, const PathCost &cost) +{ if (dir.X > 0) { directions[DIR_XP] = cost; } @@ -480,28 +514,112 @@ void path_gridnode::set_cost(v3s16 dir,path_cost cost) { } } +void GridNodeContainer::initNode(v3s16 ipos, PathGridnode *p_node) +{ + const NodeDefManager *ndef = m_pathf->m_ndef; + PathGridnode &elem = *p_node; + + v3s16 realpos = m_pathf->getRealPos(ipos); + + MapNode current = m_pathf->m_map->getNode(realpos); + MapNode below = m_pathf->m_map->getNode(realpos + v3s16(0, -1, 0)); + + + if ((current.param0 == CONTENT_IGNORE) || + (below.param0 == CONTENT_IGNORE)) { + DEBUG_OUT("Pathfinder: " << PP(realpos) << + " current or below is invalid element" << std::endl); + if (current.param0 == CONTENT_IGNORE) { + elem.type = 'i'; + DEBUG_OUT(PP(ipos) << ": " << 'i' << std::endl); + } + return; + } + + //don't add anything if it isn't an air node + if (ndef->get(current).walkable || !ndef->get(below).walkable) { + DEBUG_OUT("Pathfinder: " << PP(realpos) + << " not on surface" << std::endl); + if (ndef->get(current).walkable) { + elem.type = 's'; + DEBUG_OUT(PP(ipos) << ": " << 's' << std::endl); + } else { + elem.type = '-'; + DEBUG_OUT(PP(ipos) << ": " << '-' << std::endl); + } + return; + } + + elem.valid = true; + elem.pos = realpos; + elem.type = 'g'; + DEBUG_OUT(PP(ipos) << ": " << 'a' << std::endl); + + if (m_pathf->m_prefetch) { + elem.directions[DIR_XP] = m_pathf->calcCost(realpos, v3s16( 1, 0, 0)); + elem.directions[DIR_XM] = m_pathf->calcCost(realpos, v3s16(-1, 0, 0)); + elem.directions[DIR_ZP] = m_pathf->calcCost(realpos, v3s16( 0, 0, 1)); + elem.directions[DIR_ZM] = m_pathf->calcCost(realpos, v3s16( 0, 0,-1)); + } +} + +ArrayGridNodeContainer::ArrayGridNodeContainer(Pathfinder *pathf, v3s16 dimensions) : + m_x_stride(dimensions.Y * dimensions.Z), + m_y_stride(dimensions.Z) +{ + m_pathf = pathf; + + m_nodes_array.resize(dimensions.X * dimensions.Y * dimensions.Z); + INFO_TARGET << "Pathfinder ArrayGridNodeContainer constructor." << std::endl; + for (int x = 0; x < dimensions.X; x++) { + for (int y = 0; y < dimensions.Y; y++) { + for (int z= 0; z < dimensions.Z; z++) { + v3s16 ipos(x, y, z); + initNode(ipos, &access(ipos)); + } + } + } +} + +PathGridnode &ArrayGridNodeContainer::access(v3s16 p) +{ + return m_nodes_array[p.X * m_x_stride + p.Y * m_y_stride + p.Z]; +} + +MapGridNodeContainer::MapGridNodeContainer(Pathfinder *pathf) +{ + m_pathf = pathf; +} + +PathGridnode &MapGridNodeContainer::access(v3s16 p) +{ + std::map::iterator it = m_nodes.find(p); + if (it != m_nodes.end()) { + return it->second; + } + PathGridnode &n = m_nodes[p]; + initNode(p, &n); + return n; +} + + + /******************************************************************************/ -std::vector pathfinder::get_Path(ServerEnvironment* env, - v3s16 source, +std::vector Pathfinder::getPath(v3s16 source, v3s16 destination, unsigned int searchdistance, unsigned int max_jump, unsigned int max_drop, - algorithm algo) { + PathAlgorithm algo) +{ #ifdef PATHFINDER_CALC_TIME timespec ts; clock_gettime(CLOCK_REALTIME, &ts); #endif std::vector retval; - //check parameters - if (env == 0) { - ERROR_TARGET << "missing environment pointer" << std::endl; - return retval; - } - + //initialization m_searchdistance = searchdistance; - m_env = env; m_maxjump = max_jump; m_maxdrop = max_drop; m_start = source; @@ -509,58 +627,92 @@ std::vector pathfinder::get_Path(ServerEnvironment* env, m_min_target_distance = -1; m_prefetch = true; - if (algo == A_PLAIN_NP) { + if (algo == PA_PLAIN_NP) { m_prefetch = false; } - int min_x = MYMIN(source.X,destination.X); - int max_x = MYMAX(source.X,destination.X); + //calculate boundaries within we're allowed to search + int min_x = MYMIN(source.X, destination.X); + int max_x = MYMAX(source.X, destination.X); - int min_y = MYMIN(source.Y,destination.Y); - int max_y = MYMAX(source.Y,destination.Y); + int min_y = MYMIN(source.Y, destination.Y); + int max_y = MYMAX(source.Y, destination.Y); - int min_z = MYMIN(source.Z,destination.Z); - int max_z = MYMAX(source.Z,destination.Z); + int min_z = MYMIN(source.Z, destination.Z); + int max_z = MYMAX(source.Z, destination.Z); - m_limits.X.min = min_x - searchdistance; - m_limits.X.max = max_x + searchdistance; - m_limits.Y.min = min_y - searchdistance; - m_limits.Y.max = max_y + searchdistance; - m_limits.Z.min = min_z - searchdistance; - m_limits.Z.max = max_z + searchdistance; + m_limits.MinEdge.X = min_x - searchdistance; + m_limits.MinEdge.Y = min_y - searchdistance; + m_limits.MinEdge.Z = min_z - searchdistance; - m_max_index_x = m_limits.X.max - m_limits.X.min; - m_max_index_y = m_limits.Y.max - m_limits.Y.min; - m_max_index_z = m_limits.Z.max - m_limits.Z.min; + m_limits.MaxEdge.X = max_x + searchdistance; + m_limits.MaxEdge.Y = max_y + searchdistance; + m_limits.MaxEdge.Z = max_z + searchdistance; - //build data map - if (!build_costmap()) { - ERROR_TARGET << "failed to build costmap" << std::endl; - return retval; + v3s16 diff = m_limits.MaxEdge - m_limits.MinEdge; + + m_max_index_x = diff.X; + m_max_index_y = diff.Y; + m_max_index_z = diff.Z; + + delete m_nodes_container; + if (diff.getLength() > 5) { + m_nodes_container = new MapGridNodeContainer(this); + } else { + m_nodes_container = new ArrayGridNodeContainer(this, diff); } #ifdef PATHFINDER_DEBUG - print_type(); - print_cost(); - print_ydir(); + printType(); + printCost(); + printYdir(); #endif + //fail if source or destination is walkable + MapNode node_at_pos = m_map->getNode(destination); + if (m_ndef->get(node_at_pos).walkable) { + VERBOSE_TARGET << "Destination is walkable. " << + "Pos: " << PP(destination) << std::endl; + return retval; + } + node_at_pos = m_map->getNode(source); + if (m_ndef->get(node_at_pos).walkable) { + VERBOSE_TARGET << "Source is walkable. " << + "Pos: " << PP(source) << std::endl; + return retval; + } + + //If source pos is hovering above air, drop + //to the first walkable node (up to m_maxdrop). + //All algorithms expect the source pos to be *directly* above + //a walkable node. + v3s16 true_source = v3s16(source); + source = walkDownwards(source, m_maxdrop); + + //If destination pos is hovering above air, go downwards + //to the first walkable node (up to m_maxjump). + //This means a hovering destination pos could be reached + //by a final upwards jump. + v3s16 true_destination = v3s16(destination); + destination = walkDownwards(destination, m_maxjump); + //validate and mark start and end pos + v3s16 StartIndex = getIndexPos(source); v3s16 EndIndex = getIndexPos(destination); - path_gridnode& startpos = getIndexElement(StartIndex); - path_gridnode& endpos = getIndexElement(EndIndex); + PathGridnode &startpos = getIndexElement(StartIndex); + PathGridnode &endpos = getIndexElement(EndIndex); if (!startpos.valid) { - VERBOSE_TARGET << "invalid startpos" << - "Index: " << PPOS(StartIndex) << - "Realpos: " << PPOS(getRealPos(StartIndex)) << std::endl; + VERBOSE_TARGET << "Invalid startpos " << + "Index: " << PP(StartIndex) << + "Realpos: " << PP(getRealPos(StartIndex)) << std::endl; return retval; } if (!endpos.valid) { - VERBOSE_TARGET << "invalid stoppos" << - "Index: " << PPOS(EndIndex) << - "Realpos: " << PPOS(getRealPos(EndIndex)) << std::endl; + VERBOSE_TARGET << "Invalid stoppos " << + "Index: " << PP(EndIndex) << + "Realpos: " << PP(getRealPos(EndIndex)) << std::endl; return retval; } @@ -570,16 +722,17 @@ std::vector pathfinder::get_Path(ServerEnvironment* env, bool update_cost_retval = false; + //calculate node costs switch (algo) { - case DIJKSTRA: - update_cost_retval = update_all_costs(StartIndex,v3s16(0,0,0),0,0); + case PA_DIJKSTRA: + update_cost_retval = updateAllCosts(StartIndex, v3s16(0, 0, 0), 0, 0); break; - case A_PLAIN_NP: - case A_PLAIN: - update_cost_retval = update_cost_heuristic(StartIndex,v3s16(0,0,0),0,0); + case PA_PLAIN_NP: + case PA_PLAIN: + update_cost_retval = updateCostHeuristic(StartIndex, EndIndex); break; default: - ERROR_TARGET << "missing algorithm"<< std::endl; + ERROR_TARGET << "Missing PathAlgorithm" << std::endl; break; } @@ -587,28 +740,55 @@ std::vector pathfinder::get_Path(ServerEnvironment* env, #ifdef PATHFINDER_DEBUG std::cout << "Path to target found!" << std::endl; - print_pathlen(); + printPathLen(); #endif //find path - std::vector path; - build_path(path,EndIndex,0); + std::vector index_path; + buildPath(index_path, EndIndex); + //Now we have a path of index positions, + //and it's in reverse. + //The "true" start or end position might be missing + //since those have been given special treatment. #ifdef PATHFINDER_DEBUG - std::cout << "Full index path:" << std::endl; - print_path(path); + std::cout << "Index path:" << std::endl; + printPath(index_path); #endif - - //finalize path + //from here we'll make the final changes to the path std::vector full_path; - for (std::vector::iterator i = path.begin(); - i != path.end(); ++i) { - full_path.push_back(getIndexElement(*i).pos); + + //calculate required size + int full_path_size = index_path.size(); + if (source != true_source) { + full_path_size++; + } + if (destination != true_destination) { + full_path_size++; + } + full_path.reserve(full_path_size); + + //manually add true_source to start of path, if needed + if (source != true_source) { + full_path.push_back(true_source); + } + //convert all index positions to "normal" positions and insert + //them into full_path in reverse + std::vector::reverse_iterator rit = index_path.rbegin(); + for (; rit != index_path.rend(); ++rit) { + full_path.push_back(getIndexElement(*rit).pos); } + //manually add true_destination to end of path, if needed + if (destination != true_destination) { + full_path.push_back(true_destination); + } + + //Done! We now have a complete path of normal positions. + #ifdef PATHFINDER_DEBUG - std::cout << "full path:" << std::endl; - print_path(full_path); + std::cout << "Full path:" << std::endl; + printPath(full_path); #endif #ifdef PATHFINDER_CALC_TIME timespec ts2; @@ -626,9 +806,9 @@ std::vector pathfinder::get_Path(ServerEnvironment* env, } else { #ifdef PATHFINDER_DEBUG - print_pathlen(); + printPathLen(); #endif - ERROR_TARGET << "failed to update cost map"<< std::endl; + INFO_TARGET << "No path found" << std::endl; } @@ -636,190 +816,88 @@ std::vector pathfinder::get_Path(ServerEnvironment* env, return retval; } -/******************************************************************************/ -pathfinder::pathfinder() : - m_max_index_x(0), - m_max_index_y(0), - m_max_index_z(0), - m_searchdistance(0), - m_maxdrop(0), - m_maxjump(0), - m_min_target_distance(0), - m_prefetch(true), - m_start(0,0,0), - m_destination(0,0,0), - m_limits(), - m_data(), - m_env(0) +Pathfinder::~Pathfinder() { - //intentionaly empty -} - -/******************************************************************************/ -v3s16 pathfinder::getRealPos(v3s16 ipos) { - - v3s16 retval = ipos; - - retval.X += m_limits.X.min; - retval.Y += m_limits.Y.min; - retval.Z += m_limits.Z.min; - - return retval; + delete m_nodes_container; } - /******************************************************************************/ -bool pathfinder::build_costmap() +v3s16 Pathfinder::getRealPos(v3s16 ipos) { - INFO_TARGET << "Pathfinder build costmap: (" << m_limits.X.min << "," - << m_limits.Z.min << ") (" - << m_limits.X.max << "," - << m_limits.Z.max << ")" - << std::endl; - m_data.resize(m_max_index_x); - for (int x = 0; x < m_max_index_x; x++) { - m_data[x].resize(m_max_index_z); - for (int z = 0; z < m_max_index_z; z++) { - m_data[x][z].resize(m_max_index_y); - - int surfaces = 0; - for (int y = 0; y < m_max_index_y; y++) { - v3s16 ipos(x,y,z); - - v3s16 realpos = getRealPos(ipos); - - MapNode current = m_env->getMap().getNodeNoEx(realpos); - MapNode below = m_env->getMap().getNodeNoEx(realpos + v3s16(0,-1,0)); - - - if ((current.param0 == CONTENT_IGNORE) || - (below.param0 == CONTENT_IGNORE)) { - DEBUG_OUT("Pathfinder: " << PPOS(realpos) << - " current or below is invalid element" << std::endl); - if (current.param0 == CONTENT_IGNORE) { - m_data[x][z][y].type = 'i'; - DEBUG_OUT(x << "," << y << "," << z << ": " << 'i' << std::endl); - } - continue; - } - - //don't add anything if it isn't an air node - if ((current.param0 != CONTENT_AIR) || - (below.param0 == CONTENT_AIR )) { - DEBUG_OUT("Pathfinder: " << PPOS(realpos) - << " not on surface" << std::endl); - if (current.param0 != CONTENT_AIR) { - m_data[x][z][y].type = 's'; - DEBUG_OUT(x << "," << y << "," << z << ": " << 's' << std::endl); - } - else { - m_data[x][z][y].type = '-'; - DEBUG_OUT(x << "," << y << "," << z << ": " << '-' << std::endl); - } - continue; - } - - surfaces++; - - m_data[x][z][y].valid = true; - m_data[x][z][y].pos = realpos; - m_data[x][z][y].type = 'g'; - DEBUG_OUT(x << "," << y << "," << z << ": " << 'a' << std::endl); - - if (m_prefetch) { - m_data[x][z][y].directions[DIR_XP] = - calc_cost(realpos,v3s16( 1,0, 0)); - m_data[x][z][y].directions[DIR_XM] = - calc_cost(realpos,v3s16(-1,0, 0)); - m_data[x][z][y].directions[DIR_ZP] = - calc_cost(realpos,v3s16( 0,0, 1)); - m_data[x][z][y].directions[DIR_ZM] = - calc_cost(realpos,v3s16( 0,0,-1)); - } - - } - - if (surfaces >= 1 ) { - for (int y = 0; y < m_max_index_y; y++) { - if (m_data[x][z][y].valid) { - m_data[x][z][y].surfaces = surfaces; - } - } - } - } - } - return true; + return m_limits.MinEdge + ipos; } /******************************************************************************/ -path_cost pathfinder::calc_cost(v3s16 pos,v3s16 dir) { - path_cost retval; +PathCost Pathfinder::calcCost(v3s16 pos, v3s16 dir) +{ + PathCost retval; retval.updated = true; v3s16 pos2 = pos + dir; //check limits - if ( (pos2.X < m_limits.X.min) || - (pos2.X >= m_limits.X.max) || - (pos2.Z < m_limits.Z.min) || - (pos2.Z >= m_limits.Z.max)) { - DEBUG_OUT("Pathfinder: " << PPOS(pos2) << + if (!m_limits.isPointInside(pos2)) { + DEBUG_OUT("Pathfinder: " << PP(pos2) << " no cost -> out of limits" << std::endl); return retval; } - MapNode node_at_pos2 = m_env->getMap().getNodeNoEx(pos2); + MapNode node_at_pos2 = m_map->getNode(pos2); //did we get information about node? if (node_at_pos2.param0 == CONTENT_IGNORE ) { VERBOSE_TARGET << "Pathfinder: (1) area at pos: " - << PPOS(pos2) << " not loaded"; + << PP(pos2) << " not loaded"; return retval; } - if (node_at_pos2.param0 == CONTENT_AIR) { + if (!m_ndef->get(node_at_pos2).walkable) { MapNode node_below_pos2 = - m_env->getMap().getNodeNoEx(pos2 + v3s16(0,-1,0)); + m_map->getNode(pos2 + v3s16(0, -1, 0)); //did we get information about node? if (node_below_pos2.param0 == CONTENT_IGNORE ) { VERBOSE_TARGET << "Pathfinder: (2) area at pos: " - << PPOS((pos2 + v3s16(0,-1,0))) << " not loaded"; + << PP((pos2 + v3s16(0, -1, 0))) << " not loaded"; return retval; } - if (node_below_pos2.param0 != CONTENT_AIR) { + //test if the same-height neighbor is suitable + if (m_ndef->get(node_below_pos2).walkable) { + //SUCCESS! retval.valid = true; retval.value = 1; - retval.direction = 0; - DEBUG_OUT("Pathfinder: "<< PPOS(pos) + retval.y_change = 0; + DEBUG_OUT("Pathfinder: "<< PP(pos) << " cost same height found" << std::endl); } else { - v3s16 testpos = pos2 - v3s16(0,-1,0); - MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos); + //test if we can fall a couple of nodes (m_maxdrop) + v3s16 testpos = pos2 + v3s16(0, -1, 0); + MapNode node_at_pos = m_map->getNode(testpos); while ((node_at_pos.param0 != CONTENT_IGNORE) && - (node_at_pos.param0 == CONTENT_AIR) && - (testpos.Y > m_limits.Y.min)) { - testpos += v3s16(0,-1,0); - node_at_pos = m_env->getMap().getNodeNoEx(testpos); + (!m_ndef->get(node_at_pos).walkable) && + (testpos.Y > m_limits.MinEdge.Y)) { + testpos += v3s16(0, -1, 0); + node_at_pos = m_map->getNode(testpos); } //did we find surface? - if ((testpos.Y >= m_limits.Y.min) && + if ((testpos.Y >= m_limits.MinEdge.Y) && (node_at_pos.param0 != CONTENT_IGNORE) && - (node_at_pos.param0 != CONTENT_AIR)) { + (m_ndef->get(node_at_pos).walkable)) { if ((pos2.Y - testpos.Y - 1) <= m_maxdrop) { + //SUCCESS! retval.valid = true; retval.value = 2; //difference of y-pos +1 (target node is ABOVE solid node) - retval.direction = ((testpos.Y - pos2.Y) +1); + retval.y_change = ((testpos.Y - pos2.Y) +1); DEBUG_OUT("Pathfinder cost below height found" << std::endl); } else { INFO_TARGET << "Pathfinder:" - " distance to surface below to big: " + " distance to surface below too big: " << (testpos.Y - pos2.Y) << " max: " << m_maxdrop << std::endl; } @@ -830,29 +908,49 @@ path_cost pathfinder::calc_cost(v3s16 pos,v3s16 dir) { } } else { - v3s16 testpos = pos2; - MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos); - - while ((node_at_pos.param0 != CONTENT_IGNORE) && - (node_at_pos.param0 != CONTENT_AIR) && - (testpos.Y < m_limits.Y.max)) { - testpos += v3s16(0,1,0); - node_at_pos = m_env->getMap().getNodeNoEx(testpos); + //test if we can jump upwards (m_maxjump) + + v3s16 targetpos = pos2; // position for jump target + v3s16 jumppos = pos; // position for checking if jumping space is free + MapNode node_target = m_map->getNode(targetpos); + MapNode node_jump = m_map->getNode(jumppos); + bool headbanger = false; // true if anything blocks jumppath + + while ((node_target.param0 != CONTENT_IGNORE) && + (m_ndef->get(node_target).walkable) && + (targetpos.Y < m_limits.MaxEdge.Y)) { + //if the jump would hit any solid node, discard + if ((node_jump.param0 == CONTENT_IGNORE) || + (m_ndef->get(node_jump).walkable)) { + headbanger = true; + break; + } + targetpos += v3s16(0, 1, 0); + jumppos += v3s16(0, 1, 0); + node_target = m_map->getNode(targetpos); + node_jump = m_map->getNode(jumppos); + + } + //check headbanger one last time + if ((node_jump.param0 == CONTENT_IGNORE) || + (m_ndef->get(node_jump).walkable)) { + headbanger = true; } - //did we find surface? - if ((testpos.Y <= m_limits.Y.max) && - (node_at_pos.param0 == CONTENT_AIR)) { + //did we find surface without banging our head? + if ((!headbanger) && (targetpos.Y <= m_limits.MaxEdge.Y) && + (!m_ndef->get(node_target).walkable)) { - if (testpos.Y - pos2.Y <= m_maxjump) { + if (targetpos.Y - pos2.Y <= m_maxjump) { + //SUCCESS! retval.valid = true; retval.value = 2; - retval.direction = (testpos.Y - pos2.Y); + retval.y_change = (targetpos.Y - pos2.Y); DEBUG_OUT("Pathfinder cost above found" << std::endl); } else { - DEBUG_OUT("Pathfinder: distance to surface above to big: " - << (testpos.Y - pos2.Y) << " max: " << m_maxjump + DEBUG_OUT("Pathfinder: distance to surface above too big: " + << (targetpos.Y - pos2.Y) << " max: " << m_maxjump << std::endl); } } @@ -864,23 +962,26 @@ path_cost pathfinder::calc_cost(v3s16 pos,v3s16 dir) { } /******************************************************************************/ -v3s16 pathfinder::getIndexPos(v3s16 pos) { - - v3s16 retval = pos; - retval.X -= m_limits.X.min; - retval.Y -= m_limits.Y.min; - retval.Z -= m_limits.Z.min; +v3s16 Pathfinder::getIndexPos(v3s16 pos) +{ + return pos - m_limits.MinEdge; +} - return retval; +/******************************************************************************/ +PathGridnode &Pathfinder::getIndexElement(v3s16 ipos) +{ + return m_nodes_container->access(ipos); } /******************************************************************************/ -path_gridnode& pathfinder::getIndexElement(v3s16 ipos) { - return m_data[ipos.X][ipos.Z][ipos.Y]; +inline PathGridnode &Pathfinder::getIdxElem(s16 x, s16 y, s16 z) +{ + return m_nodes_container->access(v3s16(x,y,z)); } /******************************************************************************/ -bool pathfinder::valid_index(v3s16 index) { +bool Pathfinder::isValidIndex(v3s16 index) +{ if ( (index.X < m_max_index_x) && (index.Y < m_max_index_y) && (index.Z < m_max_index_z) && @@ -893,7 +994,8 @@ bool pathfinder::valid_index(v3s16 index) { } /******************************************************************************/ -v3s16 pathfinder::invert(v3s16 pos) { +v3s16 Pathfinder::invert(v3s16 pos) +{ v3s16 retval = pos; retval.X *=-1; @@ -904,12 +1006,12 @@ v3s16 pathfinder::invert(v3s16 pos) { } /******************************************************************************/ -bool pathfinder::update_all_costs( v3s16 ipos, - v3s16 srcdir, - int current_cost, - int level) { - - path_gridnode& g_pos = getIndexElement(ipos); +bool Pathfinder::updateAllCosts(v3s16 ipos, + v3s16 srcdir, + int current_cost, + int level) +{ + PathGridnode &g_pos = getIndexElement(ipos); g_pos.totalcost = current_cost; g_pos.sourcedir = srcdir; @@ -924,35 +1026,34 @@ bool pathfinder::update_all_costs( v3s16 ipos, bool retval = false; - std::vector directions; - - directions.push_back(v3s16( 1,0, 0)); - directions.push_back(v3s16(-1,0, 0)); - directions.push_back(v3s16( 0,0, 1)); - directions.push_back(v3s16( 0,0,-1)); + // the 4 cardinal directions + const static v3s16 directions[4] = { + v3s16(1,0, 0), + v3s16(-1,0, 0), + v3s16(0,0, 1), + v3s16(0,0,-1) + }; - for (unsigned int i=0; i < directions.size(); i++) { - if (directions[i] != srcdir) { - path_cost cost = g_pos.get_cost(directions[i]); + for (v3s16 direction : directions) { + if (direction != srcdir) { + PathCost cost = g_pos.getCost(direction); if (cost.valid) { - directions[i].Y = cost.direction; + direction.Y = cost.y_change; - v3s16 ipos2 = ipos + directions[i]; + v3s16 ipos2 = ipos + direction; - if (!valid_index(ipos2)) { - DEBUG_OUT(LVL " Pathfinder: " << PPOS(ipos2) << - " out of range (" << m_limits.X.max << "," << - m_limits.Y.max << "," << m_limits.Z.max - <<")" << std::endl); + if (!isValidIndex(ipos2)) { + DEBUG_OUT(LVL " Pathfinder: " << PP(ipos2) << + " out of range, max=" << PP(m_limits.MaxEdge) << std::endl); continue; } - path_gridnode& g_pos2 = getIndexElement(ipos2); + PathGridnode &g_pos2 = getIndexElement(ipos2); if (!g_pos2.valid) { VERBOSE_TARGET << LVL "Pathfinder: no data for new position: " - << PPOS(ipos2) << std::endl; + << PP(ipos2) << std::endl; continue; } @@ -969,23 +1070,23 @@ bool pathfinder::update_all_costs( v3s16 ipos, if ((g_pos2.totalcost < 0) || (g_pos2.totalcost > new_cost)) { DEBUG_OUT(LVL "Pathfinder: updating path at: "<< - PPOS(ipos2) << " from: " << g_pos2.totalcost << " to "<< + PP(ipos2) << " from: " << g_pos2.totalcost << " to "<< new_cost << std::endl); - if (update_all_costs(ipos2,invert(directions[i]), - new_cost,level)) { + if (updateAllCosts(ipos2, invert(direction), + new_cost, level)) { retval = true; } } else { DEBUG_OUT(LVL "Pathfinder:" " already found shorter path to: " - << PPOS(ipos2) << std::endl); + << PP(ipos2) << std::endl); } } else { DEBUG_OUT(LVL "Pathfinder:" " not moving to invalid direction: " - << PPOS(directions[i]) << std::endl); + << PP(directions[i]) << std::endl); } } } @@ -993,228 +1094,207 @@ bool pathfinder::update_all_costs( v3s16 ipos, } /******************************************************************************/ -int pathfinder::get_manhattandistance(v3s16 pos) { - - int min_x = MYMIN(pos.X,m_destination.X); - int max_x = MYMAX(pos.X,m_destination.X); - int min_z = MYMIN(pos.Z,m_destination.Z); - int max_z = MYMAX(pos.Z,m_destination.Z); +int Pathfinder::getXZManhattanDist(v3s16 pos) +{ + int min_x = MYMIN(pos.X, m_destination.X); + int max_x = MYMAX(pos.X, m_destination.X); + int min_z = MYMIN(pos.Z, m_destination.Z); + int max_z = MYMAX(pos.Z, m_destination.Z); return (max_x - min_x) + (max_z - min_z); } -/******************************************************************************/ -v3s16 pathfinder::get_dir_heuristic(std::vector& directions,path_gridnode& g_pos) { - int minscore = -1; - v3s16 retdir = v3s16(0,0,0); - v3s16 srcpos = g_pos.pos; - DEBUG_OUT("Pathfinder: remaining dirs at beginning:" - << directions.size() << std::endl); - for (std::vector::iterator iter = directions.begin(); - iter != directions.end(); - ++iter) { - v3s16 pos1 = v3s16(srcpos.X + iter->X,0,srcpos.Z+iter->Z); - - int cur_manhattan = get_manhattandistance(pos1); - path_cost cost = g_pos.get_cost(*iter); +/******************************************************************************/ +bool Pathfinder::updateCostHeuristic(v3s16 isource, v3s16 idestination) +{ + // A* search algorithm. + + // The open list contains the pathfinder nodes that still need to be + // checked. The priority queue sorts the pathfinder nodes by + // estimated cost, with lowest cost on the top. + std::priority_queue, PathfinderCompareHeuristic> + openList(PathfinderCompareHeuristic(this)); + + v3s16 source = getRealPos(isource); + v3s16 destination = getRealPos(idestination); + + // initial position + openList.push(source); + + // the 4 cardinal directions + const static v3s16 directions[4] = { + v3s16(1,0, 0), + v3s16(-1,0, 0), + v3s16(0,0, 1), + v3s16(0,0,-1) + }; - if (!cost.updated) { - cost = calc_cost(g_pos.pos,*iter); - g_pos.set_cost(*iter,cost); + v3s16 current_pos; + PathGridnode& s_pos = getIndexElement(isource); + s_pos.source = true; + s_pos.totalcost = 0; + + // estimated cost from start to finish + int cur_manhattan = getXZManhattanDist(destination); + s_pos.estimated_cost = cur_manhattan; + + while (!openList.empty()) { + // Pick node with lowest total cost estimate. + // The "cheapest" node is always on top. + current_pos = openList.top(); + openList.pop(); + v3s16 ipos = getIndexPos(current_pos); + + // check if node is inside searchdistance and valid + if (!isValidIndex(ipos)) { + DEBUG_OUT(LVL " Pathfinder: " << PP(current_pos) << + " out of search distance, max=" << PP(m_limits.MaxEdge) << std::endl); + continue; } - if (cost.valid) { - int score = cost.value + cur_manhattan; - - if ((minscore < 0)|| (score < minscore)) { - minscore = score; - retdir = *iter; - } + PathGridnode& g_pos = getIndexElement(ipos); + g_pos.is_closed = true; + g_pos.is_open = false; + if (!g_pos.valid) { + continue; } - } - if (retdir != v3s16(0,0,0)) { - for (std::vector::iterator iter = directions.begin(); - iter != directions.end(); - ++iter) { - if(*iter == retdir) { - DEBUG_OUT("Pathfinder: removing return direction" << std::endl); - directions.erase(iter); - break; - } + if (current_pos == destination) { + // destination found, terminate + g_pos.target = true; + return true; } - } - else { - DEBUG_OUT("Pathfinder: didn't find any valid direction clearing" - << std::endl); - directions.clear(); - } - DEBUG_OUT("Pathfinder: remaining dirs at end:" << directions.size() - << std::endl); - return retdir; -} - -/******************************************************************************/ -bool pathfinder::update_cost_heuristic( v3s16 ipos, - v3s16 srcdir, - int current_cost, - int level) { - - path_gridnode& g_pos = getIndexElement(ipos); - g_pos.totalcost = current_cost; - g_pos.sourcedir = srcdir; - - level ++; - - //check if target has been found - if (g_pos.target) { - m_min_target_distance = current_cost; - DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl); - return true; - } - - bool retval = false; - - std::vector directions; - - directions.push_back(v3s16( 1,0, 0)); - directions.push_back(v3s16(-1,0, 0)); - directions.push_back(v3s16( 0,0, 1)); - directions.push_back(v3s16( 0,0,-1)); - v3s16 direction = get_dir_heuristic(directions,g_pos); + // for this node, check the 4 cardinal directions + for (v3s16 direction_flat : directions) { + int current_totalcost = g_pos.totalcost; - while (direction != v3s16(0,0,0) && (!retval)) { - - if (direction != srcdir) { - path_cost cost = g_pos.get_cost(direction); - - if (cost.valid) { - direction.Y = cost.direction; - - v3s16 ipos2 = ipos + direction; - - if (!valid_index(ipos2)) { - DEBUG_OUT(LVL " Pathfinder: " << PPOS(ipos2) << - " out of range (" << m_limits.X.max << "," << - m_limits.Y.max << "," << m_limits.Z.max - <<")" << std::endl); - direction = get_dir_heuristic(directions,g_pos); - continue; - } - - path_gridnode& g_pos2 = getIndexElement(ipos2); - - if (!g_pos2.valid) { - VERBOSE_TARGET << LVL "Pathfinder: no data for new position: " - << PPOS(ipos2) << std::endl; - direction = get_dir_heuristic(directions,g_pos); - continue; - } - - assert(cost.value > 0); - - int new_cost = current_cost + cost.value; - - // check if there already is a smaller path - if ((m_min_target_distance > 0) && - (m_min_target_distance < new_cost)) { - DEBUG_OUT(LVL "Pathfinder:" - " already longer than best already found path " - << PPOS(ipos2) << std::endl); - return false; - } - - if ((g_pos2.totalcost < 0) || - (g_pos2.totalcost > new_cost)) { - DEBUG_OUT(LVL "Pathfinder: updating path at: "<< - PPOS(ipos2) << " from: " << g_pos2.totalcost << " to "<< - new_cost << " srcdir=" << - PPOS(invert(direction))<< std::endl); - if (update_cost_heuristic(ipos2,invert(direction), - new_cost,level)) { - retval = true; - } - } - else { - DEBUG_OUT(LVL "Pathfinder:" - " already found shorter path to: " - << PPOS(ipos2) << std::endl); - } + // get cost from current node to currently checked direction + PathCost cost = g_pos.getCost(direction_flat); + if (!cost.updated) { + cost = calcCost(current_pos, direction_flat); + g_pos.setCost(direction_flat, cost); } - else { - DEBUG_OUT(LVL "Pathfinder:" - " not moving to invalid direction: " - << PPOS(direction) << std::endl); + // update Y component of direction if neighbor requires jump or fall + v3s16 direction_3d = v3s16(direction_flat); + direction_3d.Y = cost.y_change; + + // get position of true neighbor + v3s16 neighbor = current_pos + direction_3d; + v3s16 ineighbor = getIndexPos(neighbor); + PathGridnode &n_pos = getIndexElement(ineighbor); + + if (cost.valid && !n_pos.is_closed && !n_pos.is_open) { + // heuristic function; estimate cost from neighbor to destination + cur_manhattan = getXZManhattanDist(neighbor); + + // add neighbor to open list + n_pos.sourcedir = invert(direction_3d); + n_pos.totalcost = current_totalcost + cost.value; + n_pos.estimated_cost = current_totalcost + cost.value + cur_manhattan; + n_pos.is_open = true; + openList.push(neighbor); } } - else { - DEBUG_OUT(LVL "Pathfinder:" - " skipping srcdir: " - << PPOS(direction) << std::endl); - } - direction = get_dir_heuristic(directions,g_pos); } - return retval; + // no path found; all possible nodes within searchdistance have been exhausted + return false; } /******************************************************************************/ -void pathfinder::build_path(std::vector& path,v3s16 pos, int level) { - level ++; - if (level > 700) { - ERROR_TARGET - << LVL "Pathfinder: path is too long aborting" << std::endl; - return; - } - - path_gridnode& g_pos = getIndexElement(pos); - if (!g_pos.valid) { - ERROR_TARGET - << LVL "Pathfinder: invalid next pos detected aborting" << std::endl; - return; - } +bool Pathfinder::buildPath(std::vector &path, v3s16 ipos) +{ + // The cost calculation should have set a source direction for all relevant nodes. + // To build the path, we go backwards from the destination until we reach the start. + for(u32 waypoints = 1; waypoints++; ) { + if (waypoints > PATHFINDER_MAX_WAYPOINTS) { + ERROR_TARGET << "Pathfinder: buildPath: path is too long (too many waypoints), aborting" << std::endl; + return false; + } + // Insert node into path + PathGridnode &g_pos = getIndexElement(ipos); + if (!g_pos.valid) { + ERROR_TARGET << "Pathfinder: buildPath: invalid next pos detected, aborting" << std::endl; + return false; + } - g_pos.is_element = true; + g_pos.is_element = true; + path.push_back(ipos); + if (g_pos.source) + // start node found, terminate + return true; - //check if source reached - if (g_pos.source) { - path.push_back(pos); - return; + // go to the node from which the pathfinder came + ipos += g_pos.sourcedir; } - build_path(path,pos + g_pos.sourcedir,level); - path.push_back(pos); + ERROR_TARGET << "Pathfinder: buildPath: no source node found" << std::endl; + return false; } /******************************************************************************/ -v3f pathfinder::tov3f(v3s16 pos) { - return v3f(BS*pos.X,BS*pos.Y,BS*pos.Z); +v3s16 Pathfinder::walkDownwards(v3s16 pos, unsigned int max_down) { + if (max_down == 0) + return pos; + v3s16 testpos = v3s16(pos); + MapNode node_at_pos = m_map->getNode(testpos); + unsigned int down = 0; + while ((node_at_pos.param0 != CONTENT_IGNORE) && + (!m_ndef->get(node_at_pos).walkable) && + (testpos.Y > m_limits.MinEdge.Y) && + (down <= max_down)) { + testpos += v3s16(0, -1, 0); + down++; + node_at_pos = m_map->getNode(testpos); + } + //did we find surface? + if ((testpos.Y >= m_limits.MinEdge.Y) && + (node_at_pos.param0 != CONTENT_IGNORE) && + (m_ndef->get(node_at_pos).walkable)) { + if (down == 0) { + pos = testpos; + } else if ((down - 1) <= max_down) { + //difference of y-pos +1 (target node is ABOVE solid node) + testpos += v3s16(0, 1, 0); + pos = testpos; + } + else { + VERBOSE_TARGET << "Pos too far above ground: " << + "Index: " << PP(getIndexPos(pos)) << + "Realpos: " << PP(getRealPos(getIndexPos(pos))) << std::endl; + } + } else { + DEBUG_OUT("Pathfinder: no surface found below pos" << std::endl); + } + return pos; } #ifdef PATHFINDER_DEBUG /******************************************************************************/ -void pathfinder::print_cost() { - print_cost(DIR_XP); - print_cost(DIR_XM); - print_cost(DIR_ZP); - print_cost(DIR_ZM); +void Pathfinder::printCost() +{ + printCost(DIR_XP); + printCost(DIR_XM); + printCost(DIR_ZP); + printCost(DIR_ZM); } /******************************************************************************/ -void pathfinder::print_ydir() { - print_ydir(DIR_XP); - print_ydir(DIR_XM); - print_ydir(DIR_ZP); - print_ydir(DIR_ZM); +void Pathfinder::printYdir() +{ + printYdir(DIR_XP); + printYdir(DIR_XM); + printYdir(DIR_ZP); + printYdir(DIR_ZM); } /******************************************************************************/ -void pathfinder::print_cost(path_directions dir) { - - std::cout << "Cost in direction: " << dir_to_name(dir) << std::endl; +void Pathfinder::printCost(PathDirections dir) +{ + std::cout << "Cost in direction: " << dirToName(dir) << std::endl; std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl; std::cout << std::setfill(' '); for (int y = 0; y < m_max_index_y; y++) { @@ -1230,9 +1310,9 @@ void pathfinder::print_cost(path_directions dir) { for (int z = 0; z < m_max_index_z; z++) { std::cout << std::setw(4) << z <<": "; for (int x = 0; x < m_max_index_x; x++) { - if (m_data[x][z][y].directions[dir].valid) + if (getIdxElem(x, y, z).directions[dir].valid) std::cout << std::setw(4) - << m_data[x][z][y].directions[dir].value; + << getIdxElem(x, y, z).directions[dir].value; else std::cout << std::setw(4) << "-"; } @@ -1243,9 +1323,9 @@ void pathfinder::print_cost(path_directions dir) { } /******************************************************************************/ -void pathfinder::print_ydir(path_directions dir) { - - std::cout << "Height difference in direction: " << dir_to_name(dir) << std::endl; +void Pathfinder::printYdir(PathDirections dir) +{ + std::cout << "Height difference in direction: " << dirToName(dir) << std::endl; std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl; std::cout << std::setfill(' '); for (int y = 0; y < m_max_index_y; y++) { @@ -1261,9 +1341,9 @@ void pathfinder::print_ydir(path_directions dir) { for (int z = 0; z < m_max_index_z; z++) { std::cout << std::setw(4) << z <<": "; for (int x = 0; x < m_max_index_x; x++) { - if (m_data[x][z][y].directions[dir].valid) + if (getIdxElem(x, y, z).directions[dir].valid) std::cout << std::setw(4) - << m_data[x][z][y].directions[dir].direction; + << getIdxElem(x, y, z).directions[dir].y_change; else std::cout << std::setw(4) << "-"; } @@ -1274,7 +1354,8 @@ void pathfinder::print_ydir(path_directions dir) { } /******************************************************************************/ -void pathfinder::print_type() { +void Pathfinder::printType() +{ std::cout << "Type of node:" << std::endl; std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl; std::cout << std::setfill(' '); @@ -1291,7 +1372,7 @@ void pathfinder::print_type() { for (int z = 0; z < m_max_index_z; z++) { std::cout << std::setw(3) << z <<": "; for (int x = 0; x < m_max_index_x; x++) { - char toshow = m_data[x][z][y].type; + char toshow = getIdxElem(x, y, z).type; std::cout << std::setw(3) << toshow; } std::cout << std::endl; @@ -1302,7 +1383,8 @@ void pathfinder::print_type() { } /******************************************************************************/ -void pathfinder::print_pathlen() { +void Pathfinder::printPathLen() +{ std::cout << "Pathlen:" << std::endl; std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl; std::cout << std::setfill(' '); @@ -1319,7 +1401,7 @@ void pathfinder::print_pathlen() { for (int z = 0; z < m_max_index_z; z++) { std::cout << std::setw(3) << z <<": "; for (int x = 0; x < m_max_index_x; x++) { - std::cout << std::setw(3) << m_data[x][z][y].totalcost; + std::cout << std::setw(3) << getIdxElem(x, y, z).totalcost; } std::cout << std::endl; } @@ -1329,7 +1411,8 @@ void pathfinder::print_pathlen() { } /******************************************************************************/ -std::string pathfinder::dir_to_name(path_directions dir) { +std::string Pathfinder::dirToName(PathDirections dir) +{ switch (dir) { case DIR_XP: return "XP"; @@ -1349,12 +1432,12 @@ std::string pathfinder::dir_to_name(path_directions dir) { } /******************************************************************************/ -void pathfinder::print_path(std::vector path) { - +void Pathfinder::printPath(std::vector path) +{ unsigned int current = 0; for (std::vector::iterator i = path.begin(); i != path.end(); ++i) { - std::cout << std::setw(3) << current << ":" << PPOS((*i)) << std::endl; + std::cout << std::setw(3) << current << ":" << PP((*i)) << std::endl; current++; } }