summaryrefslogtreecommitdiff
path: root/servers/physics_3d/body_3d_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d/body_3d_sw.cpp')
-rw-r--r--servers/physics_3d/body_3d_sw.cpp336
1 files changed, 184 insertions, 152 deletions
diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index ea6064cb4c..41745545d8 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -29,12 +29,14 @@
/*************************************************************************/
#include "body_3d_sw.h"
+
#include "area_3d_sw.h"
+#include "body_direct_state_3d_sw.h"
#include "space_3d_sw.h"
-void Body3DSW::_update_inertia() {
- if (get_space() && !inertia_update_list.in_list()) {
- get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+void Body3DSW::_mass_properties_changed() {
+ if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) {
+ get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
}
}
@@ -42,7 +44,7 @@ void Body3DSW::_update_transform_dependant() {
center_of_mass = get_transform().basis.xform(center_of_mass_local);
principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
- // update inertia tensor
+ // Update inertia tensor.
Basis tb = principal_inertia_axes;
Basis tbt = tb.transposed();
Basis diag;
@@ -50,74 +52,95 @@ void Body3DSW::_update_transform_dependant() {
_inv_inertia_tensor = tb * diag * tbt;
}
-void Body3DSW::update_inertias() {
+void Body3DSW::update_mass_properties() {
// Update shapes and motions.
switch (mode) {
case PhysicsServer3D::BODY_MODE_DYNAMIC: {
- // Update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet)
real_t total_area = 0;
-
for (int i = 0; i < get_shape_count(); i++) {
+ if (is_shape_disabled(i)) {
+ continue;
+ }
+
total_area += get_shape_area(i);
}
- // We have to recompute the center of mass.
- center_of_mass_local.zero();
+ if (calculate_center_of_mass) {
+ // We have to recompute the center of mass.
+ center_of_mass_local.zero();
- if (total_area != 0.0) {
- for (int i = 0; i < get_shape_count(); i++) {
- real_t area = get_shape_area(i);
+ if (total_area != 0.0) {
+ for (int i = 0; i < get_shape_count(); i++) {
+ if (is_shape_disabled(i)) {
+ continue;
+ }
- real_t mass = area * this->mass / total_area;
+ real_t area = get_shape_area(i);
- // NOTE: we assume that the shape origin is also its center of mass.
- center_of_mass_local += mass * get_shape_transform(i).origin;
- }
+ real_t mass = area * this->mass / total_area;
+
+ // NOTE: we assume that the shape origin is also its center of mass.
+ center_of_mass_local += mass * get_shape_transform(i).origin;
+ }
- center_of_mass_local /= mass;
+ center_of_mass_local /= mass;
+ }
}
- // Recompute the inertia tensor.
- Basis inertia_tensor;
- inertia_tensor.set_zero();
- bool inertia_set = false;
+ if (calculate_inertia) {
+ // Recompute the inertia tensor.
+ Basis inertia_tensor;
+ inertia_tensor.set_zero();
+ bool inertia_set = false;
- for (int i = 0; i < get_shape_count(); i++) {
- if (is_shape_disabled(i)) {
- continue;
- }
+ for (int i = 0; i < get_shape_count(); i++) {
+ if (is_shape_disabled(i)) {
+ continue;
+ }
- real_t area = get_shape_area(i);
- if (area == 0.0) {
- continue;
- }
+ real_t area = get_shape_area(i);
+ if (area == 0.0) {
+ continue;
+ }
+
+ inertia_set = true;
- inertia_set = true;
+ const Shape3DSW *shape = get_shape(i);
- const Shape3DSW *shape = get_shape(i);
+ real_t mass = area * this->mass / total_area;
- real_t mass = area * this->mass / total_area;
+ Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix();
+ Transform3D shape_transform = get_shape_transform(i);
+ Basis shape_basis = shape_transform.basis.orthonormalized();
- Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix();
- Transform3D shape_transform = get_shape_transform(i);
- Basis shape_basis = shape_transform.basis.orthonormalized();
+ // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
+ shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
- // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
- shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
+ Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
+ inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
+ }
- Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
- inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
- }
+ // Set the inertia to a valid value when there are no valid shapes.
+ if (!inertia_set) {
+ inertia_tensor.set_diagonal(Vector3(1.0, 1.0, 1.0));
+ }
- // Set the inertia to a valid value when there are no valid shapes.
- if (!inertia_set) {
- inertia_tensor.set_diagonal(Vector3(1.0, 1.0, 1.0));
- }
+ // Handle partial custom inertia.
+ if (inertia.x > 0.0) {
+ inertia_tensor[0][0] = inertia.x;
+ }
+ if (inertia.y > 0.0) {
+ inertia_tensor[1][1] = inertia.y;
+ }
+ if (inertia.z > 0.0) {
+ inertia_tensor[2][2] = inertia.z;
+ }
- // Compute the principal axes of inertia.
- principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
- _inv_inertia = inertia_tensor.get_main_diagonal().inverse();
+ // Compute the principal axes of inertia.
+ principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
+ _inv_inertia = inertia_tensor.get_main_diagonal().inverse();
+ }
if (mass) {
_inv_mass = 1.0 / mass;
@@ -126,10 +149,9 @@ void Body3DSW::update_inertias() {
}
} break;
-
case PhysicsServer3D::BODY_MODE_KINEMATIC:
case PhysicsServer3D::BODY_MODE_STATIC: {
- _inv_inertia_tensor.set_zero();
+ _inv_inertia = Vector3();
_inv_mass = 0;
} break;
case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: {
@@ -139,11 +161,15 @@ void Body3DSW::update_inertias() {
} break;
}
- //_update_shapes();
-
_update_transform_dependant();
}
+void Body3DSW::reset_mass_properties() {
+ calculate_inertia = true;
+ calculate_center_of_mass = true;
+ _mass_properties_changed();
+}
+
void Body3DSW::set_active(bool p_active) {
if (active == p_active) {
return;
@@ -163,7 +189,7 @@ void Body3DSW::set_active(bool p_active) {
}
}
-void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) {
+void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) {
switch (p_param) {
case PhysicsServer3D::BODY_PARAM_BOUNCE: {
bounce = p_value;
@@ -172,10 +198,33 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value)
friction = p_value;
} break;
case PhysicsServer3D::BODY_PARAM_MASS: {
- ERR_FAIL_COND(p_value <= 0);
- mass = p_value;
- _update_inertia();
-
+ real_t mass_value = p_value;
+ ERR_FAIL_COND(mass_value <= 0);
+ mass = mass_value;
+ if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC) {
+ _mass_properties_changed();
+ }
+ } break;
+ case PhysicsServer3D::BODY_PARAM_INERTIA: {
+ inertia = p_value;
+ if ((inertia.x <= 0.0) || (inertia.y <= 0.0) || (inertia.z <= 0.0)) {
+ calculate_inertia = true;
+ if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
+ _mass_properties_changed();
+ }
+ } else {
+ calculate_inertia = false;
+ if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
+ principal_inertia_axes_local.set_diagonal(Vector3(1.0, 1.0, 1.0));
+ _inv_inertia = inertia.inverse();
+ _update_transform_dependant();
+ }
+ }
+ } break;
+ case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+ calculate_center_of_mass = false;
+ center_of_mass_local = p_value;
+ _update_transform_dependant();
} break;
case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
gravity_scale = p_value;
@@ -191,7 +240,7 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value)
}
}
-real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
+Variant Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
switch (p_param) {
case PhysicsServer3D::BODY_PARAM_BOUNCE: {
return bounce;
@@ -202,6 +251,16 @@ real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
case PhysicsServer3D::BODY_PARAM_MASS: {
return mass;
} break;
+ case PhysicsServer3D::BODY_PARAM_INERTIA: {
+ if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
+ return _inv_inertia.inverse();
+ } else {
+ return Vector3();
+ }
+ } break;
+ case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+ return center_of_mass;
+ } break;
case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
return gravity_scale;
} break;
@@ -224,40 +283,42 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) {
mode = p_mode;
switch (p_mode) {
- //CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
case PhysicsServer3D::BODY_MODE_STATIC:
case PhysicsServer3D::BODY_MODE_KINEMATIC: {
_set_inv_transform(get_transform().affine_inverse());
_inv_mass = 0;
+ _inv_inertia = Vector3();
_set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC);
- //set_active(p_mode==PhysicsServer3D::BODY_MODE_KINEMATIC);
set_active(p_mode == PhysicsServer3D::BODY_MODE_KINEMATIC && contacts.size());
linear_velocity = Vector3();
angular_velocity = Vector3();
if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && prev != mode) {
first_time_kinematic = true;
}
+ _update_transform_dependant();
} break;
case PhysicsServer3D::BODY_MODE_DYNAMIC: {
_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+ if (!calculate_inertia) {
+ principal_inertia_axes_local.set_diagonal(Vector3(1.0, 1.0, 1.0));
+ _inv_inertia = inertia.inverse();
+ _update_transform_dependant();
+ }
+ _mass_properties_changed();
_set_static(false);
set_active(true);
} break;
case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: {
_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+ _inv_inertia = Vector3();
+ angular_velocity = Vector3();
+ _update_transform_dependant();
_set_static(false);
set_active(true);
- angular_velocity = Vector3();
- } break;
+ }
}
-
- _update_inertia();
- /*
- if (get_space())
- _update_queries();
- */
}
PhysicsServer3D::BodyMode Body3DSW::get_mode() const {
@@ -265,7 +326,7 @@ PhysicsServer3D::BodyMode Body3DSW::get_mode() const {
}
void Body3DSW::_shapes_changed() {
- _update_inertia();
+ _mass_properties_changed();
}
void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
@@ -300,10 +361,12 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
} break;
case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
linear_velocity = p_variant;
+ constant_linear_velocity = linear_velocity;
wakeup();
} break;
case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
angular_velocity = p_variant;
+ constant_angular_velocity = angular_velocity;
wakeup();
} break;
@@ -324,7 +387,7 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
} break;
case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
can_sleep = p_variant;
- if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
+ if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
set_active(true);
}
@@ -356,8 +419,8 @@ Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
void Body3DSW::set_space(Space3DSW *p_space) {
if (get_space()) {
- if (inertia_update_list.in_list()) {
- get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
+ if (mass_properties_update_list.in_list()) {
+ get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list);
}
if (active_list.in_list()) {
get_space()->body_remove_from_active_list(&active_list);
@@ -370,26 +433,17 @@ void Body3DSW::set_space(Space3DSW *p_space) {
_set_space(p_space);
if (get_space()) {
- _update_inertia();
+ _mass_properties_changed();
if (active) {
get_space()->body_add_to_active_list(&active_list);
}
}
-
- first_integration = true;
}
-void Body3DSW::_compute_area_gravity_and_dampenings(const Area3DSW *p_area) {
- if (p_area->is_gravity_point()) {
- if (p_area->get_gravity_distance_scale() > 0) {
- Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
- gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2));
- } else {
- gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
- }
- } else {
- gravity += p_area->get_gravity_vector() * p_area->get_gravity();
- }
+void Body3DSW::_compute_area_gravity_and_damping(const Area3DSW *p_area) {
+ Vector3 area_gravity;
+ p_area->compute_gravity(get_transform().get_origin(), area_gravity);
+ gravity += area_gravity;
area_linear_damp += p_area->get_linear_damp();
area_angular_damp += p_area->get_angular_damp();
@@ -431,7 +485,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
switch (mode) {
case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
- _compute_area_gravity_and_dampenings(aa[i].area);
+ _compute_area_gravity_and_damping(aa[i].area);
stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
} break;
case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
@@ -439,7 +493,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
gravity = Vector3(0, 0, 0);
area_angular_damp = 0;
area_linear_damp = 0;
- _compute_area_gravity_and_dampenings(aa[i].area);
+ _compute_area_gravity_and_damping(aa[i].area);
stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
} break;
default: {
@@ -449,7 +503,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
}
if (!stopped) {
- _compute_area_gravity_and_dampenings(def_area);
+ _compute_area_gravity_and_damping(def_area);
}
gravity *= gravity_scale;
@@ -478,7 +532,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
//compute motion, angular and etc. velocities from prev transform
motion = new_transform.origin - get_transform().origin;
do_motion = true;
- linear_velocity = motion / p_step;
+ linear_velocity = constant_linear_velocity + motion / p_step;
//compute a FAKE angular velocity, not so easy
Basis rot = new_transform.basis.orthonormalized() * get_transform().basis.orthonormalized().transposed();
@@ -487,9 +541,9 @@ void Body3DSW::integrate_forces(real_t p_step) {
rot.get_axis_angle(axis, angle);
axis.normalize();
- angular_velocity = axis * (angle / p_step);
+ angular_velocity = constant_angular_velocity + axis * (angle / p_step);
} else {
- if (!omit_force_integration && !first_integration) {
+ if (!omit_force_integration) {
//overridden by direct state query
Vector3 force = gravity * mass;
@@ -523,7 +577,6 @@ void Body3DSW::integrate_forces(real_t p_step) {
applied_force = Vector3();
applied_torque = Vector3();
- first_integration = false;
//motion=linear_velocity*p_step;
@@ -543,7 +596,7 @@ void Body3DSW::integrate_velocities(real_t p_step) {
return;
}
- if (fi_callback) {
+ if (fi_callback_data || body_state_callback) {
get_space()->body_add_to_state_query_list(&direct_state_query_list);
}
@@ -578,7 +631,7 @@ void Body3DSW::integrate_velocities(real_t p_step) {
real_t ang_vel = total_angular_velocity.length();
Transform3D transform = get_transform();
- if (ang_vel != 0.0) {
+ if (!Math::is_zero_approx(ang_vel)) {
Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
Basis rot(ang_vel_axis, ang_vel * p_step);
Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1);
@@ -600,11 +653,6 @@ void Body3DSW::integrate_velocities(real_t p_step) {
_set_inv_transform(get_transform().inverse());
_update_transform_dependant();
-
- /*
- if (fi_callback) {
- get_space()->body_add_to_state_query_list(&direct_state_query_list);
- */
}
/*
@@ -650,7 +698,7 @@ void Body3DSW::wakeup_neighbours() {
continue;
}
Body3DSW *b = n[i];
- if (b->mode != PhysicsServer3D::BODY_MODE_DYNAMIC) {
+ if (b->mode < PhysicsServer3D::BODY_MODE_DYNAMIC) {
continue;
}
@@ -662,24 +710,23 @@ void Body3DSW::wakeup_neighbours() {
}
void Body3DSW::call_queries() {
- if (fi_callback) {
- PhysicsDirectBodyState3DSW *dbs = PhysicsDirectBodyState3DSW::singleton;
- dbs->body = this;
-
- Variant v = dbs;
-
- Object *obj = fi_callback->callable.get_object();
- if (!obj) {
+ if (fi_callback_data) {
+ if (!fi_callback_data->callable.get_object()) {
set_force_integration_callback(Callable());
} else {
- const Variant *vp[2] = { &v, &fi_callback->udata };
+ Variant direct_state_variant = get_direct_state();
+ const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata };
Callable::CallError ce;
- int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2;
+ int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2;
Variant rv;
- fi_callback->callable.call(vp, argc, rv, ce);
+ fi_callback_data->callable.call(vp, argc, rv, ce);
}
}
+
+ if (body_state_callback_instance) {
+ (body_state_callback)(body_state_callback_instance, get_direct_state());
+ }
}
bool Body3DSW::sleep_test(real_t p_step) {
@@ -699,60 +746,45 @@ bool Body3DSW::sleep_test(real_t p_step) {
}
}
+void Body3DSW::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) {
+ body_state_callback_instance = p_instance;
+ body_state_callback = p_callback;
+}
+
void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
- if (fi_callback) {
- memdelete(fi_callback);
- fi_callback = nullptr;
+ if (p_callable.get_object()) {
+ if (!fi_callback_data) {
+ fi_callback_data = memnew(ForceIntegrationCallbackData);
+ }
+ fi_callback_data->callable = p_callable;
+ fi_callback_data->udata = p_udata;
+ } else if (fi_callback_data) {
+ memdelete(fi_callback_data);
+ fi_callback_data = nullptr;
}
+}
- if (p_callable.get_object()) {
- fi_callback = memnew(ForceIntegrationCallback);
- fi_callback->callable = p_callable;
- fi_callback->udata = p_udata;
+PhysicsDirectBodyState3DSW *Body3DSW::get_direct_state() {
+ if (!direct_state) {
+ direct_state = memnew(PhysicsDirectBodyState3DSW);
+ direct_state->body = this;
}
+ return direct_state;
}
Body3DSW::Body3DSW() :
CollisionObject3DSW(TYPE_BODY),
-
active_list(this),
- inertia_update_list(this),
+ mass_properties_update_list(this),
direct_state_query_list(this) {
- mode = PhysicsServer3D::BODY_MODE_DYNAMIC;
- active = true;
-
- mass = 1;
- _inv_mass = 1;
- bounce = 0;
- friction = 1;
- omit_force_integration = false;
- //applied_torque=0;
- island_step = 0;
- first_time_kinematic = false;
- first_integration = false;
_set_static(false);
-
- contact_count = 0;
- gravity_scale = 1.0;
- linear_damp = -1;
- angular_damp = -1;
- area_angular_damp = 0;
- area_linear_damp = 0;
-
- still_time = 0;
- continuous_cd = false;
- can_sleep = true;
- fi_callback = nullptr;
}
Body3DSW::~Body3DSW() {
- if (fi_callback) {
- memdelete(fi_callback);
+ if (fi_callback_data) {
+ memdelete(fi_callback_data);
+ }
+ if (direct_state) {
+ memdelete(direct_state);
}
-}
-
-PhysicsDirectBodyState3DSW *PhysicsDirectBodyState3DSW::singleton = nullptr;
-
-PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() {
- return body->get_space()->get_direct_state();
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-29 13:41:56 +0000