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                                         int old_cost = g_pos2.totalcost;
 691                                         DEBUG_OUT(LVL "Pathfinder: updating path at: "<<
 692                                                         PPOS(ipos2) << " from: " << old_cost << " to "<<
 693                                                         new_cost << std::endl);
 694                                         if (update_all_costs(ipos2,invert(directions[i]),
 695                                                                                         new_cost,level)) {
 696                                                 retval = true;
 697                                                 }
 698                                         }
 699                                 else {
 700                                         DEBUG_OUT(LVL "Pathfinder:"
 701                                                         " already found shorter path to: "
 702                                                         << PPOS(ipos2) << std::endl);
 703                                 }
 704                         }
 705                         else {
 706                                 DEBUG_OUT(LVL "Pathfinder:"
 707                                                 " not moving to invalid direction: "
 708                                                 << PPOS(directions[i]) << std::endl);
 709                         }
 710                 }
 711         }
 712         return retval;
 713 }
 714
 715 /******************************************************************************/
 716 int pathfinder::get_manhattandistance(v3s16 pos) {
 717
 718         int min_x = MYMIN(pos.X,m_destination.X);
 719         int max_x = MYMAX(pos.X,m_destination.X);
 720         int min_z = MYMIN(pos.Z,m_destination.Z);
 721         int max_z = MYMAX(pos.Z,m_destination.Z);
 722
 723         return (max_x - min_x) + (max_z - min_z);
 724 }
 725
 726 /******************************************************************************/
 727 v3s16 pathfinder::get_dir_heuristic(std::vector<v3s16>& directions,path_gridnode& g_pos) {
 728         int   minscore = -1;
 729         v3s16 retdir   = v3s16(0,0,0);
 730         v3s16 srcpos = g_pos.pos;
 731         DEBUG_OUT("Pathfinder: remaining dirs at beginning:"
 732                                 << directions.size() << std::endl);
 733
 734         for (std::vector<v3s16>::iterator iter = directions.begin();
 735                         iter != directions.end();
 736                         iter ++) {
 737
 738                 v3s16 pos1 =  v3s16(srcpos.X + iter->X,0,srcpos.Z+iter->Z);
 739
 740                 int cur_manhattan = get_manhattandistance(pos1);
 741                 path_cost cost    = g_pos.get_cost(*iter);
 742
 743                 if (!cost.updated) {
 744                         cost = calc_cost(g_pos.pos,*iter);
 745                         g_pos.set_cost(*iter,cost);
 746                 }
 747
 748                 if (cost.valid) {
 749                         int score = cost.value + cur_manhattan;
 750
 751                         if ((minscore < 0)|| (score < minscore)) {
 752                                 minscore = score;
 753                                 retdir = *iter;
 754                         }
 755                 }
 756         }
 757
 758         if (retdir != v3s16(0,0,0)) {
 759                 for (std::vector<v3s16>::iterator iter = directions.begin();
 760                                         iter != directions.end();
 761                                         iter ++) {
 762                         if(*iter == retdir) {
 763                                 DEBUG_OUT("Pathfinder: removing return direction" << std::endl);
 764                                 directions.erase(iter);
 765                                 break;
 766                         }
 767                 }
 768         }
 769         else {
 770                 DEBUG_OUT("Pathfinder: didn't find any valid direction clearing"
 771                                         << std::endl);
 772                 directions.clear();
 773         }
 774         DEBUG_OUT("Pathfinder: remaining dirs at end:" << directions.size()
 775                                 << std::endl);
 776         return retdir;
 777 }
 778
 779 /******************************************************************************/
 780 bool pathfinder::update_cost_heuristic( v3s16 ipos,
 781                                                                         v3s16 srcdir,
 782                                                                         int current_cost,
 783                                                                         int level) {
 784
 785         path_gridnode& g_pos = getIndexElement(ipos);
 786         g_pos.totalcost = current_cost;
 787         g_pos.sourcedir = srcdir;
 788
 789         level ++;
 790
 791         //check if target has been found
 792         if (g_pos.target) {
 793                 m_min_target_distance = current_cost;
 794                 DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl);
 795                 return true;
 796         }
 797
 798         bool retval = false;
 799
 800         std::vector<v3s16> directions;
 801
 802         directions.push_back(v3s16( 1,0, 0));
 803         directions.push_back(v3s16(-1,0, 0));
 804         directions.push_back(v3s16( 0,0, 1));
 805         directions.push_back(v3s16( 0,0,-1));
 806
 807         v3s16 direction = get_dir_heuristic(directions,g_pos);
 808
 809         while (direction != v3s16(0,0,0) && (!retval)) {
 810
 811                 if (direction != srcdir) {
 812                         path_cost cost = g_pos.get_cost(direction);
 813
 814                         if (cost.valid) {
 815                                 direction.Y = cost.direction;
 816
 817                                 v3s16 ipos2 = ipos + direction;
 818
 819                                 if (!valid_index(ipos2)) {
 820                                         DEBUG_OUT(LVL " Pathfinder: " << PPOS(ipos2) <<
 821                                                         " out of range (" << m_limits.X.max << "," <<
 822                                                         m_limits.Y.max << "," << m_limits.Z.max
 823                                                         <<")" << std::endl);
 824                                         continue;
 825                                 }
 826
 827                                 path_gridnode& g_pos2 = getIndexElement(ipos2);
 828
 829                                 if (!g_pos2.valid) {
 830                                         VERBOSE_TARGET << LVL "Pathfinder: no data for new position: "
 831                                                                                                 << PPOS(ipos2) << std::endl;
 832                                         continue;
 833                                 }
 834
 835                                 assert(cost.value > 0);
 836
 837                                 int new_cost = current_cost + cost.value;
 838
 839                                 // check if there already is a smaller path
 840                                 if ((m_min_target_distance > 0) &&
 841                                                 (m_min_target_distance < new_cost)) {
 842                                         DEBUG_OUT(LVL "Pathfinder:"
 843                                                         " already longer than best already found path "
 844                                                         << PPOS(ipos2) << std::endl);
 845                                         return false;
 846                                 }
 847
 848                                 if ((g_pos2.totalcost < 0) ||
 849                                                 (g_pos2.totalcost > new_cost)) {
 850                                         int old_cost = g_pos2.totalcost;
 851                                         DEBUG_OUT(LVL "Pathfinder: updating path at: "<<
 852                                                         PPOS(ipos2) << " from: " << old_cost << " to "<<
 853                                                         new_cost << " srcdir=" <<
 854                                                         PPOS(invert(direction))<< std::endl);
 855                                         if (update_cost_heuristic(ipos2,invert(direction),
 856                                                                                         new_cost,level)) {
 857                                                 retval = true;
 858                                                 }
 859                                         }
 860                                 else {
 861                                         DEBUG_OUT(LVL "Pathfinder:"
 862                                                         " already found shorter path to: "
 863                                                         << PPOS(ipos2) << std::endl);
 864                                 }
 865                         }
 866                         else {
 867                                 DEBUG_OUT(LVL "Pathfinder:"
 868                                                 " not moving to invalid direction: "
 869                                                 << PPOS(direction) << std::endl);
 870                         }
 871                 }
 872                 else {
 873                         DEBUG_OUT(LVL "Pathfinder:"
 874                                                         " skipping srcdir: "
 875                                                         << PPOS(direction) << std::endl);
 876                 }
 877                 direction = get_dir_heuristic(directions,g_pos);
 878         }
 879         return retval;
 880 }
 881
 882 /******************************************************************************/
 883 void pathfinder::build_path(std::vector<v3s16>& path,v3s16 pos, int level) {
 884         level ++;
 885         if (level > 1000) {
 886                 ERROR_TARGET
 887                 << LVL "Pathfinder: path is too long aborting" << std::endl;
 888                 return;
 889         }
 890
 891         path_gridnode& g_pos = getIndexElement(pos);
 892         if (!g_pos.valid) {
 893                 ERROR_TARGET
 894                 << LVL "Pathfinder: invalid next pos detected aborting" << std::endl;
 895                 return;
 896         }
 897
 898         g_pos.is_element = true;
 899
 900         //check if source reached
 901         if (g_pos.source) {
 902                 path.push_back(pos);
 903                 return;
 904         }
 905
 906         build_path(path,pos + g_pos.sourcedir,level);
 907         path.push_back(pos);
 908 }
 909
 910 /******************************************************************************/
 911 v3f pathfinder::tov3f(v3s16 pos) {
 912         return v3f(BS*pos.X,BS*pos.Y,BS*pos.Z);
 913 }
 914
 915 #ifdef PATHFINDER_DEBUG
 916
 917 /******************************************************************************/
 918 void pathfinder::print_cost() {
 919         print_cost(DIR_XP);
 920         print_cost(DIR_XM);
 921         print_cost(DIR_ZP);
 922         print_cost(DIR_ZM);
 923 }
 924
 925 /******************************************************************************/
 926 void pathfinder::print_ydir() {
 927         print_ydir(DIR_XP);
 928         print_ydir(DIR_XM);
 929         print_ydir(DIR_ZP);
 930         print_ydir(DIR_ZM);
 931 }
 932
 933 /******************************************************************************/
 934 void pathfinder::print_cost(path_directions dir) {
 935
 936         std::cout << "Cost in direction: " << dir_to_name(dir) << std::endl;
 937         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
 938         std::cout << std::setfill(' ');
 939         for (int y = 0; y < m_max_index_y; y++) {
 940
 941                 std::cout << "Level: " << y << std::endl;
 942
 943                 std::cout << std::setw(4) << " " << "  ";
 944                 for (int x = 0; x < m_max_index_x; x++) {
 945                         std::cout << std::setw(4) << x;
 946                 }
 947                 std::cout << std::endl;
 948
 949                 for (int z = 0; z < m_max_index_z; z++) {
 950                         std::cout << std::setw(4) << z <<": ";
 951                         for (int x = 0; x < m_max_index_x; x++) {
 952                                 if (m_data[x][z][y].directions[dir].valid)
 953                                         std::cout << std::setw(4)
 954                                                 << m_data[x][z][y].directions[dir].value;
 955                                 else
 956                                         std::cout << std::setw(4) << "-";
 957                                 }
 958                         std::cout << std::endl;
 959                 }
 960                 std::cout << std::endl;
 961         }
 962 }
 963
 964 /******************************************************************************/
 965 void pathfinder::print_ydir(path_directions dir) {
 966
 967         std::cout << "Height difference in direction: " << dir_to_name(dir) << std::endl;
 968         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
 969         std::cout << std::setfill(' ');
 970         for (int y = 0; y < m_max_index_y; y++) {
 971
 972                 std::cout << "Level: " << y << std::endl;
 973
 974                 std::cout << std::setw(4) << " " << "  ";
 975                 for (int x = 0; x < m_max_index_x; x++) {
 976                         std::cout << std::setw(4) << x;
 977                 }
 978                 std::cout << std::endl;
 979
 980                 for (int z = 0; z < m_max_index_z; z++) {
 981                         std::cout << std::setw(4) << z <<": ";
 982                         for (int x = 0; x < m_max_index_x; x++) {
 983                                 if (m_data[x][z][y].directions[dir].valid)
 984                                         std::cout << std::setw(4)
 985                                                 << m_data[x][z][y].directions[dir].direction;
 986                                 else
 987                                         std::cout << std::setw(4) << "-";
 988                                 }
 989                         std::cout << std::endl;
 990                 }
 991                 std::cout << std::endl;
 992         }
 993 }
 994
 995 /******************************************************************************/
 996 void pathfinder::print_type() {
 997         std::cout << "Type of node:" << std::endl;
 998         std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
 999         std::cout << std::setfill(' ');
1000         for (int y = 0; y < m_max_index_y; y++) {
1001
1002                 std::cout << "Level: " << y << std::endl;
1003
1004                 std::cout << std::setw(3) << " " << "  ";
1005                 for (int x = 0; x < m_max_index_x; x++) {
1006                         std::cout << std::setw(3) << x;
1007                 }
1008                 std::cout << std::endl;
1009
1010                 for (int z = 0; z < m_max_index_z; z++) {
1011                         std::cout << std::setw(3) << z <<": ";
1012                         for (int x = 0; x < m_max_index_x; x++) {
1013                                 char toshow = m_data[x][z][y].type;
1014                                 std::cout << std::setw(3) << toshow;
1015                         }
1016                         std::cout << std::endl;
1017                 }
1018                 std::cout << std::endl;
1019         }
1020         std::cout << std::endl;
1021 }
1022
1023 /******************************************************************************/
1024 void pathfinder::print_pathlen() {
1025         std::cout << "Pathlen:" << std::endl;
1026                 std::cout << std::setfill('-') << std::setw(80) << "-" << std::endl;
1027                 std::cout << std::setfill(' ');
1028                 for (int y = 0; y < m_max_index_y; y++) {
1029
1030                         std::cout << "Level: " << y << std::endl;
1031
1032                         std::cout << std::setw(3) << " " << "  ";
1033                         for (int x = 0; x < m_max_index_x; x++) {
1034                                 std::cout << std::setw(3) << x;
1035                         }
1036                         std::cout << std::endl;
1037
1038                         for (int z = 0; z < m_max_index_z; z++) {
1039                                 std::cout << std::setw(3) << z <<": ";
1040                                 for (int x = 0; x < m_max_index_x; x++) {
1041                                         std::cout << std::setw(3) << m_data[x][z][y].totalcost;
1042                                 }
1043                                 std::cout << std::endl;
1044                         }
1045                         std::cout << std::endl;
1046                 }
1047                 std::cout << std::endl;
1048 }
1049
1050 /******************************************************************************/
1051 std::string pathfinder::dir_to_name(path_directions dir) {
1052         switch (dir) {
1053         case DIR_XP:
1054                 return "XP";
1055                 break;
1056         case DIR_XM:
1057                 return "XM";
1058                 break;
1059         case DIR_ZP:
1060                 return "ZP";
1061                 break;
1062         case DIR_ZM:
1063                 return "ZM";
1064                 break;
1065         default:
1066                 return "UKN";
1067         }
1068 }
1069
1070 /******************************************************************************/
1071 void pathfinder::print_path(std::vector<v3s16> path) {
1072
1073         unsigned int current = 0;
1074         for (std::vector<v3s16>::iterator i = path.begin();
1075                         i != path.end(); i++) {
1076                 std::cout << std::setw(3) << current << ":" << PPOS((*i)) << std::endl;
1077                 current++;
1078         }
1079 }
1080
1081 #endif