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