Ticket #3925: Squares.patch

source/simulation2/components/CCmpPathfinder_Vertex.cpp

@@ -138 +138 @@
     CFixedVector2D abn = (b - a).Perpendicular();
 
     // Edges of general non-axis-aligned shapes
-    for (size_t i = 0; i < edges.size(); ++i)
+    for (const Edge& edge : edges)
     {
-        CFixedVector2D p0 = edges[i].p0;
-        CFixedVector2D p1 = edges[i].p1;
-
-        CFixedVector2D d = (p1 - p0).Perpendicular();
+        CFixedVector2D d = (edge.p1 - edge.p0).Perpendicular();
 
         // If 'a' is behind the edge, we can't cross
-        fixed q = (a - p0).Dot(d);
+        fixed q = (a - edge.p0).Dot(d);
         if (q < fixed::Zero())
             continue;
 
         // If 'b' is in front of the edge, we can't cross
-        fixed r = (b - p0).Dot(d);
+        fixed r = (b - edge.p0).Dot(d);
         if (r > fixed::Zero())
             continue;
 
         // The ray is crossing the infinitely-extended edge from in front to behind.
         // Check the finite edge is crossing the infinitely-extended ray too.
         // (Given the previous tests, it can only be crossing in one direction.)
-        fixed s = (p0 - a).Dot(abn);
+        fixed s = (edge.p0 - a).Dot(abn);
         if (s > fixed::Zero())
             continue;
 
-        fixed t = (p1 - a).Dot(abn);
+        fixed t = (edge.p1 - a).Dot(abn);
         if (t < fixed::Zero())
             continue;
 
@@ -184 +181 @@
 
     CFixedVector2D abn = (b - a).Perpendicular();
 
-    for (size_t i = 0; i < edges.size(); ++i)
+    for (const EdgeAA& edge : edges)
     {
-        if (b.X < edges[i].p0.X)
+        if (b.X < edge.p0.X)
             continue;
 
-        CFixedVector2D p0 (edges[i].p0.X, edges[i].c1);
-        fixed s = (p0 - a).Dot(abn);
+        CFixedVector2D p1 (edge.p0.X, edge.c1);
+        fixed s = (p1 - a).Dot(abn);
         if (s > fixed::Zero())
             continue;
 
-        CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y);
-        fixed t = (p1 - a).Dot(abn);
+        fixed t = (edge.p0 - a).Dot(abn);
         if (t < fixed::Zero())
             continue;
 
@@ -212 +208 @@
 
     CFixedVector2D abn = (b - a).Perpendicular();
 
-    for (size_t i = 0; i < edges.size(); ++i)
+    for (const EdgeAA& edge : edges)
     {
-        if (b.X > edges[i].p0.X)
+        if (b.X > edge.p0.X)
             continue;
 
-        CFixedVector2D p0 (edges[i].p0.X, edges[i].c1);
-        fixed s = (p0 - a).Dot(abn);
+        CFixedVector2D p1 (edge.p0.X, edge.c1);
+        fixed s = (p1 - a).Dot(abn);
         if (s > fixed::Zero())
             continue;
 
-        CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y);
-        fixed t = (p1 - a).Dot(abn);
+        fixed t = (edge.p0 - a).Dot(abn);
         if (t < fixed::Zero())
             continue;
 
@@ -240 +235 @@
 
     CFixedVector2D abn = (b - a).Perpendicular();
 
-    for (size_t i = 0; i < edges.size(); ++i)
+    for (const EdgeAA& edge : edges)
     {
-        if (b.Y < edges[i].p0.Y)
+        if (b.Y < edge.p0.Y)
             continue;
 
-        CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y);
-        fixed s = (p0 - a).Dot(abn);
+        fixed s = (edge.p0 - a).Dot(abn);
         if (s > fixed::Zero())
             continue;
 
-        CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y);
+        CFixedVector2D p1 (edge.c1, edge.p0.Y);
         fixed t = (p1 - a).Dot(abn);
         if (t < fixed::Zero())
             continue;
@@ -268 +262 @@
 
     CFixedVector2D abn = (b - a).Perpendicular();
 
-    for (size_t i = 0; i < edges.size(); ++i)
+    for (const EdgeAA& edge : edges)
     {
-        if (b.Y > edges[i].p0.Y)
+        if (b.Y > edge.p0.Y)
             continue;
 
-        CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y);
-        fixed s = (p0 - a).Dot(abn);
+        fixed s = (edge.p0 - a).Dot(abn);
         if (s > fixed::Zero())
             continue;
 
-        CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y);
+        CFixedVector2D p1 (edge.c1, edge.p0.Y);
         fixed t = (p1 - a).Dot(abn);
         if (t < fixed::Zero())
             continue;
@@ -289 +282 @@
     return true;
 }
 
