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