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