+inline static bool CheckVisibilitySquares(const CFixedVector2D& start, const CFixedVector2D& end, const std::vector<EdgeAA>& Squares)
+{
+    for (const EdgeAA& square : Squares)
+    {
+        CFixedVector2D a = start - square.p0;
+        CFixedVector2D b = end - square.p0;
+        fixed c = square.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 +566 @@
 {
     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 +613 @@
     // 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 +662 @@
     
     // 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 +705 @@
         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 +721 @@
         // 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 (const EdgeAA& sq : aaSquares)
     {
         // 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 - sq.p0.X).Absolute() <= sq.c1 &&
+            (start.p.Y - sq.p0.Y).Absolute() <= sq.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 - sq.p0.X).Absolute() <= sq.c1 &&
+                (vertexes[j].p.Y - sq.p0.Y).Absolute() <= sq.c1)
                 vertexes[j].status = Vertex::CLOSED;
     }
 
@@ -725 +744 @@
     {
 #define PUSH_POINT(p) STMT(xz.push_back(p.X.ToFloat()); xz.push_back(p.Y.ToFloat()))
         // Render the vertexes as little Pac-Man shapes to indicate quadrant direction
-        for (size_t i = 0; i < vertexes.size(); ++i)
+        for (const Vertex& vtx : vertexes)
         {
             m_DebugOverlayShortPathLines.emplace_back();
             m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1);
 
-            float x = vertexes[i].p.X.ToFloat();
-            float z = vertexes[i].p.Y.ToFloat();
+            float x = vtx.p.X.ToFloat();
+            float z = vtx.p.Y.ToFloat();
 
             float a0 = 0, a1 = 0;
             // Get arc start/end angles depending on quadrant (if any)
-            if      (vertexes[i].quadInward == QUADRANT_BL) { a0 = -0.25f; a1 = 0.50f; }
-            else if (vertexes[i].quadInward == QUADRANT_TR) { a0 =  0.25f; a1 = 1.00f; }
-            else if (vertexes[i].quadInward == QUADRANT_TL) { a0 = -0.50f; a1 = 0.25f; }
-            else if (vertexes[i].quadInward == QUADRANT_BR) { a0 =  0.00f; a1 = 0.75f; }
+            if      (vtx.quadInward == QUADRANT_BL) { a0 = -0.25f; a1 = 0.50f; }
+            else if (vtx.quadInward == QUADRANT_TR) { a0 =  0.25f; a1 = 1.00f; }
+            else if (vtx.quadInward == QUADRANT_TL) { a0 = -0.50f; a1 = 0.25f; }
+            else if (vtx.quadInward == QUADRANT_BR) { a0 =  0.00f; a1 = 0.75f; }
 
             if (a0 == a1)
                 SimRender::ConstructCircleOnGround(GetSimContext(), x, z, 0.5f,
@@ -750 +769 @@
         }
 
         // Render the edges
-        for (size_t i = 0; i < edges.size(); ++i)
+        for (const Edge& edg : edges)
         {
             m_DebugOverlayShortPathLines.emplace_back();
             m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
             std::vector<float> xz;
-            PUSH_POINT(edges[i].p0);
-            PUSH_POINT(edges[i].p1);
+            PUSH_POINT(edg.p0);
+            PUSH_POINT(edg.p1);
 
             // Add an arrowhead to indicate the direction
-            CFixedVector2D d = edges[i].p1 - edges[i].p0;
+            CFixedVector2D d = edg.p1 - edg.p0;
             d.Normalize(fixed::FromInt(1)/8);
-            CFixedVector2D p2 = edges[i].p1 - d*2;
+            CFixedVector2D p2 = edg.p1 - d*2;
             CFixedVector2D p3 = p2 + d.Perpendicular();
             CFixedVector2D p4 = p2 - d.Perpendicular();
             PUSH_POINT(p3);
             PUSH_POINT(p4);
-            PUSH_POINT(edges[i].p1);
+            PUSH_POINT(edg.p1);
 
             SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
         }
 #undef PUSH_POINT
 
-        // Render the axis-aligned squares
-        for (size_t i = 0; i < edgeSquares.size(); ++i)
+        // Render the axis-aligned rectangles
+        for (const Square& s : edgeSquares)
         {
             m_DebugOverlayShortPathLines.push_back(SOverlayLine());
             m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
             std::vector<float> xz;
-            Square s = edgeSquares[i];
             xz.push_back(s.p0.X.ToFloat());
             xz.push_back(s.p0.Y.ToFloat());
             xz.push_back(s.p0.X.ToFloat());
@@ -791 +809 @@
             xz.push_back(s.p0.Y.ToFloat());
             SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
         }
+        // Render the axis-aligned squares
+        for (const EdgeAA& sq : aaSquares)
+        {
+            m_DebugOverlayShortPathLines.push_back(SOverlayLine());
+            m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
+            std::vector<float> xz;
+            CFixedVector2D sqp0 = sq.p0 - CFixedVector2D(sq.c1, sq.c1);
+            CFixedVector2D sqp1 = sq.p0 + CFixedVector2D(sq.c1, sq.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 +868 @@
         // 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 +928 @@
             }
 
             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) &&