src/pathfinder.cpp

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