src/pathfinder.cpp

   1 /*
   2 Minetest
   3 Copyright (C) 2013 sapier, sapier at gmx dot net
   4 Copyright (C) 2016 est31, <MTest31@outlook.com>
   5
   6 This program is free software; you can redistribute it and/or modify
   7 it under the terms of the GNU Lesser General Public License as published by
   8 the Free Software Foundation; either version 2.1 of the License, or
   9 (at your option) any later version.
  10
  11 This program is distributed in the hope that it will be useful,
  12 but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 GNU Lesser General Public License for more details.
  15
  16 You should have received a copy of the GNU Lesser General Public License along
  17 with this program; if not, write to the Free Software Foundation, Inc.,
  18 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  19 */
  20
  21 /******************************************************************************/
  22 /* Includes                                                                   */
  23 /******************************************************************************/
  24
  25 #include "pathfinder.h"
  26 #include "map.h"
  27 #include "nodedef.h"
  28
  29 //#define PATHFINDER_DEBUG
  30 //#define PATHFINDER_CALC_TIME
  31
  32 #ifdef PATHFINDER_DEBUG
  33         #include <string>
  34 #endif
  35 #ifdef PATHFINDER_DEBUG
  36         #include <iomanip>
  37 #endif
  38 #ifdef PATHFINDER_CALC_TIME
  39         #include <sys/time.h>
  40 #endif
  41
  42 /******************************************************************************/
  43 /* Typedefs and macros                                                        */
  44 /******************************************************************************/
  45
  46 #define LVL "(" << level << ")" <<
  47
  48 #ifdef PATHFINDER_DEBUG
  49 #define DEBUG_OUT(a)     std::cout << a
  50 #define INFO_TARGET      std::cout
  51 #define VERBOSE_TARGET   std::cout
  52 #define ERROR_TARGET     std::cout
  53 #else
  54 #define DEBUG_OUT(a)     while(0)
  55 #define INFO_TARGET      infostream << "Pathfinder: "
  56 #define VERBOSE_TARGET   verbosestream << "Pathfinder: "
  57 #define ERROR_TARGET     warningstream << "Pathfinder: "
  58 #endif
  59
  60 #define PATHFINDER_MAX_WAYPOINTS 700
  61
  62 /******************************************************************************/
  63 /* Class definitions                                                          */
  64 /******************************************************************************/
  65
  66
  67 /** representation of cost in specific direction */
  68 class PathCost {
  69 public:
  70
  71         /** default constructor */
  72         PathCost() = default;
  73
  74         /** copy constructor */
  75         PathCost(const PathCost &b);
  76
  77         /** assignment operator */
  78         PathCost &operator= (const PathCost &b);
  79
  80         bool valid = false;              /**< movement is possible         */
  81         int  value = 0;                  /**< cost of movement             */
  82         int  y_change = 0;               /**< change of y position of movement */
  83         bool updated = false;            /**< this cost has ben calculated */
  84
  85 };
  86
  87
  88 /** representation of a mapnode to be used for pathfinding */
  89 class PathGridnode {
  90
  91 public:
  92         /** default constructor */
  93         PathGridnode() = default;
  94
  95         /** copy constructor */
  96         PathGridnode(const PathGridnode &b);
  97
  98         /**
  99          * assignment operator
 100          * @param b node to copy
 101          */
 102         PathGridnode &operator= (const PathGridnode &b);
 103
 104         /**
 105          * read cost in a specific direction
 106          * @param dir direction of cost to fetch
 107          */
 108         PathCost getCost(v3s16 dir);
 109
 110         /**
 111          * set cost value for movement
 112          * @param dir direction to set cost for
 113          * @cost cost to set
 114          */
 115         void      setCost(v3s16 dir, const PathCost &cost);
 116
 117         bool      valid = false;               /**< node is on surface                    */
 118         bool      target = false;              /**< node is target position               */
 119         bool      source = false;              /**< node is stating position              */
 120         int       totalcost = -1;              /**< cost to move here from starting point */
 121         int       estimated_cost = -1;         /**< totalcost + heuristic cost to end     */
 122         v3s16     sourcedir;                   /**< origin of movement for current cost   */
 123         v3s16     pos;                         /**< real position of node                 */
 124         PathCost directions[4];                /**< cost in different directions          */
 125         bool      is_closed = false;           /**< for A* search: if true, is in closed list */
 126         bool      is_open = false;             /**< for A* search: if true, is in open list */
 127
 128         /* debug values */
 129         bool      is_element = false;          /**< node is element of path detected      */
 130         char      type = 'u';                  /**< Type of pathfinding node.
 131                                                 * u = unknown
 132                                                 * i = invalid
 133                                                 * s = surface (walkable node)
 134                                                 * - = non-walkable node (e.g. air) above surface
 135                                                 * g = other non-walkable node
 136                                                 */
 137 };
 138
 139 class Pathfinder;
 140 class PathfinderCompareHeuristic;
 141
 142 /** Abstract class to manage the map data */
 143 class GridNodeContainer {
 144 public:
 145         virtual PathGridnode &access(v3s16 p)=0;
 146         virtual ~GridNodeContainer() = default;
 147
 148 protected:
 149         Pathfinder *m_pathf;
 150
 151         void initNode(v3s16 ipos, PathGridnode *p_node);
 152 };
 153
 154 class ArrayGridNodeContainer : public GridNodeContainer {
 155 public:
 156         virtual ~ArrayGridNodeContainer() = default;
 157
 158         ArrayGridNodeContainer(Pathfinder *pathf, v3s16 dimensions);
 159         virtual PathGridnode &access(v3s16 p);
 160 private:
 161         v3s16 m_dimensions;
 162
 163         int m_x_stride;
 164         int m_y_stride;
 165         std::vector<PathGridnode> m_nodes_array;
 166 };
 167
 168 class MapGridNodeContainer : public GridNodeContainer {
 169 public:
 170         virtual ~MapGridNodeContainer() = default;
 171
 172         MapGridNodeContainer(Pathfinder *pathf);
 173         virtual PathGridnode &access(v3s16 p);
 174 private:
 175         std::map<v3s16, PathGridnode> m_nodes;
 176 };
 177
 178 /** class doing pathfinding */
 179 class Pathfinder {
 180
 181 public:
 182         Pathfinder() = delete;
 183         Pathfinder(Map *map, const NodeDefManager *ndef) : m_map(map), m_ndef(ndef) {}
 184
 185         ~Pathfinder();
 186
 187         /**
 188          * path evaluation function
 189          * @param env environment to look for path
 190          * @param source origin of path
 191          * @param destination end position of path
 192          * @param searchdistance maximum number of nodes to look in each direction
 193          * @param max_jump maximum number of blocks a path may jump up
 194          * @param max_drop maximum number of blocks a path may drop
 195          * @param algo Algorithm to use for finding a path
 196          */
 197         std::vector<v3s16> getPath(v3s16 source,
 198                         v3s16 destination,
 199                         unsigned int searchdistance,
 200                         unsigned int max_jump,
 201                         unsigned int max_drop,
 202                         PathAlgorithm algo);
 203
 204 private:
 205         /* helper functions */
 206
 207         /**
 208          * transform index pos to mappos
 209          * @param ipos a index position
 210          * @return map position
 211          */
 212         v3s16          getRealPos(v3s16 ipos);
 213
 214         /**
 215          * transform mappos to index pos
 216          * @param pos a real pos
 217          * @return index position
 218          */
 219         v3s16          getIndexPos(v3s16 pos);
 220
 221         /**
 222          * get gridnode at a specific index position
 223          * @param ipos index position
 224          * @return gridnode for index
 225          */
 226         PathGridnode &getIndexElement(v3s16 ipos);
 227
 228         /**
 229          * Get gridnode at a specific index position
 230          * @return gridnode for index
 231          */
 232         PathGridnode &getIdxElem(s16 x, s16 y, s16 z);
 233
 234         /**
 235          * invert a 3D position (change sign of coordinates)
 236          * @param pos 3D position
 237          * @return pos *-1
 238          */
 239         v3s16          invert(v3s16 pos);
 240
 241         /**
 242          * check if a index is within current search area
 243          * @param index position to validate
 244          * @return true/false
 245          */
 246         bool           isValidIndex(v3s16 index);
 247
 248
 249         /* algorithm functions */
 250
 251         /**
 252          * calculate 2D Manhattan distance to target
 253          * @param pos position to calc distance
 254          * @return integer distance
 255          */
 256         int           getXZManhattanDist(v3s16 pos);
 257
 258         /**
 259          * calculate cost of movement
 260          * @param pos real world position to start movement
 261          * @param dir direction to move to
 262          * @return cost information
 263          */
 264         PathCost     calcCost(v3s16 pos, v3s16 dir);
 265
 266         /**
 267          * recursive update whole search areas total cost information
 268          * @param ipos position to check next
 269          * @param srcdir positionc checked last time
 270          * @param total_cost cost of moving to ipos
 271          * @param level current recursion depth
 272          * @return true/false path to destination has been found
 273          */
 274         bool          updateAllCosts(v3s16 ipos, v3s16 srcdir, int current_cost, int level);
 275
 276         /**
 277          * try to find a path to destination using a heuristic function
 278          * to estimate distance to target (A* search algorithm)
 279          * @param isource start position (index pos)
 280          * @param idestination end position (index pos)
 281          * @return true/false path to destination has been found
 282          */
 283         bool          updateCostHeuristic(v3s16 isource, v3s16 idestination);
 284
 285         /**
 286          * build a vector containing all nodes from destination to source;
 287          * to be called after the node costs have been processed
 288          * @param path vector to add nodes to
 289          * @param ipos initial pos to check (index pos)
 290          * @return true/false path has been fully built
 291          */
 292         bool          buildPath(std::vector<v3s16> &path, v3s16 ipos);
 293
 294         /**
 295          * go downwards from a position until some barrier
 296          * is hit.
 297          * @param pos position from which to go downwards
 298          * @param max_down maximum distance to go downwards
 299          * @return new position after movement; if too far down,
 300          * pos is returned
 301          */
 302         v3s16         walkDownwards(v3s16 pos, unsigned int max_down);
 303
 304         /* variables */
 305         int m_max_index_x = 0;            /**< max index of search area in x direction  */
 306         int m_max_index_y = 0;            /**< max index of search area in y direction  */
 307         int m_max_index_z = 0;            /**< max index of search area in z direction  */
 308
 309
 310         int m_searchdistance = 0;         /**< max distance to search in each direction */
 311         int m_maxdrop = 0;                /**< maximum number of blocks a path may drop */
 312         int m_maxjump = 0;                /**< maximum number of blocks a path may jump */
 313         int m_min_target_distance = 0;    /**< current smalest path to target           */
 314
 315         bool m_prefetch = true;              /**< prefetch cost data                       */
 316
 317         v3s16 m_start;                /**< source position                          */
 318         v3s16 m_destination;          /**< destination position                     */
 319
 320         core::aabbox3d<s16> m_limits; /**< position limits in real map coordinates  */
 321
 322         /** contains all map data already collected and analyzed.
 323                 Access it via the getIndexElement/getIdxElem methods. */
 324         friend class GridNodeContainer;
 325         GridNodeContainer *m_nodes_container = nullptr;
 326
 327         Map *m_map = nullptr;
 328
 329         const NodeDefManager *m_ndef = nullptr;
 330
 331         friend class PathfinderCompareHeuristic;
 332
 333 #ifdef PATHFINDER_DEBUG
 334
 335         /**
 336          * print collected cost information
 337          */
 338         void printCost();
 339
 340         /**
 341          * print collected cost information in a specific direction
 342          * @param dir direction to print
 343          */
 344         void printCost(PathDirections dir);
 345
 346         /**
 347          * print type of node as evaluated
 348          */
 349         void printType();
 350
 351         /**
 352          * print pathlenght for all nodes in search area
 353          */
 354         void printPathLen();
 355
 356         /**
 357          * print a path
 358          * @param path path to show
 359          */
 360         void printPath(std::vector<v3s16> path);
 361
 362         /**
 363          * print y direction for all movements
 364          */
 365         void printYdir();
 366
 367         /**
 368          * print y direction for moving in a specific direction
 369          * @param dir direction to show data
 370          */
 371         void printYdir(PathDirections dir);
 372
 373         /**
 374          * helper function to translate a direction to speaking text
 375          * @param dir direction to translate
 376          * @return textual name of direction
 377          */
 378         std::string dirToName(PathDirections dir);
 379 #endif
 380 };
 381
 382 /** Helper class for the open list priority queue in the A* pathfinder
 383  *  to sort the pathfinder nodes by cost.
 384  */
 385 class PathfinderCompareHeuristic
 386 {
 387         private:
 388                 Pathfinder *myPathfinder;
 389         public:
 390                 PathfinderCompareHeuristic(Pathfinder *pf)
 391                 {
 392                         myPathfinder = pf;
 393                 }
 394                 bool operator() (v3s16 pos1, v3s16 pos2) {
 395                         v3s16 ipos1 = myPathfinder->getIndexPos(pos1);
 396                         v3s16 ipos2 = myPathfinder->getIndexPos(pos2);
 397                         PathGridnode &g_pos1 = myPathfinder->getIndexElement(ipos1);
 398                         PathGridnode &g_pos2 = myPathfinder->getIndexElement(ipos2);
 399                         if (!g_pos1.valid)
 400                                 return false;
 401                         if (!g_pos2.valid)
 402                                 return false;
 403                         return g_pos1.estimated_cost > g_pos2.estimated_cost;
 404                 }
 405 };
 406
 407 /******************************************************************************/
 408 /* implementation                                                             */
 409 /******************************************************************************/
 410
 411 std::vector<v3s16> get_path(Map* map, const NodeDefManager *ndef,
 412                 v3s16 source,
 413                 v3s16 destination,
 414                 unsigned int searchdistance,
 415                 unsigned int max_jump,
 416                 unsigned int max_drop,
 417                 PathAlgorithm algo)
 418 {
 419         return Pathfinder(map, ndef).getPath(source, destination,
 420                                 searchdistance, max_jump, max_drop, algo);
 421 }
 422
 423 /******************************************************************************/
 424 PathCost::PathCost(const PathCost &b)
 425 {
 426         valid     = b.valid;
 427         y_change  = b.y_change;
 428         value     = b.value;
 429         updated   = b.updated;
 430 }
 431
 432 /******************************************************************************/
 433 PathCost &PathCost::operator= (const PathCost &b)
 434 {
 435         valid     = b.valid;
 436         y_change  = b.y_change;
 437         value     = b.value;
 438         updated   = b.updated;
 439
 440         return *this;
 441 }
 442
 443 /******************************************************************************/
 444 PathGridnode::PathGridnode(const PathGridnode &b)
 445 :       valid(b.valid),
 446         target(b.target),
 447         source(b.source),
 448         totalcost(b.totalcost),
 449         sourcedir(b.sourcedir),
 450         pos(b.pos),
 451         is_element(b.is_element),
 452         type(b.type)
 453 {
 454
 455         directions[DIR_XP] = b.directions[DIR_XP];
 456         directions[DIR_XM] = b.directions[DIR_XM];
 457         directions[DIR_ZP] = b.directions[DIR_ZP];
 458         directions[DIR_ZM] = b.directions[DIR_ZM];
 459 }
 460
 461 /******************************************************************************/
 462 PathGridnode &PathGridnode::operator= (const PathGridnode &b)
 463 {
 464         valid      = b.valid;
 465         target     = b.target;
 466         source     = b.source;
 467         is_element = b.is_element;
 468         totalcost  = b.totalcost;
 469         sourcedir  = b.sourcedir;
 470         pos        = b.pos;
 471         type       = b.type;
 472
 473         directions[DIR_XP] = b.directions[DIR_XP];
 474         directions[DIR_XM] = b.directions[DIR_XM];
 475         directions[DIR_ZP] = b.directions[DIR_ZP];
 476         directions[DIR_ZM] = b.directions[DIR_ZM];
 477
 478         return *this;
 479 }
 480
 481 /******************************************************************************/
 482 PathCost PathGridnode::getCost(v3s16 dir)
 483 {
 484         if (dir.X > 0) {
 485                 return directions[DIR_XP];
 486         }
 487         if (dir.X < 0) {
 488                 return directions[DIR_XM];
 489         }
 490         if (dir.Z > 0) {
 491                 return directions[DIR_ZP];
 492         }
 493         if (dir.Z < 0) {
 494                 return directions[DIR_ZM];
 495         }
 496         PathCost retval;
 497         return retval;
 498 }
 499
 500 /******************************************************************************/
 501 void PathGridnode::setCost(v3s16 dir, const PathCost &cost)
 502 {
 503         if (dir.X > 0) {
 504                 directions[DIR_XP] = cost;
 505         }
 506         if (dir.X < 0) {
 507                 directions[DIR_XM] = cost;
 508         }
 509         if (dir.Z > 0) {
 510                 directions[DIR_ZP] = cost;
 511         }
 512         if (dir.Z < 0) {
 513                 directions[DIR_ZM] = cost;
 514         }
 515 }
 516
 517 void GridNodeContainer::initNode(v3s16 ipos, PathGridnode *p_node)
 518 {
 519         const NodeDefManager *ndef = m_pathf->m_ndef;
 520         PathGridnode &elem = *p_node;
 521
 522         v3s16 realpos = m_pathf->getRealPos(ipos);
 523
 524         MapNode current = m_pathf->m_map->getNode(realpos);
 525         MapNode below   = m_pathf->m_map->getNode(realpos + v3s16(0, -1, 0));
 526
 527
 528         if ((current.param0 == CONTENT_IGNORE) ||
 529                         (below.param0 == CONTENT_IGNORE)) {
 530                 DEBUG_OUT("Pathfinder: " << PP(realpos) <<
 531                         " current or below is invalid element" << std::endl);
 532                 if (current.param0 == CONTENT_IGNORE) {
 533                         elem.type = 'i';
 534                         DEBUG_OUT(PP(ipos) << ": " << 'i' << std::endl);
 535                 }
 536                 return;
 537         }
 538
 539         //don't add anything if it isn't an air node
 540         if (ndef->get(current).walkable || !ndef->get(below).walkable) {
 541                         DEBUG_OUT("Pathfinder: " << PP(realpos)
 542                                 << " not on surface" << std::endl);
 543                         if (ndef->get(current).walkable) {
 544                                 elem.type = 's';
 545                                 DEBUG_OUT(PP(ipos) << ": " << 's' << std::endl);
 546                         } else {
 547                                 elem.type = '-';
 548                                 DEBUG_OUT(PP(ipos) << ": " << '-' << std::endl);
 549                         }
 550                         return;
 551         }
 552
 553         elem.valid = true;
 554         elem.pos   = realpos;
 555         elem.type  = 'g';
 556         DEBUG_OUT(PP(ipos) << ": " << 'a' << std::endl);
 557
 558         if (m_pathf->m_prefetch) {
 559                 elem.directions[DIR_XP] = m_pathf->calcCost(realpos, v3s16( 1, 0, 0));
 560                 elem.directions[DIR_XM] = m_pathf->calcCost(realpos, v3s16(-1, 0, 0));
 561                 elem.directions[DIR_ZP] = m_pathf->calcCost(realpos, v3s16( 0, 0, 1));
 562                 elem.directions[DIR_ZM] = m_pathf->calcCost(realpos, v3s16( 0, 0,-1));
 563         }
 564 }
 565
 566 ArrayGridNodeContainer::ArrayGridNodeContainer(Pathfinder *pathf, v3s16 dimensions) :
 567         m_x_stride(dimensions.Y * dimensions.Z),
 568         m_y_stride(dimensions.Z)
 569 {
 570         m_pathf = pathf;
 571
 572         m_nodes_array.resize(dimensions.X * dimensions.Y * dimensions.Z);
 573         INFO_TARGET << "Pathfinder ArrayGridNodeContainer constructor." << std::endl;
 574         for (int x = 0; x < dimensions.X; x++) {
 575                 for (int y = 0; y < dimensions.Y; y++) {
 576                         for (int z= 0; z < dimensions.Z; z++) {
 577                                 v3s16 ipos(x, y, z);
 578                                 initNode(ipos, &access(ipos));
 579                         }
 580                 }
 581         }
 582 }
 583
 584 PathGridnode &ArrayGridNodeContainer::access(v3s16 p)
 585 {
 586         return m_nodes_array[p.X * m_x_stride + p.Y * m_y_stride + p.Z];
 587 }
 588
 589 MapGridNodeContainer::MapGridNodeContainer(Pathfinder *pathf)
 590 {
 591         m_pathf = pathf;
 592 }
 593
 594 PathGridnode &MapGridNodeContainer::access(v3s16 p)
 595 {
 596         std::map<v3s16, PathGridnode>::iterator it = m_nodes.find(p);
 597         if (it != m_nodes.end()) {
 598                 return it->second;
 599         }
 600         PathGridnode &n = m_nodes[p];
 601         initNode(p, &n);
 602         return n;
 603 }
 604
 605
 606
 607 /******************************************************************************/
 608 std::vector<v3s16> Pathfinder::getPath(v3s16 source,
 609                                                         v3s16 destination,
 610                                                         unsigned int searchdistance,
 611                                                         unsigned int max_jump,
 612                                                         unsigned int max_drop,
 613                                                         PathAlgorithm algo)
 614 {
 615 #ifdef PATHFINDER_CALC_TIME
 616         timespec ts;
 617         clock_gettime(CLOCK_REALTIME, &ts);
 618 #endif
 619         std::vector<v3s16> retval;
 620
 621         //initialization
 622         m_searchdistance = searchdistance;
 623         m_maxjump = max_jump;
 624         m_maxdrop = max_drop;
 625         m_start       = source;
 626         m_destination = destination;
 627         m_min_target_distance = -1;
 628         m_prefetch = true;
 629
 630         if (algo == PA_PLAIN_NP) {
 631                 m_prefetch = false;
 632         }
 633
 634         //calculate boundaries within we're allowed to search
 635         int min_x = MYMIN(source.X, destination.X);
 636         int max_x = MYMAX(source.X, destination.X);
 637
 638         int min_y = MYMIN(source.Y, destination.Y);
 639         int max_y = MYMAX(source.Y, destination.Y);
 640
 641         int min_z = MYMIN(source.Z, destination.Z);
 642         int max_z = MYMAX(source.Z, destination.Z);
 643
 644         m_limits.MinEdge.X = min_x - searchdistance;
 645         m_limits.MinEdge.Y = min_y - searchdistance;
 646         m_limits.MinEdge.Z = min_z - searchdistance;
 647
 648         m_limits.MaxEdge.X = max_x + searchdistance;
 649         m_limits.MaxEdge.Y = max_y + searchdistance;
 650         m_limits.MaxEdge.Z = max_z + searchdistance;
 651
 652         v3s16 diff = m_limits.MaxEdge - m_limits.MinEdge;
 653
 654         m_max_index_x = diff.X;
 655         m_max_index_y = diff.Y;
 656         m_max_index_z = diff.Z;
 657
 658         delete m_nodes_container;
 659         if (diff.getLength() > 5) {
 660                 m_nodes_container = new MapGridNodeContainer(this);
 661         } else {
 662                 m_nodes_container = new ArrayGridNodeContainer(this, diff);
 663         }
 664 #ifdef PATHFINDER_DEBUG
 665         printType();
 666         printCost();
 667         printYdir();
 668 #endif
 669
 670         //fail if source or destination is walkable
 671         MapNode node_at_pos = m_map->getNode(destination);
 672         if (m_ndef->get(node_at_pos).walkable) {
 673                 VERBOSE_TARGET << "Destination is walkable. " <<
 674                                 "Pos: " << PP(destination) << std::endl;
 675                 return retval;
 676         }
 677         node_at_pos = m_map->getNode(source);
 678         if (m_ndef->get(node_at_pos).walkable) {
 679                 VERBOSE_TARGET << "Source is walkable. " <<
 680                                 "Pos: " << PP(source) << std::endl;
 681                 return retval;
 682         }
 683
 684         //If source pos is hovering above air, drop
 685         //to the first walkable node (up to m_maxdrop).
 686         //All algorithms expect the source pos to be *directly* above
 687         //a walkable node.
 688         v3s16 true_source = v3s16(source);
 689         source = walkDownwards(source, m_maxdrop);
 690
 691         //If destination pos is hovering above air, go downwards
 692         //to the first walkable node (up to m_maxjump).
 693         //This means a hovering destination pos could be reached
 694         //by a final upwards jump.
 695         v3s16 true_destination = v3s16(destination);
 696         destination = walkDownwards(destination, m_maxjump);
 697
 698         //validate and mark start and end pos
 699
 700         v3s16 StartIndex  = getIndexPos(source);
 701         v3s16 EndIndex    = getIndexPos(destination);
 702
 703         PathGridnode &startpos = getIndexElement(StartIndex);
 704         PathGridnode &endpos   = getIndexElement(EndIndex);
 705
 706         if (!startpos.valid) {
 707                 VERBOSE_TARGET << "Invalid startpos " <<
 708                                 "Index: " << PP(StartIndex) <<
 709                                 "Realpos: " << PP(getRealPos(StartIndex)) << std::endl;
 710                 return retval;
 711         }
 712         if (!endpos.valid) {
 713                 VERBOSE_TARGET << "Invalid stoppos " <<
 714                                 "Index: " << PP(EndIndex) <<
 715                                 "Realpos: " << PP(getRealPos(EndIndex)) << std::endl;
 716                 return retval;
 717         }
 718
 719         endpos.target      = true;
 720         startpos.source    = true;
 721         startpos.totalcost = 0;
 722
 723         bool update_cost_retval = false;
 724
 725         //calculate node costs
 726         switch (algo) {
 727                 case PA_DIJKSTRA:
 728                         update_cost_retval = updateAllCosts(StartIndex, v3s16(0, 0, 0), 0, 0);
 729                         break;
 730                 case PA_PLAIN_NP:
 731                 case PA_PLAIN:
 732                         update_cost_retval = updateCostHeuristic(StartIndex, EndIndex);
 733                         break;
 734                 default:
 735                         ERROR_TARGET << "Missing PathAlgorithm" << std::endl;
 736                         break;
 737         }
 738
 739         if (update_cost_retval) {
 740
 741 #ifdef PATHFINDER_DEBUG
 742                 std::cout << "Path to target found!" << std::endl;
 743                 printPathLen();
 744 #endif
 745
 746                 //find path
 747                 std::vector<v3s16> index_path;
 748                 buildPath(index_path, EndIndex);
 749                 //Now we have a path of index positions,
 750                 //and it's in reverse.
 751                 //The "true" start or end position might be missing
 752                 //since those have been given special treatment.
 753
 754 #ifdef PATHFINDER_DEBUG
 755                 std::cout << "Index path:" << std::endl;
 756                 printPath(index_path);
 757 #endif
 758                 //from here we'll make the final changes to the path
 759                 std::vector<v3s16> full_path;
 760
 761                 //calculate required size
 762                 int full_path_size = index_path.size();
 763                 if (source != true_source) {
 764                         full_path_size++;
 765                 }
 766                 if (destination != true_destination) {
 767                         full_path_size++;
 768                 }
 769                 full_path.reserve(full_path_size);
 770
 771                 //manually add true_source to start of path, if needed
 772                 if (source != true_source) {
 773                         full_path.push_back(true_source);
 774                 }
 775                 //convert all index positions to "normal" positions and insert
 776                 //them into full_path in reverse
 777                 std::vector<v3s16>::reverse_iterator rit = index_path.rbegin();
 778                 for (; rit != index_path.rend(); ++rit) {
 779                         full_path.push_back(getIndexElement(*rit).pos);
 780                 }
 781                 //manually add true_destination to end of path, if needed
 782                 if (destination != true_destination) {
 783                         full_path.push_back(true_destination);
 784                 }
 785
 786                 //Done! We now have a complete path of normal positions.
 787
 788
 789 #ifdef PATHFINDER_DEBUG
 790                 std::cout << "Full path:" << std::endl;
 791                 printPath(full_path);
 792 #endif
 793 #ifdef PATHFINDER_CALC_TIME
 794                 timespec ts2;
 795                 clock_gettime(CLOCK_REALTIME, &ts2);
 796
 797                 int ms = (ts2.tv_nsec - ts.tv_nsec)/(1000*1000);
 798                 int us = ((ts2.tv_nsec - ts.tv_nsec) - (ms*1000*1000))/1000;
 799                 int ns = ((ts2.tv_nsec - ts.tv_nsec) - ( (ms*1000*1000) + (us*1000)));
 800
 801
 802                 std::cout << "Calculating path took: " << (ts2.tv_sec - ts.tv_sec) <<
 803                                 "s " << ms << "ms " << us << "us " << ns << "ns " << std::endl;
 804 #endif
 805                 return full_path;
 806         }
 807         else {
 808 #ifdef PATHFINDER_DEBUG
 809                 printPathLen();
 810 #endif
 811                 INFO_TARGET << "No path found" << std::endl;
 812         }
 813
 814
 815         //return
 816         return retval;
 817 }
 818
 819 Pathfinder::~Pathfinder()
 820 {
 821         delete m_nodes_container;
 822 }
 823 /******************************************************************************/
 824 v3s16 Pathfinder::getRealPos(v3s16 ipos)
 825 {
 826         return m_limits.MinEdge + ipos;
 827 }
 828
 829 /******************************************************************************/
 830 PathCost Pathfinder::calcCost(v3s16 pos, v3s16 dir)
 831 {
 832         PathCost retval;
 833
 834         retval.updated = true;
 835
 836         v3s16 pos2 = pos + dir;
 837
 838         //check limits
 839         if (!m_limits.isPointInside(pos2)) {
 840                 DEBUG_OUT("Pathfinder: " << PP(pos2) <<
 841                                 " no cost -> out of limits" << std::endl);
 842                 return retval;
 843         }
 844
 845         MapNode node_at_pos2 = m_map->getNode(pos2);
 846
 847         //did we get information about node?
 848         if (node_at_pos2.param0 == CONTENT_IGNORE ) {
 849                         VERBOSE_TARGET << "Pathfinder: (1) area at pos: "
 850                                         << PP(pos2) << " not loaded";
 851                         return retval;
 852         }
 853
 854         if (!m_ndef->get(node_at_pos2).walkable) {
 855                 MapNode node_below_pos2 =
 856                         m_map->getNode(pos2 + v3s16(0, -1, 0));
 857
 858                 //did we get information about node?
 859                 if (node_below_pos2.param0 == CONTENT_IGNORE ) {
 860                                 VERBOSE_TARGET << "Pathfinder: (2) area at pos: "
 861                                         << PP((pos2 + v3s16(0, -1, 0))) << " not loaded";
 862                                 return retval;
 863                 }
 864
 865                 //test if the same-height neighbor is suitable
 866                 if (m_ndef->get(node_below_pos2).walkable) {
 867                         //SUCCESS!
 868                         retval.valid = true;
 869                         retval.value = 1;
 870                         retval.y_change = 0;
 871                         DEBUG_OUT("Pathfinder: "<< PP(pos)
 872                                         << " cost same height found" << std::endl);
 873                 }
 874                 else {
 875                         //test if we can fall a couple of nodes (m_maxdrop)
 876                         v3s16 testpos = pos2 + v3s16(0, -1, 0);
 877                         MapNode node_at_pos = m_map->getNode(testpos);
 878
 879                         while ((node_at_pos.param0 != CONTENT_IGNORE) &&
 880                                         (!m_ndef->get(node_at_pos).walkable) &&
 881                                         (testpos.Y > m_limits.MinEdge.Y)) {
 882                                 testpos += v3s16(0, -1, 0);
 883                                 node_at_pos = m_map->getNode(testpos);
 884                         }
 885
 886                         //did we find surface?
 887                         if ((testpos.Y >= m_limits.MinEdge.Y) &&
 888                                         (node_at_pos.param0 != CONTENT_IGNORE) &&
 889                                         (m_ndef->get(node_at_pos).walkable)) {
 890                                 if ((pos2.Y - testpos.Y - 1) <= m_maxdrop) {
 891                                         //SUCCESS!
 892                                         retval.valid = true;
 893                                         retval.value = 2;
 894                                         //difference of y-pos +1 (target node is ABOVE solid node)
 895                                         retval.y_change = ((testpos.Y - pos2.Y) +1);
 896                                         DEBUG_OUT("Pathfinder cost below height found" << std::endl);
 897                                 }
 898                                 else {
 899                                         INFO_TARGET << "Pathfinder:"
 900                                                         " distance to surface below too big: "
 901                                                         << (testpos.Y - pos2.Y) << " max: " << m_maxdrop
 902                                                         << std::endl;
 903                                 }
 904                         }
 905                         else {
 906                                 DEBUG_OUT("Pathfinder: no surface below found" << std::endl);
 907                         }
 908                 }
 909         }
 910         else {
 911                 //test if we can jump upwards (m_maxjump)
 912
 913                 v3s16 targetpos = pos2; // position for jump target
 914                 v3s16 jumppos = pos; // position for checking if jumping space is free
 915                 MapNode node_target = m_map->getNode(targetpos);
 916                 MapNode node_jump = m_map->getNode(jumppos);
 917                 bool headbanger = false; // true if anything blocks jumppath
 918
 919                 while ((node_target.param0 != CONTENT_IGNORE) &&
 920                                 (m_ndef->get(node_target).walkable) &&
 921                                 (targetpos.Y < m_limits.MaxEdge.Y)) {
 922                         //if the jump would hit any solid node, discard
 923                         if ((node_jump.param0 == CONTENT_IGNORE) ||
 924                                         (m_ndef->get(node_jump).walkable)) {
 925                                         headbanger = true;
 926                                 break;
 927                         }
 928                         targetpos += v3s16(0, 1, 0);
 929                         jumppos   += v3s16(0, 1, 0);
 930                         node_target = m_map->getNode(targetpos);
 931                         node_jump   = m_map->getNode(jumppos);
 932
 933                 }
 934                 //check headbanger one last time
 935                 if ((node_jump.param0 == CONTENT_IGNORE) ||
 936                         (m_ndef->get(node_jump).walkable)) {
 937                         headbanger = true;
 938                 }
 939
 940                 //did we find surface without banging our head?
 941                 if ((!headbanger) && (targetpos.Y <= m_limits.MaxEdge.Y) &&
 942                                 (!m_ndef->get(node_target).walkable)) {
 943
 944                         if (targetpos.Y - pos2.Y <= m_maxjump) {
 945                                 //SUCCESS!
 946                                 retval.valid = true;
 947                                 retval.value = 2;
 948                                 retval.y_change = (targetpos.Y - pos2.Y);
 949                                 DEBUG_OUT("Pathfinder cost above found" << std::endl);
 950                         }
 951                         else {
 952                                 DEBUG_OUT("Pathfinder: distance to surface above too big: "
 953                                                 << (targetpos.Y - pos2.Y) << " max: " << m_maxjump
 954                                                 << std::endl);
 955                         }
 956                 }
 957                 else {
 958                         DEBUG_OUT("Pathfinder: no surface above found" << std::endl);
 959                 }
 960         }
 961         return retval;
 962 }
 963
 964 /******************************************************************************/
 965 v3s16 Pathfinder::getIndexPos(v3s16 pos)
 966 {
 967         return pos - m_limits.MinEdge;
 968 }
 969
 970 /******************************************************************************/
 971 PathGridnode &Pathfinder::getIndexElement(v3s16 ipos)
 972 {
 973         return m_nodes_container->access(ipos);
 974 }
 975
 976 /******************************************************************************/
 977 inline PathGridnode &Pathfinder::getIdxElem(s16 x, s16 y, s16 z)
 978 {
 979         return m_nodes_container->access(v3s16(x,y,z));
 980 }
 981
 982 /******************************************************************************/
 983 bool Pathfinder::isValidIndex(v3s16 index)
 984 {
 985         if (    (index.X < m_max_index_x) &&
 986                         (index.Y < m_max_index_y) &&
 987                         (index.Z < m_max_index_z) &&
 988                         (index.X >= 0) &&
 989                         (index.Y >= 0) &&
 990                         (index.Z >= 0))
 991                 return true;
 992
 993         return false;
 994 }
 995
 996 /******************************************************************************/
 997 v3s16 Pathfinder::invert(v3s16 pos)
 998 {
 999         v3s16 retval = pos;
1000
1001         retval.X *=-1;
1002         retval.Y *=-1;
1003         retval.Z *=-1;
1004
1005         return retval;
1006 }
1007
1008 /******************************************************************************/
1009 bool Pathfinder::updateAllCosts(v3s16 ipos,
1010                                                                 v3s16 srcdir,
1011                                                                 int current_cost,
1012                                                                 int level)
1013 {
1014         PathGridnode &g_pos = getIndexElement(ipos);
1015         g_pos.totalcost = current_cost;
1016         g_pos.sourcedir = srcdir;
1017
1018         level ++;
1019
1020         //check if target has been found
1021         if (g_pos.target) {
1022                 m_min_target_distance = current_cost;
1023                 DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl);
1024                 return true;
1025         }
1026
1027         bool retval = false;
1028
1029         // the 4 cardinal directions
1030         const static v3s16 directions[4] = {
1031                 v3s16(1,0, 0),
1032                 v3s16(-1,0, 0),
1033                 v3s16(0,0, 1),
1034                 v3s16(0,0,-1)
1035         };
1036
1037         for (v3s16 direction : directions) {
1038                 if (direction != srcdir) {
1039                         PathCost cost = g_pos.getCost(direction);
1040
1041                         if (cost.valid) {
1042                                 direction.Y = cost.y_change;
1043
1044                                 v3s16 ipos2 = ipos + direction;
1045
1046                                 if (!isValidIndex(ipos2)) {
1047                                         DEBUG_OUT(LVL " Pathfinder: " << PP(ipos2) <<
1048                                                 " out of range, max=" << PP(m_limits.MaxEdge) << std::endl);
1049                                         continue;
1050                                 }
1051
1052                                 PathGridnode &g_pos2 = getIndexElement(ipos2);
1053
1054                                 if (!g_pos2.valid) {
1055                                         VERBOSE_TARGET << LVL "Pathfinder: no data for new position: "
1056                                                                                                 << PP(ipos2) << std::endl;
1057                                         continue;
1058                                 }
1059
1060                                 assert(cost.value > 0);
1061
1062                                 int new_cost = current_cost + cost.value;
1063
1064                                 // check if there already is a smaller path
1065                                 if ((m_min_target_distance > 0) &&
1066                                                 (m_min_target_distance < new_cost)) {
1067                                         return false;
1068                                 }
1069
1070                                 if ((g_pos2.totalcost < 0) ||
1071                                                 (g_pos2.totalcost > new_cost)) {
1072                                         DEBUG_OUT(LVL "Pathfinder: updating path at: "<<
1073                                                         PP(ipos2) << " from: " << g_pos2.totalcost << " to "<<
1074                                                         new_cost << std::endl);
1075                                         if (updateAllCosts(ipos2, invert(direction),
1076                                                                                         new_cost, level)) {
1077                                                 retval = true;
1078                                                 }
1079                                         }
1080                                 else {
1081                                         DEBUG_OUT(LVL "Pathfinder:"
1082                                                         " already found shorter path to: "
1083                                                         << PP(ipos2) << std::endl);
1084                                 }
1085                         }
1086                         else {
1087                                 DEBUG_OUT(LVL "Pathfinder:"
1088                                                 " not moving to invalid direction: "
1089                                                 << PP(directions[i]) << std::endl);
1090                         }
1091                 }
1092         }
1093         return retval;
1094 }
1095
1096 /******************************************************************************/
1097 int Pathfinder::getXZManhattanDist(v3s16 pos)
1098 {
1099         int min_x = MYMIN(pos.X, m_destination.X);
1100         int max_x = MYMAX(pos.X, m_destination.X);
1101         int min_z = MYMIN(pos.Z, m_destination.Z);
1102         int max_z = MYMAX(pos.Z, m_destination.Z);
1103
1104         return (max_x - min_x) + (max_z - min_z);
1105 }
1106
1107
1108
1109 /******************************************************************************/
1110 bool Pathfinder::updateCostHeuristic(v3s16 isource, v3s16 idestination)
1111 {
1112         // A* search algorithm.
1113
1114         // The open list contains the pathfinder nodes that still need to be
1115         // checked. The priority queue sorts the pathfinder nodes by
1116         // estimated cost, with lowest cost on the top.
1117         std::priority_queue<v3s16, std::vector<v3s16>, PathfinderCompareHeuristic>
1118                         openList(PathfinderCompareHeuristic(this));
1119
1120         v3s16 source = getRealPos(isource);
1121         v3s16 destination = getRealPos(idestination);
1122
1123         // initial position
1124         openList.push(source);
1125
1126         // the 4 cardinal directions
1127         const static v3s16 directions[4] = {
1128                 v3s16(1,0, 0),
1129                 v3s16(-1,0, 0),
1130                 v3s16(0,0, 1),
1131                 v3s16(0,0,-1)
1132         };
1133
1134         v3s16 current_pos;
1135         PathGridnode& s_pos = getIndexElement(isource);
1136         s_pos.source = true;
1137         s_pos.totalcost = 0;
1138
1139         // estimated cost from start to finish
1140         int cur_manhattan = getXZManhattanDist(destination);
1141         s_pos.estimated_cost = cur_manhattan;
1142
1143         while (!openList.empty()) {
1144                 // Pick node with lowest total cost estimate.
1145                 // The "cheapest" node is always on top.
1146                 current_pos = openList.top();
1147                 openList.pop();
1148                 v3s16 ipos = getIndexPos(current_pos);
1149
1150                 // check if node is inside searchdistance and valid
1151                 if (!isValidIndex(ipos)) {
1152                         DEBUG_OUT(LVL " Pathfinder: " << PP(current_pos) <<
1153                                 " out of search distance, max=" << PP(m_limits.MaxEdge) << std::endl);
1154                         continue;
1155                 }
1156
1157                 PathGridnode& g_pos = getIndexElement(ipos);
1158                 g_pos.is_closed = true;
1159                 g_pos.is_open = false;
1160                 if (!g_pos.valid) {
1161                         continue;
1162                 }
1163
1164                 if (current_pos == destination) {
1165                         // destination found, terminate
1166                         g_pos.target = true;
1167                         return true;
1168                 }
1169
1170                 // for this node, check the 4 cardinal directions
1171                 for (v3s16 direction_flat : directions) {
1172                         int current_totalcost = g_pos.totalcost;
1173
1174                         // get cost from current node to currently checked direction
1175                         PathCost cost = g_pos.getCost(direction_flat);
1176                         if (!cost.updated) {
1177                                 cost = calcCost(current_pos, direction_flat);
1178                                 g_pos.setCost(direction_flat, cost);
1179                         }
1180                         // update Y component of direction if neighbor requires jump or fall
1181                         v3s16 direction_3d = v3s16(direction_flat);
1182                         direction_3d.Y = cost.y_change;
1183
1184                         // get position of true neighbor
1185                         v3s16 neighbor = current_pos + direction_3d;
1186                         v3s16 ineighbor = getIndexPos(neighbor);
1187                         PathGridnode &n_pos = getIndexElement(ineighbor);
1188
1189                         if (cost.valid && !n_pos.is_closed && !n_pos.is_open) {
1190                                 // heuristic function; estimate cost from neighbor to destination
1191                                 cur_manhattan = getXZManhattanDist(neighbor);
1192
1193                                 // add neighbor to open list
1194                                 n_pos.sourcedir = invert(direction_3d);
1195                                 n_pos.totalcost = current_totalcost + cost.value;
1196                                 n_pos.estimated_cost = current_totalcost + cost.value + cur_manhattan;
1197                                 n_pos.is_open = true;
1198                                 openList.push(neighbor);
1199                         }
1200                 }
1201         }
1202         // no path found; all possible nodes within searchdistance have been exhausted
1203         return false;
1204 }
1205
1206 /******************************************************************************/
1207 bool Pathfinder::buildPath(std::vector<v3s16> &path, v3s16 ipos)
1208 {
1209         // The cost calculation should have set a source direction for all relevant nodes.
1210         // To build the path, we go backwards from the destination until we reach the start.
1211         for(u32 waypoints = 1; waypoints++; ) {
1212                 if (waypoints > PATHFINDER_MAX_WAYPOINTS) {
1213                         ERROR_TARGET << "Pathfinder: buildPath: path is too long (too many waypoints), aborting" << std::endl;
1214                         return false;
1215                 }
1216                 // Insert node into path
1217                 PathGridnode &g_pos = getIndexElement(ipos);
1218                 if (!g_pos.valid) {
1219                         ERROR_TARGET << "Pathfinder: buildPath: invalid next pos detected, aborting" << std::endl;
1220                         return false;
1221                 }
1222
1223                 g_pos.is_element = true;
1224                 path.push_back(ipos);
1225                 if (g_pos.source)
1226                         // start node found, terminate
1227                         return true;
1228
1229                 // go to the node from which the pathfinder came
1230                 ipos += g_pos.sourcedir;
1231         }
1232
1233         ERROR_TARGET << "Pathfinder: buildPath: no source node found" << std::endl;
1234         return false;
1235 }
1236
1237 /******************************************************************************/
1238 v3s16 Pathfinder::walkDownwards(v3s16 pos, unsigned int max_down) {
1239         if (max_down == 0)
1240                 return pos;
1241         v3s16 testpos = v3s16(pos);
1242         MapNode node_at_pos = m_map->getNode(testpos);
1243         unsigned int down = 0;
1244         while ((node_at_pos.param0 != CONTENT_IGNORE) &&
1245                         (!m_ndef->get(node_at_pos).walkable) &&
1246                         (testpos.Y > m_limits.MinEdge.Y) &&
1247                         (down <= max_down)) {
1248                 testpos += v3s16(0, -1, 0);
1249                 down++;
1250                 node_at_pos = m_map->getNode(testpos);
1251         }
1252         //did we find surface?
1253         if ((testpos.Y >= m_limits.MinEdge.Y) &&
1254                         (node_at_pos.param0 != CONTENT_IGNORE) &&
1255                         (m_ndef->get(node_at_pos).walkable)) {
1256                 if (down == 0) {
1257                         pos = testpos;
1258                 } else if ((down - 1) <= max_down) {
1259                         //difference of y-pos +1 (target node is ABOVE solid node)
1260                         testpos += v3s16(0, 1, 0);
1261                         pos = testpos;
1262                 }
1263                 else {
1264                         VERBOSE_TARGET << "Pos too far above ground: " <<
1265                                 "Index: " << PP(getIndexPos(pos)) <<
1266                                 "Realpos: " << PP(getRealPos(getIndexPos(pos))) << std::endl;
1267                 }
1268         } else {
1269                 DEBUG_OUT("Pathfinder: no surface found below pos" << std::endl);
1270         }
1271         return pos;
1272 }
1273
1274 #ifdef PATHFINDER_DEBUG
1275
1276 /******************************************************************************/
1277 void Pathfinder::printCost()
1278 {
1279         printCost(DIR_XP);
1280         printCost(DIR_XM);
1281         printCost(DIR_ZP);
1282         printCost(DIR_ZM);
1283 }
1284
1285 /******************************************************************************/
1286 void Pathfinder::printYdir()
1287 {
1288         printYdir(DIR_XP);
1289         printYdir(DIR_XM);
1290         printYdir(DIR_ZP);
1291         printYdir(DIR_ZM);
1292 }
1293
1294 /******************************************************************************/
1295 void Pathfinder::printCost(PathDirections dir)
1296 {
1297         std::cout << "Cost in direction: " << dirToName(dir) << std::endl;
1298         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1299         std::cout << std::setfill(' ');
1300         for (int y = 0; y < m_max_index_y; y++) {
1301
1302                 std::cout << "Level: " << y << std::endl;
1303
1304                 std::cout << std::setw(4) << " " << "  ";
1305                 for (int x = 0; x < m_max_index_x; x++) {
1306                         std::cout << std::setw(4) << x;
1307                 }
1308                 std::cout << std::endl;
1309
1310                 for (int z = 0; z < m_max_index_z; z++) {
1311                         std::cout << std::setw(4) << z <<": ";
1312                         for (int x = 0; x < m_max_index_x; x++) {
1313                                 if (getIdxElem(x, y, z).directions[dir].valid)
1314                                         std::cout << std::setw(4)
1315                                                 << getIdxElem(x, y, z).directions[dir].value;
1316                                 else
1317                                         std::cout << std::setw(4) << "-";
1318                                 }
1319                         std::cout << std::endl;
1320                 }
1321                 std::cout << std::endl;
1322         }
1323 }
1324
1325 /******************************************************************************/
1326 void Pathfinder::printYdir(PathDirections dir)
1327 {
1328         std::cout << "Height difference in direction: " << dirToName(dir) << std::endl;
1329         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1330         std::cout << std::setfill(' ');
1331         for (int y = 0; y < m_max_index_y; y++) {
1332
1333                 std::cout << "Level: " << y << std::endl;
1334
1335                 std::cout << std::setw(4) << " " << "  ";
1336                 for (int x = 0; x < m_max_index_x; x++) {
1337                         std::cout << std::setw(4) << x;
1338                 }
1339                 std::cout << std::endl;
1340
1341                 for (int z = 0; z < m_max_index_z; z++) {
1342                         std::cout << std::setw(4) << z <<": ";
1343                         for (int x = 0; x < m_max_index_x; x++) {
1344                                 if (getIdxElem(x, y, z).directions[dir].valid)
1345                                         std::cout << std::setw(4)
1346                                                 << getIdxElem(x, y, z).directions[dir].y_change;
1347                                 else
1348                                         std::cout << std::setw(4) << "-";
1349                                 }
1350                         std::cout << std::endl;
1351                 }
1352                 std::cout << std::endl;
1353         }
1354 }
1355
1356 /******************************************************************************/
1357 void Pathfinder::printType()
1358 {
1359         std::cout << "Type of node:" << std::endl;
1360         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1361         std::cout << std::setfill(' ');
1362         for (int y = 0; y < m_max_index_y; y++) {
1363
1364                 std::cout << "Level: " << y << std::endl;
1365
1366                 std::cout << std::setw(3) << " " << "  ";
1367                 for (int x = 0; x < m_max_index_x; x++) {
1368                         std::cout << std::setw(3) << x;
1369                 }
1370                 std::cout << std::endl;
1371
1372                 for (int z = 0; z < m_max_index_z; z++) {
1373                         std::cout << std::setw(3) << z <<": ";
1374                         for (int x = 0; x < m_max_index_x; x++) {
1375                                 char toshow = getIdxElem(x, y, z).type;
1376                                 std::cout << std::setw(3) << toshow;
1377                         }
1378                         std::cout << std::endl;
1379                 }
1380                 std::cout << std::endl;
1381         }
1382         std::cout << std::endl;
1383 }
1384
1385 /******************************************************************************/
1386 void Pathfinder::printPathLen()
1387 {
1388         std::cout << "Pathlen:" << std::endl;
1389                 std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1390                 std::cout << std::setfill(' ');
1391                 for (int y = 0; y < m_max_index_y; y++) {
1392
1393                         std::cout << "Level: " << y << std::endl;
1394
1395                         std::cout << std::setw(3) << " " << "  ";
1396                         for (int x = 0; x < m_max_index_x; x++) {
1397                                 std::cout << std::setw(3) << x;
1398                         }
1399                         std::cout << std::endl;
1400
1401                         for (int z = 0; z < m_max_index_z; z++) {
1402                                 std::cout << std::setw(3) << z <<": ";
1403                                 for (int x = 0; x < m_max_index_x; x++) {
1404                                         std::cout << std::setw(3) << getIdxElem(x, y, z).totalcost;
1405                                 }
1406                                 std::cout << std::endl;
1407                         }
1408                         std::cout << std::endl;
1409                 }
1410                 std::cout << std::endl;
1411 }
1412
1413 /******************************************************************************/
1414 std::string Pathfinder::dirToName(PathDirections dir)
1415 {
1416         switch (dir) {
1417         case DIR_XP:
1418                 return "XP";
1419                 break;
1420         case DIR_XM:
1421                 return "XM";
1422                 break;
1423         case DIR_ZP:
1424                 return "ZP";
1425                 break;
1426         case DIR_ZM:
1427                 return "ZM";
1428                 break;
1429         default:
1430                 return "UKN";
1431         }
1432 }
1433
1434 /******************************************************************************/
1435 void Pathfinder::printPath(std::vector<v3s16> path)
1436 {
1437         unsigned int current = 0;
1438         for (std::vector<v3s16>::iterator i = path.begin();
1439                         i != path.end(); ++i) {
1440                 std::cout << std::setw(3) << current << ":" << PP((*i)) << std::endl;
1441                 current++;
1442         }
1443 }
1444
1445 #endif