src/pathfinder.cpp

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