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
NavcellContainsCircle(int i,int j,fixed x,fixed z,fixed r,bool inside)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
NavcellContainsSquare(int i,int j,fixed x,fixed z,CFixedVector2D u,CFixedVector2D v,fixed hw,fixed hh,bool inside)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
NavcellContainsGoal(int i,int j) const95 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
NavcellRectContainsGoal(int i0,int j0,int i1,int j1,int * gi,int * gj) const118 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
RectContainsGoal(entity_pos_t x0,entity_pos_t z0,entity_pos_t x1,entity_pos_t z1) const249 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
DistanceToPoint(CFixedVector2D pos) const294 fixed PathGoal::DistanceToPoint(CFixedVector2D pos) const
295 {
296 CFixedVector2D d(pos.X - x, pos.Y - z);
297
298 switch (type)
299 {
300 case POINT:
301 return d.Length();
302
303 case CIRCLE:
304 return d.CompareLength(hw) <= 0 ? fixed::Zero() : d.Length() - hw;
305
306 case INVERTED_CIRCLE:
307 return d.CompareLength(hw) >= 0 ? fixed::Zero() : hw - d.Length();
308
309 case SQUARE:
310 {
311 CFixedVector2D halfSize(hw, hh);
312 return Geometry::PointIsInSquare(d, u, v, halfSize) ?
313 fixed::Zero() : Geometry::DistanceToSquare(d, u, v, halfSize);
314 }
315
316 case INVERTED_SQUARE:
317 {
318 CFixedVector2D halfSize(hw, hh);
319 return !Geometry::PointIsInSquare(d, u, v, halfSize) ?
320 fixed::Zero() : Geometry::DistanceToSquare(d, u, v, halfSize);
321 }
322
323 NODEFAULT;
324 }
325 }
326
NearestPointOnGoal(CFixedVector2D pos) const327 CFixedVector2D PathGoal::NearestPointOnGoal(CFixedVector2D pos) const
328 {
329 CFixedVector2D g(x, z);
330
331 switch (type)
332 {
333 case POINT:
334 return g;
335
336 case CIRCLE:
337 {
338 CFixedVector2D d(pos.X - x, pos.Y - z);
339 if (d.CompareLength(hw) <= 0)
340 return pos;
341
342 d.Normalize(hw);
343 return g + d;
344 }
345
346 case INVERTED_CIRCLE:
347 {
348 CFixedVector2D d(pos.X - x, pos.Y - z);
349 if (d.CompareLength(hw) >= 0)
350 return pos;
351
352 if (d.IsZero())
353 d = CFixedVector2D(fixed::FromInt(1), fixed::Zero()); // some arbitrary direction
354 d.Normalize(hw);
355 return g + d;
356 }
357
358 case SQUARE:
359 {
360 CFixedVector2D halfSize(hw, hh);
361 CFixedVector2D d(pos.X - x, pos.Y - z);
362 return Geometry::PointIsInSquare(d, u, v, halfSize) ?
363 pos : g + Geometry::NearestPointOnSquare(d, u, v, halfSize);
364 }
365
366 case INVERTED_SQUARE:
367 {
368 CFixedVector2D halfSize(hw, hh);
369 CFixedVector2D d(pos.X - x, pos.Y - z);
370 return !Geometry::PointIsInSquare(d, u, v, halfSize) ?
371 pos : g + Geometry::NearestPointOnSquare(d, u, v, halfSize);
372 }
373
374 NODEFAULT;
375 }
376 }
377