92 std::vector<std::pair<Position, float>>
FindPath(
93 const std::shared_ptr<World>& world,
95 const int dist_tolerance,
const int min_end_dist,
const int min_end_dist_xz,
96 const bool allow_jump,
const bool take_damage,
const float step_height
101 std::pair<Position, float> pos;
104 PathNode(
const std::pair<Position, float>& p,
const float s)
110 bool operator>(
const PathNode& rhs)
const
112 return score > rhs.score;
117 return static_cast<float>(std::abs(a.
x - b.
x) + std::abs(a.
y - b.
y) + std::abs(a.
z - b.
z));
121 constexpr int budget_visit = 15000;
123 const std::array<Position, 4> neighbour_offsets = {
Position(1, 0, 0),
Position(-1, 0, 0),
Position(0, 0, 1),
Position(0, 0, -1) };
124 std::priority_queue<PathNode, std::vector<PathNode>, std::greater<PathNode> > nodes_to_explore;
125 std::unordered_map<std::pair<Position, float>, std::pair<Position, float>,
PosFloatPairHash> came_from;
126 std::unordered_map<std::pair<Position, float>, float,
PosFloatPairHash> cost;
128 const Blockstate* block = world->GetBlock(start);
130 came_from[nodes_to_explore.top().pos] = nodes_to_explore.top().pos;
131 cost[nodes_to_explore.top().pos] = 0.0f;
136 bool suitable_location_found =
false;
138 bool end_reached =
false;
140 bool end_is_inside_solid =
false;
141 block = world->GetBlock(end);
142 end_is_inside_solid = block !=
nullptr && block->
IsSolid();
144 while (!nodes_to_explore.empty())
147 PathNode current_node = nodes_to_explore.top();
148 nodes_to_explore.pop();
150 end_reached |= current_node.pos.first == end;
151 suitable_location_found |=
152 std::abs(end.
x - current_node.pos.first.x) + std::abs(end.
y - current_node.pos.first.y) + std::abs(end.
z - current_node.pos.first.z) <= dist_tolerance &&
153 std::abs(end.
x - current_node.pos.first.x) + std::abs(end.
y - current_node.pos.first.y) + std::abs(end.
z - current_node.pos.first.z) >= min_end_dist &&
154 std::abs(end.
x - current_node.pos.first.x) + std::abs(end.
z - current_node.pos.first.z) >= min_end_dist_xz;
157 count_visit > budget_visit ||
159 (suitable_location_found && (end_reached || end_is_inside_solid)))
165 std::array<PathfindingBlockstate, 6> vertical_surroundings;
175 block = world->GetBlock(pos);
177 pos = current_node.pos.first +
Position(0, 1, 0);
178 block = world->GetBlock(pos);
181 pos = current_node.pos.first;
182 block = world->GetBlock(pos);
184 const bool can_jump =
185 vertical_surroundings[2].GetBlockstate() !=
nullptr &&
186 vertical_surroundings[2].GetBlockstate()->CanJumpWhenFeetInside();
190 if (!vertical_surroundings[2].IsSolid() && !vertical_surroundings[2].IsHazardous())
194 pos = current_node.pos.first +
Position(0, -1, 0);
195 block = world->GetBlock(pos);
199 if (!vertical_surroundings[3].IsSolid() && !vertical_surroundings[3].IsHazardous())
201 pos = current_node.pos.first +
Position(0, -2, 0);
202 block = world->GetBlock(pos);
204 pos = current_node.pos.first +
Position(0, -3, 0);
205 block = world->GetBlock(pos);
218 if (vertical_surroundings[2].IsClimbable()
219 && !vertical_surroundings[1].IsSolid()
220 && !vertical_surroundings[1].IsHazardous()
221 && !vertical_surroundings[0].IsSolid()
222 && !vertical_surroundings[0].IsHazardous()
225 const float new_cost = cost[current_node.pos] + 1.0f;
226 const std::pair<Position, float> new_pos = {
227 current_node.pos.first +
Position(0, 1, 0),
228 current_node.pos.first.y + 1.0f
230 auto it = cost.find(new_pos);
232 if (it == cost.end() ||
233 new_cost < it->second)
235 cost[new_pos] = new_cost;
236 nodes_to_explore.emplace(PathNode(
238 new_cost + PathNode::Heuristic(new_pos.first, end))
240 came_from[new_pos] = current_node.pos;
251 && vertical_surroundings[1].IsClimbable()
252 && !vertical_surroundings[0].IsSolid()
253 && !vertical_surroundings[0].IsHazardous()
254 && (vertical_surroundings[2].IsSolid() ||
255 vertical_surroundings[3].IsSolid())
258 const float new_cost = cost[current_node.pos] + 1.5f;
259 const std::pair<Position, float> new_pos = {
260 current_node.pos.first +
Position(0, 1, 0),
261 current_node.pos.first.y + 1.0f
263 auto it = cost.find(new_pos);
265 if (it == cost.end() ||
266 new_cost < it->second)
268 cost[new_pos] = new_cost;
269 nodes_to_explore.emplace(PathNode(
271 new_cost + PathNode::Heuristic(new_pos.first, end))
273 came_from[new_pos] = current_node.pos;
283 if (!vertical_surroundings[2].IsSolid() &&
284 vertical_surroundings[3].IsClimbable()
287 const float new_cost = cost[current_node.pos] + 1.0f;
288 const std::pair<Position, float> new_pos = {
289 current_node.pos.first +
Position(0, -1, 0),
290 current_node.pos.first.y - 1.0f
292 auto it = cost.find(new_pos);
294 if (it == cost.end() ||
295 new_cost < it->second)
297 cost[new_pos] = new_cost;
298 nodes_to_explore.emplace(PathNode(new_pos, new_cost + PathNode::Heuristic(new_pos.first, end)));
299 came_from[new_pos] = current_node.pos;
309 if (!vertical_surroundings[2].IsSolid() &&
310 vertical_surroundings[3].IsClimbable()
311 && vertical_surroundings[4].IsEmpty()
312 && !vertical_surroundings[5].IsEmpty()
313 && !vertical_surroundings[5].IsHazardous()
316 const bool above_block = vertical_surroundings[5].IsClimbable() || vertical_surroundings[5].GetHeight() + 1e-3f > current_node.pos.first.y - 2;
317 const float new_cost = cost[current_node.pos] + 3.0f - 1.0f * above_block;
318 const std::pair<Position, float> new_pos = {
319 current_node.pos.first +
Position(0, -3 + 1 * above_block, 0),
320 above_block ? std::max(current_node.pos.first.y - 2.0f, vertical_surroundings[5].GetHeight()) : vertical_surroundings[5].GetHeight()
322 auto it = cost.find(new_pos);
324 if (it == cost.end() ||
325 new_cost < it->second)
327 cost[new_pos] = new_cost;
328 nodes_to_explore.emplace(PathNode(
330 new_cost + PathNode::Heuristic(new_pos.first, end))
332 came_from[new_pos] = current_node.pos;
344 if (vertical_surroundings[2].IsClimbable()
345 && vertical_surroundings[3].IsEmpty()
346 && vertical_surroundings[4].IsEmpty()
347 && !vertical_surroundings[5].IsEmpty()
348 && !vertical_surroundings[5].IsHazardous()
351 const bool above_block = vertical_surroundings[5].IsClimbable() || vertical_surroundings[5].GetHeight() + 1e-3f > current_node.pos.first.y - 2;
352 const float new_cost = cost[current_node.pos] + 3.0f - 1.0f * above_block;
353 const std::pair<Position, float> new_pos = {
354 current_node.pos.first +
Position(0, -3 + 1 * above_block, 0),
355 above_block ? std::max(current_node.pos.first.y - 2.0f, vertical_surroundings[5].GetHeight()) : vertical_surroundings[5].GetHeight()
357 auto it = cost.find(new_pos);
359 if (it == cost.end() ||
360 new_cost < it->second)
362 cost[new_pos] = new_cost;
363 nodes_to_explore.emplace(PathNode(
365 new_cost + PathNode::Heuristic(new_pos.first, end))
367 came_from[new_pos] = current_node.pos;
380 if (!vertical_surroundings[2].IsSolid() &&
381 vertical_surroundings[3].IsClimbable()
382 && vertical_surroundings[4].IsEmpty()
383 && vertical_surroundings[5].IsEmpty()
386 for (
int y = -4; current_node.pos.first.y + y >= world->GetMinY(); --y)
388 pos = current_node.pos.first +
Position(0, y, 0);
389 block = world->GetBlock(pos);
399 const float new_cost = cost[current_node.pos] + std::abs(y);
400 const std::pair<Position, float> new_pos = {
401 current_node.pos.first +
Position(0, y + 1, 0),
402 current_node.pos.first.y + y + 1.0f
404 auto it = cost.find(new_pos);
406 if (it == cost.end() ||
407 new_cost < it->second)
409 cost[new_pos] = new_cost;
410 nodes_to_explore.emplace(PathNode(
412 new_cost + PathNode::Heuristic(new_pos.first, end))
414 came_from[new_pos] = current_node.pos;
425 for (
int i = 0; i < neighbour_offsets.size(); ++i)
427 const Position next_location = current_node.pos.first + neighbour_offsets[i];
428 const Position next_next_location = next_location + neighbour_offsets[i];
431 std::array<PathfindingBlockstate, 12> horizontal_surroundings;
443 pos = next_location +
Position(0, 2, 0);
444 block = world->GetBlock(pos);
446 pos = next_location +
Position(0, 1, 0);
447 block = world->GetBlock(pos);
449 const bool horizontal_movement =
450 (!horizontal_surroundings[1].IsSolid() ||
451 (horizontal_surroundings[1].GetHeight() - current_node.pos.second < 1.25f &&
452 !horizontal_surroundings[0].IsSolid() && !horizontal_surroundings[0].IsHazardous())
453 ) && !horizontal_surroundings[1].IsHazardous();
456 if (horizontal_movement)
459 block = world->GetBlock(pos);
461 pos = next_location +
Position(0, -1, 0);
462 block = world->GetBlock(pos);
464 pos = next_location +
Position(0, -2, 0);
465 block = world->GetBlock(pos);
467 pos = next_location +
Position(0, -3, 0);
468 block = world->GetBlock(pos);
474 if (allow_jump && can_jump)
476 pos = next_next_location +
Position(0, 2, 0);
477 block = world->GetBlock(pos);
479 pos = next_next_location +
Position(0, 1, 0);
480 block = world->GetBlock(pos);
482 pos = next_next_location;
483 block = world->GetBlock(pos);
485 pos = next_next_location +
Position(0, -1, 0);
486 block = world->GetBlock(pos);
488 pos = next_next_location +
Position(0, -2, 0);
489 block = world->GetBlock(pos);
491 pos = next_next_location +
Position(0, -3, 0);
492 block = world->GetBlock(pos);
507 if (!horizontal_surroundings[1].IsSolid()
508 && !horizontal_surroundings[1].IsHazardous()
509 && !horizontal_surroundings[2].IsSolid()
510 && !horizontal_surroundings[2].IsHazardous()
511 && !horizontal_surroundings[3].IsEmpty()
512 && !horizontal_surroundings[3].IsHazardous()
513 && (!horizontal_surroundings[3].IsFluid()
514 || !vertical_surroundings[3].IsFluid()
515 || horizontal_surroundings[2].IsFluid()
516 || vertical_surroundings[2].IsFluid())
519 const bool above_block = horizontal_surroundings[2].IsClimbable() || horizontal_surroundings[3].IsClimbable() || horizontal_surroundings[3].GetHeight() + 1e-3f > current_node.pos.first.y;
520 const float new_cost = cost[current_node.pos] + 2.0f - 1.0f * above_block;
521 const std::pair<Position, float> new_pos = {
522 next_location +
Position(0, 1 - 1 * above_block, 0),
523 above_block ? std::max(
static_cast<float>(next_location.
y), horizontal_surroundings[3].GetHeight()) : std::max(horizontal_surroundings[3].GetHeight(), horizontal_surroundings[4].GetHeight())
525 auto it = cost.find(new_pos);
527 if (it == cost.end() ||
528 new_cost < it->second)
530 cost[new_pos] = new_cost;
531 nodes_to_explore.emplace(PathNode(
533 new_cost + PathNode::Heuristic(new_pos.first, end))
535 came_from[new_pos] = current_node.pos;
547 && !vertical_surroundings[0].IsSolid()
548 && !vertical_surroundings[0].IsHazardous()
549 && vertical_surroundings[1].IsEmpty()
550 && (vertical_surroundings[2].IsSolid() || !vertical_surroundings[3].IsClimbable())
551 && !horizontal_surroundings[0].IsSolid()
552 && !horizontal_surroundings[0].IsHazardous()
553 && !horizontal_surroundings[1].IsEmpty()
554 && !horizontal_surroundings[1].IsHazardous()
555 && horizontal_surroundings[1].GetHeight() - current_node.pos.second < 1.25f
558 const float new_cost = cost[current_node.pos] + 2.5f;
559 const std::pair<Position, float> new_pos = {
561 std::max(horizontal_surroundings[1].GetHeight(), horizontal_surroundings[2].GetHeight())
563 auto it = cost.find(new_pos);
565 if (it == cost.end() ||
566 new_cost < it->second)
568 cost[new_pos] = new_cost;
569 nodes_to_explore.emplace(PathNode(
571 new_cost + PathNode::Heuristic(new_pos.first, end))
573 came_from[new_pos] = current_node.pos;
583 if ((can_jump || horizontal_surroundings[2].GetHeight() - current_node.pos.second < step_height)
584 && !vertical_surroundings[0].IsSolid()
585 && !vertical_surroundings[0].IsHazardous()
586 && vertical_surroundings[1].IsEmpty()
587 && (vertical_surroundings[2].IsSolid() || (vertical_surroundings[2].IsEmpty() && vertical_surroundings[3].IsSolid()))
588 && !horizontal_surroundings[0].IsSolid()
589 && !horizontal_surroundings[0].IsHazardous()
590 && !horizontal_surroundings[1].IsSolid()
591 && !horizontal_surroundings[1].IsHazardous()
592 && !horizontal_surroundings[2].IsEmpty()
593 && !horizontal_surroundings[2].IsHazardous()
594 && horizontal_surroundings[2].GetHeight() - current_node.pos.second < 1.25f
597 const bool above_block = horizontal_surroundings[1].IsClimbable() || horizontal_surroundings[2].IsClimbable() || horizontal_surroundings[2].GetHeight() + 1e-3f > current_node.pos.first.y + 1;
598 const float new_cost = cost[current_node.pos] + 1.0f + 1.0f * above_block + 0.5f * (horizontal_surroundings[1].IsClimbable() || horizontal_surroundings[2].GetHeight() - vertical_surroundings[2].GetHeight() > 0.5);
599 const std::pair<Position, float> new_pos = {
600 next_location +
Position(0, 1 * above_block, 0),
601 above_block ? std::max(current_node.pos.first.y + 1.0f, horizontal_surroundings[2].GetHeight()) : std::max(horizontal_surroundings[2].GetHeight(), horizontal_surroundings[3].GetHeight())
603 auto it = cost.find(new_pos);
605 if (it == cost.end() ||
606 new_cost < it->second)
608 cost[new_pos] = new_cost;
609 nodes_to_explore.emplace(PathNode(
611 new_cost + PathNode::Heuristic(new_pos.first, end))
613 came_from[new_pos] = current_node.pos;
623 if (!horizontal_surroundings[1].IsSolid()
624 && !horizontal_surroundings[1].IsHazardous()
625 && horizontal_surroundings[2].IsEmpty()
626 && horizontal_surroundings[3].IsEmpty()
627 && !horizontal_surroundings[4].IsEmpty()
628 && !horizontal_surroundings[4].IsHazardous()
631 const bool above_block = horizontal_surroundings[4].IsClimbable() || horizontal_surroundings[4].GetHeight() + 1e-3f > current_node.pos.first.y - 1;
632 const float new_cost = cost[current_node.pos] + 3.5f - 1.0f * above_block;
633 const std::pair<Position, float> new_pos = {
634 next_location +
Position(0, -2 + 1 * above_block, 0),
635 above_block ? std::max(current_node.pos.first.y - 1.0f, horizontal_surroundings[4].GetHeight()) : std::max(horizontal_surroundings[4].GetHeight(), horizontal_surroundings[5].GetHeight())
637 auto it = cost.find(new_pos);
639 if (it == cost.end() ||
640 new_cost < it->second)
642 cost[new_pos] = new_cost;
643 nodes_to_explore.emplace(PathNode(
645 new_cost + PathNode::Heuristic(new_pos.first, end))
647 came_from[new_pos] = current_node.pos;
657 if (!horizontal_surroundings[1].IsSolid()
658 && !horizontal_surroundings[1].IsHazardous()
659 && horizontal_surroundings[2].IsEmpty()
660 && horizontal_surroundings[3].IsEmpty()
661 && horizontal_surroundings[4].IsEmpty()
662 && !horizontal_surroundings[5].IsEmpty()
663 && !horizontal_surroundings[5].IsHazardous()
666 const bool above_block = horizontal_surroundings[5].IsClimbable() || horizontal_surroundings[5].GetHeight() + 1e-3f > current_node.pos.first.y - 2;
667 const float new_cost = cost[current_node.pos] + 4.5f - 1.0f * above_block;
668 const std::pair<Position, float> new_pos = {
669 next_location +
Position(0, -3 + 1 * above_block, 0),
670 above_block ? std::max(current_node.pos.first.y - 2.0f, horizontal_surroundings[5].GetHeight()) : horizontal_surroundings[5].GetHeight()
672 auto it = cost.find(new_pos);
674 if (it == cost.end() ||
675 new_cost < it->second)
677 cost[new_pos] = new_cost;
678 nodes_to_explore.emplace(PathNode(
680 new_cost + PathNode::Heuristic(new_pos.first, end))
682 came_from[new_pos] = current_node.pos;
694 if (!horizontal_surroundings[1].IsSolid()
695 && !horizontal_surroundings[1].IsHazardous()
696 && horizontal_surroundings[2].IsEmpty()
697 && horizontal_surroundings[3].IsEmpty()
698 && horizontal_surroundings[4].IsEmpty()
699 && horizontal_surroundings[5].IsEmpty()
702 for (
int y = -4; next_location.
y + y >= world->GetMinY(); --y)
704 pos = next_location +
Position(0, y, 0);
705 block = world->GetBlock(pos);
715 const float new_cost = cost[current_node.pos] + std::abs(y) + 1.5f;
716 const std::pair<Position, float> new_pos = {
717 next_location +
Position(0, y + 1, 0),
718 next_location.
y + y + 1.0f
720 auto it = cost.find(new_pos);
722 if (it == cost.end() ||
723 new_cost < it->second)
725 cost[new_pos] = new_cost;
726 nodes_to_explore.emplace(PathNode(
728 new_cost + PathNode::Heuristic(new_pos.first, end))
730 came_from[new_pos] = current_node.pos;
742 || vertical_surroundings[0].IsSolid()
743 || vertical_surroundings[0].IsHazardous()
744 || !vertical_surroundings[1].IsEmpty()
745 || vertical_surroundings[3].IsFluid()
746 || vertical_surroundings[3].IsEmpty()
747 || horizontal_surroundings[0].IsSolid()
748 || horizontal_surroundings[0].IsHazardous()
749 || !horizontal_surroundings[1].IsEmpty()
750 || !horizontal_surroundings[2].IsEmpty()
751 || horizontal_surroundings[6].IsSolid()
752 || horizontal_surroundings[6].IsHazardous()
765 if (!horizontal_surroundings[7].IsEmpty()
766 && !horizontal_surroundings[7].IsHazardous()
767 && horizontal_surroundings[7].GetHeight() - current_node.pos.second < 1.25f
772 const float new_cost = cost[current_node.pos] + 5.0f;
773 const std::pair<Position, float> new_pos = {
774 next_next_location +
Position(0, 1, 0),
775 std::max(horizontal_surroundings[7].GetHeight(), horizontal_surroundings[8].GetHeight()),
777 auto it = cost.find(new_pos);
779 if (it == cost.end() ||
780 new_cost < it->second)
782 cost[new_pos] = new_cost;
783 nodes_to_explore.emplace(PathNode(
785 new_cost + PathNode::Heuristic(new_pos.first, end))
787 came_from[new_pos] = current_node.pos;
797 if (horizontal_surroundings[7].IsEmpty()
798 && !horizontal_surroundings[8].IsEmpty()
799 && !horizontal_surroundings[8].IsHazardous()
800 && horizontal_surroundings[8].GetHeight() - current_node.pos.second < 1.25f
803 const bool above_block = horizontal_surroundings[8].IsClimbable() || horizontal_surroundings[8].GetHeight() + 1e-3f > current_node.pos.first.y + 1;
806 const float new_cost = cost[current_node.pos] + 3.0f + 1.0f * above_block;
807 const std::pair<Position, float> new_pos = {
808 next_next_location +
Position(0, above_block * 1, 0),
809 above_block ? std::max(current_node.pos.first.y + 1.0f, horizontal_surroundings[8].GetHeight()) : std::max(horizontal_surroundings[8].GetHeight(), horizontal_surroundings[9].GetHeight())
811 auto it = cost.find(new_pos);
813 if (it == cost.end() ||
814 new_cost < it->second)
816 cost[new_pos] = new_cost;
817 nodes_to_explore.emplace(PathNode(
819 new_cost + PathNode::Heuristic(new_pos.first, end))
821 came_from[new_pos] = current_node.pos;
831 if (horizontal_surroundings[7].IsEmpty()
832 && horizontal_surroundings[8].IsEmpty()
833 && !horizontal_surroundings[9].IsEmpty()
834 && !horizontal_surroundings[9].IsHazardous()
837 const bool above_block = horizontal_surroundings[9].IsClimbable() || horizontal_surroundings[9].GetHeight() + 1e-3f > current_node.pos.first.y;
838 const float new_cost = cost[current_node.pos] + 3.5f - 1.0f * above_block;
839 const std::pair<Position, float> new_pos = {
840 next_next_location +
Position(0, -1 + 1 * above_block, 0),
841 above_block ? std::max(
static_cast<float>(current_node.pos.first.y), horizontal_surroundings[9].GetHeight()) : std::max(horizontal_surroundings[9].GetHeight(), horizontal_surroundings[10].GetHeight())
843 auto it = cost.find(new_pos);
845 if (it == cost.end() ||
846 new_cost < it->second)
848 cost[new_pos] = new_cost;
849 nodes_to_explore.emplace(PathNode(
851 new_cost + PathNode::Heuristic(new_pos.first, end))
853 came_from[new_pos] = current_node.pos;
863 if (horizontal_surroundings[7].IsEmpty()
864 && horizontal_surroundings[8].IsEmpty()
865 && horizontal_surroundings[9].IsEmpty()
866 && !horizontal_surroundings[10].IsEmpty()
867 && !horizontal_surroundings[10].IsHazardous()
870 const bool above_block = horizontal_surroundings[10].IsClimbable() || horizontal_surroundings[10].GetHeight() + 1e-3f > current_node.pos.first.y - 1;
871 const float new_cost = cost[current_node.pos] + 4.5f - 1.0f * above_block;
872 const std::pair<Position, float> new_pos = {
873 next_next_location +
Position(0, -2 + 1 * above_block, 0),
874 above_block ? std::max(current_node.pos.first.y - 1.0f, horizontal_surroundings[10].GetHeight()) : std::max(horizontal_surroundings[10].GetHeight(), horizontal_surroundings[11].GetHeight())
876 auto it = cost.find(new_pos);
878 if (it == cost.end() ||
879 new_cost < it->second)
881 cost[new_pos] = new_cost;
882 nodes_to_explore.emplace(PathNode(
884 new_cost + PathNode::Heuristic(new_pos.first, end))
886 came_from[new_pos] = current_node.pos;
896 if (horizontal_surroundings[7].IsEmpty()
897 && horizontal_surroundings[8].IsEmpty()
898 && horizontal_surroundings[9].IsEmpty()
899 && horizontal_surroundings[10].IsEmpty()
900 && !horizontal_surroundings[11].IsEmpty()
901 && !horizontal_surroundings[11].IsHazardous()
904 const bool above_block = horizontal_surroundings[11].IsClimbable() || horizontal_surroundings[11].GetHeight() + 1e-3f > current_node.pos.first.y - 2;
905 const float new_cost = cost[current_node.pos] + 6.5f - 1.0f * above_block;
906 const std::pair<Position, float> new_pos = {
907 next_next_location +
Position(0, -3 + 1 * above_block, 0),
908 above_block ? std::max(current_node.pos.first.y - 2.0f, horizontal_surroundings[11].GetHeight()) : horizontal_surroundings[1].GetHeight()
910 auto it = cost.find(new_pos);
912 if (it == cost.end() ||
913 new_cost < it->second)
915 cost[new_pos] = new_cost;
916 nodes_to_explore.emplace(PathNode(
918 new_cost + PathNode::Heuristic(new_pos.first, end))
920 came_from[new_pos] = current_node.pos;
926 auto it_end_path = came_from.begin();
933 int best_dist = std::numeric_limits<int>::max();
934 int best_dist_start = std::numeric_limits<int>::max();
935 for (
auto it = came_from.begin(); it != came_from.end(); ++it)
937 const Position diff = it->first.first - end;
938 const int d_xz = std::abs(diff.
x) + std::abs(diff.
z);
939 const int d = d_xz + std::abs(diff.
y);
940 const Position diff_start = it->first.first - start;
941 const int d_start = std::abs(diff_start.
x) + std::abs(diff_start.
y) + std::abs(diff_start.
z);
942 if (d <= dist_tolerance && d >= min_end_dist && d_xz >= min_end_dist_xz &&
943 (d_start < best_dist_start || (d_start == best_dist_start && d < best_dist))
947 best_dist_start = d_start;
955 if (best_dist == std::numeric_limits<int>::max())
957 for (
auto it = came_from.begin(); it != came_from.end(); ++it)
959 const Position diff = it->first.first - end;
960 const int d_xz = std::abs(diff.
x) + std::abs(diff.
z);
961 const int d = d_xz + std::abs(diff.
y);
962 const Position diff_start = it->first.first - start;
963 const int d_start = std::abs(diff_start.
x) + std::abs(diff_start.
y) + std::abs(diff_start.
z);
964 if (d < best_dist || (d == best_dist && d_start < best_dist_start))
967 best_dist_start = d_start;
973 std::deque<std::pair<Position, float>> output_deque;
974 output_deque.push_front(it_end_path->first);
975 while (it_end_path->second.first != start)
977 it_end_path = came_from.find(it_end_path->second);
978 output_deque.push_front(it_end_path->first);
981 return std::vector<std::pair<Position, float>>(output_deque.begin(), output_deque.end());
994 static_cast<int>(std::floor(target_position.
x)),
995 static_cast<int>(std::floor(target_position.
y)),
996 static_cast<int>(std::floor(target_position.
z))
999 const Vector3<double> motion_vector = target_position - local_player->GetPosition();
1000 const double half_player_width = 0.5 * local_player->GetWidth();
1001 const Vector3<double> horizontal_target_position(target_position.
x, 0.0, target_position.
z);
1004 std::shared_ptr<World> world = client.
GetWorld();
1006 if (speed_factor != 1.0f)
1008 local_player->SetAttributeModifier(
1012 speed_factor - 1.0f,
1020 local_player->LookAt(look_at_target,
true);
1023 if (std::abs(motion_vector.
x) > 0.5 || std::abs(motion_vector.
z) > 0.5)
1026 if (motion_vector.
y > 0.5 || std::abs(motion_vector.
x) > 1.5 || std::abs(motion_vector.
z) > 1.5)
1029 if (std::abs(motion_vector.
x) < 1.5 && std::abs(motion_vector.
z) < 1.5)
1031 local_player->SetInputsJump(
true);
1035 return local_player->GetY() >= target_block.
y;
1036 }, client, 40.0 * ms_per_tick))
1045 std::floor(local_player->GetX()) + 0.5,
1047 std::floor(local_player->GetZ()) + 0.5
1052 if (local_player->GetDirtyInputs())
1058 local_player->LookAt(look_at_target,
false);
1059 local_player->SetInputsForward(1.0);
1060 local_player->SetInputsSprint(sprint);
1062 if (
Vector3<double>(local_player->GetX(), 0.0, local_player->GetZ()).
SqrDist(current_block_center_xz) > half_player_width * half_player_width)
1065 local_player->SetInputsJump(
true);
1070 }, client, 20.0 * ms_per_tick))
1079 if (local_player->GetDirtyInputs())
1085 const Vector3<double> current_motion_vector = target_position - current_pos;
1087 if ((current_motion_vector.
x * motion_vector.
x + current_motion_vector.
z * motion_vector.
z) < 0.0 ||
1088 Vector3<double>(current_pos.
x, 0.0, current_pos.
z).
SqrDist(horizontal_target_position) < (0.5 - half_player_width) * (0.5 - half_player_width))
1093 local_player->LookAt(look_at_target,
false);
1095 double forward = 1.0;
1098 if (motion_vector.
y < -0.5 &&
1099 static_cast<int>(std::floor(current_pos.
x)) == target_block.
x &&
1100 static_cast<int>(std::floor(current_pos.
z)) == target_block.
z)
1102 if (std::max(std::abs(speed.
x), std::abs(speed.
z)) > 0.12)
1106 else if (std::max(std::abs(speed.
x), std::abs(speed.
z)) > 0.06)
1111 local_player->SetInputsForward(forward);
1112 local_player->SetInputsSprint(sprint && (forward == 1.0));
1115 }, client, (std::abs(motion_vector.
x) + std::abs(motion_vector.
z) + (motion_vector.
y < -0.5)) * 20.0 * ms_per_tick))
1127 if (local_player->GetDirtyInputs())
1132 if (
static_cast<int>(std::floor(local_player->GetY())) <= target_block.
y &&
1133 (local_player->GetOnGround() || local_player->IsClimbing() || local_player->IsInFluid()))
1140 static_cast<int>(std::floor(current_pos.x)),
1141 static_cast<int>(std::floor(current_pos.y)),
1142 static_cast<int>(std::floor(current_pos.z))
1144 local_player->SetInputsSneak(feet_block !=
nullptr &&
1146 local_player->SetInputsJump(local_player->GetFlying());
1149 if (
Vector3<double>(current_pos.x, 0.0, current_pos.z).
SqrDist(horizontal_target_position) > (0.5 - half_player_width) * (0.5 - half_player_width))
1151 local_player->LookAt(look_at_target,
false);
1152 local_player->SetInputsForward(1.0);
1156 }, client, 20.0 * ms_per_tick + (1 + std::abs(motion_vector.
y))))
1165 if (local_player->GetDirtyInputs())
1171 if (
static_cast<int>(std::floor(current_pos.y)) >= target_block.
y)
1176 local_player->SetInputsJump(
true);
1179 if (
Vector3<double>(current_pos.x, 0.0, current_pos.z).
SqrDist(horizontal_target_position) > (0.5 - half_player_width) * (0.5 - half_player_width))
1181 local_player->LookAt(look_at_target,
false);
1182 local_player->SetInputsForward(1.0);
1186 }, client, 20.0 * ms_per_tick * (1 + std::abs(motion_vector.
y))))
1194 if (local_player->GetDirtyInputs())
1200 if (local_player->GetY() >= target_position.
y && local_player->GetY() - target_position.
y < 0.2)
1207 static_cast<int>(std::floor(current_pos.x)),
1208 static_cast<int>(std::floor(current_pos.y)),
1209 static_cast<int>(std::floor(current_pos.z))
1211 local_player->SetInputsSneak(feet_block !=
nullptr &&
1213 local_player->SetInputsJump(local_player->GetFlying());
1216 if (
Vector3<double>(current_pos.x, 0.0, current_pos.z).
SqrDist(horizontal_target_position) > (0.5 - half_player_width) * (0.5 - half_player_width))
1218 local_player->LookAt(look_at_target,
false);
1219 local_player->SetInputsForward(1.0);
1223 }, client, 20.0 * ms_per_tick);