src/pathfinder.cpp

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