1/**************************************************************************/
2/* godot_collision_solver_3d_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_3d_sat.h"
32
33#include "gjk_epa.h"
34
35#include "core/math/geometry_3d.h"
36
37#define fallback_collision_solver gjk_epa_calculate_penetration
38
39#define _BACKFACE_NORMAL_THRESHOLD -0.0002
40
41// Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders.
42
43/*
44 * Cylinder-trimesh and Cylinder-box colliders by Alen Ladavac
45 * Ported to ODE by Nguyen Binh
46 */
47
48/*************************************************************************
49 * *
50 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
51 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
52 * *
53 * This library is free software; you can redistribute it and/or *
54 * modify it under the terms of EITHER: *
55 * (1) The GNU Lesser General Public License as published by the Free *
56 * Software Foundation; either version 2.1 of the License, or (at *
57 * your option) any later version. The text of the GNU Lesser *
58 * General Public License is included with this library in the *
59 * file LICENSE.TXT. *
60 * (2) The BSD-style license that is included with this library in *
61 * the file LICENSE-BSD.TXT. *
62 * *
63 * This library is distributed in the hope that it will be useful, *
64 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
65 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
66 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
67 * *
68 *************************************************************************/
69
70struct _CollectorCallback {
71 GodotCollisionSolver3D::CallbackResult callback = nullptr;
72 void *userdata = nullptr;
73 bool swap = false;
74 bool collided = false;
75 Vector3 normal;
76 Vector3 *prev_axis = nullptr;
77
78 _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B, Vector3 p_normal) {
79 if (p_normal.dot(p_point_B - p_point_A) < 0)
80 p_normal = -p_normal;
81 if (swap) {
82 callback(p_point_B, 0, p_point_A, 0, -p_normal, userdata);
83 } else {
84 callback(p_point_A, 0, p_point_B, 0, p_normal, userdata);
85 }
86 }
87};
88
89typedef void (*GenerateContactsFunc)(const Vector3 *, int, const Vector3 *, int, _CollectorCallback *);
90
91static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
92#ifdef DEBUG_ENABLED
93 ERR_FAIL_COND(p_point_count_A != 1);
94 ERR_FAIL_COND(p_point_count_B != 1);
95#endif
96
97 p_callback->call(*p_points_A, *p_points_B, p_callback->normal);
98}
99
100static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
101#ifdef DEBUG_ENABLED
102 ERR_FAIL_COND(p_point_count_A != 1);
103 ERR_FAIL_COND(p_point_count_B != 2);
104#endif
105
106 Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B);
107 p_callback->call(*p_points_A, closest_B, p_callback->normal);
108}
109
110static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
111#ifdef DEBUG_ENABLED
112 ERR_FAIL_COND(p_point_count_A != 1);
113 ERR_FAIL_COND(p_point_count_B < 3);
114#endif
115
116 Plane plane(p_points_B[0], p_points_B[1], p_points_B[2]);
117 Vector3 closest_B = plane.project(*p_points_A);
118 p_callback->call(*p_points_A, closest_B, plane.get_normal());
119}
120
121static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
122#ifdef DEBUG_ENABLED
123 ERR_FAIL_COND(p_point_count_A != 1);
124 ERR_FAIL_COND(p_point_count_B != 3);
125#endif
126
127 Plane plane(p_points_B[0], p_points_B[1], p_points_B[2]);
128 Vector3 closest_B = plane.project(*p_points_A);
129 p_callback->call(*p_points_A, closest_B, plane.get_normal());
130}
131
132static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
133#ifdef DEBUG_ENABLED
134 ERR_FAIL_COND(p_point_count_A != 2);
135 ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix
136#endif
137
138 Vector3 rel_A = p_points_A[1] - p_points_A[0];
139 Vector3 rel_B = p_points_B[1] - p_points_B[0];
140
141 Vector3 c = rel_A.cross(rel_B).cross(rel_B);
142
143 if (Math::is_zero_approx(rel_A.dot(c))) {
144 // should handle somehow..
145 //ERR_PRINT("TODO FIX");
146 //return;
147
148 Vector3 axis = rel_A.normalized(); //make an axis
149 Vector3 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]);
150 Vector3 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]);
151
152 //sort all 4 points in axis
153 real_t dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) };
154
155 SortArray<real_t> sa;
156 sa.sort(dvec, 4);
157
158 //use the middle ones as contacts
159 p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1], p_callback->normal);
160 p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2], p_callback->normal);
161
162 return;
163 }
164
165 real_t d = (c.dot(p_points_B[0]) - p_points_A[0].dot(c)) / rel_A.dot(c);
166
167 if (d < 0.0) {
168 d = 0.0;
169 } else if (d > 1.0) {
170 d = 1.0;
171 }
172
173 Vector3 closest_A = p_points_A[0] + rel_A * d;
174 Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(closest_A, p_points_B);
175 // The normal should be perpendicular to both edges.
176 Vector3 normal = rel_A.cross(rel_B);
177 real_t normal_len = normal.length();
178 if (normal_len > 1e-3)
179 normal /= normal_len;
180 else
181 normal = p_callback->normal;
182 p_callback->call(closest_A, closest_B, normal);
183}
184
185static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
186#ifdef DEBUG_ENABLED
187 ERR_FAIL_COND(p_point_count_A != 2);
188 ERR_FAIL_COND(p_point_count_B != 3);
189#endif
190
191 const Vector3 &circle_B_pos = p_points_B[0];
192 Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos;
193 Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos;
194
195 real_t circle_B_radius = circle_B_line_1.length();
196 Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
197
198 Plane circle_plane(circle_B_normal, circle_B_pos);
199
200 static const int max_clip = 2;
201 Vector3 contact_points[max_clip];
202 int num_points = 0;
203
204 // Project edge point in circle plane.
205 const Vector3 &edge_A_1 = p_points_A[0];
206 Vector3 proj_point_1 = circle_plane.project(edge_A_1);
207
208 Vector3 dist_vec = proj_point_1 - circle_B_pos;
209 real_t dist_sq = dist_vec.length_squared();
210
211 // Point 1 is inside disk, add as contact point.
212 if (dist_sq <= circle_B_radius * circle_B_radius) {
213 contact_points[num_points] = edge_A_1;
214 ++num_points;
215 }
216
217 const Vector3 &edge_A_2 = p_points_A[1];
218 Vector3 proj_point_2 = circle_plane.project(edge_A_2);
219
220 Vector3 dist_vec_2 = proj_point_2 - circle_B_pos;
221 real_t dist_sq_2 = dist_vec_2.length_squared();
222
223 // Point 2 is inside disk, add as contact point.
224 if (dist_sq_2 <= circle_B_radius * circle_B_radius) {
225 contact_points[num_points] = edge_A_2;
226 ++num_points;
227 }
228
229 if (num_points < 2) {
230 Vector3 line_vec = proj_point_2 - proj_point_1;
231 real_t line_length_sq = line_vec.length_squared();
232
233 // Create a quadratic formula of the form ax^2 + bx + c = 0
234 real_t a, b, c;
235
236 a = line_length_sq;
237 b = 2.0 * dist_vec.dot(line_vec);
238 c = dist_sq - circle_B_radius * circle_B_radius;
239
240 // Solve for t.
241 real_t sqrtterm = b * b - 4.0 * a * c;
242
243 // If the term we intend to square root is less than 0 then the answer won't be real,
244 // so the line doesn't intersect.
245 if (sqrtterm >= 0) {
246 sqrtterm = Math::sqrt(sqrtterm);
247
248 Vector3 edge_dir = edge_A_2 - edge_A_1;
249
250 real_t fraction_1 = (-b - sqrtterm) / (2.0 * a);
251 if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) {
252 Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir;
253 ERR_FAIL_COND(num_points >= max_clip);
254 contact_points[num_points] = face_point_1;
255 ++num_points;
256 }
257
258 real_t fraction_2 = (-b + sqrtterm) / (2.0 * a);
259 if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) {
260 Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir;
261 ERR_FAIL_COND(num_points >= max_clip);
262 contact_points[num_points] = face_point_2;
263 ++num_points;
264 }
265 }
266 }
267
268 // Generate contact points.
269 for (int i = 0; i < num_points; i++) {
270 const Vector3 &contact_point_A = contact_points[i];
271
272 real_t d = circle_plane.distance_to(contact_point_A);
273 Vector3 closest_B = contact_point_A - circle_plane.normal * d;
274
275 if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) {
276 continue;
277 }
278
279 p_callback->call(contact_point_A, closest_B, circle_plane.get_normal());
280 }
281}
282
283static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
284#ifdef DEBUG_ENABLED
285 ERR_FAIL_COND(p_point_count_A < 2);
286 ERR_FAIL_COND(p_point_count_B < 3);
287#endif
288
289 static const int max_clip = 32;
290
291 Vector3 _clipbuf1[max_clip];
292 Vector3 _clipbuf2[max_clip];
293 Vector3 *clipbuf_src = _clipbuf1;
294 Vector3 *clipbuf_dst = _clipbuf2;
295 int clipbuf_len = p_point_count_A;
296
297 // copy A points to clipbuf_src
298 for (int i = 0; i < p_point_count_A; i++) {
299 clipbuf_src[i] = p_points_A[i];
300 }
301
302 Plane plane_B(p_points_B[0], p_points_B[1], p_points_B[2]);
303
304 // go through all of B points
305 for (int i = 0; i < p_point_count_B; i++) {
306 int i_n = (i + 1) % p_point_count_B;
307
308 Vector3 edge0_B = p_points_B[i];
309 Vector3 edge1_B = p_points_B[i_n];
310
311 Vector3 clip_normal = (edge0_B - edge1_B).cross(plane_B.normal).normalized();
312 // make a clip plane
313
314 Plane clip(clip_normal, edge0_B);
315 // avoid double clip if A is edge
316 int dst_idx = 0;
317 bool edge = clipbuf_len == 2;
318 for (int j = 0; j < clipbuf_len; j++) {
319 int j_n = (j + 1) % clipbuf_len;
320
321 Vector3 edge0_A = clipbuf_src[j];
322 Vector3 edge1_A = clipbuf_src[j_n];
323
324 real_t dist0 = clip.distance_to(edge0_A);
325 real_t dist1 = clip.distance_to(edge1_A);
326
327 if (dist0 <= 0) { // behind plane
328
329 ERR_FAIL_COND(dst_idx >= max_clip);
330 clipbuf_dst[dst_idx++] = clipbuf_src[j];
331 }
332
333 // check for different sides and non coplanar
334 //if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) {
335 if ((dist0 * dist1) < 0 && !(edge && j)) {
336 // calculate intersection
337 Vector3 rel = edge1_A - edge0_A;
338 real_t den = clip.normal.dot(rel);
339 real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den;
340 Vector3 inters = edge0_A + rel * dist;
341
342 ERR_FAIL_COND(dst_idx >= max_clip);
343 clipbuf_dst[dst_idx] = inters;
344 dst_idx++;
345 }
346 }
347
348 clipbuf_len = dst_idx;
349 SWAP(clipbuf_src, clipbuf_dst);
350 }
351
352 // generate contacts
353 //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]);
354
355 for (int i = 0; i < clipbuf_len; i++) {
356 real_t d = plane_B.distance_to(clipbuf_src[i]);
357
358 Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d;
359
360 if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) {
361 continue;
362 }
363
364 p_callback->call(clipbuf_src[i], closest_B, plane_B.get_normal());
365 }
366}
367
368static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
369#ifdef DEBUG_ENABLED
370 ERR_FAIL_COND(p_point_count_A < 3);
371 ERR_FAIL_COND(p_point_count_B != 3);
372#endif
373
374 const Vector3 &circle_B_pos = p_points_B[0];
375 Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos;
376 Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos;
377
378 // Clip face with circle segments.
379 static const int circle_segments = 8;
380 Vector3 circle_points[circle_segments];
381
382 real_t angle_delta = 2.0 * Math_PI / circle_segments;
383
384 for (int i = 0; i < circle_segments; ++i) {
385 Vector3 point_pos = circle_B_pos;
386 point_pos += circle_B_line_1 * Math::cos(i * angle_delta);
387 point_pos += circle_B_line_2 * Math::sin(i * angle_delta);
388 circle_points[i] = point_pos;
389 }
390
391 _generate_contacts_face_face(p_points_A, p_point_count_A, circle_points, circle_segments, p_callback);
392
393 // Clip face with circle plane.
394 Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
395
396 Plane circle_plane(circle_B_normal, circle_B_pos);
397
398 static const int max_clip = 32;
399 Vector3 contact_points[max_clip];
400 int num_points = 0;
401
402 for (int i = 0; i < p_point_count_A; i++) {
403 int i_n = (i + 1) % p_point_count_A;
404
405 const Vector3 &edge0_A = p_points_A[i];
406 const Vector3 &edge1_A = p_points_A[i_n];
407
408 real_t dist0 = circle_plane.distance_to(edge0_A);
409 real_t dist1 = circle_plane.distance_to(edge1_A);
410
411 // First point in front of plane, generate contact point.
412 if (dist0 * circle_plane.d >= 0) {
413 ERR_FAIL_COND(num_points >= max_clip);
414 contact_points[num_points] = edge0_A;
415 ++num_points;
416 }
417
418 // Points on different sides, generate contact point.
419 if (dist0 * dist1 < 0) {
420 // calculate intersection
421 Vector3 rel = edge1_A - edge0_A;
422 real_t den = circle_plane.normal.dot(rel);
423 real_t dist = -(circle_plane.normal.dot(edge0_A) - circle_plane.d) / den;
424 Vector3 inters = edge0_A + rel * dist;
425
426 ERR_FAIL_COND(num_points >= max_clip);
427 contact_points[num_points] = inters;
428 ++num_points;
429 }
430 }
431
432 // Generate contact points.
433 for (int i = 0; i < num_points; i++) {
434 const Vector3 &contact_point_A = contact_points[i];
435
436 real_t d = circle_plane.distance_to(contact_point_A);
437 Vector3 closest_B = contact_point_A - circle_plane.normal * d;
438
439 if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) {
440 continue;
441 }
442
443 p_callback->call(contact_point_A, closest_B, circle_plane.get_normal());
444 }
445}
446
447static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
448#ifdef DEBUG_ENABLED
449 ERR_FAIL_COND(p_point_count_A != 3);
450 ERR_FAIL_COND(p_point_count_B != 3);
451#endif
452
453 const Vector3 &circle_A_pos = p_points_A[0];
454 Vector3 circle_A_line_1 = p_points_A[1] - circle_A_pos;
455 Vector3 circle_A_line_2 = p_points_A[2] - circle_A_pos;
456
457 real_t circle_A_radius = circle_A_line_1.length();
458 Vector3 circle_A_normal = circle_A_line_1.cross(circle_A_line_2).normalized();
459
460 const Vector3 &circle_B_pos = p_points_B[0];
461 Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos;
462 Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos;
463
464 real_t circle_B_radius = circle_B_line_1.length();
465 Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
466
467 static const int max_clip = 4;
468 Vector3 contact_points[max_clip];
469 int num_points = 0;
470
471 Vector3 centers_diff = circle_B_pos - circle_A_pos;
472 Vector3 norm_proj = circle_A_normal.dot(centers_diff) * circle_A_normal;
473 Vector3 comp_proj = centers_diff - norm_proj;
474 real_t proj_dist = comp_proj.length();
475 if (!Math::is_zero_approx(proj_dist)) {
476 comp_proj /= proj_dist;
477 if ((proj_dist > circle_A_radius - circle_B_radius) && (proj_dist > circle_B_radius - circle_A_radius)) {
478 // Circles are overlapping, use the 2 points of intersection as contacts.
479 real_t radius_a_sqr = circle_A_radius * circle_A_radius;
480 real_t radius_b_sqr = circle_B_radius * circle_B_radius;
481 real_t d_sqr = proj_dist * proj_dist;
482 real_t s = (1.0 + (radius_a_sqr - radius_b_sqr) / d_sqr) * 0.5;
483 real_t h = Math::sqrt(MAX(radius_a_sqr - d_sqr * s * s, 0.0));
484 Vector3 midpoint = circle_A_pos + s * comp_proj * proj_dist;
485 Vector3 h_vec = h * circle_A_normal.cross(comp_proj);
486
487 Vector3 point_A = midpoint + h_vec;
488 contact_points[num_points] = point_A;
489 ++num_points;
490
491 point_A = midpoint - h_vec;
492 contact_points[num_points] = point_A;
493 ++num_points;
494
495 // Add 2 points from circle A and B along the line between the centers.
496 point_A = circle_A_pos + comp_proj * circle_A_radius;
497 contact_points[num_points] = point_A;
498 ++num_points;
499
500 point_A = circle_B_pos - comp_proj * circle_B_radius - norm_proj;
501 contact_points[num_points] = point_A;
502 ++num_points;
503 } // Otherwise one circle is inside the other one, use 3 arbitrary equidistant points.
504 } // Otherwise circles are concentric, use 3 arbitrary equidistant points.
505
506 if (num_points == 0) {
507 // Generate equidistant points.
508 if (circle_A_radius < circle_B_radius) {
509 // Circle A inside circle B.
510 for (int i = 0; i < 3; ++i) {
511 Vector3 circle_A_point = circle_A_pos;
512 circle_A_point += circle_A_line_1 * Math::cos(2.0 * Math_PI * i / 3.0);
513 circle_A_point += circle_A_line_2 * Math::sin(2.0 * Math_PI * i / 3.0);
514
515 contact_points[num_points] = circle_A_point;
516 ++num_points;
517 }
518 } else {
519 // Circle B inside circle A.
520 for (int i = 0; i < 3; ++i) {
521 Vector3 circle_B_point = circle_B_pos;
522 circle_B_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0);
523 circle_B_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0);
524
525 Vector3 circle_A_point = circle_B_point - norm_proj;
526
527 contact_points[num_points] = circle_A_point;
528 ++num_points;
529 }
530 }
531 }
532
533 Plane circle_B_plane(circle_B_normal, circle_B_pos);
534
535 // Generate contact points.
536 for (int i = 0; i < num_points; i++) {
537 const Vector3 &contact_point_A = contact_points[i];
538
539 real_t d = circle_B_plane.distance_to(contact_point_A);
540 Vector3 closest_B = contact_point_A - circle_B_plane.normal * d;
541
542 if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) {
543 continue;
544 }
545
546 p_callback->call(contact_point_A, closest_B, circle_B_plane.get_normal());
547 }
548}
549
550static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, GodotShape3D::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, GodotShape3D::FeatureType p_feature_type_B, _CollectorCallback *p_callback) {
551#ifdef DEBUG_ENABLED
552 ERR_FAIL_COND(p_point_count_A < 1);
553 ERR_FAIL_COND(p_point_count_B < 1);
554#endif
555
556 static const GenerateContactsFunc generate_contacts_func_table[4][4] = {
557 {
558 _generate_contacts_point_point,
559 _generate_contacts_point_edge,
560 _generate_contacts_point_face,
561 _generate_contacts_point_circle,
562 },
563 {
564 nullptr,
565 _generate_contacts_edge_edge,
566 _generate_contacts_face_face,
567 _generate_contacts_edge_circle,
568 },
569 {
570 nullptr,
571 nullptr,
572 _generate_contacts_face_face,
573 _generate_contacts_face_circle,
574 },
575 {
576 nullptr,
577 nullptr,
578 nullptr,
579 _generate_contacts_circle_circle,
580 },
581 };
582
583 int pointcount_B;
584 int pointcount_A;
585 const Vector3 *points_A;
586 const Vector3 *points_B;
587 int version_A;
588 int version_B;
589
590 if (p_feature_type_A > p_feature_type_B) {
591 //swap
592 p_callback->swap = !p_callback->swap;
593 p_callback->normal = -p_callback->normal;
594
595 pointcount_B = p_point_count_A;
596 pointcount_A = p_point_count_B;
597 points_A = p_points_B;
598 points_B = p_points_A;
599 version_A = p_feature_type_B;
600 version_B = p_feature_type_A;
601 } else {
602 pointcount_B = p_point_count_B;
603 pointcount_A = p_point_count_A;
604 points_A = p_points_A;
605 points_B = p_points_B;
606 version_A = p_feature_type_A;
607 version_B = p_feature_type_B;
608 }
609
610 GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
611 ERR_FAIL_COND(!contacts_func);
612 contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback);
613}
614
615template <class ShapeA, class ShapeB, bool withMargin = false>
616class SeparatorAxisTest {
617 const ShapeA *shape_A = nullptr;
618 const ShapeB *shape_B = nullptr;
619 const Transform3D *transform_A = nullptr;
620 const Transform3D *transform_B = nullptr;
621 real_t best_depth = 1e15;
622 _CollectorCallback *callback = nullptr;
623 real_t margin_A = 0.0;
624 real_t margin_B = 0.0;
625 Vector3 separator_axis;
626
627public:
628 Vector3 best_axis;
629
630 _FORCE_INLINE_ bool test_previous_axis() {
631 if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) {
632 return test_axis(*callback->prev_axis);
633 } else {
634 return true;
635 }
636 }
637
638 _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) {
639 Vector3 axis = p_axis;
640
641 if (axis.is_zero_approx()) {
642 // strange case, try an upwards separator
643 axis = Vector3(0.0, 1.0, 0.0);
644 }
645
646 real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0;
647
648 shape_A->project_range(axis, *transform_A, min_A, max_A);
649 shape_B->project_range(axis, *transform_B, min_B, max_B);
650
651 if (withMargin) {
652 min_A -= margin_A;
653 max_A += margin_A;
654 min_B -= margin_B;
655 max_B += margin_B;
656 }
657
658 min_B -= (max_A - min_A) * 0.5;
659 max_B += (max_A - min_A) * 0.5;
660
661 min_B -= (min_A + max_A) * 0.5;
662 max_B -= (min_A + max_A) * 0.5;
663
664 if (min_B > 0.0 || max_B < 0.0) {
665 separator_axis = axis;
666 return false; // doesn't contain 0
667 }
668
669 //use the smallest depth
670
671 if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0
672 min_B = -min_B;
673 }
674
675 if (max_B < min_B) {
676 if (max_B < best_depth) {
677 best_depth = max_B;
678 best_axis = axis;
679 }
680 } else {
681 if (min_B < best_depth) {
682 best_depth = min_B;
683 best_axis = -axis; // keep it as A axis
684 }
685 }
686
687 return true;
688 }
689
690 static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
691 SeparatorAxisTest<ShapeA, ShapeB, withMargin> *separator = (SeparatorAxisTest<ShapeA, ShapeB, withMargin> *)p_userdata;
692 Vector3 axis = (p_point_B - p_point_A);
693 real_t depth = axis.length();
694
695 // Filter out bogus directions with a threshold and re-testing axis.
696 if (separator->best_depth - depth > 0.001) {
697 separator->test_axis(axis / depth);
698 }
699 }
700
701 _FORCE_INLINE_ void generate_contacts() {
702 // nothing to do, don't generate
703 if (best_axis == Vector3(0.0, 0.0, 0.0)) {
704 return;
705 }
706
707 if (!callback->callback) {
708 //just was checking intersection?
709 callback->collided = true;
710 if (callback->prev_axis) {
711 *callback->prev_axis = best_axis;
712 }
713 return;
714 }
715
716 static const int max_supports = 16;
717
718 Vector3 supports_A[max_supports];
719 int support_count_A;
720 GodotShape3D::FeatureType support_type_A;
721 shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A);
722 for (int i = 0; i < support_count_A; i++) {
723 supports_A[i] = transform_A->xform(supports_A[i]);
724 }
725
726 if (withMargin) {
727 for (int i = 0; i < support_count_A; i++) {
728 supports_A[i] += -best_axis * margin_A;
729 }
730 }
731
732 Vector3 supports_B[max_supports];
733 int support_count_B;
734 GodotShape3D::FeatureType support_type_B;
735 shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B);
736 for (int i = 0; i < support_count_B; i++) {
737 supports_B[i] = transform_B->xform(supports_B[i]);
738 }
739
740 if (withMargin) {
741 for (int i = 0; i < support_count_B; i++) {
742 supports_B[i] += best_axis * margin_B;
743 }
744 }
745
746 callback->normal = best_axis;
747 if (callback->prev_axis) {
748 *callback->prev_axis = best_axis;
749 }
750 _generate_contacts_from_supports(supports_A, support_count_A, support_type_A, supports_B, support_count_B, support_type_B, callback);
751
752 callback->collided = true;
753 }
754
755 _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform3D &p_transform_A, const ShapeB *p_shape_B, const Transform3D &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) {
756 shape_A = p_shape_A;
757 shape_B = p_shape_B;
758 transform_A = &p_transform_A;
759 transform_B = &p_transform_B;
760 callback = p_callback;
761 margin_A = p_margin_A;
762 margin_B = p_margin_B;
763 }
764};
765
766/****** SAT TESTS *******/
767
768typedef void (*CollisionFunc)(const GodotShape3D *, const Transform3D &, const GodotShape3D *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t);
769
770// Perform analytic sphere-sphere collision and report results to collector
771template <bool withMargin>
772static void analytic_sphere_collision(const Vector3 &p_origin_a, real_t p_radius_a, const Vector3 &p_origin_b, real_t p_radius_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
773 // Expand the spheres by the margins if enabled
774 if (withMargin) {
775 p_radius_a += p_margin_a;
776 p_radius_b += p_margin_b;
777 }
778
779 // Get the vector from sphere B to A
780 Vector3 b_to_a = p_origin_a - p_origin_b;
781
782 // Get the length from B to A
783 real_t b_to_a_len = b_to_a.length();
784
785 // Calculate the sphere overlap, and bail if not overlapping
786 real_t overlap = p_radius_a + p_radius_b - b_to_a_len;
787 if (overlap < 0)
788 return;
789
790 // Report collision
791 p_collector->collided = true;
792
793 // Bail if there is no callback to receive the A and B collision points.
794 if (!p_collector->callback) {
795 return;
796 }
797
798 // Normalize the B to A vector
799 if (b_to_a_len < CMP_EPSILON) {
800 b_to_a = Vector3(0, 1, 0); // Spheres coincident, use arbitrary direction
801 } else {
802 b_to_a /= b_to_a_len;
803 }
804
805 // Report collision points. The operations below are intended to minimize
806 // floating-point precision errors. This is done by calculating the first
807 // collision point from the smaller sphere, and then jumping across to
808 // the larger spheres collision point using the overlap distance. This
809 // jump is usually small even if the large sphere is massive, and so the
810 // second point will not suffer from precision errors.
811 if (p_radius_a < p_radius_b) {
812 Vector3 point_a = p_origin_a - b_to_a * p_radius_a;
813 Vector3 point_b = point_a + b_to_a * overlap;
814 p_collector->call(point_a, point_b, b_to_a); // Consider adding b_to_a vector
815 } else {
816 Vector3 point_b = p_origin_b + b_to_a * p_radius_b;
817 Vector3 point_a = point_b - b_to_a * overlap;
818 p_collector->call(point_a, point_b, b_to_a); // Consider adding b_to_a vector
819 }
820}
821
822template <bool withMargin>
823static void _collision_sphere_sphere(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
824 const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
825 const GodotSphereShape3D *sphere_B = static_cast<const GodotSphereShape3D *>(p_b);
826
827 // Perform an analytic sphere collision between the two spheres
828 analytic_sphere_collision<withMargin>(
829 p_transform_a.origin,
830 sphere_A->get_radius() * p_transform_a.basis[0].length(),
831 p_transform_b.origin,
832 sphere_B->get_radius() * p_transform_b.basis[0].length(),
833 p_collector,
834 p_margin_a,
835 p_margin_b);
836}
837
838template <bool withMargin>
839static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
840 const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
841 const GodotBoxShape3D *box_B = static_cast<const GodotBoxShape3D *>(p_b);
842
843 // Find the point on the box nearest to the center of the sphere.
844
845 Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin);
846 Vector3 extents = box_B->get_half_extents();
847 Vector3 nearest(MIN(MAX(center.x, -extents.x), extents.x),
848 MIN(MAX(center.y, -extents.y), extents.y),
849 MIN(MAX(center.z, -extents.z), extents.z));
850 nearest = p_transform_b.xform(nearest);
851
852 // See if it is inside the sphere.
853
854 Vector3 delta = nearest - p_transform_a.origin;
855 real_t length = delta.length();
856 real_t radius = sphere_A->get_radius() * p_transform_a.basis[0].length();
857 if (length > radius + p_margin_a + p_margin_b) {
858 return;
859 }
860 p_collector->collided = true;
861 if (!p_collector->callback) {
862 return;
863 }
864 Vector3 axis;
865 if (length == 0) {
866 // The box passes through the sphere center. Select an axis based on the box's center.
867 axis = (p_transform_b.origin - nearest).normalized();
868 } else {
869 axis = delta / length;
870 }
871 Vector3 point_a = p_transform_a.origin + (radius + p_margin_a) * axis;
872 Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
873 p_collector->call(point_a, point_b, axis);
874}
875
876template <bool withMargin>
877static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
878 const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
879 const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
880
881 real_t scale_A = p_transform_a.basis[0].length();
882 real_t scale_B = p_transform_b.basis[0].length();
883
884 // Construct the capsule segment (ball-center to ball-center)
885 Vector3 capsule_segment[2];
886 Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
887 capsule_segment[0] = p_transform_b.origin + capsule_axis;
888 capsule_segment[1] = p_transform_b.origin - capsule_axis;
889
890 // Get the capsules closest segment-point to the sphere
891 Vector3 capsule_closest = Geometry3D::get_closest_point_to_segment(p_transform_a.origin, capsule_segment);
892
893 // Perform an analytic sphere collision between the sphere and the sphere-collider in the capsule
894 analytic_sphere_collision<withMargin>(
895 p_transform_a.origin,
896 sphere_A->get_radius() * scale_A,
897 capsule_closest,
898 capsule_B->get_radius() * scale_B,
899 p_collector,
900 p_margin_a,
901 p_margin_b);
902}
903
904template <bool withMargin>
905static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radius_b, real_t p_height_b, const Transform3D &p_transform_a, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
906 // Find the point on the cylinder nearest to the center of the sphere.
907
908 Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin);
909 Vector3 nearest = center;
910 real_t scale_A = p_transform_a.basis[0].length();
911 real_t r = Math::sqrt(center.x * center.x + center.z * center.z);
912 if (r > p_radius_b) {
913 real_t scale = p_radius_b / r;
914 nearest.x *= scale;
915 nearest.z *= scale;
916 }
917 real_t half_height = p_height_b / 2;
918 nearest.y = MIN(MAX(center.y, -half_height), half_height);
919 nearest = p_transform_b.xform(nearest);
920
921 // See if it is inside the sphere.
922
923 Vector3 delta = nearest - p_transform_a.origin;
924 real_t length = delta.length();
925 if (length > p_radius_a * scale_A + p_margin_a + p_margin_b) {
926 return;
927 }
928 p_collector->collided = true;
929 if (!p_collector->callback) {
930 return;
931 }
932 Vector3 axis;
933 if (length == 0) {
934 // The cylinder passes through the sphere center. Select an axis based on the cylinder's center.
935 axis = (p_transform_b.origin - nearest).normalized();
936 } else {
937 axis = delta / length;
938 }
939 Vector3 point_a = p_transform_a.origin + (p_radius_a * scale_A + p_margin_a) * axis;
940 Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
941 p_collector->call(point_a, point_b, axis);
942}
943
944template <bool withMargin>
945static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
946 const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
947 const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
948
949 analytic_sphere_cylinder_collision<withMargin>(sphere_A->get_radius(), cylinder_B->get_radius(), cylinder_B->get_height(), p_transform_a, p_transform_b, p_collector, p_margin_a, p_margin_b);
950}
951
952template <bool withMargin>
953static void _collision_sphere_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
954 const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
955 const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
956
957 SeparatorAxisTest<GodotSphereShape3D, GodotConvexPolygonShape3D, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
958
959 if (!separator.test_previous_axis()) {
960 return;
961 }
962
963 const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh();
964
965 const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
966 int face_count = mesh.faces.size();
967 const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr();
968 int edge_count = mesh.edges.size();
969 const Vector3 *vertices = mesh.vertices.ptr();
970 int vertex_count = mesh.vertices.size();
971
972 // Precalculating this makes the transforms faster.
973 Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
974
975 // faces of B
976 for (int i = 0; i < face_count; i++) {
977 Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized();
978
979 if (!separator.test_axis(axis)) {
980 return;
981 }
982 }
983
984 // edges of B
985 for (int i = 0; i < edge_count; i++) {
986 Vector3 v1 = p_transform_b.xform(vertices[edges[i].vertex_a]);
987 Vector3 v2 = p_transform_b.xform(vertices[edges[i].vertex_b]);
988 Vector3 v3 = p_transform_a.origin;
989
990 Vector3 n1 = v2 - v1;
991 Vector3 n2 = v2 - v3;
992
993 Vector3 axis = n1.cross(n2).cross(n1).normalized();
994
995 if (!separator.test_axis(axis)) {
996 return;
997 }
998 }
999
1000 // vertices of B
1001 for (int i = 0; i < vertex_count; i++) {
1002 Vector3 v1 = p_transform_b.xform(vertices[i]);
1003 Vector3 v2 = p_transform_a.origin;
1004
1005 Vector3 axis = (v2 - v1).normalized();
1006
1007 if (!separator.test_axis(axis)) {
1008 return;
1009 }
1010 }
1011
1012 separator.generate_contacts();
1013}
1014
1015template <bool withMargin>
1016static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1017 const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
1018 const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
1019
1020 SeparatorAxisTest<GodotSphereShape3D, GodotFaceShape3D, withMargin> separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1021
1022 Vector3 vertex[3] = {
1023 p_transform_b.xform(face_B->vertex[0]),
1024 p_transform_b.xform(face_B->vertex[1]),
1025 p_transform_b.xform(face_B->vertex[2]),
1026 };
1027
1028 Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
1029
1030 if (!separator.test_axis(normal)) {
1031 return;
1032 }
1033
1034 // edges and points of B
1035 for (int i = 0; i < 3; i++) {
1036 Vector3 n1 = vertex[i] - p_transform_a.origin;
1037 if (n1.dot(normal) < 0.0) {
1038 n1 *= -1.0;
1039 }
1040
1041 if (!separator.test_axis(n1.normalized())) {
1042 return;
1043 }
1044
1045 Vector3 n2 = vertex[(i + 1) % 3] - vertex[i];
1046
1047 Vector3 axis = n1.cross(n2).cross(n2).normalized();
1048 if (axis.dot(normal) < 0.0) {
1049 axis *= -1.0;
1050 }
1051
1052 if (!separator.test_axis(axis)) {
1053 return;
1054 }
1055 }
1056
1057 if (!face_B->backface_collision) {
1058 if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
1059 if (face_B->invert_backface_collision) {
1060 separator.best_axis = separator.best_axis.bounce(normal);
1061 } else {
1062 // Just ignore backface collision.
1063 return;
1064 }
1065 }
1066 }
1067
1068 separator.generate_contacts();
1069}
1070
1071template <bool withMargin>
1072static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1073 const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
1074 const GodotBoxShape3D *box_B = static_cast<const GodotBoxShape3D *>(p_b);
1075
1076 SeparatorAxisTest<GodotBoxShape3D, GodotBoxShape3D, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1077
1078 if (!separator.test_previous_axis()) {
1079 return;
1080 }
1081
1082 // test faces of A
1083
1084 for (int i = 0; i < 3; i++) {
1085 Vector3 axis = p_transform_a.basis.get_column(i).normalized();
1086
1087 if (!separator.test_axis(axis)) {
1088 return;
1089 }
1090 }
1091
1092 // test faces of B
1093
1094 for (int i = 0; i < 3; i++) {
1095 Vector3 axis = p_transform_b.basis.get_column(i).normalized();
1096
1097 if (!separator.test_axis(axis)) {
1098 return;
1099 }
1100 }
1101
1102 // test combined edges
1103 for (int i = 0; i < 3; i++) {
1104 for (int j = 0; j < 3; j++) {
1105 Vector3 axis = p_transform_a.basis.get_column(i).cross(p_transform_b.basis.get_column(j));
1106
1107 if (Math::is_zero_approx(axis.length_squared())) {
1108 continue;
1109 }
1110 axis.normalize();
1111
1112 if (!separator.test_axis(axis)) {
1113 return;
1114 }
1115 }
1116 }
1117
1118 if (withMargin) {
1119 //add endpoint test between closest vertices and edges
1120
1121 // calculate closest point to sphere
1122
1123 Vector3 ab_vec = p_transform_b.origin - p_transform_a.origin;
1124
1125 Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec);
1126
1127 Vector3 support_a = p_transform_a.xform(Vector3(
1128
1129 (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
1130 (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
1131 (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z));
1132
1133 Vector3 cnormal_b = p_transform_b.basis.xform_inv(-ab_vec);
1134
1135 Vector3 support_b = p_transform_b.xform(Vector3(
1136
1137 (cnormal_b.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x,
1138 (cnormal_b.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y,
1139 (cnormal_b.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z));
1140
1141 Vector3 axis_ab = (support_a - support_b);
1142
1143 if (!separator.test_axis(axis_ab.normalized())) {
1144 return;
1145 }
1146
1147 //now try edges, which become cylinders!
1148
1149 for (int i = 0; i < 3; i++) {
1150 //a ->b
1151 Vector3 axis_a = p_transform_a.basis.get_column(i);
1152
1153 if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) {
1154 return;
1155 }
1156
1157 //b ->a
1158 Vector3 axis_b = p_transform_b.basis.get_column(i);
1159
1160 if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) {
1161 return;
1162 }
1163 }
1164 }
1165
1166 separator.generate_contacts();
1167}
1168
1169template <bool withMargin>
1170static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1171 const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
1172 const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
1173
1174 SeparatorAxisTest<GodotBoxShape3D, GodotCapsuleShape3D, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1175
1176 if (!separator.test_previous_axis()) {
1177 return;
1178 }
1179
1180 // faces of A
1181 for (int i = 0; i < 3; i++) {
1182 Vector3 axis = p_transform_a.basis.get_column(i).normalized();
1183
1184 if (!separator.test_axis(axis)) {
1185 return;
1186 }
1187 }
1188
1189 Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized();
1190
1191 // edges of A, capsule cylinder
1192
1193 for (int i = 0; i < 3; i++) {
1194 // cylinder
1195 Vector3 box_axis = p_transform_a.basis.get_column(i);
1196 Vector3 axis = box_axis.cross(cyl_axis);
1197 if (Math::is_zero_approx(axis.length_squared())) {
1198 continue;
1199 }
1200
1201 if (!separator.test_axis(axis.normalized())) {
1202 return;
1203 }
1204 }
1205
1206 // points of A, capsule cylinder
1207 // this sure could be made faster somehow..
1208
1209 for (int i = 0; i < 2; i++) {
1210 for (int j = 0; j < 2; j++) {
1211 for (int k = 0; k < 2; k++) {
1212 Vector3 he = box_A->get_half_extents();
1213 he.x *= (i * 2 - 1);
1214 he.y *= (j * 2 - 1);
1215 he.z *= (k * 2 - 1);
1216 Vector3 point = p_transform_a.origin;
1217 for (int l = 0; l < 3; l++) {
1218 point += p_transform_a.basis.get_column(l) * he[l];
1219 }
1220
1221 //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized();
1222 Vector3 axis = Plane(cyl_axis).project(point).normalized();
1223
1224 if (!separator.test_axis(axis)) {
1225 return;
1226 }
1227 }
1228 }
1229 }
1230
1231 // capsule balls, edges of A
1232
1233 for (int i = 0; i < 2; i++) {
1234 Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
1235
1236 Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis);
1237
1238 Vector3 cnormal = p_transform_a.xform_inv(sphere_pos);
1239
1240 Vector3 cpoint = p_transform_a.xform(Vector3(
1241
1242 (cnormal.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
1243 (cnormal.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
1244 (cnormal.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z));
1245
1246 // use point to test axis
1247 Vector3 point_axis = (sphere_pos - cpoint).normalized();
1248
1249 if (!separator.test_axis(point_axis)) {
1250 return;
1251 }
1252
1253 // test edges of A
1254
1255 for (int j = 0; j < 3; j++) {
1256 Vector3 axis = point_axis.cross(p_transform_a.basis.get_column(j)).cross(p_transform_a.basis.get_column(j)).normalized();
1257
1258 if (!separator.test_axis(axis)) {
1259 return;
1260 }
1261 }
1262 }
1263
1264 separator.generate_contacts();
1265}
1266
1267template <bool withMargin>
1268static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1269 const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
1270 const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
1271
1272 SeparatorAxisTest<GodotBoxShape3D, GodotCylinderShape3D, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1273
1274 if (!separator.test_previous_axis()) {
1275 return;
1276 }
1277
1278 // Faces of A.
1279 for (int i = 0; i < 3; i++) {
1280 Vector3 axis = p_transform_a.basis.get_column(i).normalized();
1281
1282 if (!separator.test_axis(axis)) {
1283 return;
1284 }
1285 }
1286
1287 Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized();
1288
1289 // Cylinder end caps.
1290 {
1291 if (!separator.test_axis(cyl_axis)) {
1292 return;
1293 }
1294 }
1295
1296 // Edges of A, cylinder lateral surface.
1297 for (int i = 0; i < 3; i++) {
1298 Vector3 box_axis = p_transform_a.basis.get_column(i);
1299 Vector3 axis = box_axis.cross(cyl_axis);
1300 if (Math::is_zero_approx(axis.length_squared())) {
1301 continue;
1302 }
1303
1304 if (!separator.test_axis(axis.normalized())) {
1305 return;
1306 }
1307 }
1308
1309 // Gather points of A.
1310 Vector3 vertices_A[8];
1311 Vector3 box_extent = box_A->get_half_extents();
1312 for (int i = 0; i < 2; i++) {
1313 for (int j = 0; j < 2; j++) {
1314 for (int k = 0; k < 2; k++) {
1315 Vector3 extent = box_extent;
1316 extent.x *= (i * 2 - 1);
1317 extent.y *= (j * 2 - 1);
1318 extent.z *= (k * 2 - 1);
1319 Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k];
1320 point = p_transform_a.origin;
1321 for (int l = 0; l < 3; l++) {
1322 point += p_transform_a.basis.get_column(l) * extent[l];
1323 }
1324 }
1325 }
1326 }
1327
1328 // Points of A, cylinder lateral surface.
1329 for (int i = 0; i < 8; i++) {
1330 const Vector3 &point = vertices_A[i];
1331 Vector3 axis = Plane(cyl_axis).project(point).normalized();
1332
1333 if (!separator.test_axis(axis)) {
1334 return;
1335 }
1336 }
1337
1338 // Edges of A, cylinder end caps rim.
1339 int edges_start_A[12] = { 0, 2, 4, 6, 0, 1, 4, 5, 0, 1, 2, 3 };
1340 int edges_end_A[12] = { 1, 3, 5, 7, 2, 3, 6, 7, 4, 5, 6, 7 };
1341
1342 Vector3 cap_axis = cyl_axis * (cylinder_B->get_height() * 0.5);
1343
1344 for (int i = 0; i < 2; i++) {
1345 Vector3 cap_pos = p_transform_b.origin + ((i == 0) ? cap_axis : -cap_axis);
1346
1347 for (int e = 0; e < 12; e++) {
1348 const Vector3 &edge_start = vertices_A[edges_start_A[e]];
1349 const Vector3 &edge_end = vertices_A[edges_end_A[e]];
1350
1351 Vector3 edge_dir = (edge_end - edge_start);
1352 edge_dir.normalize();
1353
1354 real_t edge_dot = edge_dir.dot(cyl_axis);
1355 if (Math::is_zero_approx(edge_dot)) {
1356 // Edge is perpendicular to cylinder axis.
1357 continue;
1358 }
1359
1360 // Calculate intersection between edge and circle plane.
1361 Vector3 edge_diff = cap_pos - edge_start;
1362 real_t diff_dot = edge_diff.dot(cyl_axis);
1363 Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot;
1364
1365 // Calculate tangent that touches intersection.
1366 Vector3 tangent = (cap_pos - intersection).cross(cyl_axis);
1367
1368 // Axis is orthogonal both to tangent and edge direction.
1369 Vector3 axis = tangent.cross(edge_dir);
1370
1371 if (!separator.test_axis(axis.normalized())) {
1372 return;
1373 }
1374 }
1375 }
1376
1377 separator.generate_contacts();
1378}
1379
1380template <bool withMargin>
1381static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1382 const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
1383 const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
1384
1385 SeparatorAxisTest<GodotBoxShape3D, GodotConvexPolygonShape3D, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1386
1387 if (!separator.test_previous_axis()) {
1388 return;
1389 }
1390
1391 const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh();
1392
1393 const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
1394 int face_count = mesh.faces.size();
1395 const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr();
1396 int edge_count = mesh.edges.size();
1397 const Vector3 *vertices = mesh.vertices.ptr();
1398 int vertex_count = mesh.vertices.size();
1399
1400 // faces of A
1401 for (int i = 0; i < 3; i++) {
1402 Vector3 axis = p_transform_a.basis.get_column(i).normalized();
1403
1404 if (!separator.test_axis(axis)) {
1405 return;
1406 }
1407 }
1408
1409 // Precalculating this makes the transforms faster.
1410 Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
1411
1412 // faces of B
1413 for (int i = 0; i < face_count; i++) {
1414 Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized();
1415
1416 if (!separator.test_axis(axis)) {
1417 return;
1418 }
1419 }
1420
1421 // A<->B edges
1422 for (int i = 0; i < 3; i++) {
1423 Vector3 e1 = p_transform_a.basis.get_column(i);
1424
1425 for (int j = 0; j < edge_count; j++) {
1426 Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]);
1427
1428 Vector3 axis = e1.cross(e2).normalized();
1429
1430 if (!separator.test_axis(axis)) {
1431 return;
1432 }
1433 }
1434 }
1435
1436 if (withMargin) {
1437 // calculate closest points between vertices and box edges
1438 for (int v = 0; v < vertex_count; v++) {
1439 Vector3 vtxb = p_transform_b.xform(vertices[v]);
1440 Vector3 ab_vec = vtxb - p_transform_a.origin;
1441
1442 Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec);
1443
1444 Vector3 support_a = p_transform_a.xform(Vector3(
1445
1446 (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
1447 (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
1448 (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z));
1449
1450 Vector3 axis_ab = support_a - vtxb;
1451
1452 if (!separator.test_axis(axis_ab.normalized())) {
1453 return;
1454 }
1455
1456 //now try edges, which become cylinders!
1457
1458 for (int i = 0; i < 3; i++) {
1459 //a ->b
1460 Vector3 axis_a = p_transform_a.basis.get_column(i);
1461
1462 if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) {
1463 return;
1464 }
1465 }
1466 }
1467
1468 //convex edges and box points
1469 for (int i = 0; i < 2; i++) {
1470 for (int j = 0; j < 2; j++) {
1471 for (int k = 0; k < 2; k++) {
1472 Vector3 he = box_A->get_half_extents();
1473 he.x *= (i * 2 - 1);
1474 he.y *= (j * 2 - 1);
1475 he.z *= (k * 2 - 1);
1476 Vector3 point = p_transform_a.origin;
1477 for (int l = 0; l < 3; l++) {
1478 point += p_transform_a.basis.get_column(l) * he[l];
1479 }
1480
1481 for (int e = 0; e < edge_count; e++) {
1482 Vector3 p1 = p_transform_b.xform(vertices[edges[e].vertex_a]);
1483 Vector3 p2 = p_transform_b.xform(vertices[edges[e].vertex_b]);
1484 Vector3 n = (p2 - p1);
1485
1486 if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) {
1487 return;
1488 }
1489 }
1490 }
1491 }
1492 }
1493 }
1494
1495 separator.generate_contacts();
1496}
1497
1498template <bool withMargin>
1499static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1500 const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
1501 const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
1502
1503 SeparatorAxisTest<GodotBoxShape3D, GodotFaceShape3D, withMargin> separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1504
1505 Vector3 vertex[3] = {
1506 p_transform_b.xform(face_B->vertex[0]),
1507 p_transform_b.xform(face_B->vertex[1]),
1508 p_transform_b.xform(face_B->vertex[2]),
1509 };
1510
1511 Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
1512
1513 if (!separator.test_axis(normal)) {
1514 return;
1515 }
1516
1517 // faces of A
1518 for (int i = 0; i < 3; i++) {
1519 Vector3 axis = p_transform_a.basis.get_column(i).normalized();
1520 if (axis.dot(normal) < 0.0) {
1521 axis *= -1.0;
1522 }
1523
1524 if (!separator.test_axis(axis)) {
1525 return;
1526 }
1527 }
1528
1529 // combined edges
1530
1531 for (int i = 0; i < 3; i++) {
1532 Vector3 e = vertex[i] - vertex[(i + 1) % 3];
1533
1534 for (int j = 0; j < 3; j++) {
1535 Vector3 axis = e.cross(p_transform_a.basis.get_column(j)).normalized();
1536 if (axis.dot(normal) < 0.0) {
1537 axis *= -1.0;
1538 }
1539
1540 if (!separator.test_axis(axis)) {
1541 return;
1542 }
1543 }
1544 }
1545
1546 if (withMargin) {
1547 // calculate closest points between vertices and box edges
1548 for (int v = 0; v < 3; v++) {
1549 Vector3 ab_vec = vertex[v] - p_transform_a.origin;
1550
1551 Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec);
1552
1553 Vector3 support_a = p_transform_a.xform(Vector3(
1554
1555 (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
1556 (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
1557 (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z));
1558
1559 Vector3 axis_ab = support_a - vertex[v];
1560 if (axis_ab.dot(normal) < 0.0) {
1561 axis_ab *= -1.0;
1562 }
1563
1564 if (!separator.test_axis(axis_ab.normalized())) {
1565 return;
1566 }
1567
1568 //now try edges, which become cylinders!
1569
1570 for (int i = 0; i < 3; i++) {
1571 //a ->b
1572 Vector3 axis_a = p_transform_a.basis.get_column(i);
1573
1574 Vector3 axis = axis_ab.cross(axis_a).cross(axis_a).normalized();
1575 if (axis.dot(normal) < 0.0) {
1576 axis *= -1.0;
1577 }
1578
1579 if (!separator.test_axis(axis)) {
1580 return;
1581 }
1582 }
1583 }
1584
1585 //convex edges and box points, there has to be a way to speed up this (get closest point?)
1586 for (int i = 0; i < 2; i++) {
1587 for (int j = 0; j < 2; j++) {
1588 for (int k = 0; k < 2; k++) {
1589 Vector3 he = box_A->get_half_extents();
1590 he.x *= (i * 2 - 1);
1591 he.y *= (j * 2 - 1);
1592 he.z *= (k * 2 - 1);
1593 Vector3 point = p_transform_a.origin;
1594 for (int l = 0; l < 3; l++) {
1595 point += p_transform_a.basis.get_column(l) * he[l];
1596 }
1597
1598 for (int e = 0; e < 3; e++) {
1599 Vector3 p1 = vertex[e];
1600 Vector3 p2 = vertex[(e + 1) % 3];
1601
1602 Vector3 n = (p2 - p1);
1603
1604 Vector3 axis = (point - p2).cross(n).cross(n).normalized();
1605 if (axis.dot(normal) < 0.0) {
1606 axis *= -1.0;
1607 }
1608
1609 if (!separator.test_axis(axis)) {
1610 return;
1611 }
1612 }
1613 }
1614 }
1615 }
1616 }
1617
1618 if (!face_B->backface_collision) {
1619 if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
1620 if (face_B->invert_backface_collision) {
1621 separator.best_axis = separator.best_axis.bounce(normal);
1622 } else {
1623 // Just ignore backface collision.
1624 return;
1625 }
1626 }
1627 }
1628
1629 separator.generate_contacts();
1630}
1631
1632template <bool withMargin>
1633static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1634 const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
1635 const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
1636
1637 real_t scale_A = p_transform_a.basis[0].length();
1638 real_t scale_B = p_transform_b.basis[0].length();
1639
1640 // Get the closest points between the capsule segments
1641 Vector3 capsule_A_closest;
1642 Vector3 capsule_B_closest;
1643 Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
1644 Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
1645 Geometry3D::get_closest_points_between_segments(
1646 p_transform_a.origin + capsule_A_axis,
1647 p_transform_a.origin - capsule_A_axis,
1648 p_transform_b.origin + capsule_B_axis,
1649 p_transform_b.origin - capsule_B_axis,
1650 capsule_A_closest,
1651 capsule_B_closest);
1652
1653 // Perform the analytic collision between the two closest capsule spheres
1654 analytic_sphere_collision<withMargin>(
1655 capsule_A_closest,
1656 capsule_A->get_radius() * scale_A,
1657 capsule_B_closest,
1658 capsule_B->get_radius() * scale_B,
1659 p_collector,
1660 p_margin_a,
1661 p_margin_b);
1662}
1663
1664template <bool withMargin>
1665static void _collision_capsule_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1666 const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
1667 const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
1668
1669 // Find the closest points between the axes of the two objects.
1670
1671 Vector3 capsule_A_closest;
1672 Vector3 cylinder_B_closest;
1673 Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
1674 Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1) * (cylinder_B->get_height() * 0.5);
1675 Geometry3D::get_closest_points_between_segments(
1676 p_transform_a.origin + capsule_A_axis,
1677 p_transform_a.origin - capsule_A_axis,
1678 p_transform_b.origin + cylinder_B_axis,
1679 p_transform_b.origin - cylinder_B_axis,
1680 capsule_A_closest,
1681 cylinder_B_closest);
1682
1683 // Perform the collision test between the cylinder and the nearest sphere on the capsule axis.
1684
1685 Transform3D sphere_transform(p_transform_a.basis, capsule_A_closest);
1686 analytic_sphere_cylinder_collision<withMargin>(capsule_A->get_radius(), cylinder_B->get_radius(), cylinder_B->get_height(), sphere_transform, p_transform_b, p_collector, p_margin_a, p_margin_b);
1687}
1688
1689template <bool withMargin>
1690static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1691 const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
1692 const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
1693
1694 SeparatorAxisTest<GodotCapsuleShape3D, GodotConvexPolygonShape3D, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1695
1696 if (!separator.test_previous_axis()) {
1697 return;
1698 }
1699
1700 const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh();
1701
1702 const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
1703 int face_count = mesh.faces.size();
1704 const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr();
1705 int edge_count = mesh.edges.size();
1706 const Vector3 *vertices = mesh.vertices.ptr();
1707
1708 // Precalculating this makes the transforms faster.
1709 Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
1710
1711 // faces of B
1712 for (int i = 0; i < face_count; i++) {
1713 Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized();
1714
1715 if (!separator.test_axis(axis)) {
1716 return;
1717 }
1718 }
1719
1720 // edges of B, capsule cylinder
1721
1722 for (int i = 0; i < edge_count; i++) {
1723 // cylinder
1724 Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].vertex_a]) - p_transform_b.basis.xform(vertices[edges[i].vertex_b]);
1725 Vector3 axis = edge_axis.cross(p_transform_a.basis.get_column(1)).normalized();
1726
1727 if (!separator.test_axis(axis)) {
1728 return;
1729 }
1730 }
1731
1732 // capsule balls, edges of B
1733
1734 for (int i = 0; i < 2; i++) {
1735 // edges of B, capsule cylinder
1736
1737 Vector3 capsule_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
1738
1739 Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis);
1740
1741 for (int j = 0; j < edge_count; j++) {
1742 Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].vertex_a]);
1743 Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]);
1744
1745 Vector3 axis = n1.cross(n2).cross(n2).normalized();
1746
1747 if (!separator.test_axis(axis)) {
1748 return;
1749 }
1750 }
1751 }
1752
1753 separator.generate_contacts();
1754}
1755
1756template <bool withMargin>
1757static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1758 const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
1759 const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
1760
1761 SeparatorAxisTest<GodotCapsuleShape3D, GodotFaceShape3D, withMargin> separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1762
1763 Vector3 vertex[3] = {
1764 p_transform_b.xform(face_B->vertex[0]),
1765 p_transform_b.xform(face_B->vertex[1]),
1766 p_transform_b.xform(face_B->vertex[2]),
1767 };
1768
1769 Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
1770
1771 if (!separator.test_axis(normal)) {
1772 return;
1773 }
1774
1775 // edges of B, capsule cylinder
1776
1777 Vector3 capsule_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
1778
1779 for (int i = 0; i < 3; i++) {
1780 // edge-cylinder
1781 Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3];
1782
1783 Vector3 axis = edge_axis.cross(capsule_axis).normalized();
1784 if (axis.dot(normal) < 0.0) {
1785 axis *= -1.0;
1786 }
1787
1788 if (!separator.test_axis(axis)) {
1789 return;
1790 }
1791
1792 Vector3 dir_axis = (p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized();
1793 if (dir_axis.dot(normal) < 0.0) {
1794 dir_axis *= -1.0;
1795 }
1796
1797 if (!separator.test_axis(dir_axis)) {
1798 return;
1799 }
1800
1801 for (int j = 0; j < 2; j++) {
1802 // point-spheres
1803 Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis);
1804
1805 Vector3 n1 = sphere_pos - vertex[i];
1806 if (n1.dot(normal) < 0.0) {
1807 n1 *= -1.0;
1808 }
1809
1810 if (!separator.test_axis(n1.normalized())) {
1811 return;
1812 }
1813
1814 Vector3 n2 = edge_axis;
1815
1816 axis = n1.cross(n2).cross(n2);
1817 if (axis.dot(normal) < 0.0) {
1818 axis *= -1.0;
1819 }
1820
1821 if (!separator.test_axis(axis.normalized())) {
1822 return;
1823 }
1824 }
1825 }
1826
1827 if (!face_B->backface_collision) {
1828 if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
1829 if (face_B->invert_backface_collision) {
1830 separator.best_axis = separator.best_axis.bounce(normal);
1831 } else {
1832 // Just ignore backface collision.
1833 return;
1834 }
1835 }
1836 }
1837
1838 separator.generate_contacts();
1839}
1840
1841template <bool withMargin>
1842static void _collision_cylinder_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1843 const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a);
1844 const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
1845
1846 SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1847
1848 Vector3 cylinder_A_axis = p_transform_a.basis.get_column(1);
1849 Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1);
1850
1851 if (!separator.test_previous_axis()) {
1852 return;
1853 }
1854
1855 // Cylinder A end caps.
1856 if (!separator.test_axis(cylinder_A_axis.normalized())) {
1857 return;
1858 }
1859
1860 // Cylinder B end caps.
1861 if (!separator.test_axis(cylinder_B_axis.normalized())) {
1862 return;
1863 }
1864
1865 Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin;
1866
1867 // Cylinder A lateral surface.
1868 if (!separator.test_axis(cylinder_A_axis.cross(cylinder_diff).cross(cylinder_A_axis).normalized())) {
1869 return;
1870 }
1871
1872 // Cylinder B lateral surface.
1873 if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) {
1874 return;
1875 }
1876
1877 real_t proj = cylinder_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(cylinder_A_axis);
1878 if (Math::is_zero_approx(proj)) {
1879 // Parallel cylinders, handle with specific axes only.
1880 // Note: GJKEPA with no margin can lead to degenerate cases in this situation.
1881 separator.generate_contacts();
1882 return;
1883 }
1884
1885 GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin>::test_contact_points;
1886
1887 // Fallback to generic algorithm to find the best separating axis.
1888 if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
1889 return;
1890 }
1891
1892 separator.generate_contacts();
1893}
1894
1895template <bool withMargin>
1896static void _collision_cylinder_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1897 const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a);
1898 const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
1899
1900 SeparatorAxisTest<GodotCylinderShape3D, GodotConvexPolygonShape3D, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1901
1902 GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCylinderShape3D, GodotConvexPolygonShape3D, withMargin>::test_contact_points;
1903
1904 // Fallback to generic algorithm to find the best separating axis.
1905 if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
1906 return;
1907 }
1908
1909 separator.generate_contacts();
1910}
1911
1912template <bool withMargin>
1913static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
1914 const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a);
1915 const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
1916
1917 SeparatorAxisTest<GodotCylinderShape3D, GodotFaceShape3D, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
1918
1919 if (!separator.test_previous_axis()) {
1920 return;
1921 }
1922
1923 Vector3 vertex[3] = {
1924 p_transform_b.xform(face_B->vertex[0]),
1925 p_transform_b.xform(face_B->vertex[1]),
1926 p_transform_b.xform(face_B->vertex[2]),
1927 };
1928
1929 Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
1930
1931 // Face B normal.
1932 if (!separator.test_axis(normal)) {
1933 return;
1934 }
1935
1936 Vector3 cyl_axis = p_transform_a.basis.get_column(1).normalized();
1937 if (cyl_axis.dot(normal) < 0.0) {
1938 cyl_axis *= -1.0;
1939 }
1940
1941 // Cylinder end caps.
1942 if (!separator.test_axis(cyl_axis)) {
1943 return;
1944 }
1945
1946 // Edges of B, cylinder lateral surface.
1947 for (int i = 0; i < 3; i++) {
1948 Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3];
1949 Vector3 axis = edge_axis.cross(cyl_axis);
1950 if (Math::is_zero_approx(axis.length_squared())) {
1951 continue;
1952 }
1953
1954 if (axis.dot(normal) < 0.0) {
1955 axis *= -1.0;
1956 }
1957
1958 if (!separator.test_axis(axis.normalized())) {
1959 return;
1960 }
1961 }
1962
1963 // Points of B, cylinder lateral surface.
1964 for (int i = 0; i < 3; i++) {
1965 const Vector3 &point = vertex[i];
1966 Vector3 axis = Plane(cyl_axis).project(point).normalized();
1967 if (axis.dot(normal) < 0.0) {
1968 axis *= -1.0;
1969 }
1970
1971 if (!separator.test_axis(axis)) {
1972 return;
1973 }
1974 }
1975
1976 // Edges of B, cylinder end caps rim.
1977 Vector3 cap_axis = cyl_axis * (cylinder_A->get_height() * 0.5);
1978
1979 for (int i = 0; i < 2; i++) {
1980 Vector3 cap_pos = p_transform_a.origin + ((i == 0) ? cap_axis : -cap_axis);
1981
1982 for (int j = 0; j < 3; j++) {
1983 const Vector3 &edge_start = vertex[j];
1984 const Vector3 &edge_end = vertex[(j + 1) % 3];
1985 Vector3 edge_dir = edge_end - edge_start;
1986 edge_dir.normalize();
1987
1988 real_t edge_dot = edge_dir.dot(cyl_axis);
1989 if (Math::is_zero_approx(edge_dot)) {
1990 // Edge is perpendicular to cylinder axis.
1991 continue;
1992 }
1993
1994 // Calculate intersection between edge and circle plane.
1995 Vector3 edge_diff = cap_pos - edge_start;
1996 real_t diff_dot = edge_diff.dot(cyl_axis);
1997 Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot;
1998
1999 // Calculate tangent that touches intersection.
2000 Vector3 tangent = (cap_pos - intersection).cross(cyl_axis);
2001
2002 // Axis is orthogonal both to tangent and edge direction.
2003 Vector3 axis = tangent.cross(edge_dir);
2004 if (axis.dot(normal) < 0.0) {
2005 axis *= -1.0;
2006 }
2007
2008 if (!separator.test_axis(axis.normalized())) {
2009 return;
2010 }
2011 }
2012 }
2013
2014 if (!face_B->backface_collision) {
2015 if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
2016 if (face_B->invert_backface_collision) {
2017 separator.best_axis = separator.best_axis.bounce(normal);
2018 } else {
2019 // Just ignore backface collision.
2020 return;
2021 }
2022 }
2023 }
2024
2025 separator.generate_contacts();
2026}
2027
2028static _FORCE_INLINE_ bool is_minkowski_face(const Vector3 &A, const Vector3 &B, const Vector3 &B_x_A, const Vector3 &C, const Vector3 &D, const Vector3 &D_x_C) {
2029 // Test if arcs AB and CD intersect on the unit sphere
2030 real_t CBA = C.dot(B_x_A);
2031 real_t DBA = D.dot(B_x_A);
2032 real_t ADC = A.dot(D_x_C);
2033 real_t BDC = B.dot(D_x_C);
2034
2035 return (CBA * DBA < 0.0f) && (ADC * BDC < 0.0f) && (CBA * BDC > 0.0f);
2036}
2037
2038template <bool withMargin>
2039static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
2040 const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a);
2041 const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
2042
2043 SeparatorAxisTest<GodotConvexPolygonShape3D, GodotConvexPolygonShape3D, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
2044
2045 if (!separator.test_previous_axis()) {
2046 return;
2047 }
2048
2049 const Geometry3D::MeshData &mesh_A = convex_polygon_A->get_mesh();
2050
2051 const Geometry3D::MeshData::Face *faces_A = mesh_A.faces.ptr();
2052 int face_count_A = mesh_A.faces.size();
2053 const Geometry3D::MeshData::Edge *edges_A = mesh_A.edges.ptr();
2054 int edge_count_A = mesh_A.edges.size();
2055 const Vector3 *vertices_A = mesh_A.vertices.ptr();
2056 int vertex_count_A = mesh_A.vertices.size();
2057
2058 const Geometry3D::MeshData &mesh_B = convex_polygon_B->get_mesh();
2059
2060 const Geometry3D::MeshData::Face *faces_B = mesh_B.faces.ptr();
2061 int face_count_B = mesh_B.faces.size();
2062 const Geometry3D::MeshData::Edge *edges_B = mesh_B.edges.ptr();
2063 int edge_count_B = mesh_B.edges.size();
2064 const Vector3 *vertices_B = mesh_B.vertices.ptr();
2065 int vertex_count_B = mesh_B.vertices.size();
2066
2067 // Precalculating this makes the transforms faster.
2068 Basis a_xform_normal = p_transform_a.basis.inverse().transposed();
2069
2070 // faces of A
2071 for (int i = 0; i < face_count_A; i++) {
2072 Vector3 axis = a_xform_normal.xform(faces_A[i].plane.normal).normalized();
2073
2074 if (!separator.test_axis(axis)) {
2075 return;
2076 }
2077 }
2078
2079 // Precalculating this makes the transforms faster.
2080 Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
2081
2082 // faces of B
2083 for (int i = 0; i < face_count_B; i++) {
2084 Vector3 axis = b_xform_normal.xform(faces_B[i].plane.normal).normalized();
2085
2086 if (!separator.test_axis(axis)) {
2087 return;
2088 }
2089 }
2090
2091 // A<->B edges
2092
2093 for (int i = 0; i < edge_count_A; i++) {
2094 Vector3 p1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_a]);
2095 Vector3 q1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_b]);
2096 Vector3 e1 = q1 - p1;
2097 Vector3 u1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_a].plane.normal).normalized();
2098 Vector3 v1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_b].plane.normal).normalized();
2099
2100 for (int j = 0; j < edge_count_B; j++) {
2101 Vector3 p2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_a]);
2102 Vector3 q2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_b]);
2103 Vector3 e2 = q2 - p2;
2104 Vector3 u2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_a].plane.normal).normalized();
2105 Vector3 v2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_b].plane.normal).normalized();
2106
2107 if (is_minkowski_face(u1, v1, -e1, -u2, -v2, -e2)) {
2108 Vector3 axis = e1.cross(e2).normalized();
2109
2110 if (!separator.test_axis(axis)) {
2111 return;
2112 }
2113 }
2114 }
2115 }
2116
2117 if (withMargin) {
2118 //vertex-vertex
2119 for (int i = 0; i < vertex_count_A; i++) {
2120 Vector3 va = p_transform_a.xform(vertices_A[i]);
2121
2122 for (int j = 0; j < vertex_count_B; j++) {
2123 if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) {
2124 return;
2125 }
2126 }
2127 }
2128 //edge-vertex (shell)
2129
2130 for (int i = 0; i < edge_count_A; i++) {
2131 Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_a]);
2132 Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_b]);
2133 Vector3 n = (e2 - e1);
2134
2135 for (int j = 0; j < vertex_count_B; j++) {
2136 Vector3 e3 = p_transform_b.xform(vertices_B[j]);
2137
2138 if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) {
2139 return;
2140 }
2141 }
2142 }
2143
2144 for (int i = 0; i < edge_count_B; i++) {
2145 Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_a]);
2146 Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_b]);
2147 Vector3 n = (e2 - e1);
2148
2149 for (int j = 0; j < vertex_count_A; j++) {
2150 Vector3 e3 = p_transform_a.xform(vertices_A[j]);
2151
2152 if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) {
2153 return;
2154 }
2155 }
2156 }
2157 }
2158
2159 separator.generate_contacts();
2160}
2161
2162template <bool withMargin>
2163static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
2164 const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a);
2165 const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
2166
2167 SeparatorAxisTest<GodotConvexPolygonShape3D, GodotFaceShape3D, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
2168
2169 const Geometry3D::MeshData &mesh = convex_polygon_A->get_mesh();
2170
2171 const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
2172 int face_count = mesh.faces.size();
2173 const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr();
2174 int edge_count = mesh.edges.size();
2175 const Vector3 *vertices = mesh.vertices.ptr();
2176 int vertex_count = mesh.vertices.size();
2177
2178 Vector3 vertex[3] = {
2179 p_transform_b.xform(face_B->vertex[0]),
2180 p_transform_b.xform(face_B->vertex[1]),
2181 p_transform_b.xform(face_B->vertex[2]),
2182 };
2183
2184 Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
2185
2186 if (!separator.test_axis(normal)) {
2187 return;
2188 }
2189
2190 // faces of A
2191 for (int i = 0; i < face_count; i++) {
2192 //Vector3 axis = p_transform_a.xform( faces[i].plane ).normal;
2193 Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized();
2194 if (axis.dot(normal) < 0.0) {
2195 axis *= -1.0;
2196 }
2197
2198 if (!separator.test_axis(axis)) {
2199 return;
2200 }
2201 }
2202
2203 // A<->B edges
2204 for (int i = 0; i < edge_count; i++) {
2205 Vector3 e1 = p_transform_a.xform(vertices[edges[i].vertex_a]) - p_transform_a.xform(vertices[edges[i].vertex_b]);
2206
2207 for (int j = 0; j < 3; j++) {
2208 Vector3 e2 = vertex[j] - vertex[(j + 1) % 3];
2209
2210 Vector3 axis = e1.cross(e2).normalized();
2211 if (axis.dot(normal) < 0.0) {
2212 axis *= -1.0;
2213 }
2214
2215 if (!separator.test_axis(axis)) {
2216 return;
2217 }
2218 }
2219 }
2220
2221 if (withMargin) {
2222 //vertex-vertex
2223 for (int i = 0; i < vertex_count; i++) {
2224 Vector3 va = p_transform_a.xform(vertices[i]);
2225
2226 for (int j = 0; j < 3; j++) {
2227 Vector3 axis = (va - vertex[j]).normalized();
2228 if (axis.dot(normal) < 0.0) {
2229 axis *= -1.0;
2230 }
2231
2232 if (!separator.test_axis(axis)) {
2233 return;
2234 }
2235 }
2236 }
2237 //edge-vertex (shell)
2238
2239 for (int i = 0; i < edge_count; i++) {
2240 Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].vertex_a]);
2241 Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].vertex_b]);
2242 Vector3 n = (e2 - e1);
2243
2244 for (int j = 0; j < 3; j++) {
2245 Vector3 e3 = vertex[j];
2246
2247 Vector3 axis = (e1 - e3).cross(n).cross(n).normalized();
2248 if (axis.dot(normal) < 0.0) {
2249 axis *= -1.0;
2250 }
2251
2252 if (!separator.test_axis(axis)) {
2253 return;
2254 }
2255 }
2256 }
2257
2258 for (int i = 0; i < 3; i++) {
2259 Vector3 e1 = vertex[i];
2260 Vector3 e2 = vertex[(i + 1) % 3];
2261 Vector3 n = (e2 - e1);
2262
2263 for (int j = 0; j < vertex_count; j++) {
2264 Vector3 e3 = p_transform_a.xform(vertices[j]);
2265
2266 Vector3 axis = (e1 - e3).cross(n).cross(n).normalized();
2267 if (axis.dot(normal) < 0.0) {
2268 axis *= -1.0;
2269 }
2270
2271 if (!separator.test_axis(axis)) {
2272 return;
2273 }
2274 }
2275 }
2276 }
2277
2278 if (!face_B->backface_collision) {
2279 if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
2280 if (face_B->invert_backface_collision) {
2281 separator.best_axis = separator.best_axis.bounce(normal);
2282 } else {
2283 // Just ignore backface collision.
2284 return;
2285 }
2286 }
2287 }
2288
2289 separator.generate_contacts();
2290}
2291
2292bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) {
2293 PhysicsServer3D::ShapeType type_A = p_shape_A->get_type();
2294
2295 ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false);
2296 ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY, false);
2297 ERR_FAIL_COND_V(p_shape_A->is_concave(), false);
2298
2299 PhysicsServer3D::ShapeType type_B = p_shape_B->get_type();
2300
2301 ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false);
2302 ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY, false);
2303 ERR_FAIL_COND_V(p_shape_B->is_concave(), false);
2304
2305 static const CollisionFunc collision_table[6][6] = {
2306 { _collision_sphere_sphere<false>,
2307 _collision_sphere_box<false>,
2308 _collision_sphere_capsule<false>,
2309 _collision_sphere_cylinder<false>,
2310 _collision_sphere_convex_polygon<false>,
2311 _collision_sphere_face<false> },
2312 { nullptr,
2313 _collision_box_box<false>,
2314 _collision_box_capsule<false>,
2315 _collision_box_cylinder<false>,
2316 _collision_box_convex_polygon<false>,
2317 _collision_box_face<false> },
2318 { nullptr,
2319 nullptr,
2320 _collision_capsule_capsule<false>,
2321 _collision_capsule_cylinder<false>,
2322 _collision_capsule_convex_polygon<false>,
2323 _collision_capsule_face<false> },
2324 { nullptr,
2325 nullptr,
2326 nullptr,
2327 _collision_cylinder_cylinder<false>,
2328 _collision_cylinder_convex_polygon<false>,
2329 _collision_cylinder_face<false> },
2330 { nullptr,
2331 nullptr,
2332 nullptr,
2333 nullptr,
2334 _collision_convex_polygon_convex_polygon<false>,
2335 _collision_convex_polygon_face<false> },
2336 { nullptr,
2337 nullptr,
2338 nullptr,
2339 nullptr,
2340 nullptr,
2341 nullptr },
2342 };
2343
2344 static const CollisionFunc collision_table_margin[6][6] = {
2345 { _collision_sphere_sphere<true>,
2346 _collision_sphere_box<true>,
2347 _collision_sphere_capsule<true>,
2348 _collision_sphere_cylinder<true>,
2349 _collision_sphere_convex_polygon<true>,
2350 _collision_sphere_face<true> },
2351 { nullptr,
2352 _collision_box_box<true>,
2353 _collision_box_capsule<true>,
2354 _collision_box_cylinder<true>,
2355 _collision_box_convex_polygon<true>,
2356 _collision_box_face<true> },
2357 { nullptr,
2358 nullptr,
2359 _collision_capsule_capsule<true>,
2360 _collision_capsule_cylinder<true>,
2361 _collision_capsule_convex_polygon<true>,
2362 _collision_capsule_face<true> },
2363 { nullptr,
2364 nullptr,
2365 nullptr,
2366 _collision_cylinder_cylinder<true>,
2367 _collision_cylinder_convex_polygon<true>,
2368 _collision_cylinder_face<true> },
2369 { nullptr,
2370 nullptr,
2371 nullptr,
2372 nullptr,
2373 _collision_convex_polygon_convex_polygon<true>,
2374 _collision_convex_polygon_face<true> },
2375 { nullptr,
2376 nullptr,
2377 nullptr,
2378 nullptr,
2379 nullptr,
2380 nullptr },
2381 };
2382
2383 _CollectorCallback callback;
2384 callback.callback = p_result_callback;
2385 callback.swap = p_swap;
2386 callback.userdata = p_userdata;
2387 callback.collided = false;
2388 callback.prev_axis = r_prev_axis;
2389
2390 const GodotShape3D *A = p_shape_A;
2391 const GodotShape3D *B = p_shape_B;
2392 const Transform3D *transform_A = &p_transform_A;
2393 const Transform3D *transform_B = &p_transform_B;
2394 real_t margin_A = p_margin_a;
2395 real_t margin_B = p_margin_b;
2396
2397 if (type_A > type_B) {
2398 SWAP(A, B);
2399 SWAP(transform_A, transform_B);
2400 SWAP(type_A, type_B);
2401 SWAP(margin_A, margin_B);
2402 callback.swap = !callback.swap;
2403 }
2404
2405 CollisionFunc collision_func;
2406 if (margin_A != 0.0 || margin_B != 0.0) {
2407 collision_func = collision_table_margin[type_A - 2][type_B - 2];
2408
2409 } else {
2410 collision_func = collision_table[type_A - 2][type_B - 2];
2411 }
2412 ERR_FAIL_COND_V(!collision_func, false);
2413
2414 collision_func(A, *transform_A, B, *transform_B, &callback, margin_A, margin_B);
2415
2416 return callback.collided;
2417}
2418