1 /*************************************************************************/
2 /* collision_solver_sw.cpp */
3 /*************************************************************************/
4 /* This file is part of: */
5 /* GODOT ENGINE */
6 /* https://godotengine.org */
7 /*************************************************************************/
8 /* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
9 /* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
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 "collision_solver_sw.h"
32 #include "collision_solver_sat.h"
33
34 #include "gjk_epa.h"
35
36 #define collision_solver sat_calculate_penetration
37 //#define collision_solver gjk_epa_calculate_penetration
38
solve_static_plane(const ShapeSW * p_shape_A,const Transform & p_transform_A,const ShapeSW * p_shape_B,const Transform & p_transform_B,CallbackResult p_result_callback,void * p_userdata,bool p_swap_result)39 bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
40
41 const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A);
42 if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE)
43 return false;
44 Plane p = p_transform_A.xform(plane->get_plane());
45
46 static const int max_supports = 16;
47 Vector3 supports[max_supports];
48 int support_count;
49
50 p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
51
52 bool found = false;
53
54 for (int i = 0; i < support_count; i++) {
55
56 supports[i] = p_transform_B.xform(supports[i]);
57 if (p.distance_to(supports[i]) >= 0)
58 continue;
59 found = true;
60
61 Vector3 support_A = p.project(supports[i]);
62
63 if (p_result_callback) {
64 if (p_swap_result)
65 p_result_callback(supports[i], support_A, p_userdata);
66 else
67 p_result_callback(support_A, supports[i], p_userdata);
68 }
69 }
70
71 return found;
72 }
73
solve_ray(const ShapeSW * p_shape_A,const Transform & p_transform_A,const ShapeSW * p_shape_B,const Transform & p_transform_B,CallbackResult p_result_callback,void * p_userdata,bool p_swap_result)74 bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
75
76 const RayShapeSW *ray = static_cast<const RayShapeSW *>(p_shape_A);
77
78 Vector3 from = p_transform_A.origin;
79 Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length();
80 Vector3 support_A = to;
81
82 Transform ai = p_transform_B.affine_inverse();
83
84 from = ai.xform(from);
85 to = ai.xform(to);
86
87 Vector3 p, n;
88 if (!p_shape_B->intersect_segment(from, to, p, n))
89 return false;
90
91 Vector3 support_B = p_transform_B.xform(p);
92 if (ray->get_slips_on_slope()) {
93 Vector3 global_n = ai.basis.xform_inv(n).normalized();
94 support_B = support_A + (support_B - support_A).length() * global_n;
95 }
96
97 if (p_result_callback) {
98 if (p_swap_result)
99 p_result_callback(support_B, support_A, p_userdata);
100 else
101 p_result_callback(support_A, support_B, p_userdata);
102 }
103 return true;
104 }
105
106 struct _ConcaveCollisionInfo {
107
108 const Transform *transform_A;
109 const ShapeSW *shape_A;
110 const Transform *transform_B;
111 CollisionSolverSW::CallbackResult result_callback;
112 void *userdata;
113 bool swap_result;
114 bool collided;
115 int aabb_tests;
116 int collisions;
117 bool tested;
118 real_t margin_A;
119 real_t margin_B;
120 Vector3 close_A, close_B;
121 };
122
concave_callback(void * p_userdata,ShapeSW * p_convex)123 void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
124
125 _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
126 cinfo.aabb_tests++;
127
128 bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, NULL, cinfo.margin_A, cinfo.margin_B);
129 if (!collided)
130 return;
131
132 cinfo.collided = true;
133 cinfo.collisions++;
134 }
135
solve_concave(const ShapeSW * p_shape_A,const Transform & p_transform_A,const ShapeSW * p_shape_B,const Transform & p_transform_B,CallbackResult p_result_callback,void * p_userdata,bool p_swap_result,real_t p_margin_A,real_t p_margin_B)136 bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) {
137
138 const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B);
139
140 _ConcaveCollisionInfo cinfo;
141 cinfo.transform_A = &p_transform_A;
142 cinfo.shape_A = p_shape_A;
143 cinfo.transform_B = &p_transform_B;
144 cinfo.result_callback = p_result_callback;
145 cinfo.userdata = p_userdata;
146 cinfo.swap_result = p_swap_result;
147 cinfo.collided = false;
148 cinfo.collisions = 0;
149 cinfo.margin_A = p_margin_A;
150 cinfo.margin_B = p_margin_B;
151
152 cinfo.aabb_tests = 0;
153
154 Transform rel_transform = p_transform_A;
155 rel_transform.origin -= p_transform_B.origin;
156
157 //quickly compute a local AABB
158
159 AABB local_aabb;
160 for (int i = 0; i < 3; i++) {
161
162 Vector3 axis(p_transform_B.basis.get_axis(i));
163 real_t axis_scale = 1.0 / axis.length();
164 axis *= axis_scale;
165
166 real_t smin, smax;
167 p_shape_A->project_range(axis, rel_transform, smin, smax);
168 smin -= p_margin_A;
169 smax += p_margin_A;
170 smin *= axis_scale;
171 smax *= axis_scale;
172
173 local_aabb.position[i] = smin;
174 local_aabb.size[i] = smax - smin;
175 }
176
177 concave_B->cull(local_aabb, concave_callback, &cinfo);
178
179 return cinfo.collided;
180 }
181
solve_static(const ShapeSW * p_shape_A,const Transform & p_transform_A,const ShapeSW * p_shape_B,const Transform & p_transform_B,CallbackResult p_result_callback,void * p_userdata,Vector3 * r_sep_axis,real_t p_margin_A,real_t p_margin_B)182 bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
183
184 PhysicsServer::ShapeType type_A = p_shape_A->get_type();
185 PhysicsServer::ShapeType type_B = p_shape_B->get_type();
186 bool concave_A = p_shape_A->is_concave();
187 bool concave_B = p_shape_B->is_concave();
188
189 bool swap = false;
190
191 if (type_A > type_B) {
192 SWAP(type_A, type_B);
193 SWAP(concave_A, concave_B);
194 swap = true;
195 }
196
197 if (type_A == PhysicsServer::SHAPE_PLANE) {
198
199 if (type_B == PhysicsServer::SHAPE_PLANE)
200 return false;
201 if (type_B == PhysicsServer::SHAPE_RAY) {
202 return false;
203 }
204
205 if (swap) {
206 return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
207 } else {
208 return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
209 }
210
211 } else if (type_A == PhysicsServer::SHAPE_RAY) {
212
213 if (type_B == PhysicsServer::SHAPE_RAY)
214 return false;
215
216 if (swap) {
217 return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
218 } else {
219 return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
220 }
221
222 } else if (concave_B) {
223
224 if (concave_A)
225 return false;
226
227 if (!swap)
228 return solve_concave(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A, p_margin_B);
229 else
230 return solve_concave(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A, p_margin_B);
231
232 } else {
233
234 return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B);
235 }
236 }
237
concave_distance_callback(void * p_userdata,ShapeSW * p_convex)238 void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) {
239
240 _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
241 cinfo.aabb_tests++;
242 if (cinfo.collided)
243 return;
244
245 Vector3 close_A, close_B;
246 cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B);
247
248 if (cinfo.collided)
249 return;
250 if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) {
251
252 cinfo.close_A = close_A;
253 cinfo.close_B = close_B;
254 cinfo.tested = true;
255 }
256
257 cinfo.collisions++;
258 }
259
solve_distance_plane(const ShapeSW * p_shape_A,const Transform & p_transform_A,const ShapeSW * p_shape_B,const Transform & p_transform_B,Vector3 & r_point_A,Vector3 & r_point_B)260 bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
261
262 const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A);
263 if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE)
264 return false;
265 Plane p = p_transform_A.xform(plane->get_plane());
266
267 static const int max_supports = 16;
268 Vector3 supports[max_supports];
269 int support_count;
270
271 p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
272
273 bool collided = false;
274 Vector3 closest;
275 real_t closest_d = 0;
276
277 for (int i = 0; i < support_count; i++) {
278
279 supports[i] = p_transform_B.xform(supports[i]);
280 real_t d = p.distance_to(supports[i]);
281 if (i == 0 || d < closest_d) {
282 closest = supports[i];
283 closest_d = d;
284 if (d <= 0)
285 collided = true;
286 }
287 }
288
289 r_point_A = p.project(closest);
290 r_point_B = closest;
291
292 return collided;
293 }
294
solve_distance(const ShapeSW * p_shape_A,const Transform & p_transform_A,const ShapeSW * p_shape_B,const Transform & p_transform_B,Vector3 & r_point_A,Vector3 & r_point_B,const AABB & p_concave_hint,Vector3 * r_sep_axis)295 bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) {
296
297 if (p_shape_A->is_concave())
298 return false;
299
300 if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) {
301
302 Vector3 a, b;
303 bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b);
304 r_point_A = b;
305 r_point_B = a;
306 return !col;
307
308 } else if (p_shape_B->is_concave()) {
309
310 if (p_shape_A->is_concave())
311 return false;
312
313 const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B);
314
315 _ConcaveCollisionInfo cinfo;
316 cinfo.transform_A = &p_transform_A;
317 cinfo.shape_A = p_shape_A;
318 cinfo.transform_B = &p_transform_B;
319 cinfo.result_callback = NULL;
320 cinfo.userdata = NULL;
321 cinfo.swap_result = false;
322 cinfo.collided = false;
323 cinfo.collisions = 0;
324 cinfo.aabb_tests = 0;
325 cinfo.tested = false;
326
327 Transform rel_transform = p_transform_A;
328 rel_transform.origin -= p_transform_B.origin;
329
330 //quickly compute a local AABB
331
332 bool use_cc_hint = p_concave_hint != AABB();
333 AABB cc_hint_aabb;
334 if (use_cc_hint) {
335 cc_hint_aabb = p_concave_hint;
336 cc_hint_aabb.position -= p_transform_B.origin;
337 }
338
339 AABB local_aabb;
340 for (int i = 0; i < 3; i++) {
341
342 Vector3 axis(p_transform_B.basis.get_axis(i));
343 real_t axis_scale = ((real_t)1.0) / axis.length();
344 axis *= axis_scale;
345
346 real_t smin, smax;
347
348 if (use_cc_hint) {
349 cc_hint_aabb.project_range_in_plane(Plane(axis, 0), smin, smax);
350 } else {
351 p_shape_A->project_range(axis, rel_transform, smin, smax);
352 }
353
354 smin *= axis_scale;
355 smax *= axis_scale;
356
357 local_aabb.position[i] = smin;
358 local_aabb.size[i] = smax - smin;
359 }
360
361 concave_B->cull(local_aabb, concave_distance_callback, &cinfo);
362 if (!cinfo.collided) {
363 r_point_A = cinfo.close_A;
364 r_point_B = cinfo.close_B;
365 }
366
367 return !cinfo.collided;
368 } else {
369
370 return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis..
371 }
372 }
373