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