diff options
Diffstat (limited to 'servers')
31 files changed, 892 insertions, 464 deletions
diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index 21ad57e344..eb9fc0e800 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -40,31 +40,48 @@ bool AreaPair2DSW::setup(real_t p_step) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area->get_space_override_mode() != PhysicsServer2D::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() != PhysicsServer2D::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() != PhysicsServer2D::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 AreaPair2DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer2D::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() != PhysicsServer2D::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 AreaPair2DSW::solve(real_t p_step) { + // Nothing to do. } AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { @@ -72,7 +89,6 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *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() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair @@ -103,33 +119,48 @@ bool Area2Pair2DSW::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 Area2Pair2DSW::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 Area2Pair2DSW::solve(real_t p_step) { + // Nothing to do. } Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { @@ -137,7 +168,6 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *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_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h index 4015aad5d1..4632a307d9 100644 --- a/servers/physics_2d/area_pair_2d_sw.h +++ b/servers/physics_2d/area_pair_2d_sw.h @@ -36,30 +36,34 @@ #include "constraint_2d_sw.h" class AreaPair2DSW : public Constraint2DSW { - Body2DSW *body; - Area2DSW *area; - int body_shape; - int area_shape; - bool colliding; + Body2DSW *body = nullptr; + Area2DSW *area = nullptr; + int body_shape = 0; + int area_shape = 0; + 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; AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape); ~AreaPair2DSW(); }; class Area2Pair2DSW : public Constraint2DSW { - Area2DSW *area_a; - Area2DSW *area_b; - int shape_a; - int shape_b; - bool colliding; + Area2DSW *area_a = nullptr; + Area2DSW *area_b = nullptr; + int shape_a = 0; + int shape_b = 0; + 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; Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b); ~Area2Pair2DSW(); diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index 9306fea70c..f78a487e27 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -104,31 +104,17 @@ void Body2DSW::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 == PhysicsServer2D::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; - } - /* - 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); - } + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); } -*/ } void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) { @@ -370,13 +356,6 @@ void Body2DSW::set_space(Space2DSW *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 = false; diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index da70ac7d4b..ff31825b83 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -87,10 +87,13 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto int least_deep = -1; real_t min_depth = 1e10; + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + for (int i = 0; i <= contact_count; i++) { Contact &c = (i == contact_count) ? contact : contacts[i]; - Vector2 global_A = A->get_transform().basis_xform(c.local_A); - Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; Vector2 axis = global_A - global_B; real_t depth = axis.dot(c.normal); @@ -124,6 +127,9 @@ void BodyPair2DSW::_validate_contacts() { real_t max_separation = space->get_contact_max_separation(); real_t max_separation2 = max_separation * max_separation; + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; @@ -134,8 +140,8 @@ void BodyPair2DSW::_validate_contacts() { } else { c.reused = false; - Vector2 global_A = A->get_transform().basis_xform(c.local_A); - Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; Vector2 axis = global_A - global_B; real_t depth = axis.dot(c.normal); @@ -220,14 +226,16 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) { } bool BodyPair2DSW::setup(real_t p_step) { - //cannot collide + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::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() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::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 { @@ -246,12 +254,12 @@ bool BodyPair2DSW::setup(real_t p_step) { _validate_contacts(); - Vector2 offset_A = A->get_transform().get_origin(); + const Vector2 &offset_A = A->get_transform().get_origin(); Transform2D xform_Au = A->get_transform().untranslated(); Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); Transform2D xform_Bu = B->get_transform(); - xform_Bu.elements[2] -= A->get_transform().get_origin(); + xform_Bu.elements[2] -= offset_A; Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); Shape2DSW *shape_A_ptr = A->get_shape(shape_A); @@ -272,13 +280,13 @@ bool BodyPair2DSW::setup(real_t p_step) { if (!collided) { //test ccd (currently just a raycast) - if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_A) { if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { collided = true; } } - if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_B) { if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { collided = true; } @@ -338,9 +346,21 @@ bool BodyPair2DSW::setup(real_t p_step) { } } + return true; +} + +bool BodyPair2DSW::pre_solve(real_t p_step) { + if (!collided || oneway_disabled) { + return false; + } + real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = 0.3; + + Shape2DSW *shape_A_ptr = A->get_shape(shape_A); + Shape2DSW *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(); @@ -351,21 +371,23 @@ bool BodyPair2DSW::setup(real_t p_step) { } } - cc = 0; - real_t inv_dt = 1.0 / p_step; bool do_process = false; + const Vector2 &offset_A = A->get_transform().get_origin(); + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - c.active = false; - Vector2 global_A = xform_Au.xform(c.local_A); - Vector2 global_B = xform_Bu.xform(c.local_B); + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; - real_t depth = c.normal.dot(global_A - global_B); + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); if (depth <= 0 || !c.reused) { continue; @@ -396,8 +418,6 @@ bool BodyPair2DSW::setup(real_t p_step) { continue; } - c.active = true; - // Precompute normal mass, tangent mass, and bias. real_t rnA = c.rA.dot(c.normal); real_t rnB = c.rB.dot(c.normal); @@ -421,8 +441,12 @@ bool BodyPair2DSW::setup(real_t p_step) { // Apply normal + friction impulse Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; - A->apply_impulse(-P, c.rA); - B->apply_impulse(P, c.rB); + if (dynamic_A) { + A->apply_impulse(-P, c.rA); + } + if (dynamic_B) { + B->apply_impulse(P, c.rB); + } } #endif @@ -434,6 +458,7 @@ bool BodyPair2DSW::setup(real_t p_step) { c.bounce = c.bounce * dv.dot(c.normal); } + c.active = true; do_process = true; } @@ -441,13 +466,12 @@ bool BodyPair2DSW::setup(real_t p_step) { } void BodyPair2DSW::solve(real_t p_step) { - if (!collided) { + if (!collided || oneway_disabled) { return; } for (int i = 0; i < contact_count; ++i) { Contact &c = contacts[i]; - cc++; if (!c.active) { continue; @@ -474,8 +498,12 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(-jb, c.rA); - B->apply_bias_impulse(jb, c.rB); + if (dynamic_A) { + A->apply_bias_impulse(-jb, c.rA); + } + if (dynamic_B) { + B->apply_bias_impulse(jb, c.rB); + } real_t jn = -(c.bounce + vn) * c.mass_normal; real_t jnOld = c.acc_normal_impulse; @@ -490,8 +518,12 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); - A->apply_impulse(-j, c.rA); - B->apply_impulse(j, c.rB); + if (dynamic_A) { + A->apply_impulse(-j, c.rA); + } + if (dynamic_B) { + B->apply_impulse(j, c.rB); + } } } @@ -504,9 +536,6 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh space = A->get_space(); A->add_constraint(this, 0); B->add_constraint(this, 1); - contact_count = 0; - collided = false; - oneway_disabled = false; } BodyPair2DSW::~BodyPair2DSW() { diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h index 31ab9b9017..4b42b44c92 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -44,13 +44,16 @@ class BodyPair2DSW : public Constraint2DSW { Body2DSW *B; }; - Body2DSW *_arr[2]; + Body2DSW *_arr[2] = { nullptr, nullptr }; }; - int shape_A; - int shape_B; + int shape_A = 0; + int shape_B = 0; - Space2DSW *space; + bool dynamic_A = false; + bool dynamic_B = false; + + Space2DSW *space = nullptr; struct Contact { Vector2 position; @@ -73,10 +76,10 @@ class BodyPair2DSW : public Constraint2DSW { Vector2 sep_axis; Contact contacts[MAX_CONTACTS]; - int contact_count; - bool collided; - bool oneway_disabled; - int cc; + int contact_count = 0; + bool collided = false; + bool oneway_disabled = false; + bool report_contacts_only = false; bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); void _validate_contacts(); @@ -84,8 +87,9 @@ class BodyPair2DSW : public Constraint2DSW { _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_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; BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B); ~BodyPair2DSW(); diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 2db3961f41..c395e59694 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -143,8 +143,8 @@ public: return shapes[p_index].metadata; } - _FORCE_INLINE_ Transform2D get_transform() const { return transform; } - _FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; } _FORCE_INLINE_ Space2DSW *get_space() const { return space; } void set_shape_as_disabled(int p_idx, bool p_disabled); diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index b724deb48e..5e8dfbe570 100644 --- a/servers/physics_2d/constraint_2d_sw.h +++ b/servers/physics_2d/constraint_2d_sw.h @@ -63,6 +63,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 ~Constraint2DSW() {} diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index 20d4b9aa1a..eaec582f9b 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -97,7 +97,10 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto } bool PinJoint2DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } @@ -148,12 +151,6 @@ bool PinJoint2DSW::setup(real_t p_step) { bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); - // apply accumulated impulse - A->apply_impulse(-P, rA); - if (B) { - B->apply_impulse(P, rB); - } - return true; } @@ -161,6 +158,18 @@ inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) { return Vector2(p_other * p_vec.y, -p_other * p_vec.x); } +bool PinJoint2DSW::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-P, rA); + } + if (B && dynamic_B) { + B->apply_impulse(P, rB); + } + + return true; +} + void PinJoint2DSW::solve(real_t p_step) { // compute relative velocity Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity()); @@ -174,8 +183,10 @@ void PinJoint2DSW::solve(real_t p_step) { Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); - A->apply_impulse(-impulse, rA); - if (B) { + if (dynamic_A) { + A->apply_impulse(-impulse, rA); + } + if (B && dynamic_B) { B->apply_impulse(impulse, rB); } @@ -262,14 +273,19 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { } bool GrooveJoint2DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } + Space2DSW *space = A->get_space(); + ERR_FAIL_COND_V(!space, false); + // calculate endpoints in worldspace Vector2 ta = A->get_transform().xform(A_groove_1); Vector2 tb = A->get_transform().xform(A_groove_2); - Space2DSW *space = A->get_space(); // calculate axis Vector2 n = -(tb - ta).orthogonal().normalized(); @@ -308,14 +324,22 @@ bool GrooveJoint2DSW::setup(real_t p_step) { real_t _b = get_bias(); gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).clamped(get_max_bias()); - // apply accumulated impulse - A->apply_impulse(-jn_acc, rA); - B->apply_impulse(jn_acc, rB); - correct = true; return true; } +bool GrooveJoint2DSW::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-jn_acc, rA); + } + if (dynamic_B) { + B->apply_impulse(jn_acc, rB); + } + + return true; +} + void GrooveJoint2DSW::solve(real_t p_step) { // compute impulse Vector2 vr = relative_velocity(A, B, rA, rB); @@ -328,8 +352,12 @@ void GrooveJoint2DSW::solve(real_t p_step) { j = jn_acc - jOld; - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } } GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) : @@ -351,7 +379,10 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_ ////////////////////////////////////////////// bool DampedSpringJoint2DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } @@ -373,12 +404,21 @@ bool DampedSpringJoint2DSW::setup(real_t p_step) { target_vrn = 0.0f; v_coef = 1.0f - Math::exp(-damping * (p_step)*k); - // apply spring force + // Calculate spring force. real_t f_spring = (rest_length - dist) * stiffness; - Vector2 j = n * f_spring * (p_step); + j = n * f_spring * (p_step); - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); + return true; +} + +bool DampedSpringJoint2DSW::pre_solve(real_t p_step) { + // Apply spring force. + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } return true; } @@ -393,8 +433,12 @@ void DampedSpringJoint2DSW::solve(real_t p_step) { target_vrn = vrn + v_damp; Vector2 j = n * v_damp * n_mass; - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } } void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h index 628de972ae..ccc5c585a0 100644 --- a/servers/physics_2d/joints_2d_sw.h +++ b/servers/physics_2d/joints_2d_sw.h @@ -39,6 +39,10 @@ class Joint2DSW : public Constraint2DSW { real_t bias; real_t max_bias; +protected: + bool dynamic_A = false; + bool dynamic_B = false; + public: _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } _FORCE_INLINE_ real_t get_max_force() const { return max_force; } @@ -49,8 +53,9 @@ public: _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } - 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 false; } + virtual void solve(real_t p_step) override {} void copy_settings_from(Joint2DSW *p_joint); @@ -90,10 +95,11 @@ class PinJoint2DSW : public Joint2DSW { real_t softness; public: - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_PIN; } + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::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 bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); real_t get_param(PhysicsServer2D::PinJointParam p_param) const; @@ -126,10 +132,11 @@ class GrooveJoint2DSW : public Joint2DSW { bool correct; public: - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_GROOVE; } + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } - virtual bool setup(real_t p_step); - virtual 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; GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b); }; @@ -153,15 +160,17 @@ class DampedSpringJoint2DSW : public Joint2DSW { Vector2 rA, rB; Vector2 n; + Vector2 j; real_t n_mass; real_t target_vrn; real_t v_coef; public: - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } - virtual bool setup(real_t p_step); - virtual 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; void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 406d750776..b8cb4cddc5 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -29,45 +29,59 @@ /*************************************************************************/ #include "step_2d_sw.h" + #include "core/os/os.h" #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 Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island) { p_body->set_island_step(_step); - p_body_island.push_back(p_body); - // Faster with reversed iterations. - for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().back(); E; E = E->prev()) { - Constraint2DSW *c = (Constraint2DSW *)E->get().first; - if (c->get_island_step() == _step) { - continue; //already processed + if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + // Only dynamic bodies are tested for activation. + p_body_island.push_back(p_body); + } + + for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().front(); E; E = E->next()) { + Constraint2DSW *constraint = (Constraint2DSW *)E->get().first; + if (constraint->get_island_step() == _step) { + continue; // Already processed. } - c->set_island_step(_step); - p_constraint_island.push_back(c); + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + all_constraints.push_back(constraint); - for (int i = 0; i < c->get_body_count(); i++) { + for (int i = 0; i < constraint->get_body_count(); i++) { if (i == E->get().second) { continue; } - Body2DSW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { - continue; //no go + Body2DSW *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. } - _populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island); + _populate_island(other_body, p_body_island, p_constraint_island); } } } -void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta) { +void Step2DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { + Constraint2DSW *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void Step2DSW::_pre_solve_island(LocalVector<Constraint2DSW *> &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) { Constraint2DSW *constraint = p_constraint_island[constraint_index]; - if (p_constraint_island[constraint_index]->setup(p_delta)) { + if (p_constraint_island[constraint_index]->pre_solve(delta)) { // Keep this constraint for solving. p_constraint_island[valid_constraint_count++] = constraint; } @@ -75,27 +89,25 @@ void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, p_constraint_island.resize(valid_constraint_count); } -void Step2DSW::_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta) { - for (int i = 0; i < p_iterations; i++) { - uint32_t constraint_count = p_constraint_island.size(); +void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const { + const LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[p_island_index]; + + for (int i = 0; i < iterations; i++) { + uint32_t constraint_count = constraint_island.size(); for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - p_constraint_island[constraint_index]->solve(p_delta); + constraint_island[constraint_index]->solve(delta); } } } -void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta) { +void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const { bool can_sleep = true; uint32_t body_count = p_body_island.size(); for (uint32_t body_index = 0; body_index < body_count; ++body_index) { Body2DSW *body = p_body_island[body_index]; - if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { - continue; // Ignore for static. - } - - if (!body->sleep_test(p_delta)) { + if (!body->sleep_test(delta)) { can_sleep = false; } } @@ -104,10 +116,6 @@ void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real for (uint32_t body_index = 0; body_index < body_count; ++body_index) { Body2DSW *body = p_body_island[body_index]; - if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { - continue; // Ignore for static. - } - bool active = body->is_active(); if (active == can_sleep) { @@ -121,6 +129,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + iterations = p_iterations; + delta = p_delta; + const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list(); /* INTEGRATE FORCES */ @@ -145,12 +156,39 @@ void Step2DSW::step(Space2DSW *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<Area2DSW>::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + Constraint2DSW *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<Constraint2DSW *> &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<Area2DSW> *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ b = body_list->first(); uint32_t body_island_count = 0; - uint32_t island_count = 0; while (b) { Body2DSW *body = b->self(); @@ -174,7 +212,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { _populate_island(body, body_island, constraint_island); - body_islands.push_back(body_island); + if (body_island.is_empty()) { + --body_island_count; + } if (constraint_island.is_empty()) { --island_count; @@ -185,37 +225,16 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { p_space->set_island_count((int)island_count); - const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list(); - - while (aml.first()) { - for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint2DSW *c = E->get(); - if (c->get_island_step() == _step) { - continue; - } - c->set_island_step(_step); - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); - } - LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - constraint_island.push_back(c); - } - p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here - } - { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); profile_begtime = profile_endtime; } - /* SETUP CONSTRAINT ISLANDS */ + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ - for (uint32_t island_index = 0; island_index < island_count; ++island_index) { - _setup_island(constraint_islands[island_index], p_delta); - } + uint32_t total_contraint_count = all_constraints.size(); + work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -223,10 +242,21 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - /* SOLVE CONSTRAINT ISLANDS */ + /* 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) { - _solve_island(constraint_islands[island_index], p_iterations, p_delta); + _pre_solve_island(constraint_islands[island_index]); + } + + /* SOLVE CONSTRAINT ISLANDS */ + + // 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, &Step2DSW::_solve_island, nullptr); + } else if (island_count > 0) { + _solve_island(0); } { //profile @@ -247,7 +277,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { /* SLEEP / WAKE UP ISLANDS */ for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { - _check_suspend(body_islands[island_index], p_delta); + _check_suspend(body_islands[island_index]); } { //profile @@ -256,6 +286,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { //profile_begtime=profile_endtime; } + all_constraints.clear(); + p_space->update(); p_space->unlock(); _step++; @@ -266,4 +298,11 @@ Step2DSW::Step2DSW() { body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); + + work_pool.init(); +} + +Step2DSW::~Step2DSW() { + work_pool.finish(); } diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h index 5af4a36f52..c51fd73a79 100644 --- a/servers/physics_2d/step_2d_sw.h +++ b/servers/physics_2d/step_2d_sw.h @@ -34,21 +34,30 @@ #include "space_2d_sw.h" #include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" class Step2DSW { uint64_t _step; + int iterations = 0; + real_t delta = 0.0; + + ThreadWorkPool work_pool; + LocalVector<LocalVector<Body2DSW *>> body_islands; LocalVector<LocalVector<Constraint2DSW *>> constraint_islands; + LocalVector<Constraint2DSW *> all_constraints; void _populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island); - void _setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta); - void _solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta); - void _check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; + void _check_suspend(LocalVector<Body2DSW *> &p_body_island) const; public: void step(Space2DSW *p_space, real_t p_delta, int p_iterations); Step2DSW(); + ~Step2DSW(); }; #endif // STEP_2D_SW_H 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 d54345821d..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; diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 28c854466f..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(); @@ -283,22 +295,26 @@ bool BodyPair3DSW::setup(real_t 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); } @@ -338,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; @@ -361,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) { @@ -384,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); @@ -400,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; @@ -421,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; } @@ -464,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; } @@ -481,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() { @@ -564,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; @@ -585,12 +623,22 @@ 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(); } @@ -599,6 +647,8 @@ bool BodySoftBodyPair3DSW::setup(real_t 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]; @@ -609,10 +659,10 @@ 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; @@ -629,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()) { @@ -637,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); } @@ -651,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(-j_vec, c.rA + body->get_center_of_mass()); + 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; @@ -675,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]; @@ -697,7 +751,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), 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); @@ -712,7 +768,9 @@ 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(-jb_com, body->get_center_of_mass(), 0.0f); + 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); } @@ -732,7 +790,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - body->apply_impulse(-j, c.rA + body->get_center_of_mass()); + 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; @@ -773,7 +833,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); + 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; @@ -781,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 16a31e167d..7b44726ef5 100644 --- a/servers/physics_3d/constraint_3d_sw.h +++ b/servers/physics_3d/constraint_3d_sw.h @@ -31,7 +31,8 @@ #ifndef CONSTRAINT_SW_H #define CONSTRAINT_SW_H -#include "body_3d_sw.h" +class Body3DSW; +class SoftBody3DSW; class Constraint3DSW { Body3DSW **_body_ptr; @@ -61,6 +62,9 @@ public: _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; } @@ -68,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 167f797bfe..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,7 +109,10 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans } bool ConeTwistJoint3DSW::setup(real_t p_timestep) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + 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; } @@ -265,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); + } } } @@ -287,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 @@ -303,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 a86e8b4e76..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,7 +310,10 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) { } bool Generic6DOFJoint3DSW::setup(real_t p_timestep) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + 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; } @@ -384,6 +394,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) { jacDiagABInv, A, pointInA, B, pointInB, + dynamic_A, dynamic_B, i, linear_axis, m_AnchorPos); } } @@ -398,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 90b82f4680..bb8858c28a 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -155,7 +155,10 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo } bool HingeJoint3DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + 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; } @@ -279,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); + } } } @@ -322,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) { @@ -337,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); + } } } @@ -359,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 75d87992d1..8eb84d1c2f 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -50,7 +50,10 @@ subject to the following restrictions: #include "pin_joint_3d_sw.h" bool PinJoint3DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + 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; } @@ -123,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 2e1ee8e770..8bd1951311 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -127,7 +127,10 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform & //----------------------------------------------------------------------------- bool SliderJoint3DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + 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; } @@ -200,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; @@ -221,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); + } } } } @@ -256,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) { @@ -268,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) { @@ -294,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/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 06f3227eab..ba18bac611 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -37,39 +37,90 @@ #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_island.push_back(p_body); - // Faster with reversed iterations. - for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().back(); E; E = E->prev()) { - Constraint3DSW *c = (Constraint3DSW *)E->key(); - if (c->get_island_step() == _step) { - continue; //already processed + 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 *constraint = (Constraint3DSW *)E->key(); + if (constraint->get_island_step() == _step) { + continue; // Already processed. } - c->set_island_step(_step); - p_constraint_island.push_back(c); + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + + all_constraints.push_back(constraint); - for (int i = 0; i < c->get_body_count(); i++) { + // 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(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(c->get_body_ptr()[i], p_body_island, p_constraint_island); + _populate_island_soft_body(soft_body, p_body_island, p_constraint_island); } } } -void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta) { +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]->setup(p_delta)) { + if (p_constraint_island[constraint_index]->pre_solve(delta)) { // Keep this constraint for solving. p_constraint_island[valid_constraint_count++] = constraint; } @@ -77,15 +128,17 @@ void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, p_constraint_island.resize(valid_constraint_count); } -void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta) { +void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) { + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index]; + int current_priority = 1; - uint32_t constraint_count = p_constraint_island.size(); + uint32_t constraint_count = constraint_island.size(); while (constraint_count > 0) { - for (int i = 0; i < p_iterations; i++) { + for (int i = 0; i < iterations; i++) { // Go through all iterations. for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - p_constraint_island[constraint_index]->solve(p_delta); + constraint_island[constraint_index]->solve(delta); } } @@ -93,28 +146,24 @@ void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, uint32_t priority_constraint_count = 0; ++current_priority; for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - Constraint3DSW *constraint = p_constraint_island[constraint_index]; + Constraint3DSW *constraint = constraint_island[constraint_index]; if (constraint->get_priority() >= current_priority) { // Keep this constraint for the next iteration. - p_constraint_island[priority_constraint_count++] = constraint; + constraint_island[priority_constraint_count++] = constraint; } } constraint_count = priority_constraint_count; } } -void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta) { +void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const { bool can_sleep = true; 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 (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - continue; // Ignore for static. - } - - if (!body->sleep_test(p_delta)) { + if (!body->sleep_test(delta)) { can_sleep = false; } } @@ -123,10 +172,6 @@ void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real for (uint32_t body_index = 0; body_index < body_count; ++body_index) { Body3DSW *body = p_body_island[body_index]; - if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - continue; // Ignore for static. - } - bool active = body->is_active(); if (active == can_sleep) { @@ -140,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(); @@ -175,12 +223,39 @@ 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 */ b = body_list->first(); uint32_t body_island_count = 0; - uint32_t island_count = 0; while (b) { Body3DSW *body = b->self(); @@ -204,7 +279,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { _populate_island(body, body_island, constraint_island); - body_islands.push_back(body_island); + if (body_island.is_empty()) { + --body_island_count; + } if (constraint_island.is_empty()) { --island_count; @@ -213,58 +290,54 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { b = b->next(); } - p_space->set_island_count((int)island_count); + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */ - const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list(); + sb = soft_body_list->first(); + while (sb) { + SoftBody3DSW *soft_body = sb->self(); - 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 (soft_body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); } - c->set_island_step(_step); + 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.push_back(c); - } - p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here - } + constraint_island.reserve(ISLAND_SIZE_RESERVE); - 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; + _populate_island_soft_body(soft_body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; } - c->set_island_step(_step); - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); + + if (constraint_island.is_empty()) { + --island_count; } - LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - constraint_island.push_back(c); } 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 */ - for (uint32_t island_index = 0; island_index < island_count; ++island_index) { - _setup_island(constraint_islands[island_index], p_delta); - } + 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(); @@ -272,12 +345,21 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - /* SOLVE CONSTRAINT ISLANDS */ + /* 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) { - // Warning: _solve_island modifies the constraint islands for optimization purpose, - // their content is not reliable after these calls and shouldn't be used anymore. - _solve_island(constraint_islands[island_index], p_iterations, p_delta); + _pre_solve_island(constraint_islands[island_index]); + } + + /* SOLVE CONSTRAINT ISLANDS */ + + // 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 @@ -298,7 +380,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { /* SLEEP / WAKE UP ISLANDS */ for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { - _check_suspend(body_islands[island_index], p_delta); + _check_suspend(body_islands[island_index]); } /* UPDATE SOFT BODY CONSTRAINTS */ @@ -315,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++; @@ -325,4 +409,11 @@ Step3DSW::Step3DSW() { 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 f406c35c3a..9c60004b24 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -34,21 +34,31 @@ #include "space_3d_sw.h" #include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" class Step3DSW { uint64_t _step; + 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 _setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta); - void _solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta); - void _check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta); + 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 |