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