Ticket #3925: Squares.2.patch

source/simulation2/components/CCmpPathfinder_Vertex.cpp

@@ -289 +289 @@
     return true;
 }
 
+inline static bool CheckVisibilitySquares(const CFixedVector2D& start, const CFixedVector2D& end, const std::vector<EdgeAA>& Squares)
+{
+    for (size_t i = 0; i < Squares.size(); ++i)
+    {
+        CFixedVector2D a = start - Squares[i].p0;
+        CFixedVector2D b = end - Squares[i].p0;
+        fixed c = Squares[i].c1;
+
+        if (-c <= a.X && a.X <= c && -c <= a.Y && a.Y <= c)
+            continue; // a is inside
+
+        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))
+            continue; // ab is entirely above/below/side the square
+
+        CFixedVector2D abp = (b - a).Perpendicular();
+        fixed s0 = a.Dot(abp).Absolute();
+        fixed s1 = (abp.X.Absolute() + abp.Y.Absolute()).Multiply(c);
+        if (s0 > s1)
+            continue;
+
+        return false;
+    }
+    return true;
+}
+
 typedef PriorityQueueHeap<u16, fixed, fixed> VertexPriorityQueue;
 
 /**
@@ -548 +573 @@
 {
     CFixedVector2D src;
     SquareSort(CFixedVector2D src) : src(src) { }
-    bool operator()(const Square& a, const Square& b)
+    bool operator()(const EdgeAA& a, const EdgeAA& b)
     {
         if ((a.p0 - src).CompareLength(b.p0 - src) < 0)
             return true;
@@ -595 +620 @@
     // List of collision edges - paths must never cross these.
     // (Edges are one-sided so intersections are fine in one direction, but not the other direction.)
     std::vector<Edge> edges;
-    std::vector<Square> edgeSquares; // axis-aligned squares; equivalent to 4 edges
+    std::vector<Square> edgeSquares; // axis-aligned rectangles; equivalent to 4 edges
+    std::vector<EdgeAA> aaSquares; // axis-aligned squares, p0 = center, c1 = hw = hh
 
     // Create impassable edges at the max-range boundary, so we can't escape the region
     // where we're meant to be searching
@@ -643 +669 @@
     
     // Change array capacities to reduce reallocations
     vertexes.reserve(vertexes.size() + squares.size()*4);
-    edgeSquares.reserve(edgeSquares.size() + squares.size()); // (assume most squares are AA)
+    edgeSquares.reserve(edgeSquares.size() + staticShapesNb); // (assume most squares are AA)
+    aaSquares.reserve(squares.size() - staticShapesNb);
 
     entity_pos_t pathfindClearance = clearance;
     
@@ -685 +712 @@
         CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v));
         CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v));
         if (aa)
-            edgeSquares.emplace_back(Square{ ev1, ev3 });
+            if (h0.X == h0.Y)
+                aaSquares.emplace_back(EdgeAA{ center, h0.X });
+            else
+                edgeSquares.emplace_back(Square{ ev1, ev3 });
         else
         {
             edges.emplace_back(Edge{ ev0, ev1 });
@@ -698 +728 @@
         // to reduce the search space
     }
 
-    // Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable)
-    for (size_t i = 0; i < edgeSquares.size(); ++i)
+    // Clip out vertices that are inside an aaSquare (i.e. trivially unreachable)
+    for (size_t i = 0; i < aaSquares.size(); ++i)
     {
         // If the start point is inside the square, ignore it
-        if (start.p.X >= edgeSquares[i].p0.X &&
-            start.p.Y >= edgeSquares[i].p0.Y &&
-            start.p.X <= edgeSquares[i].p1.X &&
-            start.p.Y <= edgeSquares[i].p1.Y)
+        if ((start.p.X - aaSquares[i].p0.X).Absolute() <= aaSquares[i].c1 &&
+            (start.p.Y - aaSquares[i].p0.Y).Absolute() <= aaSquares[i].c1)
             continue;
 
         // Remove every non-start/goal vertex that is inside an edgeSquare;
         // since remove() would be inefficient, just mark it as closed instead.
         for (size_t j = 2; j < vertexes.size(); ++j)
-            if (vertexes[j].p.X >= edgeSquares[i].p0.X &&
-                vertexes[j].p.Y >= edgeSquares[i].p0.Y &&
-                vertexes[j].p.X <= edgeSquares[i].p1.X &&
-                vertexes[j].p.Y <= edgeSquares[i].p1.Y)
+            if ((vertexes[j].p.X - aaSquares[i].p0.X).Absolute() <= aaSquares[i].c1 &&
+                (vertexes[j].p.Y - aaSquares[i].p0.Y).Absolute() <= aaSquares[i].c1)
                 vertexes[j].status = Vertex::CLOSED;
     }
 
@@ -772 +798 @@
         }
 #undef PUSH_POINT
 
-        // Render the axis-aligned squares
+        // Render the axis-aligned rectangles
         for (size_t i = 0; i < edgeSquares.size(); ++i)
         {
             m_DebugOverlayShortPathLines.push_back(SOverlayLine());
@@ -791 +817 @@
             xz.push_back(s.p0.Y.ToFloat());
             SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
         }
+        // Render the axis-aligned squares
+        for (size_t i = 0; i < aaSquares.size(); ++i)
+        {
+            m_DebugOverlayShortPathLines.push_back(SOverlayLine());
+            m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
+            std::vector<float> xz;
+            EdgeAA s = aaSquares[i];
+            CFixedVector2D sqp0 = s.p0 - CFixedVector2D(s.c1, s.c1);
+            CFixedVector2D sqp1 = s.p0 + CFixedVector2D(s.c1, s.c1);
+            xz.push_back(sqp0.X.ToFloat());
+            xz.push_back(sqp0.Y.ToFloat());
+            xz.push_back(sqp0.X.ToFloat());
+            xz.push_back(sqp1.Y.ToFloat());
+            xz.push_back(sqp1.X.ToFloat());
+            xz.push_back(sqp1.Y.ToFloat());
+            xz.push_back(sqp1.X.ToFloat());
+            xz.push_back(sqp0.Y.ToFloat());
+            xz.push_back(sqp0.X.ToFloat());
+            xz.push_back(sqp0.Y.ToFloat());
+            SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
+        }
     }
 
     // Do an A* search over the vertex/visibility graph:
@@ -830 +877 @@
         // The heuristic based on distance is very rough, especially for squares that are further away;
         // we're also only really interested in the closest squares since they are the only ones that block a lot of rays.
         // Thus we only do a partial sort; the threshold is just a somewhat reasonable value.
-        if (edgeSquares.size() > 8)
-            std::partial_sort(edgeSquares.begin(), edgeSquares.begin() + 8, edgeSquares.end(), SquareSort(vertexes[curr.id].p));
+        if (aaSquares.size() > 8)
+            std::partial_sort(aaSquares.begin(), aaSquares.begin() + 8, aaSquares.end(), SquareSort(vertexes[curr.id].p));
 
         std::vector<Edge> edgesUnaligned;
         std::vector<EdgeAA> edgesLeft;
@@ -890 +937 @@
             }
 
             visible = visible &&
+                CheckVisibilitySquares(vertexes[curr.id].p, npos, aaSquares) &&
                 CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) &&
                 CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) &&
                 CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) &&