1/**************************************************************************/
2/* godot_collision_solver_2d_sat.cpp */
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#include "godot_collision_solver_2d_sat.h"
32
33#include "core/math/geometry_2d.h"
34
35struct _CollectorCallback2D {
36 GodotCollisionSolver2D::CallbackResult callback = nullptr;
37 void *userdata = nullptr;
38 bool swap = false;
39 bool collided = false;
40 Vector2 normal;
41 Vector2 *sep_axis = nullptr;
42
43 _FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) {
44 if (swap) {
45 callback(p_point_B, p_point_A, userdata);
46 } else {
47 callback(p_point_A, p_point_B, userdata);
48 }
49 }
50};
51
52typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *);
53
54_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
55#ifdef DEBUG_ENABLED
56 ERR_FAIL_COND(p_point_count_A != 1);
57 ERR_FAIL_COND(p_point_count_B != 1);
58#endif
59
60 p_collector->call(*p_points_A, *p_points_B);
61}
62
63_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
64#ifdef DEBUG_ENABLED
65 ERR_FAIL_COND(p_point_count_A != 1);
66 ERR_FAIL_COND(p_point_count_B != 2);
67#endif
68
69 Vector2 closest_B = Geometry2D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B);
70 p_collector->call(*p_points_A, closest_B);
71}
72
73struct _generate_contacts_Pair {
74 bool a = false;
75 int idx = 0;
76 real_t d = 0.0;
77 _FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; }
78};
79
80_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
81#ifdef DEBUG_ENABLED
82 ERR_FAIL_COND(p_point_count_A != 2);
83 ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix
84#endif
85
86 Vector2 n = p_collector->normal;
87 Vector2 t = n.orthogonal();
88 real_t dA = n.dot(p_points_A[0]);
89 real_t dB = n.dot(p_points_B[0]);
90
91 _generate_contacts_Pair dvec[4];
92
93 dvec[0].d = t.dot(p_points_A[0]);
94 dvec[0].a = true;
95 dvec[0].idx = 0;
96 dvec[1].d = t.dot(p_points_A[1]);
97 dvec[1].a = true;
98 dvec[1].idx = 1;
99 dvec[2].d = t.dot(p_points_B[0]);
100 dvec[2].a = false;
101 dvec[2].idx = 0;
102 dvec[3].d = t.dot(p_points_B[1]);
103 dvec[3].a = false;
104 dvec[3].idx = 1;
105
106 SortArray<_generate_contacts_Pair> sa;
107 sa.sort(dvec, 4);
108
109 for (int i = 1; i <= 2; i++) {
110 if (dvec[i].a) {
111 Vector2 a = p_points_A[dvec[i].idx];
112 Vector2 b = n.plane_project(dB, a);
113 if (n.dot(a) > n.dot(b) - CMP_EPSILON) {
114 continue;
115 }
116 p_collector->call(a, b);
117 } else {
118 Vector2 b = p_points_B[dvec[i].idx];
119 Vector2 a = n.plane_project(dA, b);
120 if (n.dot(a) > n.dot(b) - CMP_EPSILON) {
121 continue;
122 }
123 p_collector->call(a, b);
124 }
125 }
126}
127
128static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
129#ifdef DEBUG_ENABLED
130 ERR_FAIL_COND(p_point_count_A < 1);
131 ERR_FAIL_COND(p_point_count_B < 1);
132#endif
133
134 static const GenerateContactsFunc generate_contacts_func_table[2][2] = {
135 {
136 _generate_contacts_point_point,
137 _generate_contacts_point_edge,
138 },
139 {
140 nullptr,
141 _generate_contacts_edge_edge,
142 }
143 };
144
145 int pointcount_B = 0;
146 int pointcount_A = 0;
147 const Vector2 *points_A = nullptr;
148 const Vector2 *points_B = nullptr;
149
150 if (p_point_count_A > p_point_count_B) {
151 //swap
152 p_collector->swap = !p_collector->swap;
153 p_collector->normal = -p_collector->normal;
154
155 pointcount_B = p_point_count_A;
156 pointcount_A = p_point_count_B;
157 points_A = p_points_B;
158 points_B = p_points_A;
159 } else {
160 pointcount_B = p_point_count_B;
161 pointcount_A = p_point_count_A;
162 points_A = p_points_A;
163 points_B = p_points_B;
164 }
165
166 int version_A = (pointcount_A > 2 ? 2 : pointcount_A) - 1;
167 int version_B = (pointcount_B > 2 ? 2 : pointcount_B) - 1;
168
169 GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
170 ERR_FAIL_COND(!contacts_func);
171 contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_collector);
172}
173
174template <class ShapeA, class ShapeB, bool castA = false, bool castB = false, bool withMargin = false>
175class SeparatorAxisTest2D {
176 const ShapeA *shape_A = nullptr;
177 const ShapeB *shape_B = nullptr;
178 const Transform2D *transform_A = nullptr;
179 const Transform2D *transform_B = nullptr;
180 real_t best_depth = 1e15;
181 Vector2 best_axis;
182#ifdef DEBUG_ENABLED
183 int best_axis_count = 0;
184 int best_axis_index = -1;
185#endif
186 Vector2 motion_A;
187 Vector2 motion_B;
188 real_t margin_A = 0.0;
189 real_t margin_B = 0.0;
190 _CollectorCallback2D *callback;
191
192public:
193 _FORCE_INLINE_ bool test_previous_axis() {
194 if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) {
195 return test_axis(*callback->sep_axis);
196 } else {
197#ifdef DEBUG_ENABLED
198 best_axis_count++;
199#endif
200 }
201 return true;
202 }
203
204 _FORCE_INLINE_ bool test_cast() {
205 if (castA) {
206 Vector2 na = motion_A.normalized();
207 if (!test_axis(na)) {
208 return false;
209 }
210 if (!test_axis(na.orthogonal())) {
211 return false;
212 }
213 }
214
215 if (castB) {
216 Vector2 nb = motion_B.normalized();
217 if (!test_axis(nb)) {
218 return false;
219 }
220 if (!test_axis(nb.orthogonal())) {
221 return false;
222 }
223 }
224
225 return true;
226 }
227
228 _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) {
229 Vector2 axis = p_axis;
230
231 if (Math::is_zero_approx(axis.x) &&
232 Math::is_zero_approx(axis.y)) {
233 // strange case, try an upwards separator
234 axis = Vector2(0.0, 1.0);
235 }
236
237 real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0;
238
239 if (castA) {
240 shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A);
241 } else {
242 shape_A->project_range(axis, *transform_A, min_A, max_A);
243 }
244
245 if (castB) {
246 shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B);
247 } else {
248 shape_B->project_range(axis, *transform_B, min_B, max_B);
249 }
250
251 if (withMargin) {
252 min_A -= margin_A;
253 max_A += margin_A;
254 min_B -= margin_B;
255 max_B += margin_B;
256 }
257
258 min_B -= (max_A - min_A) * 0.5;
259 max_B += (max_A - min_A) * 0.5;
260
261 real_t dmin = min_B - (min_A + max_A) * 0.5;
262 real_t dmax = max_B - (min_A + max_A) * 0.5;
263
264 if (dmin > 0.0 || dmax < 0.0) {
265 if (callback && callback->sep_axis) {
266 *callback->sep_axis = axis;
267 }
268#ifdef DEBUG_ENABLED
269 best_axis_count++;
270#endif
271
272 return false; // doesn't contain 0
273 }
274
275 //use the smallest depth
276
277 dmin = Math::abs(dmin);
278
279 if (dmax < dmin) {
280 if (dmax < best_depth) {
281 best_depth = dmax;
282 best_axis = axis;
283#ifdef DEBUG_ENABLED
284 best_axis_index = best_axis_count;
285#endif
286 }
287 } else {
288 if (dmin < best_depth) {
289 best_depth = dmin;
290 best_axis = -axis; // keep it as A axis
291#ifdef DEBUG_ENABLED
292 best_axis_index = best_axis_count;
293#endif
294 }
295 }
296
297#ifdef DEBUG_ENABLED
298 best_axis_count++;
299#endif
300
301 return true;
302 }
303
304 _FORCE_INLINE_ void generate_contacts() {
305 // nothing to do, don't generate
306 if (best_axis == Vector2(0.0, 0.0)) {
307 return;
308 }
309
310 if (callback) {
311 callback->collided = true;
312
313 if (!callback->callback) {
314 return; //only collide, no callback
315 }
316 }
317 static const int max_supports = 2;
318
319 Vector2 supports_A[max_supports];
320 int support_count_A;
321 if (castA) {
322 shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A);
323 } else {
324 shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A);
325 for (int i = 0; i < support_count_A; i++) {
326 supports_A[i] = transform_A->xform(supports_A[i]);
327 }
328 }
329
330 if (withMargin) {
331 for (int i = 0; i < support_count_A; i++) {
332 supports_A[i] += -best_axis * margin_A;
333 }
334 }
335
336 Vector2 supports_B[max_supports];
337 int support_count_B;
338 if (castB) {
339 shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B);
340 } else {
341 shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B);
342 for (int i = 0; i < support_count_B; i++) {
343 supports_B[i] = transform_B->xform(supports_B[i]);
344 }
345 }
346
347 if (withMargin) {
348 for (int i = 0; i < support_count_B; i++) {
349 supports_B[i] += best_axis * margin_B;
350 }
351 }
352 if (callback) {
353 callback->normal = best_axis;
354 _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
355
356 if (callback->sep_axis && *callback->sep_axis != Vector2()) {
357 *callback->sep_axis = Vector2(); //invalidate previous axis (no test)
358 }
359 }
360 }
361
362 _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) {
363 margin_A = p_margin_A;
364 margin_B = p_margin_B;
365 shape_A = p_shape_A;
366 shape_B = p_shape_B;
367 transform_A = &p_transform_a;
368 transform_B = &p_transform_b;
369 motion_A = p_motion_A;
370 motion_B = p_motion_B;
371 callback = p_collector;
372 }
373};
374
375/****** SAT TESTS *******/
376
377#define TEST_POINT(m_a, m_b) \
378 ((!separator.test_axis(((m_a) - (m_b)).normalized())) || \
379 (castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) || \
380 (castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \
381 (castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized())))
382
383typedef void (*CollisionFunc)(const GodotShape2D *, const Transform2D &, const GodotShape2D *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t);
384
385template <bool castA, bool castB, bool withMargin>
386static void _collision_segment_segment(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
387 const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
388 const GodotSegmentShape2D *segment_B = static_cast<const GodotSegmentShape2D *>(p_b);
389
390 SeparatorAxisTest2D<GodotSegmentShape2D, GodotSegmentShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
391
392 if (!separator.test_previous_axis()) {
393 return;
394 }
395 //this collision is kind of pointless
396
397 if (!separator.test_cast()) {
398 return;
399 }
400
401 if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
402 return;
403 }
404 if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) {
405 return;
406 }
407
408 if (withMargin) {
409 //points grow to circles
410
411 if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) {
412 return;
413 }
414 if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) {
415 return;
416 }
417 if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) {
418 return;
419 }
420 if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) {
421 return;
422 }
423 }
424
425 separator.generate_contacts();
426}
427
428template <bool castA, bool castB, bool withMargin>
429static void _collision_segment_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
430 const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
431 const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b);
432
433 SeparatorAxisTest2D<GodotSegmentShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
434
435 if (!separator.test_previous_axis()) {
436 return;
437 }
438
439 if (!separator.test_cast()) {
440 return;
441 }
442
443 //segment normal
444 if (!separator.test_axis(
445 (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().orthogonal())) {
446 return;
447 }
448
449 //endpoint a vs circle
450 if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) {
451 return;
452 }
453 //endpoint b vs circle
454 if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) {
455 return;
456 }
457
458 separator.generate_contacts();
459}
460
461template <bool castA, bool castB, bool withMargin>
462static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
463 const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
464 const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
465
466 SeparatorAxisTest2D<GodotSegmentShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
467
468 if (!separator.test_previous_axis()) {
469 return;
470 }
471
472 if (!separator.test_cast()) {
473 return;
474 }
475
476 if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
477 return;
478 }
479
480 if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
481 return;
482 }
483
484 if (!separator.test_axis(p_transform_b.columns[1].normalized())) {
485 return;
486 }
487
488 if (withMargin) {
489 Transform2D inv = p_transform_b.affine_inverse();
490
491 Vector2 a = p_transform_a.xform(segment_A->get_a());
492 Vector2 b = p_transform_a.xform(segment_A->get_b());
493
494 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) {
495 return;
496 }
497 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) {
498 return;
499 }
500
501 if constexpr (castA) {
502 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) {
503 return;
504 }
505 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) {
506 return;
507 }
508 }
509
510 if constexpr (castB) {
511 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) {
512 return;
513 }
514 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) {
515 return;
516 }
517 }
518
519 if constexpr (castA && castB) {
520 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) {
521 return;
522 }
523 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) {
524 return;
525 }
526 }
527 }
528
529 separator.generate_contacts();
530}
531
532template <bool castA, bool castB, bool withMargin>
533static void _collision_segment_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
534 const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
535 const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
536
537 SeparatorAxisTest2D<GodotSegmentShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
538
539 if (!separator.test_previous_axis()) {
540 return;
541 }
542
543 if (!separator.test_cast()) {
544 return;
545 }
546
547 if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
548 return;
549 }
550
551 if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
552 return;
553 }
554
555 real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
556
557 if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
558 return;
559 }
560 if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
561 return;
562 }
563 if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
564 return;
565 }
566 if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
567 return;
568 }
569
570 separator.generate_contacts();
571}
572
573template <bool castA, bool castB, bool withMargin>
574static void _collision_segment_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
575 const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
576 const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
577
578 SeparatorAxisTest2D<GodotSegmentShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
579
580 if (!separator.test_previous_axis()) {
581 return;
582 }
583
584 if (!separator.test_cast()) {
585 return;
586 }
587
588 if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) {
589 return;
590 }
591
592 for (int i = 0; i < convex_B->get_point_count(); i++) {
593 if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
594 return;
595 }
596
597 if (withMargin) {
598 if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) {
599 return;
600 }
601 if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) {
602 return;
603 }
604 }
605 }
606
607 separator.generate_contacts();
608}
609
610/////////
611
612template <bool castA, bool castB, bool withMargin>
613static void _collision_circle_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
614 const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
615 const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b);
616
617 SeparatorAxisTest2D<GodotCircleShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
618
619 if (!separator.test_previous_axis()) {
620 return;
621 }
622
623 if (!separator.test_cast()) {
624 return;
625 }
626
627 if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) {
628 return;
629 }
630
631 separator.generate_contacts();
632}
633
634template <bool castA, bool castB, bool withMargin>
635static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
636 const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
637 const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
638
639 SeparatorAxisTest2D<GodotCircleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
640
641 if (!separator.test_previous_axis()) {
642 return;
643 }
644
645 if (!separator.test_cast()) {
646 return;
647 }
648
649 const Vector2 &sphere = p_transform_a.columns[2];
650 const Vector2 *axis = &p_transform_b.columns[0];
651 //const Vector2& half_extents = rectangle_B->get_half_extents();
652
653 if (!separator.test_axis(axis[0].normalized())) {
654 return;
655 }
656
657 if (!separator.test_axis(axis[1].normalized())) {
658 return;
659 }
660
661 Transform2D binv = p_transform_b.affine_inverse();
662 {
663 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) {
664 return;
665 }
666 }
667
668 if constexpr (castA) {
669 Vector2 sphereofs = sphere + p_motion_a;
670 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
671 return;
672 }
673 }
674
675 if constexpr (castB) {
676 Vector2 sphereofs = sphere - p_motion_b;
677 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
678 return;
679 }
680 }
681
682 if constexpr (castA && castB) {
683 Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
684 if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) {
685 return;
686 }
687 }
688
689 separator.generate_contacts();
690}
691
692template <bool castA, bool castB, bool withMargin>
693static void _collision_circle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
694 const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
695 const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
696
697 SeparatorAxisTest2D<GodotCircleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
698
699 if (!separator.test_previous_axis()) {
700 return;
701 }
702
703 if (!separator.test_cast()) {
704 return;
705 }
706
707 //capsule axis
708 if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
709 return;
710 }
711
712 real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
713
714 //capsule endpoints
715 if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) {
716 return;
717 }
718 if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) {
719 return;
720 }
721
722 separator.generate_contacts();
723}
724
725template <bool castA, bool castB, bool withMargin>
726static void _collision_circle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
727 const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
728 const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
729
730 SeparatorAxisTest2D<GodotCircleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
731
732 if (!separator.test_previous_axis()) {
733 return;
734 }
735
736 if (!separator.test_cast()) {
737 return;
738 }
739
740 //poly faces and poly points vs circle
741 for (int i = 0; i < convex_B->get_point_count(); i++) {
742 if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) {
743 return;
744 }
745
746 if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
747 return;
748 }
749 }
750
751 separator.generate_contacts();
752}
753
754/////////
755
756template <bool castA, bool castB, bool withMargin>
757static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
758 const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
759 const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
760
761 SeparatorAxisTest2D<GodotRectangleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
762
763 if (!separator.test_previous_axis()) {
764 return;
765 }
766
767 if (!separator.test_cast()) {
768 return;
769 }
770
771 //box faces A
772 if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
773 return;
774 }
775
776 if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
777 return;
778 }
779
780 //box faces B
781 if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
782 return;
783 }
784
785 if (!separator.test_axis(p_transform_b.columns[1].normalized())) {
786 return;
787 }
788
789 if constexpr (withMargin) {
790 Transform2D invA = p_transform_a.affine_inverse();
791 Transform2D invB = p_transform_b.affine_inverse();
792
793 if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) {
794 return;
795 }
796
797 if constexpr (castA || castB) {
798 Transform2D aofs = p_transform_a;
799 aofs.columns[2] += p_motion_a;
800
801 Transform2D bofs = p_transform_b;
802 bofs.columns[2] += p_motion_b;
803
804 [[maybe_unused]] Transform2D aofsinv = aofs.affine_inverse();
805 [[maybe_unused]] Transform2D bofsinv = bofs.affine_inverse();
806
807 if constexpr (castA) {
808 if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) {
809 return;
810 }
811 }
812
813 if constexpr (castB) {
814 if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) {
815 return;
816 }
817 }
818
819 if constexpr (castA && castB) {
820 if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) {
821 return;
822 }
823 }
824 }
825 }
826
827 separator.generate_contacts();
828}
829
830template <bool castA, bool castB, bool withMargin>
831static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
832 const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
833 const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
834
835 SeparatorAxisTest2D<GodotRectangleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
836
837 if (!separator.test_previous_axis()) {
838 return;
839 }
840
841 if (!separator.test_cast()) {
842 return;
843 }
844
845 //box faces
846 if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
847 return;
848 }
849
850 if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
851 return;
852 }
853
854 //capsule axis
855 if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
856 return;
857 }
858
859 //box endpoints to capsule circles
860
861 Transform2D boxinv = p_transform_a.affine_inverse();
862
863 real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
864
865 for (int i = 0; i < 2; i++) {
866 {
867 Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
868
869 if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
870 return;
871 }
872 }
873
874 if constexpr (castA) {
875 Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
876 capsule_endpoint -= p_motion_a;
877
878 if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
879 return;
880 }
881 }
882
883 if constexpr (castB) {
884 Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
885 capsule_endpoint += p_motion_b;
886
887 if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
888 return;
889 }
890 }
891
892 if constexpr (castA && castB) {
893 Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir;
894 capsule_endpoint -= p_motion_a;
895 capsule_endpoint += p_motion_b;
896
897 if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
898 return;
899 }
900 }
901
902 capsule_dir *= -1.0;
903 }
904
905 separator.generate_contacts();
906}
907
908template <bool castA, bool castB, bool withMargin>
909static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
910 const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
911 const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
912
913 SeparatorAxisTest2D<GodotRectangleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
914
915 if (!separator.test_previous_axis()) {
916 return;
917 }
918
919 if (!separator.test_cast()) {
920 return;
921 }
922
923 //box faces
924 if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
925 return;
926 }
927
928 if (!separator.test_axis(p_transform_a.columns[1].normalized())) {
929 return;
930 }
931
932 //convex faces
933 Transform2D boxinv;
934 if constexpr (withMargin) {
935 boxinv = p_transform_a.affine_inverse();
936 }
937 for (int i = 0; i < convex_B->get_point_count(); i++) {
938 if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
939 return;
940 }
941
942 if constexpr (withMargin) {
943 //all points vs all points need to be tested if margin exist
944 if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) {
945 return;
946 }
947 if constexpr (castA) {
948 if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) {
949 return;
950 }
951 }
952 if constexpr (castB) {
953 if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) {
954 return;
955 }
956 }
957 if constexpr (castA && castB) {
958 if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) {
959 return;
960 }
961 }
962 }
963 }
964
965 separator.generate_contacts();
966}
967
968/////////
969
970template <bool castA, bool castB, bool withMargin>
971static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
972 const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a);
973 const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
974
975 SeparatorAxisTest2D<GodotCapsuleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
976
977 if (!separator.test_previous_axis()) {
978 return;
979 }
980
981 if (!separator.test_cast()) {
982 return;
983 }
984
985 //capsule axis
986
987 if (!separator.test_axis(p_transform_b.columns[0].normalized())) {
988 return;
989 }
990
991 if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
992 return;
993 }
994
995 //capsule endpoints
996
997 real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
998 for (int i = 0; i < 2; i++) {
999 Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir_A;
1000
1001 real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
1002 for (int j = 0; j < 2; j++) {
1003 Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir_B;
1004
1005 if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) {
1006 return;
1007 }
1008
1009 capsule_dir_B *= -1.0;
1010 }
1011
1012 capsule_dir_A *= -1.0;
1013 }
1014
1015 separator.generate_contacts();
1016}
1017
1018template <bool castA, bool castB, bool withMargin>
1019static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
1020 const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a);
1021 const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
1022
1023 SeparatorAxisTest2D<GodotCapsuleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
1024
1025 if (!separator.test_previous_axis()) {
1026 return;
1027 }
1028
1029 if (!separator.test_cast()) {
1030 return;
1031 }
1032
1033 //capsule axis
1034
1035 if (!separator.test_axis(p_transform_a.columns[0].normalized())) {
1036 return;
1037 }
1038
1039 //poly vs capsule
1040 for (int i = 0; i < convex_B->get_point_count(); i++) {
1041 Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i));
1042
1043 real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
1044 for (int j = 0; j < 2; j++) {
1045 Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir;
1046
1047 if (TEST_POINT(capsule_endpoint_A, cpoint)) {
1048 return;
1049 }
1050
1051 capsule_dir *= -1.0;
1052 }
1053
1054 if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
1055 return;
1056 }
1057 }
1058
1059 separator.generate_contacts();
1060}
1061
1062/////////
1063
1064template <bool castA, bool castB, bool withMargin>
1065static void _collision_convex_polygon_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
1066 const GodotConvexPolygonShape2D *convex_A = static_cast<const GodotConvexPolygonShape2D *>(p_a);
1067 const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
1068
1069 SeparatorAxisTest2D<GodotConvexPolygonShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
1070
1071 if (!separator.test_previous_axis()) {
1072 return;
1073 }
1074
1075 if (!separator.test_cast()) {
1076 return;
1077 }
1078
1079 for (int i = 0; i < convex_A->get_point_count(); i++) {
1080 if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) {
1081 return;
1082 }
1083 }
1084
1085 for (int i = 0; i < convex_B->get_point_count(); i++) {
1086 if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
1087 return;
1088 }
1089 }
1090
1091 if (withMargin) {
1092 for (int i = 0; i < convex_A->get_point_count(); i++) {
1093 for (int j = 0; j < convex_B->get_point_count(); j++) {
1094 if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) {
1095 return;
1096 }
1097 }
1098 }
1099 }
1100
1101 separator.generate_contacts();
1102}
1103
1104////////
1105
1106bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) {
1107 PhysicsServer2D::ShapeType type_A = p_shape_A->get_type();
1108
1109 ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false);
1110 ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false);
1111 ERR_FAIL_COND_V(p_shape_A->is_concave(), false);
1112
1113 PhysicsServer2D::ShapeType type_B = p_shape_B->get_type();
1114
1115 ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false);
1116 ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false);
1117 ERR_FAIL_COND_V(p_shape_B->is_concave(), false);
1118
1119 static const CollisionFunc collision_table[5][5] = {
1120 { _collision_segment_segment<false, false, false>,
1121 _collision_segment_circle<false, false, false>,
1122 _collision_segment_rectangle<false, false, false>,
1123 _collision_segment_capsule<false, false, false>,
1124 _collision_segment_convex_polygon<false, false, false> },
1125 { nullptr,
1126 _collision_circle_circle<false, false, false>,
1127 _collision_circle_rectangle<false, false, false>,
1128 _collision_circle_capsule<false, false, false>,
1129 _collision_circle_convex_polygon<false, false, false> },
1130 { nullptr,
1131 nullptr,
1132 _collision_rectangle_rectangle<false, false, false>,
1133 _collision_rectangle_capsule<false, false, false>,
1134 _collision_rectangle_convex_polygon<false, false, false> },
1135 { nullptr,
1136 nullptr,
1137 nullptr,
1138 _collision_capsule_capsule<false, false, false>,
1139 _collision_capsule_convex_polygon<false, false, false> },
1140 { nullptr,
1141 nullptr,
1142 nullptr,
1143 nullptr,
1144 _collision_convex_polygon_convex_polygon<false, false, false> }
1145
1146 };
1147
1148 static const CollisionFunc collision_table_castA[5][5] = {
1149 { _collision_segment_segment<true, false, false>,
1150 _collision_segment_circle<true, false, false>,
1151 _collision_segment_rectangle<true, false, false>,
1152 _collision_segment_capsule<true, false, false>,
1153 _collision_segment_convex_polygon<true, false, false> },
1154 { nullptr,
1155 _collision_circle_circle<true, false, false>,
1156 _collision_circle_rectangle<true, false, false>,
1157 _collision_circle_capsule<true, false, false>,
1158 _collision_circle_convex_polygon<true, false, false> },
1159 { nullptr,
1160 nullptr,
1161 _collision_rectangle_rectangle<true, false, false>,
1162 _collision_rectangle_capsule<true, false, false>,
1163 _collision_rectangle_convex_polygon<true, false, false> },
1164 { nullptr,
1165 nullptr,
1166 nullptr,
1167 _collision_capsule_capsule<true, false, false>,
1168 _collision_capsule_convex_polygon<true, false, false> },
1169 { nullptr,
1170 nullptr,
1171 nullptr,
1172 nullptr,
1173 _collision_convex_polygon_convex_polygon<true, false, false> }
1174
1175 };
1176
1177 static const CollisionFunc collision_table_castB[5][5] = {
1178 { _collision_segment_segment<false, true, false>,
1179 _collision_segment_circle<false, true, false>,
1180 _collision_segment_rectangle<false, true, false>,
1181 _collision_segment_capsule<false, true, false>,
1182 _collision_segment_convex_polygon<false, true, false> },
1183 { nullptr,
1184 _collision_circle_circle<false, true, false>,
1185 _collision_circle_rectangle<false, true, false>,
1186 _collision_circle_capsule<false, true, false>,
1187 _collision_circle_convex_polygon<false, true, false> },
1188 { nullptr,
1189 nullptr,
1190 _collision_rectangle_rectangle<false, true, false>,
1191 _collision_rectangle_capsule<false, true, false>,
1192 _collision_rectangle_convex_polygon<false, true, false> },
1193 { nullptr,
1194 nullptr,
1195 nullptr,
1196 _collision_capsule_capsule<false, true, false>,
1197 _collision_capsule_convex_polygon<false, true, false> },
1198 { nullptr,
1199 nullptr,
1200 nullptr,
1201 nullptr,
1202 _collision_convex_polygon_convex_polygon<false, true, false> }
1203
1204 };
1205
1206 static const CollisionFunc collision_table_castA_castB[5][5] = {
1207 { _collision_segment_segment<true, true, false>,
1208 _collision_segment_circle<true, true, false>,
1209 _collision_segment_rectangle<true, true, false>,
1210 _collision_segment_capsule<true, true, false>,
1211 _collision_segment_convex_polygon<true, true, false> },
1212 { nullptr,
1213 _collision_circle_circle<true, true, false>,
1214 _collision_circle_rectangle<true, true, false>,
1215 _collision_circle_capsule<true, true, false>,
1216 _collision_circle_convex_polygon<true, true, false> },
1217 { nullptr,
1218 nullptr,
1219 _collision_rectangle_rectangle<true, true, false>,
1220 _collision_rectangle_capsule<true, true, false>,
1221 _collision_rectangle_convex_polygon<true, true, false> },
1222 { nullptr,
1223 nullptr,
1224 nullptr,
1225 _collision_capsule_capsule<true, true, false>,
1226 _collision_capsule_convex_polygon<true, true, false> },
1227 { nullptr,
1228 nullptr,
1229 nullptr,
1230 nullptr,
1231 _collision_convex_polygon_convex_polygon<true, true, false> }
1232
1233 };
1234
1235 static const CollisionFunc collision_table_margin[5][5] = {
1236 { _collision_segment_segment<false, false, true>,
1237 _collision_segment_circle<false, false, true>,
1238 _collision_segment_rectangle<false, false, true>,
1239 _collision_segment_capsule<false, false, true>,
1240 _collision_segment_convex_polygon<false, false, true> },
1241 { nullptr,
1242 _collision_circle_circle<false, false, true>,
1243 _collision_circle_rectangle<false, false, true>,
1244 _collision_circle_capsule<false, false, true>,
1245 _collision_circle_convex_polygon<false, false, true> },
1246 { nullptr,
1247 nullptr,
1248 _collision_rectangle_rectangle<false, false, true>,
1249 _collision_rectangle_capsule<false, false, true>,
1250 _collision_rectangle_convex_polygon<false, false, true> },
1251 { nullptr,
1252 nullptr,
1253 nullptr,
1254 _collision_capsule_capsule<false, false, true>,
1255 _collision_capsule_convex_polygon<false, false, true> },
1256 { nullptr,
1257 nullptr,
1258 nullptr,
1259 nullptr,
1260 _collision_convex_polygon_convex_polygon<false, false, true> }
1261
1262 };
1263
1264 static const CollisionFunc collision_table_castA_margin[5][5] = {
1265 { _collision_segment_segment<true, false, true>,
1266 _collision_segment_circle<true, false, true>,
1267 _collision_segment_rectangle<true, false, true>,
1268 _collision_segment_capsule<true, false, true>,
1269 _collision_segment_convex_polygon<true, false, true> },
1270 { nullptr,
1271 _collision_circle_circle<true, false, true>,
1272 _collision_circle_rectangle<true, false, true>,
1273 _collision_circle_capsule<true, false, true>,
1274 _collision_circle_convex_polygon<true, false, true> },
1275 { nullptr,
1276 nullptr,
1277 _collision_rectangle_rectangle<true, false, true>,
1278 _collision_rectangle_capsule<true, false, true>,
1279 _collision_rectangle_convex_polygon<true, false, true> },
1280 { nullptr,
1281 nullptr,
1282 nullptr,
1283 _collision_capsule_capsule<true, false, true>,
1284 _collision_capsule_convex_polygon<true, false, true> },
1285 { nullptr,
1286 nullptr,
1287 nullptr,
1288 nullptr,
1289 _collision_convex_polygon_convex_polygon<true, false, true> }
1290
1291 };
1292
1293 static const CollisionFunc collision_table_castB_margin[5][5] = {
1294 { _collision_segment_segment<false, true, true>,
1295 _collision_segment_circle<false, true, true>,
1296 _collision_segment_rectangle<false, true, true>,
1297 _collision_segment_capsule<false, true, true>,
1298 _collision_segment_convex_polygon<false, true, true> },
1299 { nullptr,
1300 _collision_circle_circle<false, true, true>,
1301 _collision_circle_rectangle<false, true, true>,
1302 _collision_circle_capsule<false, true, true>,
1303 _collision_circle_convex_polygon<false, true, true> },
1304 { nullptr,
1305 nullptr,
1306 _collision_rectangle_rectangle<false, true, true>,
1307 _collision_rectangle_capsule<false, true, true>,
1308 _collision_rectangle_convex_polygon<false, true, true> },
1309 { nullptr,
1310 nullptr,
1311 nullptr,
1312 _collision_capsule_capsule<false, true, true>,
1313 _collision_capsule_convex_polygon<false, true, true> },
1314 { nullptr,
1315 nullptr,
1316 nullptr,
1317 nullptr,
1318 _collision_convex_polygon_convex_polygon<false, true, true> }
1319
1320 };
1321
1322 static const CollisionFunc collision_table_castA_castB_margin[5][5] = {
1323 { _collision_segment_segment<true, true, true>,
1324 _collision_segment_circle<true, true, true>,
1325 _collision_segment_rectangle<true, true, true>,
1326 _collision_segment_capsule<true, true, true>,
1327 _collision_segment_convex_polygon<true, true, true> },
1328 { nullptr,
1329 _collision_circle_circle<true, true, true>,
1330 _collision_circle_rectangle<true, true, true>,
1331 _collision_circle_capsule<true, true, true>,
1332 _collision_circle_convex_polygon<true, true, true> },
1333 { nullptr,
1334 nullptr,
1335 _collision_rectangle_rectangle<true, true, true>,
1336 _collision_rectangle_capsule<true, true, true>,
1337 _collision_rectangle_convex_polygon<true, true, true> },
1338 { nullptr,
1339 nullptr,
1340 nullptr,
1341 _collision_capsule_capsule<true, true, true>,
1342 _collision_capsule_convex_polygon<true, true, true> },
1343 { nullptr,
1344 nullptr,
1345 nullptr,
1346 nullptr,
1347 _collision_convex_polygon_convex_polygon<true, true, true> }
1348
1349 };
1350
1351 _CollectorCallback2D callback;
1352 callback.callback = p_result_callback;
1353 callback.swap = p_swap;
1354 callback.userdata = p_userdata;
1355 callback.collided = false;
1356 callback.sep_axis = sep_axis;
1357
1358 const GodotShape2D *A = p_shape_A;
1359 const GodotShape2D *B = p_shape_B;
1360 const Transform2D *transform_A = &p_transform_A;
1361 const Transform2D *transform_B = &p_transform_B;
1362 const Vector2 *motion_A = &p_motion_A;
1363 const Vector2 *motion_B = &p_motion_B;
1364 real_t margin_A = p_margin_A, margin_B = p_margin_B;
1365
1366 if (type_A > type_B) {
1367 SWAP(A, B);
1368 SWAP(transform_A, transform_B);
1369 SWAP(type_A, type_B);
1370 SWAP(motion_A, motion_B);
1371 SWAP(margin_A, margin_B);
1372 callback.swap = !callback.swap;
1373 }
1374
1375 CollisionFunc collision_func;
1376
1377 if (p_margin_A || p_margin_B) {
1378 if (*motion_A == Vector2() && *motion_B == Vector2()) {
1379 collision_func = collision_table_margin[type_A - 2][type_B - 2];
1380 } else if (*motion_A != Vector2() && *motion_B == Vector2()) {
1381 collision_func = collision_table_castA_margin[type_A - 2][type_B - 2];
1382 } else if (*motion_A == Vector2() && *motion_B != Vector2()) {
1383 collision_func = collision_table_castB_margin[type_A - 2][type_B - 2];
1384 } else {
1385 collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2];
1386 }
1387 } else {
1388 if (*motion_A == Vector2() && *motion_B == Vector2()) {
1389 collision_func = collision_table[type_A - 2][type_B - 2];
1390 } else if (*motion_A != Vector2() && *motion_B == Vector2()) {
1391 collision_func = collision_table_castA[type_A - 2][type_B - 2];
1392 } else if (*motion_A == Vector2() && *motion_B != Vector2()) {
1393 collision_func = collision_table_castB[type_A - 2][type_B - 2];
1394 } else {
1395 collision_func = collision_table_castA_castB[type_A - 2][type_B - 2];
1396 }
1397 }
1398
1399 ERR_FAIL_COND_V(!collision_func, false);
1400
1401 collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B);
1402
1403 return callback.collided;
1404}
1405