diff options
Diffstat (limited to 'servers/physics_2d')
24 files changed, 557 insertions, 1413 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 0a91931354..6f244deb1e 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -43,7 +43,7 @@ void Body2DSW::update_inertias() { //update shapes and motions switch (mode) { - case PhysicsServer2D::BODY_MODE_RIGID: { + case PhysicsServer2D::BODY_MODE_DYNAMIC: { if (user_inertia) { _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; break; @@ -87,7 +87,7 @@ void Body2DSW::update_inertias() { _inv_inertia = 0; _inv_mass = 0; } break; - case PhysicsServer2D::BODY_MODE_CHARACTER: { + case PhysicsServer2D::BODY_MODE_DYNAMIC_LOCKED: { _inv_inertia = 0; _inv_mass = 1.0 / mass; @@ -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) { @@ -218,14 +204,14 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { first_time_kinematic = true; } } break; - case PhysicsServer2D::BODY_MODE_RIGID: { + case PhysicsServer2D::BODY_MODE_DYNAMIC: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; _set_static(false); set_active(true); } break; - case PhysicsServer2D::BODY_MODE_CHARACTER: { + case PhysicsServer2D::BODY_MODE_DYNAMIC_LOCKED: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = 0; _set_static(false); @@ -233,7 +219,7 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { angular_velocity = 0; } break; } - if (p_mode == PhysicsServer2D::BODY_MODE_RIGID && _inv_inertia == 0) { + if (p_mode == PhysicsServer2D::BODY_MODE_DYNAMIC && _inv_inertia == 0) { _update_inertia(); } /* @@ -281,25 +267,16 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va } break; case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { - /* - if (mode==PhysicsServer2D::BODY_MODE_STATIC) - break; - */ linear_velocity = p_variant; wakeup(); } break; case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { - /* - if (mode!=PhysicsServer2D::BODY_MODE_RIGID) - break; - */ angular_velocity = p_variant; wakeup(); } break; case PhysicsServer2D::BODY_STATE_SLEEPING: { - //? if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { break; } @@ -318,7 +295,7 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va } break; case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode == PhysicsServer2D::BODY_MODE_RIGID && !active && !can_sleep) { + if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) { set_active(true); } @@ -370,13 +347,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; @@ -572,7 +542,7 @@ void Body2DSW::wakeup_neighbours() { continue; } Body2DSW *b = n[i]; - if (b->mode != PhysicsServer2D::BODY_MODE_RIGID) { + if (b->mode != PhysicsServer2D::BODY_MODE_DYNAMIC) { continue; } @@ -591,16 +561,17 @@ void Body2DSW::call_queries() { Variant v = dbs; const Variant *vp[2] = { &v, &fi_callback->callback_udata }; - Object *obj = ObjectDB::get_instance(fi_callback->id); + Object *obj = fi_callback->callable.get_object(); if (!obj) { - set_force_integration_callback(ObjectID(), StringName()); + set_force_integration_callback(Callable()); } else { Callable::CallError ce; + Variant rv; if (fi_callback->callback_udata.get_type() != Variant::NIL) { - obj->call(fi_callback->method, vp, 2, ce); + fi_callback->callable.call(vp, 2, rv, ce); } else { - obj->call(fi_callback->method, vp, 1, ce); + fi_callback->callable.call(vp, 1, rv, ce); } } } @@ -608,9 +579,7 @@ void Body2DSW::call_queries() { bool Body2DSW::sleep_test(real_t p_step) { if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - return true; // - } else if (mode == PhysicsServer2D::BODY_MODE_CHARACTER) { - return !active; // characters and kinematic bodies don't sleep unless asked to sleep + return true; } else if (!can_sleep) { return false; } @@ -625,16 +594,15 @@ bool Body2DSW::sleep_test(real_t p_step) { } } -void Body2DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { +void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { if (fi_callback) { memdelete(fi_callback); fi_callback = nullptr; } - if (p_id.is_valid()) { + if (p_callable.get_object()) { fi_callback = memnew(ForceIntegrationCallback); - fi_callback->id = p_id; - fi_callback->method = p_method; + fi_callback->callable = p_callable; fi_callback->callback_udata = p_udata; } } @@ -644,7 +612,7 @@ Body2DSW::Body2DSW() : active_list(this), inertia_update_list(this), direct_state_query_list(this) { - mode = PhysicsServer2D::BODY_MODE_RIGID; + mode = PhysicsServer2D::BODY_MODE_DYNAMIC; active = true; angular_velocity = 0; biased_angular_velocity = 0; diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index 7ea4ac697c..b4a95651cb 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -117,8 +117,7 @@ class Body2DSW : public CollisionObject2DSW { int contact_count; struct ForceIntegrationCallback { - ObjectID id; - StringName method; + Callable callable; Variant callback_udata; }; @@ -131,7 +130,7 @@ class Body2DSW : public CollisionObject2DSW { friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose public: - void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); + void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); _FORCE_INLINE_ void add_area(Area2DSW *p_area) { int index = areas.find(AreaCMP(p_area)); 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/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp deleted file mode 100644 index 17424629a9..0000000000 --- a/servers/physics_2d/broad_phase_2d_basic.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_basic.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "broad_phase_2d_basic.h" - -BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) { - current++; - - Element e; - e.owner = p_object_; - e._static = false; - e.subindex = p_subindex; - - element_map[current] = e; - return current; -} - -void BroadPhase2DBasic::move(ID p_id, const Rect2 &p_aabb) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get().aabb = p_aabb; -} - -void BroadPhase2DBasic::set_static(ID p_id, bool p_static) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get()._static = p_static; -} - -void BroadPhase2DBasic::remove(ID p_id) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - element_map.erase(E); -} - -CollisionObject2DSW *BroadPhase2DBasic::get_object(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, nullptr); - return E->get().owner; -} - -bool BroadPhase2DBasic::is_static(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, false); - return E->get()._static; -} - -int BroadPhase2DBasic::get_subindex(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, -1); - return E->get().subindex; -} - -int BroadPhase2DBasic::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect2 aabb = E->get().aabb; - if (aabb.intersects_segment(p_from, p_to)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} - -int BroadPhase2DBasic::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect2 aabb = E->get().aabb; - if (aabb.intersects(p_aabb)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} - -void BroadPhase2DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_userdata = p_userdata; - pair_callback = p_pair_callback; -} - -void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_userdata = p_userdata; - unpair_callback = p_unpair_callback; -} - -void BroadPhase2DBasic::update() { - // recompute pairs - for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) { - for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) { - Element *elem_A = &I->get(); - Element *elem_B = &J->get(); - - if (elem_A->owner == elem_B->owner) { - continue; - } - - bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static); - - PairKey key(I->key(), J->key()); - - Map<PairKey, void *>::Element *E = pair_map.find(key); - - if (!pair_ok && E) { - if (unpair_callback) { - unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata); - } - pair_map.erase(key); - } - - if (pair_ok && !E) { - void *data = nullptr; - if (pair_callback) { - data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata); - if (data) { - pair_map.insert(key, data); - } - } - } - } - } -} - -BroadPhase2DSW *BroadPhase2DBasic::_create() { - return memnew(BroadPhase2DBasic); -} - -BroadPhase2DBasic::BroadPhase2DBasic() { - current = 1; - unpair_callback = nullptr; - unpair_userdata = nullptr; - pair_callback = nullptr; - pair_userdata = nullptr; -} diff --git a/servers/physics_2d/broad_phase_2d_bvh.cpp b/servers/physics_2d/broad_phase_2d_bvh.cpp new file mode 100644 index 0000000000..5f53f4a012 --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_bvh.cpp @@ -0,0 +1,116 @@ +/*************************************************************************/ +/* broad_phase_2d_bvh.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "broad_phase_2d_bvh.h" +#include "collision_object_2d_sw.h" + +BroadPhase2DSW::ID BroadPhase2DBVH::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { + ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? + return oid + 1; +} + +void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) { + bvh.move(p_id - 1, p_aabb); +} + +void BroadPhase2DBVH::set_static(ID p_id, bool p_static) { + CollisionObject2DSW *it = bvh.get(p_id - 1); + bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? +} + +void BroadPhase2DBVH::remove(ID p_id) { + bvh.erase(p_id - 1); +} + +CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const { + CollisionObject2DSW *it = bvh.get(p_id - 1); + ERR_FAIL_COND_V(!it, nullptr); + return it; +} + +bool BroadPhase2DBVH::is_static(ID p_id) const { + return !bvh.is_pairable(p_id - 1); +} + +int BroadPhase2DBVH::get_subindex(ID p_id) const { + return bvh.get_subindex(p_id - 1); +} + +int BroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); +} + +int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); +} + +void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B) { + BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); + if (!bpo->pair_callback) { + return nullptr; + } + + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); +} + +void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B, void *pairdata) { + BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); + if (!bpo->unpair_callback) { + return; + } + + bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); +} + +void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + pair_callback = p_pair_callback; + pair_userdata = p_userdata; +} + +void BroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + unpair_callback = p_unpair_callback; + unpair_userdata = p_userdata; +} + +void BroadPhase2DBVH::update() { + bvh.update(); +} + +BroadPhase2DSW *BroadPhase2DBVH::_create() { + return memnew(BroadPhase2DBVH); +} + +BroadPhase2DBVH::BroadPhase2DBVH() { + bvh.set_pair_callback(_pair_callback, this); + bvh.set_unpair_callback(_unpair_callback, this); + pair_callback = nullptr; + pair_userdata = nullptr; + unpair_userdata = nullptr; +} diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_bvh.h index ca1db360fb..6c11d2561b 100644 --- a/servers/physics_2d/broad_phase_2d_basic.h +++ b/servers/physics_2d/broad_phase_2d_bvh.h @@ -1,5 +1,5 @@ /*************************************************************************/ -/* broad_phase_2d_basic.h */ +/* broad_phase_2d_bvh.h */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,49 +28,19 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef BROAD_PHASE_2D_BASIC_H -#define BROAD_PHASE_2D_BASIC_H +#ifndef BROAD_PHASE_2D_BVH_H +#define BROAD_PHASE_2D_BVH_H -#include "core/templates/map.h" -#include "space_2d_sw.h" -class BroadPhase2DBasic : public BroadPhase2DSW { - struct Element { - CollisionObject2DSW *owner; - bool _static; - Rect2 aabb; - int subindex; - }; +#include "broad_phase_2d_sw.h" +#include "core/math/bvh.h" +#include "core/math/rect2.h" +#include "core/math/vector2.h" - Map<ID, Element> element_map; +class BroadPhase2DBVH : public BroadPhase2DSW { + BVH_Manager<CollisionObject2DSW, true, 128, Rect2, Vector2> bvh; - ID current; - - struct PairKey { - union { - struct { - ID a; - ID b; - }; - uint64_t key; - }; - - _FORCE_INLINE_ bool operator<(const PairKey &p_key) const { - return key < p_key.key; - } - - PairKey() { key = 0; } - PairKey(ID p_a, ID p_b) { - if (p_a > p_b) { - a = p_b; - b = p_a; - } else { - a = p_a; - b = p_b; - } - } - }; - - Map<PairKey, void *> pair_map; + static void *_pair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int); + static void _unpair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int, void *); PairCallback pair_callback; void *pair_userdata; @@ -79,7 +49,7 @@ class BroadPhase2DBasic : public BroadPhase2DSW { public: // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0); + virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); virtual void move(ID p_id, const Rect2 &p_aabb); virtual void set_static(ID p_id, bool p_static); virtual void remove(ID p_id); @@ -97,7 +67,7 @@ public: virtual void update(); static BroadPhase2DSW *_create(); - BroadPhase2DBasic(); + BroadPhase2DBVH(); }; -#endif // BROAD_PHASE_2D_BASIC_H +#endif // BROAD_PHASE_2D_BVH_H diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp deleted file mode 100644 index 6cfe6908d1..0000000000 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ /dev/null @@ -1,735 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_hash_grid.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "broad_phase_2d_hash_grid.h" -#include "collision_object_2d_sw.h" -#include "core/config/project_settings.h" - -#define LARGE_ELEMENT_FI 1.01239812 - -void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) { - Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with); - - ERR_FAIL_COND(p_elem->_static && p_with->_static); - - if (!E) { - PairData *pd = memnew(PairData); - p_elem->paired[p_with] = pd; - p_with->paired[p_elem] = pd; - } else { - E->get()->rc++; - } -} - -void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) { - Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with); - - ERR_FAIL_COND(!E); //this should really be paired.. - - E->get()->rc--; - - if (E->get()->rc == 0) { - if (E->get()->colliding) { - //uncollide - if (unpair_callback) { - unpair_callback(p_elem->owner, p_elem->subindex, p_with->owner, p_with->subindex, E->get()->ud, unpair_userdata); - } - } - - memdelete(E->get()); - p_elem->paired.erase(E); - p_with->paired.erase(p_elem); - } -} - -void BroadPhase2DHashGrid::_check_motion(Element *p_elem) { - for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) { - bool physical_collision = p_elem->aabb.intersects(E->key()->aabb); - bool logical_collision = p_elem->owner->test_collision_mask(E->key()->owner); - - if (physical_collision) { - if (!E->get()->colliding || (logical_collision && !E->get()->ud && pair_callback)) { - E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata); - } else if (E->get()->colliding && !logical_collision && E->get()->ud && unpair_callback) { - unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata); - E->get()->ud = nullptr; - } - E->get()->colliding = true; - } else { // No physcial_collision - if (E->get()->colliding && unpair_callback) { - unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata); - } - E->get()->colliding = false; - } - } -} - -void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) { - Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues - if (sz.width * sz.height > large_object_min_surface) { - //large object, do not use grid, must check against all elements - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - if (E->key() == p_elem->self) { - continue; // do not pair against itself - } - if (E->get().owner == p_elem->owner) { - continue; - } - if (E->get()._static && p_static) { - continue; - } - - _pair_attempt(p_elem, &E->get()); - } - - large_elements[p_elem].inc(); - return; - } - - Point2i from = (p_rect.position / cell_size).floor(); - Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor(); - - for (int i = from.x; i <= to.x; i++) { - for (int j = from.y; j <= to.y; j++) { - PosKey pk; - pk.x = i; - pk.y = j; - - uint32_t idx = pk.hash() % hash_table_size; - PosBin *pb = hash_table[idx]; - - while (pb) { - if (pb->key == pk) { - break; - } - - pb = pb->next; - } - - bool entered = false; - - if (!pb) { - //does not exist, create! - pb = memnew(PosBin); - pb->key = pk; - pb->next = hash_table[idx]; - hash_table[idx] = pb; - } - - if (p_static) { - if (pb->static_object_set[p_elem].inc() == 1) { - entered = true; - } - } else { - if (pb->object_set[p_elem].inc() == 1) { - entered = true; - } - } - - if (entered) { - for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - if (E->key()->owner == p_elem->owner) { - continue; - } - _pair_attempt(p_elem, E->key()); - } - - if (!p_static) { - for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) { - if (E->key()->owner == p_elem->owner) { - continue; - } - _pair_attempt(p_elem, E->key()); - } - } - } - } - } - - //pair separatedly with large elements - - for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (E->key() == p_elem) { - continue; // do not pair against itself - } - if (E->key()->owner == p_elem->owner) { - continue; - } - if (E->key()->_static && p_static) { - continue; - } - - _pair_attempt(E->key(), p_elem); - } -} - -void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) { - Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); - if (sz.width * sz.height > large_object_min_surface) { - //unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static - Map<Element *, PairData *>::Element *E = p_elem->paired.front(); - while (E) { - Map<Element *, PairData *>::Element *next = E->next(); - _unpair_attempt(p_elem, E->key()); - E = next; - } - - if (large_elements[p_elem].dec() == 0) { - large_elements.erase(p_elem); - } - return; - } - - Point2i from = (p_rect.position / cell_size).floor(); - Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor(); - - for (int i = from.x; i <= to.x; i++) { - for (int j = from.y; j <= to.y; j++) { - PosKey pk; - pk.x = i; - pk.y = j; - - uint32_t idx = pk.hash() % hash_table_size; - PosBin *pb = hash_table[idx]; - - while (pb) { - if (pb->key == pk) { - break; - } - - pb = pb->next; - } - - ERR_CONTINUE(!pb); //should exist!! - - bool exited = false; - - if (p_static) { - if (pb->static_object_set[p_elem].dec() == 0) { - pb->static_object_set.erase(p_elem); - exited = true; - } - } else { - if (pb->object_set[p_elem].dec() == 0) { - pb->object_set.erase(p_elem); - exited = true; - } - } - - if (exited) { - for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - if (E->key()->owner == p_elem->owner) { - continue; - } - _unpair_attempt(p_elem, E->key()); - } - - if (!p_static) { - for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) { - if (E->key()->owner == p_elem->owner) { - continue; - } - _unpair_attempt(p_elem, E->key()); - } - } - } - - if (pb->object_set.is_empty() && pb->static_object_set.is_empty()) { - if (hash_table[idx] == pb) { - hash_table[idx] = pb->next; - } else { - PosBin *px = hash_table[idx]; - - while (px) { - if (px->next == pb) { - px->next = pb->next; - break; - } - - px = px->next; - } - - ERR_CONTINUE(!px); - } - - memdelete(pb); - } - } - } - - for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (E->key() == p_elem) { - continue; // do not pair against itself - } - if (E->key()->owner == p_elem->owner) { - continue; - } - if (E->key()->_static && p_static) { - continue; - } - - //unpair from large elements - _unpair_attempt(p_elem, E->key()); - } -} - -BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) { - current++; - - Element e; - e.owner = p_object; - e._static = false; - e.subindex = p_subindex; - e.self = current; - e.pass = 0; - - element_map[current] = e; - return current; -} - -void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - - Element &e = E->get(); - - if (p_aabb != e.aabb) { - if (p_aabb != Rect2()) { - _enter_grid(&e, p_aabb, e._static); - } - if (e.aabb != Rect2()) { - _exit_grid(&e, e.aabb, e._static); - } - e.aabb = p_aabb; - } - - _check_motion(&e); -} - -void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - - Element &e = E->get(); - - if (e._static == p_static) { - return; - } - - if (e.aabb != Rect2()) { - _exit_grid(&e, e.aabb, e._static); - } - - e._static = p_static; - - if (e.aabb != Rect2()) { - _enter_grid(&e, e.aabb, e._static); - _check_motion(&e); - } -} - -void BroadPhase2DHashGrid::remove(ID p_id) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - - Element &e = E->get(); - - if (e.aabb != Rect2()) { - _exit_grid(&e, e.aabb, e._static); - } - - element_map.erase(p_id); -} - -CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, nullptr); - return E->get().owner; -} - -bool BroadPhase2DHashGrid::is_static(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, false); - return E->get()._static; -} - -int BroadPhase2DHashGrid::get_subindex(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, -1); - return E->get().subindex; -} - -template <bool use_aabb, bool use_segment> -void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index) { - PosKey pk; - pk.x = p_cell.x; - pk.y = p_cell.y; - - uint32_t idx = pk.hash() % hash_table_size; - PosBin *pb = hash_table[idx]; - - while (pb) { - if (pb->key == pk) { - break; - } - - pb = pb->next; - } - - if (!pb) { - return; - } - - for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - if (index >= p_max_results) { - break; - } - if (E->key()->pass == pass) { - continue; - } - - E->key()->pass = pass; - - if (use_aabb && !p_aabb.intersects(E->key()->aabb)) { - continue; - } - - if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) { - continue; - } - - p_results[index] = E->key()->owner; - p_result_indices[index] = E->key()->subindex; - index++; - } - - for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) { - if (index >= p_max_results) { - break; - } - if (E->key()->pass == pass) { - continue; - } - - if (use_aabb && !p_aabb.intersects(E->key()->aabb)) { - continue; - } - - if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) { - continue; - } - - E->key()->pass = pass; - p_results[index] = E->key()->owner; - p_result_indices[index] = E->key()->subindex; - index++; - } -} - -int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - pass++; - - Vector2 dir = (p_to - p_from); - if (dir == Vector2()) { - return 0; - } - //avoid divisions by zero - dir.normalize(); - if (dir.x == 0.0) { - dir.x = 0.000001; - } - if (dir.y == 0.0) { - dir.y = 0.000001; - } - Vector2 delta = dir.abs(); - - delta.x = cell_size / delta.x; - delta.y = cell_size / delta.y; - - Point2i pos = (p_from / cell_size).floor(); - Point2i end = (p_to / cell_size).floor(); - - Point2i step = Vector2(SGN(dir.x), SGN(dir.y)); - - Vector2 max; - - if (dir.x < 0) { - max.x = (Math::floor((double)pos.x) * cell_size - p_from.x) / dir.x; - } else { - max.x = (Math::floor((double)pos.x + 1) * cell_size - p_from.x) / dir.x; - } - - if (dir.y < 0) { - max.y = (Math::floor((double)pos.y) * cell_size - p_from.y) / dir.y; - } else { - max.y = (Math::floor((double)pos.y + 1) * cell_size - p_from.y) / dir.y; - } - - int cullcount = 0; - _cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount); - - bool reached_x = false; - bool reached_y = false; - - while (true) { - if (max.x < max.y) { - max.x += delta.x; - pos.x += step.x; - } else { - max.y += delta.y; - pos.y += step.y; - } - - if (step.x > 0) { - if (pos.x >= end.x) { - reached_x = true; - } - } else if (pos.x <= end.x) { - reached_x = true; - } - - if (step.y > 0) { - if (pos.y >= end.y) { - reached_y = true; - } - } else if (pos.y <= end.y) { - reached_y = true; - } - - _cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount); - - if (reached_x && reached_y) { - break; - } - } - - for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (cullcount >= p_max_results) { - break; - } - if (E->key()->pass == pass) { - continue; - } - - E->key()->pass = pass; - - /* - if (use_aabb && !p_aabb.intersects(E->key()->aabb)) - continue; - */ - - if (!E->key()->aabb.intersects_segment(p_from, p_to)) { - continue; - } - - p_results[cullcount] = E->key()->owner; - p_result_indices[cullcount] = E->key()->subindex; - cullcount++; - } - - return cullcount; -} - -int BroadPhase2DHashGrid::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - pass++; - - Point2i from = (p_aabb.position / cell_size).floor(); - Point2i to = ((p_aabb.position + p_aabb.size) / cell_size).floor(); - int cullcount = 0; - - for (int i = from.x; i <= to.x; i++) { - for (int j = from.y; j <= to.y; j++) { - _cull<true, false>(Point2i(i, j), p_aabb, Point2(), Point2(), p_results, p_max_results, p_result_indices, cullcount); - } - } - - for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (cullcount >= p_max_results) { - break; - } - if (E->key()->pass == pass) { - continue; - } - - E->key()->pass = pass; - - if (!p_aabb.intersects(E->key()->aabb)) { - continue; - } - - /* - if (!E->key()->aabb.intersects_segment(p_from,p_to)) - continue; - */ - - p_results[cullcount] = E->key()->owner; - p_result_indices[cullcount] = E->key()->subindex; - cullcount++; - } - return cullcount; -} - -void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_callback = p_pair_callback; - pair_userdata = p_userdata; -} - -void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_callback = p_unpair_callback; - unpair_userdata = p_userdata; -} - -void BroadPhase2DHashGrid::update() { -} - -BroadPhase2DSW *BroadPhase2DHashGrid::_create() { - return memnew(BroadPhase2DHashGrid); -} - -BroadPhase2DHashGrid::BroadPhase2DHashGrid() { - hash_table_size = GLOBAL_DEF("physics/2d/bp_hash_table_size", 4096); - ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/bp_hash_table_size", PropertyInfo(Variant::INT, "physics/2d/bp_hash_table_size", PROPERTY_HINT_RANGE, "0,8192,1,or_greater")); - hash_table_size = Math::larger_prime(hash_table_size); - hash_table = memnew_arr(PosBin *, hash_table_size); - - cell_size = GLOBAL_DEF("physics/2d/cell_size", 128); - ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/cell_size", PropertyInfo(Variant::INT, "physics/2d/cell_size", PROPERTY_HINT_RANGE, "0,512,1,or_greater")); - - large_object_min_surface = GLOBAL_DEF("physics/2d/large_object_surface_threshold_in_cells", 512); - ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/large_object_surface_threshold_in_cells", PropertyInfo(Variant::INT, "physics/2d/large_object_surface_threshold_in_cells", PROPERTY_HINT_RANGE, "0,1024,1,or_greater")); - - for (uint32_t i = 0; i < hash_table_size; i++) { - hash_table[i] = nullptr; - } - pass = 1; - - current = 0; -} - -BroadPhase2DHashGrid::~BroadPhase2DHashGrid() { - for (uint32_t i = 0; i < hash_table_size; i++) { - while (hash_table[i]) { - PosBin *pb = hash_table[i]; - hash_table[i] = pb->next; - memdelete(pb); - } - } - - memdelete_arr(hash_table); -} - -/* 3D version of voxel traversal: - -public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth) -{ - // Implementation is based on: - // "A Fast Voxel Traversal Algorithm for Ray Tracing" - // John Amanatides, Andrew Woo - // http://www.cse.yorku.ca/~amana/research/grid.pdf - // https://web.archive.org/web/20100616193049/http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf - - // NOTES: - // * This code assumes that the ray's position and direction are in 'cell coordinates', which means - // that one unit equals one cell in all directions. - // * When the ray doesn't start within the voxel grid, calculate the first position at which the - // ray could enter the grid. If it never enters the grid, there is nothing more to do here. - // * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite. - // * The Point3D structure is a simple structure having three integer fields (X, Y and Z). - - // The cell in which the ray starts. - Point3D start = GetCellAt(ray.Position); // Rounds the position's X, Y and Z down to the nearest integer values. - int x = start.X; - int y = start.Y; - int z = start.Z; - - // Determine which way we go. - int stepX = Math.Sign(ray.Direction.X); - int stepY = Math.Sign(ray.Direction.Y); - int stepZ = Math.Sign(ray.Direction.Z); - - // Calculate cell boundaries. When the step (i.e. direction sign) is positive, - // the next boundary is AFTER our current position, meaning that we have to add 1. - // Otherwise, it is BEFORE our current position, in which case we add nothing. - Point3D cellBoundary = new Point3D( - x + (stepX > 0 ? 1 : 0), - y + (stepY > 0 ? 1 : 0), - z + (stepZ > 0 ? 1 : 0)); - - // NOTE: For the following calculations, the result will be Single.PositiveInfinity - // when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand - // value of the division also equals zero, the result is Single.NaN, which is not OK. - - // Determine how far we can travel along the ray before we hit a voxel boundary. - Vector3 tMax = new Vector3( - (cellBoundary.X - ray.Position.X) / ray.Direction.X, // Boundary is a plane on the YZ axis. - (cellBoundary.Y - ray.Position.Y) / ray.Direction.Y, // Boundary is a plane on the XZ axis. - (cellBoundary.Z - ray.Position.Z) / ray.Direction.Z); // Boundary is a plane on the XY axis. - if (Single.IsNaN(tMax.X)) tMax.X = Single.PositiveInfinity; - if (Single.IsNaN(tMax.Y)) tMax.Y = Single.PositiveInfinity; - if (Single.IsNaN(tMax.Z)) tMax.Z = Single.PositiveInfinity; - - // Determine how far we must travel along the ray before we have crossed a gridcell. - Vector3 tDelta = new Vector3( - stepX / ray.Direction.X, // Crossing the width of a cell. - stepY / ray.Direction.Y, // Crossing the height of a cell. - stepZ / ray.Direction.Z); // Crossing the depth of a cell. - if (Single.IsNaN(tDelta.X)) tDelta.X = Single.PositiveInfinity; - if (Single.IsNaN(tDelta.Y)) tDelta.Y = Single.PositiveInfinity; - if (Single.IsNaN(tDelta.Z)) tDelta.Z = Single.PositiveInfinity; - - // For each step, determine which distance to the next voxel boundary is lowest (i.e. - // which voxel boundary is nearest) and walk that way. - for (int i = 0; i < maxDepth; i++) - { - // Return it. - yield return new Point3D(x, y, z); - - // Do the next step. - if (tMax.X < tMax.Y && tMax.X < tMax.Z) - { - // tMax.X is the lowest, an YZ cell boundary plane is nearest. - x += stepX; - tMax.X += tDelta.X; - } - else if (tMax.Y < tMax.Z) - { - // tMax.Y is the lowest, an XZ cell boundary plane is nearest. - y += stepY; - tMax.Y += tDelta.Y; - } - else - { - // tMax.Z is the lowest, an XY cell boundary plane is nearest. - z += stepZ; - tMax.Z += tDelta.Z; - } - } - - */ diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h deleted file mode 100644 index eb7c8879ac..0000000000 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ /dev/null @@ -1,187 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_hash_grid.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef BROAD_PHASE_2D_HASH_GRID_H -#define BROAD_PHASE_2D_HASH_GRID_H - -#include "broad_phase_2d_sw.h" -#include "core/templates/map.h" - -class BroadPhase2DHashGrid : public BroadPhase2DSW { - struct PairData { - bool colliding; - int rc; - void *ud; - PairData() { - colliding = false; - rc = 1; - ud = nullptr; - } - }; - - struct Element { - ID self; - CollisionObject2DSW *owner; - bool _static; - Rect2 aabb; - int subindex; - uint64_t pass; - Map<Element *, PairData *> paired; - }; - - struct RC { - int ref; - - _FORCE_INLINE_ int inc() { - ref++; - return ref; - } - _FORCE_INLINE_ int dec() { - ref--; - return ref; - } - - _FORCE_INLINE_ RC() { - ref = 0; - } - }; - - Map<ID, Element> element_map; - Map<Element *, RC> large_elements; - - ID current; - - uint64_t pass; - - struct PairKey { - union { - struct { - ID a; - ID b; - }; - uint64_t key; - }; - - _FORCE_INLINE_ bool operator<(const PairKey &p_key) const { - return key < p_key.key; - } - - PairKey() { key = 0; } - PairKey(ID p_a, ID p_b) { - if (p_a > p_b) { - a = p_b; - b = p_a; - } else { - a = p_a; - b = p_b; - } - } - }; - - Map<PairKey, PairData> pair_map; - - int cell_size; - int large_object_min_surface; - - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; - - void _enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static); - void _exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static); - template <bool use_aabb, bool use_segment> - _FORCE_INLINE_ void _cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index); - - struct PosKey { - union { - struct { - int32_t x; - int32_t y; - }; - uint64_t key; - }; - - _FORCE_INLINE_ uint32_t hash() const { - uint64_t k = key; - k = (~k) + (k << 18); // k = (k << 18) - k - 1; - k = k ^ (k >> 31); - k = k * 21; // k = (k + (k << 2)) + (k << 4); - k = k ^ (k >> 11); - k = k + (k << 6); - k = k ^ (k >> 22); - return k; - } - - bool operator==(const PosKey &p_key) const { return key == p_key.key; } - _FORCE_INLINE_ bool operator<(const PosKey &p_key) const { - return key < p_key.key; - } - }; - - struct PosBin { - PosKey key; - Map<Element *, RC> object_set; - Map<Element *, RC> static_object_set; - PosBin *next; - }; - - uint32_t hash_table_size; - PosBin **hash_table; - - void _pair_attempt(Element *p_elem, Element *p_with); - void _unpair_attempt(Element *p_elem, Element *p_with); - void _check_motion(Element *p_elem); - -public: - virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0); - virtual void move(ID p_id, const Rect2 &p_aabb); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObject2DSW *get_object(ID p_id) const; - virtual bool is_static(ID p_id) const; - virtual int get_subindex(ID p_id) const; - - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); - - virtual void update(); - - static BroadPhase2DSW *_create(); - - BroadPhase2DHashGrid(); - ~BroadPhase2DHashGrid(); -}; - -#endif // BROAD_PHASE_2D_HASH_GRID_H diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h index d17ee6e2d6..0f82f06b9c 100644 --- a/servers/physics_2d/broad_phase_2d_sw.h +++ b/servers/physics_2d/broad_phase_2d_sw.h @@ -48,7 +48,7 @@ public: typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata); // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0) = 0; + virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0; virtual void move(ID p_id, const Rect2 &p_aabb) = 0; virtual void set_static(ID p_id, bool p_static) = 0; virtual void remove(ID p_id) = 0; diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index 7a2f312263..fa87dc1f3f 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -182,19 +182,19 @@ void CollisionObject2DSW::_update_shapes() { continue; } - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } - //not quite correct, should compute the next matrix.. Rect2 shape_aabb = s.shape->get_aabb(); Transform2D xform = transform * s.xform; shape_aabb = xform.xform(shape_aabb); + shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); s.aabb_cache = shape_aabb; - s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); - space->get_broadphase()->move(s.bpid, s.aabb_cache); + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); } } @@ -209,11 +209,6 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { continue; } - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } - //not quite correct, should compute the next matrix.. Rect2 shape_aabb = s.shape->get_aabb(); Transform2D xform = transform * s.xform; @@ -221,6 +216,11 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion s.aabb_cache = shape_aabb; + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + space->get_broadphase()->move(s.bpid, shape_aabb); } } 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 c7b556deba..5a0a628fbc 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -97,8 +97,16 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto } bool PinJoint2DSW::setup(real_t p_step) { + 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); + rA = A->get_transform().basis_xform(anchor_A); rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B; @@ -143,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; } @@ -156,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()); @@ -169,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); } @@ -257,10 +273,19 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { } bool GrooveJoint2DSW::setup(real_t p_step) { + 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(); @@ -297,16 +322,24 @@ bool GrooveJoint2DSW::setup(real_t p_step) { Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); 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); + gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias()); 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); @@ -315,12 +348,16 @@ void GrooveJoint2DSW::solve(real_t p_step) { Vector2 jOld = jn_acc; j += jOld; - jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).clamped(jn_max); + jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max); 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) : @@ -342,6 +379,13 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_ ////////////////////////////////////////////// bool DampedSpringJoint2DSW::setup(real_t p_step) { + 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; + } + rA = A->get_transform().basis_xform(anchor_A); rB = B->get_transform().basis_xform(anchor_B); @@ -360,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); + + return true; +} - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); +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; } @@ -380,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/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp index 1040437ca7..1c2dca0259 100644 --- a/servers/physics_2d/physics_server_2d_sw.cpp +++ b/servers/physics_2d/physics_server_2d_sw.cpp @@ -30,8 +30,7 @@ #include "physics_server_2d_sw.h" -#include "broad_phase_2d_basic.h" -#include "broad_phase_2d_hash_grid.h" +#include "broad_phase_2d_bvh.h" #include "collision_solver_2d_sw.h" #include "core/config/project_settings.h" #include "core/debugger/engine_debugger.h" @@ -927,10 +926,10 @@ int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } -void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) { +void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); - body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method, p_udata); + body->set_force_integration_callback(p_callable, p_udata); } bool PhysicsServer2DSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { @@ -1238,6 +1237,10 @@ void PhysicsServer2DSW::set_active(bool p_active) { active = p_active; }; +void PhysicsServer2DSW::set_collision_iterations(int p_iterations) { + iterations = p_iterations; +}; + void PhysicsServer2DSW::init() { doing_sync = false; last_step = 0.001; @@ -1356,8 +1359,7 @@ PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = nullptr; PhysicsServer2DSW::PhysicsServer2DSW(bool p_using_threads) { singletonsw = this; - BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create; - //BroadPhase2DSW::create_func=BroadPhase2DBasic::_create; + BroadPhase2DSW::create_func = BroadPhase2DBVH::_create; active = true; island_count = 0; diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h index 65c5df0fce..5002bf5fc8 100644 --- a/servers/physics_2d/physics_server_2d_sw.h +++ b/servers/physics_2d/physics_server_2d_sw.h @@ -242,13 +242,13 @@ public: virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; virtual int body_get_max_contacts_reported(RID p_body) const override; - virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) override; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override; virtual void body_set_pickable(RID p_body, bool p_pickable) override; - virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override; - virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override; + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override; + virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.08) override; // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override; @@ -288,6 +288,8 @@ public: virtual void end_sync() override; virtual void finish() override; + virtual void set_collision_iterations(int p_iterations) override; + virtual bool is_flushing_queries() const override { return flushing_queries; } int get_process_info(ProcessInfo p_info) override; diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.cpp b/servers/physics_2d/physics_server_2d_wrap_mt.cpp index 790c87cc44..930b19c2cb 100644 --- a/servers/physics_2d/physics_server_2d_wrap_mt.cpp +++ b/servers/physics_2d/physics_server_2d_wrap_mt.cpp @@ -56,7 +56,7 @@ void PhysicsServer2DWrapMT::thread_loop() { step_thread_up.set(); while (!exit.is_set()) { // flush commands one by one, until exit is requested - command_queue.wait_and_flush_one(); + command_queue.wait_and_flush(); } command_queue.flush_all(); // flush all diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h index 3577f706de..c776641699 100644 --- a/servers/physics_2d/physics_server_2d_wrap_mt.h +++ b/servers/physics_2d/physics_server_2d_wrap_mt.h @@ -245,7 +245,7 @@ public: FUNC2(body_set_omit_force_integration, RID, bool); FUNC1RC(bool, body_is_omitting_force_integration, RID); - FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &); + FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override { return physics_2d_server->body_collide_shape(p_body, p_body_shape, p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); @@ -303,6 +303,7 @@ public: FUNC1(free, RID); FUNC1(set_active, bool); + FUNC1(set_collision_iterations, int); virtual void init() override; virtual void step(real_t p_step) override; diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 4f12248c3e..1380e57b57 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -827,7 +827,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction Vector2 lv = b->get_linear_velocity(); //compute displacement from linear velocity @@ -1109,7 +1109,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction Vector2 lv = b->get_linear_velocity(); //compute displacement from linear velocity 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 |