source/simulation2/components/CCmpPathfinder_Vertex.cpp

   1 /* Copyright (C) 2015 Wildfire Games.
   2  * This file is part of 0 A.D.
   3  *
   4  * 0 A.D. is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * 0 A.D. is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 /**
  19  * @file
  20  * Vertex-based algorithm for CCmpPathfinder.
  21  * Computes paths around the corners of rectangular obstructions.
  22  *
  23  * Useful search term for this algorithm: "points of visibility".
  24  *
  25  * Since we sometimes want to use this for avoiding moving units, there is no
  26  * pre-computation - the whole visibility graph is effectively regenerated for
  27  * each path, and it does A* over that graph.
  28  *
  29  * This scales very poorly in the number of obstructions, so it should be used
  30  * with a limited range and not exceedingly frequently.
  31  */
  32
  33 #include "precompiled.h"
  34
  35 #include "CCmpPathfinder_Common.h"
  36
  37 #include "lib/timer.h"
  38 #include "ps/Profile.h"
  39 #include "simulation2/components/ICmpObstructionManager.h"
  40 #include "simulation2/helpers/PriorityQueue.h"
  41 #include "simulation2/helpers/Render.h"
  42
  43 /* Quadrant optimisation:
  44  * (loosely based on GPG2 "Optimizing Points-of-Visibility Pathfinding")
  45  *
  46  * Consider the vertex ("@") at a corner of an axis-aligned rectangle ("#"):
  47  *
  48  * TL  :  TR
  49  *     :
  50  * ####@ - - -
  51  * #####
  52  * #####
  53  * BL ##  BR
  54  *
  55  * The area around the vertex is split into TopLeft, BottomRight etc quadrants.
  56  *
  57  * If the shortest path reaches this vertex, it cannot continue to a vertex in
  58  * the BL quadrant (it would be blocked by the shape).
  59  * Since the shortest path is wrapped tightly around the edges of obstacles,
  60  * if the path approached this vertex from the TL quadrant,
  61  * it cannot continue to the TL or TR quadrants (the path could be shorter if it
  62  * skipped this vertex).
  63  * Therefore it must continue to a vertex in the BR quadrant (so this vertex is in
  64  * *that* vertex's TL quadrant).
  65  *
  66  * That lets us significantly reduce the search space by quickly discarding vertexes
  67  * from the wrong quadrants.
  68  *
  69  * (This causes badness if the path starts from inside the shape, so we add some hacks
  70  * for that case.)
  71  *
  72  * (For non-axis-aligned rectangles it's harder to do this computation, so we'll
  73  * not bother doing any discarding for those.)
  74  */
  75 static const u8 QUADRANT_NONE = 0;
  76 static const u8 QUADRANT_BL = 1;
  77 static const u8 QUADRANT_TR = 2;
  78 static const u8 QUADRANT_TL = 4;
  79 static const u8 QUADRANT_BR = 8;
  80 static const u8 QUADRANT_BLTR = QUADRANT_BL|QUADRANT_TR;
  81 static const u8 QUADRANT_TLBR = QUADRANT_TL|QUADRANT_BR;
  82 static const u8 QUADRANT_ALL = QUADRANT_BLTR|QUADRANT_TLBR;
  83
  84 // A vertex around the corners of an obstruction
  85 // (paths will be sequences of these vertexes)
  86 struct Vertex
  87 {
  88         enum
  89         {
  90                 UNEXPLORED,
  91                 OPEN,
  92                 CLOSED,
  93         };
  94
  95         CFixedVector2D p;
  96         fixed g, h;
  97         u16 pred;
  98         u8 status;
  99         u8 quadInward : 4; // the quadrant which is inside the shape (or NONE)
 100         u8 quadOutward : 4; // the quadrants of the next point on the path which this vertex must be in, given 'pred'
 101 };
 102
 103 // Obstruction edges (paths will not cross any of these).
 104 // Defines the two points of the edge.
 105 struct Edge
 106 {
 107         CFixedVector2D p0, p1;
 108 };
 109
 110 // Axis-aligned obstruction squares (paths will not cross any of these).
 111 // Defines the opposing corners of an axis-aligned square
 112 // (from which four individual edges can be trivially computed), requiring p0 <= p1
 113 struct Square
 114 {
 115         CFixedVector2D p0, p1;
 116 };
 117
 118 // Axis-aligned obstruction edges.
 119 // p0 defines one end; c1 is either the X or Y coordinate of the other end,
 120 // depending on the context in which this is used.
 121 struct EdgeAA
 122 {
 123         CFixedVector2D p0;
 124         fixed c1;
 125 };
 126
 127 // When computing vertexes to insert into the search graph,
 128 // add a small delta so that the vertexes of an edge don't get interpreted
 129 // as crossing the edge (given minor numerical inaccuracies)
 130 static const entity_pos_t EDGE_EXPAND_DELTA = entity_pos_t::FromInt(1)/16;
 131
 132 /**
 133  * Check whether a ray from 'a' to 'b' crosses any of the edges.
 134  * (Edges are one-sided so it's only considered a cross if going from front to back.)
 135  */
 136 inline static bool CheckVisibility(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<Edge>& edges)
 137 {
 138         CFixedVector2D abn = (b - a).Perpendicular();
 139
 140         // Edges of general non-axis-aligned shapes
 141         for (size_t i = 0; i < edges.size(); ++i)
 142         {
 143                 CFixedVector2D p0 = edges[i].p0;
 144                 CFixedVector2D p1 = edges[i].p1;
 145
 146                 CFixedVector2D d = (p1 - p0).Perpendicular();
 147
 148                 // If 'a' is behind the edge, we can't cross
 149                 fixed q = (a - p0).Dot(d);
 150                 if (q < fixed::Zero())
 151                         continue;
 152
 153                 // If 'b' is in front of the edge, we can't cross
 154                 fixed r = (b - p0).Dot(d);
 155                 if (r > fixed::Zero())
 156                         continue;
 157
 158                 // The ray is crossing the infinitely-extended edge from in front to behind.
 159                 // Check the finite edge is crossing the infinitely-extended ray too.
 160                 // (Given the previous tests, it can only be crossing in one direction.)
 161                 fixed s = (p0 - a).Dot(abn);
 162                 if (s > fixed::Zero())
 163                         continue;
 164
 165                 fixed t = (p1 - a).Dot(abn);
 166                 if (t < fixed::Zero())
 167                         continue;
 168
 169                 return false;
 170         }
 171
 172         return true;
 173 }
 174
 175 // Handle the axis-aligned shape edges separately (for performance):
 176 // (These are specialised versions of the general unaligned edge code.
 177 // They assume the caller has already excluded edges for which 'a' is
 178 // on the wrong side.)
 179
 180 inline static bool CheckVisibilityLeft(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<EdgeAA>& edges)
 181 {
 182         if (a.X >= b.X)
 183                 return true;
 184
 185         CFixedVector2D abn = (b - a).Perpendicular();
 186
 187         for (size_t i = 0; i < edges.size(); ++i)
 188         {
 189                 if (b.X < edges[i].p0.X)
 190                         continue;
 191
 192                 CFixedVector2D p0 (edges[i].p0.X, edges[i].c1);
 193                 fixed s = (p0 - a).Dot(abn);
 194                 if (s > fixed::Zero())
 195                         continue;
 196
 197                 CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y);
 198                 fixed t = (p1 - a).Dot(abn);
 199                 if (t < fixed::Zero())
 200                         continue;
 201
 202                 return false;
 203         }
 204
 205         return true;
 206 }
 207
 208 inline static bool CheckVisibilityRight(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<EdgeAA>& edges)
 209 {
 210         if (a.X <= b.X)
 211                 return true;
 212
 213         CFixedVector2D abn = (b - a).Perpendicular();
 214
 215         for (size_t i = 0; i < edges.size(); ++i)
 216         {
 217                 if (b.X > edges[i].p0.X)
 218                         continue;
 219
 220                 CFixedVector2D p0 (edges[i].p0.X, edges[i].c1);
 221                 fixed s = (p0 - a).Dot(abn);
 222                 if (s > fixed::Zero())
 223                         continue;
 224
 225                 CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y);
 226                 fixed t = (p1 - a).Dot(abn);
 227                 if (t < fixed::Zero())
 228                         continue;
 229
 230                 return false;
 231         }
 232
 233         return true;
 234 }
 235
 236 inline static bool CheckVisibilityBottom(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<EdgeAA>& edges)
 237 {
 238         if (a.Y >= b.Y)
 239                 return true;
 240
 241         CFixedVector2D abn = (b - a).Perpendicular();
 242
 243         for (size_t i = 0; i < edges.size(); ++i)
 244         {
 245                 if (b.Y < edges[i].p0.Y)
 246                         continue;
 247
 248                 CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y);
 249                 fixed s = (p0 - a).Dot(abn);
 250                 if (s > fixed::Zero())
 251                         continue;
 252
 253                 CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y);
 254                 fixed t = (p1 - a).Dot(abn);
 255                 if (t < fixed::Zero())
 256                         continue;
 257
 258                 return false;
 259         }
 260
 261         return true;
 262 }
 263
 264 inline static bool CheckVisibilityTop(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<EdgeAA>& edges)
 265 {
 266         if (a.Y <= b.Y)
 267                 return true;
 268
 269         CFixedVector2D abn = (b - a).Perpendicular();
 270
 271         for (size_t i = 0; i < edges.size(); ++i)
 272         {
 273                 if (b.Y > edges[i].p0.Y)
 274                         continue;
 275
 276                 CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y);
 277                 fixed s = (p0 - a).Dot(abn);
 278                 if (s > fixed::Zero())
 279                         continue;
 280
 281                 CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y);
 282                 fixed t = (p1 - a).Dot(abn);
 283                 if (t < fixed::Zero())
 284                         continue;
 285
 286                 return false;
 287         }
 288
 289         return true;
 290 }
 291
 292 typedef PriorityQueueHeap<u16, fixed, fixed> VertexPriorityQueue;
 293
 294 /**
 295  * Add edges and vertexes to represent the boundaries between passable and impassable
 296  * navcells (for impassable terrain).
 297  * Navcells i0 <= i <= i1, j0 <= j <= j1 will be considered.
 298  */
 299 static void AddTerrainEdges(std::vector<Edge>& edges, std::vector<Vertex>& vertexes,
 300         int i0, int j0, int i1, int j1,
 301         pass_class_t passClass, const Grid<NavcellData>& grid)
 302 {
 303         PROFILE("AddTerrainEdges");
 304
 305         // Clamp the coordinates so we won't attempt to sample outside of the grid.
 306         // (This assumes the outermost ring of navcells (which are always impassable)
 307         // won't have a boundary with any passable navcells. TODO: is that definitely
 308         // safe enough?)
 309
 310         i0 = clamp(i0, 1, grid.m_W-2);
 311         j0 = clamp(j0, 1, grid.m_H-2);
 312         i1 = clamp(i1, 1, grid.m_W-2);
 313         j1 = clamp(j1, 1, grid.m_H-2);
 314
 315         for (int j = j0; j <= j1; ++j)
 316         {
 317                 for (int i = i0; i <= i1; ++i)
 318                 {
 319                         if (IS_PASSABLE(grid.get(i, j), passClass))
 320                                 continue;
 321
 322                         if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i+1, j+1), passClass))
 323                         {
 324                                 Vertex vert;
 325                                 vert.status = Vertex::UNEXPLORED;
 326                                 vert.quadOutward = QUADRANT_ALL;
 327                                 vert.quadInward = QUADRANT_BL;
 328                                 vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
 329                                 vertexes.push_back(vert);
 330                         }
 331
 332                         if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i-1, j+1), passClass))
 333                         {
 334                                 Vertex vert;
 335                                 vert.status = Vertex::UNEXPLORED;
 336                                 vert.quadOutward = QUADRANT_ALL;
 337                                 vert.quadInward = QUADRANT_BR;
 338                                 vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
 339                                 vertexes.push_back(vert);
 340                         }
 341
 342                         if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i+1, j-1), passClass))
 343                         {
 344                                 Vertex vert;
 345                                 vert.status = Vertex::UNEXPLORED;
 346                                 vert.quadOutward = QUADRANT_ALL;
 347                                 vert.quadInward = QUADRANT_TL;
 348                                 vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
 349                                 vertexes.push_back(vert);
 350                         }
 351
 352                         if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i-1, j-1), passClass))
 353                         {
 354                                 Vertex vert;
 355                                 vert.status = Vertex::UNEXPLORED;
 356                                 vert.quadOutward = QUADRANT_ALL;
 357                                 vert.quadInward = QUADRANT_TR;
 358                                 vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
 359                                 vertexes.push_back(vert);
 360                         }
 361                 }
 362         }
 363
 364         // XXX rewrite this stuff
 365
 366         for (int j = j0; j < j1; ++j)
 367         {
 368                 std::vector<u16> segmentsR;
 369                 std::vector<u16> segmentsL;
 370
 371                 for (int i = i0; i <= i1; ++i)
 372                 {
 373                         bool a = IS_PASSABLE(grid.get(i, j+1), passClass);
 374                         bool b = IS_PASSABLE(grid.get(i, j), passClass);
 375                         if (a && !b)
 376                                 segmentsL.push_back(i);
 377                         if (b && !a)
 378                                 segmentsR.push_back(i);
 379                 }
 380
 381                 if (!segmentsR.empty())
 382                 {
 383                         segmentsR.push_back(0); // sentinel value to simplify the loop
 384                         u16 ia = segmentsR[0];
 385                         u16 ib = ia + 1;
 386                         for (size_t n = 1; n < segmentsR.size(); ++n)
 387                         {
 388                                 if (segmentsR[n] == ib)
 389                                         ++ib;
 390                                 else
 391                                 {
 392                                         CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
 393                                         CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
 394                                         edges.emplace_back(Edge{ v0, v1 });
 395
 396                                         ia = segmentsR[n];
 397                                         ib = ia + 1;
 398                                 }
 399                         }
 400                 }
 401
 402                 if (!segmentsL.empty())
 403                 {
 404                         segmentsL.push_back(0); // sentinel value to simplify the loop
 405                         u16 ia = segmentsL[0];
 406                         u16 ib = ia + 1;
 407                         for (size_t n = 1; n < segmentsL.size(); ++n)
 408                         {
 409                                 if (segmentsL[n] == ib)
 410                                         ++ib;
 411                                 else
 412                                 {
 413                                         CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
 414                                         CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
 415                                         edges.emplace_back(Edge{ v0, v1 });
 416
 417                                         ia = segmentsL[n];
 418                                         ib = ia + 1;
 419                                 }
 420                         }
 421                 }
 422         }
 423
 424         for (int i = i0; i < i1; ++i)
 425         {
 426                 std::vector<u16> segmentsU;
 427                 std::vector<u16> segmentsD;
 428
 429                 for (int j = j0; j <= j1; ++j)
 430                 {
 431                         bool a = IS_PASSABLE(grid.get(i+1, j), passClass);
 432                         bool b = IS_PASSABLE(grid.get(i, j), passClass);
 433                         if (a && !b)
 434                                 segmentsU.push_back(j);
 435                         if (b && !a)
 436                                 segmentsD.push_back(j);
 437                 }
 438
 439                 if (!segmentsU.empty())
 440                 {
 441                         segmentsU.push_back(0); // sentinel value to simplify the loop
 442                         u16 ja = segmentsU[0];
 443                         u16 jb = ja + 1;
 444                         for (size_t n = 1; n < segmentsU.size(); ++n)
 445                         {
 446                                 if (segmentsU[n] == jb)
 447                                         ++jb;
 448                                 else
 449                                 {
 450                                         CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE);
 451                                         CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE);
 452                                         edges.emplace_back(Edge{ v0, v1 });
 453
 454                                         ja = segmentsU[n];
 455                                         jb = ja + 1;
 456                                 }
 457                         }
 458                 }
 459
 460                 if (!segmentsD.empty())
 461                 {
 462                         segmentsD.push_back(0); // sentinel value to simplify the loop
 463                         u16 ja = segmentsD[0];
 464                         u16 jb = ja + 1;
 465                         for (size_t n = 1; n < segmentsD.size(); ++n)
 466                         {
 467                                 if (segmentsD[n] == jb)
 468                                         ++jb;
 469                                 else
 470                                 {
 471                                         CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE);
 472                                         CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE);
 473                                         edges.emplace_back(Edge{ v0, v1 });
 474
 475                                         ja = segmentsD[n];
 476                                         jb = ja + 1;
 477                                 }
 478                         }
 479                 }
 480         }
 481 }
 482
 483 static void SplitAAEdges(const CFixedVector2D& a,
 484                 const std::vector<Edge>& edges,
 485                 const std::vector<Square>& squares,
 486                 std::vector<Edge>& edgesUnaligned,
 487                 std::vector<EdgeAA>& edgesLeft, std::vector<EdgeAA>& edgesRight,
 488                 std::vector<EdgeAA>& edgesBottom, std::vector<EdgeAA>& edgesTop)
 489 {
 490         edgesLeft.reserve(squares.size());
 491         edgesRight.reserve(squares.size());
 492         edgesBottom.reserve(squares.size());
 493         edgesTop.reserve(squares.size());
 494
 495         for (const Square& square : squares)
 496         {
 497                 if (a.X <= square.p0.X)
 498                         edgesLeft.emplace_back(EdgeAA{ square.p0, square.p1.Y });
 499                 if (a.X >= square.p1.X)
 500                         edgesRight.emplace_back(EdgeAA{ square.p1, square.p0.Y });
 501                 if (a.Y <= square.p0.Y)
 502                         edgesBottom.emplace_back(EdgeAA{ square.p0, square.p1.X });
 503                 if (a.Y >= square.p1.Y)
 504                         edgesTop.emplace_back(EdgeAA{ square.p1, square.p0.X });
 505         }
 506
 507         for (const Edge& edge : edges)
 508         {
 509                 if (edge.p0.X == edge.p1.X)
 510                 {
 511                         if (edge.p1.Y < edge.p0.Y)
 512                         {
 513                                 if (!(a.X <= edge.p0.X))
 514                                         continue;
 515                                 edgesLeft.emplace_back(EdgeAA{ edge.p1, edge.p0.Y });
 516                         }
 517                         else
 518                         {
 519                                 if (!(a.X >= edge.p0.X))
 520                                         continue;
 521                                 edgesRight.emplace_back(EdgeAA{ edge.p1, edge.p0.Y });
 522                         }
 523                 }
 524                 else if (edge.p0.Y == edge.p1.Y)
 525                 {
 526                         if (edge.p0.X < edge.p1.X)
 527                         {
 528                                 if (!(a.Y <= edge.p0.Y))
 529                                         continue;
 530                                 edgesBottom.emplace_back(EdgeAA{ edge.p0, edge.p1.X });
 531                         }
 532                         else
 533                         {
 534                                 if (!(a.Y >= edge.p0.Y))
 535                                         continue;
 536                                 edgesTop.emplace_back(EdgeAA{ edge.p0, edge.p1.X });
 537                         }
 538                 }
 539                 else
 540                         edgesUnaligned.push_back(edge);
 541         }
 542 }
 543
 544 /**
 545  * Functor for sorting edge-squares by approximate proximity to a fixed point.
 546  */
 547 struct SquareSort
 548 {
 549         CFixedVector2D src;
 550         SquareSort(CFixedVector2D src) : src(src) { }
 551         bool operator()(const Square& a, const Square& b)
 552         {
 553                 if ((a.p0 - src).CompareLength(b.p0 - src) < 0)
 554                         return true;
 555                 return false;
 556         }
 557 };
 558
 559 void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
 560         entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance,
 561         entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& path)
 562 {
 563         PROFILE3("ComputeShortPath");
 564
 565         m_DebugOverlayShortPathLines.clear();
 566
 567         if (m_DebugOverlay)
 568         {
 569                 // Render the goal shape
 570                 m_DebugOverlayShortPathLines.push_back(SOverlayLine());
 571                 m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1);
 572                 switch (goal.type)
 573                 {
 574                 case PathGoal::POINT:
 575                 {
 576                         SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true);
 577                         break;
 578                 }
 579                 case PathGoal::CIRCLE:
 580                 case PathGoal::INVERTED_CIRCLE:
 581                 {
 582                         SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true);
 583                         break;
 584                 }
 585                 case PathGoal::SQUARE:
 586                 case PathGoal::INVERTED_SQUARE:
 587                 {
 588                         float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat());
 589                         SimRender::ConstructSquareOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true);
 590                         break;
 591                 }
 592                 }
 593         }
 594
 595         // List of collision edges - paths must never cross these.
 596         // (Edges are one-sided so intersections are fine in one direction, but not the other direction.)
 597         std::vector<Edge> edges;
 598         std::vector<Square> edgeSquares; // axis-aligned squares; equivalent to 4 edges
 599
 600         // Create impassable edges at the max-range boundary, so we can't escape the region
 601         // where we're meant to be searching
 602         fixed rangeXMin = x0 - range;
 603         fixed rangeXMax = x0 + range;
 604         fixed rangeZMin = z0 - range;
 605         fixed rangeZMax = z0 + range;
 606
 607         // we don't actually add the "search space" edges as edges, since we may want to cross the
 608         // in some cases (such as if we need to go around an obstruction that's partly out of the search range)
 609
 610         // List of obstruction vertexes (plus start/end points); we'll try to find paths through
 611         // the graph defined by these vertexes
 612         std::vector<Vertex> vertexes;
 613
 614         // Add the start point to the graph
 615         CFixedVector2D posStart(x0, z0);
 616         fixed hStart = (posStart - goal.NearestPointOnGoal(posStart)).Length();
 617         Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL };
 618         vertexes.push_back(start);
 619         const size_t START_VERTEX_ID = 0;
 620
 621         // Add the goal vertex to the graph.
 622         // Since the goal isn't always a point, this a special magic virtual vertex which moves around - whenever
 623         // we look at it from another vertex, it is moved to be the closest point on the goal shape to that vertex.
 624         Vertex end = { CFixedVector2D(goal.x, goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL };
 625         vertexes.push_back(end);
 626         const size_t GOAL_VERTEX_ID = 1;
 627
 628         // Add terrain obstructions
 629         {
 630                 u16 i0, j0, i1, j1;
 631                 Pathfinding::NearestNavcell(rangeXMin, rangeZMin, i0, j0, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE);
 632                 Pathfinding::NearestNavcell(rangeXMax, rangeZMax, i1, j1, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE);
 633                 AddTerrainEdges(edges, vertexes, i0, j0, i1, j1, passClass, *m_TerrainOnlyGrid);
 634         }
 635
 636         // Find all the obstruction squares that might affect us
 637         CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSystemEntity());
 638         std::vector<ICmpObstructionManager::ObstructionSquare> squares;
 639         size_t staticShapesNb = 0;
 640         cmpObstructionManager->GetStaticObstructionsInRange(filter, rangeXMin - clearance, rangeZMin - clearance, rangeXMax + clearance, rangeZMax + clearance, squares);
 641         staticShapesNb = squares.size();
 642         cmpObstructionManager->GetUnitObstructionsInRange(filter, rangeXMin - clearance, rangeZMin - clearance, rangeXMax + clearance, rangeZMax + clearance, squares);
 643
 644         // Change array capacities to reduce reallocations
 645         vertexes.reserve(vertexes.size() + squares.size()*4);
 646         edgeSquares.reserve(edgeSquares.size() + squares.size()); // (assume most squares are AA)
 647
 648         entity_pos_t pathfindClearance = clearance;
 649
 650         // Convert each obstruction square into collision edges and search graph vertexes
 651         for (size_t i = 0; i < squares.size(); ++i)
 652         {
 653                 CFixedVector2D center(squares[i].x, squares[i].z);
 654                 CFixedVector2D u = squares[i].u;
 655                 CFixedVector2D v = squares[i].v;
 656
 657                 if (i >= staticShapesNb)
 658                         pathfindClearance = clearance - entity_pos_t::FromInt(1)/2;
 659
 660                 // Expand the vertexes by the moving unit's collision radius, to find the
 661                 // closest we can get to it
 662
 663                 CFixedVector2D hd0(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA,   squares[i].hh + pathfindClearance + EDGE_EXPAND_DELTA);
 664                 CFixedVector2D hd1(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA, -(squares[i].hh + pathfindClearance + EDGE_EXPAND_DELTA));
 665
 666                 // Check whether this is an axis-aligned square
 667                 bool aa = (u.X == fixed::FromInt(1) && u.Y == fixed::Zero() && v.X == fixed::Zero() && v.Y == fixed::FromInt(1));
 668
 669                 Vertex vert;
 670                 vert.status = Vertex::UNEXPLORED;
 671                 vert.quadInward = QUADRANT_NONE;
 672                 vert.quadOutward = QUADRANT_ALL;
 673                 vert.p.X = center.X - hd0.Dot(u); vert.p.Y = center.Y + hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_BR; vertexes.push_back(vert);
 674                 vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_TR; vertexes.push_back(vert);
 675                 vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_TL; vertexes.push_back(vert);
 676                 vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; vertexes.push_back(vert);
 677
 678                 // Add the edges:
 679
 680                 CFixedVector2D h0(squares[i].hw + pathfindClearance, squares[i].hh + pathfindClearance);
 681                 CFixedVector2D h1(squares[i].hw + pathfindClearance, -(squares[i].hh + pathfindClearance));
 682
 683                 CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v));
 684                 CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v));
 685                 CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v));
 686                 CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v));
 687                 if (aa)
 688                         edgeSquares.emplace_back(Square{ ev1, ev3 });
 689                 else
 690                 {
 691                         edges.emplace_back(Edge{ ev0, ev1 });
 692                         edges.emplace_back(Edge{ ev1, ev2 });
 693                         edges.emplace_back(Edge{ ev2, ev3 });
 694                         edges.emplace_back(Edge{ ev3, ev0 });
 695                 }
 696
 697                 // TODO: should clip out vertexes and edges that are outside the range,
 698                 // to reduce the search space
 699         }
 700
 701         ENSURE(vertexes.size() < 65536); // we store array indexes as u16
 702
 703         // Render the debug overlay
 704         if (m_DebugOverlay)
 705         {
 706 #define PUSH_POINT(p) STMT(xz.push_back(p.X.ToFloat()); xz.push_back(p.Y.ToFloat()))
 707                 // Render the vertexes as little Pac-Man shapes to indicate quadrant direction
 708                 for (size_t i = 0; i < vertexes.size(); ++i)
 709                 {
 710                         m_DebugOverlayShortPathLines.emplace_back();
 711                         m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1);
 712
 713                         float x = vertexes[i].p.X.ToFloat();
 714                         float z = vertexes[i].p.Y.ToFloat();
 715
 716                         float a0 = 0, a1 = 0;
 717                         // Get arc start/end angles depending on quadrant (if any)
 718                         if      (vertexes[i].quadInward == QUADRANT_BL) { a0 = -0.25f; a1 = 0.50f; }
 719                         else if (vertexes[i].quadInward == QUADRANT_TR) { a0 =  0.25f; a1 = 1.00f; }
 720                         else if (vertexes[i].quadInward == QUADRANT_TL) { a0 = -0.50f; a1 = 0.25f; }
 721                         else if (vertexes[i].quadInward == QUADRANT_BR) { a0 =  0.00f; a1 = 0.75f; }
 722
 723                         if (a0 == a1)
 724                                 SimRender::ConstructCircleOnGround(GetSimContext(), x, z, 0.5f,
 725                                         m_DebugOverlayShortPathLines.back(), true);
 726                         else
 727                                 SimRender::ConstructClosedArcOnGround(GetSimContext(), x, z, 0.5f,
 728                                         a0 * ((float)M_PI*2.0f), a1 * ((float)M_PI*2.0f),
 729                                         m_DebugOverlayShortPathLines.back(), true);
 730                 }
 731
 732                 // Render the edges
 733                 for (size_t i = 0; i < edges.size(); ++i)
 734                 {
 735                         m_DebugOverlayShortPathLines.emplace_back();
 736                         m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
 737                         std::vector<float> xz;
 738                         PUSH_POINT(edges[i].p0);
 739                         PUSH_POINT(edges[i].p1);
 740
 741                         // Add an arrowhead to indicate the direction
 742                         CFixedVector2D d = edges[i].p1 - edges[i].p0;
 743                         d.Normalize(fixed::FromInt(1)/8);
 744                         CFixedVector2D p2 = edges[i].p1 - d*2;
 745                         CFixedVector2D p3 = p2 + d.Perpendicular();
 746                         CFixedVector2D p4 = p2 - d.Perpendicular();
 747                         PUSH_POINT(p3);
 748                         PUSH_POINT(p4);
 749                         PUSH_POINT(edges[i].p1);
 750
 751                         SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
 752                 }
 753 #undef PUSH_POINT
 754
 755                 // Render the axis-aligned squares
 756                 for (size_t i = 0; i < edgeSquares.size(); ++i)
 757                 {
 758                         m_DebugOverlayShortPathLines.push_back(SOverlayLine());
 759                         m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
 760                         std::vector<float> xz;
 761                         Square s = edgeSquares[i];
 762                         xz.push_back(s.p0.X.ToFloat());
 763                         xz.push_back(s.p0.Y.ToFloat());
 764                         xz.push_back(s.p0.X.ToFloat());
 765                         xz.push_back(s.p1.Y.ToFloat());
 766                         xz.push_back(s.p1.X.ToFloat());
 767                         xz.push_back(s.p1.Y.ToFloat());
 768                         xz.push_back(s.p1.X.ToFloat());
 769                         xz.push_back(s.p0.Y.ToFloat());
 770                         xz.push_back(s.p0.X.ToFloat());
 771                         xz.push_back(s.p0.Y.ToFloat());
 772                         SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
 773                 }
 774         }
 775
 776         // Do an A* search over the vertex/visibility graph:
 777
 778         // Since we are just measuring Euclidean distance the heuristic is admissible,
 779         // so we never have to re-examine a node once it's been moved to the closed set.
 780
 781         // To save time in common cases, we don't precompute a graph of valid edges between vertexes;
 782         // we do it lazily instead. When the search algorithm reaches a vertex, we examine every other
 783         // vertex and see if we can reach it without hitting any collision edges, and ignore the ones
 784         // we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked
 785         // as closed), we won't be doing any redundant visibility computations.
 786
 787         PROFILE_START("Short pathfinding - A*");
 788
 789         VertexPriorityQueue open;
 790         VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h };
 791         open.push(qiStart);
 792
 793         u16 idBest = START_VERTEX_ID;
 794         fixed hBest = start.h;
 795
 796         while (!open.empty())
 797         {
 798                 // Move best tile from open to closed
 799                 VertexPriorityQueue::Item curr = open.pop();
 800                 vertexes[curr.id].status = Vertex::CLOSED;
 801
 802                 // If we've reached the destination, stop
 803                 if (curr.id == GOAL_VERTEX_ID)
 804                 {
 805                         idBest = curr.id;
 806                         break;
 807                 }
 808
 809                 // Sort the edges so ones nearer this vertex are checked first by CheckVisibility,
 810                 // since they're more likely to block the rays
 811                 std::sort(edgeSquares.begin(), edgeSquares.end(), SquareSort(vertexes[curr.id].p));
 812
 813                 std::vector<Edge> edgesUnaligned;
 814                 std::vector<EdgeAA> edgesLeft;
 815                 std::vector<EdgeAA> edgesRight;
 816                 std::vector<EdgeAA> edgesBottom;
 817                 std::vector<EdgeAA> edgesTop;
 818                 SplitAAEdges(vertexes[curr.id].p, edges, edgeSquares, edgesUnaligned, edgesLeft, edgesRight, edgesBottom, edgesTop);
 819
 820                 // Check the lines to every other vertex
 821                 for (size_t n = 0; n < vertexes.size(); ++n)
 822                 {
 823                         if (vertexes[n].status == Vertex::CLOSED)
 824                                 continue;
 825
 826                         // If this is the magical goal vertex, move it to near the current vertex
 827                         CFixedVector2D npos;
 828                         if (n == GOAL_VERTEX_ID)
 829                         {
 830                                 npos = goal.NearestPointOnGoal(vertexes[curr.id].p);
 831
 832                                 // To prevent integer overflows later on, we need to ensure all vertexes are
 833                                 // 'close' to the source. The goal might be far away (not a good idea but
 834                                 // sometimes it happens), so clamp it to the current search range
 835                                 npos.X = clamp(npos.X, rangeXMin, rangeXMax);
 836                                 npos.Y = clamp(npos.Y, rangeZMin, rangeZMax);
 837                         }
 838                         else
 839                         {
 840                                 npos = vertexes[n].p;
 841                         }
 842
 843                         // Work out which quadrant(s) we're approaching the new vertex from
 844                         u8 quad = 0;
 845                         if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL;
 846                         if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR;
 847                         if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL;
 848                         if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BR;
 849
 850                         // Check that the new vertex is in the right quadrant for the old vertex
 851                         if (!(vertexes[curr.id].quadOutward & quad))
 852                         {
 853                                 // Hack: Always head towards the goal if possible, to avoid missing it if it's
 854                                 // inside another unit
 855                                 if (n != GOAL_VERTEX_ID)
 856                                 {
 857                                         continue;
 858                                 }
 859                         }
 860
 861                         bool visible =
 862                                 CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) &&
 863                                 CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) &&
 864                                 CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) &&
 865                                 CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) &&
 866                                 CheckVisibility(vertexes[curr.id].p, npos, edgesUnaligned);
 867
 868                         /*
 869                         // Render the edges that we examine
 870                         m_DebugOverlayShortPathLines.push_back(SOverlayLine());
 871                         m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5);
 872                         std::vector<float> xz;
 873                         xz.push_back(vertexes[curr.id].p.X.ToFloat());
 874                         xz.push_back(vertexes[curr.id].p.Y.ToFloat());
 875                         xz.push_back(npos.X.ToFloat());
 876                         xz.push_back(npos.Y.ToFloat());
 877                         SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false);
 878                         //*/
 879
 880                         if (visible)
 881                         {
 882                                 fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length();
 883
 884                                 // If this is a new tile, compute the heuristic distance
 885                                 if (vertexes[n].status == Vertex::UNEXPLORED)
 886                                 {
 887                                         // Add it to the open list:
 888                                         vertexes[n].status = Vertex::OPEN;
 889                                         vertexes[n].g = g;
 890                                         vertexes[n].h = goal.DistanceToPoint(npos);
 891                                         vertexes[n].pred = curr.id;
 892
 893                                         // If this is an axis-aligned shape, the path must continue in the same quadrant
 894                                         // direction (but not go into the inside of the shape).
 895                                         // Hack: If we started *inside* a shape then perhaps headed to its corner (e.g. the unit
 896                                         // was very near another unit), don't restrict further pathing.
 897                                         if (vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8)))
 898                                                 vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
 899
 900                                         if (n == GOAL_VERTEX_ID)
 901                                                 vertexes[n].p = npos; // remember the new best goal position
 902
 903                                         VertexPriorityQueue::Item t = { (u16)n, g + vertexes[n].h, vertexes[n].h };
 904                                         open.push(t);
 905
 906                                         // Remember the heuristically best vertex we've seen so far, in case we never actually reach the target
 907                                         if (vertexes[n].h < hBest)
 908                                         {
 909                                                 idBest = (u16)n;
 910                                                 hBest = vertexes[n].h;
 911                                         }
 912                                 }
 913                                 else // must be OPEN
 914                                 {
 915                                         // If we've already seen this tile, and the new path to this tile does not have a
 916                                         // better cost, then stop now
 917                                         if (g >= vertexes[n].g)
 918                                                 continue;
 919
 920                                         // Otherwise, we have a better path, so replace the old one with the new cost/parent
 921                                         fixed gprev = vertexes[n].g;
 922                                         vertexes[n].g = g;
 923                                         vertexes[n].pred = curr.id;
 924
 925                                         // If this is an axis-aligned shape, the path must continue in the same quadrant
 926                                         // direction (but not go into the inside of the shape).
 927                                         if (vertexes[n].quadInward)
 928                                                 vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
 929
 930                                         if (n == GOAL_VERTEX_ID)
 931                                                 vertexes[n].p = npos; // remember the new best goal position
 932
 933                                         open.promote((u16)n, gprev + vertexes[n].h, g + vertexes[n].h, vertexes[n].h);
 934                                 }
 935                         }
 936                 }
 937         }
 938
 939         // Reconstruct the path (in reverse)
 940         for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred)
 941                 path.m_Waypoints.emplace_back(Waypoint{ vertexes[id].p.X, vertexes[id].p.Y });
 942
 943         PROFILE_END("Short pathfinding - A*");
 944 }