From 448c41a3e4ba4ae7f1ffc3138ecfb7f85a6c8435 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Mon, 19 Apr 2021 18:38:11 -0700 Subject: Godot Physics collisions and solver processed on threads Use ThreadWorkPool to process physics step tasks in multiple threads. Collisions are all processed in parallel and solving impulses is processed in parallel for rigid body islands. Additional changes: - Proper islands for soft bodies linked to active bodies - All moving areas are on separate islands (can be parallelized) - Fix inconsistencies with body islands (Kinematic bodies could link bodies together or not depending on the processing order) - Completely prevent static bodies to be active (it could cause islands to be wrongly created and cause dangerous multi-threading operations as well as inconsistencies in created islands) - Apply impulses only on dynamic bodies to avoid unsafe multi-threaded operations (static bodies can be on multiple islands) - Removed inverted iterations when populating body islands, it's now faster in regular order (maybe after fixing inconsistencies) --- servers/physics_2d/area_pair_2d_sw.cpp | 102 +++++++++++------- servers/physics_2d/area_pair_2d_sw.h | 32 +++--- servers/physics_2d/body_2d_sw.cpp | 35 ++----- servers/physics_2d/body_pair_2d_sw.cpp | 89 ++++++++++------ servers/physics_2d/body_pair_2d_sw.h | 24 +++-- servers/physics_2d/collision_object_2d_sw.h | 4 +- servers/physics_2d/constraint_2d_sw.h | 1 + servers/physics_2d/joints_2d_sw.cpp | 92 +++++++++++----- servers/physics_2d/joints_2d_sw.h | 31 ++++-- servers/physics_2d/step_2d_sw.cpp | 157 +++++++++++++++++----------- servers/physics_2d/step_2d_sw.h | 15 ++- 11 files changed, 365 insertions(+), 217 deletions(-) (limited to 'servers/physics_2d') 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;i0) { - 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 &p_body_island, LocalVector &p_constraint_island) { p_body->set_island_step(_step); - p_body_island.push_back(p_body); - // Faster with reversed iterations. - for (const List>::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>::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 &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 &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 &p_constraint_island, p_constraint_island.resize(valid_constraint_count); } -void Step2DSW::_solve_island(LocalVector &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 &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 &p_body_island, real_t p_delta) { +void Step2DSW::_check_suspend(LocalVector &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 &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::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::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set::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 &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 *)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::List &aml = p_space->get_moved_area_list(); - - while (aml.first()) { - for (const Set::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 &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - constraint_island.push_back(c); - } - p_space->area_remove_from_moved_list((SelfList *)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> body_islands; LocalVector> constraint_islands; + LocalVector all_constraints; void _populate_island(Body2DSW *p_body, LocalVector &p_body_island, LocalVector &p_constraint_island); - void _setup_island(LocalVector &p_constraint_island, real_t p_delta); - void _solve_island(LocalVector &p_constraint_island, int p_iterations, real_t p_delta); - void _check_suspend(const LocalVector &p_body_island, real_t p_delta); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; + void _check_suspend(LocalVector &p_body_island) const; public: void step(Space2DSW *p_space, real_t p_delta, int p_iterations); Step2DSW(); + ~Step2DSW(); }; #endif // STEP_2D_SW_H -- cgit v1.2.3