Ticket #3588: geometry1.2.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")
@@ -289 +290 @@
     return true;
 }
 
+// Unaligned squares are handled here.
+inline static bool CheckVisibilitySquares(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<ICmpObstructionManager::ObstructionSquare> squares)
+{
+    for (size_t i = 0; i < squares.size(); ++i)
+    {
+        const CFixedVector2D center = CFixedVector2D(squares[i].x, squares[i].z);
+        const CFixedVector2D halfSize = CFixedVector2D(squares[i].hw, squares[i].hh);
+        if (Geometry::TestRaySquare(a - center, b - center, squares[i].u, squares[i].v, halfSize))
+            return false;
+    }
+    return true;
+}
+
 typedef PriorityQueueHeap<u16, fixed, fixed> VertexPriorityQueue;
 
 /**
@@ -483 +497 @@
 static void SplitAAEdges(const CFixedVector2D& a,
         const std::vector<Edge>& edges,
         const std::vector<Square>& squares,
-        std::vector<Edge>& edgesUnaligned,
         std::vector<EdgeAA>& edgesLeft, std::vector<EdgeAA>& edgesRight,
         std::vector<EdgeAA>& edgesBottom, std::vector<EdgeAA>& edgesTop)
 {
@@ -536 +549 @@
                 edgesTop.emplace_back(EdgeAA{ edge.p0, edge.p1.X });
             }
         }
-        else
-            edgesUnaligned.push_back(edge);
     }
 }
 
@@ -648 +659 @@
     entity_pos_t pathfindClearance = clearance;
     
     // Convert each obstruction square into collision edges and search graph vertexes
+    std::vector<ICmpObstructionManager::ObstructionSquare> squaresUnaligned;
     for (size_t i = 0; i < squares.size(); ++i)
     {
         CFixedVector2D center(squares[i].x, squares[i].z);
@@ -677 +689 @@
 
         // Add the edges:
 
-        CFixedVector2D h0(squares[i].hw + pathfindClearance, squares[i].hh + pathfindClearance);
-        CFixedVector2D h1(squares[i].hw + pathfindClearance, -(squares[i].hh + pathfindClearance));
+        if(aa)
+        {
+            CFixedVector2D h0(squares[i].hw + pathfindClearance, squares[i].hh + pathfindClearance);
+            CFixedVector2D h1(squares[i].hw + pathfindClearance, -(squares[i].hh + pathfindClearance));
 
-        CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v));
-        CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v));
-        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)
+            CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v));
+            CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v));
             edgeSquares.emplace_back(Square{ ev1, ev3 });
-        else
-        {
-            edges.emplace_back(Edge{ ev0, ev1 });
-            edges.emplace_back(Edge{ ev1, ev2 });
-            edges.emplace_back(Edge{ ev2, ev3 });
-            edges.emplace_back(Edge{ ev3, ev0 });
         }
+        else
+            squaresUnaligned.push_back(squares[i]);
 
         // TODO: should clip out vertexes and edges that are outside the range,
         // to reduce the search space
@@ -771 +778 @@
             xz.push_back(s.p0.Y.ToFloat());
             SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
         }
+        // Render the unaligned squares
+        for (size_t i = 0; i < squaresUnaligned.size(); ++i)
+        {
+            m_DebugOverlayShortPathLines.push_back(SOverlayLine());
+            m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
+            std::vector<float> xz;
+            ICmpObstructionManager::ObstructionSquare s = squaresUnaligned[i];
+            CFixedVector2D h0(s.hw + clearance, s.hh + clearance);
+            CFixedVector2D h1(s.hw + clearance, -(s.hh + clearance));
+            CFixedVector2D u = s.u;
+            CFixedVector2D v = s.v;
+            CFixedVector2D center = CFixedVector2D(s.x, s.z);
+
+            CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v));
+            CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v));
+            CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v));
+            CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v));
+            xz.push_back(ev0.X.ToFloat());
+            xz.push_back(ev0.Y.ToFloat());
+            xz.push_back(ev1.X.ToFloat());
+            xz.push_back(ev1.Y.ToFloat());
+            xz.push_back(ev2.X.ToFloat());
+            xz.push_back(ev2.Y.ToFloat());
+            xz.push_back(ev3.X.ToFloat());
+            xz.push_back(ev3.Y.ToFloat());
+            xz.push_back(ev0.X.ToFloat());
+            xz.push_back(ev0.Y.ToFloat());
+            SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
+        }
     }
 
     // Do an A* search over the vertex/visibility graph:
@@ -810 +846 @@
         // 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);
+        SplitAAEdges(vertexes[curr.id].p, edges, edgeSquares, edgesLeft, edgesRight, edgesBottom, edgesTop);
 
         // Check the lines to every other vertex
         for (size_t n = 0; n < vertexes.size(); ++n)
@@ -871 +906 @@
                 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);
+                CheckVisibilitySquares(vertexes[curr.id].p, npos, squaresUnaligned);
 
             /*
             // Render the edges that we examine

source/simulation2/helpers/Geometry.cpp

@@ -1 @@
-/* Copyright (C) 2015 Wildfire Games.
+/* Copyright (C) 2016 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -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;
+    // Check if all points are on one side of a->b.
+    // This can be simplified by using the following equivalence:
+    // (x +- y +- z) are all > 0 or all < 0 <=> |x| > |y| + |z|
+    // and the linearity of scalar products.
+    // Furthermore, we already computed
+    // u.Dot((b-a).Perpendicular()) = (u.Perpendicular()).Dot(b-a) = av - bv, v.Dot(...) similarly
+    // Also, a.Dot(a.Perpendicular()) = 0.
+    fixed s0 = a.Dot(b.Perpendicular()).Absolute();
+    fixed s1 = (av - bv).Absolute().Multiply(hw) + (bu - au).Absolute().Multiply(hh);
+    return (s0 <= s1);
 }
 
 // Exactly like TestRaySquare with u=(1,0), v=(0,1)
@@ -248 +240 @@
     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 +259 @@
     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())
+    // We only check the "counterclockwisest" point.
+    // But first we check if the center is on the wrong side.
+    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(