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 #ifdef PATHFINDER_DEBUG
  30 #include <iomanip>
  31 #endif
  32 #ifdef PATHFINDER_CALC_TIME
  33         #include <sys/time.h>
  34 #endif
  35
  36 /******************************************************************************/
  37 /* Typedefs and macros                                                        */
  38 /******************************************************************************/
  39
  40 //#define PATHFINDER_CALC_TIME
  41
  42 /** shortcut to print a 3d pos */
  43 #define PPOS(pos) "(" << pos.X << "," << pos.Y << "," << pos.Z << ")"
  44
  45 #define LVL "(" << level << ")" <<
  46
  47 #ifdef PATHFINDER_DEBUG
  48 #define DEBUG_OUT(a)     std::cout << a
  49 #define INFO_TARGET      std::cout
  50 #define VERBOSE_TARGET   std::cout
  51 #define ERROR_TARGET     std::cout
  52 #else
  53 #define DEBUG_OUT(a)     while(0)
  54 #define INFO_TARGET      infostream
  55 #define VERBOSE_TARGET   verbosestream
  56 #define ERROR_TARGET     errorstream
  57 #endif
  58
  59 /******************************************************************************/
  60 /* implementation                                                             */
  61 /******************************************************************************/
  62
  63 std::vector<v3s16> get_Path(ServerEnvironment* env,
  64                                                         v3s16 source,
  65                                                         v3s16 destination,
  66                                                         unsigned int searchdistance,
  67                                                         unsigned int max_jump,
  68                                                         unsigned int max_drop,
  69                                                         algorithm algo) {
  70
  71         pathfinder searchclass;
  72
  73         return searchclass.get_Path(env,
  74                                 source,destination,
  75                                 searchdistance,max_jump,max_drop,algo);
  76 }
  77
  78 /******************************************************************************/
  79 path_cost::path_cost()
  80 :       valid(false),
  81         value(0),
  82         direction(0),
  83         updated(false)
  84 {
  85         //intentionaly empty
  86 }
  87
  88 /******************************************************************************/
  89 path_cost::path_cost(const path_cost& b) {
  90         valid     = b.valid;
  91         direction = b.direction;
  92         value     = b.value;
  93         updated   = b.updated;
  94 }
  95
  96 /******************************************************************************/
  97 path_cost& path_cost::operator= (const path_cost& b) {
  98         valid     = b.valid;
  99         direction = b.direction;
 100         value     = b.value;
 101         updated   = b.updated;
 102
 103         return *this;
 104 }
 105
 106 /******************************************************************************/
 107 path_gridnode::path_gridnode()
 108 :       valid(false),
 109         target(false),
 110         source(false),
 111         totalcost(-1),
 112         sourcedir(v3s16(0,0,0)),
 113         surfaces(0),
 114         pos(v3s16(0,0,0)),
 115         is_element(false),
 116         type('u')
 117 {
 118         //intentionaly empty
 119 }
 120
 121 /******************************************************************************/
 122 path_gridnode::path_gridnode(const path_gridnode& b)
 123 :       valid(b.valid),
 124         target(b.target),
 125         source(b.source),
 126         totalcost(b.totalcost),
 127         sourcedir(b.sourcedir),
 128         surfaces(b.surfaces),
 129         pos(b.pos),
 130         is_element(b.is_element),
 131         type(b.type)
 132         {
 133
 134         directions[DIR_XP] = b.directions[DIR_XP];
 135         directions[DIR_XM] = b.directions[DIR_XM];
 136         directions[DIR_ZP] = b.directions[DIR_ZP];
 137         directions[DIR_ZM] = b.directions[DIR_ZM];
 138 }
 139
 140 /******************************************************************************/
 141 path_gridnode& path_gridnode::operator= (const path_gridnode& b) {
 142         valid      = b.valid;
 143         target     = b.target;
 144         source     = b.source;
 145         is_element = b.is_element;
 146         totalcost  = b.totalcost;
 147         sourcedir  = b.sourcedir;
 148         surfaces   = b.surfaces;
 149         pos        = b.pos;
 150         type       = b.type;
 151
 152         directions[DIR_XP] = b.directions[DIR_XP];
 153         directions[DIR_XM] = b.directions[DIR_XM];
 154         directions[DIR_ZP] = b.directions[DIR_ZP];
 155         directions[DIR_ZM] = b.directions[DIR_ZM];
 156
 157         return *this;
 158 }
 159
 160 /******************************************************************************/
 161 path_cost path_gridnode::get_cost(v3s16 dir) {
 162         if (dir.X > 0) {
 163                 return directions[DIR_XP];
 164         }
 165         if (dir.X < 0) {
 166                 return directions[DIR_XM];
 167         }
 168         if (dir.Z > 0) {
 169                 return directions[DIR_ZP];
 170         }
 171         if (dir.Z < 0) {
 172                 return directions[DIR_ZM];
 173         }
 174         path_cost retval;
 175         return retval;
 176 }
 177
 178 /******************************************************************************/
 179 void path_gridnode::set_cost(v3s16 dir,path_cost cost) {
 180         if (dir.X > 0) {
 181                 directions[DIR_XP] = cost;
 182         }
 183         if (dir.X < 0) {
 184                 directions[DIR_XM] = cost;
 185         }
 186         if (dir.Z > 0) {
 187                 directions[DIR_ZP] = cost;
 188         }
 189         if (dir.Z < 0) {
 190                 directions[DIR_ZM] = cost;
 191         }
 192 }
 193
 194 /******************************************************************************/
 195 std::vector<v3s16> pathfinder::get_Path(ServerEnvironment* env,
 196                                                         v3s16 source,
 197                                                         v3s16 destination,
 198                                                         unsigned int searchdistance,
 199                                                         unsigned int max_jump,
 200                                                         unsigned int max_drop,
 201                                                         algorithm algo) {
 202 #ifdef PATHFINDER_CALC_TIME
 203         timespec ts;
 204         clock_gettime(CLOCK_REALTIME, &ts);
 205 #endif
 206         std::vector<v3s16> retval;
 207
 208         //check parameters
 209         if (env == 0) {
 210                 ERROR_TARGET << "missing environment pointer" << std::endl;
 211                 return retval;
 212         }
 213
 214         m_searchdistance = searchdistance;
 215         m_env = env;
 216         m_maxjump = max_jump;
 217         m_maxdrop = max_drop;
 218         m_start       = source;
 219         m_destination = destination;
 220         m_min_target_distance = -1;
 221         m_prefetch = true;
 222
 223         if (algo == A_PLAIN_NP) {
 224                 m_prefetch = false;
 225         }
 226
 227         int min_x = MYMIN(source.X,destination.X);
 228         int max_x = MYMAX(source.X,destination.X);
 229
 230         int min_y = MYMIN(source.Y,destination.Y);
 231         int max_y = MYMAX(source.Y,destination.Y);
 232
 233         int min_z = MYMIN(source.Z,destination.Z);
 234         int max_z = MYMAX(source.Z,destination.Z);
 235
 236         m_limits.X.min = min_x - searchdistance;
 237         m_limits.X.max = max_x + searchdistance;
 238         m_limits.Y.min = min_y - searchdistance;
 239         m_limits.Y.max = max_y + searchdistance;
 240         m_limits.Z.min = min_z - searchdistance;
 241         m_limits.Z.max = max_z + searchdistance;
 242
 243         m_max_index_x = m_limits.X.max - m_limits.X.min;
 244         m_max_index_y = m_limits.Y.max - m_limits.Y.min;
 245         m_max_index_z = m_limits.Z.max - m_limits.Z.min;
 246
 247         //build data map
 248         if (!build_costmap()) {
 249                 ERROR_TARGET << "failed to build costmap" << std::endl;
 250                 return retval;
 251         }
 252 #ifdef PATHFINDER_DEBUG
 253         print_type();
 254         print_cost();
 255         print_ydir();
 256 #endif
 257
 258         //validate and mark start and end pos
 259         v3s16 StartIndex  = getIndexPos(source);
 260         v3s16 EndIndex    = getIndexPos(destination);
 261
 262         path_gridnode& startpos = getIndexElement(StartIndex);
 263         path_gridnode& endpos   = getIndexElement(EndIndex);
 264
 265         if (!startpos.valid) {
 266                 ERROR_TARGET << "invalid startpos" <<
 267                                 "Index: " << PPOS(StartIndex) <<
 268                                 "Realpos: " << PPOS(getRealPos(StartIndex)) << std::endl;
 269                 return retval;
 270         }
 271         if (!endpos.valid) {
 272                 ERROR_TARGET << "invalid stoppos" <<
 273                                 "Index: " << PPOS(EndIndex) <<
 274                                 "Realpos: " << PPOS(getRealPos(EndIndex)) << std::endl;
 275                 return retval;
 276         }
 277
 278         endpos.target      = true;
 279         startpos.source    = true;
 280         startpos.totalcost = 0;
 281
 282         bool update_cost_retval = false;
 283
 284         switch (algo) {
 285                 case DIJKSTRA:
 286                         update_cost_retval = update_all_costs(StartIndex,v3s16(0,0,0),0,0);
 287                         break;
 288                 case A_PLAIN_NP:
 289                 case A_PLAIN:
 290                         update_cost_retval = update_cost_heuristic(StartIndex,v3s16(0,0,0),0,0);
 291                         break;
 292                 default:
 293                         ERROR_TARGET << "missing algorithm"<< std::endl;
 294                         break;
 295         }
 296
 297         if (update_cost_retval) {
 298
 299 #ifdef PATHFINDER_DEBUG
 300                 std::cout << "Path to target found!" << std::endl;
 301                 print_pathlen();
 302 #endif
 303
 304                 //find path
 305                 std::vector<v3s16> path;
 306                 build_path(path,EndIndex,0);
 307
 308 #ifdef PATHFINDER_DEBUG
 309                 std::cout << "Full index path:" << std::endl;
 310                 print_path(path);
 311 #endif
 312
 313                 //optimize path
 314                 std::vector<v3s16> optimized_path;
 315
 316                 std::vector<v3s16>::iterator startpos = path.begin();
 317                 optimized_path.push_back(source);
 318
 319                 for (std::vector<v3s16>::iterator i = path.begin();
 320                                         i != path.end(); i++) {
 321                         if (!m_env->line_of_sight(
 322                                 tov3f(getIndexElement(*startpos).pos),
 323                                 tov3f(getIndexElement(*i).pos))) {
 324                                 optimized_path.push_back(getIndexElement(*(i-1)).pos);
 325                                 startpos = (i-1);
 326                         }
 327                 }
 328
 329                 optimized_path.push_back(destination);
 330
 331 #ifdef PATHFINDER_DEBUG
 332                 std::cout << "Optimized path:" << std::endl;
 333                 print_path(optimized_path);
 334 #endif
 335 #ifdef PATHFINDER_CALC_TIME
 336                 timespec ts2;
 337                 clock_gettime(CLOCK_REALTIME, &ts2);
 338
 339                 int ms = (ts2.tv_nsec - ts.tv_nsec)/(1000*1000);
 340                 int us = ((ts2.tv_nsec - ts.tv_nsec) - (ms*1000*1000))/1000;
 341                 int ns = ((ts2.tv_nsec - ts.tv_nsec) - ( (ms*1000*1000) + (us*1000)));
 342
 343
 344                 std::cout << "Calculating path took: " << (ts2.tv_sec - ts.tv_sec) <<
 345                                 "s " << ms << "ms " << us << "us " << ns << "ns " << std::endl;
 346 #endif
 347                 return optimized_path;
 348         }
 349         else {
 350 #ifdef PATHFINDER_DEBUG
 351                 print_pathlen();
 352 #endif
 353                 ERROR_TARGET << "failed to update cost map"<< std::endl;
 354         }
 355
 356
 357         //return
 358         return retval;
 359 }
 360
 361 /******************************************************************************/
 362 pathfinder::pathfinder() :
 363         m_max_index_x(0),
 364         m_max_index_y(0),
 365         m_max_index_z(0),
 366         m_searchdistance(0),
 367         m_maxdrop(0),
 368         m_maxjump(0),
 369         m_min_target_distance(0),
 370         m_prefetch(true),
 371         m_start(0,0,0),
 372         m_destination(0,0,0),
 373         m_limits(),
 374         m_data(),
 375         m_env(0)
 376 {
 377         //intentionaly empty
 378 }
 379
 380 /******************************************************************************/
 381 v3s16 pathfinder::getRealPos(v3s16 ipos) {
 382
 383         v3s16 retval = ipos;
 384
 385         retval.X += m_limits.X.min;
 386         retval.Y += m_limits.Y.min;
 387         retval.Z += m_limits.Z.min;
 388
 389         return retval;
 390 }
 391
 392 /******************************************************************************/
 393 bool pathfinder::build_costmap()
 394 {
 395         INFO_TARGET << "Pathfinder build costmap: (" << m_limits.X.min << ","
 396                                                                                                 << m_limits.Z.min << ") ("
 397                                                                                                 << m_limits.X.max << ","
 398                                                                                                 << m_limits.Z.max << ")"
 399                                                                                                 << std::endl;
 400         m_data.resize(m_max_index_x);
 401         for (int x = 0; x < m_max_index_x; x++) {
 402                 m_data[x].resize(m_max_index_z);
 403                 for (int z = 0; z < m_max_index_z; z++) {
 404                         m_data[x][z].resize(m_max_index_y);
 405
 406                         int surfaces = 0;
 407                         for (int y = 0; y < m_max_index_y; y++) {
 408                                 v3s16 ipos(x,y,z);
 409
 410                                 v3s16 realpos = getRealPos(ipos);
 411
 412                                 MapNode current = m_env->getMap().getNodeNoEx(realpos);
 413                                 MapNode below   = m_env->getMap().getNodeNoEx(realpos + v3s16(0,-1,0));
 414
 415
 416                                 if ((current.param0 == CONTENT_IGNORE) ||
 417                                                 (below.param0 == CONTENT_IGNORE)) {
 418                                         DEBUG_OUT("Pathfinder: " << PPOS(realpos) <<
 419                                                         " current or below is invalid element" << std::endl);
 420                                         if (current.param0 == CONTENT_IGNORE) {
 421                                                 m_data[x][z][y].type = 'i';
 422                                                 DEBUG_OUT(x << "," << y << "," << z << ": " << 'i' << std::endl);
 423                                         }
 424                                         continue;
 425                                 }
 426
 427                                 //don't add anything if it isn't an air node
 428                                 if ((current.param0 != CONTENT_AIR) ||
 429                                                 (below.param0 == CONTENT_AIR )) {
 430                                                 DEBUG_OUT("Pathfinder: " << PPOS(realpos)
 431                                                                 << " not on surface" << std::endl);
 432                                                 if (current.param0 != CONTENT_AIR) {
 433                                                         m_data[x][z][y].type = 's';
 434                                                         DEBUG_OUT(x << "," << y << "," << z << ": " << 's' << std::endl);
 435                                                 }
 436                                                 else {
 437                                                         m_data[x][z][y].type   = '-';
 438                                                         DEBUG_OUT(x << "," << y << "," << z << ": " << '-' << std::endl);
 439                                                 }
 440                                                 continue;
 441                                 }
 442
 443                                 surfaces++;
 444
 445                                 m_data[x][z][y].valid  = true;
 446                                 m_data[x][z][y].pos    = realpos;
 447                                 m_data[x][z][y].type   = 'g';
 448                                 DEBUG_OUT(x << "," << y << "," << z << ": " << 'a' << std::endl);
 449
 450                                 if (m_prefetch) {
 451                                 m_data[x][z][y].directions[DIR_XP] =
 452                                                                                         calc_cost(realpos,v3s16( 1,0, 0));
 453                                 m_data[x][z][y].directions[DIR_XM] =
 454                                                                                         calc_cost(realpos,v3s16(-1,0, 0));
 455                                 m_data[x][z][y].directions[DIR_ZP] =
 456                                                                                         calc_cost(realpos,v3s16( 0,0, 1));
 457                                 m_data[x][z][y].directions[DIR_ZM] =
 458                                                                                         calc_cost(realpos,v3s16( 0,0,-1));
 459                                 }
 460
 461                         }
 462
 463                         if (surfaces >= 1 ) {
 464                                 for (int y = 0; y < m_max_index_y; y++) {
 465                                         if (m_data[x][z][y].valid) {
 466                                                 m_data[x][z][y].surfaces = surfaces;
 467                                         }
 468                                 }
 469                         }
 470                 }
 471         }
 472         return true;
 473 }
 474
 475 /******************************************************************************/
 476 path_cost pathfinder::calc_cost(v3s16 pos,v3s16 dir) {
 477         path_cost retval;
 478
 479         retval.updated = true;
 480
 481         v3s16 pos2 = pos + dir;
 482
 483         //check limits
 484         if (    (pos2.X < m_limits.X.min) ||
 485                         (pos2.X >= m_limits.X.max) ||
 486                         (pos2.Z < m_limits.Z.min) ||
 487                         (pos2.Z >= m_limits.Z.max)) {
 488                 DEBUG_OUT("Pathfinder: " << PPOS(pos2) <<
 489                                 " no cost -> out of limits" << std::endl);
 490                 return retval;
 491         }
 492
 493         MapNode node_at_pos2 = m_env->getMap().getNodeNoEx(pos2);
 494
 495         //did we get information about node?
 496         if (node_at_pos2.param0 == CONTENT_IGNORE ) {
 497                         VERBOSE_TARGET << "Pathfinder: (1) area at pos: "
 498                                         << PPOS(pos2) << " not loaded";
 499                         return retval;
 500         }
 501
 502         if (node_at_pos2.param0 == CONTENT_AIR) {
 503                 MapNode node_below_pos2 =
 504                                                         m_env->getMap().getNodeNoEx(pos2 + v3s16(0,-1,0));
 505
 506                 //did we get information about node?
 507                 if (node_below_pos2.param0 == CONTENT_IGNORE ) {
 508                                 VERBOSE_TARGET << "Pathfinder: (2) area at pos: "
 509                                         << PPOS((pos2 + v3s16(0,-1,0))) << " not loaded";
 510                                 return retval;
 511                 }
 512
 513                 if (node_below_pos2.param0 != CONTENT_AIR) {
 514                         retval.valid = true;
 515                         retval.value = 1;
 516                         retval.direction = 0;
 517                         DEBUG_OUT("Pathfinder: "<< PPOS(pos)
 518                                         << " cost same height found" << std::endl);
 519                 }
 520                 else {
 521                         v3s16 testpos = pos2 - v3s16(0,-1,0);
 522                         MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos);
 523
 524                         while ((node_at_pos.param0 != CONTENT_IGNORE) &&
 525                                         (node_at_pos.param0 == CONTENT_AIR) &&
 526                                         (testpos.Y > m_limits.Y.min)) {
 527                                 testpos += v3s16(0,-1,0);
 528                                 node_at_pos = m_env->getMap().getNodeNoEx(testpos);
 529                         }
 530
 531                         //did we find surface?
 532                         if ((testpos.Y >= m_limits.Y.min) &&
 533                                         (node_at_pos.param0 != CONTENT_IGNORE) &&
 534                                         (node_at_pos.param0 != CONTENT_AIR)) {
 535                                 if (((pos2.Y - testpos.Y)*-1) <= m_maxdrop) {
 536                                         retval.valid = true;
 537                                         retval.value = 2;
 538                                         //difference of y-pos +1 (target node is ABOVE solid node)
 539                                         retval.direction = ((testpos.Y - pos2.Y) +1);
 540                                         DEBUG_OUT("Pathfinder cost below height found" << std::endl);
 541                                 }
 542                                 else {
 543                                         INFO_TARGET << "Pathfinder:"
 544                                                         " distance to surface below to big: "
 545                                                         << (testpos.Y - pos2.Y) << " max: " << m_maxdrop
 546                                                         << std::endl;
 547                                 }
 548                         }
 549                         else {
 550                                 DEBUG_OUT("Pathfinder: no surface below found" << std::endl);
 551                         }
 552                 }
 553         }
 554         else {
 555                 v3s16 testpos = pos2;
 556                 MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos);
 557
 558                 while ((node_at_pos.param0 != CONTENT_IGNORE) &&
 559                                 (node_at_pos.param0 != CONTENT_AIR) &&
 560                                 (testpos.Y < m_limits.Y.max)) {
 561                         testpos += v3s16(0,1,0);
 562                         node_at_pos = m_env->getMap().getNodeNoEx(testpos);
 563                 }
 564
 565                 //did we find surface?
 566                 if ((testpos.Y <= m_limits.Y.max) &&
 567                                 (node_at_pos.param0 == CONTENT_AIR)) {
 568
 569                         if (testpos.Y - pos2.Y <= m_maxjump) {
 570                                 retval.valid = true;
 571                                 retval.value = 2;
 572                                 retval.direction = (testpos.Y - pos2.Y);
 573                                 DEBUG_OUT("Pathfinder cost above found" << std::endl);
 574                         }
 575                         else {
 576                                 DEBUG_OUT("Pathfinder: distance to surface above to big: "
 577                                                 << (testpos.Y - pos2.Y) << " max: " << m_maxjump
 578                                                 << std::endl);
 579                         }
 580                 }
 581                 else {
 582                         DEBUG_OUT("Pathfinder: no surface above found" << std::endl);
 583                 }
 584         }
 585         return retval;
 586 }
 587
 588 /******************************************************************************/
 589 v3s16 pathfinder::getIndexPos(v3s16 pos) {
 590
 591         v3s16 retval = pos;
 592         retval.X -= m_limits.X.min;
 593         retval.Y -= m_limits.Y.min;
 594         retval.Z -= m_limits.Z.min;
 595
 596         return retval;
 597 }
 598
 599 /******************************************************************************/
 600 path_gridnode& pathfinder::getIndexElement(v3s16 ipos) {
 601         return m_data[ipos.X][ipos.Z][ipos.Y];
 602 }
 603
 604 /******************************************************************************/
 605 bool pathfinder::valid_index(v3s16 index) {
 606         if (    (index.X < m_max_index_x) &&
 607                         (index.Y < m_max_index_y) &&
 608                         (index.Z < m_max_index_z) &&
 609                         (index.X >= 0) &&
 610                         (index.Y >= 0) &&
 611                         (index.Z >= 0))
 612                 return true;
 613
 614         return false;
 615 }
 616
 617 /******************************************************************************/
 618 v3s16 pathfinder::invert(v3s16 pos) {
 619         v3s16 retval = pos;
 620
 621         retval.X *=-1;
 622         retval.Y *=-1;
 623         retval.Z *=-1;
 624
 625         return retval;
 626 }
 627
 628 /******************************************************************************/
 629 bool pathfinder::update_all_costs(      v3s16 ipos,
 630                                                                         v3s16 srcdir,
 631                                                                         int current_cost,
 632                                                                         int level) {
 633
 634         path_gridnode& g_pos = getIndexElement(ipos);
 635         g_pos.totalcost = current_cost;
 636         g_pos.sourcedir = srcdir;
 637
 638         level ++;
 639
 640         //check if target has been found
 641         if (g_pos.target) {
 642                 m_min_target_distance = current_cost;
 643                 DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl);
 644                 return true;
 645         }
 646
 647         bool retval = false;
 648
 649         std::vector<v3s16> directions;
 650
 651         directions.push_back(v3s16( 1,0, 0));
 652         directions.push_back(v3s16(-1,0, 0));
 653         directions.push_back(v3s16( 0,0, 1));
 654         directions.push_back(v3s16( 0,0,-1));
 655
 656         for (unsigned int i=0; i < directions.size(); i++) {
 657                 if (directions[i] != srcdir) {
 658                         path_cost cost = g_pos.get_cost(directions[i]);
 659
 660                         if (cost.valid) {
 661                                 directions[i].Y = cost.direction;
 662
 663                                 v3s16 ipos2 = ipos + directions[i];
 664
 665                                 if (!valid_index(ipos2)) {
 666                                         DEBUG_OUT(LVL " Pathfinder: " << PPOS(ipos2) <<
 667                                                         " out of range (" << m_limits.X.max << "," <<
 668                                                         m_limits.Y.max << "," << m_limits.Z.max
 669                                                         <<")" << std::endl);
 670                                         continue;
 671                                 }
 672
 673                                 path_gridnode& g_pos2 = getIndexElement(ipos2);
 674
 675                                 if (!g_pos2.valid) {
 676                                         VERBOSE_TARGET << LVL "Pathfinder: no data for new position: "
 677                                                                                                 << PPOS(ipos2) << std::endl;
 678                                         continue;
 679                                 }
 680
 681                                 assert(cost.value > 0);
 682
 683                                 int new_cost = current_cost + cost.value;
 684
 685                                 // check if there already is a smaller path
 686                                 if ((m_min_target_distance > 0) &&
 687                                                 (m_min_target_distance < new_cost)) {
 688                                         return false;
 689                                 }
 690
 691                                 if ((g_pos2.totalcost < 0) ||
 692                                                 (g_pos2.totalcost > new_cost)) {
 693                                         DEBUG_OUT(LVL "Pathfinder: updating path at: "<<
 694                                                         PPOS(ipos2) << " from: " << g_pos2.totalcost << " to "<<
 695                                                         new_cost << std::endl);
 696                                         if (update_all_costs(ipos2,invert(directions[i]),
 697                                                                                         new_cost,level)) {
 698                                                 retval = true;
 699                                                 }
 700                                         }
 701                                 else {
 702                                         DEBUG_OUT(LVL "Pathfinder:"
 703                                                         " already found shorter path to: "
 704                                                         << PPOS(ipos2) << std::endl);
 705                                 }
 706                         }
 707                         else {
 708                                 DEBUG_OUT(LVL "Pathfinder:"
 709                                                 " not moving to invalid direction: "
 710                                                 << PPOS(directions[i]) << std::endl);
 711                         }
 712                 }
 713         }
 714         return retval;
 715 }
 716
 717 /******************************************************************************/
 718 int pathfinder::get_manhattandistance(v3s16 pos) {
 719
 720         int min_x = MYMIN(pos.X,m_destination.X);
 721         int max_x = MYMAX(pos.X,m_destination.X);
 722         int min_z = MYMIN(pos.Z,m_destination.Z);
 723         int max_z = MYMAX(pos.Z,m_destination.Z);
 724
 725         return (max_x - min_x) + (max_z - min_z);
 726 }
 727
 728 /******************************************************************************/
 729 v3s16 pathfinder::get_dir_heuristic(std::vector<v3s16>& directions,path_gridnode& g_pos) {
 730         int   minscore = -1;
 731         v3s16 retdir   = v3s16(0,0,0);
 732         v3s16 srcpos = g_pos.pos;
 733         DEBUG_OUT("Pathfinder: remaining dirs at beginning:"
 734                                 << directions.size() << std::endl);
 735
 736         for (std::vector<v3s16>::iterator iter = directions.begin();
 737                         iter != directions.end();
 738                         iter ++) {
 739
 740                 v3s16 pos1 =  v3s16(srcpos.X + iter->X,0,srcpos.Z+iter->Z);
 741
 742                 int cur_manhattan = get_manhattandistance(pos1);
 743                 path_cost cost    = g_pos.get_cost(*iter);
 744
 745                 if (!cost.updated) {
 746                         cost = calc_cost(g_pos.pos,*iter);
 747                         g_pos.set_cost(*iter,cost);
 748                 }
 749
 750                 if (cost.valid) {
 751                         int score = cost.value + cur_manhattan;
 752
 753                         if ((minscore < 0)|| (score < minscore)) {
 754                                 minscore = score;
 755                                 retdir = *iter;
 756                         }
 757                 }
 758         }
 759
 760         if (retdir != v3s16(0,0,0)) {
 761                 for (std::vector<v3s16>::iterator iter = directions.begin();
 762                                         iter != directions.end();
 763                                         iter ++) {
 764                         if(*iter == retdir) {
 765                                 DEBUG_OUT("Pathfinder: removing return direction" << std::endl);
 766                                 directions.erase(iter);
 767                                 break;
 768                         }
 769                 }
 770         }
 771         else {
 772                 DEBUG_OUT("Pathfinder: didn't find any valid direction clearing"
 773                                         << std::endl);
 774                 directions.clear();
 775         }
 776         DEBUG_OUT("Pathfinder: remaining dirs at end:" << directions.size()
 777                                 << std::endl);
 778         return retdir;
 779 }
 780
 781 /******************************************************************************/
 782 bool pathfinder::update_cost_heuristic( v3s16 ipos,
 783                                                                         v3s16 srcdir,
 784                                                                         int current_cost,
 785                                                                         int level) {
 786
 787         path_gridnode& g_pos = getIndexElement(ipos);
 788         g_pos.totalcost = current_cost;
 789         g_pos.sourcedir = srcdir;
 790
 791         level ++;
 792
 793         //check if target has been found
 794         if (g_pos.target) {
 795                 m_min_target_distance = current_cost;
 796                 DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl);
 797                 return true;
 798         }
 799
 800         bool retval = false;
 801
 802         std::vector<v3s16> directions;
 803
 804         directions.push_back(v3s16( 1,0, 0));
 805         directions.push_back(v3s16(-1,0, 0));
 806         directions.push_back(v3s16( 0,0, 1));
 807         directions.push_back(v3s16( 0,0,-1));
 808
 809         v3s16 direction = get_dir_heuristic(directions,g_pos);
 810
 811         while (direction != v3s16(0,0,0) && (!retval)) {
 812
 813                 if (direction != srcdir) {
 814                         path_cost cost = g_pos.get_cost(direction);
 815
 816                         if (cost.valid) {
 817                                 direction.Y = cost.direction;
 818
 819                                 v3s16 ipos2 = ipos + direction;
 820
 821                                 if (!valid_index(ipos2)) {
 822                                         DEBUG_OUT(LVL " Pathfinder: " << PPOS(ipos2) <<
 823                                                         " out of range (" << m_limits.X.max << "," <<
 824                                                         m_limits.Y.max << "," << m_limits.Z.max
 825                                                         <<")" << std::endl);
 826                                         continue;
 827                                 }
 828
 829                                 path_gridnode& g_pos2 = getIndexElement(ipos2);
 830
 831                                 if (!g_pos2.valid) {
 832                                         VERBOSE_TARGET << LVL "Pathfinder: no data for new position: "
 833                                                                                                 << PPOS(ipos2) << std::endl;
 834                                         continue;
 835                                 }
 836
 837                                 assert(cost.value > 0);
 838
 839                                 int new_cost = current_cost + cost.value;
 840
 841                                 // check if there already is a smaller path
 842                                 if ((m_min_target_distance > 0) &&
 843                                                 (m_min_target_distance < new_cost)) {
 844                                         DEBUG_OUT(LVL "Pathfinder:"
 845                                                         " already longer than best already found path "
 846                                                         << PPOS(ipos2) << std::endl);
 847                                         return false;
 848                                 }
 849
 850                                 if ((g_pos2.totalcost < 0) ||
 851                                                 (g_pos2.totalcost > new_cost)) {
 852                                         DEBUG_OUT(LVL "Pathfinder: updating path at: "<<
 853                                                         PPOS(ipos2) << " from: " << g_pos2.totalcost << " to "<<
 854                                                         new_cost << " srcdir=" <<
 855                                                         PPOS(invert(direction))<< std::endl);
 856                                         if (update_cost_heuristic(ipos2,invert(direction),
 857                                                                                         new_cost,level)) {
 858                                                 retval = true;
 859                                                 }
 860                                         }
 861                                 else {
 862                                         DEBUG_OUT(LVL "Pathfinder:"
 863                                                         " already found shorter path to: "
 864                                                         << PPOS(ipos2) << std::endl);
 865                                 }
 866                         }
 867                         else {
 868                                 DEBUG_OUT(LVL "Pathfinder:"
 869                                                 " not moving to invalid direction: "
 870                                                 << PPOS(direction) << std::endl);
 871                         }
 872                 }
 873                 else {
 874                         DEBUG_OUT(LVL "Pathfinder:"
 875                                                         " skipping srcdir: "
 876                                                         << PPOS(direction) << std::endl);
 877                 }
 878                 direction = get_dir_heuristic(directions,g_pos);
 879         }
 880         return retval;
 881 }
 882
 883 /******************************************************************************/
 884 void pathfinder::build_path(std::vector<v3s16>& path,v3s16 pos, int level) {
 885         level ++;
 886         if (level > 700) {
 887                 ERROR_TARGET
 888                 << LVL "Pathfinder: path is too long aborting" << std::endl;
 889                 return;
 890         }
 891
 892         path_gridnode& g_pos = getIndexElement(pos);
 893         if (!g_pos.valid) {
 894                 ERROR_TARGET
 895                 << LVL "Pathfinder: invalid next pos detected aborting" << std::endl;
 896                 return;
 897         }
 898
 899         g_pos.is_element = true;
 900
 901         //check if source reached
 902         if (g_pos.source) {
 903                 path.push_back(pos);
 904                 return;
 905         }
 906
 907         build_path(path,pos + g_pos.sourcedir,level);
 908         path.push_back(pos);
 909 }
 910
 911 /******************************************************************************/
 912 v3f pathfinder::tov3f(v3s16 pos) {
 913         return v3f(BS*pos.X,BS*pos.Y,BS*pos.Z);
 914 }
 915
 916 #ifdef PATHFINDER_DEBUG
 917
 918 /******************************************************************************/
 919 void pathfinder::print_cost() {
 920         print_cost(DIR_XP);
 921         print_cost(DIR_XM);
 922         print_cost(DIR_ZP);
 923         print_cost(DIR_ZM);
 924 }
 925
 926 /******************************************************************************/
 927 void pathfinder::print_ydir() {
 928         print_ydir(DIR_XP);
 929         print_ydir(DIR_XM);
 930         print_ydir(DIR_ZP);
 931         print_ydir(DIR_ZM);
 932 }
 933
 934 /******************************************************************************/
 935 void pathfinder::print_cost(path_directions dir) {
 936
 937         std::cout << "Cost in direction: " << dir_to_name(dir) << std::endl;
 938         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
 939         std::cout << std::setfill(' ');
 940         for (int y = 0; y < m_max_index_y; y++) {
 941
 942                 std::cout << "Level: " << y << std::endl;
 943
 944                 std::cout << std::setw(4) << " " << "  ";
 945                 for (int x = 0; x < m_max_index_x; x++) {
 946                         std::cout << std::setw(4) << x;
 947                 }
 948                 std::cout << std::endl;
 949
 950                 for (int z = 0; z < m_max_index_z; z++) {
 951                         std::cout << std::setw(4) << z <<": ";
 952                         for (int x = 0; x < m_max_index_x; x++) {
 953                                 if (m_data[x][z][y].directions[dir].valid)
 954                                         std::cout << std::setw(4)
 955                                                 << m_data[x][z][y].directions[dir].value;
 956                                 else
 957                                         std::cout << std::setw(4) << "-";
 958                                 }
 959                         std::cout << std::endl;
 960                 }
 961                 std::cout << std::endl;
 962         }
 963 }
 964
 965 /******************************************************************************/
 966 void pathfinder::print_ydir(path_directions dir) {
 967
 968         std::cout << "Height difference in direction: " << dir_to_name(dir) << std::endl;
 969         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
 970         std::cout << std::setfill(' ');
 971         for (int y = 0; y < m_max_index_y; y++) {
 972
 973                 std::cout << "Level: " << y << std::endl;
 974
 975                 std::cout << std::setw(4) << " " << "  ";
 976                 for (int x = 0; x < m_max_index_x; x++) {
 977                         std::cout << std::setw(4) << x;
 978                 }
 979                 std::cout << std::endl;
 980
 981                 for (int z = 0; z < m_max_index_z; z++) {
 982                         std::cout << std::setw(4) << z <<": ";
 983                         for (int x = 0; x < m_max_index_x; x++) {
 984                                 if (m_data[x][z][y].directions[dir].valid)
 985                                         std::cout << std::setw(4)
 986                                                 << m_data[x][z][y].directions[dir].direction;
 987                                 else
 988                                         std::cout << std::setw(4) << "-";
 989                                 }
 990                         std::cout << std::endl;
 991                 }
 992                 std::cout << std::endl;
 993         }
 994 }
 995
 996 /******************************************************************************/
 997 void pathfinder::print_type() {
 998         std::cout << "Type of node:" << std::endl;
 999         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1000         std::cout << std::setfill(' ');
1001         for (int y = 0; y < m_max_index_y; y++) {
1002
1003                 std::cout << "Level: " << y << std::endl;
1004
1005                 std::cout << std::setw(3) << " " << "  ";
1006                 for (int x = 0; x < m_max_index_x; x++) {
1007                         std::cout << std::setw(3) << x;
1008                 }
1009                 std::cout << std::endl;
1010
1011                 for (int z = 0; z < m_max_index_z; z++) {
1012                         std::cout << std::setw(3) << z <<": ";
1013                         for (int x = 0; x < m_max_index_x; x++) {
1014                                 char toshow = m_data[x][z][y].type;
1015                                 std::cout << std::setw(3) << toshow;
1016                         }
1017                         std::cout << std::endl;
1018                 }
1019                 std::cout << std::endl;
1020         }
1021         std::cout << std::endl;
1022 }
1023
1024 /******************************************************************************/
1025 void pathfinder::print_pathlen() {
1026         std::cout << "Pathlen:" << std::endl;
1027                 std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1028                 std::cout << std::setfill(' ');
1029                 for (int y = 0; y < m_max_index_y; y++) {
1030
1031                         std::cout << "Level: " << y << std::endl;
1032
1033                         std::cout << std::setw(3) << " " << "  ";
1034                         for (int x = 0; x < m_max_index_x; x++) {
1035                                 std::cout << std::setw(3) << x;
1036                         }
1037                         std::cout << std::endl;
1038
1039                         for (int z = 0; z < m_max_index_z; z++) {
1040                                 std::cout << std::setw(3) << z <<": ";
1041                                 for (int x = 0; x < m_max_index_x; x++) {
1042                                         std::cout << std::setw(3) << m_data[x][z][y].totalcost;
1043                                 }
1044                                 std::cout << std::endl;
1045                         }
1046                         std::cout << std::endl;
1047                 }
1048                 std::cout << std::endl;
1049 }
1050
1051 /******************************************************************************/
1052 std::string pathfinder::dir_to_name(path_directions dir) {
1053         switch (dir) {
1054         case DIR_XP:
1055                 return "XP";
1056                 break;
1057         case DIR_XM:
1058                 return "XM";
1059                 break;
1060         case DIR_ZP:
1061                 return "ZP";
1062                 break;
1063         case DIR_ZM:
1064                 return "ZM";
1065                 break;
1066         default:
1067                 return "UKN";
1068         }
1069 }
1070
1071 /******************************************************************************/
1072 void pathfinder::print_path(std::vector<v3s16> path) {
1073
1074         unsigned int current = 0;
1075         for (std::vector<v3s16>::iterator i = path.begin();
1076                         i != path.end(); i++) {
1077                 std::cout << std::setw(3) << current << ":" << PPOS((*i)) << std::endl;
1078                 current++;
1079         }
1080 }
1081
1082 #endif