Ticket #3588: geometry2.patch

source/simulation2/components/CCmpPathfinder_Vertex.cpp

@@ -39 +39 @@
 #include "simulation2/components/ICmpObstructionManager.h"
 #include "simulation2/helpers/PriorityQueue.h"
 #include "simulation2/helpers/Render.h"
+#include "simulation2/helpers/Geometry.h"
 
 /* Quadrant optimisation:
  * (loosely based on GPG2 "Optimizing Points-of-Visibility Pathfinding")
@@ -186 +187 @@
 
     for (size_t i = 0; i < edges.size(); ++i)
     {
-        if (b.X < edges[i].p0.X)
+        if (a.X > edges[i].p0.X || b.X < edges[i].p0.X)
             continue;
 
         CFixedVector2D p0 (edges[i].p0.X, edges[i].c1);
@@ -214 +215 @@
 
     for (size_t i = 0; i < edges.size(); ++i)
     {
-        if (b.X > edges[i].p0.X)
+        if (a.X < edges[i].p0.X || b.X > edges[i].p0.X)
             continue;
 
         CFixedVector2D p0 (edges[i].p0.X, edges[i].c1);
@@ -242 +243 @@
 
     for (size_t i = 0; i < edges.size(); ++i)
     {
-        if (b.Y < edges[i].p0.Y)
+        if (a.Y > edges[i].p0.Y || b.Y < edges[i].p0.Y)
             continue;
 
         CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y);
@@ -270 +271 @@
 
     for (size_t i = 0; i < edges.size(); ++i)
     {
-        if (b.Y > edges[i].p0.Y)
+        if (a.Y < edges[i].p0.Y || b.Y > edges[i].p0.Y)
             continue;
 
         CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y);
@@ -289 +290 @@
     return true;
 }
 
+inline static bool CheckVisibilityAASquares(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<Square> edgeSquares)
+{
+    for (size_t i = 0; i < edgeSquares.size(); ++i)
+    {
+        CFixedVector2D offset = (edgeSquares[i].p0 + edgeSquares[i].p1)/2;
+        CFixedVector2D halfSize = (edgeSquares[i].p1 - edgeSquares[i].p0)/2;
+        if (Geometry::TestRayAASquare(a - offset, b - offset, halfSize))
+            return false;
+    }
+    return true;
+}
+
 typedef PriorityQueueHeap<u16, fixed, fixed> VertexPriorityQueue;
 
 /**
@@ -480 +493 @@
     }
 }
 
-static void SplitAAEdges(const CFixedVector2D& a,
-        const std::vector<Edge>& edges,
-        const std::vector<Square>& squares,
+static void SplitAAEdges(const std::vector<Edge>& edges,
         std::vector<Edge>& edgesUnaligned,
         std::vector<EdgeAA>& edgesLeft, std::vector<EdgeAA>& edgesRight,
         std::vector<EdgeAA>& edgesBottom, std::vector<EdgeAA>& edgesTop)
 {
-    edgesLeft.reserve(squares.size());
-    edgesRight.reserve(squares.size());
-    edgesBottom.reserve(squares.size());
-    edgesTop.reserve(squares.size());
-
-    for (const Square& square : squares)
-    {
-        if (a.X <= square.p0.X)
-            edgesLeft.emplace_back(EdgeAA{ square.p0, square.p1.Y });
-        if (a.X >= square.p1.X)
-            edgesRight.emplace_back(EdgeAA{ square.p1, square.p0.Y });
-        if (a.Y <= square.p0.Y)
-            edgesBottom.emplace_back(EdgeAA{ square.p0, square.p1.X });
-        if (a.Y >= square.p1.Y)
-            edgesTop.emplace_back(EdgeAA{ square.p1, square.p0.X });
-    }
-
     for (const Edge& edge : edges)
     {
         if (edge.p0.X == edge.p1.X)
         {
             if (edge.p1.Y < edge.p0.Y)
-            {
-                if (!(a.X <= edge.p0.X))
-                    continue;
                 edgesLeft.emplace_back(EdgeAA{ edge.p1, edge.p0.Y });
-            }
             else
-            {
-                if (!(a.X >= edge.p0.X))
-                    continue;
                 edgesRight.emplace_back(EdgeAA{ edge.p1, edge.p0.Y });
-            }
         }
         else if (edge.p0.Y == edge.p1.Y)
         {
             if (edge.p0.X < edge.p1.X)
-            {
-                if (!(a.Y <= edge.p0.Y))
-                    continue;
                 edgesBottom.emplace_back(EdgeAA{ edge.p0, edge.p1.X });
-            }
             else
-            {
-                if (!(a.Y >= edge.p0.Y))
-                    continue;
                 edgesTop.emplace_back(EdgeAA{ edge.p0, edge.p1.X });
-            }
         }
         else
             edgesUnaligned.push_back(edge);
@@ -793 +771 @@
     u16 idBest = START_VERTEX_ID;
     fixed hBest = start.h;
 
+    // Detect axis-aligned edges for faster visibility checking.
+    std::vector<Edge> edgesUnaligned;
+    std::vector<EdgeAA> edgesLeft;
+    std::vector<EdgeAA> edgesRight;
+    std::vector<EdgeAA> edgesBottom;
+    std::vector<EdgeAA> edgesTop;
+    SplitAAEdges(edges, edgesUnaligned, edgesLeft, edgesRight, edgesBottom, edgesTop);
+
     while (!open.empty())
     {
         // Move best tile from open to closed
@@ -810 +796 @@
         // since they're more likely to block the rays
         std::sort(edgeSquares.begin(), edgeSquares.end(), SquareSort(vertexes[curr.id].p));
 
-        std::vector<Edge> edgesUnaligned;
-        std::vector<EdgeAA> edgesLeft;
-        std::vector<EdgeAA> edgesRight;
-        std::vector<EdgeAA> edgesBottom;
-        std::vector<EdgeAA> edgesTop;
-        SplitAAEdges(vertexes[curr.id].p, edges, edgeSquares, edgesUnaligned, edgesLeft, edgesRight, edgesBottom, edgesTop);
-
         // Check the lines to every other vertex
         for (size_t n = 0; n < vertexes.size(); ++n)
         {
@@ -871 +850 @@
                 CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) &&
                 CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) &&
                 CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) &&
-                CheckVisibility(vertexes[curr.id].p, npos, edgesUnaligned);
+                CheckVisibility(vertexes[curr.id].p, npos, edgesUnaligned) &&
+                CheckVisibilityAASquares(vertexes[curr.id].p, npos, edgeSquares);
 
             /*
             // Render the edges that we examine

source/simulation2/helpers/Geometry.cpp

@@ -196 +196 @@
     /*
      * We only consider collisions to be when the ray goes from outside to inside the shape (and possibly out again).
      * Various cases to consider:
-     *   'a' inside, 'b' inside -> no collision
-     *   'a' inside, 'b' outside -> no collision
-     *   'a' outside, 'b' inside -> collision
-     *   'a' outside, 'b' outside -> depends; use separating axis theorem:
+     *   'a' inside -> no collision
+     *   use separating axis theorem:
      *     if the ray's bounding box is outside the square -> no collision
      *     if the whole square is on the same side of the ray -> no collision
      *     otherwise -> collision
@@ -218 +216 @@
     fixed bu = b.Dot(u);
     fixed bv = b.Dot(v);
 
-    if (-hw <= bu && bu <= hw && -hh <= bv && bv <= hh) // TODO: isn't this subsumed by the next checks?
-        return true; // a is outside, b is inside
-
     if ((au < -hw && bu < -hw) || (au > hw && bu > hw) || (av < -hh && bv < -hh) || (av > hh && bv > hh))
         return false; // ab is entirely above/below/side the square
 
     CFixedVector2D abp = (b - a).Perpendicular();
-    fixed s0 = abp.Dot((u.Multiply(hw) + v.Multiply(hh)) - a);
-    fixed s1 = abp.Dot((u.Multiply(hw) - v.Multiply(hh)) - a);
-    fixed s2 = abp.Dot((-u.Multiply(hw) - v.Multiply(hh)) - a);
-    fixed s3 = abp.Dot((-u.Multiply(hw) + v.Multiply(hh)) - a);
-    if (s0.IsZero() || s1.IsZero() || s2.IsZero() || s3.IsZero())
-        return true; // ray intersects the corner
-
-    bool sign = (s0 < fixed::Zero());
-    if ((s1 < fixed::Zero()) != sign || (s2 < fixed::Zero()) != sign || (s3 < fixed::Zero()) != sign)
-        return true; // ray cuts through the square
-
-    return false;
+    fixed s0 = a.Dot(abp).Absolute();
+    fixed s1 = u.Dot(abp).Absolute().Multiply(hw) + v.Dot(abp).Absolute().Multiply(hh);
+    return (s0 <= s1);
 }
 
 // Exactly like TestRaySquare with u=(1,0), v=(0,1)
@@ -248 +234 @@
     if (-hw <= a.X && a.X <= hw && -hh <= a.Y && a.Y <= hh)
         return false; // a is inside
 
-    if (-hw <= b.X && b.X <= hw && -hh <= b.Y && b.Y <= hh) // TODO: isn't this subsumed by the next checks?
-        return true; // a is outside, b is inside
-
     if ((a.X < -hw && b.X < -hw) || (a.X > hw && b.X > hw) || (a.Y < -hh && b.Y < -hh) || (a.Y > hh && b.Y > hh))
         return false; // ab is entirely above/below/side the square
 
     CFixedVector2D abp = (b - a).Perpendicular();
-    fixed s0 = abp.Dot(CFixedVector2D(hw, hh) - a);
-    fixed s1 = abp.Dot(CFixedVector2D(hw, -hh) - a);
-    fixed s2 = abp.Dot(CFixedVector2D(-hw, -hh) - a);
-    fixed s3 = abp.Dot(CFixedVector2D(-hw, hh) - a);
-    if (s0.IsZero() || s1.IsZero() || s2.IsZero() || s3.IsZero())
-        return true; // ray intersects the corner
-
-    bool sign = (s0 < fixed::Zero());
-    if ((s1 < fixed::Zero()) != sign || (s2 < fixed::Zero()) != sign || (s3 < fixed::Zero()) != sign)
-        return true; // ray cuts through the square
-
-    return false;
+    fixed s0 = a.Dot(abp).Absolute();
+    fixed s1 = abp.X.Absolute().Multiply(hw) + abp.Y.Absolute().Multiply(hh);
+    return (s0 <= s1);
 }
 
 /**
@@ -279 +253 @@
     fixed hh = halfSize.Y;
 
     CFixedVector2D p = axis.Perpendicular();
-    if (p.Dot((u.Multiply(hw) + v.Multiply(hh)) - a) <= fixed::Zero())
-        return true;
-    if (p.Dot((u.Multiply(hw) - v.Multiply(hh)) - a) <= fixed::Zero())
+    fixed s0 = p.Dot(a);
+    if (s0 >= fixed::Zero())
         return true;
-    if (p.Dot((-u.Multiply(hw) - v.Multiply(hh)) - a) <= fixed::Zero())
-        return true;
-    if (p.Dot((-u.Multiply(hw) + v.Multiply(hh)) - a) <= fixed::Zero())
-        return true;
-
-    return false;
+    s0 += u.Dot(p).Absolute().Multiply(hw) + v.Dot(p).Absolute().Multiply(hh);
+    return (s0 >= fixed::Zero());
 }
 
 bool Geometry::TestSquareSquare(