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                 for (const v3s16 &i : path) {
 711                         full_path.push_back(getIndexElement(i).pos);
 712                 }
 713
 714 #ifdef PATHFINDER_DEBUG
 715                 std::cout << "full path:" << std::endl;
 716                 printPath(full_path);
 717 #endif
 718 #ifdef PATHFINDER_CALC_TIME
 719                 timespec ts2;
 720                 clock_gettime(CLOCK_REALTIME, &ts2);
 721
 722                 int ms = (ts2.tv_nsec - ts.tv_nsec)/(1000*1000);
 723                 int us = ((ts2.tv_nsec - ts.tv_nsec) - (ms*1000*1000))/1000;
 724                 int ns = ((ts2.tv_nsec - ts.tv_nsec) - ( (ms*1000*1000) + (us*1000)));
 725
 726
 727                 std::cout << "Calculating path took: " << (ts2.tv_sec - ts.tv_sec) <<
 728                                 "s " << ms << "ms " << us << "us " << ns << "ns " << std::endl;
 729 #endif
 730                 return full_path;
 731         }
 732         else {
 733 #ifdef PATHFINDER_DEBUG
 734                 printPathLen();
 735 #endif
 736                 ERROR_TARGET << "failed to update cost map"<< std::endl;
 737         }
 738
 739
 740         //return
 741         return retval;
 742 }
 743
 744 Pathfinder::~Pathfinder()
 745 {
 746         delete m_nodes_container;
 747 }
 748 /******************************************************************************/
 749 v3s16 Pathfinder::getRealPos(v3s16 ipos)
 750 {
 751         return m_limits.MinEdge + ipos;
 752 }
 753
 754 /******************************************************************************/
 755 PathCost Pathfinder::calcCost(v3s16 pos, v3s16 dir)
 756 {
 757         const NodeDefManager *ndef = m_env->getGameDef()->ndef();
 758         PathCost retval;
 759
 760         retval.updated = true;
 761
 762         v3s16 pos2 = pos + dir;
 763
 764         //check limits
 765         if (!m_limits.isPointInside(pos2)) {
 766                 DEBUG_OUT("Pathfinder: " << PP(pos2) <<
 767                                 " no cost -> out of limits" << std::endl);
 768                 return retval;
 769         }
 770
 771         MapNode node_at_pos2 = m_env->getMap().getNodeNoEx(pos2);
 772
 773         //did we get information about node?
 774         if (node_at_pos2.param0 == CONTENT_IGNORE ) {
 775                         VERBOSE_TARGET << "Pathfinder: (1) area at pos: "
 776                                         << PP(pos2) << " not loaded";
 777                         return retval;
 778         }
 779
 780         if (!ndef->get(node_at_pos2).walkable) {
 781                 MapNode node_below_pos2 =
 782                                                         m_env->getMap().getNodeNoEx(pos2 + v3s16(0, -1, 0));
 783
 784                 //did we get information about node?
 785                 if (node_below_pos2.param0 == CONTENT_IGNORE ) {
 786                                 VERBOSE_TARGET << "Pathfinder: (2) area at pos: "
 787                                         << PP((pos2 + v3s16(0, -1, 0))) << " not loaded";
 788                                 return retval;
 789                 }
 790
 791                 if (ndef->get(node_below_pos2).walkable) {
 792                         retval.valid = true;
 793                         retval.value = 1;
 794                         retval.direction = 0;
 795                         DEBUG_OUT("Pathfinder: "<< PP(pos)
 796                                         << " cost same height found" << std::endl);
 797                 }
 798                 else {
 799                         v3s16 testpos = pos2 - v3s16(0, -1, 0);
 800                         MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos);
 801
 802                         while ((node_at_pos.param0 != CONTENT_IGNORE) &&
 803                                         (!ndef->get(node_at_pos).walkable) &&
 804                                         (testpos.Y > m_limits.MinEdge.Y)) {
 805                                 testpos += v3s16(0, -1, 0);
 806                                 node_at_pos = m_env->getMap().getNodeNoEx(testpos);
 807                         }
 808
 809                         //did we find surface?
 810                         if ((testpos.Y >= m_limits.MinEdge.Y) &&
 811                                         (node_at_pos.param0 != CONTENT_IGNORE) &&
 812                                         (ndef->get(node_at_pos).walkable)) {
 813                                 if ((pos2.Y - testpos.Y - 1) <= m_maxdrop) {
 814                                         retval.valid = true;
 815                                         retval.value = 2;
 816                                         //difference of y-pos +1 (target node is ABOVE solid node)
 817                                         retval.direction = ((testpos.Y - pos2.Y) +1);
 818                                         DEBUG_OUT("Pathfinder cost below height found" << std::endl);
 819                                 }
 820                                 else {
 821                                         INFO_TARGET << "Pathfinder:"
 822                                                         " distance to surface below to big: "
 823                                                         << (testpos.Y - pos2.Y) << " max: " << m_maxdrop
 824                                                         << std::endl;
 825                                 }
 826                         }
 827                         else {
 828                                 DEBUG_OUT("Pathfinder: no surface below found" << std::endl);
 829                         }
 830                 }
 831         }
 832         else {
 833                 v3s16 testpos = pos2;
 834                 MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos);
 835
 836                 while ((node_at_pos.param0 != CONTENT_IGNORE) &&
 837                                 (ndef->get(node_at_pos).walkable) &&
 838                                 (testpos.Y < m_limits.MaxEdge.Y)) {
 839                         testpos += v3s16(0, 1, 0);
 840                         node_at_pos = m_env->getMap().getNodeNoEx(testpos);
 841                 }
 842
 843                 //did we find surface?
 844                 if ((testpos.Y <= m_limits.MaxEdge.Y) &&
 845                                 (!ndef->get(node_at_pos).walkable)) {
 846
 847                         if (testpos.Y - pos2.Y <= m_maxjump) {
 848                                 retval.valid = true;
 849                                 retval.value = 2;
 850                                 retval.direction = (testpos.Y - pos2.Y);
 851                                 DEBUG_OUT("Pathfinder cost above found" << std::endl);
 852                         }
 853                         else {
 854                                 DEBUG_OUT("Pathfinder: distance to surface above to big: "
 855                                                 << (testpos.Y - pos2.Y) << " max: " << m_maxjump
 856                                                 << std::endl);
 857                         }
 858                 }
 859                 else {
 860                         DEBUG_OUT("Pathfinder: no surface above found" << std::endl);
 861                 }
 862         }
 863         return retval;
 864 }
 865
 866 /******************************************************************************/
 867 v3s16 Pathfinder::getIndexPos(v3s16 pos)
 868 {
 869         return pos - m_limits.MinEdge;
 870 }
 871
 872 /******************************************************************************/
 873 PathGridnode &Pathfinder::getIndexElement(v3s16 ipos)
 874 {
 875         return m_nodes_container->access(ipos);
 876 }
 877
 878 /******************************************************************************/
 879 inline PathGridnode &Pathfinder::getIdxElem(s16 x, s16 y, s16 z)
 880 {
 881         return m_nodes_container->access(v3s16(x,y,z));
 882 }
 883
 884 /******************************************************************************/
 885 bool Pathfinder::isValidIndex(v3s16 index)
 886 {
 887         if (    (index.X < m_max_index_x) &&
 888                         (index.Y < m_max_index_y) &&
 889                         (index.Z < m_max_index_z) &&
 890                         (index.X >= 0) &&
 891                         (index.Y >= 0) &&
 892                         (index.Z >= 0))
 893                 return true;
 894
 895         return false;
 896 }
 897
 898 /******************************************************************************/
 899 v3s16 Pathfinder::invert(v3s16 pos)
 900 {
 901         v3s16 retval = pos;
 902
 903         retval.X *=-1;
 904         retval.Y *=-1;
 905         retval.Z *=-1;
 906
 907         return retval;
 908 }
 909
 910 /******************************************************************************/
 911 bool Pathfinder::updateAllCosts(v3s16 ipos,
 912                                                                 v3s16 srcdir,
 913                                                                 int current_cost,
 914                                                                 int level)
 915 {
 916         PathGridnode &g_pos = getIndexElement(ipos);
 917         g_pos.totalcost = current_cost;
 918         g_pos.sourcedir = srcdir;
 919
 920         level ++;
 921
 922         //check if target has been found
 923         if (g_pos.target) {
 924                 m_min_target_distance = current_cost;
 925                 DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl);
 926                 return true;
 927         }
 928
 929         bool retval = false;
 930
 931         std::vector<v3s16> directions;
 932
 933         directions.emplace_back(1,0, 0);
 934         directions.emplace_back(-1,0, 0);
 935         directions.emplace_back(0,0, 1);
 936         directions.emplace_back(0,0,-1);
 937
 938         for (v3s16 &direction : directions) {
 939                 if (direction != srcdir) {
 940                         PathCost cost = g_pos.getCost(direction);
 941
 942                         if (cost.valid) {
 943                                 direction.Y = cost.direction;
 944
 945                                 v3s16 ipos2 = ipos + direction;
 946
 947                                 if (!isValidIndex(ipos2)) {
 948                                         DEBUG_OUT(LVL " Pathfinder: " << PP(ipos2) <<
 949                                                 " out of range, max=" << PP(m_limits.MaxEdge) << std::endl);
 950                                         continue;
 951                                 }
 952
 953                                 PathGridnode &g_pos2 = getIndexElement(ipos2);
 954
 955                                 if (!g_pos2.valid) {
 956                                         VERBOSE_TARGET << LVL "Pathfinder: no data for new position: "
 957                                                                                                 << PP(ipos2) << std::endl;
 958                                         continue;
 959                                 }
 960
 961                                 assert(cost.value > 0);
 962
 963                                 int new_cost = current_cost + cost.value;
 964
 965                                 // check if there already is a smaller path
 966                                 if ((m_min_target_distance > 0) &&
 967                                                 (m_min_target_distance < new_cost)) {
 968                                         return false;
 969                                 }
 970
 971                                 if ((g_pos2.totalcost < 0) ||
 972                                                 (g_pos2.totalcost > new_cost)) {
 973                                         DEBUG_OUT(LVL "Pathfinder: updating path at: "<<
 974                                                         PP(ipos2) << " from: " << g_pos2.totalcost << " to "<<
 975                                                         new_cost << std::endl);
 976                                         if (updateAllCosts(ipos2, invert(direction),
 977                                                                                         new_cost, level)) {
 978                                                 retval = true;
 979                                                 }
 980                                         }
 981                                 else {
 982                                         DEBUG_OUT(LVL "Pathfinder:"
 983                                                         " already found shorter path to: "
 984                                                         << PP(ipos2) << std::endl);
 985                                 }
 986                         }
 987                         else {
 988                                 DEBUG_OUT(LVL "Pathfinder:"
 989                                                 " not moving to invalid direction: "
 990                                                 << PP(directions[i]) << std::endl);
 991                         }
 992                 }
 993         }
 994         return retval;
 995 }
 996
 997 /******************************************************************************/
 998 int Pathfinder::getXZManhattanDist(v3s16 pos)
 999 {
1000         int min_x = MYMIN(pos.X, m_destination.X);
1001         int max_x = MYMAX(pos.X, m_destination.X);
1002         int min_z = MYMIN(pos.Z, m_destination.Z);
1003         int max_z = MYMAX(pos.Z, m_destination.Z);
1004
1005         return (max_x - min_x) + (max_z - min_z);
1006 }
1007
1008 /******************************************************************************/
1009 v3s16 Pathfinder::getDirHeuristic(std::vector<v3s16> &directions, PathGridnode &g_pos)
1010 {
1011         int   minscore = -1;
1012         v3s16 retdir   = v3s16(0, 0, 0);
1013         v3s16 srcpos = g_pos.pos;
1014         DEBUG_OUT("Pathfinder: remaining dirs at beginning:"
1015                                 << directions.size() << std::endl);
1016
1017         for (v3s16 &direction : directions) {
1018
1019                 v3s16 pos1 = v3s16(srcpos.X + direction.X, 0, srcpos.Z+ direction.Z);
1020
1021                 int cur_manhattan = getXZManhattanDist(pos1);
1022                 PathCost cost    = g_pos.getCost(direction);
1023
1024                 if (!cost.updated) {
1025                         cost = calcCost(g_pos.pos, direction);
1026                         g_pos.setCost(direction, cost);
1027                 }
1028
1029                 if (cost.valid) {
1030                         int score = cost.value + cur_manhattan;
1031
1032                         if ((minscore < 0)|| (score < minscore)) {
1033                                 minscore = score;
1034                                 retdir = direction;
1035                         }
1036                 }
1037         }
1038
1039         if (retdir != v3s16(0, 0, 0)) {
1040                 for (std::vector<v3s16>::iterator iter = directions.begin();
1041                                         iter != directions.end();
1042                                         ++iter) {
1043                         if(*iter == retdir) {
1044                                 DEBUG_OUT("Pathfinder: removing return direction" << std::endl);
1045                                 directions.erase(iter);
1046                                 break;
1047                         }
1048                 }
1049         }
1050         else {
1051                 DEBUG_OUT("Pathfinder: didn't find any valid direction clearing"
1052                                         << std::endl);
1053                 directions.clear();
1054         }
1055         DEBUG_OUT("Pathfinder: remaining dirs at end:" << directions.size()
1056                                 << std::endl);
1057         return retdir;
1058 }
1059
1060 /******************************************************************************/
1061 bool Pathfinder::updateCostHeuristic(   v3s16 ipos,
1062                                                                                 v3s16 srcdir,
1063                                                                                 int current_cost,
1064                                                                                 int level)
1065 {
1066
1067         PathGridnode &g_pos = getIndexElement(ipos);
1068         g_pos.totalcost = current_cost;
1069         g_pos.sourcedir = srcdir;
1070
1071         level ++;
1072
1073         //check if target has been found
1074         if (g_pos.target) {
1075                 m_min_target_distance = current_cost;
1076                 DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl);
1077                 return true;
1078         }
1079
1080         bool retval = false;
1081
1082         std::vector<v3s16> directions;
1083
1084         directions.emplace_back(1, 0,  0);
1085         directions.emplace_back(-1, 0,  0);
1086         directions.emplace_back(0, 0,  1);
1087         directions.emplace_back(0, 0, -1);
1088
1089         v3s16 direction = getDirHeuristic(directions, g_pos);
1090
1091         while (direction != v3s16(0, 0, 0) && (!retval)) {
1092
1093                 if (direction != srcdir) {
1094                         PathCost cost = g_pos.getCost(direction);
1095
1096                         if (cost.valid) {
1097                                 direction.Y = cost.direction;
1098
1099                                 v3s16 ipos2 = ipos + direction;
1100
1101                                 if (!isValidIndex(ipos2)) {
1102                                         DEBUG_OUT(LVL " Pathfinder: " << PP(ipos2) <<
1103                                                 " out of range, max=" << PP(m_limits.MaxEdge) << std::endl);
1104                                         direction = getDirHeuristic(directions, g_pos);
1105                                         continue;
1106                                 }
1107
1108                                 PathGridnode &g_pos2 = getIndexElement(ipos2);
1109
1110                                 if (!g_pos2.valid) {
1111                                         VERBOSE_TARGET << LVL "Pathfinder: no data for new position: "
1112                                                                                                 << PP(ipos2) << std::endl;
1113                                         direction = getDirHeuristic(directions, g_pos);
1114                                         continue;
1115                                 }
1116
1117                                 assert(cost.value > 0);
1118
1119                                 int new_cost = current_cost + cost.value;
1120
1121                                 // check if there already is a smaller path
1122                                 if ((m_min_target_distance > 0) &&
1123                                                 (m_min_target_distance < new_cost)) {
1124                                         DEBUG_OUT(LVL "Pathfinder:"
1125                                                         " already longer than best already found path "
1126                                                         << PP(ipos2) << std::endl);
1127                                         return false;
1128                                 }
1129
1130                                 if ((g_pos2.totalcost < 0) ||
1131                                                 (g_pos2.totalcost > new_cost)) {
1132                                         DEBUG_OUT(LVL "Pathfinder: updating path at: "<<
1133                                                         PP(ipos2) << " from: " << g_pos2.totalcost << " to "<<
1134                                                         new_cost << " srcdir=" <<
1135                                                         PP(invert(direction))<< std::endl);
1136                                         if (updateCostHeuristic(ipos2, invert(direction),
1137                                                                                         new_cost, level)) {
1138                                                 retval = true;
1139                                                 }
1140                                         }
1141                                 else {
1142                                         DEBUG_OUT(LVL "Pathfinder:"
1143                                                         " already found shorter path to: "
1144                                                         << PP(ipos2) << std::endl);
1145                                 }
1146                         }
1147                         else {
1148                                 DEBUG_OUT(LVL "Pathfinder:"
1149                                                 " not moving to invalid direction: "
1150                                                 << PP(direction) << std::endl);
1151                         }
1152                 }
1153                 else {
1154                         DEBUG_OUT(LVL "Pathfinder:"
1155                                                         " skipping srcdir: "
1156                                                         << PP(direction) << std::endl);
1157                 }
1158                 direction = getDirHeuristic(directions, g_pos);
1159         }
1160         return retval;
1161 }
1162
1163 /******************************************************************************/
1164 void Pathfinder::buildPath(std::vector<v3s16> &path, v3s16 pos, int level)
1165 {
1166         level ++;
1167         if (level > 700) {
1168                 ERROR_TARGET
1169                         << LVL "Pathfinder: path is too long aborting" << std::endl;
1170                 return;
1171         }
1172
1173         PathGridnode &g_pos = getIndexElement(pos);
1174         if (!g_pos.valid) {
1175                 ERROR_TARGET
1176                         << LVL "Pathfinder: invalid next pos detected aborting" << std::endl;
1177                 return;
1178         }
1179
1180         g_pos.is_element = true;
1181
1182         //check if source reached
1183         if (g_pos.source) {
1184                 path.push_back(pos);
1185                 return;
1186         }
1187
1188         buildPath(path, pos + g_pos.sourcedir, level);
1189         path.push_back(pos);
1190 }
1191
1192 /******************************************************************************/
1193 v3f Pathfinder::tov3f(v3s16 pos)
1194 {
1195         return v3f(BS * pos.X, BS * pos.Y, BS * pos.Z);
1196 }
1197
1198 #ifdef PATHFINDER_DEBUG
1199
1200 /******************************************************************************/
1201 void Pathfinder::printCost()
1202 {
1203         printCost(DIR_XP);
1204         printCost(DIR_XM);
1205         printCost(DIR_ZP);
1206         printCost(DIR_ZM);
1207 }
1208
1209 /******************************************************************************/
1210 void Pathfinder::printYdir()
1211 {
1212         printYdir(DIR_XP);
1213         printYdir(DIR_XM);
1214         printYdir(DIR_ZP);
1215         printYdir(DIR_ZM);
1216 }
1217
1218 /******************************************************************************/
1219 void Pathfinder::printCost(PathDirections dir)
1220 {
1221         std::cout << "Cost in direction: " << dirToName(dir) << std::endl;
1222         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1223         std::cout << std::setfill(' ');
1224         for (int y = 0; y < m_max_index_y; y++) {
1225
1226                 std::cout << "Level: " << y << std::endl;
1227
1228                 std::cout << std::setw(4) << " " << "  ";
1229                 for (int x = 0; x < m_max_index_x; x++) {
1230                         std::cout << std::setw(4) << x;
1231                 }
1232                 std::cout << std::endl;
1233
1234                 for (int z = 0; z < m_max_index_z; z++) {
1235                         std::cout << std::setw(4) << z <<": ";
1236                         for (int x = 0; x < m_max_index_x; x++) {
1237                                 if (getIdxElem(x, y, z).directions[dir].valid)
1238                                         std::cout << std::setw(4)
1239                                                 << getIdxElem(x, y, z).directions[dir].value;
1240                                 else
1241                                         std::cout << std::setw(4) << "-";
1242                                 }
1243                         std::cout << std::endl;
1244                 }
1245                 std::cout << std::endl;
1246         }
1247 }
1248
1249 /******************************************************************************/
1250 void Pathfinder::printYdir(PathDirections dir)
1251 {
1252         std::cout << "Height difference in direction: " << dirToName(dir) << std::endl;
1253         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1254         std::cout << std::setfill(' ');
1255         for (int y = 0; y < m_max_index_y; y++) {
1256
1257                 std::cout << "Level: " << y << std::endl;
1258
1259                 std::cout << std::setw(4) << " " << "  ";
1260                 for (int x = 0; x < m_max_index_x; x++) {
1261                         std::cout << std::setw(4) << x;
1262                 }
1263                 std::cout << std::endl;
1264
1265                 for (int z = 0; z < m_max_index_z; z++) {
1266                         std::cout << std::setw(4) << z <<": ";
1267                         for (int x = 0; x < m_max_index_x; x++) {
1268                                 if (getIdxElem(x, y, z).directions[dir].valid)
1269                                         std::cout << std::setw(4)
1270                                                 << getIdxElem(x, y, z).directions[dir].direction;
1271                                 else
1272                                         std::cout << std::setw(4) << "-";
1273                                 }
1274                         std::cout << std::endl;
1275                 }
1276                 std::cout << std::endl;
1277         }
1278 }
1279
1280 /******************************************************************************/
1281 void Pathfinder::printType()
1282 {
1283         std::cout << "Type of node:" << std::endl;
1284         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1285         std::cout << std::setfill(' ');
1286         for (int y = 0; y < m_max_index_y; y++) {
1287
1288                 std::cout << "Level: " << y << std::endl;
1289
1290                 std::cout << std::setw(3) << " " << "  ";
1291                 for (int x = 0; x < m_max_index_x; x++) {
1292                         std::cout << std::setw(3) << x;
1293                 }
1294                 std::cout << std::endl;
1295
1296                 for (int z = 0; z < m_max_index_z; z++) {
1297                         std::cout << std::setw(3) << z <<": ";
1298                         for (int x = 0; x < m_max_index_x; x++) {
1299                                 char toshow = getIdxElem(x, y, z).type;
1300                                 std::cout << std::setw(3) << toshow;
1301                         }
1302                         std::cout << std::endl;
1303                 }
1304                 std::cout << std::endl;
1305         }
1306         std::cout << std::endl;
1307 }
1308
1309 /******************************************************************************/
1310 void Pathfinder::printPathLen()
1311 {
1312         std::cout << "Pathlen:" << std::endl;
1313                 std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1314                 std::cout << std::setfill(' ');
1315                 for (int y = 0; y < m_max_index_y; y++) {
1316
1317                         std::cout << "Level: " << y << std::endl;
1318
1319                         std::cout << std::setw(3) << " " << "  ";
1320                         for (int x = 0; x < m_max_index_x; x++) {
1321                                 std::cout << std::setw(3) << x;
1322                         }
1323                         std::cout << std::endl;
1324
1325                         for (int z = 0; z < m_max_index_z; z++) {
1326                                 std::cout << std::setw(3) << z <<": ";
1327                                 for (int x = 0; x < m_max_index_x; x++) {
1328                                         std::cout << std::setw(3) << getIdxElem(x, y, z).totalcost;
1329                                 }
1330                                 std::cout << std::endl;
1331                         }
1332                         std::cout << std::endl;
1333                 }
1334                 std::cout << std::endl;
1335 }
1336
1337 /******************************************************************************/
1338 std::string Pathfinder::dirToName(PathDirections dir)
1339 {
1340         switch (dir) {
1341         case DIR_XP:
1342                 return "XP";
1343                 break;
1344         case DIR_XM:
1345                 return "XM";
1346                 break;
1347         case DIR_ZP:
1348                 return "ZP";
1349                 break;
1350         case DIR_ZM:
1351                 return "ZM";
1352                 break;
1353         default:
1354                 return "UKN";
1355         }
1356 }
1357
1358 /******************************************************************************/
1359 void Pathfinder::printPath(std::vector<v3s16> path)
1360 {
1361         unsigned int current = 0;
1362         for (std::vector<v3s16>::iterator i = path.begin();
1363                         i != path.end(); ++i) {
1364                 std::cout << std::setw(3) << current << ":" << PP((*i)) << std::endl;
1365                 current++;
1366         }
1367 }
1368
1369 #endif