1/**************************************************************************/
2/* godot_body_pair_2d.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_2d.h"
32
33#include "godot_collision_solver_2d.h"
34#include "godot_space_2d.h"
35
36#define ACCUMULATE_IMPULSES
37
38#define MIN_VELOCITY 0.001
39#define MAX_BIAS_ROTATION (Math_PI / 8)
40
41void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) {
42 GodotBodyPair2D *self = static_cast<GodotBodyPair2D *>(p_self);
43
44 self->_contact_added_callback(p_point_A, p_point_B);
45}
46
47void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) {
48 Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
49 Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B);
50
51 int new_index = contact_count;
52
53 ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
54
55 Contact contact;
56 contact.local_A = local_A;
57 contact.local_B = local_B;
58 contact.normal = (p_point_A - p_point_B).normalized();
59 contact.used = true;
60
61 // Attempt to determine if the contact will be reused.
62 real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius();
63
64 for (int i = 0; i < contact_count; i++) {
65 Contact &c = contacts[i];
66 if (c.local_A.distance_squared_to(local_A) < (recycle_radius_2) &&
67 c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) {
68 contact.acc_normal_impulse = c.acc_normal_impulse;
69 contact.acc_tangent_impulse = c.acc_tangent_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 c = contact;
73 return;
74 }
75 }
76
77 // Figure out if the contact amount must be reduced to fit the new contact.
78 if (new_index == MAX_CONTACTS) {
79 // Remove the contact with the minimum depth.
80
81 const Transform2D &transform_A = A->get_transform();
82 const Transform2D &transform_B = B->get_transform();
83
84 int least_deep = -1;
85 real_t min_depth;
86
87 // Start with depth for new contact.
88 {
89 Vector2 global_A = transform_A.basis_xform(contact.local_A);
90 Vector2 global_B = transform_B.basis_xform(contact.local_B) + offset_B;
91
92 Vector2 axis = global_A - global_B;
93 min_depth = axis.dot(contact.normal);
94 }
95
96 for (int i = 0; i < contact_count; i++) {
97 const Contact &c = contacts[i];
98 Vector2 global_A = transform_A.basis_xform(c.local_A);
99 Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
100
101 Vector2 axis = global_A - global_B;
102 real_t depth = axis.dot(c.normal);
103
104 if (depth < min_depth) {
105 min_depth = depth;
106 least_deep = i;
107 }
108 }
109
110 if (least_deep > -1) {
111 // Replace the least deep contact by the new one.
112 contacts[least_deep] = contact;
113 }
114
115 return;
116 }
117
118 contacts[new_index] = contact;
119 contact_count++;
120}
121
122void GodotBodyPair2D::_validate_contacts() {
123 // Make sure to erase contacts that are no longer valid.
124 real_t max_separation = space->get_contact_max_separation();
125 real_t max_separation2 = max_separation * max_separation;
126
127 const Transform2D &transform_A = A->get_transform();
128 const Transform2D &transform_B = B->get_transform();
129
130 for (int i = 0; i < contact_count; i++) {
131 Contact &c = contacts[i];
132
133 bool erase = false;
134 if (!c.used) {
135 // Was left behind in previous frame.
136 erase = true;
137 } else {
138 c.used = false;
139
140 Vector2 global_A = transform_A.basis_xform(c.local_A);
141 Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
142 Vector2 axis = global_A - global_B;
143 real_t depth = axis.dot(c.normal);
144
145 if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
146 erase = true;
147 }
148 }
149
150 if (erase) {
151 // Contact no longer needed, remove.
152
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 GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B) {
170 Vector2 motion = p_A->get_linear_velocity() * p_step;
171 real_t mlen = motion.length();
172 if (mlen < CMP_EPSILON) {
173 return false;
174 }
175
176 Vector2 mnormal = motion / mlen;
177
178 real_t min = 0.0, max = 0.0;
179 p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max);
180
181 // Did it move enough in this direction to even attempt raycast?
182 // Let's say it should move more than 1/3 the size of the object in that axis.
183 bool fast_object = mlen > (max - min) * 0.3;
184 if (!fast_object) {
185 return false;
186 }
187
188 // A is moving fast enough that tunneling might occur. See if it's really about to collide.
189
190 // Roughly predict body B's position in the next frame (ignoring collisions).
191 Transform2D predicted_xform_B = p_xform_B.translated(p_B->get_linear_velocity() * p_step);
192
193 // Cast a segment from support in motion normal, in the same direction of motion by motion length.
194 // Support point will the farthest forward collision point along the movement vector.
195 // i.e. the point that should hit B first if any collision does occur.
196
197 // convert mnormal into body A's local xform because get_support requires (and returns) local coordinates.
198 int a;
199 Vector2 s[2];
200 p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform_inv(mnormal).normalized(), s, a);
201 Vector2 from = p_xform_A.xform(s[0]);
202 // Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
203 // This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close.
204 Vector2 to = from + motion;
205
206 Transform2D from_inv = predicted_xform_B.affine_inverse();
207
208 // Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
209 // At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd. But it still works out.
210 Vector2 local_from = from_inv.xform(from - motion * 0.1);
211 Vector2 local_to = from_inv.xform(to);
212
213 Vector2 rpos, rnorm;
214 if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
215 // there was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
216 // actually collide yet on next frame. We'll probably check again next frame once they're closer.
217 return false;
218 }
219
220 // Check one-way collision based on motion direction.
221 if (p_A->get_shape(p_shape_A)->allows_one_way_collision() && p_B->is_shape_set_as_one_way_collision(p_shape_B)) {
222 Vector2 direction = predicted_xform_B.columns[1].normalized();
223 if (direction.dot(mnormal) < CMP_EPSILON) {
224 collided = false;
225 oneway_disabled = true;
226 return false;
227 }
228 }
229
230 // Shorten the linear velocity so it does not hit, but gets close enough,
231 // next frame will hit softly or soft enough.
232 Vector2 hitpos = predicted_xform_B.xform(rpos);
233
234 real_t newlen = hitpos.distance_to(from) + (max - min) * 0.01; // adding 1% of body length to the distance between collision and support point should cause body A's support point to arrive just within B's collider next frame.
235 p_A->set_linear_velocity(mnormal * (newlen / p_step));
236
237 return true;
238}
239
240real_t combine_bounce(GodotBody2D *A, GodotBody2D *B) {
241 return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1);
242}
243
244real_t combine_friction(GodotBody2D *A, GodotBody2D *B) {
245 return ABS(MIN(A->get_friction(), B->get_friction()));
246}
247
248bool GodotBodyPair2D::setup(real_t p_step) {
249 check_ccd = false;
250
251 if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
252 collided = false;
253 return false;
254 }
255
256 collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B);
257 collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A);
258
259 report_contacts_only = false;
260 if (!collide_A && !collide_B) {
261 if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
262 report_contacts_only = true;
263 } else {
264 collided = false;
265 return false;
266 }
267 }
268
269 //use local A coordinates to avoid numerical issues on collision detection
270 offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
271
272 _validate_contacts();
273
274 const Vector2 &offset_A = A->get_transform().get_origin();
275 Transform2D xform_Au = A->get_transform().untranslated();
276 Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
277
278 Transform2D xform_Bu = B->get_transform();
279 xform_Bu.columns[2] -= offset_A;
280 Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
281
282 GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
283 GodotShape2D *shape_B_ptr = B->get_shape(shape_B);
284
285 Vector2 motion_A, motion_B;
286
287 if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) {
288 motion_A = A->get_motion();
289 }
290 if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) {
291 motion_B = B->get_motion();
292 }
293
294 bool prev_collided = collided;
295
296 collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis);
297 if (!collided) {
298 oneway_disabled = false;
299
300 if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
301 check_ccd = true;
302 return true;
303 }
304
305 if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
306 check_ccd = true;
307 return true;
308 }
309
310 return false;
311 }
312
313 if (oneway_disabled) {
314 return false;
315 }
316
317 if (!prev_collided) {
318 if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) {
319 Vector2 direction = xform_A.columns[1].normalized();
320 bool valid = false;
321 for (int i = 0; i < contact_count; i++) {
322 Contact &c = contacts[i];
323 if (c.normal.dot(direction) > -CMP_EPSILON) { // Greater (normal inverted).
324 continue;
325 }
326 valid = true;
327 break;
328 }
329 if (!valid) {
330 collided = false;
331 oneway_disabled = true;
332 return false;
333 }
334 }
335
336 if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) {
337 Vector2 direction = xform_B.columns[1].normalized();
338 bool valid = false;
339 for (int i = 0; i < contact_count; i++) {
340 Contact &c = contacts[i];
341 if (c.normal.dot(direction) < CMP_EPSILON) { // Less (normal ok).
342 continue;
343 }
344 valid = true;
345 break;
346 }
347 if (!valid) {
348 collided = false;
349 oneway_disabled = true;
350 return false;
351 }
352 }
353 }
354
355 return true;
356}
357
358bool GodotBodyPair2D::pre_solve(real_t p_step) {
359 if (oneway_disabled) {
360 return false;
361 }
362
363 if (!collided) {
364 if (check_ccd) {
365 const Vector2 &offset_A = A->get_transform().get_origin();
366 Transform2D xform_Au = A->get_transform().untranslated();
367 Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
368
369 Transform2D xform_Bu = B->get_transform();
370 xform_Bu.columns[2] -= offset_A;
371 Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
372
373 if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
374 _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
375 }
376
377 if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
378 _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
379 }
380 }
381
382 return false;
383 }
384
385 real_t max_penetration = space->get_contact_max_allowed_penetration();
386
387 real_t bias = space->get_contact_bias();
388
389 GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
390 GodotShape2D *shape_B_ptr = B->get_shape(shape_B);
391
392 if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
393 if (shape_A_ptr->get_custom_bias() == 0) {
394 bias = shape_B_ptr->get_custom_bias();
395 } else if (shape_B_ptr->get_custom_bias() == 0) {
396 bias = shape_A_ptr->get_custom_bias();
397 } else {
398 bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5;
399 }
400 }
401
402 real_t inv_dt = 1.0 / p_step;
403
404 bool do_process = false;
405
406 const Vector2 &offset_A = A->get_transform().get_origin();
407 const Transform2D &transform_A = A->get_transform();
408 const Transform2D &transform_B = B->get_transform();
409
410 real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0;
411 real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0;
412
413 real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
414 real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
415
416 for (int i = 0; i < contact_count; i++) {
417 Contact &c = contacts[i];
418 c.active = false;
419
420 Vector2 global_A = transform_A.basis_xform(c.local_A);
421 Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
422
423 Vector2 axis = global_A - global_B;
424 real_t depth = axis.dot(c.normal);
425
426 if (depth <= 0.0) {
427 continue;
428 }
429
430#ifdef DEBUG_ENABLED
431 if (space->is_debugging_contacts()) {
432 space->add_debug_contact(global_A + offset_A);
433 space->add_debug_contact(global_B + offset_A);
434 }
435#endif
436
437 c.rA = global_A - A->get_center_of_mass();
438 c.rB = global_B - B->get_center_of_mass() - offset_B;
439
440 // Precompute normal mass, tangent mass, and bias.
441 real_t rnA = c.rA.dot(c.normal);
442 real_t rnB = c.rB.dot(c.normal);
443 real_t kNormal = inv_mass_A + inv_mass_B;
444 kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB);
445 c.mass_normal = 1.0f / kNormal;
446
447 Vector2 tangent = c.normal.orthogonal();
448 real_t rtA = c.rA.dot(tangent);
449 real_t rtB = c.rB.dot(tangent);
450 real_t kTangent = inv_mass_A + inv_mass_B;
451 kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB);
452 c.mass_tangent = 1.0f / kTangent;
453
454 c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
455 c.depth = depth;
456
457 Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
458
459 c.acc_impulse -= P;
460
461 if (A->can_report_contacts() || B->can_report_contacts()) {
462 Vector2 crB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity();
463 Vector2 crA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + A->get_linear_velocity();
464 if (A->can_report_contacts()) {
465 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);
466 }
467 if (B->can_report_contacts()) {
468 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);
469 }
470 }
471
472 if (report_contacts_only) {
473 collided = false;
474 continue;
475 }
476
477#ifdef ACCUMULATE_IMPULSES
478 {
479 // Apply normal + friction impulse
480 if (collide_A) {
481 A->apply_impulse(-P, c.rA + A->get_center_of_mass());
482 }
483 if (collide_B) {
484 B->apply_impulse(P, c.rB + B->get_center_of_mass());
485 }
486 }
487#endif
488
489 c.bounce = combine_bounce(A, B);
490 if (c.bounce) {
491 Vector2 crA(-A->get_prev_angular_velocity() * c.rA.y, A->get_prev_angular_velocity() * c.rA.x);
492 Vector2 crB(-B->get_prev_angular_velocity() * c.rB.y, B->get_prev_angular_velocity() * c.rB.x);
493 Vector2 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA;
494 c.bounce = c.bounce * dv.dot(c.normal);
495 }
496
497 c.active = true;
498 do_process = true;
499 }
500
501 return do_process;
502}
503
504void GodotBodyPair2D::solve(real_t p_step) {
505 if (!collided || oneway_disabled) {
506 return;
507 }
508
509 const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
510
511 real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
512 real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
513
514 for (int i = 0; i < contact_count; ++i) {
515 Contact &c = contacts[i];
516
517 if (!c.active) {
518 continue;
519 }
520
521 // Relative velocity at contact
522
523 Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
524 Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
525 Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
526
527 Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x);
528 Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x);
529 Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
530
531 real_t vn = dv.dot(c.normal);
532 real_t vbn = dbv.dot(c.normal);
533
534 Vector2 tangent = c.normal.orthogonal();
535 real_t vt = dv.dot(tangent);
536
537 real_t jbn = (c.bias - vbn) * c.mass_normal;
538 real_t jbnOld = c.acc_bias_impulse;
539 c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
540
541 Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
542
543 if (collide_A) {
544 A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av);
545 }
546 if (collide_B) {
547 B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av);
548 }
549
550 crbA = Vector2(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x);
551 crbB = Vector2(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x);
552 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
553
554 vbn = dbv.dot(c.normal);
555
556 if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
557 real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B);
558 real_t jbnOld_com = c.acc_bias_impulse_center_of_mass;
559 c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f);
560
561 Vector2 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
562
563 if (collide_A) {
564 A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
565 }
566 if (collide_B) {
567 B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
568 }
569 }
570
571 real_t jn = -(c.bounce + vn) * c.mass_normal;
572 real_t jnOld = c.acc_normal_impulse;
573 c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
574
575 real_t friction = combine_friction(A, B);
576
577 real_t jtMax = friction * c.acc_normal_impulse;
578 real_t jt = -vt * c.mass_tangent;
579 real_t jtOld = c.acc_tangent_impulse;
580 c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax);
581
582 Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
583
584 if (collide_A) {
585 A->apply_impulse(-j, c.rA + A->get_center_of_mass());
586 }
587 if (collide_B) {
588 B->apply_impulse(j, c.rB + B->get_center_of_mass());
589 }
590 c.acc_impulse -= j;
591 }
592}
593
594GodotBodyPair2D::GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B) :
595 GodotConstraint2D(_arr, 2) {
596 A = p_A;
597 B = p_B;
598 shape_A = p_shape_A;
599 shape_B = p_shape_B;
600 space = A->get_space();
601 A->add_constraint(this, 0);
602 B->add_constraint(this, 1);
603}
604
605GodotBodyPair2D::~GodotBodyPair2D() {
606 A->remove_constraint(this, 0);
607 B->remove_constraint(this, 1);
608}
609