diff --git a/source/simulation2/components/CCmpPathfinder_Vertex.cpp b/source/simulation2/components/CCmpPathfinder_Vertex.cpp
index 6ed042c..3d8b798 100644
a
|
b
|
inline static bool CheckVisibilityTop(const CFixedVector2D& a, const CFixedVecto
|
289 | 289 | return true; |
290 | 290 | } |
291 | 291 | |
| 292 | inline static bool CheckVisibilitySquares(const CFixedVector2D& start, const CFixedVector2D& end, const std::vector<EdgeAA>& Squares) |
| 293 | { |
| 294 | for (size_t i = 0; i < Squares.size(); ++i) |
| 295 | { |
| 296 | CFixedVector2D a = start - Squares[i].p0; |
| 297 | CFixedVector2D b = end - Squares[i].p0; |
| 298 | fixed c = Squares[i].c1; |
| 299 | |
| 300 | if (-c <= a.X && a.X <= c && -c <= a.Y && a.Y <= c) |
| 301 | continue; // a is inside |
| 302 | |
| 303 | if ((a.X < -c && b.X < -c) || (a.X > c && b.X > c) || (a.Y < -c && b.Y < -c) || (a.Y > c && b.Y > c)) |
| 304 | continue; // ab is entirely above/below/side the square |
| 305 | |
| 306 | CFixedVector2D abp = (b - a).Perpendicular(); |
| 307 | fixed s0 = a.Dot(abp).Absolute(); |
| 308 | fixed s1 = (abp.X.Absolute() + abp.Y.Absolute()).Multiply(c); |
| 309 | if (s0 > s1) |
| 310 | continue; |
| 311 | |
| 312 | return false; |
| 313 | } |
| 314 | return true; |
| 315 | } |
| 316 | |
292 | 317 | typedef PriorityQueueHeap<u16, fixed, fixed> VertexPriorityQueue; |
293 | 318 | |
294 | 319 | /** |
… |
… |
struct SquareSort
|
548 | 573 | { |
549 | 574 | CFixedVector2D src; |
550 | 575 | SquareSort(CFixedVector2D src) : src(src) { } |
551 | | bool operator()(const Square& a, const Square& b) |
| 576 | bool operator()(const EdgeAA& a, const EdgeAA& b) |
552 | 577 | { |
553 | 578 | if ((a.p0 - src).CompareLength(b.p0 - src) < 0) |
554 | 579 | return true; |
… |
… |
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
|
595 | 620 | // List of collision edges - paths must never cross these. |
596 | 621 | // (Edges are one-sided so intersections are fine in one direction, but not the other direction.) |
597 | 622 | std::vector<Edge> edges; |
598 | | std::vector<Square> edgeSquares; // axis-aligned squares; equivalent to 4 edges |
| 623 | std::vector<Square> edgeSquares; // axis-aligned rectangles; equivalent to 4 edges |
| 624 | std::vector<EdgeAA> aaSquares; // axis-aligned squares, p0 = center, c1 = hw = hh |
599 | 625 | |
600 | 626 | // Create impassable edges at the max-range boundary, so we can't escape the region |
601 | 627 | // where we're meant to be searching |
… |
… |
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
|
643 | 669 | |
644 | 670 | // Change array capacities to reduce reallocations |
645 | 671 | vertexes.reserve(vertexes.size() + squares.size()*4); |
646 | | edgeSquares.reserve(edgeSquares.size() + squares.size()); // (assume most squares are AA) |
| 672 | edgeSquares.reserve(edgeSquares.size() + staticShapesNb); // (assume most squares are AA) |
| 673 | aaSquares.reserve(squares.size() - staticShapesNb); |
647 | 674 | |
648 | 675 | entity_pos_t pathfindClearance = clearance; |
649 | 676 | |
… |
… |
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
|
685 | 712 | CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v)); |
686 | 713 | CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v)); |
687 | 714 | if (aa) |
688 | | edgeSquares.emplace_back(Square{ ev1, ev3 }); |
| 715 | if (h0.X == h0.Y) |
| 716 | aaSquares.emplace_back(EdgeAA{ center, h0.X }); |
| 717 | else |
| 718 | edgeSquares.emplace_back(Square{ ev1, ev3 }); |
689 | 719 | else |
690 | 720 | { |
691 | 721 | edges.emplace_back(Edge{ ev0, ev1 }); |
… |
… |
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
|
698 | 728 | // to reduce the search space |
699 | 729 | } |
700 | 730 | |
701 | | // Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable) |
702 | | for (size_t i = 0; i < edgeSquares.size(); ++i) |
| 731 | // Clip out vertices that are inside an aaSquare (i.e. trivially unreachable) |
| 732 | for (size_t i = 0; i < aaSquares.size(); ++i) |
703 | 733 | { |
704 | 734 | // If the start point is inside the square, ignore it |
705 | | if (start.p.X >= edgeSquares[i].p0.X && |
706 | | start.p.Y >= edgeSquares[i].p0.Y && |
707 | | start.p.X <= edgeSquares[i].p1.X && |
708 | | start.p.Y <= edgeSquares[i].p1.Y) |
| 735 | if ((start.p.X - aaSquares[i].p0.X).Absolute() <= aaSquares[i].c1 && |
| 736 | (start.p.Y - aaSquares[i].p0.Y).Absolute() <= aaSquares[i].c1) |
709 | 737 | continue; |
710 | 738 | |
711 | 739 | // Remove every non-start/goal vertex that is inside an edgeSquare; |
712 | 740 | // since remove() would be inefficient, just mark it as closed instead. |
713 | 741 | for (size_t j = 2; j < vertexes.size(); ++j) |
714 | | if (vertexes[j].p.X >= edgeSquares[i].p0.X && |
715 | | vertexes[j].p.Y >= edgeSquares[i].p0.Y && |
716 | | vertexes[j].p.X <= edgeSquares[i].p1.X && |
717 | | vertexes[j].p.Y <= edgeSquares[i].p1.Y) |
| 742 | if ((vertexes[j].p.X - aaSquares[i].p0.X).Absolute() <= aaSquares[i].c1 && |
| 743 | (vertexes[j].p.Y - aaSquares[i].p0.Y).Absolute() <= aaSquares[i].c1) |
718 | 744 | vertexes[j].status = Vertex::CLOSED; |
719 | 745 | } |
720 | 746 | |
… |
… |
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
|
772 | 798 | } |
773 | 799 | #undef PUSH_POINT |
774 | 800 | |
775 | | // Render the axis-aligned squares |
| 801 | // Render the axis-aligned rectangles |
776 | 802 | for (size_t i = 0; i < edgeSquares.size(); ++i) |
777 | 803 | { |
778 | 804 | m_DebugOverlayShortPathLines.push_back(SOverlayLine()); |
… |
… |
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
|
791 | 817 | xz.push_back(s.p0.Y.ToFloat()); |
792 | 818 | SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); |
793 | 819 | } |
| 820 | // Render the axis-aligned squares |
| 821 | for (size_t i = 0; i < aaSquares.size(); ++i) |
| 822 | { |
| 823 | m_DebugOverlayShortPathLines.push_back(SOverlayLine()); |
| 824 | m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); |
| 825 | std::vector<float> xz; |
| 826 | EdgeAA s = aaSquares[i]; |
| 827 | CFixedVector2D sqp0 = s.p0 - CFixedVector2D(s.c1, s.c1); |
| 828 | CFixedVector2D sqp1 = s.p0 + CFixedVector2D(s.c1, s.c1); |
| 829 | xz.push_back(sqp0.X.ToFloat()); |
| 830 | xz.push_back(sqp0.Y.ToFloat()); |
| 831 | xz.push_back(sqp0.X.ToFloat()); |
| 832 | xz.push_back(sqp1.Y.ToFloat()); |
| 833 | xz.push_back(sqp1.X.ToFloat()); |
| 834 | xz.push_back(sqp1.Y.ToFloat()); |
| 835 | xz.push_back(sqp1.X.ToFloat()); |
| 836 | xz.push_back(sqp0.Y.ToFloat()); |
| 837 | xz.push_back(sqp0.X.ToFloat()); |
| 838 | xz.push_back(sqp0.Y.ToFloat()); |
| 839 | SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); |
| 840 | } |
794 | 841 | } |
795 | 842 | |
796 | 843 | // Do an A* search over the vertex/visibility graph: |
… |
… |
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
|
830 | 877 | // The heuristic based on distance is very rough, especially for squares that are further away; |
831 | 878 | // we're also only really interested in the closest squares since they are the only ones that block a lot of rays. |
832 | 879 | // Thus we only do a partial sort; the threshold is just a somewhat reasonable value. |
833 | | if (edgeSquares.size() > 8) |
834 | | std::partial_sort(edgeSquares.begin(), edgeSquares.begin() + 8, edgeSquares.end(), SquareSort(vertexes[curr.id].p)); |
| 880 | if (aaSquares.size() > 8) |
| 881 | std::partial_sort(aaSquares.begin(), aaSquares.begin() + 8, aaSquares.end(), SquareSort(vertexes[curr.id].p)); |
835 | 882 | |
836 | 883 | std::vector<Edge> edgesUnaligned; |
837 | 884 | std::vector<EdgeAA> edgesLeft; |
… |
… |
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
|
890 | 937 | } |
891 | 938 | |
892 | 939 | visible = visible && |
| 940 | CheckVisibilitySquares(vertexes[curr.id].p, npos, aaSquares) && |
893 | 941 | CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) && |
894 | 942 | CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) && |
895 | 943 | CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) && |