diff options
Diffstat (limited to 'servers/physics_3d')
39 files changed, 1626 insertions, 1060 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index 364f63e4ad..630ab7e229 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -163,6 +163,20 @@ void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant & case PhysicsServer3D::AREA_PARAM_PRIORITY: priority = p_value; break; + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: + ERR_FAIL_COND_MSG(wind_force_magnitude < 0, "Wind force magnitude must be a non-negative real number, but a negative number was specified."); + wind_force_magnitude = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: + wind_source = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: + wind_direction = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: + ERR_FAIL_COND_MSG(wind_attenuation_factor < 0, "Wind attenuation factor must be a non-negative real number, but a negative number was specified."); + wind_attenuation_factor = p_value; + break; } } @@ -184,6 +198,14 @@ Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const { return angular_damp; case PhysicsServer3D::AREA_PARAM_PRIORITY: return priority; + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: + return wind_force_magnitude; + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: + return wind_source; + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: + return wind_direction; + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: + return wind_attenuation_factor; } return Variant(); @@ -282,22 +304,32 @@ void Area3DSW::call_queries() { } } +void Area3DSW::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const { + if (is_gravity_point()) { + const real_t gravity_distance_scale = get_gravity_distance_scale(); + Vector3 v = get_transform().xform(get_gravity_vector()) - p_position; + if (gravity_distance_scale > 0) { + const real_t v_length = v.length(); + if (v_length > 0) { + const real_t v_scaled = v_length * gravity_distance_scale; + r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); + } else { + r_gravity = Vector3(); + } + } else { + r_gravity = v.normalized() * get_gravity(); + } + } else { + r_gravity = get_gravity_vector() * get_gravity(); + } +} + Area3DSW::Area3DSW() : CollisionObject3DSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { _set_static(true); //areas are never active - space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; - gravity = 9.80665; - gravity_vector = Vector3(0, -1, 0); - gravity_is_point = false; - gravity_distance_scale = 0; - point_attenuation = 1; - angular_damp = 0.1; - linear_damp = 0.1; - priority = 0; set_ray_pickable(false); - monitorable = false; } Area3DSW::~Area3DSW() { diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h index 5959ee1e95..af5c23949c 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -34,7 +34,6 @@ #include "collision_object_3d_sw.h" #include "core/templates/self_list.h" #include "servers/physics_server_3d.h" -//#include "servers/physics_3d/query_sw.h" class Space3DSW; class Body3DSW; @@ -42,16 +41,20 @@ class SoftBody3DSW; class Constraint3DSW; class Area3DSW : public CollisionObject3DSW { - PhysicsServer3D::AreaSpaceOverrideMode space_override_mode; - real_t gravity; - Vector3 gravity_vector; - bool gravity_is_point; - real_t gravity_distance_scale; - real_t point_attenuation; - real_t linear_damp; - real_t angular_damp; - int priority; - bool monitorable; + PhysicsServer3D::AreaSpaceOverrideMode space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + real_t gravity = 9.80665; + Vector3 gravity_vector = Vector3(0, -1, 0); + bool gravity_is_point = false; + real_t gravity_distance_scale = 0.0; + real_t point_attenuation = 1.0; + real_t linear_damp = 0.1; + real_t angular_damp = 0.1; + real_t wind_force_magnitude = 0.0; + real_t wind_attenuation_factor = 0.0; + Vector3 wind_source; + Vector3 wind_direction; + int priority = 0; + bool monitorable = false; ObjectID monitor_callback_id; StringName monitor_callback_method; @@ -65,8 +68,8 @@ class Area3DSW : public CollisionObject3DSW { struct BodyKey { RID rid; ObjectID instance_id; - uint32_t body_shape; - uint32_t area_shape; + uint32_t body_shape = 0; + uint32_t area_shape = 0; _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { if (rid == p_key.rid) { @@ -87,28 +90,21 @@ class Area3DSW : public CollisionObject3DSW { }; struct BodyState { - int state; + int state = 0; _FORCE_INLINE_ void inc() { state++; } _FORCE_INLINE_ void dec() { state--; } - _FORCE_INLINE_ BodyState() { state = 0; } }; Map<BodyKey, BodyState> monitored_soft_bodies; Map<BodyKey, BodyState> monitored_bodies; Map<BodyKey, BodyState> monitored_areas; - //virtual void shape_changed_notify(ShapeSW *p_shape); - //virtual void shape_deleted_notify(ShapeSW *p_shape); - Set<Constraint3DSW *> constraints; virtual void _shapes_changed(); void _queue_monitor_update(); public: - //_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; } - //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } - void set_monitor_callback(ObjectID p_id, const StringName &p_method); _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } @@ -154,6 +150,18 @@ public: _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ int get_priority() const { return priority; } + _FORCE_INLINE_ void set_wind_force_magnitude(real_t p_wind_force_magnitude) { wind_force_magnitude = p_wind_force_magnitude; } + _FORCE_INLINE_ real_t get_wind_force_magnitude() const { return wind_force_magnitude; } + + _FORCE_INLINE_ void set_wind_attenuation_factor(real_t p_wind_attenuation_factor) { wind_attenuation_factor = p_wind_attenuation_factor; } + _FORCE_INLINE_ real_t get_wind_attenuation_factor() const { return wind_attenuation_factor; } + + _FORCE_INLINE_ void set_wind_source(const Vector3 &p_wind_source) { wind_source = p_wind_source; } + _FORCE_INLINE_ const Vector3 &get_wind_source() const { return wind_source; } + + _FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; } + _FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; } + _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } _FORCE_INLINE_ const Set<Constraint3DSW *> &get_constraints() const { return constraints; } @@ -168,6 +176,8 @@ public: void call_queries(); + void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const; + Area3DSW(); ~Area3DSW(); }; @@ -221,8 +231,8 @@ void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, u } struct AreaCMP { - Area3DSW *area; - int refCount; + Area3DSW *area = nullptr; + int refCount = 0; _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } _FORCE_INLINE_ AreaCMP() {} diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index e740565da6..bf4f0035b4 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -33,7 +33,7 @@ bool AreaPair3DSW::setup(real_t p_step) { bool result = false; - if (area->interacts_with(body) && CollisionSolver3DSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) { + if (area->collides_with(body) && CollisionSolver3DSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) { result = true; } @@ -109,46 +109,51 @@ AreaPair3DSW::~AreaPair3DSW() { //////////////////////////////////////////////////// bool Area2Pair3DSW::setup(real_t p_step) { - bool result = false; - if (area_a->interacts_with(area_b) && CollisionSolver3DSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) { - result = true; + bool result_a = area_a->collides_with(area_b); + bool result_b = area_b->collides_with(area_a); + if ((result_a || result_b) && !CollisionSolver3DSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) { + result_a = false; + result_b = false; } - process_collision = false; - if (result != colliding) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - process_collision = true; - } else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + bool process_collision = false; + + process_collision_a = false; + if (result_a != colliding_a) { + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + process_collision_a = true; process_collision = true; } + colliding_a = result_a; + } - colliding = result; + process_collision_b = false; + if (result_b != colliding_b) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + process_collision_b = true; + process_collision = true; + } + colliding_b = result_b; } return process_collision; } bool Area2Pair3DSW::pre_solve(real_t p_step) { - if (!process_collision) { - return false; + if (process_collision_a) { + if (colliding_a) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } else { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } } - if (colliding) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + if (process_collision_b) { + if (colliding_b) { area_b->add_area_to_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } - } else { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + } else { area_b->remove_area_from_query(area_a, shape_a, shape_b); } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } } return false; // Never do any post solving. @@ -168,16 +173,18 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area } Area2Pair3DSW::~Area2Pair3DSW() { - if (colliding) { - if (area_b->has_area_monitor_callback()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - + if (colliding_a) { if (area_a->has_area_monitor_callback()) { area_a->remove_area_from_query(area_b, shape_b, shape_a); } } + if (colliding_b) { + if (area_b->has_area_monitor_callback()) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + area_a->remove_constraint(this); area_b->remove_constraint(this); } @@ -187,7 +194,7 @@ Area2Pair3DSW::~Area2Pair3DSW() { bool AreaSoftBodyPair3DSW::setup(real_t p_step) { bool result = false; if ( - area->interacts_with(soft_body) && + area->collides_with(soft_body) && CollisionSolver3DSW::solve_static( soft_body->get_shape(soft_body_shape), soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape), diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h index 8cc9e9ad63..4572dcbb23 100644 --- a/servers/physics_3d/area_pair_3d_sw.h +++ b/servers/physics_3d/area_pair_3d_sw.h @@ -58,8 +58,10 @@ class Area2Pair3DSW : public Constraint3DSW { Area3DSW *area_b; int shape_a; int shape_b; - bool colliding = false; - bool process_collision = false; + bool colliding_a = false; + bool colliding_b = false; + bool process_collision_a = false; + bool process_collision_b = false; public: virtual bool setup(real_t p_step) override; diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index 0c4079332d..5924e249a5 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 = Basis::from_scale(shape->get_moment_of_inertia(mass)); + 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 = Basis(); + } - // 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 = Basis(); + _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 = Basis(); + _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); } @@ -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(); } diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index efb114a325..fc47040389 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -28,34 +28,39 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef BODY_SW_H -#define BODY_SW_H +#ifndef BODY_3D_SW_H +#define BODY_3D_SW_H #include "area_3d_sw.h" #include "collision_object_3d_sw.h" #include "core/templates/vset.h" 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 @@ -67,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; @@ -101,36 +108,44 @@ class Body3DSW : public CollisionObject3DSW { struct Contact { Vector3 local_pos; Vector3 local_normal; - real_t depth; - int local_shape; + real_t depth = 0.0; + int local_shape = 0; Vector3 collider_pos; - int collider_shape; + int collider_shape = 0; ObjectID collider_instance_id; RID collider; Vector3 collider_velocity_at_pos; }; 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; - struct ForceIntegrationCallback { + struct ForceIntegrationCallbackData { Callable callable; Variant udata; }; - ForceIntegrationCallback *fi_callback; + ForceIntegrationCallbackData *fi_callback_data = nullptr; - uint64_t island_step; + PhysicsDirectBodyState3DSW *direct_state = nullptr; - _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area3DSW *p_area); + uint64_t island_step = 0; + + _FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area3DSW *p_area); _FORCE_INLINE_ void _update_transform_dependant(); friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose public: + void set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback); void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); + PhysicsDirectBodyState3DSW *get_direct_state(); + _FORCE_INLINE_ void add_area(Area3DSW *p_area) { int index = areas.find(AreaCMP(p_area)); if (index > -1) { @@ -242,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; @@ -262,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; } @@ -349,96 +365,4 @@ void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_no c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; } -class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D { - GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D); - -public: - static PhysicsDirectBodyState3DSW *singleton; - Body3DSW *body; - real_t step; - - virtual Vector3 get_total_gravity() const override { return body->gravity; } // get gravity vector working on this body space/area - virtual real_t get_total_angular_damp() const override { return body->area_angular_damp; } // get density of this body space/area - virtual real_t get_total_linear_damp() const override { return body->area_linear_damp; } // get density of this body space/area - - virtual Vector3 get_center_of_mass() const override { return body->get_center_of_mass(); } - virtual Basis get_principal_inertia_axes() const override { return body->get_principal_inertia_axes(); } - - virtual real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass - virtual Vector3 get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space - virtual Basis get_inverse_inertia_tensor() const override { return body->get_inv_inertia_tensor(); } // get density of this body space - - virtual void set_linear_velocity(const Vector3 &p_velocity) override { body->set_linear_velocity(p_velocity); } - virtual Vector3 get_linear_velocity() const override { return body->get_linear_velocity(); } - - virtual void set_angular_velocity(const Vector3 &p_velocity) override { body->set_angular_velocity(p_velocity); } - virtual Vector3 get_angular_velocity() const override { return body->get_angular_velocity(); } - - virtual void set_transform(const Transform3D &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); } - virtual Transform3D get_transform() const override { return body->get_transform(); } - - virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override { return body->get_velocity_in_local_point(p_position); } - - virtual void add_central_force(const Vector3 &p_force) override { body->add_central_force(p_force); } - virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override { - body->add_force(p_force, p_position); - } - virtual void add_torque(const Vector3 &p_torque) override { body->add_torque(p_torque); } - virtual void apply_central_impulse(const Vector3 &p_impulse) override { body->apply_central_impulse(p_impulse); } - virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override { - body->apply_impulse(p_impulse, p_position); - } - virtual void apply_torque_impulse(const Vector3 &p_impulse) override { body->apply_torque_impulse(p_impulse); } - - virtual void set_sleep_state(bool p_sleep) override { body->set_active(!p_sleep); } - virtual bool is_sleeping() const override { return !body->is_active(); } - - virtual int get_contact_count() const override { return body->contact_count; } - - virtual Vector3 get_contact_local_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].local_pos; - } - virtual Vector3 get_contact_local_normal(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].local_normal; - } - virtual real_t get_contact_impulse(int p_contact_idx) const override { - return 0.0f; // Only implemented for bullet - } - virtual int get_contact_local_shape(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); - return body->contacts[p_contact_idx].local_shape; - } - - virtual RID get_contact_collider(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); - return body->contacts[p_contact_idx].collider; - } - virtual Vector3 get_contact_collider_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].collider_pos; - } - virtual ObjectID get_contact_collider_id(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); - return body->contacts[p_contact_idx].collider_instance_id; - } - virtual int get_contact_collider_shape(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); - return body->contacts[p_contact_idx].collider_shape; - } - virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].collider_velocity_at_pos; - } - - virtual PhysicsDirectSpaceState3D *get_space_state() override; - - virtual real_t get_step() const override { return step; } - PhysicsDirectBodyState3DSW() { - singleton = this; - body = nullptr; - } -}; - -#endif // BODY__SW_H +#endif // BODY_3D_SW_H diff --git a/servers/physics_3d/body_direct_state_3d_sw.cpp b/servers/physics_3d/body_direct_state_3d_sw.cpp new file mode 100644 index 0000000000..d197dd288d --- /dev/null +++ b/servers/physics_3d/body_direct_state_3d_sw.cpp @@ -0,0 +1,182 @@ +/*************************************************************************/ +/* body_direct_state_3d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "body_direct_state_3d_sw.h" + +#include "body_3d_sw.h" +#include "space_3d_sw.h" + +Vector3 PhysicsDirectBodyState3DSW::get_total_gravity() const { + return body->gravity; +} + +real_t PhysicsDirectBodyState3DSW::get_total_angular_damp() const { + return body->area_angular_damp; +} + +real_t PhysicsDirectBodyState3DSW::get_total_linear_damp() const { + return body->area_linear_damp; +} + +Vector3 PhysicsDirectBodyState3DSW::get_center_of_mass() const { + return body->get_center_of_mass(); +} + +Basis PhysicsDirectBodyState3DSW::get_principal_inertia_axes() const { + return body->get_principal_inertia_axes(); +} + +real_t PhysicsDirectBodyState3DSW::get_inverse_mass() const { + return body->get_inv_mass(); +} + +Vector3 PhysicsDirectBodyState3DSW::get_inverse_inertia() const { + return body->get_inv_inertia(); +} + +Basis PhysicsDirectBodyState3DSW::get_inverse_inertia_tensor() const { + return body->get_inv_inertia_tensor(); +} + +void PhysicsDirectBodyState3DSW::set_linear_velocity(const Vector3 &p_velocity) { + body->set_linear_velocity(p_velocity); +} + +Vector3 PhysicsDirectBodyState3DSW::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void PhysicsDirectBodyState3DSW::set_angular_velocity(const Vector3 &p_velocity) { + body->set_angular_velocity(p_velocity); +} + +Vector3 PhysicsDirectBodyState3DSW::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void PhysicsDirectBodyState3DSW::set_transform(const Transform3D &p_transform) { + body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); +} + +Transform3D PhysicsDirectBodyState3DSW::get_transform() const { + return body->get_transform(); +} + +Vector3 PhysicsDirectBodyState3DSW::get_velocity_at_local_position(const Vector3 &p_position) const { + return body->get_velocity_in_local_point(p_position); +} + +void PhysicsDirectBodyState3DSW::add_central_force(const Vector3 &p_force) { + body->add_central_force(p_force); +} + +void PhysicsDirectBodyState3DSW::add_force(const Vector3 &p_force, const Vector3 &p_position) { + body->add_force(p_force, p_position); +} + +void PhysicsDirectBodyState3DSW::add_torque(const Vector3 &p_torque) { + body->add_torque(p_torque); +} + +void PhysicsDirectBodyState3DSW::apply_central_impulse(const Vector3 &p_impulse) { + body->apply_central_impulse(p_impulse); +} + +void PhysicsDirectBodyState3DSW::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { + body->apply_impulse(p_impulse, p_position); +} + +void PhysicsDirectBodyState3DSW::apply_torque_impulse(const Vector3 &p_impulse) { + body->apply_torque_impulse(p_impulse); +} + +void PhysicsDirectBodyState3DSW::set_sleep_state(bool p_sleep) { + body->set_active(!p_sleep); +} + +bool PhysicsDirectBodyState3DSW::is_sleeping() const { + return !body->is_active(); +} + +int PhysicsDirectBodyState3DSW::get_contact_count() const { + return body->contact_count; +} + +Vector3 PhysicsDirectBodyState3DSW::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_pos; +} + +Vector3 PhysicsDirectBodyState3DSW::get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_normal; +} + +real_t PhysicsDirectBodyState3DSW::get_contact_impulse(int p_contact_idx) const { + return 0.0f; // Only implemented for bullet +} + +int PhysicsDirectBodyState3DSW::get_contact_local_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); + return body->contacts[p_contact_idx].local_shape; +} + +RID PhysicsDirectBodyState3DSW::get_contact_collider(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); + return body->contacts[p_contact_idx].collider; +} + +Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_pos; +} + +ObjectID PhysicsDirectBodyState3DSW::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); + return body->contacts[p_contact_idx].collider_instance_id; +} + +int PhysicsDirectBodyState3DSW::get_contact_collider_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); + return body->contacts[p_contact_idx].collider_shape; +} + +Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_velocity_at_pos; +} + +PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() { + return body->get_space()->get_direct_state(); +} + +real_t PhysicsDirectBodyState3DSW::get_step() const { + return body->get_space()->get_last_step(); +} diff --git a/servers/physics_3d/body_direct_state_3d_sw.h b/servers/physics_3d/body_direct_state_3d_sw.h new file mode 100644 index 0000000000..5132376715 --- /dev/null +++ b/servers/physics_3d/body_direct_state_3d_sw.h @@ -0,0 +1,94 @@ +/*************************************************************************/ +/* body_direct_state_3d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef BODY_DIRECT_STATE_3D_SW_H +#define BODY_DIRECT_STATE_3D_SW_H + +#include "servers/physics_server_3d.h" + +class Body3DSW; + +class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D { + GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D); + +public: + Body3DSW *body = nullptr; + + virtual Vector3 get_total_gravity() const override; + virtual real_t get_total_angular_damp() const override; + virtual real_t get_total_linear_damp() const override; + + virtual Vector3 get_center_of_mass() const override; + virtual Basis get_principal_inertia_axes() const override; + + virtual real_t get_inverse_mass() const override; + virtual Vector3 get_inverse_inertia() const override; + virtual Basis get_inverse_inertia_tensor() const override; + + virtual void set_linear_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_linear_velocity() const override; + + virtual void set_angular_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_angular_velocity() const override; + + virtual void set_transform(const Transform3D &p_transform) override; + virtual Transform3D get_transform() const override; + + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override; + + virtual void add_central_force(const Vector3 &p_force) override; + virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void add_torque(const Vector3 &p_torque) override; + virtual void apply_central_impulse(const Vector3 &p_impulse) override; + virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; + virtual void apply_torque_impulse(const Vector3 &p_impulse) override; + + virtual void set_sleep_state(bool p_sleep) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector3 get_contact_local_position(int p_contact_idx) const override; + virtual Vector3 get_contact_local_normal(int p_contact_idx) const override; + virtual real_t get_contact_impulse(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + + virtual PhysicsDirectSpaceState3D *get_space_state() override; + + virtual real_t get_step() const override; +}; + +#endif // BODY_DIRECT_STATE_3D_SW_H diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 19d6a46880..01afb07e13 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -41,18 +41,18 @@ protected: struct Contact { Vector3 position; Vector3 normal; - int index_A, index_B; + int index_A = 0, index_B = 0; Vector3 local_A, local_B; - real_t acc_normal_impulse; // accumulated normal impulse (Pn) + real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) - real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) - real_t acc_bias_impulse_center_of_mass; // accumulated normal impulse for position bias applied to com - real_t mass_normal; - real_t bias; - real_t bounce; - - real_t depth; - bool active; + real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) + real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com + real_t mass_normal = 0.0; + real_t bias = 0.0; + real_t bounce = 0.0; + + real_t depth = 0.0; + bool active = false; Vector3 rA, rB; // Offset in world orientation with respect to center of mass }; diff --git a/servers/physics_3d/broad_phase_3d_bvh.cpp b/servers/physics_3d/broad_phase_3d_bvh.cpp index f9f64f786d..d89e0e1f6d 100644 --- a/servers/physics_3d/broad_phase_3d_bvh.cpp +++ b/servers/physics_3d/broad_phase_3d_bvh.cpp @@ -114,7 +114,4 @@ BroadPhase3DSW *BroadPhase3DBVH::_create() { BroadPhase3DBVH::BroadPhase3DBVH() { bvh.set_pair_callback(_pair_callback, this); bvh.set_unpair_callback(_unpair_callback, this); - pair_callback = nullptr; - pair_userdata = nullptr; - unpair_userdata = nullptr; } diff --git a/servers/physics_3d/broad_phase_3d_bvh.h b/servers/physics_3d/broad_phase_3d_bvh.h index 30b8b7f2aa..03131c9db2 100644 --- a/servers/physics_3d/broad_phase_3d_bvh.h +++ b/servers/physics_3d/broad_phase_3d_bvh.h @@ -40,10 +40,10 @@ class BroadPhase3DBVH : public BroadPhase3DSW { static void *_pair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int); static void _unpair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int, void *); - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; + PairCallback pair_callback = nullptr; + void *pair_userdata = nullptr; + UnpairCallback unpair_callback = nullptr; + void *unpair_userdata = nullptr; public: // 0 is an invalid ID diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp index 24c7d7b85c..098f627d11 100644 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -236,11 +236,5 @@ void CollisionObject3DSW::_shape_changed() { CollisionObject3DSW::CollisionObject3DSW(Type p_type) : pending_shape_update_list(this) { - _static = true; type = p_type; - space = nullptr; - - collision_layer = 1; - collision_mask = 1; - ray_pickable = true; } diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 6ffab54645..3aa48946b7 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -56,26 +56,24 @@ private: Type type; RID self; ObjectID instance_id; - uint32_t collision_layer; - uint32_t collision_mask; + uint32_t collision_layer = 1; + uint32_t collision_mask = 1; struct Shape { Transform3D xform; Transform3D xform_inv; BroadPhase3DSW::ID bpid; AABB aabb_cache; //for rayqueries - real_t area_cache; - Shape3DSW *shape; - bool disabled; - - Shape() { disabled = false; } + real_t area_cache = 0.0; + Shape3DSW *shape = nullptr; + bool disabled = false; }; Vector<Shape> shapes; - Space3DSW *space; + Space3DSW *space = nullptr; Transform3D transform; Transform3D inv_transform; - bool _static; + bool _static = true; SelfList<CollisionObject3DSW> pending_shape_update_list; @@ -102,7 +100,7 @@ protected: virtual void _shapes_changed() = 0; void _set_space(Space3DSW *p_space); - bool ray_pickable; + bool ray_pickable = true; CollisionObject3DSW(Type p_type); diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index 6e6a2cb9e7..76738bb746 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -66,11 +66,11 @@ struct _CollectorCallback { CollisionSolver3DSW::CallbackResult callback; - void *userdata; - bool swap; - bool collided; + void *userdata = nullptr; + bool swap = false; + bool collided = false; Vector3 normal; - Vector3 *prev_axis; + Vector3 *prev_axis = nullptr; _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { if (swap) { @@ -606,15 +606,15 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po template <class ShapeA, class ShapeB, bool withMargin = false> class SeparatorAxisTest { - const ShapeA *shape_A; - const ShapeB *shape_B; - const Transform3D *transform_A; - const Transform3D *transform_B; - real_t best_depth; + const ShapeA *shape_A = nullptr; + const ShapeB *shape_B = nullptr; + const Transform3D *transform_A = nullptr; + const Transform3D *transform_B = nullptr; + real_t best_depth = 1e15; Vector3 best_axis; - _CollectorCallback *callback; - real_t margin_A; - real_t margin_B; + _CollectorCallback *callback = nullptr; + real_t margin_A = 0.0; + real_t margin_B = 0.0; Vector3 separator_axis; public: @@ -749,7 +749,6 @@ public: } _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform3D &p_transform_A, const ShapeB *p_shape_B, const Transform3D &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { - best_depth = 1e15; shape_A = p_shape_A; shape_B = p_shape_B; transform_A = &p_transform_A; @@ -2272,12 +2271,14 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); - ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_PLANE, false); + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_A->is_concave(), false); PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); - ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_PLANE, false); + ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_B->is_concave(), false); static const CollisionFunc collision_table[6][6] = { @@ -2382,10 +2383,10 @@ bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_ CollisionFunc collision_func; if (margin_A != 0.0 || margin_B != 0.0) { - collision_func = collision_table_margin[type_A - 1][type_B - 1]; + collision_func = collision_table_margin[type_A - 2][type_B - 2]; } else { - collision_func = collision_table[type_A - 1][type_B - 1]; + collision_func = collision_table[type_A - 2][type_B - 2]; } ERR_FAIL_COND_V(!collision_func, false); diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index dcecac1c73..dcc363638e 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -37,12 +37,12 @@ #define collision_solver sat_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { +bool CollisionSolver3DSW::solve_static_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + const WorldBoundaryShape3DSW *world_boundary = static_cast<const WorldBoundaryShape3DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { return false; } - Plane p = p_transform_A.xform(plane->get_plane()); + Plane p = p_transform_A.xform(world_boundary->get_plane()); static const int max_supports = 16; Vector3 supports[max_supports]; @@ -89,6 +89,49 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T return found; } +bool CollisionSolver3DSW::solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { + const SeparationRayShape3DSW *ray = static_cast<const SeparationRayShape3DSW *>(p_shape_A); + + Vector3 from = p_transform_A.origin; + Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin); + Vector3 support_A = to; + + Transform3D ai = p_transform_B.affine_inverse(); + + from = ai.xform(from); + to = ai.xform(to); + + Vector3 p, n; + if (!p_shape_B->intersect_segment(from, to, p, n)) { + return false; + } + + // Discard contacts when the ray is fully contained inside the shape. + if (n == Vector3()) { + return false; + } + + // Discard contacts in the wrong direction. + if (n.dot(from - to) < CMP_EPSILON) { + return false; + } + + Vector3 support_B = p_transform_B.xform(p); + if (ray->get_slide_on_slope()) { + Vector3 global_n = ai.basis.xform_inv(n).normalized(); + support_B = support_A + (support_B - support_A).length() * global_n; + } + + if (p_result_callback) { + if (p_swap_result) { + p_result_callback(support_B, 0, support_A, 0, p_userdata); + } else { + p_result_callback(support_A, 0, support_B, 0, p_userdata); + } + } + return true; +} + struct _SoftBodyContactCollisionInfo { int node_index = 0; CollisionSolver3DSW::CallbackResult result_callback = nullptr; @@ -135,17 +178,17 @@ bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void * transform_B.origin = query_cinfo.node_transform.xform(node_position); query_cinfo.contact_info.node_index = p_node_index; - solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info); + bool collided = solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info); #ifdef DEBUG_ENABLED ++query_cinfo.node_query_count; #endif - // Continue with the query. - return false; + // Stop at first collision if contacts are not needed. + return (collided && !query_cinfo.contact_info.result_callback); } -void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) { +bool CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) { _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); query_cinfo.shape_A = p_convex; @@ -167,9 +210,14 @@ void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); + bool collided = (query_cinfo.contact_info.contact_count > 0); + #ifdef DEBUG_ENABLED ++query_cinfo.convex_query_count; #endif + + // Stop at first collision if contacts are not needed. + return (collided && !query_cinfo.contact_info.result_callback); } bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { @@ -243,17 +291,20 @@ struct _ConcaveCollisionInfo { Vector3 close_A, close_B; }; -void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) { +bool CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) { _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); cinfo.aabb_tests++; bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, nullptr, cinfo.margin_A, cinfo.margin_B); if (!collided) { - return; + return false; } cinfo.collided = true; cinfo.collisions++; + + // Stop at first collision if contacts are not needed. + return !cinfo.result_callback; } bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { @@ -314,8 +365,11 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo swap = true; } - if (type_A == PhysicsServer3D::SHAPE_PLANE) { - if (type_B == PhysicsServer3D::SHAPE_PLANE) { + if (type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + if (type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + return false; + } + if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { return false; } if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { @@ -323,9 +377,20 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } if (swap) { - return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + } else { + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + } + + } else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + return false; + } + + if (swap) { + return solve_separation_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_B); } else { - return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_separation_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A); } } else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { @@ -356,19 +421,18 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } } -void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) { +bool CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) { _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); cinfo.aabb_tests++; - if (cinfo.collided) { - return; - } Vector3 close_A, close_B; cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B); if (cinfo.collided) { - return; + // No need to process any more result. + return true; } + if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { cinfo.close_A = close_A; cinfo.close_B = close_B; @@ -376,14 +440,15 @@ void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW } cinfo.collisions++; + return false; } -bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { - const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { +bool CollisionSolver3DSW::solve_distance_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { + const WorldBoundaryShape3DSW *world_boundary = static_cast<const WorldBoundaryShape3DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { return false; } - Plane p = p_transform_A.xform(plane->get_plane()); + Plane p = p_transform_A.xform(world_boundary->get_plane()); static const int max_supports = 16; Vector3 supports[max_supports]; @@ -435,9 +500,9 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans return false; } - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { Vector3 a, b; - bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); + bool col = solve_distance_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); r_point_A = b; r_point_B = a; return !col; diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index a5dd7d48eb..0a9ea7c0eb 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -40,14 +40,14 @@ public: private: static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata); static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); - static void soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); - static void concave_callback(void *p_userdata, Shape3DSW *p_convex); - static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); + static bool concave_callback(void *p_userdata, Shape3DSW *p_convex); + static bool solve_static_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static bool solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); - static void concave_distance_callback(void *p_userdata, Shape3DSW *p_convex); - static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); + static bool concave_distance_callback(void *p_userdata, Shape3DSW *p_convex); + static bool solve_distance_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); public: static bool solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index 2df991563d..a1dbdd0a70 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -96,7 +96,7 @@ struct sResults { Vector3 witnesses[2]; Vector3 normal; - real_t distance; + real_t distance = 0.0; }; // Shorthands diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp index 7315e9c709..bb9cc1bf67 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -92,20 +92,8 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans m_rbAFrame = rbAFrame; m_rbBFrame = rbBFrame; - m_swingSpan1 = Math_TAU / 8.0; - m_swingSpan2 = Math_TAU / 8.0; - m_twistSpan = Math_TAU; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - - m_angularOnly = false; - m_solveTwistLimit = false; - m_solveSwingLimit = false; - A->add_constraint(this, 0); B->add_constraint(this, 1); - - m_appliedImpulse = 0; } bool ConeTwistJoint3DSW::setup(real_t p_timestep) { diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h index 608847352c..bf7e593820 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -67,39 +67,39 @@ public: Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = { nullptr, nullptr }; }; - JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints + JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints - real_t m_appliedImpulse; + real_t m_appliedImpulse = 0.0; Transform3D m_rbAFrame; Transform3D m_rbBFrame; - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; + real_t m_limitSoftness = 0.0; + real_t m_biasFactor = 0.3; + real_t m_relaxationFactor = 1.0; - real_t m_swingSpan1; - real_t m_swingSpan2; - real_t m_twistSpan; + real_t m_swingSpan1 = Math_TAU / 8.0; + real_t m_swingSpan2 = 0.0; + real_t m_twistSpan = 0.0; Vector3 m_swingAxis; Vector3 m_twistAxis; - real_t m_kSwing; - real_t m_kTwist; + real_t m_kSwing = 0.0; + real_t m_kTwist = 0.0; - real_t m_twistLimitSign; - real_t m_swingCorrection; - real_t m_twistCorrection; + real_t m_twistLimitSign = 0.0; + real_t m_swingCorrection = 0.0; + real_t m_twistCorrection = 0.0; - real_t m_accSwingLimitImpulse; - real_t m_accTwistLimitImpulse; + real_t m_accSwingLimitImpulse = 0.0; + real_t m_accTwistLimitImpulse = 0.0; - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; + bool m_angularOnly = false; + bool m_solveTwistLimit = false; + bool m_solveSwingLimit = false; public: virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h index d0f3dbbd35..6492e40393 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -65,43 +65,28 @@ class G6DOFRotationalLimitMotor3DSW { public: //! limit_parameters //!@{ - real_t m_loLimit; //!< joint limit - real_t m_hiLimit; //!< joint limit - real_t m_targetVelocity; //!< target motor velocity - real_t m_maxMotorForce; //!< max force on motor - real_t m_maxLimitForce; //!< max force on limit - real_t m_damping; //!< Damping. - real_t m_limitSoftness; //! Relaxation factor - real_t m_ERP; //!< Error tolerance factor when joint is at limit - real_t m_bounce; //!< restitution factor - bool m_enableMotor; - bool m_enableLimit; + real_t m_loLimit = -1e30; //!< joint limit + real_t m_hiLimit = 1e30; //!< joint limit + real_t m_targetVelocity = 0.0; //!< target motor velocity + real_t m_maxMotorForce = 0.1; //!< max force on motor + real_t m_maxLimitForce = 300.0; //!< max force on limit + real_t m_damping = 1.0; //!< Damping. + real_t m_limitSoftness = 0.5; //! Relaxation factor + real_t m_ERP = 0.5; //!< Error tolerance factor when joint is at limit + real_t m_bounce = 0.0; //!< restitution factor + bool m_enableMotor = false; + bool m_enableLimit = false; //!@} //! temp_variables //!@{ - real_t m_currentLimitError; //!< How much is violated this limit - int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit - real_t m_accumulatedImpulse; + real_t m_currentLimitError = 0.0; //!< How much is violated this limit + int m_currentLimit = 0; //!< 0=free, 1=at lo limit, 2=at hi limit + real_t m_accumulatedImpulse = 0.0; //!@} - G6DOFRotationalLimitMotor3DSW() { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_maxLimitForce = 300.0f; - m_loLimit = -1e30; - m_hiLimit = 1e30; - m_ERP = 0.5f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - m_enableLimit = false; - } + G6DOFRotationalLimitMotor3DSW() {} //! Is limited bool isLimited() { @@ -125,30 +110,16 @@ public: class G6DOFTranslationalLimitMotor3DSW { public: - Vector3 m_lowerLimit; //!< the constraint lower limits - Vector3 m_upperLimit; //!< the constraint upper limits - Vector3 m_accumulatedImpulse; + Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits + Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits + Vector3 m_accumulatedImpulse = Vector3(0.0, 0.0, 0.0); //! Linear_Limit_parameters //!@{ - Vector3 m_limitSoftness; //!< Softness for linear limit - Vector3 m_damping; //!< Damping for linear limit - Vector3 m_restitution; //! Bounce parameter for linear limit + Vector3 m_limitSoftness = Vector3(0.7, 0.7, 0.7); //!< Softness for linear limit + Vector3 m_damping = Vector3(1.0, 1.0, 1.0); //!< Damping for linear limit + Vector3 m_restitution = Vector3(0.5, 0.5, 0.5); //! Bounce parameter for linear limit //!@} - bool enable_limit[3]; - - G6DOFTranslationalLimitMotor3DSW() { - m_lowerLimit = Vector3(0.f, 0.f, 0.f); - m_upperLimit = Vector3(0.f, 0.f, 0.f); - m_accumulatedImpulse = Vector3(0.f, 0.f, 0.f); - - m_limitSoftness = Vector3(1, 1, 1) * 0.7f; - m_damping = Vector3(1, 1, 1) * real_t(1.0f); - m_restitution = Vector3(1, 1, 1) * real_t(0.5f); - - enable_limit[0] = true; - enable_limit[1] = true; - enable_limit[2] = true; - } + bool enable_limit[3] = { true, true, true }; //! Test limit /*! @@ -180,7 +151,7 @@ protected: Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = { nullptr, nullptr }; }; //! relative_frames @@ -208,7 +179,7 @@ protected: protected: //! temporal variables //!@{ - real_t m_timeStep; + real_t m_timeStep = 0.0; Transform3D m_calculatedTransformA; Transform3D m_calculatedTransformB; Vector3 m_calculatedAxisAngleDiff; @@ -216,7 +187,7 @@ protected: Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes - bool m_useLinearReferenceFrameA; + bool m_useLinearReferenceFrameA = false; //!@} diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index b928f18231..a45fcf7eb5 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -79,21 +79,6 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D & m_rbBFrame.basis[1][2] *= real_t(-1.); m_rbBFrame.basis[2][2] *= real_t(-1.); - //start with free - m_lowerLimit = Math_PI; - m_upperLimit = -Math_PI; - - m_useLimit = false; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - - tau = 0.3; - - m_angularOnly = false; - m_enableAngularMotor = false; - A->add_constraint(this, 0); B->add_constraint(this, 1); } @@ -135,21 +120,6 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo rbAxisB1.y, rbAxisB2.y, -axisInB.y, rbAxisB1.z, rbAxisB2.z, -axisInB.z); - //start with free - m_lowerLimit = Math_PI; - m_upperLimit = -Math_PI; - - m_useLimit = false; - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; - - tau = 0.3; - - m_angularOnly = false; - m_enableAngularMotor = false; - A->add_constraint(this, 0); B->add_constraint(this, 1); } diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index 22eb2f4660..a4ceff9ffe 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -60,7 +60,7 @@ class HingeJoint3DSW : public Joint3DSW { Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = {}; }; JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints @@ -69,31 +69,31 @@ class HingeJoint3DSW : public Joint3DSW { Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis. Transform3D m_rbBFrame; - real_t m_motorTargetVelocity; - real_t m_maxMotorImpulse; + real_t m_motorTargetVelocity = 0.0; + real_t m_maxMotorImpulse = 0.0; - real_t m_limitSoftness; - real_t m_biasFactor; - real_t m_relaxationFactor; + real_t m_limitSoftness = 0.9; + real_t m_biasFactor = 0.3; + real_t m_relaxationFactor = 1.0; - real_t m_lowerLimit; - real_t m_upperLimit; + real_t m_lowerLimit = Math_PI; + real_t m_upperLimit = -Math_PI; - real_t m_kHinge; + real_t m_kHinge = 0.0; - real_t m_limitSign; - real_t m_correction; + real_t m_limitSign = 0.0; + real_t m_correction = 0.0; - real_t m_accLimitImpulse; + real_t m_accLimitImpulse = 0.0; - real_t tau; + real_t tau = 0.3; - bool m_useLimit; - bool m_angularOnly; - bool m_enableAngularMotor; - bool m_solveLimit; + bool m_useLimit = false; + bool m_angularOnly = false; + bool m_enableAngularMotor = false; + bool m_solveLimit = false; - real_t m_appliedImpulse; + real_t m_appliedImpulse = 0.0; public: virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; } diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h index 6afa70c816..7294ff78e3 100644 --- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h +++ b/servers/physics_3d/joints/jacobian_entry_3d_sw.h @@ -163,7 +163,7 @@ public: Vector3 m_0MinvJt; Vector3 m_1MinvJt; //Optimization: can be stored in the w/last component of one of the vectors - real_t m_Adiag; + real_t m_Adiag = 1.0; }; #endif // JACOBIAN_ENTRY_SW_H diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index 8eb84d1c2f..f41151ec0e 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -171,11 +171,6 @@ PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW m_pivotInA = p_pos_a; m_pivotInB = p_pos_b; - m_tau = 0.3; - m_damping = 1; - m_impulseClamp = 0; - m_appliedImpulse = 0; - A->add_constraint(this, 0); B->add_constraint(this, 1); } diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index 3d91452850..79af48f2a5 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -60,15 +60,15 @@ class PinJoint3DSW : public Joint3DSW { Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = {}; }; - real_t m_tau; //bias - real_t m_damping; - real_t m_impulseClamp; - real_t m_appliedImpulse; + real_t m_tau = 0.3; //bias + real_t m_damping = 1.0; + real_t m_impulseClamp = 0.0; + real_t m_appliedImpulse = 0.0; - JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints + JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints Vector3 m_pivotInA; Vector3 m_pivotInB; diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 1895fe1e2e..e10ed436d5 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -72,41 +72,6 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { return (y < 0.0f) ? -angle : angle; } -void SliderJoint3DSW::initParams() { - m_lowerLinLimit = real_t(1.0); - m_upperLinLimit = real_t(-1.0); - m_lowerAngLimit = real_t(0.); - m_upperAngLimit = real_t(0.); - m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirLin = real_t(0.); - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = real_t(0.); - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - - m_poweredLinMotor = false; - m_targetLinMotorVelocity = real_t(0.); - m_maxLinMotorForce = real_t(0.); - m_accumulatedLinMotorImpulse = real_t(0.0); - - m_poweredAngMotor = false; - m_targetAngMotorVelocity = real_t(0.); - m_maxAngMotorForce = real_t(0.); - m_accumulatedAngMotorImpulse = real_t(0.0); -} // SliderJointSW::initParams() - //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- @@ -120,8 +85,6 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D A->add_constraint(this, 0); B->add_constraint(this, 1); - - initParams(); } // SliderJointSW::SliderJointSW() //----------------------------------------------------------------------------- diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index f357bbd67a..d32ad9469e 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -73,53 +73,53 @@ protected: Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = { nullptr, nullptr }; }; Transform3D m_frameInA; Transform3D m_frameInB; // linear limits - real_t m_lowerLinLimit; - real_t m_upperLinLimit; + real_t m_lowerLinLimit = 1.0; + real_t m_upperLinLimit = -1.0; // angular limits - real_t m_lowerAngLimit; - real_t m_upperAngLimit; + real_t m_lowerAngLimit = 0.0; + real_t m_upperAngLimit = 0.0; // softness, restitution and damping for different cases // DirLin - moving inside linear limits // LimLin - hitting linear limit // DirAng - moving inside angular limits // LimAng - hitting angular limit // OrthoLin, OrthoAng - against constraint axis - real_t m_softnessDirLin; - real_t m_restitutionDirLin; - real_t m_dampingDirLin; - real_t m_softnessDirAng; - real_t m_restitutionDirAng; - real_t m_dampingDirAng; - real_t m_softnessLimLin; - real_t m_restitutionLimLin; - real_t m_dampingLimLin; - real_t m_softnessLimAng; - real_t m_restitutionLimAng; - real_t m_dampingLimAng; - real_t m_softnessOrthoLin; - real_t m_restitutionOrthoLin; - real_t m_dampingOrthoLin; - real_t m_softnessOrthoAng; - real_t m_restitutionOrthoAng; - real_t m_dampingOrthoAng; + real_t m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingDirLin = 0.0; + real_t m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingDirAng = 0.0; + real_t m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; // for interlal use - bool m_solveLinLim; - bool m_solveAngLim; + bool m_solveLinLim = false; + bool m_solveAngLim = false; - JacobianEntry3DSW m_jacLin[3]; - real_t m_jacLinDiagABInv[3]; + JacobianEntry3DSW m_jacLin[3] = {}; + real_t m_jacLinDiagABInv[3] = {}; - JacobianEntry3DSW m_jacAng[3]; + JacobianEntry3DSW m_jacAng[3] = {}; - real_t m_timeStep; + real_t m_timeStep = 0.0; Transform3D m_calculatedTransformA; Transform3D m_calculatedTransformB; @@ -132,23 +132,20 @@ protected: Vector3 m_relPosA; Vector3 m_relPosB; - real_t m_linPos; + real_t m_linPos = 0.0; - real_t m_angDepth; - real_t m_kAngle; + real_t m_angDepth = 0.0; + real_t m_kAngle = 0.0; - bool m_poweredLinMotor; - real_t m_targetLinMotorVelocity; - real_t m_maxLinMotorForce; - real_t m_accumulatedLinMotorImpulse; + bool m_poweredLinMotor = false; + real_t m_targetLinMotorVelocity = 0.0; + real_t m_maxLinMotorForce = 0.0; + real_t m_accumulatedLinMotorImpulse = 0.0; - bool m_poweredAngMotor; - real_t m_targetAngMotorVelocity; - real_t m_maxAngMotorForce; - real_t m_accumulatedAngMotorImpulse; - - //------------------------ - void initParams(); + bool m_poweredAngMotor = false; + real_t m_targetAngMotorVelocity = 0.0; + real_t m_maxAngMotorForce = 0.0; + real_t m_accumulatedAngMotorImpulse = 0.0; public: // constructors diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index fbb374bd74..8bfadeb356 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -30,6 +30,7 @@ #include "physics_server_3d_sw.h" +#include "body_direct_state_3d_sw.h" #include "broad_phase_3d_bvh.h" #include "core/debugger/engine_debugger.h" #include "core/os/os.h" @@ -42,8 +43,14 @@ #define FLUSH_QUERY_CHECK(m_object) \ ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); -RID PhysicsServer3DSW::plane_shape_create() { - Shape3DSW *shape = memnew(PlaneShape3DSW); +RID PhysicsServer3DSW::world_boundary_shape_create() { + Shape3DSW *shape = memnew(WorldBoundaryShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::separation_ray_shape_create() { + Shape3DSW *shape = memnew(SeparationRayShape3DSW); RID rid = shape_owner.make_rid(shape); shape->set_self(rid); return rid; @@ -636,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); @@ -836,6 +850,12 @@ int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } +void PhysicsServer3DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + body->set_state_sync_callback(p_instance, p_callback); +} + void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -848,7 +868,7 @@ void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { body->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, MotionResult *r_result, const Set<RID> &p_exclude) { +bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -856,18 +876,19 @@ bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result, p_exclude); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); } PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, nullptr); + ERR_FAIL_NULL_V(body, nullptr); + + ERR_FAIL_NULL_V(body->get_space(), nullptr); ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - direct_state->body = body; - return direct_state; + return body->get_direct_state(); } /* SOFT BODY */ @@ -1572,10 +1593,8 @@ void PhysicsServer3DSW::set_collision_iterations(int p_iterations) { }; void PhysicsServer3DSW::init() { - last_step = 0.001; iterations = 8; // 8? stepper = memnew(Step3DSW); - direct_state = memnew(PhysicsDirectBodyState3DSW); }; void PhysicsServer3DSW::step(real_t p_step) { @@ -1587,9 +1606,6 @@ void PhysicsServer3DSW::step(real_t p_step) { _update_shapes(); - last_step = p_step; - PhysicsDirectBodyState3DSW::singleton->step = p_step; - island_count = 0; active_objects = 0; collision_pairs = 0; @@ -1665,7 +1681,6 @@ void PhysicsServer3DSW::end_sync() { void PhysicsServer3DSW::finish() { memdelete(stepper); - memdelete(direct_state); }; int PhysicsServer3DSW::get_process_info(ProcessInfo p_info) { @@ -1729,11 +1744,5 @@ PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) { singletonsw = this; BroadPhase3DSW::create_func = BroadPhase3DBVH::_create; - island_count = 0; - active_objects = 0; - collision_pairs = 0; using_threads = p_using_threads; - active = true; - flushing_queries = false; - doing_sync = false; }; diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index 6e59a77e89..c34f8bff7a 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -42,23 +42,20 @@ class PhysicsServer3DSW : public PhysicsServer3D { GDCLASS(PhysicsServer3DSW, PhysicsServer3D); friend class PhysicsDirectSpaceState3DSW; - bool active; - int iterations; - real_t last_step; + bool active = true; + int iterations = 0; - int island_count; - int active_objects; - int collision_pairs; + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; - bool using_threads; - bool doing_sync; - bool flushing_queries; + bool using_threads = false; + bool doing_sync = false; + bool flushing_queries = false; - Step3DSW *stepper; + Step3DSW *stepper = nullptr; Set<const Space3DSW *> active_spaces; - PhysicsDirectBodyState3DSW *direct_state; - mutable RID_PtrOwner<Shape3DSW, true> shape_owner; mutable RID_PtrOwner<Space3DSW, true> space_owner; mutable RID_PtrOwner<Area3DSW, true> area_owner; @@ -82,7 +79,8 @@ public: static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); - virtual RID plane_shape_create() override; + virtual RID world_boundary_shape_create() override; + virtual RID separation_ray_shape_create() override; virtual RID sphere_shape_create() override; virtual RID box_shape_create() override; virtual RID capsule_shape_create() override; @@ -200,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; @@ -237,11 +237,12 @@ public: virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; virtual int body_get_max_contacts_reported(RID p_body) const override; + virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; - virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, const Set<RID> &p_exclude = Set<RID>()) override; + virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override; // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.cpp b/servers/physics_3d/physics_server_3d_wrap_mt.cpp index 0a89c1a9c9..c424100bba 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.cpp +++ b/servers/physics_3d/physics_server_3d_wrap_mt.cpp @@ -119,8 +119,6 @@ PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool command_queue(p_create_thread) { physics_3d_server = p_contained; create_thread = p_create_thread; - step_pending = 0; - step_thread_up = false; pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); @@ -131,7 +129,6 @@ PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool } main_thread = Thread::get_caller_id(); - first_frame = true; } PhysicsServer3DWrapMT::~PhysicsServer3DWrapMT() { diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index 75174628bf..a5683b99c3 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -58,7 +58,7 @@ class PhysicsServer3DWrapMT : public PhysicsServer3D { bool create_thread = false; Semaphore step_sem; - int step_pending; + int step_pending = 0; void thread_step(real_t p_delta); void thread_flush(); @@ -78,7 +78,8 @@ public: #include "servers/server_wrap_mt_common.h" //FUNC1RID(shape,ShapeType); todo fix - FUNCRID(plane_shape) + FUNCRID(world_boundary_shape) + FUNCRID(separation_ray_shape) FUNCRID(sphere_shape) FUNCRID(box_shape) FUNCRID(capsule_shape) @@ -209,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); @@ -245,13 +248,14 @@ public: FUNC2(body_set_omit_force_integration, RID, bool); FUNC1RC(bool, body_is_omitting_force_integration, RID); + FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback); FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); FUNC2(body_set_ray_pickable, RID, bool); - bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, const Set<RID> &p_exclude = Set<RID>()) override { + bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result, p_exclude); + return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); } // this function only works on physics process, errors and returns null otherwise diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index a84405de81..1533d6e592 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -101,30 +101,25 @@ const Map<ShapeOwner3DSW *, int> &Shape3DSW::get_owners() const { return owners; } -Shape3DSW::Shape3DSW() { - custom_bias = 0; - configured = false; -} - Shape3DSW::~Shape3DSW() { ERR_FAIL_COND(owners.size()); } -Plane PlaneShape3DSW::get_plane() const { +Plane WorldBoundaryShape3DSW::get_plane() const { return plane; } -void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { +void WorldBoundaryShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { // gibberish, a plane is infinity r_min = -1e7; r_max = 1e7; } -Vector3 PlaneShape3DSW::get_support(const Vector3 &p_normal) const { +Vector3 WorldBoundaryShape3DSW::get_support(const Vector3 &p_normal) const { return p_normal * 1e15; } -bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool WorldBoundaryShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { bool inters = plane.intersects_segment(p_begin, p_end, &r_result); if (inters) { r_normal = plane.normal; @@ -132,11 +127,11 @@ bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_ return inters; } -bool PlaneShape3DSW::intersect_point(const Vector3 &p_point) const { +bool WorldBoundaryShape3DSW::intersect_point(const Vector3 &p_point) const { return plane.distance_to(p_point) < 0; } -Vector3 PlaneShape3DSW::get_closest_point_to(const Vector3 &p_point) const { +Vector3 WorldBoundaryShape3DSW::get_closest_point_to(const Vector3 &p_point) const { if (plane.is_point_over(p_point)) { return plane.project(p_point); } else { @@ -144,26 +139,108 @@ Vector3 PlaneShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } } -Vector3 PlaneShape3DSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); //wtf +Vector3 WorldBoundaryShape3DSW::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); // not applicable. } -void PlaneShape3DSW::_setup(const Plane &p_plane) { +void WorldBoundaryShape3DSW::_setup(const Plane &p_plane) { plane = p_plane; configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); } -void PlaneShape3DSW::set_data(const Variant &p_data) { +void WorldBoundaryShape3DSW::set_data(const Variant &p_data) { _setup(p_data); } -Variant PlaneShape3DSW::get_data() const { +Variant WorldBoundaryShape3DSW::get_data() const { return plane; } -PlaneShape3DSW::PlaneShape3DSW() { +WorldBoundaryShape3DSW::WorldBoundaryShape3DSW() { +} + +// + +real_t SeparationRayShape3DSW::get_length() const { + return length; +} + +bool SeparationRayShape3DSW::get_slide_on_slope() const { + return slide_on_slope; +} + +void SeparationRayShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // don't think this will be even used + r_min = 0; + r_max = 1; +} + +Vector3 SeparationRayShape3DSW::get_support(const Vector3 &p_normal) const { + if (p_normal.z > 0) { + return Vector3(0, 0, length); + } else { + return Vector3(0, 0, 0); + } +} + +void SeparationRayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = Vector3(0, 0, 0); + r_supports[1] = Vector3(0, 0, length); + } else if (p_normal.z > 0) { + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = Vector3(0, 0, length); + } else { + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = Vector3(0, 0, 0); + } } +bool SeparationRayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + return false; //simply not possible +} + +bool SeparationRayShape3DSW::intersect_point(const Vector3 &p_point) const { + return false; //simply not possible +} + +Vector3 SeparationRayShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + Vector3 s[2] = { + Vector3(0, 0, 0), + Vector3(0, 0, length) + }; + + return Geometry3D::get_closest_point_to_segment(p_point, s); +} + +Vector3 SeparationRayShape3DSW::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); +} + +void SeparationRayShape3DSW::_setup(real_t p_length, bool p_slide_on_slope) { + length = p_length; + slide_on_slope = p_slide_on_slope; + configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); +} + +void SeparationRayShape3DSW::set_data(const Variant &p_data) { + Dictionary d = p_data; + _setup(d["length"], d["slide_on_slope"]); +} + +Variant SeparationRayShape3DSW::get_data() const { + Dictionary d; + d["length"] = length; + d["slide_on_slope"] = slide_on_slope; + return d; +} + +SeparationRayShape3DSW::SeparationRayShape3DSW() {} + /********** SPHERE *************/ real_t SphereShape3DSW::get_radius() const { @@ -226,9 +303,7 @@ Variant SphereShape3DSW::get_data() const { return radius; } -SphereShape3DSW::SphereShape3DSW() { - radius = 0; -} +SphereShape3DSW::SphereShape3DSW() {} /********** BOX *************/ @@ -417,8 +492,7 @@ Variant BoxShape3DSW::get_data() const { return half_extents; } -BoxShape3DSW::BoxShape3DSW() { -} +BoxShape3DSW::BoxShape3DSW() {} /********** CAPSULE *************/ @@ -583,9 +657,7 @@ Variant CapsuleShape3DSW::get_data() const { return d; } -CapsuleShape3DSW::CapsuleShape3DSW() { - height = radius = 0; -} +CapsuleShape3DSW::CapsuleShape3DSW() {} /********** CYLINDER *************/ @@ -763,9 +835,7 @@ Variant CylinderShape3DSW::get_data() const { return d; } -CylinderShape3DSW::CylinderShape3DSW() { - height = radius = 0; -} +CylinderShape3DSW::CylinderShape3DSW() {} /********** CONVEX POLYGON *************/ @@ -1297,11 +1367,11 @@ Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) co return Vector3(); } -void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { +bool ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { const BVH *bvh = &p_params->bvh[p_idx]; if (!p_params->aabb.intersects(bvh->aabb)) { - return; + return false; } if (bvh->face_index >= 0) { @@ -1311,20 +1381,27 @@ void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { face->vertex[0] = p_params->vertices[f->indices[0]]; face->vertex[1] = p_params->vertices[f->indices[1]]; face->vertex[2] = p_params->vertices[f->indices[2]]; - p_params->callback(p_params->userdata, face); - + if (p_params->callback(p_params->userdata, face)) { + return true; + } } else { if (bvh->left >= 0) { - _cull(bvh->left, p_params); + if (_cull(bvh->left, p_params)) { + return true; + } } if (bvh->right >= 0) { - _cull(bvh->right, p_params); + if (_cull(bvh->right, p_params)) { + return true; + } } } + + return false; } -void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { +void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { // make matrix local to concave if (faces.size() == 0) { return; @@ -1584,6 +1661,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; @@ -1596,11 +1684,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; @@ -1608,7 +1696,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; @@ -1617,13 +1705,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; @@ -1631,136 +1757,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); } } @@ -1790,7 +1961,7 @@ void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, i r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5); } -void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { +void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { if (heights.is_empty()) { return; } @@ -1825,14 +1996,18 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, voi _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; - p_callback(p_userdata, &face); + face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal; + if (p_callback(p_userdata, &face)) { + return; + } // 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; - p_callback(p_userdata, &face); + face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal; + if (p_callback(p_userdata, &face)) { + return; + } } } } @@ -1847,6 +2022,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; @@ -1863,6 +2107,8 @@ void HeightMapShape3DSW::_setup(const Vector<real_t> &p_heights, int p_width, in aabb.position -= local_origin; + _build_accelerator(); + configure(aabb); } @@ -1921,7 +2167,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 c11c3a08f6..061d66a085 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -32,18 +32,8 @@ #define SHAPE_SW_H #include "core/math/geometry_3d.h" +#include "core/templates/local_vector.h" #include "servers/physics_server_3d.h" -/* - -SHAPE_LINE, ///< plane:"plane" -SHAPE_SEGMENT, ///< real_t:"length" -SHAPE_CIRCLE, ///< real_t:"radius" -SHAPE_RECTANGLE, ///< vec3:"extents" -SHAPE_CONVEX_POLYGON, ///< array of planes:"planes" -SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array) -SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error - -*/ class Shape3DSW; @@ -58,8 +48,8 @@ public: class Shape3DSW { RID self; AABB aabb; - bool configured; - real_t custom_bias; + bool configured = false; + real_t custom_bias = 0.0; Map<ShapeOwner3DSW *, int> owners; @@ -105,22 +95,24 @@ public: bool is_owner(ShapeOwner3DSW *p_owner) const; const Map<ShapeOwner3DSW *, int> &get_owners() const; - Shape3DSW(); + Shape3DSW() {} virtual ~Shape3DSW(); }; class ConcaveShape3DSW : public Shape3DSW { public: - virtual bool is_concave() const { return true; } - typedef void (*Callback)(void *p_userdata, Shape3DSW *p_convex); - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } + virtual bool is_concave() const override { return true; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; + // Returns true to stop the query. + typedef bool (*QueryCallback)(void *p_userdata, Shape3DSW *p_convex); + + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; ConcaveShape3DSW() {} }; -class PlaneShape3DSW : public Shape3DSW { +class WorldBoundaryShape3DSW : public Shape3DSW { Plane plane; void _setup(const Plane &p_plane); @@ -128,46 +120,74 @@ class PlaneShape3DSW : public Shape3DSW { public: Plane get_plane() const; - virtual real_t get_area() const { return INFINITY; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_PLANE; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } + virtual real_t get_area() const override { return INFINITY; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_WORLD_BOUNDARY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; - PlaneShape3DSW(); + WorldBoundaryShape3DSW(); +}; + +class SeparationRayShape3DSW : public Shape3DSW { + real_t length = 1.0; + bool slide_on_slope = false; + + void _setup(real_t p_length, bool p_slide_on_slope); + +public: + real_t get_length() const; + bool get_slide_on_slope() const; + + virtual real_t get_area() const override { return 0.0; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SEPARATION_RAY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + SeparationRayShape3DSW(); }; class SphereShape3DSW : public Shape3DSW { - real_t radius; + real_t radius = 0.0; void _setup(real_t p_radius); public: real_t get_radius() const; - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } + virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SPHERE; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SPHERE; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; SphereShape3DSW(); }; @@ -178,28 +198,28 @@ class BoxShape3DSW : public Shape3DSW { public: _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } - virtual real_t get_area() const { return 8 * half_extents.x * half_extents.y * half_extents.z; } + virtual real_t get_area() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_BOX; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_BOX; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; BoxShape3DSW(); }; class CapsuleShape3DSW : public Shape3DSW { - real_t height; - real_t radius; + real_t height = 0.0; + real_t radius = 0.0; void _setup(real_t p_height, real_t p_radius); @@ -207,28 +227,28 @@ public: _FORCE_INLINE_ real_t get_height() const { return height; } _FORCE_INLINE_ real_t get_radius() const { return radius; } - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; } + virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CAPSULE; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CAPSULE; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; CapsuleShape3DSW(); }; class CylinderShape3DSW : public Shape3DSW { - real_t height; - real_t radius; + real_t height = 0.0; + real_t radius = 0.0; void _setup(real_t p_height, real_t p_radius); @@ -236,21 +256,21 @@ public: _FORCE_INLINE_ real_t get_height() const { return height; } _FORCE_INLINE_ real_t get_radius() const { return radius; } - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CYLINDER; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CYLINDER; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; CylinderShape3DSW(); }; @@ -263,19 +283,19 @@ struct ConvexPolygonShape3DSW : public Shape3DSW { public: const Geometry3D::MeshData &get_mesh() const { return mesh; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; ConvexPolygonShape3DSW(); }; @@ -288,7 +308,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { struct Face { Vector3 normal; - int indices[3]; + int indices[3] = {}; }; Vector<Face> faces; @@ -296,17 +316,17 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { struct BVH { AABB aabb; - int left; - int right; + int left = 0; + int right = 0; - int face_index; + int face_index = 0; }; Vector<BVH> bvh; struct _CullParams { AABB aabb; - Callback callback = nullptr; + QueryCallback callback = nullptr; void *userdata = nullptr; const Face *faces = nullptr; const Vector3 *vertices = nullptr; @@ -332,7 +352,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { bool backface_collision = false; void _cull_segment(int p_idx, _SegmentCullParams *p_params) const; - void _cull(int p_idx, _CullParams *p_params) const; + bool _cull(int p_idx, _CullParams *p_params) const; void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx); @@ -341,21 +361,21 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { public: Vector<Vector3> get_faces() const; - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; ConcavePolygonShape3DSW(); }; @@ -366,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]; } @@ -378,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: @@ -385,20 +425,20 @@ public: int get_width() const; int get_depth() const; - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_HEIGHTMAP; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_HEIGHTMAP; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; HeightMapShape3DSW(); }; @@ -409,32 +449,32 @@ struct FaceShape3DSW : public Shape3DSW { Vector3 vertex[3]; bool backface_collision = false; - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } - void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const; - Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; - bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - Vector3 get_moment_of_inertia(real_t p_mass) const; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; - virtual void set_data(const Variant &p_data) {} - virtual Variant get_data() const { return Variant(); } + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } FaceShape3DSW(); }; struct MotionShape3DSW : public Shape3DSW { - Shape3DSW *shape; + Shape3DSW *shape = nullptr; Vector3 motion; - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } - void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { Vector3 cast = p_transform.basis.xform(motion); real_t mina, maxa; real_t minb, maxb; @@ -446,22 +486,23 @@ struct MotionShape3DSW : public Shape3DSW { r_max = MAX(maxa, maxb); } - Vector3 get_support(const Vector3 &p_normal) const { + virtual Vector3 get_support(const Vector3 &p_normal) const override { Vector3 support = shape->get_support(p_normal); if (p_normal.dot(motion) > 0) { support += motion; } return support; } - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } - bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; } - virtual bool intersect_point(const Vector3 &p_point) const { return false; } - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; } - Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override { return false; } + virtual bool intersect_point(const Vector3 &p_point) const override { return false; } + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override { return p_point; } + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); } - virtual void set_data(const Variant &p_data) {} - virtual Variant get_data() const { return Variant(); } + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } MotionShape3DSW() { configure(AABB()); } }; diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp index 73b81444e1..5f6e202c73 100644 --- a/servers/physics_3d/soft_body_3d_sw.cpp +++ b/servers/physics_3d/soft_body_3d_sw.cpp @@ -165,7 +165,7 @@ void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_s p_rendering_server_handler->set_aabb(bounds); } -void SoftBody3DSW::update_normals() { +void SoftBody3DSW::update_normals_and_centroids() { uint32_t i, ni; for (i = 0, ni = nodes.size(); i < ni; ++i) { @@ -180,6 +180,7 @@ void SoftBody3DSW::update_normals() { face.n[2]->n += n; face.normal = n; face.normal.normalize(); + face.centroid = 0.33333333333 * (face.n[0]->x + face.n[1]->x + face.n[2]->x); } for (i = 0, ni = nodes.size(); i < ni; ++i) { @@ -310,7 +311,7 @@ void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) { face_tree.clear(); - update_normals(); + update_normals_and_centroids(); update_bounds(); update_constants(); } @@ -574,7 +575,7 @@ bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vecto reoptimize_link_order(); update_constants(); - update_normals(); + update_normals_and_centroids(); update_bounds(); return true; @@ -898,47 +899,86 @@ void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) { } } -void SoftBody3DSW::apply_forces() { - if (pressure_coefficient < CMP_EPSILON) { - return; - } +void SoftBody3DSW::apply_forces(bool p_has_wind_forces) { + int ac = areas.size(); if (nodes.is_empty()) { return; } uint32_t i, ni; + int32_t j; - // Calculate volume. real_t volume = 0.0; const Vector3 &org = nodes[0].x; + + // Iterate over faces (try not to iterate elsewhere if possible). for (i = 0, ni = faces.size(); i < ni; ++i) { + bool stopped = false; const Face &face = faces[i]; + + Vector3 wind_force(0, 0, 0); + + // Compute volume. volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org)); + + // Compute nodal forces from area winds. + if (ac && p_has_wind_forces) { + const AreaCMP *aa = &areas[0]; + for (j = ac - 1; j >= 0 && !stopped; j--) { + PhysicsServer3D::AreaSpaceOverrideMode mode = aa[j].area->get_space_override_mode(); + switch (mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + wind_force += _compute_area_windforce(aa[j].area, &face); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + wind_force = _compute_area_windforce(aa[j].area, &face); + stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + + for (j = 0; j < 3; j++) { + Node *current_node = face.n[j]; + current_node->f += wind_force; + } + } } volume /= 6.0; - // Apply per node forces. - real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; - for (i = 0, ni = nodes.size(); i < ni; ++i) { - Node &node = nodes[i]; - if (node.im > 0) { - node.f += node.n * (node.area * ivolumetp); + // Apply nodal pressure forces. + if (pressure_coefficient > CMP_EPSILON) { + real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; + for (i = 0, ni = nodes.size(); i < ni; ++i) { + Node &node = nodes[i]; + if (node.im > 0) { + node.f += node.n * (node.area * ivolumetp); + } } } } void SoftBody3DSW::_compute_area_gravity(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(); - } + Vector3 area_gravity; + p_area->compute_gravity(get_transform().get_origin(), area_gravity); + gravity += area_gravity; +} + +Vector3 SoftBody3DSW::_compute_area_windforce(const Area3DSW *p_area, const Face *p_face) { + real_t wfm = p_area->get_wind_force_magnitude(); + real_t waf = p_area->get_wind_attenuation_factor(); + const Vector3 &wd = p_area->get_wind_direction(); + const Vector3 &ws = p_area->get_wind_source(); + real_t projection_on_tri_normal = vec3_dot(p_face->normal, wd); + real_t projection_toward_centroid = vec3_dot(p_face->centroid - ws, wd); + real_t attenuation_over_distance = pow(projection_toward_centroid, -waf); + real_t nodal_force_magnitude = wfm * 0.33333333333 * p_face->ra * projection_on_tri_normal * attenuation_over_distance; + return nodal_force_magnitude * p_face->normal; } void SoftBody3DSW::predict_motion(real_t p_delta) { @@ -952,11 +992,15 @@ void SoftBody3DSW::predict_motion(real_t p_delta) { int ac = areas.size(); bool stopped = false; + bool has_wind_forces = false; if (ac) { areas.sort(); const AreaCMP *aa = &areas[0]; for (int i = ac - 1; i >= 0 && !stopped; i--) { + // Avoids unnecessary loop in apply_forces(). + has_wind_forces = has_wind_forces || aa[i].area->get_wind_force_magnitude() > CMP_EPSILON; + PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); switch (mode) { case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: @@ -978,7 +1022,9 @@ void SoftBody3DSW::predict_motion(real_t p_delta) { // Apply forces. add_velocity(gravity * p_delta); - apply_forces(); + if (pressure_coefficient > CMP_EPSILON || has_wind_forces) { + apply_forces(has_wind_forces); + } // Avoid soft body from 'exploding' so use some upper threshold of maximum motion // that a node can travel per frame. @@ -1057,7 +1103,7 @@ void SoftBody3DSW::solve_constraints(real_t p_delta) { node.q = node.x; } - update_normals(); + update_normals_and_centroids(); } void SoftBody3DSW::solve_links(real_t kst, real_t ti) { diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h index be0620c506..58fd234fde 100644 --- a/servers/physics_3d/soft_body_3d_sw.h +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -71,6 +71,7 @@ class SoftBody3DSW : public CollisionObject3DSW { }; struct Face { + Vector3 centroid; Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers Vector3 normal; // Normal real_t ra = 0.0; // Rest area @@ -114,6 +115,7 @@ class SoftBody3DSW : public CollisionObject3DSW { uint64_t island_step = 0; _FORCE_INLINE_ void _compute_area_gravity(const Area3DSW *p_area); + _FORCE_INLINE_ Vector3 _compute_area_windforce(const Area3DSW *p_area, const Face *p_face); public: SoftBody3DSW(); @@ -220,7 +222,7 @@ protected: virtual void _shapes_changed(); private: - void update_normals(); + void update_normals_and_centroids(); void update_bounds(); void update_constants(); void update_area(); @@ -231,7 +233,7 @@ private: void add_velocity(const Vector3 &p_velocity); - void apply_forces(); + void apply_forces(bool p_has_wind_forces); bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices); void generate_bending_constraints(int p_distance); diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index f9e55ad525..bf72e90932 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -569,7 +569,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { return amount; } -bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, const Set<RID> &p_exclude) { +bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { //give me back regular physics engine logic //this is madness //and most people using this function will think @@ -714,9 +714,19 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co continue; } - Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); + // Colliding separation rays allows to properly snap to the ground, + // otherwise it's not needed in regular motion. + if (!p_collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) { + // When slide on slope is on, separation ray shape acts like a regular shape. + if (!static_cast<SeparationRayShape3DSW *>(body_shape)->get_slide_on_slope()) { + continue; + } + } + + Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); + Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse(); MotionShape3DSW mshape; mshape.shape = body_shape; @@ -967,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() { @@ -1049,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()); } } @@ -1132,18 +1142,6 @@ PhysicsDirectSpaceState3DSW *Space3DSW::get_direct_state() { } Space3DSW::Space3DSW() { - collision_pairs = 0; - active_objects = 0; - island_count = 0; - contact_debug_count = 0; - - locked = false; - contact_recycle_radius = 0.01; - contact_max_separation = 0.05; - contact_max_allowed_penetration = 0.01; - test_motion_min_contact_depth = 0.00001; - - constraint_bias = 0.01; body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0)); body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); @@ -1153,14 +1151,9 @@ Space3DSW::Space3DSW() { broadphase = BroadPhase3DSW::create_func(); broadphase->set_pair_callback(_broadphase_pair, this); broadphase->set_unpair_callback(_broadphase_unpair, this); - area = nullptr; direct_access = memnew(PhysicsDirectSpaceState3DSW); direct_access->space = this; - - for (int i = 0; i < ELAPSED_TIME_MAX; i++) { - elapsed_time[i] = 0; - } } Space3DSW::~Space3DSW() { diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index 9b5b4de069..aa4557d136 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(); @@ -72,14 +72,14 @@ public: }; private: - uint64_t elapsed_time[ELAPSED_TIME_MAX]; + uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; PhysicsDirectSpaceState3DSW *direct_access; RID self; 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; @@ -90,13 +90,13 @@ private: Set<CollisionObject3DSW *> objects; - Area3DSW *area; + Area3DSW *area = nullptr; - real_t contact_recycle_radius; - real_t contact_max_separation; - real_t contact_max_allowed_penetration; - real_t constraint_bias; - real_t test_motion_min_contact_depth; + real_t contact_recycle_radius = 0.01; + real_t contact_max_separation = 0.05; + real_t contact_max_allowed_penetration = 0.01; + real_t constraint_bias = 0.01; + real_t test_motion_min_contact_depth = 0.00001; enum { INTERSECTION_QUERY_MAX = 2048 @@ -110,16 +110,18 @@ private: real_t body_time_to_sleep; real_t body_angular_velocity_damp_ratio; - bool locked; + bool locked = false; - int island_count; - int active_objects; - int collision_pairs; + real_t last_step = 0.001; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; RID static_global_body; Vector<Vector3> contact_debug; - int contact_debug_count; + int contact_debug_count = 0; friend class PhysicsDirectSpaceState3DSW; @@ -135,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); @@ -174,6 +176,9 @@ public: void lock(); void unlock(); + real_t get_last_step() const { return last_step; } + void set_last_step(real_t p_step) { last_step = p_step; } + void set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value); real_t get_param(PhysicsServer3D::SpaceParameter p_param) const; @@ -203,7 +208,7 @@ public: void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } - bool test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, const Set<RID> &p_exclude = Set<RID>()); + bool test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()); Space3DSW(); ~Space3DSW(); diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index ba18bac611..7c18944b4d 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -185,6 +185,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + p_space->set_last_step(p_delta); + iterations = p_iterations; delta = p_delta; @@ -405,8 +407,6 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { } Step3DSW::Step3DSW() { - _step = 1; - body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE); all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h index 9c60004b24..f2f879104a 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -37,7 +37,7 @@ #include "core/templates/thread_work_pool.h" class Step3DSW { - uint64_t _step; + uint64_t _step = 1; int iterations = 0; real_t delta = 0.0; |