LCOV - code coverage report
Current view: top level - source/simulation2/helpers - PathGoal.cpp (source / functions) Hit Total Coverage
Test: 0 A.D. test coverage report Lines: 82 175 46.9 %
Date: 2023-01-19 00:18:29 Functions: 5 7 71.4 %

          Line data    Source code
       1             : /* Copyright (C) 2018 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         685 : 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         685 :     entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE);
      34         685 :     entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE);
      35         685 :     entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE;
      36         685 :     entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE;
      37             : 
      38         685 :     if (inside)
      39             :     {
      40             :         // Get the point inside the navcell closest to (x,z)
      41         685 :         entity_pos_t nx = Clamp(x, x0, x1);
      42         685 :         entity_pos_t nz = Clamp(z, z0, z1);
      43             :         // Check if that point is inside the circle
      44         685 :         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           0 :             (CFixedVector2D(x0, z0) - CFixedVector2D(x, z)).CompareLength(r) >= 0
      53           0 :          || (CFixedVector2D(x1, z0) - CFixedVector2D(x, z)).CompareLength(r) >= 0
      54           0 :          || (CFixedVector2D(x0, z1) - CFixedVector2D(x, z)).CompareLength(r) >= 0
      55           0 :          || (CFixedVector2D(x1, z1) - CFixedVector2D(x, z)).CompareLength(r) >= 0
      56           0 :         );
      57             :     }
      58             : }
      59             : 
      60           0 : 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           0 :     entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE);
      69           0 :     entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE);
      70           0 :     entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE;
      71           0 :     entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE;
      72             : 
      73           0 :     if (inside)
      74             :     {
      75             :         // Get the point inside the navcell closest to (x,z)
      76           0 :         entity_pos_t nx = Clamp(x, x0, x1);
      77           0 :         entity_pos_t nz = Clamp(z, z0, z1);
      78             :         // Check if that point is inside the circle
      79           0 :         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           0 :             !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z0 - z), u, v, CFixedVector2D(hw, hh))
      88           0 :          || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z0 - z), u, v, CFixedVector2D(hw, hh))
      89           0 :          || !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z1 - z), u, v, CFixedVector2D(hw, hh))
      90           0 :          || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z1 - z), u, v, CFixedVector2D(hw, hh))
      91           0 :         );
      92             :     }
      93             : }
      94             : 
      95         685 : bool PathGoal::NavcellContainsGoal(int i, int j) const
      96             : {
      97         685 :     switch (type)
      98             :     {
      99           0 :     case POINT:
     100             :     {
     101             :         // Only accept a single navcell
     102           0 :         int gi = (x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
     103           0 :         int gj = (z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
     104           0 :         return gi == i && gj == j;
     105             :     }
     106         685 :     case CIRCLE:
     107         685 :         return NavcellContainsCircle(i, j, x, z, hw, true);
     108           0 :     case INVERTED_CIRCLE:
     109           0 :         return NavcellContainsCircle(i, j, x, z, hw, false);
     110           0 :     case SQUARE:
     111           0 :         return NavcellContainsSquare(i, j, x, z, u, v, hw, hh, true);
     112           0 :     case INVERTED_SQUARE:
     113           0 :         return NavcellContainsSquare(i, j, x, z, u, v, hw, hh, false);
     114           0 :     NODEFAULT;
     115             :     }
     116             : }
     117             : 
     118           0 : 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           0 :     int imin = std::min(i0, i1);
     122           0 :     int imax = std::max(i0, i1);
     123           0 :     int jmin = std::min(j0, j1);
     124           0 :     int jmax = std::max(j0, j1);
     125             : 
     126             :     // Direction to iterate from (i0,j0) towards (i1,j1)
     127           0 :     int di = i1 < i0 ? -1 : +1;
     128           0 :     int dj = j1 < j0 ? -1 : +1;
     129             : 
     130           0 :     switch (type)
     131             :     {
     132           0 :     case POINT:
     133             :     {
     134             :         // Calculate the navcell that contains the point goal
     135           0 :         int i = (x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
     136           0 :         int j = (z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
     137             :         // If that goal navcell is in the given range, return it
     138           0 :         if (imin <= i && i <= imax && jmin <= j && j <= jmax)
     139             :         {
     140           0 :             if (gi)
     141           0 :                 *gi = i;
     142           0 :             if (gj)
     143           0 :                 *gj = j;
     144           0 :             return true;
     145             :         }
     146           0 :         return false;
     147             :     }
     148             : 
     149           0 :     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           0 :         for (int j = j0; jmin <= j && j <= jmax; j += dj)
     157             :         {
     158           0 :             for (int i = i0; imin <= i && i <= imax; i += di)
     159             :             {
     160           0 :                 if (NavcellContainsCircle(i, j, x, z, hw, true))
     161             :                 {
     162           0 :                     if (gi)
     163           0 :                         *gi = i;
     164           0 :                     if (gj)
     165           0 :                         *gj = j;
     166           0 :                     return true;
     167             :                 }
     168             :             }
     169             :         }
     170           0 :         return false;
     171             :     }
     172             : 
     173           0 :     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           0 :         for (int j = j0; jmin <= j && j <= jmax; j += dj)
     181             :         {
     182           0 :             for (int i = i0; imin <= i && i <= imax; i += di)
     183             :             {
     184           0 :                 if (NavcellContainsCircle(i, j, x, z, hw, false))
     185             :                 {
     186           0 :                     if (gi)
     187           0 :                         *gi = i;
     188           0 :                     if (gj)
     189           0 :                         *gj = j;
     190           0 :                     return true;
     191             :                 }
     192             :             }
     193             :         }
     194           0 :         return false;
     195             :     }
     196             : 
     197           0 :     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           0 :         for (int j = j0; jmin <= j && j <= jmax; j += dj)
     205             :         {
     206           0 :             for (int i = i0; imin <= i && i <= imax; i += di)
     207             :             {
     208           0 :                 if (NavcellContainsSquare(i, j, x, z, u, v, hw, hh, true))
     209             :                 {
     210           0 :                     if (gi)
     211           0 :                         *gi = i;
     212           0 :                     if (gj)
     213           0 :                         *gj = j;
     214           0 :                     return true;
     215             :                 }
     216             :             }
     217             :         }
     218           0 :         return false;
     219             :     }
     220             : 
     221           0 :     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           0 :         for (int j = j0; jmin <= j && j <= jmax; j += dj)
     229             :         {
     230           0 :             for (int i = i0; imin <= i && i <= imax; i += di)
     231             :             {
     232           0 :                 if (NavcellContainsSquare(i, j, x, z, u, v, hw, hh, false))
     233             :                 {
     234           0 :                     if (gi)
     235           0 :                         *gi = i;
     236           0 :                     if (gj)
     237           0 :                         *gj = j;
     238           0 :                     return true;
     239             :                 }
     240             :             }
     241             :         }
     242           0 :         return false;
     243             :     }
     244             : 
     245           0 :     NODEFAULT;
     246             :     }
     247             : }
     248             : 
     249          23 : bool PathGoal::RectContainsGoal(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1) const
     250             : {
     251          23 :     switch (type)
     252             :     {
     253           5 :     case POINT:
     254           5 :         return x0 <= x && x <= x1 && z0 <= z && z <= z1;
     255             : 
     256           4 :     case CIRCLE:
     257             :     {
     258           4 :         entity_pos_t nx = Clamp(x, x0, x1);
     259           4 :         entity_pos_t nz = Clamp(z, z0, z1);
     260           4 :         return (CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(hw) <= 0;
     261             :     }
     262             : 
     263           4 :     case INVERTED_CIRCLE:
     264             :     {
     265             :         return (
     266           8 :             (CFixedVector2D(x0, z0) - CFixedVector2D(x, z)).CompareLength(hw) >= 0
     267           5 :          || (CFixedVector2D(x1, z0) - CFixedVector2D(x, z)).CompareLength(hw) >= 0
     268           5 :          || (CFixedVector2D(x0, z1) - CFixedVector2D(x, z)).CompareLength(hw) >= 0
     269           9 :          || (CFixedVector2D(x1, z1) - CFixedVector2D(x, z)).CompareLength(hw) >= 0
     270           4 :         );
     271             :     }
     272             : 
     273           5 :     case SQUARE:
     274             :     {
     275           5 :         entity_pos_t nx = Clamp(x, x0, x1);
     276           5 :         entity_pos_t nz = Clamp(z, z0, z1);
     277           5 :         return Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh));
     278             :     }
     279             : 
     280           5 :     case INVERTED_SQUARE:
     281             :     {
     282             :         return (
     283          10 :             !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z0 - z), u, v, CFixedVector2D(hw, hh))
     284           7 :          || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z0 - z), u, v, CFixedVector2D(hw, hh))
     285           7 :          || !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z1 - z), u, v, CFixedVector2D(hw, hh))
     286          12 :          || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z1 - z), u, v, CFixedVector2D(hw, hh))
     287           5 :         );
     288             :     }
     289             : 
     290           0 :     NODEFAULT;
     291             :     }
     292             : }
     293             : 
     294          10 : fixed PathGoal::DistanceToPoint(CFixedVector2D pos) const
     295             : {
     296          10 :     CFixedVector2D d(pos.X - x, pos.Y - z);
     297             : 
     298          10 :     switch (type)
     299             :     {
     300           2 :     case POINT:
     301           2 :         return d.Length();
     302             : 
     303           2 :     case CIRCLE:
     304           2 :         return d.CompareLength(hw) <= 0 ? fixed::Zero() : d.Length() - hw;
     305             : 
     306           2 :     case INVERTED_CIRCLE:
     307           2 :         return d.CompareLength(hw) >= 0 ? fixed::Zero() : hw - d.Length();
     308             : 
     309           2 :     case SQUARE:
     310             :     {
     311           2 :         CFixedVector2D halfSize(hw, hh);
     312           2 :         return Geometry::PointIsInSquare(d, u, v, halfSize) ?
     313           2 :             fixed::Zero() : Geometry::DistanceToSquare(d, u, v, halfSize);
     314             :     }
     315             : 
     316           2 :     case INVERTED_SQUARE:
     317             :     {
     318           2 :         CFixedVector2D halfSize(hw, hh);
     319           2 :         return !Geometry::PointIsInSquare(d, u, v, halfSize) ?
     320           2 :             fixed::Zero() : Geometry::DistanceToSquare(d, u, v, halfSize);
     321             :     }
     322             : 
     323           0 :     NODEFAULT;
     324             :     }
     325             : }
     326             : 
     327          10 : CFixedVector2D PathGoal::NearestPointOnGoal(CFixedVector2D pos) const
     328             : {
     329          10 :     CFixedVector2D g(x, z);
     330             : 
     331          10 :     switch (type)
     332             :     {
     333           2 :     case POINT:
     334           2 :         return g;
     335             : 
     336           2 :     case CIRCLE:
     337             :     {
     338           2 :         CFixedVector2D d(pos.X - x, pos.Y - z);
     339           2 :         if (d.CompareLength(hw) <= 0)
     340           1 :             return pos;
     341             : 
     342           1 :         d.Normalize(hw);
     343           1 :         return g + d;
     344             :     }
     345             : 
     346           2 :     case INVERTED_CIRCLE:
     347             :     {
     348           2 :         CFixedVector2D d(pos.X - x, pos.Y - z);
     349           2 :         if (d.CompareLength(hw) >= 0)
     350           1 :             return pos;
     351             : 
     352           1 :         if (d.IsZero())
     353           0 :             d = CFixedVector2D(fixed::FromInt(1), fixed::Zero()); // some arbitrary direction
     354           1 :         d.Normalize(hw);
     355           1 :         return g + d;
     356             :     }
     357             : 
     358           2 :     case SQUARE:
     359             :     {
     360           2 :         CFixedVector2D halfSize(hw, hh);
     361           2 :         CFixedVector2D d(pos.X - x, pos.Y - z);
     362           2 :         return Geometry::PointIsInSquare(d, u, v, halfSize) ?
     363           2 :             pos : g + Geometry::NearestPointOnSquare(d, u, v, halfSize);
     364             :     }
     365             : 
     366           2 :     case INVERTED_SQUARE:
     367             :     {
     368           2 :         CFixedVector2D halfSize(hw, hh);
     369           2 :         CFixedVector2D d(pos.X - x, pos.Y - z);
     370           2 :         return !Geometry::PointIsInSquare(d, u, v, halfSize) ?
     371           2 :             pos : g + Geometry::NearestPointOnSquare(d, u, v, halfSize);
     372             :     }
     373             : 
     374           0 :     NODEFAULT;
     375             :     }
     376             : }

Generated by: LCOV version 1.13