Ticket #1942: short-range_otp.patch

source/simulation2/helpers/PriorityQueue.h

@@ -34 +34 @@
 {
     bool operator()(const Item& a, const Item& b) const
     {
+        if (!b.secondPass && a.secondPass) // return to already opened ones last
+            return true;
+        if (!a.secondPass && b.secondPass)
+            return false;
         if (CMP()(b.rank, a.rank)) // higher costs are lower priority
             return true;
         if (CMP()(a.rank, b.rank))
@@ -69 +73 @@
         ID id;
         R rank; // f = g+h (estimated total cost of path through here)
         H h; // heuristic cost
+        bool secondPass;
     };
 
     void push(const Item& item)

source/simulation2/components/CCmpPathfinder_Vertex.cpp

@@ -90 +90 @@
         UNEXPLORED,
         OPEN,
         CLOSED,
+        UNDERSTUDY,
     };
 
     CFixedVector2D p;
@@ -795 +796 @@
     PROFILE_START("Short pathfinding - A*");
 
     VertexPriorityQueue open;
-    VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h };
+    VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h, false };
     open.push(qiStart);
 
     u16 idBest = START_VERTEX_ID;
     fixed hBest = start.h;
 
+    fixed maxDistFirstPass = entity_pos_t::FromInt(15);
+    bool hadASecondPass = false;
+    static u64 totalExam = 0;
+    
     while (!open.empty())
     {
         // Move best tile from open to closed
         VertexPriorityQueue::Item curr = open.pop();
+        // If this is the second pass we need to remember it and we'll be closed at the end of this loop.
+        bool secondPass = (vertexes[curr.id].status == Vertex::UNDERSTUDY);
         vertexes[curr.id].status = Vertex::CLOSED;
+        if (secondPass)
+            hadASecondPass = true;
 
         // If we've reached the destination, stop
         if (curr.id == GOAL_VERTEX_ID)
@@ -828 +837 @@
         // Check the lines to every other vertex
         for (size_t n = 0; n < vertexes.size(); ++n)
         {
-            if (vertexes[n].status == Vertex::CLOSED)
+            if (vertexes[n].status == Vertex::CLOSED || vertexes[n].status == Vertex::UNDERSTUDY)
                 continue;
 
             // If this is the magical goal vertex, move it to near the current vertex
@@ -866 +875 @@
                 }
             }
 
+            fixed distFromN = (vertexes[curr.id].p - npos).Length();
+            fixed g = vertexes[curr.id].g + distFromN;
+            
+            // Try and be clever based on distance to not check visibility to unwanted points.
+            // We do not study points we know that wouldn't improve our path anyway
+            // This first optimization does not change the algorithm itself.
+            // The second optimization assumes we won't be stuck in a unit-made U-shaped dead-end
+            // And tries to consider only potentially interesting vertices.
+            // Those are those closer to the goal than us, and those not too far away
+            // However sometimes we have to backtrack, usually the overall behavior is either
+            // much better or about equivalent.
+            if(g >= vertexes[n].g && vertexes[n].status != Vertex::UNEXPLORED)
+                continue;
+            
+            fixed h =  goal.DistanceToPoint(npos);
+            if (!hadASecondPass && (h > (hBest + entity_pos_t::FromInt(4)) || distFromN > maxDistFirstPass))
+            {
+                if (vertexes[curr.id].status != Vertex::UNDERSTUDY)
+                {
+                    // add us back to the list for a second pass if necessary.
+                    VertexPriorityQueue::Item newMe = curr;
+                    newMe.secondPass = true;
+                    open.push(newMe);
+                    vertexes[curr.id].status = Vertex::UNDERSTUDY;
+                }
+                continue;
+            }
+            
+            ++totalExam;
+
             bool visible =
                 CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) &&
                 CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) &&
@@ -883 +922 @@
             xz.push_back(npos.X.ToFloat());
             xz.push_back(npos.Y.ToFloat());
             SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false);
-            //*/
+             }//*/
 
             if (visible)
             {
-                fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length();
-
                 // If this is a new tile, compute the heuristic distance
                 if (vertexes[n].status == Vertex::UNEXPLORED)
                 {
@@ -895 +932 @@
                     // Add it to the open list:
                     vertexes[n].status = Vertex::OPEN;
                     vertexes[n].g = g;
-                    vertexes[n].h = goal.DistanceToPoint(npos);
+                    vertexes[n].h = h;
                     vertexes[n].pred = curr.id;
 
                     // If this is an axis-aligned shape, the path must continue in the same quadrant
@@ -908 +945 @@
                     if (n == GOAL_VERTEX_ID)
                         vertexes[n].p = npos; // remember the new best goal position
 
-                    VertexPriorityQueue::Item t = { (u16)n, g + vertexes[n].h, vertexes[n].h };
+                    VertexPriorityQueue::Item t = { (u16)n, g + vertexes[n].h, vertexes[n].h, false };
                     open.push(t);
 
                     // Remember the heuristically best vertex we've seen so far, in case we never actually reach the target
@@ -920 +957 @@
                 }
                 else // must be OPEN
                 {
-                    // If we've already seen this tile, and the new path to this tile does not have a
-                    // better cost, then stop now
-                    if (g >= vertexes[n].g)
-                        continue;
 
-                    // Otherwise, we have a better path, so replace the old one with the new cost/parent
+                    // Since this new tile is a better path, replace the old one with the new cost/parent
                     fixed gprev = vertexes[n].g;
                     vertexes[n].g = g;
                     vertexes[n].pred = curr.id;
@@ -944 +977 @@
         }
     }
 
+    std::cout << totalExam << std::endl;
     // Reconstruct the path (in reverse)
     for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred)
         path.m_Waypoints.emplace_back(Waypoint{ vertexes[id].p.X, vertexes[id].p.Y });