diff options
Diffstat (limited to 'servers/physics_3d')
26 files changed, 995 insertions, 377 deletions
diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index 4de5f1ba47..2073df7dee 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -40,31 +40,48 @@ bool AreaPair3DSW::setup(real_t p_step) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - body->add_area(area); - } - if (area->has_monitor_callback()) { - area->add_body_to_query(body, body_shape, area_shape); - } - - } else { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + process_collision = true; + } else if (area->has_monitor_callback()) { + process_collision = true; } colliding = result; } - return false; //never do any post solving + return process_collision; +} + +bool AreaPair3DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. } void AreaPair3DSW::solve(real_t p_step) { + // Nothing to do. } AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) { @@ -72,7 +89,6 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, area = p_area; body_shape = p_body_shape; area_shape = p_area_shape; - colliding = false; body->add_constraint(this, 0); area->add_constraint(this); if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { @@ -103,33 +119,48 @@ bool Area2Pair3DSW::setup(real_t p_step) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - 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()) { - 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); - } + 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()) { + process_collision = true; } colliding = result; } - return false; //never do any post solving + return process_collision; +} + +bool Area2Pair3DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + 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()) { + 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. } void Area2Pair3DSW::solve(real_t p_step) { + // Nothing to do. } Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b) { @@ -137,7 +168,6 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area area_b = p_area_b; shape_a = p_shape_a; shape_b = p_shape_b; - colliding = false; area_a->add_constraint(this); area_b->add_constraint(this); } diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h index fbdaa25cbb..596d893082 100644 --- a/servers/physics_3d/area_pair_3d_sw.h +++ b/servers/physics_3d/area_pair_3d_sw.h @@ -40,11 +40,13 @@ class AreaPair3DSW : public Constraint3DSW { Area3DSW *area; int body_shape; int area_shape; - bool colliding; + bool colliding = false; + bool process_collision = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape); ~AreaPair3DSW(); @@ -55,11 +57,13 @@ class Area2Pair3DSW : public Constraint3DSW { Area3DSW *area_b; int shape_a; int shape_b; - bool colliding; + bool colliding = false; + bool process_collision = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b); ~Area2Pair3DSW(); diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index 64ba0cb09d..4357c474e4 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -145,31 +145,17 @@ void Body3DSW::set_active(bool p_active) { } active = p_active; - if (!p_active) { - if (get_space()) { - get_space()->body_remove_from_active_list(&active_list); - } - } else { + + if (active) { if (mode == PhysicsServer3D::BODY_MODE_STATIC) { - return; //static bodies can't become active - } - if (get_space()) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { get_space()->body_add_to_active_list(&active_list); } - - //still_time=0; + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); } - /* - if (!space) - return; - - for(int i=0;i<get_shape_count();i++) { - Shape &s=shapes[i]; - if (s.bpid>0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } - } -*/ } void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) { @@ -392,13 +378,6 @@ void Body3DSW::set_space(Space3DSW *p_space) { if (active) { get_space()->body_add_to_active_list(&active_list); } - /* - _update_queries(); - if (is_active()) { - active=false; - set_active(true); - } - */ } first_integration = true; @@ -693,15 +672,16 @@ void Body3DSW::call_queries() { Variant v = dbs; - Object *obj = ObjectDB::get_instance(fi_callback->id); + Object *obj = fi_callback->callable.get_object(); if (!obj) { - set_force_integration_callback(ObjectID(), StringName()); + set_force_integration_callback(Callable()); } else { const Variant *vp[2] = { &v, &fi_callback->udata }; Callable::CallError ce; int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2; - obj->call(fi_callback->method, vp, argc, ce); + Variant rv; + fi_callback->callable.call(vp, argc, rv, ce); } } } @@ -725,16 +705,15 @@ bool Body3DSW::sleep_test(real_t p_step) { } } -void Body3DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { +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_id.is_valid()) { + if (p_callable.get_object()) { fi_callback = memnew(ForceIntegrationCallback); - fi_callback->id = p_id; - fi_callback->method = p_method; + fi_callback->callable = p_callable; fi_callback->udata = p_udata; } } @@ -761,8 +740,6 @@ Body3DSW::Body3DSW() : omit_force_integration = false; //applied_torque=0; island_step = 0; - island_next = nullptr; - island_list_next = nullptr; first_time_kinematic = false; first_integration = false; _set_static(false); diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index e87ff2364b..9afb8cd56f 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -127,16 +127,13 @@ class Body3DSW : public CollisionObject3DSW { int contact_count; struct ForceIntegrationCallback { - ObjectID id; - StringName method; + Callable callable; Variant udata; }; ForceIntegrationCallback *fi_callback; uint64_t island_step; - Body3DSW *island_next; - Body3DSW *island_list_next; _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area3DSW *p_area); @@ -145,7 +142,7 @@ class Body3DSW : public CollisionObject3DSW { friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose public: - void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); + void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); void set_kinematic_margin(real_t p_margin); _FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; } @@ -189,12 +186,6 @@ public: _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ Body3DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Body3DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Body3DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Body3DSW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint); } const Map<Constraint3DSW *, int> &get_constraint_map() const { return constraint_map; } diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 36114c0c91..cdb3da665e 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -212,14 +212,16 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) { } bool BodyPair3DSW::setup(real_t p_step) { - //cannot collide + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; } - bool report_contacts_only = false; - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + report_contacts_only = false; + if (!dynamic_A && !dynamic_B) { if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { report_contacts_only = true; } else { @@ -237,7 +239,7 @@ bool BodyPair3DSW::setup(real_t p_step) { validate_contacts(); - Vector3 offset_A = A->get_transform().get_origin(); + const Vector3 &offset_A = A->get_transform().get_origin(); Transform xform_Au = Transform(A->get_transform().basis, Vector3()); Transform xform_A = xform_Au * A->get_shape_transform(shape_A); @@ -248,27 +250,37 @@ bool BodyPair3DSW::setup(real_t p_step) { Shape3DSW *shape_A_ptr = A->get_shape(shape_A); Shape3DSW *shape_B_ptr = B->get_shape(shape_B); - bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - this->collided = collided; + collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); if (!collided) { //test ccd (currently just a raycast) - if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) { + if (A->is_continuous_collision_detection_enabled() && dynamic_A && !dynamic_B) { _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); } - if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) { + if (B->is_continuous_collision_detection_enabled() && dynamic_B && !dynamic_A) { _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); } return false; } + return true; +} + +bool BodyPair3DSW::pre_solve(real_t p_step) { + if (!collided) { + return false; + } + real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = (real_t)0.3; + Shape3DSW *shape_A_ptr = A->get_shape(shape_A); + Shape3DSW *shape_B_ptr = B->get_shape(shape_B); + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias() == 0) { bias = shape_B_ptr->get_custom_bias(); @@ -281,22 +293,28 @@ bool BodyPair3DSW::setup(real_t p_step) { real_t inv_dt = 1.0 / p_step; + bool do_process = false; + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; c.active = false; - Vector3 global_A = xform_Au.xform(c.local_A); - Vector3 global_B = xform_Bu.xform(c.local_B); + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; - real_t depth = c.normal.dot(global_A - global_B); + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); if (depth <= 0) { continue; } #ifdef DEBUG_ENABLED - if (space->is_debugging_contacts()) { + const Vector3 &offset_A = A->get_transform().get_origin(); space->add_debug_contact(global_A + offset_A); space->add_debug_contact(global_B + offset_A); } @@ -323,6 +341,7 @@ bool BodyPair3DSW::setup(real_t p_step) { } c.active = true; + do_process = true; // Precompute normal mass, tangent mass, and bias. Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal)); @@ -335,8 +354,12 @@ bool BodyPair3DSW::setup(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); - B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); + if (dynamic_A) { + A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); + } + if (dynamic_B) { + B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); + } c.acc_bias_impulse = 0; c.acc_bias_impulse_center_of_mass = 0; @@ -350,7 +373,7 @@ bool BodyPair3DSW::setup(real_t p_step) { } } - return true; + return do_process; } void BodyPair3DSW::solve(real_t p_step) { @@ -358,6 +381,8 @@ void BodyPair3DSW::solve(real_t p_step) { return; } + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; if (!c.active) { @@ -381,8 +406,12 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); - B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); + if (dynamic_A) { + A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av); + } + if (dynamic_B) { + B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av); + } crbA = A->get_biased_angular_velocity().cross(c.rA); crbB = B->get_biased_angular_velocity().cross(c.rB); @@ -397,8 +426,12 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); - A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); - B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + if (dynamic_A) { + A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); + } + if (dynamic_B) { + B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + } } c.active = true; @@ -418,8 +451,12 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - A->apply_impulse(-j, c.rA + A->get_center_of_mass()); - B->apply_impulse(j, c.rB + B->get_center_of_mass()); + if (dynamic_A) { + A->apply_impulse(-j, c.rA + A->get_center_of_mass()); + } + if (dynamic_B) { + B->apply_impulse(j, c.rB + B->get_center_of_mass()); + } c.active = true; } @@ -461,8 +498,12 @@ void BodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); - B->apply_impulse(jt, c.rB + B->get_center_of_mass()); + if (dynamic_A) { + A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); + } + if (dynamic_B) { + B->apply_impulse(jt, c.rB + B->get_center_of_mass()); + } c.active = true; } @@ -478,8 +519,6 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh space = A->get_space(); A->add_constraint(this, 0); B->add_constraint(this, 1); - contact_count = 0; - collided = false; } BodyPair3DSW::~BodyPair3DSW() { @@ -561,6 +600,8 @@ void BodySoftBodyPair3DSW::validate_contacts() { } bool BodySoftBodyPair3DSW::setup(real_t p_step) { + body_dynamic = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + if (!body->test_collision_mask(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) { collided = false; return false; @@ -582,18 +623,32 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { Shape3DSW *shape_A_ptr = body->get_shape(body_shape); Shape3DSW *shape_B_ptr = soft_body->get_shape(0); - bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - this->collided = collided; + collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); + + return collided; +} + +bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { + if (!collided) { + return false; + } real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = (real_t)0.3; + + Shape3DSW *shape_A_ptr = body->get_shape(body_shape); + if (shape_A_ptr->get_custom_bias()) { bias = shape_A_ptr->get_custom_bias(); } real_t inv_dt = 1.0 / p_step; + bool do_process = false; + + const Transform &transform_A = body->get_transform(); + uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; @@ -604,16 +659,17 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { continue; } - Vector3 global_A = xform_Au.xform(c.local_A); + Vector3 global_A = transform_A.xform(c.local_A); Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; - - real_t depth = c.normal.dot(global_A - global_B); + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); if (depth <= 0) { continue; } c.active = true; + do_process = true; #ifdef DEBUG_ENABLED @@ -623,7 +679,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { } #endif - c.rA = global_A - xform_Au.origin - body->get_center_of_mass(); + c.rA = global_A - transform_A.origin - body->get_center_of_mass(); c.rB = global_B; if (body->can_report_contacts()) { @@ -631,7 +687,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA); } - if (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { + if (body_dynamic) { body->set_active(true); } @@ -645,7 +701,9 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - body->apply_impulse(c.rA + body->get_center_of_mass(), -j_vec); + if (body_dynamic) { + body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass()); + } soft_body->apply_node_impulse(c.index_B, j_vec); c.acc_bias_impulse = 0; c.acc_bias_impulse_center_of_mass = 0; @@ -661,7 +719,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { } } - return true; + return do_process; } void BodySoftBodyPair3DSW::solve(real_t p_step) { @@ -669,6 +727,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { return; } + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; @@ -691,7 +751,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - body->apply_bias_impulse(c.rA + body->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step); + if (body_dynamic) { + body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av); + } soft_body->apply_node_bias_impulse(c.index_B, jb); crbA = body->get_biased_angular_velocity().cross(c.rA); @@ -706,8 +768,10 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); - body->apply_bias_impulse(body->get_center_of_mass(), -jb_com, 0.0f); - soft_body->apply_node_bias_impulse(c.index_B, -jb_com); + if (body_dynamic) { + body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f); + } + soft_body->apply_node_bias_impulse(c.index_B, jb_com); } c.active = true; @@ -726,7 +790,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - body->apply_impulse(c.rA + body->get_center_of_mass(), -j); + if (body_dynamic) { + body->apply_impulse(-j, c.rA + body->get_center_of_mass()); + } soft_body->apply_node_impulse(c.index_B, j); c.active = true; @@ -767,7 +833,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - body->apply_impulse(c.rA + body->get_center_of_mass(), -jt); + if (body_dynamic) { + body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); + } soft_body->apply_node_impulse(c.index_B, jt); c.active = true; @@ -775,7 +843,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { } } -BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) { +BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) : + BodyContact3DSW(&body, 1) { body = p_A; soft_body = p_B; body_shape = p_shape_A; diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 74dddfa6aa..3f425ba2d7 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -57,9 +57,9 @@ protected: }; Vector3 sep_axis; - bool collided; + bool collided = false; - Space3DSW *space; + Space3DSW *space = nullptr; BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : Constraint3DSW(p_body_ptr, p_body_count) { @@ -77,16 +77,21 @@ class BodyPair3DSW : public BodyContact3DSW { Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = { nullptr, nullptr }; }; - int shape_A; - int shape_B; + int shape_A = 0; + int shape_B = 0; + + bool dynamic_A = false; + bool dynamic_B = false; + + bool report_contacts_only = false; Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection Contact contacts[MAX_CONTACTS]; - int contact_count; + int contact_count = 0; static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); @@ -96,18 +101,21 @@ class BodyPair3DSW : public BodyContact3DSW { bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B); public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B); ~BodyPair3DSW(); }; class BodySoftBodyPair3DSW : public BodyContact3DSW { - Body3DSW *body; - SoftBody3DSW *soft_body; + Body3DSW *body = nullptr; + SoftBody3DSW *soft_body = nullptr; - int body_shape; + int body_shape = 0; + + bool body_dynamic = false; LocalVector<Contact> contacts; @@ -118,8 +126,12 @@ class BodySoftBodyPair3DSW : public BodyContact3DSW { void validate_contacts(); public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const override { return soft_body; } + virtual int get_soft_body_count() const override { return 1; } BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B); ~BodySoftBodyPair3DSW(); diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h index 2571335c43..7b44726ef5 100644 --- a/servers/physics_3d/constraint_3d_sw.h +++ b/servers/physics_3d/constraint_3d_sw.h @@ -31,14 +31,13 @@ #ifndef CONSTRAINT_SW_H #define CONSTRAINT_SW_H -#include "body_3d_sw.h" +class Body3DSW; +class SoftBody3DSW; class Constraint3DSW { Body3DSW **_body_ptr; int _body_count; uint64_t island_step; - Constraint3DSW *island_next; - Constraint3DSW *island_list_next; int priority; bool disabled_collisions_between_bodies; @@ -60,15 +59,12 @@ public: _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ Constraint3DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Constraint3DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Constraint3DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Constraint3DSW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ int get_body_count() const { return _body_count; } + virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const { return nullptr; } + virtual int get_soft_body_count() const { return 0; } + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ int get_priority() const { return priority; } @@ -76,6 +72,7 @@ public: _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0; virtual ~Constraint3DSW() {} 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 9c4493f4a2..e9efddf165 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -109,6 +109,13 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans } bool ConeTwistJoint3DSW::setup(real_t p_timestep) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + m_appliedImpulse = real_t(0.); //set bias, sign, clear accumulator @@ -261,8 +268,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } } } @@ -283,8 +294,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { Vector3 impulse = m_swingAxis * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } } // solve twist limit @@ -299,8 +314,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { Vector3 impulse = m_twistAxis * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } } } } 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 4e4d4e7f0c..b871ea50db 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -102,10 +102,10 @@ public: bool m_solveSwingLimit; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame); diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp index 13b389251f..7c504764a7 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -82,7 +82,7 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) { real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( real_t timeStep, Vector3 &axis, real_t jacDiagABInv, - Body3DSW *body0, Body3DSW *body1) { + Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) { if (!needApplyTorques()) { return 0.0f; } @@ -138,8 +138,10 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( Vector3 motorImp = clippedMotorImpulse * axis; - body0->apply_torque_impulse(motorImp); - if (body1) { + if (p_body0_dynamic) { + body0->apply_torque_impulse(motorImp); + } + if (body1 && p_body1_dynamic) { body1->apply_torque_impulse(-motorImp); } @@ -154,6 +156,7 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( real_t jacDiagABInv, Body3DSW *body1, const Vector3 &pointInA, Body3DSW *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, int limit_index, const Vector3 &axis_normal_on_a, const Vector3 &anchorPos) { @@ -205,8 +208,12 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; Vector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1->apply_impulse(impulse_vector, rel_pos1); - body2->apply_impulse(-impulse_vector, rel_pos2); + if (p_body1_dynamic) { + body1->apply_impulse(impulse_vector, rel_pos1); + } + if (p_body2_dynamic) { + body2->apply_impulse(-impulse_vector, rel_pos2); + } return normalImpulse; } @@ -303,6 +310,13 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) { } bool Generic6DOFJoint3DSW::setup(real_t p_timestep) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + // Clear accumulated impulses for the next simulation step m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.)); int i; @@ -380,6 +394,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) { jacDiagABInv, A, pointInA, B, pointInB, + dynamic_A, dynamic_B, i, linear_axis, m_AnchorPos); } } @@ -394,7 +409,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) { angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); - m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B); + m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B); } } } 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 d61a033231..8af76cefc2 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -120,7 +120,7 @@ public: int testLimitValue(real_t test_value); //! apply the correction impulses for two bodies - real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1); + real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic); }; class G6DOFTranslationalLimitMotor3DSW { @@ -166,6 +166,7 @@ public: real_t jacDiagABInv, Body3DSW *body1, const Vector3 &pointInA, Body3DSW *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, int limit_index, const Vector3 &axis_normal_on_a, const Vector3 &anchorPos); @@ -234,10 +235,10 @@ protected: public: Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_6DOF; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; //! Calcs global transform of the offsets /*! diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index 2b9f0038b4..bb8858c28a 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -155,6 +155,13 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo } bool HingeJoint3DSW::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + m_appliedImpulse = real_t(0.); if (!m_angularOnly) { @@ -275,8 +282,12 @@ void HingeJoint3DSW::solve(real_t p_step) { real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } } } @@ -318,8 +329,12 @@ void HingeJoint3DSW::solve(real_t p_step) { angularError *= (real_t(1.) / denom2) * relaxation; } - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } // solve limit if (m_solveLimit) { @@ -333,8 +348,12 @@ void HingeJoint3DSW::solve(real_t p_step) { impulseMag = m_accLimitImpulse - temp; Vector3 impulse = axisA * impulseMag * m_limitSign; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } } } @@ -355,8 +374,12 @@ void HingeJoint3DSW::solve(real_t p_step) { clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; Vector3 motorImp = clippedMotorImpulse * axisA; - A->apply_torque_impulse(motorImp + angularLimit); - B->apply_torque_impulse(-motorImp - angularLimit); + if (dynamic_A) { + A->apply_torque_impulse(motorImp + angularLimit); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp - angularLimit); + } } } } diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index b6117aa0bc..2100f5de44 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -96,10 +96,10 @@ class HingeJoint3DSW : public Joint3DSW { real_t m_appliedImpulse; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_HINGE; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; real_t get_hinge_angle(); diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index 9f708ce151..8eb84d1c2f 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -50,6 +50,13 @@ subject to the following restrictions: #include "pin_joint_3d_sw.h" bool PinJoint3DSW::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + m_appliedImpulse = real_t(0.); Vector3 normal(0, 0, 0); @@ -119,8 +126,12 @@ void PinJoint3DSW::solve(real_t p_step) { m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } normal[i] = 0; } diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index 1875983527..3d91452850 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -74,10 +74,10 @@ class PinJoint3DSW : public Joint3DSW { Vector3 m_pivotInB; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_PIN; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value); real_t get_param(PhysicsServer3D::PinJointParam p_param) const; diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 0adc471797..8bd1951311 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -127,6 +127,13 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform & //----------------------------------------------------------------------------- bool SliderJoint3DSW::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + //calculate transforms m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformB = B->get_transform() * m_frameInB; @@ -196,8 +203,12 @@ void SliderJoint3DSW::solve(real_t p_step) { // calcutate and apply impulse real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; Vector3 impulse_vector = normal * normalImpulse; - A->apply_impulse(impulse_vector, m_relPosA); - B->apply_impulse(-impulse_vector, m_relPosB); + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } if (m_poweredLinMotor && (!i)) { // apply linear motor if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { real_t desiredMotorVel = m_targetLinMotorVelocity; @@ -217,8 +228,12 @@ void SliderJoint3DSW::solve(real_t p_step) { m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; - A->apply_impulse(impulse_vector, m_relPosA); - B->apply_impulse(-impulse_vector, m_relPosB); + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } } } } @@ -252,8 +267,12 @@ void SliderJoint3DSW::solve(real_t p_step) { angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; } // apply impulse - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } real_t impulseMag; //solve angular limits if (m_solveAngLim) { @@ -264,8 +283,12 @@ void SliderJoint3DSW::solve(real_t p_step) { impulseMag *= m_kAngle * m_softnessDirAng; } Vector3 impulse = axisA * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } //apply angular motor if (m_poweredAngMotor) { if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { @@ -290,8 +313,12 @@ void SliderJoint3DSW::solve(real_t p_step) { m_accumulatedAngMotorImpulse = new_acc; // apply clamped impulse Vector3 motorImp = angImpulse * axisA; - A->apply_torque_impulse(motorImp); - B->apply_torque_impulse(-motorImp); + if (dynamic_A) { + A->apply_torque_impulse(motorImp); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp); + } } } } // SliderJointSW::solveConstraint() diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index f52f6ace27..ef5891d0f9 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -240,10 +240,10 @@ public: void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value); real_t get_param(PhysicsServer3D::SliderJointParam p_param) const; - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_SLIDER; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; } }; #endif // SLIDER_JOINT_SW_H diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/joints_3d_sw.h index 225a71aca9..e2514674ea 100644 --- a/servers/physics_3d/joints_3d_sw.h +++ b/servers/physics_3d/joints_3d_sw.h @@ -35,9 +35,14 @@ #include "constraint_3d_sw.h" class Joint3DSW : public Constraint3DSW { +protected: + bool dynamic_A = false; + bool dynamic_B = false; + public: - virtual bool setup(real_t p_step) { return false; } - virtual void solve(real_t p_step) {} + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return true; } + virtual void solve(real_t p_step) override {} void copy_settings_from(Joint3DSW *p_joint) { set_self(p_joint->get_self()); diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index 3d0063b0fa..c08e2b5794 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -857,10 +857,10 @@ int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } -void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) { +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); - body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method, p_udata); + body->set_force_integration_callback(p_callable, p_udata); } void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index f92652bfad..0b42f1d605 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -241,7 +241,7 @@ 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_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) 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; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index 49ae60db92..69d0fcf3ed 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -249,7 +249,7 @@ public: FUNC2(body_set_omit_force_integration, RID, bool); FUNC1RC(bool, body_is_omitting_force_integration, RID); - FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &); + FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); FUNC2(body_set_ray_pickable, RID, bool); diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index bf0946a0e2..ccd37ca742 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -30,10 +30,28 @@ #include "shape_3d_sw.h" +#include "core/io/image.h" #include "core/math/geometry_3d.h" #include "core/math/quick_hull.h" #include "core/templates/sort_array.h" +// HeightMapShape3DSW is based on Bullet btHeightfieldTerrainShape. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 @@ -891,6 +909,9 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve const Vector3 *vertices = mesh.vertices.ptr(); int vc = mesh.vertices.size(); + r_amount = 0; + ERR_FAIL_COND_MSG(vc == 0, "Convex polygon shape has no vertices."); + //find vertex first real_t max = 0; int vtx = 0; @@ -1614,7 +1635,7 @@ ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { /* HEIGHT MAP SHAPE */ -Vector<real_t> HeightMapShape3DSW::get_heights() const { +Vector<float> HeightMapShape3DSW::get_heights() const { return heights; } @@ -1626,10 +1647,6 @@ int HeightMapShape3DSW::get_depth() const { return depth; } -real_t HeightMapShape3DSW::get_cell_size() const { - return cell_size; -} - void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { //not very useful, but not very used either p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max); @@ -1640,7 +1657,198 @@ Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const { return get_aabb().get_support(p_normal); } +struct _HeightmapSegmentCullParams { + Vector3 from; + Vector3 to; + Vector3 dir; + + Vector3 result; + Vector3 normal; + + const HeightMapShape3DSW *heightmap = nullptr; + FaceShape3DSW *face = nullptr; +}; + +_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) { + Vector3 res; + Vector3 normal; + if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal)) { + p_params.result = res; + p_params.normal = normal; + return true; + } + + return false; +} + +_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, int p_x, int p_z) { + // 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.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; + } + + // 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.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; + } + + 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()) { + return false; + } + + Vector3 local_begin = p_begin + local_origin; + Vector3 local_end = p_end + local_origin; + + 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; + + // 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); + + 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(); + + if (ray_dist_proj < CMP_EPSILON) { + ray_dir_proj = Vector2(); + } else { + ray_dir_proj /= ray_dist_proj; + } + + 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 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; + + 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; + } + } 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; + } + } else { + cross_z = infinite; // Will never cross on Z. + } + + int x = floor(local_begin.x); + int z = floor(local_begin.z); + + // Workaround cases where the ray starts at an integer position. + if (Math::abs(cross_x) < CMP_EPSILON) { + 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::abs(cross_z) < CMP_EPSILON) { + cross_z += delta_z; + if (z_step == -1) { + z -= 1; + } + } + + // 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); + + x = x_start; + z = z_start; + + if (_heightmap_cell_cull_segment(params, x, z)) { + 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 >= width - 1) || (z >= depth - 1)) { + break; + } + + if (_heightmap_cell_cull_segment(params, x, z)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + + if (dist > ray_dist_proj) { + break; + } + } + } + return false; } @@ -1652,7 +1860,66 @@ Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const { return Vector3(); } +void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const { + const AABB &aabb = get_aabb(); + + Vector3 pos_local = aabb.position + local_origin; + + Vector3 clamped_point(p_point); + clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x); + clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y); + clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z); + + r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5); + r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5); + 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 { + if (heights.is_empty()) { + return; + } + + AABB local_aabb = p_local_aabb; + local_aabb.position += local_origin; + + // Quantize the aabb, and adjust the start/end ranges. + int aabb_min[3]; + int aabb_max[3]; + _get_cell(local_aabb.position, aabb_min[0], aabb_min[1], aabb_min[2]); + _get_cell(local_aabb.position + local_aabb.size, aabb_max[0], aabb_max[1], aabb_max[2]); + + // Expand the min/max quantized values. + // This is to catch the case where the input aabb falls between grid points. + for (int i = 0; i < 3; ++i) { + aabb_min[i]--; + aabb_max[i]++; + } + + int start_x = MAX(0, aabb_min[0]); + int end_x = MIN(width - 1, aabb_max[0]); + int start_z = MAX(0, aabb_min[2]); + int end_z = MIN(depth - 1, aabb_max[2]); + + FaceShape3DSW face; + face.backface_collision = true; + + for (int z = start_z; z < end_z; z++) { + for (int x = start_x; x < end_x; x++) { + // First triangle. + _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); + + // 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); + } + } } Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const { @@ -1665,58 +1932,102 @@ 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::_setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) { +void HeightMapShape3DSW::_setup(const Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { heights = p_heights; width = p_width; depth = p_depth; - cell_size = p_cell_size; - - const real_t *r = heights.ptr(); + // Initialize aabb. AABB aabb; + aabb.position = Vector3(0.0, p_min_height, 0.0); + aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); - for (int i = 0; i < depth; i++) { - for (int j = 0; j < width; j++) { - real_t h = r[i * width + j]; + // Initialize origin as the aabb center. + local_origin = aabb.position + 0.5 * aabb.size; + local_origin.y = 0.0; - Vector3 pos(j * cell_size, h, i * cell_size); - if (i == 0 || j == 0) { - aabb.position = pos; - } else { - aabb.expand_to(pos); - } - } - } + aabb.position -= local_origin; configure(aabb); } void HeightMapShape3DSW::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + Dictionary d = p_data; ERR_FAIL_COND(!d.has("width")); ERR_FAIL_COND(!d.has("depth")); - ERR_FAIL_COND(!d.has("cell_size")); ERR_FAIL_COND(!d.has("heights")); int width = d["width"]; int depth = d["depth"]; - real_t cell_size = d["cell_size"]; - Vector<real_t> heights = d["heights"]; - ERR_FAIL_COND(width <= 0); - ERR_FAIL_COND(depth <= 0); - ERR_FAIL_COND(cell_size <= CMP_EPSILON); - ERR_FAIL_COND(heights.size() != (width * depth)); - _setup(heights, width, depth, cell_size); + ERR_FAIL_COND(width <= 0.0); + ERR_FAIL_COND(depth <= 0.0); + + Variant heights_variant = d["heights"]; + Vector<float> heights_buffer; + if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) { + // Ready-to-use heights can be passed. + heights_buffer = heights_variant; + } else if (heights_variant.get_type() == Variant::OBJECT) { + // If an image is passed, we have to convert it. + // This would be expensive to do with a script, so it's nice to have it here. + Ref<Image> image = heights_variant; + ERR_FAIL_COND(image.is_null()); + ERR_FAIL_COND(image->get_format() != Image::FORMAT_RF); + + PackedByteArray im_data = image->get_data(); + heights_buffer.resize(image->get_width() * image->get_height()); + + float *w = heights_buffer.ptrw(); + float *rp = (float *)im_data.ptr(); + for (int i = 0; i < heights_buffer.size(); ++i) { + w[i] = rp[i]; + } + } else { + ERR_FAIL_MSG("Expected PackedFloat32Array or float Image."); + } + + // Compute min and max heights or use precomputed values. + real_t min_height = 0.0; + real_t max_height = 0.0; + if (d.has("min_height") && d.has("max_height")) { + min_height = d["min_height"]; + max_height = d["max_height"]; + } else { + int heights_size = heights.size(); + for (int i = 0; i < heights_size; ++i) { + float h = heights[i]; + if (h < min_height) { + min_height = h; + } else if (h > max_height) { + max_height = h; + } + } + } + + ERR_FAIL_COND(min_height > max_height); + + ERR_FAIL_COND(heights_buffer.size() != (width * depth)); + + // If specified, min and max height will be used as precomputed values. + _setup(heights_buffer, width, depth, min_height, max_height); } Variant HeightMapShape3DSW::get_data() const { - ERR_FAIL_V(Variant()); + Dictionary d; + d["width"] = width; + d["depth"] = depth; + + const AABB &aabb = get_aabb(); + d["min_height"] = aabb.position.y; + d["max_height"] = aabb.position.y + aabb.size.y; + + d["heights"] = heights; + + return d; } HeightMapShape3DSW::HeightMapShape3DSW() { - width = 0; - depth = 0; - cell_size = 0; } diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index 988e76c699..4d2b6ffbed 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -81,7 +81,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const = 0; - _FORCE_INLINE_ AABB get_aabb() const { return aabb; } + _FORCE_INLINE_ const AABB &get_aabb() const { return aabb; } _FORCE_INLINE_ bool is_configured() const { return configured; } virtual bool is_concave() const { return false; } @@ -389,21 +389,29 @@ public: }; struct HeightMapShape3DSW : public ConcaveShape3DSW { - Vector<real_t> heights; - int width; - int depth; - real_t cell_size; + Vector<float> heights; + int width = 0; + int depth = 0; + Vector3 local_origin; - //void _cull_segment(int p_idx,_SegmentCullParams *p_params) const; - //void _cull(int p_idx,_CullParams *p_params) const; + _FORCE_INLINE_ float _get_height(int p_x, int p_z) const { + return heights[(p_z * width) + p_x]; + } + + _FORCE_INLINE_ void _get_point(int p_x, int p_z, Vector3 &r_point) const { + r_point.x = p_x - 0.5 * (width - 1.0); + r_point.y = _get_height(p_x, p_z); + r_point.z = p_z - 0.5 * (depth - 1.0); + } + + void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const; - void _setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size); + void _setup(const Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); public: - Vector<real_t> get_heights() const; + Vector<float> get_heights() const; int get_width() const; int get_depth() const; - real_t get_cell_size() const; virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_HEIGHTMAP; } diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h index 6c0451ff45..98e554218b 100644 --- a/servers/physics_3d/soft_body_3d_sw.h +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -106,6 +106,8 @@ class SoftBody3DSW : public CollisionObject3DSW { VSet<RID> exceptions; + uint64_t island_step = 0; + public: SoftBody3DSW(); @@ -124,6 +126,9 @@ public: _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; } + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + virtual void set_space(Space3DSW *p_space); void set_mesh(const Ref<Mesh> &p_mesh); diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index 9a73e11562..ba18bac611 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -33,109 +33,150 @@ #include "core/os/os.h" -void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island) { +#define BODY_ISLAND_COUNT_RESERVE 128 +#define BODY_ISLAND_SIZE_RESERVE 512 +#define ISLAND_COUNT_RESERVE 128 +#define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 + +void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) { p_body->set_island_step(_step); - p_body->set_island_next(*p_island); - *p_island = p_body; + + if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { + // Only dynamic bodies are tested for activation. + p_body_island.push_back(p_body); + } for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - Constraint3DSW *c = (Constraint3DSW *)E->key(); - if (c->get_island_step() == _step) { - continue; //already processed + Constraint3DSW *constraint = (Constraint3DSW *)E->key(); + if (constraint->get_island_step() == _step) { + continue; // Already processed. } - c->set_island_step(_step); - c->set_island_next(*p_constraint_island); - *p_constraint_island = c; + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); - for (int i = 0; i < c->get_body_count(); i++) { + all_constraints.push_back(constraint); + + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { if (i == E->get()) { continue; } - Body3DSW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - continue; //no go + Body3DSW *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. } - _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); + _populate_island(other_body, p_body_island, p_constraint_island); + } + + // Find connected soft bodies. + for (int i = 0; i < constraint->get_soft_body_count(); i++) { + SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i); + if (soft_body->get_island_step() == _step) { + continue; // Already processed. + } + _populate_island_soft_body(soft_body, p_body_island, p_constraint_island); } } } -void Step3DSW::_setup_island(Constraint3DSW *p_island, real_t p_delta) { - Constraint3DSW *ci = p_island; - while (ci) { - ci->setup(p_delta); - //todo remove from island if process fails - ci = ci->get_island_next(); +void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) { + p_soft_body->set_island_step(_step); + + for (Set<Constraint3DSW *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) { + Constraint3DSW *constraint = (Constraint3DSW *)E->get(); + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + + all_constraints.push_back(constraint); + + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { + Body3DSW *body = constraint->get_body_ptr()[i]; + if (body->get_island_step() == _step) { + continue; // Already processed. + } + if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(body, p_body_island, p_constraint_island); + } + } +} + +void Step3DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { + Constraint3DSW *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const { + uint32_t constraint_count = p_constraint_island.size(); + uint32_t valid_constraint_count = 0; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + Constraint3DSW *constraint = p_constraint_island[constraint_index]; + if (p_constraint_island[constraint_index]->pre_solve(delta)) { + // Keep this constraint for solving. + p_constraint_island[valid_constraint_count++] = constraint; + } } + p_constraint_island.resize(valid_constraint_count); } -void Step3DSW::_solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta) { - int at_priority = 1; +void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) { + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index]; - while (p_island) { - for (int i = 0; i < p_iterations; i++) { - Constraint3DSW *ci = p_island; - while (ci) { - ci->solve(p_delta); - ci = ci->get_island_next(); + int current_priority = 1; + + uint32_t constraint_count = constraint_island.size(); + while (constraint_count > 0) { + for (int i = 0; i < iterations; i++) { + // Go through all iterations. + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + constraint_island[constraint_index]->solve(delta); } } - at_priority++; - - { - Constraint3DSW *ci = p_island; - Constraint3DSW *prev = nullptr; - while (ci) { - if (ci->get_priority() < at_priority) { - if (prev) { - prev->set_island_next(ci->get_island_next()); //remove - } else { - p_island = ci->get_island_next(); - } - } else { - prev = ci; - } - - ci = ci->get_island_next(); + // Check priority to keep only higher priority constraints. + uint32_t priority_constraint_count = 0; + ++current_priority; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + Constraint3DSW *constraint = constraint_island[constraint_index]; + if (constraint->get_priority() >= current_priority) { + // Keep this constraint for the next iteration. + constraint_island[priority_constraint_count++] = constraint; } } + constraint_count = priority_constraint_count; } } -void Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) { +void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const { bool can_sleep = true; - Body3DSW *b = p_island; - while (b) { - if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } + uint32_t body_count = p_body_island.size(); + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + Body3DSW *body = p_body_island[body_index]; - if (!b->sleep_test(p_delta)) { + if (!body->sleep_test(delta)) { can_sleep = false; } - - b = b->get_island_next(); } - //put all to sleep or wake up everyoen - - b = p_island; - while (b) { - if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } + // Put all to sleep or wake up everyone. + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + Body3DSW *body = p_body_island[body_index]; - bool active = b->is_active(); + bool active = body->is_active(); if (active == can_sleep) { - b->set_active(!can_sleep); + body->set_active(!can_sleep); } - - b = b->get_island_next(); } } @@ -144,6 +185,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + iterations = p_iterations; + delta = p_delta; + const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list(); const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list(); @@ -179,81 +223,121 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - /* GENERATE CONSTRAINT ISLANDS */ + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + Constraint3DSW *constraint = E->get(); + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ - Body3DSW *island_list = nullptr; - Constraint3DSW *constraint_island_list = nullptr; b = body_list->first(); - int island_count = 0; + uint32_t body_island_count = 0; while (b) { Body3DSW *body = b->self(); if (body->get_island_step() != _step) { - Body3DSW *island = nullptr; - Constraint3DSW *constraint_island = nullptr; - _populate_island(body, &island, &constraint_island); - - island->set_island_list_next(island_list); - island_list = island; + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); - if (constraint_island) { - constraint_island->set_island_list_next(constraint_island_list); - constraint_island_list = constraint_island; - island_count++; + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); } - } - b = b->next(); - } + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); - p_space->set_island_count(island_count); + _populate_island(body, body_island, constraint_island); - const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list(); + if (body_island.is_empty()) { + --body_island_count; + } - while (aml.first()) { - for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint3DSW *c = E->get(); - if (c->get_island_step() == _step) { - continue; + if (constraint_island.is_empty()) { + --island_count; } - c->set_island_step(_step); - c->set_island_next(nullptr); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; } - p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here + b = b->next(); } + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */ + sb = soft_body_list->first(); while (sb) { - for (const Set<Constraint3DSW *>::Element *E = sb->self()->get_constraints().front(); E; E = E->next()) { - Constraint3DSW *c = E->get(); - if (c->get_island_step() == _step) - continue; - c->set_island_step(_step); - c->set_island_next(NULL); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; + SoftBody3DSW *soft_body = sb->self(); + + if (soft_body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); + + _populate_island_soft_body(soft_body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; + } + + if (constraint_island.is_empty()) { + --island_count; + } } sb = sb->next(); } + p_space->set_island_count((int)island_count); + { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); profile_begtime = profile_endtime; } - /* SETUP CONSTRAINT ISLANDS */ + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ - { - Constraint3DSW *ci = constraint_island_list; - while (ci) { - _setup_island(ci, p_delta); - ci = ci->get_island_list_next(); - } - } + uint32_t total_contraint_count = all_constraints.size(); + work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -261,15 +345,21 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } + /* PRE-SOLVE CONSTRAINT ISLANDS */ + + // Warning: This doesn't run on threads, because it involves thread-unsafe processing. + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _pre_solve_island(constraint_islands[island_index]); + } + /* SOLVE CONSTRAINT ISLANDS */ - { - Constraint3DSW *ci = constraint_island_list; - while (ci) { - //iterating each island separatedly improves cache efficiency - _solve_island(ci, p_iterations, p_delta); - ci = ci->get_island_list_next(); - } + // Warning: _solve_island modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + if (island_count > 1) { + work_pool.do_work(island_count, this, &Step3DSW::_solve_island, nullptr); + } else if (island_count > 0) { + _solve_island(0); } { //profile @@ -289,12 +379,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { /* SLEEP / WAKE UP ISLANDS */ - { - Body3DSW *bi = island_list; - while (bi) { - _check_suspend(bi, p_delta); - bi = bi->get_island_list_next(); - } + for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { + _check_suspend(body_islands[island_index]); } /* UPDATE SOFT BODY CONSTRAINTS */ @@ -311,6 +397,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } + all_constraints.clear(); + p_space->update(); p_space->unlock(); _step++; @@ -318,4 +406,14 @@ 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); + + work_pool.init(); +} + +Step3DSW::~Step3DSW() { + work_pool.finish(); } diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h index 55c48ec0eb..9c60004b24 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -33,17 +33,32 @@ #include "space_3d_sw.h" +#include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" + class Step3DSW { uint64_t _step; - void _populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island); - void _setup_island(Constraint3DSW *p_island, real_t p_delta); - void _solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta); - void _check_suspend(Body3DSW *p_island, real_t p_delta); + int iterations = 0; + real_t delta = 0.0; + + ThreadWorkPool work_pool; + + LocalVector<LocalVector<Body3DSW *>> body_islands; + LocalVector<LocalVector<Constraint3DSW *>> constraint_islands; + LocalVector<Constraint3DSW *> all_constraints; + + void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island); + void _populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr); + void _check_suspend(const LocalVector<Body3DSW *> &p_body_island) const; public: void step(Space3DSW *p_space, real_t p_delta, int p_iterations); Step3DSW(); + ~Step3DSW(); }; #endif // STEP__SW_H |