1 /*************************************************************************/
2 /* body_pair_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 "body_pair_sw.h"
31 #include "collision_solver_sw.h"
32 #include "os/os.h"
33 #include "space_sw.h"
34
35 /*
36 #define NO_ACCUMULATE_IMPULSES
37 #define NO_SPLIT_IMPULSES
38
39 #define NO_FRICTION
40 */
41
42 #define NO_TANGENTIALS
43 /* BODY PAIR */
44
45 //#define ALLOWED_PENETRATION 0.01
46 #define RELAXATION_TIMESTEPS 3
47 #define MIN_VELOCITY 0.0001
48
_contact_added_callback(const Vector3 & p_point_A,const Vector3 & p_point_B,void * p_userdata)49 void BodyPairSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
50
51 BodyPairSW *pair = (BodyPairSW *)p_userdata;
52 pair->contact_added_callback(p_point_A, p_point_B);
53 }
54
contact_added_callback(const Vector3 & p_point_A,const Vector3 & p_point_B)55 void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B) {
56
57 // check if we already have the contact
58
59 //Vector3 local_A = A->get_inv_transform().xform(p_point_A);
60 //Vector3 local_B = B->get_inv_transform().xform(p_point_B);
61
62 Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A);
63 Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B);
64
65 int new_index = contact_count;
66
67 ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
68
69 Contact contact;
70
71 contact.acc_normal_impulse = 0;
72 contact.acc_bias_impulse = 0;
73 contact.acc_tangent_impulse = Vector3();
74 contact.local_A = local_A;
75 contact.local_B = local_B;
76 contact.normal = (p_point_A - p_point_B).normalized();
77
78 // attempt to determine if the contact will be reused
79 real_t contact_recycle_radius = space->get_contact_recycle_radius();
80
81 for (int i = 0; i < contact_count; i++) {
82
83 Contact &c = contacts[i];
84 if (
85 c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) &&
86 c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) {
87
88 contact.acc_normal_impulse = c.acc_normal_impulse;
89 contact.acc_bias_impulse = c.acc_bias_impulse;
90 contact.acc_tangent_impulse = c.acc_tangent_impulse;
91 new_index = i;
92 break;
93 }
94 }
95
96 // figure out if the contact amount must be reduced to fit the new contact
97
98 if (new_index == MAX_CONTACTS) {
99
100 // remove the contact with the minimum depth
101
102 int least_deep = -1;
103 float min_depth = 1e10;
104
105 for (int i = 0; i <= contact_count; i++) {
106
107 Contact &c = (i == contact_count) ? contact : contacts[i];
108 Vector3 global_A = A->get_transform().basis.xform(c.local_A);
109 Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B;
110
111 Vector3 axis = global_A - global_B;
112 float depth = axis.dot(c.normal);
113
114 if (depth < min_depth) {
115
116 min_depth = depth;
117 least_deep = i;
118 }
119 }
120
121 ERR_FAIL_COND(least_deep == -1);
122
123 if (least_deep < contact_count) { //replace the last deep contact by the new one
124
125 contacts[least_deep] = contact;
126 }
127
128 return;
129 }
130
131 contacts[new_index] = contact;
132
133 if (new_index == contact_count) {
134
135 contact_count++;
136 }
137 }
138
validate_contacts()139 void BodyPairSW::validate_contacts() {
140
141 //make sure to erase contacts that are no longer valid
142
143 real_t contact_max_separation = space->get_contact_max_separation();
144 for (int i = 0; i < contact_count; i++) {
145
146 Contact &c = contacts[i];
147
148 Vector3 global_A = A->get_transform().basis.xform(c.local_A);
149 Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B;
150 Vector3 axis = global_A - global_B;
151 float depth = axis.dot(c.normal);
152
153 if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
154 // contact no longer needed, remove
155
156 if ((i + 1) < contact_count) {
157 // swap with the last one
158 SWAP(contacts[i], contacts[contact_count - 1]);
159 }
160
161 i--;
162 contact_count--;
163 }
164 }
165 }
166
_test_ccd(float p_step,BodySW * p_A,int p_shape_A,const Transform & p_xform_A,BodySW * p_B,int p_shape_B,const Transform & p_xform_B)167 bool BodyPairSW::_test_ccd(float p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &p_xform_B) {
168
169 Vector3 motion = p_A->get_linear_velocity() * p_step;
170 real_t mlen = motion.length();
171 if (mlen < CMP_EPSILON)
172 return false;
173
174 Vector3 mnormal = motion / mlen;
175
176 real_t min, max;
177 p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max);
178 bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction
179
180 if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
181 return false;
182 }
183
184 //cast a segment from support in motion normal, in the same direction of motion by motion length
185 //support is the worst case collision point, so real collision happened before
186 Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
187 Vector3 from = p_xform_A.xform(s);
188 Vector3 to = from + motion;
189
190 Transform from_inv = p_xform_B.affine_inverse();
191
192 Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
193 Vector3 local_to = from_inv.xform(to);
194
195 Vector3 rpos, rnorm;
196 if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
197 return false;
198 }
199
200 //shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough
201 Vector3 hitpos = p_xform_B.xform(rpos);
202
203 float newlen = hitpos.distance_to(from) - (max - min) * 0.01;
204 p_A->set_linear_velocity((mnormal * newlen) / p_step);
205
206 return true;
207 }
208
setup(float p_step)209 bool BodyPairSW::setup(float p_step) {
210
211 //one or both shapes have been removed
212 if (shape_A == -1 || shape_B == -1) {
213 collided = false;
214 return false;
215 }
216
217 //cannot collide
218 if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) {
219 collided = false;
220 return false;
221 }
222
223 offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
224
225 validate_contacts();
226
227 Vector3 offset_A = A->get_transform().get_origin();
228 Transform xform_Au = Transform(A->get_transform().basis, Vector3());
229 Transform xform_A = xform_Au * A->get_shape_transform(shape_A);
230
231 Transform xform_Bu = B->get_transform();
232 xform_Bu.origin -= offset_A;
233 Transform xform_B = xform_Bu * B->get_shape_transform(shape_B);
234
235 ShapeSW *shape_A_ptr = A->get_shape(shape_A);
236 ShapeSW *shape_B_ptr = B->get_shape(shape_B);
237
238 bool collided = CollisionSolverSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
239 this->collided = collided;
240
241 if (!collided) {
242
243 //test ccd (currently just a raycast)
244
245 if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) {
246 _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
247 }
248
249 if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) {
250 _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
251 }
252
253 return false;
254 }
255
256 real_t max_penetration = space->get_contact_max_allowed_penetration();
257
258 float bias = 0.3f;
259
260 if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
261
262 if (shape_A_ptr->get_custom_bias() == 0)
263 bias = shape_B_ptr->get_custom_bias();
264 else if (shape_B_ptr->get_custom_bias() == 0)
265 bias = shape_A_ptr->get_custom_bias();
266 else
267 bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5;
268 }
269
270 real_t inv_dt = 1.0 / p_step;
271
272 for (int i = 0; i < contact_count; i++) {
273
274 Contact &c = contacts[i];
275 c.active = false;
276
277 Vector3 global_A = xform_Au.xform(c.local_A);
278 Vector3 global_B = xform_Bu.xform(c.local_B);
279
280 real_t depth = c.normal.dot(global_A - global_B);
281
282 if (depth <= 0) {
283 c.active = false;
284 continue;
285 }
286
287 c.active = true;
288
289 #ifdef DEBUG_ENABLED
290
291 if (space->is_debugging_contacts()) {
292 space->add_debug_contact(global_A + offset_A);
293 space->add_debug_contact(global_B + offset_A);
294 }
295 #endif
296
297 c.rA = global_A;
298 c.rB = global_B - offset_B;
299
300 // contact query reporting...
301 #if 0
302 if (A->get_body_type() == PhysicsServer::BODY_CHARACTER)
303 static_cast<CharacterBodySW*>(A)->report_character_contact( global_A, global_B, B );
304 if (B->get_body_type() == PhysicsServer::BODY_CHARACTER)
305 static_cast<CharacterBodySW*>(B)->report_character_contact( global_B, global_A, A );
306 if (A->has_contact_query())
307 A->report_contact( global_A, global_B, B );
308 if (B->has_contact_query())
309 B->report_contact( global_B, global_A, A );
310 #endif
311
312 if (A->can_report_contacts()) {
313 Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity();
314 A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA);
315 }
316
317 if (B->can_report_contacts()) {
318 Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity();
319 B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB);
320 }
321
322 if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) {
323 c.active = false;
324 collided = false;
325 continue;
326 }
327
328 c.active = true;
329
330 // Precompute normal mass, tangent mass, and bias.
331 Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal));
332 Vector3 inertia_B = B->get_inv_inertia_tensor().xform(c.rB.cross(c.normal));
333 real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
334 kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB));
335 c.mass_normal = 1.0f / kNormal;
336
337 #if 1
338 c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
339
340 #else
341 if (depth > max_penetration) {
342 c.bias = (depth - max_penetration) * (1.0 / (p_step * (1.0 / RELAXATION_TIMESTEPS)));
343 } else {
344 float approach = -0.1f * (depth - max_penetration) / (CMP_EPSILON + max_penetration);
345 approach = CLAMP(approach, CMP_EPSILON, 1.0);
346 c.bias = approach * (depth - max_penetration) * (1.0 / p_step);
347 }
348 #endif
349 c.depth = depth;
350
351 Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
352 A->apply_impulse(c.rA, -j_vec);
353 B->apply_impulse(c.rB, j_vec);
354 c.acc_bias_impulse = 0;
355 Vector3 jb_vec = c.normal * c.acc_bias_impulse;
356 A->apply_bias_impulse(c.rA, -jb_vec);
357 B->apply_bias_impulse(c.rB, jb_vec);
358
359 c.bounce = MAX(A->get_bounce(), B->get_bounce());
360 if (c.bounce) {
361
362 Vector3 crA = A->get_angular_velocity().cross(c.rA);
363 Vector3 crB = B->get_angular_velocity().cross(c.rB);
364 Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
365 //normal impule
366 c.bounce = c.bounce * dv.dot(c.normal);
367 }
368 }
369
370 return true;
371 }
372
solve(float p_step)373 void BodyPairSW::solve(float p_step) {
374
375 if (!collided)
376 return;
377
378 for (int i = 0; i < contact_count; i++) {
379
380 Contact &c = contacts[i];
381 if (!c.active)
382 continue;
383
384 c.active = false; //try to deactivate, will activate itself if still needed
385
386 //bias impule
387
388 Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA);
389 Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB);
390 Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
391
392 real_t vbn = dbv.dot(c.normal);
393
394 if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
395
396 real_t jbn = (-vbn + c.bias) * c.mass_normal;
397 real_t jbnOld = c.acc_bias_impulse;
398 c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
399
400 Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
401
402 A->apply_bias_impulse(c.rA, -jb);
403 B->apply_bias_impulse(c.rB, jb);
404
405 c.active = true;
406 }
407
408 Vector3 crA = A->get_angular_velocity().cross(c.rA);
409 Vector3 crB = B->get_angular_velocity().cross(c.rB);
410 Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
411
412 //normal impule
413 real_t vn = dv.dot(c.normal);
414
415 if (Math::abs(vn) > MIN_VELOCITY) {
416
417 real_t jn = -(c.bounce + vn) * c.mass_normal;
418 real_t jnOld = c.acc_normal_impulse;
419 c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
420
421 Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
422
423 A->apply_impulse(c.rA, -j);
424 B->apply_impulse(c.rB, j);
425
426 c.active = true;
427 }
428
429 //friction impule
430
431 real_t friction = A->get_friction() * B->get_friction();
432
433 Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA);
434 Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB);
435
436 Vector3 dtv = lvB - lvA;
437 real_t tn = c.normal.dot(dtv);
438
439 // tangential velocity
440 Vector3 tv = dtv - c.normal * tn;
441 real_t tvl = tv.length();
442
443 if (tvl > MIN_VELOCITY) {
444
445 tv /= tvl;
446
447 Vector3 temp1 = A->get_inv_inertia_tensor().xform(c.rA.cross(tv));
448 Vector3 temp2 = B->get_inv_inertia_tensor().xform(c.rB.cross(tv));
449
450 real_t t = -tvl /
451 (A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
452
453 Vector3 jt = t * tv;
454
455 Vector3 jtOld = c.acc_tangent_impulse;
456 c.acc_tangent_impulse += jt;
457
458 real_t fi_len = c.acc_tangent_impulse.length();
459 real_t jtMax = c.acc_normal_impulse * friction;
460
461 if (fi_len > CMP_EPSILON && fi_len > jtMax) {
462
463 c.acc_tangent_impulse *= jtMax / fi_len;
464 }
465
466 jt = c.acc_tangent_impulse - jtOld;
467
468 A->apply_impulse(c.rA, -jt);
469 B->apply_impulse(c.rB, jt);
470
471 c.active = true;
472 }
473 }
474 }
475
shift_shape_indices(const CollisionObjectSW * p_object,int p_removed_index)476 void BodyPairSW::shift_shape_indices(const CollisionObjectSW *p_object, int p_removed_index) {
477
478 if (p_object == A) {
479 if (shape_A == p_removed_index)
480 shape_A = -1;
481 else if (shape_A > p_removed_index)
482 shape_A--;
483 } else if (p_object == B) {
484 if (shape_B == p_removed_index)
485 shape_B = -1;
486 else if (shape_B > p_removed_index)
487 shape_B--;
488 }
489 }
490
BodyPairSW(BodySW * p_A,int p_shape_A,BodySW * p_B,int p_shape_B)491 BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B) :
492 ConstraintSW(_arr, 2) {
493
494 A = p_A;
495 B = p_B;
496 shape_A = p_shape_A;
497 shape_B = p_shape_B;
498 space = A->get_space();
499 A->add_constraint(this, 0);
500 B->add_constraint(this, 1);
501 contact_count = 0;
502 collided = false;
503 }
504
~BodyPairSW()505 BodyPairSW::~BodyPairSW() {
506
507 A->remove_constraint(this);
508 B->remove_constraint(this);
509 }
510