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 : }
|
