Line data Source code
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 0 : 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 0 : pathfinder searchclass;
72 :
73 : return searchclass.get_Path(env,
74 : source,destination,
75 0 : searchdistance,max_jump,max_drop,algo);
76 : }
77 :
78 : /******************************************************************************/
79 0 : path_cost::path_cost()
80 : : valid(false),
81 : value(0),
82 : direction(0),
83 0 : updated(false)
84 : {
85 : //intentionaly empty
86 0 : }
87 :
88 : /******************************************************************************/
89 0 : path_cost::path_cost(const path_cost& b) {
90 0 : valid = b.valid;
91 0 : direction = b.direction;
92 0 : value = b.value;
93 0 : updated = b.updated;
94 0 : }
95 :
96 : /******************************************************************************/
97 0 : path_cost& path_cost::operator= (const path_cost& b) {
98 0 : valid = b.valid;
99 0 : direction = b.direction;
100 0 : value = b.value;
101 0 : updated = b.updated;
102 :
103 0 : return *this;
104 : }
105 :
106 : /******************************************************************************/
107 0 : 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 0 : type('u')
117 : {
118 : //intentionaly empty
119 0 : }
120 :
121 : /******************************************************************************/
122 0 : path_gridnode::path_gridnode(const path_gridnode& b)
123 0 : : valid(b.valid),
124 0 : target(b.target),
125 0 : source(b.source),
126 0 : totalcost(b.totalcost),
127 : sourcedir(b.sourcedir),
128 0 : surfaces(b.surfaces),
129 : pos(b.pos),
130 0 : is_element(b.is_element),
131 0 : type(b.type)
132 : {
133 :
134 0 : directions[DIR_XP] = b.directions[DIR_XP];
135 0 : directions[DIR_XM] = b.directions[DIR_XM];
136 0 : directions[DIR_ZP] = b.directions[DIR_ZP];
137 0 : directions[DIR_ZM] = b.directions[DIR_ZM];
138 0 : }
139 :
140 : /******************************************************************************/
141 0 : path_gridnode& path_gridnode::operator= (const path_gridnode& b) {
142 0 : valid = b.valid;
143 0 : target = b.target;
144 0 : source = b.source;
145 0 : is_element = b.is_element;
146 0 : totalcost = b.totalcost;
147 0 : sourcedir = b.sourcedir;
148 0 : surfaces = b.surfaces;
149 0 : pos = b.pos;
150 0 : type = b.type;
151 :
152 0 : directions[DIR_XP] = b.directions[DIR_XP];
153 0 : directions[DIR_XM] = b.directions[DIR_XM];
154 0 : directions[DIR_ZP] = b.directions[DIR_ZP];
155 0 : directions[DIR_ZM] = b.directions[DIR_ZM];
156 :
157 0 : return *this;
158 : }
159 :
160 : /******************************************************************************/
161 0 : path_cost path_gridnode::get_cost(v3s16 dir) {
162 0 : if (dir.X > 0) {
163 0 : return directions[DIR_XP];
164 : }
165 0 : if (dir.X < 0) {
166 0 : return directions[DIR_XM];
167 : }
168 0 : if (dir.Z > 0) {
169 0 : return directions[DIR_ZP];
170 : }
171 0 : if (dir.Z < 0) {
172 0 : return directions[DIR_ZM];
173 : }
174 0 : path_cost retval;
175 0 : return retval;
176 : }
177 :
178 : /******************************************************************************/
179 0 : void path_gridnode::set_cost(v3s16 dir,path_cost cost) {
180 0 : if (dir.X > 0) {
181 0 : directions[DIR_XP] = cost;
182 : }
183 0 : if (dir.X < 0) {
184 0 : directions[DIR_XM] = cost;
185 : }
186 0 : if (dir.Z > 0) {
187 0 : directions[DIR_ZP] = cost;
188 : }
189 0 : if (dir.Z < 0) {
190 0 : directions[DIR_ZM] = cost;
191 : }
192 0 : }
193 :
194 : /******************************************************************************/
195 0 : 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 0 : std::vector<v3s16> retval;
207 :
208 : //check parameters
209 0 : if (env == 0) {
210 0 : ERROR_TARGET << "missing environment pointer" << std::endl;
211 0 : return retval;
212 : }
213 :
214 0 : m_searchdistance = searchdistance;
215 0 : m_env = env;
216 0 : m_maxjump = max_jump;
217 0 : m_maxdrop = max_drop;
218 0 : m_start = source;
219 0 : m_destination = destination;
220 0 : m_min_target_distance = -1;
221 0 : m_prefetch = true;
222 :
223 0 : if (algo == A_PLAIN_NP) {
224 0 : m_prefetch = false;
225 : }
226 :
227 0 : int min_x = MYMIN(source.X,destination.X);
228 0 : int max_x = MYMAX(source.X,destination.X);
229 :
230 0 : int min_y = MYMIN(source.Y,destination.Y);
231 0 : int max_y = MYMAX(source.Y,destination.Y);
232 :
233 0 : int min_z = MYMIN(source.Z,destination.Z);
234 0 : int max_z = MYMAX(source.Z,destination.Z);
235 :
236 0 : m_limits.X.min = min_x - searchdistance;
237 0 : m_limits.X.max = max_x + searchdistance;
238 0 : m_limits.Y.min = min_y - searchdistance;
239 0 : m_limits.Y.max = max_y + searchdistance;
240 0 : m_limits.Z.min = min_z - searchdistance;
241 0 : m_limits.Z.max = max_z + searchdistance;
242 :
243 0 : m_max_index_x = m_limits.X.max - m_limits.X.min;
244 0 : m_max_index_y = m_limits.Y.max - m_limits.Y.min;
245 0 : m_max_index_z = m_limits.Z.max - m_limits.Z.min;
246 :
247 : //build data map
248 0 : if (!build_costmap()) {
249 0 : ERROR_TARGET << "failed to build costmap" << std::endl;
250 0 : 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 0 : v3s16 StartIndex = getIndexPos(source);
260 0 : v3s16 EndIndex = getIndexPos(destination);
261 :
262 0 : path_gridnode& startpos = getIndexElement(StartIndex);
263 0 : path_gridnode& endpos = getIndexElement(EndIndex);
264 :
265 0 : if (!startpos.valid) {
266 0 : VERBOSE_TARGET << "invalid startpos" <<
267 0 : "Index: " << PPOS(StartIndex) <<
268 0 : "Realpos: " << PPOS(getRealPos(StartIndex)) << std::endl;
269 0 : return retval;
270 : }
271 0 : if (!endpos.valid) {
272 0 : VERBOSE_TARGET << "invalid stoppos" <<
273 0 : "Index: " << PPOS(EndIndex) <<
274 0 : "Realpos: " << PPOS(getRealPos(EndIndex)) << std::endl;
275 0 : return retval;
276 : }
277 :
278 0 : endpos.target = true;
279 0 : startpos.source = true;
280 0 : startpos.totalcost = 0;
281 :
282 0 : bool update_cost_retval = false;
283 :
284 0 : switch (algo) {
285 : case DIJKSTRA:
286 0 : update_cost_retval = update_all_costs(StartIndex,v3s16(0,0,0),0,0);
287 0 : break;
288 : case A_PLAIN_NP:
289 : case A_PLAIN:
290 0 : update_cost_retval = update_cost_heuristic(StartIndex,v3s16(0,0,0),0,0);
291 0 : break;
292 : default:
293 0 : ERROR_TARGET << "missing algorithm"<< std::endl;
294 0 : break;
295 : }
296 :
297 0 : 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 0 : std::vector<v3s16> path;
306 0 : 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 0 : std::vector<v3s16> full_path;
315 0 : for (std::vector<v3s16>::iterator i = path.begin();
316 0 : i != path.end(); i++) {
317 0 : 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 0 : return full_path;
337 : }
338 : else {
339 : #ifdef PATHFINDER_DEBUG
340 : print_pathlen();
341 : #endif
342 0 : ERROR_TARGET << "failed to update cost map"<< std::endl;
343 : }
344 :
345 :
346 : //return
347 0 : return retval;
348 : }
349 :
350 : /******************************************************************************/
351 0 : 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 0 : m_env(0)
365 : {
366 : //intentionaly empty
367 0 : }
368 :
369 : /******************************************************************************/
370 0 : v3s16 pathfinder::getRealPos(v3s16 ipos) {
371 :
372 0 : v3s16 retval = ipos;
373 :
374 0 : retval.X += m_limits.X.min;
375 0 : retval.Y += m_limits.Y.min;
376 0 : retval.Z += m_limits.Z.min;
377 :
378 0 : return retval;
379 : }
380 :
381 : /******************************************************************************/
382 0 : bool pathfinder::build_costmap()
383 : {
384 0 : INFO_TARGET << "Pathfinder build costmap: (" << m_limits.X.min << ","
385 0 : << m_limits.Z.min << ") ("
386 0 : << m_limits.X.max << ","
387 0 : << m_limits.Z.max << ")"
388 0 : << std::endl;
389 0 : m_data.resize(m_max_index_x);
390 0 : for (int x = 0; x < m_max_index_x; x++) {
391 0 : m_data[x].resize(m_max_index_z);
392 0 : for (int z = 0; z < m_max_index_z; z++) {
393 0 : m_data[x][z].resize(m_max_index_y);
394 :
395 0 : int surfaces = 0;
396 0 : for (int y = 0; y < m_max_index_y; y++) {
397 0 : v3s16 ipos(x,y,z);
398 :
399 0 : v3s16 realpos = getRealPos(ipos);
400 :
401 0 : MapNode current = m_env->getMap().getNodeNoEx(realpos);
402 0 : MapNode below = m_env->getMap().getNodeNoEx(realpos + v3s16(0,-1,0));
403 :
404 :
405 0 : if ((current.param0 == CONTENT_IGNORE) ||
406 0 : (below.param0 == CONTENT_IGNORE)) {
407 : DEBUG_OUT("Pathfinder: " << PPOS(realpos) <<
408 : " current or below is invalid element" << std::endl);
409 0 : if (current.param0 == CONTENT_IGNORE) {
410 0 : m_data[x][z][y].type = 'i';
411 : DEBUG_OUT(x << "," << y << "," << z << ": " << 'i' << std::endl);
412 : }
413 0 : continue;
414 : }
415 :
416 : //don't add anything if it isn't an air node
417 0 : if ((current.param0 != CONTENT_AIR) ||
418 0 : (below.param0 == CONTENT_AIR )) {
419 : DEBUG_OUT("Pathfinder: " << PPOS(realpos)
420 : << " not on surface" << std::endl);
421 0 : if (current.param0 != CONTENT_AIR) {
422 0 : m_data[x][z][y].type = 's';
423 : DEBUG_OUT(x << "," << y << "," << z << ": " << 's' << std::endl);
424 : }
425 : else {
426 0 : m_data[x][z][y].type = '-';
427 : DEBUG_OUT(x << "," << y << "," << z << ": " << '-' << std::endl);
428 : }
429 0 : continue;
430 : }
431 :
432 0 : surfaces++;
433 :
434 0 : m_data[x][z][y].valid = true;
435 0 : m_data[x][z][y].pos = realpos;
436 0 : m_data[x][z][y].type = 'g';
437 : DEBUG_OUT(x << "," << y << "," << z << ": " << 'a' << std::endl);
438 :
439 0 : if (m_prefetch) {
440 0 : m_data[x][z][y].directions[DIR_XP] =
441 0 : calc_cost(realpos,v3s16( 1,0, 0));
442 0 : m_data[x][z][y].directions[DIR_XM] =
443 0 : calc_cost(realpos,v3s16(-1,0, 0));
444 0 : m_data[x][z][y].directions[DIR_ZP] =
445 0 : calc_cost(realpos,v3s16( 0,0, 1));
446 0 : m_data[x][z][y].directions[DIR_ZM] =
447 0 : calc_cost(realpos,v3s16( 0,0,-1));
448 : }
449 :
450 : }
451 :
452 0 : if (surfaces >= 1 ) {
453 0 : for (int y = 0; y < m_max_index_y; y++) {
454 0 : if (m_data[x][z][y].valid) {
455 0 : m_data[x][z][y].surfaces = surfaces;
456 : }
457 : }
458 : }
459 : }
460 : }
461 0 : return true;
462 : }
463 :
464 : /******************************************************************************/
465 0 : path_cost pathfinder::calc_cost(v3s16 pos,v3s16 dir) {
466 0 : path_cost retval;
467 :
468 0 : retval.updated = true;
469 :
470 0 : v3s16 pos2 = pos + dir;
471 :
472 : //check limits
473 0 : if ( (pos2.X < m_limits.X.min) ||
474 0 : (pos2.X >= m_limits.X.max) ||
475 0 : (pos2.Z < m_limits.Z.min) ||
476 0 : (pos2.Z >= m_limits.Z.max)) {
477 : DEBUG_OUT("Pathfinder: " << PPOS(pos2) <<
478 : " no cost -> out of limits" << std::endl);
479 0 : return retval;
480 : }
481 :
482 0 : MapNode node_at_pos2 = m_env->getMap().getNodeNoEx(pos2);
483 :
484 : //did we get information about node?
485 0 : if (node_at_pos2.param0 == CONTENT_IGNORE ) {
486 0 : VERBOSE_TARGET << "Pathfinder: (1) area at pos: "
487 0 : << PPOS(pos2) << " not loaded";
488 0 : return retval;
489 : }
490 :
491 0 : if (node_at_pos2.param0 == CONTENT_AIR) {
492 : MapNode node_below_pos2 =
493 0 : m_env->getMap().getNodeNoEx(pos2 + v3s16(0,-1,0));
494 :
495 : //did we get information about node?
496 0 : if (node_below_pos2.param0 == CONTENT_IGNORE ) {
497 0 : VERBOSE_TARGET << "Pathfinder: (2) area at pos: "
498 0 : << PPOS((pos2 + v3s16(0,-1,0))) << " not loaded";
499 0 : return retval;
500 : }
501 :
502 0 : if (node_below_pos2.param0 != CONTENT_AIR) {
503 0 : retval.valid = true;
504 0 : retval.value = 1;
505 0 : retval.direction = 0;
506 : DEBUG_OUT("Pathfinder: "<< PPOS(pos)
507 : << " cost same height found" << std::endl);
508 : }
509 : else {
510 0 : v3s16 testpos = pos2 - v3s16(0,-1,0);
511 0 : MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos);
512 :
513 0 : while ((node_at_pos.param0 != CONTENT_IGNORE) &&
514 0 : (node_at_pos.param0 == CONTENT_AIR) &&
515 0 : (testpos.Y > m_limits.Y.min)) {
516 0 : testpos += v3s16(0,-1,0);
517 0 : node_at_pos = m_env->getMap().getNodeNoEx(testpos);
518 : }
519 :
520 : //did we find surface?
521 0 : if ((testpos.Y >= m_limits.Y.min) &&
522 0 : (node_at_pos.param0 != CONTENT_IGNORE) &&
523 0 : (node_at_pos.param0 != CONTENT_AIR)) {
524 0 : if ((pos2.Y - testpos.Y - 1) <= m_maxdrop) {
525 0 : retval.valid = true;
526 0 : retval.value = 2;
527 : //difference of y-pos +1 (target node is ABOVE solid node)
528 0 : retval.direction = ((testpos.Y - pos2.Y) +1);
529 : DEBUG_OUT("Pathfinder cost below height found" << std::endl);
530 : }
531 : else {
532 0 : INFO_TARGET << "Pathfinder:"
533 0 : " distance to surface below to big: "
534 0 : << (testpos.Y - pos2.Y) << " max: " << m_maxdrop
535 0 : << std::endl;
536 : }
537 : }
538 : else {
539 : DEBUG_OUT("Pathfinder: no surface below found" << std::endl);
540 : }
541 : }
542 : }
543 : else {
544 0 : v3s16 testpos = pos2;
545 0 : MapNode node_at_pos = m_env->getMap().getNodeNoEx(testpos);
546 :
547 0 : while ((node_at_pos.param0 != CONTENT_IGNORE) &&
548 0 : (node_at_pos.param0 != CONTENT_AIR) &&
549 0 : (testpos.Y < m_limits.Y.max)) {
550 0 : testpos += v3s16(0,1,0);
551 0 : node_at_pos = m_env->getMap().getNodeNoEx(testpos);
552 : }
553 :
554 : //did we find surface?
555 0 : if ((testpos.Y <= m_limits.Y.max) &&
556 0 : (node_at_pos.param0 == CONTENT_AIR)) {
557 :
558 0 : if (testpos.Y - pos2.Y <= m_maxjump) {
559 0 : retval.valid = true;
560 0 : retval.value = 2;
561 0 : 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 0 : return retval;
575 : }
576 :
577 : /******************************************************************************/
578 0 : v3s16 pathfinder::getIndexPos(v3s16 pos) {
579 :
580 0 : v3s16 retval = pos;
581 0 : retval.X -= m_limits.X.min;
582 0 : retval.Y -= m_limits.Y.min;
583 0 : retval.Z -= m_limits.Z.min;
584 :
585 0 : return retval;
586 : }
587 :
588 : /******************************************************************************/
589 0 : path_gridnode& pathfinder::getIndexElement(v3s16 ipos) {
590 0 : return m_data[ipos.X][ipos.Z][ipos.Y];
591 : }
592 :
593 : /******************************************************************************/
594 0 : bool pathfinder::valid_index(v3s16 index) {
595 0 : if ( (index.X < m_max_index_x) &&
596 0 : (index.Y < m_max_index_y) &&
597 0 : (index.Z < m_max_index_z) &&
598 0 : (index.X >= 0) &&
599 0 : (index.Y >= 0) &&
600 0 : (index.Z >= 0))
601 0 : return true;
602 :
603 0 : return false;
604 : }
605 :
606 : /******************************************************************************/
607 0 : v3s16 pathfinder::invert(v3s16 pos) {
608 0 : v3s16 retval = pos;
609 :
610 0 : retval.X *=-1;
611 0 : retval.Y *=-1;
612 0 : retval.Z *=-1;
613 :
614 0 : return retval;
615 : }
616 :
617 : /******************************************************************************/
618 0 : bool pathfinder::update_all_costs( v3s16 ipos,
619 : v3s16 srcdir,
620 : int current_cost,
621 : int level) {
622 :
623 0 : path_gridnode& g_pos = getIndexElement(ipos);
624 0 : g_pos.totalcost = current_cost;
625 0 : g_pos.sourcedir = srcdir;
626 :
627 0 : level ++;
628 :
629 : //check if target has been found
630 0 : if (g_pos.target) {
631 0 : m_min_target_distance = current_cost;
632 : DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl);
633 0 : return true;
634 : }
635 :
636 0 : bool retval = false;
637 :
638 0 : std::vector<v3s16> directions;
639 :
640 0 : directions.push_back(v3s16( 1,0, 0));
641 0 : directions.push_back(v3s16(-1,0, 0));
642 0 : directions.push_back(v3s16( 0,0, 1));
643 0 : directions.push_back(v3s16( 0,0,-1));
644 :
645 0 : for (unsigned int i=0; i < directions.size(); i++) {
646 0 : if (directions[i] != srcdir) {
647 0 : path_cost cost = g_pos.get_cost(directions[i]);
648 :
649 0 : if (cost.valid) {
650 0 : directions[i].Y = cost.direction;
651 :
652 0 : v3s16 ipos2 = ipos + directions[i];
653 :
654 0 : 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 0 : continue;
660 : }
661 :
662 0 : path_gridnode& g_pos2 = getIndexElement(ipos2);
663 :
664 0 : if (!g_pos2.valid) {
665 0 : VERBOSE_TARGET << LVL "Pathfinder: no data for new position: "
666 0 : << PPOS(ipos2) << std::endl;
667 0 : continue;
668 : }
669 :
670 : assert(cost.value > 0);
671 :
672 0 : int new_cost = current_cost + cost.value;
673 :
674 : // check if there already is a smaller path
675 0 : if ((m_min_target_distance > 0) &&
676 0 : (m_min_target_distance < new_cost)) {
677 0 : return false;
678 : }
679 :
680 0 : if ((g_pos2.totalcost < 0) ||
681 0 : (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 0 : if (update_all_costs(ipos2,invert(directions[i]),
686 : new_cost,level)) {
687 0 : 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 0 : return retval;
704 : }
705 :
706 : /******************************************************************************/
707 0 : int pathfinder::get_manhattandistance(v3s16 pos) {
708 :
709 0 : int min_x = MYMIN(pos.X,m_destination.X);
710 0 : int max_x = MYMAX(pos.X,m_destination.X);
711 0 : int min_z = MYMIN(pos.Z,m_destination.Z);
712 0 : int max_z = MYMAX(pos.Z,m_destination.Z);
713 :
714 0 : return (max_x - min_x) + (max_z - min_z);
715 : }
716 :
717 : /******************************************************************************/
718 0 : v3s16 pathfinder::get_dir_heuristic(std::vector<v3s16>& directions,path_gridnode& g_pos) {
719 0 : int minscore = -1;
720 0 : v3s16 retdir = v3s16(0,0,0);
721 0 : v3s16 srcpos = g_pos.pos;
722 : DEBUG_OUT("Pathfinder: remaining dirs at beginning:"
723 : << directions.size() << std::endl);
724 :
725 0 : for (std::vector<v3s16>::iterator iter = directions.begin();
726 0 : iter != directions.end();
727 : iter ++) {
728 :
729 0 : v3s16 pos1 = v3s16(srcpos.X + iter->X,0,srcpos.Z+iter->Z);
730 :
731 0 : int cur_manhattan = get_manhattandistance(pos1);
732 0 : path_cost cost = g_pos.get_cost(*iter);
733 :
734 0 : if (!cost.updated) {
735 0 : cost = calc_cost(g_pos.pos,*iter);
736 0 : g_pos.set_cost(*iter,cost);
737 : }
738 :
739 0 : if (cost.valid) {
740 0 : int score = cost.value + cur_manhattan;
741 :
742 0 : if ((minscore < 0)|| (score < minscore)) {
743 0 : minscore = score;
744 0 : retdir = *iter;
745 : }
746 : }
747 : }
748 :
749 0 : if (retdir != v3s16(0,0,0)) {
750 0 : for (std::vector<v3s16>::iterator iter = directions.begin();
751 0 : iter != directions.end();
752 : iter ++) {
753 0 : if(*iter == retdir) {
754 : DEBUG_OUT("Pathfinder: removing return direction" << std::endl);
755 0 : directions.erase(iter);
756 0 : break;
757 : }
758 : }
759 : }
760 : else {
761 : DEBUG_OUT("Pathfinder: didn't find any valid direction clearing"
762 : << std::endl);
763 0 : directions.clear();
764 : }
765 : DEBUG_OUT("Pathfinder: remaining dirs at end:" << directions.size()
766 : << std::endl);
767 0 : return retdir;
768 : }
769 :
770 : /******************************************************************************/
771 0 : bool pathfinder::update_cost_heuristic( v3s16 ipos,
772 : v3s16 srcdir,
773 : int current_cost,
774 : int level) {
775 :
776 0 : path_gridnode& g_pos = getIndexElement(ipos);
777 0 : g_pos.totalcost = current_cost;
778 0 : g_pos.sourcedir = srcdir;
779 :
780 0 : level ++;
781 :
782 : //check if target has been found
783 0 : if (g_pos.target) {
784 0 : m_min_target_distance = current_cost;
785 : DEBUG_OUT(LVL " Pathfinder: target found!" << std::endl);
786 0 : return true;
787 : }
788 :
789 0 : bool retval = false;
790 :
791 0 : std::vector<v3s16> directions;
792 :
793 0 : directions.push_back(v3s16( 1,0, 0));
794 0 : directions.push_back(v3s16(-1,0, 0));
795 0 : directions.push_back(v3s16( 0,0, 1));
796 0 : directions.push_back(v3s16( 0,0,-1));
797 :
798 0 : v3s16 direction = get_dir_heuristic(directions,g_pos);
799 :
800 0 : while (direction != v3s16(0,0,0) && (!retval)) {
801 :
802 0 : if (direction != srcdir) {
803 0 : path_cost cost = g_pos.get_cost(direction);
804 :
805 0 : if (cost.valid) {
806 0 : direction.Y = cost.direction;
807 :
808 0 : v3s16 ipos2 = ipos + direction;
809 :
810 0 : 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 0 : direction = get_dir_heuristic(directions,g_pos);
816 0 : continue;
817 : }
818 :
819 0 : path_gridnode& g_pos2 = getIndexElement(ipos2);
820 :
821 0 : if (!g_pos2.valid) {
822 0 : VERBOSE_TARGET << LVL "Pathfinder: no data for new position: "
823 0 : << PPOS(ipos2) << std::endl;
824 0 : direction = get_dir_heuristic(directions,g_pos);
825 0 : continue;
826 : }
827 :
828 : assert(cost.value > 0);
829 :
830 0 : int new_cost = current_cost + cost.value;
831 :
832 : // check if there already is a smaller path
833 0 : if ((m_min_target_distance > 0) &&
834 0 : (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 0 : return false;
839 : }
840 :
841 0 : if ((g_pos2.totalcost < 0) ||
842 0 : (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 0 : if (update_cost_heuristic(ipos2,invert(direction),
848 : new_cost,level)) {
849 0 : 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 0 : direction = get_dir_heuristic(directions,g_pos);
870 : }
871 0 : return retval;
872 : }
873 :
874 : /******************************************************************************/
875 0 : void pathfinder::build_path(std::vector<v3s16>& path,v3s16 pos, int level) {
876 0 : level ++;
877 0 : if (level > 700) {
878 0 : ERROR_TARGET
879 0 : << LVL "Pathfinder: path is too long aborting" << std::endl;
880 0 : return;
881 : }
882 :
883 0 : path_gridnode& g_pos = getIndexElement(pos);
884 0 : if (!g_pos.valid) {
885 0 : ERROR_TARGET
886 0 : << LVL "Pathfinder: invalid next pos detected aborting" << std::endl;
887 0 : return;
888 : }
889 :
890 0 : g_pos.is_element = true;
891 :
892 : //check if source reached
893 0 : if (g_pos.source) {
894 0 : path.push_back(pos);
895 0 : return;
896 : }
897 :
898 0 : build_path(path,pos + g_pos.sourcedir,level);
899 0 : path.push_back(pos);
900 : }
901 :
902 : /******************************************************************************/
903 0 : v3f pathfinder::tov3f(v3s16 pos) {
904 0 : return v3f(BS*pos.X,BS*pos.Y,BS*pos.Z);
905 3 : }
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
|
