1/**************************************************************************/
2/* godot_body_pair_3d.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_body_pair_3d.h"
32
33#include "godot_collision_solver_3d.h"
34#include "godot_space_3d.h"
35
36#include "core/os/os.h"
37
38#define MIN_VELOCITY 0.0001
39#define MAX_BIAS_ROTATION (Math_PI / 8)
40
41void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
42 GodotBodyPair3D *pair = static_cast<GodotBodyPair3D *>(p_userdata);
43 pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal);
44}
45
46void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) {
47 Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A);
48 Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B);
49
50 int new_index = contact_count;
51
52 ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
53
54 Contact contact;
55 contact.index_A = p_index_A;
56 contact.index_B = p_index_B;
57 contact.local_A = local_A;
58 contact.local_B = local_B;
59 contact.normal = (p_point_A - p_point_B).normalized();
60 contact.used = true;
61
62 // Attempt to determine if the contact will be reused.
63 real_t contact_recycle_radius = space->get_contact_recycle_radius();
64
65 for (int i = 0; i < contact_count; i++) {
66 Contact &c = contacts[i];
67 if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) &&
68 c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) {
69 contact.acc_normal_impulse = c.acc_normal_impulse;
70 contact.acc_bias_impulse = c.acc_bias_impulse;
71 contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
72 contact.acc_tangent_impulse = c.acc_tangent_impulse;
73 c = contact;
74 return;
75 }
76 }
77
78 // Figure out if the contact amount must be reduced to fit the new contact.
79 if (new_index == MAX_CONTACTS) {
80 // Remove the contact with the minimum depth.
81
82 const Basis &basis_A = A->get_transform().basis;
83 const Basis &basis_B = B->get_transform().basis;
84
85 int least_deep = -1;
86 real_t min_depth;
87
88 // Start with depth for new contact.
89 {
90 Vector3 global_A = basis_A.xform(contact.local_A);
91 Vector3 global_B = basis_B.xform(contact.local_B) + offset_B;
92
93 Vector3 axis = global_A - global_B;
94 min_depth = axis.dot(contact.normal);
95 }
96
97 for (int i = 0; i < contact_count; i++) {
98 const Contact &c = contacts[i];
99 Vector3 global_A = basis_A.xform(c.local_A);
100 Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
101
102 Vector3 axis = global_A - global_B;
103 real_t depth = axis.dot(c.normal);
104
105 if (depth < min_depth) {
106 min_depth = depth;
107 least_deep = i;
108 }
109 }
110
111 if (least_deep > -1) {
112 // Replace the least deep contact by the new one.
113 contacts[least_deep] = contact;
114 }
115
116 return;
117 }
118
119 contacts[new_index] = contact;
120 contact_count++;
121}
122
123void GodotBodyPair3D::validate_contacts() {
124 // Make sure to erase contacts that are no longer valid.
125 real_t max_separation = space->get_contact_max_separation();
126 real_t max_separation2 = max_separation * max_separation;
127
128 const Basis &basis_A = A->get_transform().basis;
129 const Basis &basis_B = B->get_transform().basis;
130
131 for (int i = 0; i < contact_count; i++) {
132 Contact &c = contacts[i];
133
134 bool erase = false;
135 if (!c.used) {
136 // Was left behind in previous frame.
137 erase = true;
138 } else {
139 c.used = false;
140
141 Vector3 global_A = basis_A.xform(c.local_A);
142 Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
143 Vector3 axis = global_A - global_B;
144 real_t depth = axis.dot(c.normal);
145
146 if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
147 erase = true;
148 }
149 }
150
151 if (erase) {
152 // Contact no longer needed, remove.
153 if ((i + 1) < contact_count) {
154 // Swap with the last one.
155 SWAP(contacts[i], contacts[contact_count - 1]);
156 }
157
158 i--;
159 contact_count--;
160 }
161 }
162}
163
164// _test_ccd prevents tunneling by slowing down a high velocity body that is about to collide so that next frame it will be at an appropriate location to collide (i.e. slight overlap)
165// Warning: the way velocity is adjusted down to cause a collision means the momentum will be weaker than it should for a bounce!
166// Process: only proceed if body A's motion is high relative to its size.
167// cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does.
168// adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it.
169bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B) {
170 GodotShape3D *shape_A_ptr = p_A->get_shape(p_shape_A);
171
172 Vector3 motion = p_A->get_linear_velocity() * p_step;
173 real_t mlen = motion.length();
174 if (mlen < CMP_EPSILON) {
175 return false;
176 }
177
178 Vector3 mnormal = motion / mlen;
179
180 real_t min = 0.0, max = 0.0;
181 shape_A_ptr->project_range(mnormal, p_xform_A, min, max);
182
183 // Did it move enough in this direction to even attempt raycast?
184 // Let's say it should move more than 1/3 the size of the object in that axis.
185 bool fast_object = mlen > (max - min) * 0.3;
186 if (!fast_object) {
187 return false; // moving slow enough that there's no chance of tunneling.
188 }
189
190 // A is moving fast enough that tunneling might occur. See if it's really about to collide.
191
192 // Roughly predict body B's position in the next frame (ignoring collisions).
193 Transform3D predicted_xform_B = p_xform_B.translated(p_B->get_linear_velocity() * p_step);
194
195 // Support points are the farthest forward points on A in the direction of the motion vector.
196 // i.e. the candidate points of which one should hit B first if any collision does occur.
197 static const int max_supports = 16;
198 Vector3 supports_A[max_supports];
199 int support_count_A;
200 GodotShape3D::FeatureType support_type_A;
201 // Convert mnormal into body A's local xform because get_supports requires (and returns) local coordinates.
202 shape_A_ptr->get_supports(p_xform_A.basis.xform_inv(mnormal).normalized(), max_supports, supports_A, support_count_A, support_type_A);
203
204 // Cast a segment from each support point of A in the motion direction.
205 int segment_support_idx = -1;
206 float segment_hit_length = FLT_MAX;
207 Vector3 segment_hit_local;
208 for (int i = 0; i < support_count_A; i++) {
209 supports_A[i] = p_xform_A.xform(supports_A[i]);
210
211 Vector3 from = supports_A[i];
212 Vector3 to = from + motion;
213
214 Transform3D from_inv = predicted_xform_B.affine_inverse();
215
216 // Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
217 // At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd.
218 // But it still works out.
219 Vector3 local_from = from_inv.xform(from - motion * 0.1);
220 Vector3 local_to = from_inv.xform(to);
221
222 Vector3 rpos, rnorm;
223 int fi = -1;
224 if (p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm, fi, true)) {
225 float hit_length = local_from.distance_to(rpos);
226 if (hit_length < segment_hit_length) {
227 segment_support_idx = i;
228 segment_hit_length = hit_length;
229 segment_hit_local = rpos;
230 }
231 }
232 }
233
234 if (segment_support_idx == -1) {
235 // There was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
236 // actually collide yet on next frame. We'll probably check again next frame once they're closer.
237 return false;
238 }
239
240 Vector3 hitpos = predicted_xform_B.xform(segment_hit_local);
241
242 real_t newlen = hitpos.distance_to(supports_A[segment_support_idx]);
243 // Adding 1% of body length to the distance between collision and support point
244 // should cause body A's support point to arrive just within B's collider next frame.
245 newlen += (max - min) * 0.01;
246 // FIXME: This doesn't always work well when colliding with a triangle face of a trimesh shape.
247
248 p_A->set_linear_velocity((mnormal * newlen) / p_step);
249
250 return true;
251}
252
253real_t combine_bounce(GodotBody3D *A, GodotBody3D *B) {
254 return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1);
255}
256
257real_t combine_friction(GodotBody3D *A, GodotBody3D *B) {
258 return ABS(MIN(A->get_friction(), B->get_friction()));
259}
260
261bool GodotBodyPair3D::setup(real_t p_step) {
262 check_ccd = false;
263
264 if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
265 collided = false;
266 return false;
267 }
268
269 collide_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && A->collides_with(B);
270 collide_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && B->collides_with(A);
271
272 report_contacts_only = false;
273 if (!collide_A && !collide_B) {
274 if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
275 report_contacts_only = true;
276 } else {
277 collided = false;
278 return false;
279 }
280 }
281
282 offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
283
284 validate_contacts();
285
286 const Vector3 &offset_A = A->get_transform().get_origin();
287 Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3());
288 Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A);
289
290 Transform3D xform_Bu = B->get_transform();
291 xform_Bu.origin -= offset_A;
292 Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B);
293
294 GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
295 GodotShape3D *shape_B_ptr = B->get_shape(shape_B);
296
297 collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
298
299 if (!collided) {
300 if (A->is_continuous_collision_detection_enabled() && collide_A) {
301 check_ccd = true;
302 return true;
303 }
304
305 if (B->is_continuous_collision_detection_enabled() && collide_B) {
306 check_ccd = true;
307 return true;
308 }
309
310 return false;
311 }
312
313 return true;
314}
315
316bool GodotBodyPair3D::pre_solve(real_t p_step) {
317 if (!collided) {
318 if (check_ccd) {
319 const Vector3 &offset_A = A->get_transform().get_origin();
320 Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3());
321 Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A);
322
323 Transform3D xform_Bu = B->get_transform();
324 xform_Bu.origin -= offset_A;
325 Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B);
326
327 if (A->is_continuous_collision_detection_enabled() && collide_A) {
328 _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
329 }
330
331 if (B->is_continuous_collision_detection_enabled() && collide_B) {
332 _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
333 }
334 }
335
336 return false;
337 }
338
339 real_t max_penetration = space->get_contact_max_allowed_penetration();
340
341 real_t bias = 0.8;
342
343 GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
344 GodotShape3D *shape_B_ptr = B->get_shape(shape_B);
345
346 if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
347 if (shape_A_ptr->get_custom_bias() == 0) {
348 bias = shape_B_ptr->get_custom_bias();
349 } else if (shape_B_ptr->get_custom_bias() == 0) {
350 bias = shape_A_ptr->get_custom_bias();
351 } else {
352 bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5;
353 }
354 }
355
356 real_t inv_dt = 1.0 / p_step;
357
358 bool do_process = false;
359
360 const Vector3 &offset_A = A->get_transform().get_origin();
361
362 const Basis &basis_A = A->get_transform().basis;
363 const Basis &basis_B = B->get_transform().basis;
364
365 Basis zero_basis;
366 zero_basis.set_zero();
367
368 const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis;
369 const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis;
370
371 real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
372 real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
373
374 for (int i = 0; i < contact_count; i++) {
375 Contact &c = contacts[i];
376 c.active = false;
377
378 Vector3 global_A = basis_A.xform(c.local_A);
379 Vector3 global_B = basis_B.xform(c.local_B) + offset_B;
380
381 Vector3 axis = global_A - global_B;
382 real_t depth = axis.dot(c.normal);
383
384 if (depth <= 0.0) {
385 continue;
386 }
387
388#ifdef DEBUG_ENABLED
389 if (space->is_debugging_contacts()) {
390 space->add_debug_contact(global_A + offset_A);
391 space->add_debug_contact(global_B + offset_A);
392 }
393#endif
394
395 c.rA = global_A - A->get_center_of_mass();
396 c.rB = global_B - B->get_center_of_mass() - offset_B;
397
398 // Precompute normal mass, tangent mass, and bias.
399 Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal));
400 Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal));
401 real_t kNormal = inv_mass_A + inv_mass_B;
402 kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB));
403 c.mass_normal = 1.0f / kNormal;
404
405 c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
406 c.depth = depth;
407
408 Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
409
410 c.acc_impulse -= j_vec;
411
412 // contact query reporting...
413
414 if (A->can_report_contacts() || B->can_report_contacts()) {
415 Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity();
416 Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity();
417
418 if (A->can_report_contacts()) {
419 A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse);
420 }
421
422 if (B->can_report_contacts()) {
423 B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, -c.acc_impulse);
424 }
425 }
426
427 if (report_contacts_only) {
428 collided = false;
429 continue;
430 }
431
432 c.active = true;
433 do_process = true;
434
435 if (collide_A) {
436 A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass());
437 }
438 if (collide_B) {
439 B->apply_impulse(j_vec, c.rB + B->get_center_of_mass());
440 }
441
442 c.bounce = combine_bounce(A, B);
443 if (c.bounce) {
444 Vector3 crA = A->get_prev_angular_velocity().cross(c.rA);
445 Vector3 crB = B->get_prev_angular_velocity().cross(c.rB);
446 Vector3 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA;
447 c.bounce = c.bounce * dv.dot(c.normal);
448 }
449 }
450
451 return do_process;
452}
453
454void GodotBodyPair3D::solve(real_t p_step) {
455 if (!collided) {
456 return;
457 }
458
459 const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
460
461 Basis zero_basis;
462 zero_basis.set_zero();
463
464 const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis;
465 const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis;
466
467 real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
468 real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
469
470 for (int i = 0; i < contact_count; i++) {
471 Contact &c = contacts[i];
472 if (!c.active) {
473 continue;
474 }
475
476 c.active = false; //try to deactivate, will activate itself if still needed
477
478 //bias impulse
479
480 Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA);
481 Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB);
482 Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
483
484 real_t vbn = dbv.dot(c.normal);
485
486 if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
487 real_t jbn = (-vbn + c.bias) * c.mass_normal;
488 real_t jbnOld = c.acc_bias_impulse;
489 c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
490
491 Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
492
493 if (collide_A) {
494 A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av);
495 }
496 if (collide_B) {
497 B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av);
498 }
499
500 crbA = A->get_biased_angular_velocity().cross(c.rA);
501 crbB = B->get_biased_angular_velocity().cross(c.rB);
502 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
503
504 vbn = dbv.dot(c.normal);
505
506 if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
507 real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B);
508 real_t jbnOld_com = c.acc_bias_impulse_center_of_mass;
509 c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f);
510
511 Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
512
513 if (collide_A) {
514 A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
515 }
516 if (collide_B) {
517 B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
518 }
519 }
520
521 c.active = true;
522 }
523
524 Vector3 crA = A->get_angular_velocity().cross(c.rA);
525 Vector3 crB = B->get_angular_velocity().cross(c.rB);
526 Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
527
528 //normal impulse
529 real_t vn = dv.dot(c.normal);
530
531 if (Math::abs(vn) > MIN_VELOCITY) {
532 real_t jn = -(c.bounce + vn) * c.mass_normal;
533 real_t jnOld = c.acc_normal_impulse;
534 c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
535
536 Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
537
538 if (collide_A) {
539 A->apply_impulse(-j, c.rA + A->get_center_of_mass());
540 }
541 if (collide_B) {
542 B->apply_impulse(j, c.rB + B->get_center_of_mass());
543 }
544 c.acc_impulse -= j;
545
546 c.active = true;
547 }
548
549 //friction impulse
550
551 real_t friction = combine_friction(A, B);
552
553 Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA);
554 Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB);
555
556 Vector3 dtv = lvB - lvA;
557 real_t tn = c.normal.dot(dtv);
558
559 // tangential velocity
560 Vector3 tv = dtv - c.normal * tn;
561 real_t tvl = tv.length();
562
563 if (tvl > MIN_VELOCITY) {
564 tv /= tvl;
565
566 Vector3 temp1 = inv_inertia_tensor_A.xform(c.rA.cross(tv));
567 Vector3 temp2 = inv_inertia_tensor_B.xform(c.rB.cross(tv));
568
569 real_t t = -tvl / (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
570
571 Vector3 jt = t * tv;
572
573 Vector3 jtOld = c.acc_tangent_impulse;
574 c.acc_tangent_impulse += jt;
575
576 real_t fi_len = c.acc_tangent_impulse.length();
577 real_t jtMax = c.acc_normal_impulse * friction;
578
579 if (fi_len > CMP_EPSILON && fi_len > jtMax) {
580 c.acc_tangent_impulse *= jtMax / fi_len;
581 }
582
583 jt = c.acc_tangent_impulse - jtOld;
584
585 if (collide_A) {
586 A->apply_impulse(-jt, c.rA + A->get_center_of_mass());
587 }
588 if (collide_B) {
589 B->apply_impulse(jt, c.rB + B->get_center_of_mass());
590 }
591 c.acc_impulse -= jt;
592
593 c.active = true;
594 }
595 }
596}
597
598GodotBodyPair3D::GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B) :
599 GodotBodyContact3D(_arr, 2) {
600 A = p_A;
601 B = p_B;
602 shape_A = p_shape_A;
603 shape_B = p_shape_B;
604 space = A->get_space();
605 A->add_constraint(this, 0);
606 B->add_constraint(this, 1);
607}
608
609GodotBodyPair3D::~GodotBodyPair3D() {
610 A->remove_constraint(this);
611 B->remove_constraint(this);
612}
613
614void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
615 GodotBodySoftBodyPair3D *pair = static_cast<GodotBodySoftBodyPair3D *>(p_userdata);
616 pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal);
617}
618
619void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) {
620 Vector3 local_A = body->get_inv_transform().xform(p_point_A);
621 Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B);
622
623 Contact contact;
624 contact.index_A = p_index_A;
625 contact.index_B = p_index_B;
626 contact.local_A = local_A;
627 contact.local_B = local_B;
628 contact.normal = (normal.dot((p_point_A - p_point_B)) < 0 ? -normal : normal);
629 contact.used = true;
630
631 // Attempt to determine if the contact will be reused.
632 real_t contact_recycle_radius = space->get_contact_recycle_radius();
633
634 uint32_t contact_count = contacts.size();
635 for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
636 Contact &c = contacts[contact_index];
637 if (c.index_B == p_index_B) {
638 if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) &&
639 c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) {
640 contact.acc_normal_impulse = c.acc_normal_impulse;
641 contact.acc_bias_impulse = c.acc_bias_impulse;
642 contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
643 contact.acc_tangent_impulse = c.acc_tangent_impulse;
644 }
645 c = contact;
646 return;
647 }
648 }
649
650 contacts.push_back(contact);
651}
652
653void GodotBodySoftBodyPair3D::validate_contacts() {
654 // Make sure to erase contacts that are no longer valid.
655 real_t max_separation = space->get_contact_max_separation();
656 real_t max_separation2 = max_separation * max_separation;
657
658 const Transform3D &transform_A = body->get_transform();
659
660 uint32_t contact_count = contacts.size();
661 for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
662 Contact &c = contacts[contact_index];
663
664 bool erase = false;
665 if (!c.used) {
666 // Was left behind in previous frame.
667 erase = true;
668 } else {
669 c.used = false;
670
671 Vector3 global_A = transform_A.xform(c.local_A);
672 Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
673 Vector3 axis = global_A - global_B;
674 real_t depth = axis.dot(c.normal);
675
676 if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
677 erase = true;
678 }
679 }
680
681 if (erase) {
682 // Contact no longer needed, remove.
683 if ((contact_index + 1) < contact_count) {
684 // Swap with the last one.
685 SWAP(c, contacts[contact_count - 1]);
686 }
687
688 contact_index--;
689 contact_count--;
690 }
691 }
692
693 contacts.resize(contact_count);
694}
695
696bool GodotBodySoftBodyPair3D::setup(real_t p_step) {
697 if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) {
698 collided = false;
699 return false;
700 }
701
702 body_collides = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && body->collides_with(soft_body);
703 soft_body_collides = soft_body->collides_with(body);
704
705 if (!body_collides && !soft_body_collides) {
706 if (body->get_max_contacts_reported() > 0) {
707 report_contacts_only = true;
708 } else {
709 collided = false;
710 return false;
711 }
712 }
713
714 const Transform3D &xform_Au = body->get_transform();
715 Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape);
716
717 Transform3D xform_Bu = soft_body->get_transform();
718 Transform3D xform_B = xform_Bu * soft_body->get_shape_transform(0);
719
720 validate_contacts();
721
722 GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
723 GodotShape3D *shape_B_ptr = soft_body->get_shape(0);
724
725 collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
726
727 return collided;
728}
729
730bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
731 if (!collided) {
732 return false;
733 }
734
735 real_t max_penetration = space->get_contact_max_allowed_penetration();
736
737 real_t bias = space->get_contact_bias();
738
739 GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
740
741 if (shape_A_ptr->get_custom_bias()) {
742 bias = shape_A_ptr->get_custom_bias();
743 }
744
745 real_t inv_dt = 1.0 / p_step;
746
747 bool do_process = false;
748
749 const Transform3D &transform_A = body->get_transform();
750
751 Basis zero_basis;
752 zero_basis.set_zero();
753
754 const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis;
755
756 real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0;
757
758 uint32_t contact_count = contacts.size();
759 for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
760 Contact &c = contacts[contact_index];
761 c.active = false;
762
763 real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0;
764 if ((node_inv_mass == 0.0) && (body_inv_mass == 0.0)) {
765 continue;
766 }
767
768 Vector3 global_A = transform_A.xform(c.local_A);
769 Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
770 Vector3 axis = global_A - global_B;
771 real_t depth = axis.dot(c.normal);
772
773 if (depth <= 0.0) {
774 continue;
775 }
776
777#ifdef DEBUG_ENABLED
778 if (space->is_debugging_contacts()) {
779 space->add_debug_contact(global_A);
780 space->add_debug_contact(global_B);
781 }
782#endif
783
784 c.rA = global_A - transform_A.origin - body->get_center_of_mass();
785 c.rB = global_B;
786
787 // Precompute normal mass, tangent mass, and bias.
788 Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal));
789 real_t kNormal = body_inv_mass + node_inv_mass;
790 kNormal += c.normal.dot(inertia_A.cross(c.rA));
791 c.mass_normal = 1.0f / kNormal;
792
793 c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
794 c.depth = depth;
795
796 Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
797 if (body_collides) {
798 body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass());
799 }
800 if (soft_body_collides) {
801 soft_body->apply_node_impulse(c.index_B, j_vec);
802 }
803 c.acc_impulse -= j_vec;
804
805 if (body->can_report_contacts()) {
806 Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity();
807 Vector3 crB = soft_body->get_node_velocity(c.index_B);
808 body->add_contact(global_A, -c.normal, depth, body_shape, crA, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crB, c.acc_impulse);
809 }
810 if (report_contacts_only) {
811 collided = false;
812 continue;
813 }
814
815 c.active = true;
816 do_process = true;
817
818 if (body_collides) {
819 body->set_active(true);
820 }
821
822 c.bounce = body->get_bounce();
823
824 if (c.bounce) {
825 Vector3 crA = body->get_angular_velocity().cross(c.rA);
826 Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA;
827
828 // Normal impulse.
829 c.bounce = c.bounce * dv.dot(c.normal);
830 }
831 }
832
833 return do_process;
834}
835
836void GodotBodySoftBodyPair3D::solve(real_t p_step) {
837 if (!collided) {
838 return;
839 }
840
841 const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
842
843 Basis zero_basis;
844 zero_basis.set_zero();
845
846 const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis;
847
848 real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0;
849
850 uint32_t contact_count = contacts.size();
851 for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
852 Contact &c = contacts[contact_index];
853 if (!c.active) {
854 continue;
855 }
856
857 c.active = false;
858
859 real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0;
860
861 // Bias impulse.
862 Vector3 crbA = body->get_biased_angular_velocity().cross(c.rA);
863 Vector3 dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA;
864
865 real_t vbn = dbv.dot(c.normal);
866
867 if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
868 real_t jbn = (-vbn + c.bias) * c.mass_normal;
869 real_t jbnOld = c.acc_bias_impulse;
870 c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
871
872 Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
873
874 if (body_collides) {
875 body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av);
876 }
877 if (soft_body_collides) {
878 soft_body->apply_node_bias_impulse(c.index_B, jb);
879 }
880
881 crbA = body->get_biased_angular_velocity().cross(c.rA);
882 dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA;
883
884 vbn = dbv.dot(c.normal);
885
886 if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
887 real_t jbn_com = (-vbn + c.bias) / (body_inv_mass + node_inv_mass);
888 real_t jbnOld_com = c.acc_bias_impulse_center_of_mass;
889 c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f);
890
891 Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
892
893 if (body_collides) {
894 body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f);
895 }
896 if (soft_body_collides) {
897 soft_body->apply_node_bias_impulse(c.index_B, jb_com);
898 }
899 }
900
901 c.active = true;
902 }
903
904 Vector3 crA = body->get_angular_velocity().cross(c.rA);
905 Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA;
906
907 // Normal impulse.
908 real_t vn = dv.dot(c.normal);
909
910 if (Math::abs(vn) > MIN_VELOCITY) {
911 real_t jn = -(c.bounce + vn) * c.mass_normal;
912 real_t jnOld = c.acc_normal_impulse;
913 c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
914
915 Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
916
917 if (body_collides) {
918 body->apply_impulse(-j, c.rA + body->get_center_of_mass());
919 }
920 if (soft_body_collides) {
921 soft_body->apply_node_impulse(c.index_B, j);
922 }
923 c.acc_impulse -= j;
924
925 c.active = true;
926 }
927
928 // Friction impulse.
929 real_t friction = body->get_friction();
930
931 Vector3 lvA = body->get_linear_velocity() + body->get_angular_velocity().cross(c.rA);
932 Vector3 lvB = soft_body->get_node_velocity(c.index_B);
933 Vector3 dtv = lvB - lvA;
934
935 real_t tn = c.normal.dot(dtv);
936
937 // Tangential velocity.
938 Vector3 tv = dtv - c.normal * tn;
939 real_t tvl = tv.length();
940
941 if (tvl > MIN_VELOCITY) {
942 tv /= tvl;
943
944 Vector3 temp1 = body_inv_inertia_tensor.xform(c.rA.cross(tv));
945
946 real_t t = -tvl / (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA)));
947
948 Vector3 jt = t * tv;
949
950 Vector3 jtOld = c.acc_tangent_impulse;
951 c.acc_tangent_impulse += jt;
952
953 real_t fi_len = c.acc_tangent_impulse.length();
954 real_t jtMax = c.acc_normal_impulse * friction;
955
956 if (fi_len > CMP_EPSILON && fi_len > jtMax) {
957 c.acc_tangent_impulse *= jtMax / fi_len;
958 }
959
960 jt = c.acc_tangent_impulse - jtOld;
961
962 if (body_collides) {
963 body->apply_impulse(-jt, c.rA + body->get_center_of_mass());
964 }
965 if (soft_body_collides) {
966 soft_body->apply_node_impulse(c.index_B, jt);
967 }
968 c.acc_impulse -= jt;
969
970 c.active = true;
971 }
972 }
973}
974
975GodotBodySoftBodyPair3D::GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B) :
976 GodotBodyContact3D(&body, 1) {
977 body = p_A;
978 soft_body = p_B;
979 body_shape = p_shape_A;
980 space = p_A->get_space();
981 body->add_constraint(this, 0);
982 soft_body->add_constraint(this);
983}
984
985GodotBodySoftBodyPair3D::~GodotBodySoftBodyPair3D() {
986 body->remove_constraint(this);
987 soft_body->remove_constraint(this);
988}
989