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-2019 Juan Linietsky, Ariel Manzur. */
9 /* Copyright (c) 2014-2019 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 #include "collision_solver_sw.h"
31 #include "collision_solver_sat.h"
32
33 #include "collision_solver_sat.h"
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
93 if (p_result_callback) {
94 if (p_swap_result)
95 p_result_callback(support_B, support_A, p_userdata);
96 else
97 p_result_callback(support_A, support_B, p_userdata);
98 }
99 return true;
100 }
101
102 struct _ConcaveCollisionInfo {
103
104 const Transform *transform_A;
105 const ShapeSW *shape_A;
106 const Transform *transform_B;
107 CollisionSolverSW::CallbackResult result_callback;
108 void *userdata;
109 bool swap_result;
110 bool collided;
111 int aabb_tests;
112 int collisions;
113 bool tested;
114 float margin_A;
115 float margin_B;
116 Vector3 close_A, close_B;
117 };
118
concave_callback(void * p_userdata,ShapeSW * p_convex)119 void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
120
121 _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
122 cinfo.aabb_tests++;
123
124 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);
125 if (!collided)
126 return;
127
128 cinfo.collided = true;
129 cinfo.collisions++;
130 }
131
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,float p_margin_A,float p_margin_B)132 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, float p_margin_A, float p_margin_B) {
133
134 const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B);
135
136 _ConcaveCollisionInfo cinfo;
137 cinfo.transform_A = &p_transform_A;
138 cinfo.shape_A = p_shape_A;
139 cinfo.transform_B = &p_transform_B;
140 cinfo.result_callback = p_result_callback;
141 cinfo.userdata = p_userdata;
142 cinfo.swap_result = p_swap_result;
143 cinfo.collided = false;
144 cinfo.collisions = 0;
145 cinfo.margin_A = p_margin_A;
146 cinfo.margin_B = p_margin_B;
147
148 cinfo.aabb_tests = 0;
149
150 Transform rel_transform = p_transform_A;
151 rel_transform.origin -= p_transform_B.origin;
152
153 //quickly compute a local AABB
154
155 AABB local_aabb;
156 for (int i = 0; i < 3; i++) {
157
158 Vector3 axis(p_transform_B.basis.get_axis(i));
159 float axis_scale = 1.0 / axis.length();
160 axis *= axis_scale;
161
162 float smin, smax;
163 p_shape_A->project_range(axis, rel_transform, smin, smax);
164 smin -= p_margin_A;
165 smax += p_margin_A;
166 smin *= axis_scale;
167 smax *= axis_scale;
168
169 local_aabb.pos[i] = smin;
170 local_aabb.size[i] = smax - smin;
171 }
172
173 concave_B->cull(local_aabb, concave_callback, &cinfo);
174 //print_line("COL AABB TESTS: "+itos(cinfo.aabb_tests));
175
176 return cinfo.collided;
177 }
178
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,float p_margin_A,float p_margin_B)179 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, float p_margin_A, float p_margin_B) {
180
181 PhysicsServer::ShapeType type_A = p_shape_A->get_type();
182 PhysicsServer::ShapeType type_B = p_shape_B->get_type();
183 bool concave_A = p_shape_A->is_concave();
184 bool concave_B = p_shape_B->is_concave();
185
186 bool swap = false;
187
188 if (type_A > type_B) {
189 SWAP(type_A, type_B);
190 SWAP(concave_A, concave_B);
191 swap = true;
192 }
193
194 if (type_A == PhysicsServer::SHAPE_PLANE) {
195
196 if (type_B == PhysicsServer::SHAPE_PLANE)
197 return false;
198 if (type_B == PhysicsServer::SHAPE_RAY) {
199 return false;
200 }
201
202 if (swap) {
203 return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
204 } else {
205 return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
206 }
207
208 } else if (type_A == PhysicsServer::SHAPE_RAY) {
209
210 if (type_B == PhysicsServer::SHAPE_RAY)
211 return false;
212
213 if (swap) {
214 return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
215 } else {
216 return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
217 }
218
219 } else if (concave_B) {
220
221 if (concave_A)
222 return false;
223
224 if (!swap)
225 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);
226 else
227 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);
228
229 } else {
230
231 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);
232 }
233
234 return false;
235 }
236
concave_distance_callback(void * p_userdata,ShapeSW * p_convex)237 void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) {
238
239 _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
240 cinfo.aabb_tests++;
241 if (cinfo.collided)
242 return;
243
244 Vector3 close_A, close_B;
245 cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B);
246
247 if (cinfo.collided)
248 return;
249 if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) {
250
251 cinfo.close_A = close_A;
252 cinfo.close_B = close_B;
253 cinfo.tested = true;
254 }
255
256 cinfo.collisions++;
257 }
258
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)259 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) {
260
261 const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A);
262 if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE)
263 return false;
264 Plane p = p_transform_A.xform(plane->get_plane());
265
266 static const int max_supports = 16;
267 Vector3 supports[max_supports];
268 int support_count;
269
270 p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
271
272 bool collided = false;
273 Vector3 closest;
274 float closest_d;
275
276 for (int i = 0; i < support_count; i++) {
277
278 supports[i] = p_transform_B.xform(supports[i]);
279 real_t d = p.distance_to(supports[i]);
280 if (i == 0 || d < closest_d) {
281 closest = supports[i];
282 closest_d = d;
283 if (d <= 0)
284 collided = true;
285 }
286 }
287
288 r_point_A = p.project(closest);
289 r_point_B = closest;
290
291 return collided;
292 }
293
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)294 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) {
295
296 if (p_shape_A->is_concave())
297 return false;
298
299 if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) {
300
301 Vector3 a, b;
302 bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b);
303 r_point_A = b;
304 r_point_B = a;
305 return !col;
306
307 } else if (p_shape_B->is_concave()) {
308
309 if (p_shape_A->is_concave())
310 return false;
311
312 const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B);
313
314 _ConcaveCollisionInfo cinfo;
315 cinfo.transform_A = &p_transform_A;
316 cinfo.shape_A = p_shape_A;
317 cinfo.transform_B = &p_transform_B;
318 cinfo.result_callback = NULL;
319 cinfo.userdata = NULL;
320 cinfo.swap_result = false;
321 cinfo.collided = false;
322 cinfo.collisions = 0;
323 cinfo.aabb_tests = 0;
324 cinfo.tested = false;
325
326 Transform rel_transform = p_transform_A;
327 rel_transform.origin -= p_transform_B.origin;
328
329 //quickly compute a local AABB
330
331 bool use_cc_hint = p_concave_hint != AABB();
332 AABB cc_hint_aabb;
333 if (use_cc_hint) {
334 cc_hint_aabb = p_concave_hint;
335 cc_hint_aabb.pos -= p_transform_B.origin;
336 }
337
338 AABB local_aabb;
339 for (int i = 0; i < 3; i++) {
340
341 Vector3 axis(p_transform_B.basis.get_axis(i));
342 float axis_scale = 1.0 / axis.length();
343 axis *= axis_scale;
344
345 float smin, smax;
346
347 if (use_cc_hint) {
348 cc_hint_aabb.project_range_in_plane(Plane(axis, 0), smin, smax);
349 } else {
350 p_shape_A->project_range(axis, rel_transform, smin, smax);
351 }
352
353 smin *= axis_scale;
354 smax *= axis_scale;
355
356 local_aabb.pos[i] = smin;
357 local_aabb.size[i] = smax - smin;
358 }
359
360 concave_B->cull(local_aabb, concave_distance_callback, &cinfo);
361 if (!cinfo.collided) {
362 // print_line(itos(cinfo.tested));
363 r_point_A = cinfo.close_A;
364 r_point_B = cinfo.close_B;
365 }
366
367 //print_line("DIST AABB TESTS: "+itos(cinfo.aabb_tests));
368
369 return !cinfo.collided;
370 } else {
371
372 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..
373 }
374
375 return false;
376 }
377