source/simulation2/helpers/PathGoal.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 #include "precompiled.h"
  19
  20 #include "PathGoal.h"
  21
  22 #include "graphics/Terrain.h"
  23 #include "Pathfinding.h"
  24
  25 #include "Geometry.h"
  26
  27 static bool NavcellContainsCircle(int i, int j, fixed x, fixed z, fixed r, bool inside)
  28 {
  29         // Accept any navcell (i,j) that contains a point which is inside[/outside]
  30         // (or on the edge of) the circle
  31
  32         // Get world-space bounds of navcell
  33         entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE);
  34         entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE);
  35         entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE;
  36         entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE;
  37
  38         if (inside)
  39         {
  40                 // Get the point inside the navcell closest to (x,z)
  41                 entity_pos_t nx = Clamp(x, x0, x1);
  42                 entity_pos_t nz = Clamp(z, z0, z1);
  43                 // Check if that point is inside the circle
  44                 return (CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(r) <= 0;
  45         }
  46         else
  47         {
  48                 // If any corner of the navcell is outside the circle, return true.
  49                 // Otherwise, since the circle is convex, there cannot be any other point
  50                 // in the navcell that is outside the circle.
  51                 return (
  52                     (CFixedVector2D(x0, z0) - CFixedVector2D(x, z)).CompareLength(r) >= 0
  53                  || (CFixedVector2D(x1, z0) - CFixedVector2D(x, z)).CompareLength(r) >= 0
  54                  || (CFixedVector2D(x0, z1) - CFixedVector2D(x, z)).CompareLength(r) >= 0
  55                  || (CFixedVector2D(x1, z1) - CFixedVector2D(x, z)).CompareLength(r) >= 0
  56                 );
  57         }
  58 }
  59
  60 static bool NavcellContainsSquare(int i, int j,
  61         fixed x, fixed z, CFixedVector2D u, CFixedVector2D v, fixed hw, fixed hh,
  62         bool inside)
  63 {
  64         // Accept any navcell (i,j) that contains a point which is inside[/outside]
  65         // (or on the edge of) the square
  66
  67         // Get world-space bounds of navcell
  68         entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE);
  69         entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE);
  70         entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE;
  71         entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE;
  72
  73         if (inside)
  74         {
  75                 // Get the point inside the navcell closest to (x,z)
  76                 entity_pos_t nx = Clamp(x, x0, x1);
  77                 entity_pos_t nz = Clamp(z, z0, z1);
  78                 // Check if that point is inside the circle
  79                 return Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh));
  80         }
  81         else
  82         {
  83                 // If any corner of the navcell is outside the square, return true.
  84                 // Otherwise, since the square is convex, there cannot be any other point
  85                 // in the navcell that is outside the square.
  86                 return (
  87                     !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z0 - z), u, v, CFixedVector2D(hw, hh))
  88                  || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z0 - z), u, v, CFixedVector2D(hw, hh))
  89                  || !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z1 - z), u, v, CFixedVector2D(hw, hh))
  90                  || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z1 - z), u, v, CFixedVector2D(hw, hh))
  91                 );
  92         }
  93 }
  94
  95 bool PathGoal::NavcellContainsGoal(int i, int j) const
  96 {
  97         switch (type)
  98         {
  99         case POINT:
 100         {
 101                 // Only accept a single navcell
 102                 int gi = (x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
 103                 int gj = (z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
 104                 return gi == i && gj == j;
 105         }
 106         case CIRCLE:
 107                 return NavcellContainsCircle(i, j, x, z, hw, true);
 108         case INVERTED_CIRCLE:
 109                 return NavcellContainsCircle(i, j, x, z, hw, false);
 110         case SQUARE:
 111                 return NavcellContainsSquare(i, j, x, z, u, v, hw, hh, true);
 112         case INVERTED_SQUARE:
 113                 return NavcellContainsSquare(i, j, x, z, u, v, hw, hh, false);
 114         NODEFAULT;
 115         }
 116 }
 117
 118 bool PathGoal::NavcellRectContainsGoal(int i0, int j0, int i1, int j1, int* gi, int* gj) const
 119 {
 120         // Get min/max to simplify range checks
 121         int imin = std::min(i0, i1);
 122         int imax = std::max(i0, i1);
 123         int jmin = std::min(j0, j1);
 124         int jmax = std::max(j0, j1);
 125
 126         // Direction to iterate from (i0,j0) towards (i1,j1)
 127         int di = i1 < i0 ? -1 : +1;
 128         int dj = j1 < j0 ? -1 : +1;
 129
 130         switch (type)
 131         {
 132         case POINT:
 133         {
 134                 // Calculate the navcell that contains the point goal
 135                 int i = (x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
 136                 int j = (z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
 137                 // If that goal navcell is in the given range, return it
 138                 if (imin <= i && i <= imax && jmin <= j && j <= jmax)
 139                 {
 140                         if (gi)
 141                                 *gi = i;
 142                         if (gj)
 143                                 *gj = j;
 144                         return true;
 145                 }
 146                 return false;
 147         }
 148
 149         case CIRCLE:
 150         {
 151                 // Loop over all navcells in the given range (starting at (i0,j0) since
 152                 // this function is meant to find the goal navcell nearest to there
 153                 // assuming jmin==jmax || imin==imax),
 154                 // and check whether any point in each navcell is within the goal circle.
 155                 // (TODO: this is pretty inefficient.)
 156                 for (int j = j0; jmin <= j && j <= jmax; j += dj)
 157                 {
 158                         for (int i = i0; imin <= i && i <= imax; i += di)
 159                         {
 160                                 if (NavcellContainsCircle(i, j, x, z, hw, true))
 161                                 {
 162                                         if (gi)
 163                                                 *gi = i;
 164                                         if (gj)
 165                                                 *gj = j;
 166                                         return true;
 167                                 }
 168                         }
 169                 }
 170                 return false;
 171         }
 172
 173         case INVERTED_CIRCLE:
 174         {
 175                 // Loop over all navcells in the given range (starting at (i0,j0) since
 176                 // this function is meant to find the goal navcell nearest to there
 177                 // assuming jmin==jmax || imin==imax),
 178                 // and check whether any point in each navcell is outside the goal circle.
 179                 // (TODO: this is pretty inefficient.)
 180                 for (int j = j0; jmin <= j && j <= jmax; j += dj)
 181                 {
 182                         for (int i = i0; imin <= i && i <= imax; i += di)
 183                         {
 184                                 if (NavcellContainsCircle(i, j, x, z, hw, false))
 185                                 {
 186                                         if (gi)
 187                                                 *gi = i;
 188                                         if (gj)
 189                                                 *gj = j;
 190                                         return true;
 191                                 }
 192                         }
 193                 }
 194                 return false;
 195         }
 196
 197         case SQUARE:
 198         {
 199                 // Loop over all navcells in the given range (starting at (i0,j0) since
 200                 // this function is meant to find the goal navcell nearest to there
 201                 // assuming jmin==jmax || imin==imax),
 202                 // and check whether any point in each navcell is within the goal square.
 203                 // (TODO: this is pretty inefficient.)
 204                 for (int j = j0; jmin <= j && j <= jmax; j += dj)
 205                 {
 206                         for (int i = i0; imin <= i && i <= imax; i += di)
 207                         {
 208                                 if (NavcellContainsSquare(i, j, x, z, u, v, hw, hh, true))
 209                                 {
 210                                         if (gi)
 211                                                 *gi = i;
 212                                         if (gj)
 213                                                 *gj = j;
 214                                         return true;
 215                                 }
 216                         }
 217                 }
 218                 return false;
 219         }
 220
 221         case INVERTED_SQUARE:
 222         {
 223                 // Loop over all navcells in the given range (starting at (i0,j0) since
 224                 // this function is meant to find the goal navcell nearest to there
 225                 // assuming jmin==jmax || imin==imax),
 226                 // and check whether any point in each navcell is outside the goal square.
 227                 // (TODO: this is pretty inefficient.)
 228                 for (int j = j0; jmin <= j && j <= jmax; j += dj)
 229                 {
 230                         for (int i = i0; imin <= i && i <= imax; i += di)
 231                         {
 232                                 if (NavcellContainsSquare(i, j, x, z, u, v, hw, hh, false))
 233                                 {
 234                                         if (gi)
 235                                                 *gi = i;
 236                                         if (gj)
 237                                                 *gj = j;
 238                                         return true;
 239                                 }
 240                         }
 241                 }
 242                 return false;
 243         }
 244
 245         NODEFAULT;
 246         }
 247 }
 248
 249 bool PathGoal::RectContainsGoal(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1) const
 250 {
 251         switch (type)
 252         {
 253         case POINT:
 254                 return x0 <= x && x <= x1 && z0 <= z && z <= z1;
 255
 256         case CIRCLE:
 257         {
 258                 entity_pos_t nx = Clamp(x, x0, x1);
 259                 entity_pos_t nz = Clamp(z, z0, z1);
 260                 return (CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(hw) <= 0;
 261         }
 262
 263         case INVERTED_CIRCLE:
 264         {
 265                 return (
 266                     (CFixedVector2D(x0, z0) - CFixedVector2D(x, z)).CompareLength(hw) >= 0
 267                  || (CFixedVector2D(x1, z0) - CFixedVector2D(x, z)).CompareLength(hw) >= 0
 268                  || (CFixedVector2D(x0, z1) - CFixedVector2D(x, z)).CompareLength(hw) >= 0
 269                  || (CFixedVector2D(x1, z1) - CFixedVector2D(x, z)).CompareLength(hw) >= 0
 270                 );
 271         }
 272
 273         case SQUARE:
 274         {
 275                 entity_pos_t nx = Clamp(x, x0, x1);
 276                 entity_pos_t nz = Clamp(z, z0, z1);
 277                 return Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh));
 278         }
 279
 280         case INVERTED_SQUARE:
 281         {
 282                 return (
 283                     !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z0 - z), u, v, CFixedVector2D(hw, hh))
 284                  || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z0 - z), u, v, CFixedVector2D(hw, hh))
 285                  || !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z1 - z), u, v, CFixedVector2D(hw, hh))
 286                  || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z1 - z), u, v, CFixedVector2D(hw, hh))
 287                 );
 288         }
 289
 290         NODEFAULT;
 291         }
 292 }
 293
 294 fixed PathGoal::DistanceToPoint(CFixedVector2D pos) const
 295 {
 296         switch (type)
 297         {
 298         case POINT:
 299                 return (pos - CFixedVector2D(x, z)).Length();
 300
 301         case CIRCLE:
 302         case INVERTED_CIRCLE:
 303                 return ((pos - CFixedVector2D(x, z)).Length() - hw).Absolute();
 304
 305         case SQUARE:
 306         case INVERTED_SQUARE:
 307         {
 308                 CFixedVector2D halfSize(hw, hh);
 309                 CFixedVector2D d(pos.X - x, pos.Y - z);
 310                 return Geometry::DistanceToSquare(d, u, v, halfSize);
 311         }
 312
 313         NODEFAULT;
 314         }
 315 }
 316
 317 CFixedVector2D PathGoal::NearestPointOnGoal(CFixedVector2D pos) const
 318 {
 319         CFixedVector2D g(x, z);
 320
 321         switch (type)
 322         {
 323         case POINT:
 324                 return g;
 325
 326         case CIRCLE:
 327         case INVERTED_CIRCLE:
 328         {
 329                 CFixedVector2D d = pos - g;
 330                 if (d.IsZero())
 331                         d = CFixedVector2D(fixed::FromInt(1), fixed::Zero()); // some arbitrary direction
 332                 d.Normalize(hw);
 333                 return g + d;
 334         }
 335
 336         case SQUARE:
 337         case INVERTED_SQUARE:
 338         {
 339                 CFixedVector2D halfSize(hw, hh);
 340                 CFixedVector2D d = pos - g;
 341                 return g + Geometry::NearestPointOnSquare(d, u, v, halfSize);
 342         }
 343
 344         NODEFAULT;
 345         }
 346 }