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