1/**************************************************************************/
2/* godot_body_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_2d.h"
32
33#include "godot_area_2d.h"
34#include "godot_body_direct_state_2d.h"
35#include "godot_space_2d.h"
36
37void GodotBody2D::_mass_properties_changed() {
38 if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) {
39 get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
40 }
41}
42
43void GodotBody2D::update_mass_properties() {
44 //update shapes and motions
45
46 switch (mode) {
47 case PhysicsServer2D::BODY_MODE_RIGID: {
48 real_t total_area = 0;
49 for (int i = 0; i < get_shape_count(); i++) {
50 if (is_shape_disabled(i)) {
51 continue;
52 }
53 total_area += get_shape_aabb(i).get_area();
54 }
55
56 if (calculate_center_of_mass) {
57 // We have to recompute the center of mass.
58 center_of_mass_local = Vector2();
59
60 if (total_area != 0.0) {
61 for (int i = 0; i < get_shape_count(); i++) {
62 if (is_shape_disabled(i)) {
63 continue;
64 }
65
66 real_t area = get_shape_aabb(i).get_area();
67
68 real_t mass_new = area * mass / total_area;
69
70 // NOTE: we assume that the shape origin is also its center of mass.
71 center_of_mass_local += mass_new * get_shape_transform(i).get_origin();
72 }
73
74 center_of_mass_local /= mass;
75 }
76 }
77
78 if (calculate_inertia) {
79 inertia = 0;
80
81 for (int i = 0; i < get_shape_count(); i++) {
82 if (is_shape_disabled(i)) {
83 continue;
84 }
85
86 const GodotShape2D *shape = get_shape(i);
87
88 real_t area = get_shape_aabb(i).get_area();
89 if (area == 0.0) {
90 continue;
91 }
92
93 real_t mass_new = area * mass / total_area;
94
95 Transform2D mtx = get_shape_transform(i);
96 Vector2 scale = mtx.get_scale();
97 Vector2 shape_origin = mtx.get_origin() - center_of_mass_local;
98 inertia += shape->get_moment_of_inertia(mass_new, scale) + mass_new * shape_origin.length_squared();
99 }
100 }
101
102 _inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0;
103
104 if (mass) {
105 _inv_mass = 1.0 / mass;
106 } else {
107 _inv_mass = 0;
108 }
109
110 } break;
111 case PhysicsServer2D::BODY_MODE_KINEMATIC:
112 case PhysicsServer2D::BODY_MODE_STATIC: {
113 _inv_inertia = 0;
114 _inv_mass = 0;
115 } break;
116 case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: {
117 _inv_inertia = 0;
118 _inv_mass = 1.0 / mass;
119
120 } break;
121 }
122
123 _update_transform_dependent();
124}
125
126void GodotBody2D::reset_mass_properties() {
127 calculate_inertia = true;
128 calculate_center_of_mass = true;
129 _mass_properties_changed();
130}
131
132void GodotBody2D::set_active(bool p_active) {
133 if (active == p_active) {
134 return;
135 }
136
137 active = p_active;
138
139 if (active) {
140 if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
141 // Static bodies can't be active.
142 active = false;
143 } else if (get_space()) {
144 get_space()->body_add_to_active_list(&active_list);
145 }
146 } else if (get_space()) {
147 get_space()->body_remove_from_active_list(&active_list);
148 }
149}
150
151void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) {
152 switch (p_param) {
153 case PhysicsServer2D::BODY_PARAM_BOUNCE: {
154 bounce = p_value;
155 } break;
156 case PhysicsServer2D::BODY_PARAM_FRICTION: {
157 friction = p_value;
158 } break;
159 case PhysicsServer2D::BODY_PARAM_MASS: {
160 real_t mass_value = p_value;
161 ERR_FAIL_COND(mass_value <= 0);
162 mass = mass_value;
163 if (mode >= PhysicsServer2D::BODY_MODE_RIGID) {
164 _mass_properties_changed();
165 }
166 } break;
167 case PhysicsServer2D::BODY_PARAM_INERTIA: {
168 real_t inertia_value = p_value;
169 if (inertia_value <= 0.0) {
170 calculate_inertia = true;
171 if (mode == PhysicsServer2D::BODY_MODE_RIGID) {
172 _mass_properties_changed();
173 }
174 } else {
175 calculate_inertia = false;
176 inertia = inertia_value;
177 if (mode == PhysicsServer2D::BODY_MODE_RIGID) {
178 _inv_inertia = 1.0 / inertia;
179 }
180 }
181 } break;
182 case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
183 calculate_center_of_mass = false;
184 center_of_mass_local = p_value;
185 _update_transform_dependent();
186 } break;
187 case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
188 if (Math::is_zero_approx(gravity_scale)) {
189 wakeup();
190 }
191 gravity_scale = p_value;
192 } break;
193 case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: {
194 int mode_value = p_value;
195 linear_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value;
196 } break;
197 case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: {
198 int mode_value = p_value;
199 angular_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value;
200 } break;
201 case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: {
202 linear_damp = p_value;
203 } break;
204 case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: {
205 angular_damp = p_value;
206 } break;
207 default: {
208 }
209 }
210}
211
212Variant GodotBody2D::get_param(PhysicsServer2D::BodyParameter p_param) const {
213 switch (p_param) {
214 case PhysicsServer2D::BODY_PARAM_BOUNCE: {
215 return bounce;
216 }
217 case PhysicsServer2D::BODY_PARAM_FRICTION: {
218 return friction;
219 }
220 case PhysicsServer2D::BODY_PARAM_MASS: {
221 return mass;
222 }
223 case PhysicsServer2D::BODY_PARAM_INERTIA: {
224 return inertia;
225 }
226 case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: {
227 return center_of_mass_local;
228 }
229 case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: {
230 return gravity_scale;
231 }
232 case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: {
233 return linear_damp_mode;
234 }
235 case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: {
236 return angular_damp_mode;
237 }
238 case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: {
239 return linear_damp;
240 }
241 case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: {
242 return angular_damp;
243 }
244 default: {
245 }
246 }
247
248 return 0;
249}
250
251void GodotBody2D::set_mode(PhysicsServer2D::BodyMode p_mode) {
252 PhysicsServer2D::BodyMode prev = mode;
253 mode = p_mode;
254
255 switch (p_mode) {
256 //CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
257 case PhysicsServer2D::BODY_MODE_STATIC:
258 case PhysicsServer2D::BODY_MODE_KINEMATIC: {
259 _set_inv_transform(get_transform().affine_inverse());
260 _inv_mass = 0;
261 _inv_inertia = 0;
262 _set_static(p_mode == PhysicsServer2D::BODY_MODE_STATIC);
263 set_active(p_mode == PhysicsServer2D::BODY_MODE_KINEMATIC && contacts.size());
264 linear_velocity = Vector2();
265 angular_velocity = 0;
266 if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && prev != mode) {
267 first_time_kinematic = true;
268 }
269 } break;
270 case PhysicsServer2D::BODY_MODE_RIGID: {
271 _inv_mass = mass > 0 ? (1.0 / mass) : 0;
272 if (!calculate_inertia) {
273 _inv_inertia = 1.0 / inertia;
274 }
275 _mass_properties_changed();
276 _set_static(false);
277 set_active(true);
278
279 } break;
280 case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: {
281 _inv_mass = mass > 0 ? (1.0 / mass) : 0;
282 _inv_inertia = 0;
283 angular_velocity = 0;
284 _set_static(false);
285 set_active(true);
286 }
287 }
288}
289
290PhysicsServer2D::BodyMode GodotBody2D::get_mode() const {
291 return mode;
292}
293
294void GodotBody2D::_shapes_changed() {
295 _mass_properties_changed();
296 wakeup();
297 wakeup_neighbours();
298}
299
300void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) {
301 switch (p_state) {
302 case PhysicsServer2D::BODY_STATE_TRANSFORM: {
303 if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
304 new_transform = p_variant;
305 //wakeup_neighbours();
306 set_active(true);
307 if (first_time_kinematic) {
308 _set_transform(p_variant);
309 _set_inv_transform(get_transform().affine_inverse());
310 first_time_kinematic = false;
311 }
312 } else if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
313 _set_transform(p_variant);
314 _set_inv_transform(get_transform().affine_inverse());
315 wakeup_neighbours();
316 } else {
317 Transform2D t = p_variant;
318 t.orthonormalize();
319 new_transform = get_transform(); //used as old to compute motion
320 if (t == new_transform) {
321 break;
322 }
323 _set_transform(t);
324 _set_inv_transform(get_transform().inverse());
325 _update_transform_dependent();
326 }
327 wakeup();
328
329 } break;
330 case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: {
331 linear_velocity = p_variant;
332 constant_linear_velocity = linear_velocity;
333 wakeup();
334
335 } break;
336 case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: {
337 angular_velocity = p_variant;
338 constant_angular_velocity = angular_velocity;
339 wakeup();
340
341 } break;
342 case PhysicsServer2D::BODY_STATE_SLEEPING: {
343 if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
344 break;
345 }
346 bool do_sleep = p_variant;
347 if (do_sleep) {
348 linear_velocity = Vector2();
349 //biased_linear_velocity=Vector3();
350 angular_velocity = 0;
351 //biased_angular_velocity=Vector3();
352 set_active(false);
353 } else {
354 if (mode != PhysicsServer2D::BODY_MODE_STATIC) {
355 set_active(true);
356 }
357 }
358 } break;
359 case PhysicsServer2D::BODY_STATE_CAN_SLEEP: {
360 can_sleep = p_variant;
361 if (mode >= PhysicsServer2D::BODY_MODE_RIGID && !active && !can_sleep) {
362 set_active(true);
363 }
364
365 } break;
366 }
367}
368
369Variant GodotBody2D::get_state(PhysicsServer2D::BodyState p_state) const {
370 switch (p_state) {
371 case PhysicsServer2D::BODY_STATE_TRANSFORM: {
372 return get_transform();
373 }
374 case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: {
375 return linear_velocity;
376 }
377 case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: {
378 return angular_velocity;
379 }
380 case PhysicsServer2D::BODY_STATE_SLEEPING: {
381 return !is_active();
382 }
383 case PhysicsServer2D::BODY_STATE_CAN_SLEEP: {
384 return can_sleep;
385 }
386 }
387
388 return Variant();
389}
390
391void GodotBody2D::set_space(GodotSpace2D *p_space) {
392 if (get_space()) {
393 wakeup_neighbours();
394
395 if (mass_properties_update_list.in_list()) {
396 get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list);
397 }
398 if (active_list.in_list()) {
399 get_space()->body_remove_from_active_list(&active_list);
400 }
401 if (direct_state_query_list.in_list()) {
402 get_space()->body_remove_from_state_query_list(&direct_state_query_list);
403 }
404 }
405
406 _set_space(p_space);
407
408 if (get_space()) {
409 _mass_properties_changed();
410 if (active) {
411 get_space()->body_add_to_active_list(&active_list);
412 }
413 }
414}
415
416void GodotBody2D::_update_transform_dependent() {
417 center_of_mass = get_transform().basis_xform(center_of_mass_local);
418}
419
420void GodotBody2D::integrate_forces(real_t p_step) {
421 if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
422 return;
423 }
424
425 ERR_FAIL_COND(!get_space());
426
427 int ac = areas.size();
428
429 bool gravity_done = false;
430 bool linear_damp_done = false;
431 bool angular_damp_done = false;
432
433 bool stopped = false;
434
435 gravity = Vector2(0, 0);
436
437 total_linear_damp = 0.0;
438 total_angular_damp = 0.0;
439
440 // Combine gravity and damping from overlapping areas in priority order.
441 if (ac) {
442 areas.sort();
443 const AreaCMP *aa = &areas[0];
444 for (int i = ac - 1; i >= 0 && !stopped; i--) {
445 if (!gravity_done) {
446 PhysicsServer2D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE);
447 if (area_gravity_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
448 Vector2 area_gravity;
449 aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity);
450 switch (area_gravity_mode) {
451 case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
452 case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
453 gravity += area_gravity;
454 gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
455 } break;
456 case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
457 case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
458 gravity = area_gravity;
459 gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
460 } break;
461 default: {
462 }
463 }
464 }
465 }
466 if (!linear_damp_done) {
467 PhysicsServer2D::AreaSpaceOverrideMode area_linear_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE);
468 if (area_linear_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
469 real_t area_linear_damp = aa[i].area->get_linear_damp();
470 switch (area_linear_damp_mode) {
471 case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
472 case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
473 total_linear_damp += area_linear_damp;
474 linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
475 } break;
476 case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
477 case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
478 total_linear_damp = area_linear_damp;
479 linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
480 } break;
481 default: {
482 }
483 }
484 }
485 }
486 if (!angular_damp_done) {
487 PhysicsServer2D::AreaSpaceOverrideMode area_angular_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE);
488 if (area_angular_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
489 real_t area_angular_damp = aa[i].area->get_angular_damp();
490 switch (area_angular_damp_mode) {
491 case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE:
492 case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
493 total_angular_damp += area_angular_damp;
494 angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
495 } break;
496 case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE:
497 case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
498 total_angular_damp = area_angular_damp;
499 angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE;
500 } break;
501 default: {
502 }
503 }
504 }
505 }
506 stopped = gravity_done && linear_damp_done && angular_damp_done;
507 }
508 }
509
510 // Add default gravity and damping from space area.
511 if (!stopped) {
512 GodotArea2D *default_area = get_space()->get_default_area();
513 ERR_FAIL_COND(!default_area);
514
515 if (!gravity_done) {
516 Vector2 default_gravity;
517 default_area->compute_gravity(get_transform().get_origin(), default_gravity);
518 gravity += default_gravity;
519 }
520
521 if (!linear_damp_done) {
522 total_linear_damp += default_area->get_linear_damp();
523 }
524
525 if (!angular_damp_done) {
526 total_angular_damp += default_area->get_angular_damp();
527 }
528 }
529
530 // Override linear damping with body's value.
531 switch (linear_damp_mode) {
532 case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: {
533 total_linear_damp += linear_damp;
534 } break;
535 case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: {
536 total_linear_damp = linear_damp;
537 } break;
538 }
539
540 // Override angular damping with body's value.
541 switch (angular_damp_mode) {
542 case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: {
543 total_angular_damp += angular_damp;
544 } break;
545 case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: {
546 total_angular_damp = angular_damp;
547 } break;
548 }
549
550 gravity *= gravity_scale;
551
552 prev_linear_velocity = linear_velocity;
553 prev_angular_velocity = angular_velocity;
554
555 Vector2 motion;
556 bool do_motion = false;
557
558 if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
559 //compute motion, angular and etc. velocities from prev transform
560 motion = new_transform.get_origin() - get_transform().get_origin();
561 linear_velocity = constant_linear_velocity + motion / p_step;
562
563 real_t rot = new_transform.get_rotation() - get_transform().get_rotation();
564 angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step;
565
566 do_motion = true;
567
568 } else {
569 if (!omit_force_integration) {
570 //overridden by direct state query
571
572 Vector2 force = gravity * mass + applied_force + constant_force;
573 real_t torque = applied_torque + constant_torque;
574
575 real_t damp = 1.0 - p_step * total_linear_damp;
576
577 if (damp < 0) { // reached zero in the given time
578 damp = 0;
579 }
580
581 real_t angular_damp_new = 1.0 - p_step * total_angular_damp;
582
583 if (angular_damp_new < 0) { // reached zero in the given time
584 angular_damp_new = 0;
585 }
586
587 linear_velocity *= damp;
588 angular_velocity *= angular_damp_new;
589
590 linear_velocity += _inv_mass * force * p_step;
591 angular_velocity += _inv_inertia * torque * p_step;
592 }
593
594 if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
595 motion = linear_velocity * p_step;
596 do_motion = true;
597 }
598 }
599
600 applied_force = Vector2();
601 applied_torque = 0.0;
602
603 biased_angular_velocity = 0.0;
604 biased_linear_velocity = Vector2();
605
606 if (do_motion) { //shapes temporarily extend for raycast
607 _update_shapes_with_motion(motion);
608 }
609
610 contact_count = 0;
611}
612
613void GodotBody2D::integrate_velocities(real_t p_step) {
614 if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
615 return;
616 }
617
618 if (fi_callback_data || body_state_callback.get_object()) {
619 get_space()->body_add_to_state_query_list(&direct_state_query_list);
620 }
621
622 if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
623 _set_transform(new_transform, false);
624 _set_inv_transform(new_transform.affine_inverse());
625 if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) {
626 set_active(false); //stopped moving, deactivate
627 }
628 return;
629 }
630
631 real_t total_angular_velocity = angular_velocity + biased_angular_velocity;
632 Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity;
633
634 real_t angle_delta = total_angular_velocity * p_step;
635 real_t angle = get_transform().get_rotation() + angle_delta;
636 Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
637
638 if (center_of_mass.length_squared() > CMP_EPSILON2) {
639 // Calculate displacement due to center of mass offset.
640 pos += center_of_mass - center_of_mass.rotated(angle_delta);
641 }
642
643 _set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED);
644 _set_inv_transform(get_transform().inverse());
645
646 if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) {
647 new_transform = get_transform();
648 }
649
650 _update_transform_dependent();
651}
652
653void GodotBody2D::wakeup_neighbours() {
654 for (const Pair<GodotConstraint2D *, int> &E : constraint_list) {
655 const GodotConstraint2D *c = E.first;
656 GodotBody2D **n = c->get_body_ptr();
657 int bc = c->get_body_count();
658
659 for (int i = 0; i < bc; i++) {
660 if (i == E.second) {
661 continue;
662 }
663 GodotBody2D *b = n[i];
664 if (b->mode < PhysicsServer2D::BODY_MODE_RIGID) {
665 continue;
666 }
667
668 if (!b->is_active()) {
669 b->set_active(true);
670 }
671 }
672 }
673}
674
675void GodotBody2D::call_queries() {
676 Variant direct_state_variant = get_direct_state();
677
678 if (fi_callback_data) {
679 if (!fi_callback_data->callable.get_object()) {
680 set_force_integration_callback(Callable());
681 } else {
682 const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata };
683
684 Callable::CallError ce;
685 Variant rv;
686 if (fi_callback_data->udata.get_type() != Variant::NIL) {
687 fi_callback_data->callable.callp(vp, 2, rv, ce);
688
689 } else {
690 fi_callback_data->callable.callp(vp, 1, rv, ce);
691 }
692 }
693 }
694
695 if (body_state_callback.get_object()) {
696 const Variant *vp[1] = { &direct_state_variant };
697 Callable::CallError ce;
698 Variant rv;
699 body_state_callback.callp(vp, 1, rv, ce);
700 }
701}
702
703bool GodotBody2D::sleep_test(real_t p_step) {
704 if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
705 return true;
706 } else if (!can_sleep) {
707 return false;
708 }
709
710 if (Math::abs(angular_velocity) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) {
711 still_time += p_step;
712
713 return still_time > get_space()->get_body_time_to_sleep();
714 } else {
715 still_time = 0; //maybe this should be set to 0 on set_active?
716 return false;
717 }
718}
719
720void GodotBody2D::set_state_sync_callback(const Callable &p_callable) {
721 body_state_callback = p_callable;
722}
723
724void GodotBody2D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
725 if (p_callable.get_object()) {
726 if (!fi_callback_data) {
727 fi_callback_data = memnew(ForceIntegrationCallbackData);
728 }
729 fi_callback_data->callable = p_callable;
730 fi_callback_data->udata = p_udata;
731 } else if (fi_callback_data) {
732 memdelete(fi_callback_data);
733 fi_callback_data = nullptr;
734 }
735}
736
737GodotPhysicsDirectBodyState2D *GodotBody2D::get_direct_state() {
738 if (!direct_state) {
739 direct_state = memnew(GodotPhysicsDirectBodyState2D);
740 direct_state->body = this;
741 }
742 return direct_state;
743}
744
745GodotBody2D::GodotBody2D() :
746 GodotCollisionObject2D(TYPE_BODY),
747 active_list(this),
748 mass_properties_update_list(this),
749 direct_state_query_list(this) {
750 _set_static(false);
751}
752
753GodotBody2D::~GodotBody2D() {
754 if (fi_callback_data) {
755 memdelete(fi_callback_data);
756 }
757 if (direct_state) {
758 memdelete(direct_state);
759 }
760}
761