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