1/**************************************************************************/
2/* geometry_2d.h */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#ifndef GEOMETRY_2D_H
32#define GEOMETRY_2D_H
33
34#include "core/math/delaunay_2d.h"
35#include "core/math/math_funcs.h"
36#include "core/math/triangulate.h"
37#include "core/math/vector2.h"
38#include "core/math/vector2i.h"
39#include "core/math/vector3.h"
40#include "core/math/vector3i.h"
41#include "core/templates/vector.h"
42
43class Geometry2D {
44public:
45 static real_t get_closest_points_between_segments(const Vector2 &p1, const Vector2 &q1, const Vector2 &p2, const Vector2 &q2, Vector2 &c1, Vector2 &c2) {
46 Vector2 d1 = q1 - p1; // Direction vector of segment S1.
47 Vector2 d2 = q2 - p2; // Direction vector of segment S2.
48 Vector2 r = p1 - p2;
49 real_t a = d1.dot(d1); // Squared length of segment S1, always nonnegative.
50 real_t e = d2.dot(d2); // Squared length of segment S2, always nonnegative.
51 real_t f = d2.dot(r);
52 real_t s, t;
53 // Check if either or both segments degenerate into points.
54 if (a <= (real_t)CMP_EPSILON && e <= (real_t)CMP_EPSILON) {
55 // Both segments degenerate into points.
56 c1 = p1;
57 c2 = p2;
58 return Math::sqrt((c1 - c2).dot(c1 - c2));
59 }
60 if (a <= (real_t)CMP_EPSILON) {
61 // First segment degenerates into a point.
62 s = 0.0;
63 t = f / e; // s = 0 => t = (b*s + f) / e = f / e
64 t = CLAMP(t, 0.0f, 1.0f);
65 } else {
66 real_t c = d1.dot(r);
67 if (e <= (real_t)CMP_EPSILON) {
68 // Second segment degenerates into a point.
69 t = 0.0;
70 s = CLAMP(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
71 } else {
72 // The general nondegenerate case starts here.
73 real_t b = d1.dot(d2);
74 real_t denom = a * e - b * b; // Always nonnegative.
75 // If segments not parallel, compute closest point on L1 to L2 and
76 // clamp to segment S1. Else pick arbitrary s (here 0).
77 if (denom != 0.0f) {
78 s = CLAMP((b * f - c * e) / denom, 0.0f, 1.0f);
79 } else {
80 s = 0.0;
81 }
82 // Compute point on L2 closest to S1(s) using
83 // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
84 t = (b * s + f) / e;
85
86 //If t in [0,1] done. Else clamp t, recompute s for the new value
87 // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
88 // and clamp s to [0, 1].
89 if (t < 0.0f) {
90 t = 0.0;
91 s = CLAMP(-c / a, 0.0f, 1.0f);
92 } else if (t > 1.0f) {
93 t = 1.0;
94 s = CLAMP((b - c) / a, 0.0f, 1.0f);
95 }
96 }
97 }
98 c1 = p1 + d1 * s;
99 c2 = p2 + d2 * t;
100 return Math::sqrt((c1 - c2).dot(c1 - c2));
101 }
102
103 static Vector2 get_closest_point_to_segment(const Vector2 &p_point, const Vector2 *p_segment) {
104 Vector2 p = p_point - p_segment[0];
105 Vector2 n = p_segment[1] - p_segment[0];
106 real_t l2 = n.length_squared();
107 if (l2 < 1e-20f) {
108 return p_segment[0]; // Both points are the same, just give any.
109 }
110
111 real_t d = n.dot(p) / l2;
112
113 if (d <= 0.0f) {
114 return p_segment[0]; // Before first point.
115 } else if (d >= 1.0f) {
116 return p_segment[1]; // After first point.
117 } else {
118 return p_segment[0] + n * d; // Inside.
119 }
120 }
121
122 static bool is_point_in_triangle(const Vector2 &s, const Vector2 &a, const Vector2 &b, const Vector2 &c) {
123 Vector2 an = a - s;
124 Vector2 bn = b - s;
125 Vector2 cn = c - s;
126
127 bool orientation = an.cross(bn) > 0;
128
129 if ((bn.cross(cn) > 0) != orientation) {
130 return false;
131 }
132
133 return (cn.cross(an) > 0) == orientation;
134 }
135
136 static Vector2 get_closest_point_to_segment_uncapped(const Vector2 &p_point, const Vector2 *p_segment) {
137 Vector2 p = p_point - p_segment[0];
138 Vector2 n = p_segment[1] - p_segment[0];
139 real_t l2 = n.length_squared();
140 if (l2 < 1e-20f) {
141 return p_segment[0]; // Both points are the same, just give any.
142 }
143
144 real_t d = n.dot(p) / l2;
145
146 return p_segment[0] + n * d; // Inside.
147 }
148
149// Disable False Positives in MSVC compiler; we correctly check for 0 here to prevent a division by 0.
150// See: https://github.com/godotengine/godot/pull/44274
151#ifdef _MSC_VER
152#pragma warning(disable : 4723)
153#endif
154
155 static bool line_intersects_line(const Vector2 &p_from_a, const Vector2 &p_dir_a, const Vector2 &p_from_b, const Vector2 &p_dir_b, Vector2 &r_result) {
156 // See http://paulbourke.net/geometry/pointlineplane/
157
158 const real_t denom = p_dir_b.y * p_dir_a.x - p_dir_b.x * p_dir_a.y;
159 if (Math::is_zero_approx(denom)) { // Parallel?
160 return false;
161 }
162
163 const Vector2 v = p_from_a - p_from_b;
164 const real_t t = (p_dir_b.x * v.y - p_dir_b.y * v.x) / denom;
165 r_result = p_from_a + t * p_dir_a;
166 return true;
167 }
168
169// Re-enable division by 0 warning
170#ifdef _MSC_VER
171#pragma warning(default : 4723)
172#endif
173
174 static bool segment_intersects_segment(const Vector2 &p_from_a, const Vector2 &p_to_a, const Vector2 &p_from_b, const Vector2 &p_to_b, Vector2 *r_result) {
175 Vector2 B = p_to_a - p_from_a;
176 Vector2 C = p_from_b - p_from_a;
177 Vector2 D = p_to_b - p_from_a;
178
179 real_t ABlen = B.dot(B);
180 if (ABlen <= 0) {
181 return false;
182 }
183 Vector2 Bn = B / ABlen;
184 C = Vector2(C.x * Bn.x + C.y * Bn.y, C.y * Bn.x - C.x * Bn.y);
185 D = Vector2(D.x * Bn.x + D.y * Bn.y, D.y * Bn.x - D.x * Bn.y);
186
187 // Fail if C x B and D x B have the same sign (segments don't intersect).
188 if ((C.y < (real_t)-CMP_EPSILON && D.y < (real_t)-CMP_EPSILON) || (C.y > (real_t)CMP_EPSILON && D.y > (real_t)CMP_EPSILON)) {
189 return false;
190 }
191
192 // Fail if segments are parallel or colinear.
193 // (when A x B == zero, i.e (C - D) x B == zero, i.e C x B == D x B)
194 if (Math::is_equal_approx(C.y, D.y)) {
195 return false;
196 }
197
198 real_t ABpos = D.x + (C.x - D.x) * D.y / (D.y - C.y);
199
200 // Fail if segment C-D crosses line A-B outside of segment A-B.
201 if ((ABpos < 0) || (ABpos > 1)) {
202 return false;
203 }
204
205 // Apply the discovered position to line A-B in the original coordinate system.
206 if (r_result) {
207 *r_result = p_from_a + B * ABpos;
208 }
209
210 return true;
211 }
212
213 static inline bool is_point_in_circle(const Vector2 &p_point, const Vector2 &p_circle_pos, real_t p_circle_radius) {
214 return p_point.distance_squared_to(p_circle_pos) <= p_circle_radius * p_circle_radius;
215 }
216
217 static real_t segment_intersects_circle(const Vector2 &p_from, const Vector2 &p_to, const Vector2 &p_circle_pos, real_t p_circle_radius) {
218 Vector2 line_vec = p_to - p_from;
219 Vector2 vec_to_line = p_from - p_circle_pos;
220
221 // Create a quadratic formula of the form ax^2 + bx + c = 0
222 real_t a, b, c;
223
224 a = line_vec.dot(line_vec);
225 b = 2 * vec_to_line.dot(line_vec);
226 c = vec_to_line.dot(vec_to_line) - p_circle_radius * p_circle_radius;
227
228 // Solve for t.
229 real_t sqrtterm = b * b - 4 * a * c;
230
231 // If the term we intend to square root is less than 0 then the answer won't be real,
232 // so it definitely won't be t in the range 0 to 1.
233 if (sqrtterm < 0) {
234 return -1;
235 }
236
237 // If we can assume that the line segment starts outside the circle (e.g. for continuous time collision detection)
238 // then the following can be skipped and we can just return the equivalent of res1.
239 sqrtterm = Math::sqrt(sqrtterm);
240 real_t res1 = (-b - sqrtterm) / (2 * a);
241 real_t res2 = (-b + sqrtterm) / (2 * a);
242
243 if (res1 >= 0 && res1 <= 1) {
244 return res1;
245 }
246 if (res2 >= 0 && res2 <= 1) {
247 return res2;
248 }
249 return -1;
250 }
251
252 enum PolyBooleanOperation {
253 OPERATION_UNION,
254 OPERATION_DIFFERENCE,
255 OPERATION_INTERSECTION,
256 OPERATION_XOR
257 };
258 enum PolyJoinType {
259 JOIN_SQUARE,
260 JOIN_ROUND,
261 JOIN_MITER
262 };
263 enum PolyEndType {
264 END_POLYGON,
265 END_JOINED,
266 END_BUTT,
267 END_SQUARE,
268 END_ROUND
269 };
270
271 static Vector<Vector<Point2>> merge_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) {
272 return _polypaths_do_operation(OPERATION_UNION, p_polygon_a, p_polygon_b);
273 }
274
275 static Vector<Vector<Point2>> clip_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) {
276 return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polygon_a, p_polygon_b);
277 }
278
279 static Vector<Vector<Point2>> intersect_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) {
280 return _polypaths_do_operation(OPERATION_INTERSECTION, p_polygon_a, p_polygon_b);
281 }
282
283 static Vector<Vector<Point2>> exclude_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) {
284 return _polypaths_do_operation(OPERATION_XOR, p_polygon_a, p_polygon_b);
285 }
286
287 static Vector<Vector<Point2>> clip_polyline_with_polygon(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) {
288 return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polyline, p_polygon, true);
289 }
290
291 static Vector<Vector<Point2>> intersect_polyline_with_polygon(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) {
292 return _polypaths_do_operation(OPERATION_INTERSECTION, p_polyline, p_polygon, true);
293 }
294
295 static Vector<Vector<Point2>> offset_polygon(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type) {
296 return _polypath_offset(p_polygon, p_delta, p_join_type, END_POLYGON);
297 }
298
299 static Vector<Vector<Point2>> offset_polyline(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) {
300 ERR_FAIL_COND_V_MSG(p_end_type == END_POLYGON, Vector<Vector<Point2>>(), "Attempt to offset a polyline like a polygon (use offset_polygon instead).");
301
302 return _polypath_offset(p_polygon, p_delta, p_join_type, p_end_type);
303 }
304
305 static Vector<int> triangulate_delaunay(const Vector<Vector2> &p_points) {
306 Vector<Delaunay2D::Triangle> tr = Delaunay2D::triangulate(p_points);
307 Vector<int> triangles;
308
309 for (int i = 0; i < tr.size(); i++) {
310 triangles.push_back(tr[i].points[0]);
311 triangles.push_back(tr[i].points[1]);
312 triangles.push_back(tr[i].points[2]);
313 }
314 return triangles;
315 }
316
317 static Vector<int> triangulate_polygon(const Vector<Vector2> &p_polygon) {
318 Vector<int> triangles;
319 if (!Triangulate::triangulate(p_polygon, triangles)) {
320 return Vector<int>(); //fail
321 }
322 return triangles;
323 }
324
325 static bool is_polygon_clockwise(const Vector<Vector2> &p_polygon) {
326 int c = p_polygon.size();
327 if (c < 3) {
328 return false;
329 }
330 const Vector2 *p = p_polygon.ptr();
331 real_t sum = 0;
332 for (int i = 0; i < c; i++) {
333 const Vector2 &v1 = p[i];
334 const Vector2 &v2 = p[(i + 1) % c];
335 sum += (v2.x - v1.x) * (v2.y + v1.y);
336 }
337
338 return sum > 0.0f;
339 }
340
341 // Alternate implementation that should be faster.
342 static bool is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon) {
343 int c = p_polygon.size();
344 if (c < 3) {
345 return false;
346 }
347 const Vector2 *p = p_polygon.ptr();
348 Vector2 further_away(-1e20, -1e20);
349 Vector2 further_away_opposite(1e20, 1e20);
350
351 for (int i = 0; i < c; i++) {
352 further_away.x = MAX(p[i].x, further_away.x);
353 further_away.y = MAX(p[i].y, further_away.y);
354 further_away_opposite.x = MIN(p[i].x, further_away_opposite.x);
355 further_away_opposite.y = MIN(p[i].y, further_away_opposite.y);
356 }
357
358 // Make point outside that won't intersect with points in segment from p_point.
359 further_away += (further_away - further_away_opposite) * Vector2(1.221313, 1.512312);
360
361 int intersections = 0;
362 for (int i = 0; i < c; i++) {
363 const Vector2 &v1 = p[i];
364 const Vector2 &v2 = p[(i + 1) % c];
365
366 Vector2 res;
367 if (segment_intersects_segment(v1, v2, p_point, further_away, &res)) {
368 intersections++;
369 if (res.is_equal_approx(p_point)) {
370 // Point is in one of the polygon edges.
371 return true;
372 }
373 }
374 }
375
376 return (intersections & 1);
377 }
378
379 static bool is_segment_intersecting_polygon(const Vector2 &p_from, const Vector2 &p_to, const Vector<Vector2> &p_polygon) {
380 int c = p_polygon.size();
381 const Vector2 *p = p_polygon.ptr();
382 for (int i = 0; i < c; i++) {
383 const Vector2 &v1 = p[i];
384 const Vector2 &v2 = p[(i + 1) % c];
385 if (segment_intersects_segment(p_from, p_to, v1, v2, nullptr)) {
386 return true;
387 }
388 }
389 return false;
390 }
391
392 static real_t vec2_cross(const Point2 &O, const Point2 &A, const Point2 &B) {
393 return (real_t)(A.x - O.x) * (B.y - O.y) - (real_t)(A.y - O.y) * (B.x - O.x);
394 }
395
396 // Returns a list of points on the convex hull in counter-clockwise order.
397 // Note: the last point in the returned list is the same as the first one.
398 static Vector<Point2> convex_hull(Vector<Point2> P) {
399 int n = P.size(), k = 0;
400 Vector<Point2> H;
401 H.resize(2 * n);
402
403 // Sort points lexicographically.
404 P.sort();
405
406 // Build lower hull.
407 for (int i = 0; i < n; ++i) {
408 while (k >= 2 && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) {
409 k--;
410 }
411 H.write[k++] = P[i];
412 }
413
414 // Build upper hull.
415 for (int i = n - 2, t = k + 1; i >= 0; i--) {
416 while (k >= t && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) {
417 k--;
418 }
419 H.write[k++] = P[i];
420 }
421
422 H.resize(k);
423 return H;
424 }
425
426 static Vector<Point2i> bresenham_line(const Point2i &p_start, const Point2i &p_end) {
427 Vector<Point2i> points;
428
429 Vector2i delta = (p_end - p_start).abs() * 2;
430 Vector2i step = (p_end - p_start).sign();
431 Vector2i current = p_start;
432
433 if (delta.x > delta.y) {
434 int err = delta.x / 2;
435
436 for (; current.x != p_end.x; current.x += step.x) {
437 points.push_back(current);
438
439 err -= delta.y;
440 if (err < 0) {
441 current.y += step.y;
442 err += delta.x;
443 }
444 }
445 } else {
446 int err = delta.y / 2;
447
448 for (; current.y != p_end.y; current.y += step.y) {
449 points.push_back(current);
450
451 err -= delta.x;
452 if (err < 0) {
453 current.x += step.x;
454 err += delta.y;
455 }
456 }
457 }
458
459 points.push_back(current);
460
461 return points;
462 }
463
464 static Vector<Vector<Vector2>> decompose_polygon_in_convex(Vector<Point2> polygon);
465
466 static void make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size);
467 static Vector<Vector3i> partial_pack_rects(const Vector<Vector2i> &p_sizes, const Size2i &p_atlas_size);
468
469private:
470 static Vector<Vector<Point2>> _polypaths_do_operation(PolyBooleanOperation p_op, const Vector<Point2> &p_polypath_a, const Vector<Point2> &p_polypath_b, bool is_a_open = false);
471 static Vector<Vector<Point2>> _polypath_offset(const Vector<Point2> &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type);
472};
473
474#endif // GEOMETRY_2D_H
475