summaryrefslogtreecommitdiff
path: root/servers/physics_3d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d')
-rw-r--r--servers/physics_3d/body_3d_sw.cpp241
-rw-r--r--servers/physics_3d/body_3d_sw.h55
-rw-r--r--servers/physics_3d/physics_server_3d_sw.cpp11
-rw-r--r--servers/physics_3d/physics_server_3d_sw.h6
-rw-r--r--servers/physics_3d/physics_server_3d_wrap_mt.h6
-rw-r--r--servers/physics_3d/shape_3d_sw.cpp387
-rw-r--r--servers/physics_3d/shape_3d_sw.h21
-rw-r--r--servers/physics_3d/space_3d_sw.cpp14
-rw-r--r--servers/physics_3d/space_3d_sw.h18
9 files changed, 499 insertions, 260 deletions
diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index 7f62e4eb85..41745545d8 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -34,9 +34,9 @@
#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);
}
}
@@ -44,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;
@@ -52,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));
+ }
- // Compute the principal axes of inertia.
- principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
- _inv_inertia = inertia_tensor.get_main_diagonal().inverse();
+ // 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();
+ }
if (mass) {
_inv_mass = 1.0 / mass;
@@ -128,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: {
@@ -141,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;
@@ -165,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;
@@ -174,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;
@@ -193,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;
@@ -204,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;
@@ -226,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 {
@@ -267,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) {
@@ -302,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;
@@ -326,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);
}
@@ -358,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);
@@ -372,13 +433,11 @@ 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_damping(const Area3DSW *p_area) {
@@ -473,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();
@@ -482,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;
@@ -518,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;
@@ -640,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;
}
@@ -717,32 +775,9 @@ PhysicsDirectBodyState3DSW *Body3DSW::get_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;
}
Body3DSW::~Body3DSW() {
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index f58f40652b..8b74c7e5b9 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/body_3d_sw.h
@@ -39,24 +39,28 @@ class Constraint3DSW;
class PhysicsDirectBodyState3DSW;
class Body3DSW : public CollisionObject3DSW {
- PhysicsServer3D::BodyMode mode;
+ PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_DYNAMIC;
Vector3 linear_velocity;
Vector3 angular_velocity;
+ Vector3 constant_linear_velocity;
+ Vector3 constant_angular_velocity;
+
Vector3 biased_linear_velocity;
Vector3 biased_angular_velocity;
- real_t mass;
- real_t bounce;
- real_t friction;
+ real_t mass = 1.0;
+ real_t bounce = 0.0;
+ real_t friction = 1.0;
+ Vector3 inertia;
- real_t linear_damp;
- real_t angular_damp;
- real_t gravity_scale;
+ real_t linear_damp = -1.0;
+ real_t angular_damp = -1.0;
+ real_t gravity_scale = 1.0;
uint16_t locked_axis = 0;
- real_t _inv_mass;
+ real_t _inv_mass = 1.0;
Vector3 _inv_inertia; // Relative to the principal axes of inertia
// Relative to the local frame of reference
@@ -68,30 +72,32 @@ class Body3DSW : public CollisionObject3DSW {
Basis principal_inertia_axes;
Vector3 center_of_mass;
+ bool calculate_inertia = true;
+ bool calculate_center_of_mass = true;
+
Vector3 gravity;
- real_t still_time;
+ real_t still_time = 0.0;
Vector3 applied_force;
Vector3 applied_torque;
- real_t area_angular_damp;
- real_t area_linear_damp;
+ real_t area_angular_damp = 0.0;
+ real_t area_linear_damp = 0.0;
SelfList<Body3DSW> active_list;
- SelfList<Body3DSW> inertia_update_list;
+ SelfList<Body3DSW> mass_properties_update_list;
SelfList<Body3DSW> direct_state_query_list;
VSet<RID> exceptions;
- bool omit_force_integration;
- bool active;
+ bool omit_force_integration = false;
+ bool active = true;
- bool first_integration;
+ bool continuous_cd = false;
+ bool can_sleep = true;
+ bool first_time_kinematic = false;
- bool continuous_cd;
- bool can_sleep;
- bool first_time_kinematic;
- void _update_inertia();
+ void _mass_properties_changed();
virtual void _shapes_changed();
Transform3D new_transform;
@@ -112,7 +118,7 @@ class Body3DSW : public CollisionObject3DSW {
};
Vector<Contact> contacts; //no contacts by default
- int contact_count;
+ int contact_count = 0;
void *body_state_callback_instance = nullptr;
PhysicsServer3D::BodyStateCallback body_state_callback = nullptr;
@@ -126,7 +132,7 @@ class Body3DSW : public CollisionObject3DSW {
PhysicsDirectBodyState3DSW *direct_state = nullptr;
- uint64_t island_step;
+ uint64_t island_step = 0;
_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area3DSW *p_area);
@@ -251,8 +257,8 @@ public:
set_active(true);
}
- void set_param(PhysicsServer3D::BodyParameter p_param, real_t);
- real_t get_param(PhysicsServer3D::BodyParameter p_param) const;
+ void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value);
+ Variant get_param(PhysicsServer3D::BodyParameter p_param) const;
void set_mode(PhysicsServer3D::BodyMode p_mode);
PhysicsServer3D::BodyMode get_mode() const;
@@ -271,7 +277,8 @@ public:
void set_space(Space3DSW *p_space);
- void update_inertias();
+ void update_mass_properties();
+ void reset_mass_properties();
_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
_FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; }
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index a214e80c6c..8de54c5c48 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -643,20 +643,27 @@ uint32_t PhysicsServer3DSW::body_get_user_flags(RID p_body) const {
return 0;
};
-void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) {
+void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
Body3DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_param(p_param, p_value);
};
-real_t PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) const {
+Variant PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) const {
Body3DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, 0);
return body->get_param(p_param);
};
+void PhysicsServer3DSW::body_reset_mass_properties(RID p_body) {
+ Body3DSW *body = body_owner.getornull(p_body);
+ ERR_FAIL_COND(!body);
+
+ return body->reset_mass_properties();
+}
+
void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
Body3DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h
index f283f83112..071ad0a694 100644
--- a/servers/physics_3d/physics_server_3d_sw.h
+++ b/servers/physics_3d/physics_server_3d_sw.h
@@ -198,8 +198,10 @@ public:
virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override;
virtual uint32_t body_get_user_flags(RID p_body) const override;
- virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override;
- virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override;
+ virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override;
+ virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override;
+
+ virtual void body_reset_mass_properties(RID p_body) override;
virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
virtual Variant body_get_state(RID p_body, BodyState p_state) const override;
diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h
index 8865bd4bd2..58986969d4 100644
--- a/servers/physics_3d/physics_server_3d_wrap_mt.h
+++ b/servers/physics_3d/physics_server_3d_wrap_mt.h
@@ -210,8 +210,10 @@ public:
FUNC2(body_set_user_flags, RID, uint32_t);
FUNC1RC(uint32_t, body_get_user_flags, RID);
- FUNC3(body_set_param, RID, BodyParameter, real_t);
- FUNC2RC(real_t, body_get_param, RID, BodyParameter);
+ FUNC3(body_set_param, RID, BodyParameter, const Variant &);
+ FUNC2RC(Variant, body_get_param, RID, BodyParameter);
+
+ FUNC1(body_reset_mass_properties, RID);
FUNC3(body_set_state, RID, BodyState, const Variant &);
FUNC2RC(Variant, body_get_state, RID, BodyState);
diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp
index b81d3272c3..945d0120be 100644
--- a/servers/physics_3d/shape_3d_sw.cpp
+++ b/servers/physics_3d/shape_3d_sw.cpp
@@ -1676,6 +1676,17 @@ struct _HeightmapSegmentCullParams {
FaceShape3DSW *face = nullptr;
};
+struct _HeightmapGridCullState {
+ real_t length = 0.0;
+ real_t length_flat = 0.0;
+
+ real_t dist = 0.0;
+ real_t prev_dist = 0.0;
+
+ int x = 0;
+ int z = 0;
+};
+
_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) {
Vector3 res;
Vector3 normal;
@@ -1688,11 +1699,11 @@ _FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_
return false;
}
-_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, int p_x, int p_z) {
+_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) {
// First triangle.
- p_params.heightmap->_get_point(p_x, p_z, p_params.face->vertex[0]);
- p_params.heightmap->_get_point(p_x + 1, p_z, p_params.face->vertex[1]);
- p_params.heightmap->_get_point(p_x, p_z + 1, p_params.face->vertex[2]);
+ p_params.heightmap->_get_point(p_state.x, p_state.z, p_params.face->vertex[0]);
+ p_params.heightmap->_get_point(p_state.x + 1, p_state.z, p_params.face->vertex[1]);
+ p_params.heightmap->_get_point(p_state.x, p_state.z + 1, p_params.face->vertex[2]);
p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
if (_heightmap_face_cull_segment(p_params)) {
return true;
@@ -1700,7 +1711,7 @@ _FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_
// Second triangle.
p_params.face->vertex[0] = p_params.face->vertex[1];
- p_params.heightmap->_get_point(p_x + 1, p_z + 1, p_params.face->vertex[1]);
+ p_params.heightmap->_get_point(p_state.x + 1, p_state.z + 1, p_params.face->vertex[1]);
p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
if (_heightmap_face_cull_segment(p_params)) {
return true;
@@ -1709,13 +1720,51 @@ _FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_
return false;
}
-bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const {
- if (heights.is_empty()) {
+_FORCE_INLINE_ bool _heightmap_chunk_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) {
+ const HeightMapShape3DSW::Range &chunk = p_params.heightmap->_get_bounds_chunk(p_state.x, p_state.z);
+
+ Vector3 enter_pos;
+ Vector3 exit_pos;
+
+ if (p_state.length_flat > CMP_EPSILON) {
+ real_t flat_to_3d = p_state.length / p_state.length_flat;
+ real_t enter_param = p_state.prev_dist * flat_to_3d;
+ real_t exit_param = p_state.dist * flat_to_3d;
+ enter_pos = p_params.from + p_params.dir * enter_param;
+ exit_pos = p_params.from + p_params.dir * exit_param;
+ } else {
+ // Consider the ray vertical.
+ // (though we shouldn't reach this often because there is an early check up-front)
+ enter_pos = p_params.from;
+ exit_pos = p_params.to;
+ }
+
+ // Transform positions to heightmap space.
+ enter_pos *= HeightMapShape3DSW::BOUNDS_CHUNK_SIZE;
+ exit_pos *= HeightMapShape3DSW::BOUNDS_CHUNK_SIZE;
+
+ // We did enter the flat projection of the AABB,
+ // but we have to check if we intersect it on the vertical axis.
+ if ((enter_pos.y > chunk.max) && (exit_pos.y > chunk.max)) {
+ return false;
+ }
+ if ((enter_pos.y < chunk.min) && (exit_pos.y < chunk.min)) {
return false;
}
- Vector3 local_begin = p_begin + local_origin;
- Vector3 local_end = p_end + local_origin;
+ return p_params.heightmap->_intersect_grid_segment(_heightmap_cell_cull_segment, enter_pos, exit_pos, p_params.heightmap->width, p_params.heightmap->depth, p_params.heightmap->local_origin, p_params.result, p_params.normal);
+}
+
+template <typename ProcessFunction>
+bool HeightMapShape3DSW::_intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const {
+ Vector3 delta = (p_end - p_begin);
+ real_t length = delta.length();
+
+ if (length < CMP_EPSILON) {
+ return false;
+ }
+
+ Vector3 local_begin = p_begin + offset;
FaceShape3DSW face;
face.backface_collision = false;
@@ -1723,136 +1772,181 @@ bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3
_HeightmapSegmentCullParams params;
params.from = p_begin;
params.to = p_end;
- params.dir = (p_end - p_begin).normalized();
+ params.dir = delta / length;
params.heightmap = this;
params.face = &face;
- // Quantize the ray begin/end.
- int begin_x = floor(local_begin.x);
- int begin_z = floor(local_begin.z);
- int end_x = floor(local_end.x);
- int end_z = floor(local_end.z);
+ _HeightmapGridCullState state;
- if ((begin_x == end_x) && (begin_z == end_z)) {
- // Simple case for rays that don't traverse the grid horizontally.
- // Just perform a test on the given cell.
- int x = CLAMP(begin_x, 0, width - 2);
- int z = CLAMP(begin_z, 0, depth - 2);
- if (_heightmap_cell_cull_segment(params, x, z)) {
- r_point = params.result;
- r_normal = params.normal;
- return true;
- }
- } else {
- // Perform grid query from projected ray.
- Vector2 ray_dir_proj(local_end.x - local_begin.x, local_end.z - local_begin.z);
- real_t ray_dist_proj = ray_dir_proj.length();
+ // Perform grid query from projected ray.
+ Vector2 ray_dir_flat(delta.x, delta.z);
+ state.length = length;
+ state.length_flat = ray_dir_flat.length();
- if (ray_dist_proj < CMP_EPSILON) {
- ray_dir_proj = Vector2();
- } else {
- ray_dir_proj /= ray_dist_proj;
- }
+ if (state.length_flat < CMP_EPSILON) {
+ ray_dir_flat = Vector2();
+ } else {
+ ray_dir_flat /= state.length_flat;
+ }
- const int x_step = (ray_dir_proj.x > CMP_EPSILON) ? 1 : ((ray_dir_proj.x < -CMP_EPSILON) ? -1 : 0);
- const int z_step = (ray_dir_proj.y > CMP_EPSILON) ? 1 : ((ray_dir_proj.y < -CMP_EPSILON) ? -1 : 0);
+ const int x_step = (ray_dir_flat.x > CMP_EPSILON) ? 1 : ((ray_dir_flat.x < -CMP_EPSILON) ? -1 : 0);
+ const int z_step = (ray_dir_flat.y > CMP_EPSILON) ? 1 : ((ray_dir_flat.y < -CMP_EPSILON) ? -1 : 0);
- const real_t infinite = 1e20;
- const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_proj.x) : infinite;
- const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_proj.y) : infinite;
+ const real_t infinite = 1e20;
+ const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_flat.x) : infinite;
+ const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_flat.y) : infinite;
- real_t cross_x; // At which value of `param` we will cross a x-axis lane?
- real_t cross_z; // At which value of `param` we will cross a z-axis lane?
+ real_t cross_x; // At which value of `param` we will cross a x-axis lane?
+ real_t cross_z; // At which value of `param` we will cross a z-axis lane?
- // X initialization.
- if (x_step != 0) {
- if (x_step == 1) {
- cross_x = (ceil(local_begin.x) - local_begin.x) * delta_x;
- } else {
- cross_x = (local_begin.x - floor(local_begin.x)) * delta_x;
- }
+ // X initialization.
+ if (x_step != 0) {
+ if (x_step == 1) {
+ cross_x = (Math::ceil(local_begin.x) - local_begin.x) * delta_x;
} else {
- cross_x = infinite; // Will never cross on X.
+ cross_x = (local_begin.x - Math::floor(local_begin.x)) * delta_x;
}
+ } else {
+ cross_x = infinite; // Will never cross on X.
+ }
- // Z initialization.
- if (z_step != 0) {
- if (z_step == 1) {
- cross_z = (ceil(local_begin.z) - local_begin.z) * delta_z;
- } else {
- cross_z = (local_begin.z - floor(local_begin.z)) * delta_z;
- }
+ // Z initialization.
+ if (z_step != 0) {
+ if (z_step == 1) {
+ cross_z = (Math::ceil(local_begin.z) - local_begin.z) * delta_z;
} else {
- cross_z = infinite; // Will never cross on Z.
+ cross_z = (local_begin.z - Math::floor(local_begin.z)) * delta_z;
}
+ } else {
+ cross_z = infinite; // Will never cross on Z.
+ }
- int x = floor(local_begin.x);
- int z = floor(local_begin.z);
+ int x = Math::floor(local_begin.x);
+ int z = Math::floor(local_begin.z);
- // Workaround cases where the ray starts at an integer position.
- if (Math::is_zero_approx(cross_x)) {
- cross_x += delta_x;
- // If going backwards, we should ignore the position we would get by the above flooring,
- // because the ray is not heading in that direction.
- if (x_step == -1) {
- x -= 1;
- }
+ // Workaround cases where the ray starts at an integer position.
+ if (Math::is_zero_approx(cross_x)) {
+ cross_x += delta_x;
+ // If going backwards, we should ignore the position we would get by the above flooring,
+ // because the ray is not heading in that direction.
+ if (x_step == -1) {
+ x -= 1;
}
+ }
- if (Math::is_zero_approx(cross_z)) {
- cross_z += delta_z;
- if (z_step == -1) {
- z -= 1;
- }
+ if (Math::is_zero_approx(cross_z)) {
+ cross_z += delta_z;
+ if (z_step == -1) {
+ z -= 1;
}
+ }
+
+ // Start inside the grid.
+ int x_start = MAX(MIN(x, p_width - 2), 0);
+ int z_start = MAX(MIN(z, p_depth - 2), 0);
- // Start inside the grid.
- int x_start = CLAMP(x, 0, width - 2);
- int z_start = CLAMP(z, 0, depth - 2);
+ // Adjust initial cross values.
+ cross_x += delta_x * x_step * (x_start - x);
+ cross_z += delta_z * z_step * (z_start - z);
- // Adjust initial cross values.
- cross_x += delta_x * x_step * (x_start - x);
- cross_z += delta_z * z_step * (z_start - z);
+ x = x_start;
+ z = z_start;
- x = x_start;
- z = z_start;
+ while (true) {
+ state.prev_dist = state.dist;
+ state.x = x;
+ state.z = z;
- if (_heightmap_cell_cull_segment(params, x, z)) {
+ if (cross_x < cross_z) {
+ // X lane.
+ x += x_step;
+ // Assign before advancing the param,
+ // to be in sync with the initialization step.
+ state.dist = cross_x;
+ cross_x += delta_x;
+ } else {
+ // Z lane.
+ z += z_step;
+ state.dist = cross_z;
+ cross_z += delta_z;
+ }
+
+ if (state.dist > state.length_flat) {
+ state.dist = state.length_flat;
+ if (p_process(params, state)) {
+ r_point = params.result;
+ r_normal = params.normal;
+ return true;
+ }
+ break;
+ }
+
+ if (p_process(params, state)) {
r_point = params.result;
r_normal = params.normal;
return true;
}
- real_t dist = 0.0;
- while (true) {
- if (cross_x < cross_z) {
- // X lane.
- x += x_step;
- // Assign before advancing the param,
- // to be in sync with the initialization step.
- dist = cross_x;
- cross_x += delta_x;
- } else {
- // Z lane.
- z += z_step;
- dist = cross_z;
- cross_z += delta_z;
- }
+ // Stop when outside the grid.
+ if ((x < 0) || (z < 0) || (x >= p_width - 1) || (z >= p_depth - 1)) {
+ break;
+ }
+ }
- // Stop when outside the grid.
- if ((x < 0) || (z < 0) || (x >= width - 1) || (z >= depth - 1)) {
- break;
- }
+ return false;
+}
- if (_heightmap_cell_cull_segment(params, x, z)) {
- r_point = params.result;
- r_normal = params.normal;
- return true;
- }
+bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const {
+ if (heights.is_empty()) {
+ return false;
+ }
- if (dist > ray_dist_proj) {
- break;
- }
+ Vector3 local_begin = p_begin + local_origin;
+ Vector3 local_end = p_end + local_origin;
+
+ // Quantize the ray begin/end.
+ int begin_x = Math::floor(local_begin.x);
+ int begin_z = Math::floor(local_begin.z);
+ int end_x = Math::floor(local_end.x);
+ int end_z = Math::floor(local_end.z);
+
+ if ((begin_x == end_x) && (begin_z == end_z)) {
+ // Simple case for rays that don't traverse the grid horizontally.
+ // Just perform a test on the given cell.
+ FaceShape3DSW face;
+ face.backface_collision = false;
+
+ _HeightmapSegmentCullParams params;
+ params.from = p_begin;
+ params.to = p_end;
+ params.dir = (p_end - p_begin).normalized();
+
+ params.heightmap = this;
+ params.face = &face;
+
+ _HeightmapGridCullState state;
+ state.x = MAX(MIN(begin_x, width - 2), 0);
+ state.z = MAX(MIN(begin_z, depth - 2), 0);
+ if (_heightmap_cell_cull_segment(params, state)) {
+ r_point = params.result;
+ r_normal = params.normal;
+ return true;
+ }
+ } else if (bounds_grid.is_empty()) {
+ // Process all cells intersecting the flat projection of the ray.
+ return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal);
+ } else {
+ Vector3 ray_diff = (p_end - p_begin);
+ real_t length_flat_sqr = ray_diff.x * ray_diff.x + ray_diff.z * ray_diff.z;
+ if (length_flat_sqr < BOUNDS_CHUNK_SIZE * BOUNDS_CHUNK_SIZE) {
+ // Don't use chunks, the ray is too short in the plane.
+ return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal);
+ } else {
+ // The ray is long, run raycast on a higher-level grid.
+ Vector3 bounds_from = p_begin / BOUNDS_CHUNK_SIZE;
+ Vector3 bounds_to = p_end / BOUNDS_CHUNK_SIZE;
+ Vector3 bounds_offset = local_origin / BOUNDS_CHUNK_SIZE;
+ return _intersect_grid_segment(_heightmap_chunk_cull_segment, bounds_from, bounds_to, bounds_grid_width, bounds_grid_depth, bounds_offset, r_point, r_normal);
}
}
@@ -1917,7 +2011,7 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback
_get_point(x, z, face.vertex[0]);
_get_point(x + 1, z, face.vertex[1]);
_get_point(x, z + 1, face.vertex[2]);
- face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal;
+ face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal;
if (p_callback(p_userdata, &face)) {
return;
}
@@ -1925,7 +2019,7 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback
// Second triangle.
face.vertex[0] = face.vertex[1];
_get_point(x + 1, z + 1, face.vertex[1]);
- face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal;
+ face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal;
if (p_callback(p_userdata, &face)) {
return;
}
@@ -1943,6 +2037,75 @@ Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const {
(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
}
+void HeightMapShape3DSW::_build_accelerator() {
+ bounds_grid.clear();
+
+ bounds_grid_width = width / BOUNDS_CHUNK_SIZE;
+ bounds_grid_depth = depth / BOUNDS_CHUNK_SIZE;
+
+ if (width % BOUNDS_CHUNK_SIZE > 0) {
+ ++bounds_grid_width; // In case terrain size isn't dividable by chunk size.
+ }
+
+ if (depth % BOUNDS_CHUNK_SIZE > 0) {
+ ++bounds_grid_depth;
+ }
+
+ uint32_t bound_grid_size = (uint32_t)(bounds_grid_width * bounds_grid_depth);
+
+ if (bound_grid_size < 2) {
+ // Grid is empty or just one chunk.
+ return;
+ }
+
+ bounds_grid.resize(bound_grid_size);
+
+ // Compute min and max height for all chunks.
+ for (int cz = 0; cz < bounds_grid_depth; ++cz) {
+ int z0 = cz * BOUNDS_CHUNK_SIZE;
+
+ for (int cx = 0; cx < bounds_grid_width; ++cx) {
+ int x0 = cx * BOUNDS_CHUNK_SIZE;
+
+ Range r;
+
+ r.min = _get_height(x0, z0);
+ r.max = r.min;
+
+ // Compute min and max height for this chunk.
+ // We have to include one extra cell to account for neighbors.
+ // Here is why:
+ // Say we have a flat terrain, and a plateau that fits a chunk perfectly.
+ //
+ // Left Right
+ // 0---0---0---1---1---1
+ // | | | | | |
+ // 0---0---0---1---1---1
+ // | | | | | |
+ // 0---0---0---1---1---1
+ // x
+ //
+ // If the AABB for the Left chunk did not share vertices with the Right,
+ // then we would fail collision tests at x due to a gap.
+ //
+ int z_max = MIN(z0 + BOUNDS_CHUNK_SIZE + 1, depth);
+ int x_max = MIN(x0 + BOUNDS_CHUNK_SIZE + 1, width);
+ for (int z = z0; z < z_max; ++z) {
+ for (int x = x0; x < x_max; ++x) {
+ real_t height = _get_height(x, z);
+ if (height < r.min) {
+ r.min = height;
+ } else if (height > r.max) {
+ r.max = height;
+ }
+ }
+ }
+
+ bounds_grid[cx + cz * bounds_grid_width] = r;
+ }
+ }
+}
+
void HeightMapShape3DSW::_setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
heights = p_heights;
width = p_width;
@@ -1959,6 +2122,8 @@ void HeightMapShape3DSW::_setup(const Vector<real_t> &p_heights, int p_width, in
aabb.position -= local_origin;
+ _build_accelerator();
+
configure(aabb);
}
@@ -2017,7 +2182,7 @@ void HeightMapShape3DSW::set_data(const Variant &p_data) {
} else {
int heights_size = heights.size();
for (int i = 0; i < heights_size; ++i) {
- float h = heights[i];
+ real_t h = heights[i];
if (h < min_height) {
min_height = h;
} else if (h > max_height) {
diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h
index b05f65f268..73eddc469c 100644
--- a/servers/physics_3d/shape_3d_sw.h
+++ b/servers/physics_3d/shape_3d_sw.h
@@ -32,6 +32,7 @@
#define SHAPE_SW_H
#include "core/math/geometry_3d.h"
+#include "core/templates/local_vector.h"
#include "servers/physics_server_3d.h"
class Shape3DSW;
@@ -385,6 +386,21 @@ struct HeightMapShape3DSW : public ConcaveShape3DSW {
int depth = 0;
Vector3 local_origin;
+ // Accelerator.
+ struct Range {
+ real_t min = 0.0;
+ real_t max = 0.0;
+ };
+ LocalVector<Range> bounds_grid;
+ int bounds_grid_width = 0;
+ int bounds_grid_depth = 0;
+
+ static const int BOUNDS_CHUNK_SIZE = 16;
+
+ _FORCE_INLINE_ const Range &_get_bounds_chunk(int p_x, int p_z) const {
+ return bounds_grid[(p_z * bounds_grid_width) + p_x];
+ }
+
_FORCE_INLINE_ real_t _get_height(int p_x, int p_z) const {
return heights[(p_z * width) + p_x];
}
@@ -397,6 +413,11 @@ struct HeightMapShape3DSW : public ConcaveShape3DSW {
void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const;
+ void _build_accelerator();
+
+ template <typename ProcessFunction>
+ bool _intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const;
+
void _setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height);
public:
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index 37dee436df..3b08184e00 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -977,12 +977,12 @@ void Space3DSW::body_remove_from_active_list(SelfList<Body3DSW> *p_body) {
active_list.remove(p_body);
}
-void Space3DSW::body_add_to_inertia_update_list(SelfList<Body3DSW> *p_body) {
- inertia_update_list.add(p_body);
+void Space3DSW::body_add_to_mass_properties_update_list(SelfList<Body3DSW> *p_body) {
+ mass_properties_update_list.add(p_body);
}
-void Space3DSW::body_remove_from_inertia_update_list(SelfList<Body3DSW> *p_body) {
- inertia_update_list.remove(p_body);
+void Space3DSW::body_remove_from_mass_properties_update_list(SelfList<Body3DSW> *p_body) {
+ mass_properties_update_list.remove(p_body);
}
BroadPhase3DSW *Space3DSW::get_broadphase() {
@@ -1059,9 +1059,9 @@ void Space3DSW::call_queries() {
void Space3DSW::setup() {
contact_debug_count = 0;
- while (inertia_update_list.first()) {
- inertia_update_list.first()->self()->update_inertias();
- inertia_update_list.remove(inertia_update_list.first());
+ while (mass_properties_update_list.first()) {
+ mass_properties_update_list.first()->self()->update_mass_properties();
+ mass_properties_update_list.remove(mass_properties_update_list.first());
}
}
diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h
index eff494a7bc..98c335cb80 100644
--- a/servers/physics_3d/space_3d_sw.h
+++ b/servers/physics_3d/space_3d_sw.h
@@ -48,12 +48,12 @@ class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D {
public:
Space3DSW *space;
- virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
- virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override;
- virtual int intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
- virtual bool cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override;
- virtual bool collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
- virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+ virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+ virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override;
+ virtual int intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+ virtual bool cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override;
+ virtual bool collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+ virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override;
PhysicsDirectSpaceState3DSW();
@@ -79,7 +79,7 @@ private:
BroadPhase3DSW *broadphase;
SelfList<Body3DSW>::List active_list;
- SelfList<Body3DSW>::List inertia_update_list;
+ SelfList<Body3DSW>::List mass_properties_update_list;
SelfList<Body3DSW>::List state_query_list;
SelfList<Area3DSW>::List monitor_query_list;
SelfList<Area3DSW>::List area_moved_list;
@@ -137,8 +137,8 @@ public:
const SelfList<Body3DSW>::List &get_active_body_list() const;
void body_add_to_active_list(SelfList<Body3DSW> *p_body);
void body_remove_from_active_list(SelfList<Body3DSW> *p_body);
- void body_add_to_inertia_update_list(SelfList<Body3DSW> *p_body);
- void body_remove_from_inertia_update_list(SelfList<Body3DSW> *p_body);
+ void body_add_to_mass_properties_update_list(SelfList<Body3DSW> *p_body);
+ void body_remove_from_mass_properties_update_list(SelfList<Body3DSW> *p_body);
void body_add_to_state_query_list(SelfList<Body3DSW> *p_body);
void body_remove_from_state_query_list(SelfList<Body3DSW> *p_body);
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-30 07:20:48 +0000