diff options
Diffstat (limited to 'servers')
24 files changed, 224 insertions, 171 deletions
diff --git a/servers/audio/audio_rb_resampler.cpp b/servers/audio/audio_rb_resampler.cpp index d9c442facf..b37965a988 100644 --- a/servers/audio/audio_rb_resampler.cpp +++ b/servers/audio/audio_rb_resampler.cpp @@ -43,7 +43,7 @@ int AudioRBResampler::get_channel_count() const { // Linear interpolation based sample rate conversion (low quality) // Note that AudioStreamPlaybackResampled::mix has better algorithm, -// but it wasn't obvious to integrate that with VideoPlayer +// but it wasn't obvious to integrate that with VideoStreamPlayer template <int C> uint32_t AudioRBResampler::_resample(AudioFrame *p_dest, int p_todo, int32_t p_increment) { uint32_t read = offset & MIX_FRAC_MASK; diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h index ba4c39737a..f00512fe92 100644 --- a/servers/physics_2d/godot_body_2d.h +++ b/servers/physics_2d/godot_body_2d.h @@ -234,9 +234,15 @@ public: angular_velocity += _inv_inertia * p_torque; } - _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { + _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2(), real_t p_max_delta_av = -1.0) { biased_linear_velocity += p_impulse * _inv_mass; - biased_angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); + if (p_max_delta_av != 0.0) { + real_t delta_av = _inv_inertia * (p_position - center_of_mass).cross(p_impulse); + if (p_max_delta_av > 0 && delta_av > p_max_delta_av) { + delta_av = p_max_delta_av; + } + biased_angular_velocity += delta_av; + } } void set_active(bool p_active); diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp index 67b0f21456..f8ec0b6512 100644 --- a/servers/physics_2d/godot_body_pair_2d.cpp +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -34,6 +34,9 @@ #define ACCUMULATE_IMPULSES +#define MIN_VELOCITY 0.001 +#define MAX_BIAS_ROTATION (Math_PI / 8) + void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) { GodotBodyPair2D *self = (GodotBodyPair2D *)p_self; @@ -41,8 +44,6 @@ void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_po } void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) { - // check if we already have the contact - Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B); @@ -51,46 +52,48 @@ void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Ve ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); Contact contact; - - contact.acc_normal_impulse = 0; - contact.acc_bias_impulse = 0; - contact.acc_tangent_impulse = 0; contact.local_A = local_A; contact.local_B = local_B; - contact.reused = true; contact.normal = (p_point_A - p_point_B).normalized(); - contact.mass_normal = 0; // will be computed in setup() - - // attempt to determine if the contact will be reused + contact.used = true; + // Attempt to determine if the contact will be reused. real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius(); for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - if ( - c.local_A.distance_squared_to(local_A) < (recycle_radius_2) && + if (c.local_A.distance_squared_to(local_A) < (recycle_radius_2) && c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) { contact.acc_normal_impulse = c.acc_normal_impulse; contact.acc_tangent_impulse = c.acc_tangent_impulse; contact.acc_bias_impulse = c.acc_bias_impulse; - new_index = i; - break; + contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; + c = contact; + return; } } - // figure out if the contact amount must be reduced to fit the new contact - + // Figure out if the contact amount must be reduced to fit the new contact. if (new_index == MAX_CONTACTS) { - // remove the contact with the minimum depth - - int least_deep = -1; - real_t min_depth = 1e10; + // Remove the contact with the minimum depth. 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]; + int least_deep = -1; + real_t min_depth; + + // Start with depth for new contact. + { + Vector2 global_A = transform_A.basis_xform(contact.local_A); + Vector2 global_B = transform_B.basis_xform(contact.local_B) + offset_B; + + Vector2 axis = global_A - global_B; + min_depth = axis.dot(contact.normal); + } + + for (int i = 0; i < contact_count; i++) { + const Contact &c = contacts[i]; Vector2 global_A = transform_A.basis_xform(c.local_A); Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; @@ -103,10 +106,8 @@ void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Ve } } - ERR_FAIL_COND(least_deep == -1); - - if (least_deep < contact_count) { //replace the last deep contact by the new one - + if (least_deep > -1) { + // Replace the least deep contact by the new one. contacts[least_deep] = contact; } @@ -114,15 +115,11 @@ void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Ve } contacts[new_index] = contact; - - if (new_index == contact_count) { - contact_count++; - } + contact_count++; } void GodotBodyPair2D::_validate_contacts() { - //make sure to erase contacts that are no longer valid - + // Make sure to erase contacts that are no longer valid. real_t max_separation = space->get_contact_max_separation(); real_t max_separation2 = max_separation * max_separation; @@ -133,11 +130,11 @@ void GodotBodyPair2D::_validate_contacts() { Contact &c = contacts[i]; bool erase = false; - if (!c.reused) { - //was left behind in previous frame + if (!c.used) { + // Was left behind in previous frame. erase = true; } else { - c.reused = false; + c.used = false; Vector2 global_A = transform_A.basis_xform(c.local_A); Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; @@ -150,10 +147,10 @@ void GodotBodyPair2D::_validate_contacts() { } if (erase) { - // contact no longer needed, remove + // Contact no longer needed, remove. if ((i + 1) < contact_count) { - // swap with the last one + // Swap with the last one. SWAP(contacts[i], contacts[contact_count - 1]); } @@ -302,9 +299,6 @@ bool GodotBodyPair2D::setup(real_t p_step) { bool valid = false; for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - if (!c.reused) { - continue; - } if (c.normal.dot(direction) > -CMP_EPSILON) { //greater (normal inverted) continue; } @@ -323,9 +317,6 @@ bool GodotBodyPair2D::setup(real_t p_step) { bool valid = false; for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - if (!c.reused) { - continue; - } if (c.normal.dot(direction) < CMP_EPSILON) { //less (normal ok) continue; } @@ -350,7 +341,7 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { real_t max_penetration = space->get_contact_max_allowed_penetration(); - real_t bias = 0.3; + real_t bias = space->get_contact_bias(); GodotShape2D *shape_A_ptr = A->get_shape(shape_A); GodotShape2D *shape_B_ptr = B->get_shape(shape_B); @@ -389,7 +380,7 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { Vector2 axis = global_A - global_B; real_t depth = axis.dot(c.normal); - if (depth <= 0.0 || !c.reused) { + if (depth <= 0.0) { continue; } @@ -400,8 +391,8 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { } #endif - c.rA = global_A; - c.rB = global_B - offset_B; + c.rA = global_A - A->get_center_of_mass(); + c.rB = global_B - B->get_center_of_mass() - offset_B; if (A->can_report_contacts()) { Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); @@ -434,7 +425,6 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); c.depth = depth; - //c.acc_bias_impulse=0; #ifdef ACCUMULATE_IMPULSES { @@ -442,10 +432,10 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; if (collide_A) { - A->apply_impulse(-P, c.rA); + A->apply_impulse(-P, c.rA + A->get_center_of_mass()); } if (collide_B) { - B->apply_impulse(P, c.rB); + B->apply_impulse(P, c.rB + B->get_center_of_mass()); } } #endif @@ -470,6 +460,11 @@ void GodotBodyPair2D::solve(real_t p_step) { return; } + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + for (int i = 0; i < contact_count; ++i) { Contact &c = contacts[i]; @@ -489,6 +484,7 @@ void GodotBodyPair2D::solve(real_t p_step) { real_t vn = dv.dot(c.normal); real_t vbn = dbv.dot(c.normal); + Vector2 tangent = c.normal.orthogonal(); real_t vt = dv.dot(tangent); @@ -499,10 +495,31 @@ void GodotBodyPair2D::solve(real_t p_step) { Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); if (collide_A) { - A->apply_bias_impulse(-jb, c.rA); + A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av); } if (collide_B) { - B->apply_bias_impulse(jb, c.rB); + B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av); + } + + crbA = Vector2(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x); + crbB = Vector2(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x); + dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B); + real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; + c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); + + Vector2 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); + + if (collide_A) { + A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); + } + if (collide_B) { + B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + } } real_t jn = -(c.bounce + vn) * c.mass_normal; @@ -519,10 +536,10 @@ void GodotBodyPair2D::solve(real_t p_step) { Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); if (collide_A) { - A->apply_impulse(-j, c.rA); + A->apply_impulse(-j, c.rA + A->get_center_of_mass()); } if (collide_B) { - B->apply_impulse(j, c.rB); + B->apply_impulse(j, c.rB + B->get_center_of_mass()); } } } diff --git a/servers/physics_2d/godot_body_pair_2d.h b/servers/physics_2d/godot_body_pair_2d.h index 0938ab542b..aa1b5b7886 100644 --- a/servers/physics_2d/godot_body_pair_2d.h +++ b/servers/physics_2d/godot_body_pair_2d.h @@ -62,13 +62,14 @@ class GodotBodyPair2D : public GodotConstraint2D { real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt) real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) + real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com real_t mass_normal, mass_tangent = 0.0; real_t bias = 0.0; real_t depth = 0.0; bool active = false; + bool used = false; Vector2 rA, rB; - bool reused = false; real_t bounce = 0.0; }; diff --git a/servers/physics_2d/godot_physics_server_2d.cpp b/servers/physics_2d/godot_physics_server_2d.cpp index 8ac27077fc..c88fd39657 100644 --- a/servers/physics_2d/godot_physics_server_2d.cpp +++ b/servers/physics_2d/godot_physics_server_2d.cpp @@ -1214,21 +1214,16 @@ void GodotPhysicsServer2D::free(RID p_rid) { } else { ERR_FAIL_MSG("Invalid ID."); } -}; +} void GodotPhysicsServer2D::set_active(bool p_active) { active = p_active; -}; - -void GodotPhysicsServer2D::set_collision_iterations(int p_iterations) { - iterations = p_iterations; -}; +} void GodotPhysicsServer2D::init() { doing_sync = false; - iterations = 8; // 8? stepper = memnew(GodotStep2D); -}; +} void GodotPhysicsServer2D::step(real_t p_step) { if (!active) { @@ -1241,16 +1236,16 @@ void GodotPhysicsServer2D::step(real_t p_step) { active_objects = 0; collision_pairs = 0; for (Set<const GodotSpace2D *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((GodotSpace2D *)E->get(), p_step, iterations); + stepper->step((GodotSpace2D *)E->get(), p_step); island_count += E->get()->get_island_count(); active_objects += E->get()->get_active_objects(); collision_pairs += E->get()->get_collision_pairs(); } -}; +} void GodotPhysicsServer2D::sync() { doing_sync = true; -}; +} void GodotPhysicsServer2D::flush_queries() { if (!active) { @@ -1308,7 +1303,7 @@ void GodotPhysicsServer2D::end_sync() { void GodotPhysicsServer2D::finish() { memdelete(stepper); -}; +} void GodotPhysicsServer2D::_update_shapes() { while (pending_shape_update_list.first()) { @@ -1340,4 +1335,4 @@ GodotPhysicsServer2D::GodotPhysicsServer2D(bool p_using_threads) { GodotBroadPhase2D::create_func = GodotBroadPhase2DBVH::_create; using_threads = p_using_threads; -}; +} diff --git a/servers/physics_2d/godot_physics_server_2d.h b/servers/physics_2d/godot_physics_server_2d.h index 1f544fee72..ad6d5e0940 100644 --- a/servers/physics_2d/godot_physics_server_2d.h +++ b/servers/physics_2d/godot_physics_server_2d.h @@ -45,7 +45,6 @@ class GodotPhysicsServer2D : public PhysicsServer2D { friend class GodotPhysicsDirectSpaceState2D; friend class GodotPhysicsDirectBodyState2D; bool active = true; - int iterations = 0; bool doing_sync = false; int island_count = 0; @@ -283,8 +282,6 @@ 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/godot_space_2d.cpp b/servers/physics_2d/godot_space_2d.cpp index 7a5eb26bb3..5e6f233667 100644 --- a/servers/physics_2d/godot_space_2d.cpp +++ b/servers/physics_2d/godot_space_2d.cpp @@ -1138,9 +1138,12 @@ void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_v case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break; - case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break; + case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + contact_bias = p_value; + break; case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: body_linear_velocity_sleep_threshold = p_value; break; @@ -1153,6 +1156,9 @@ void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_v case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break; + case PhysicsServer2D::SPACE_PARAM_SOLVER_ITERATIONS: + solver_iterations = p_value; + break; } } @@ -1162,8 +1168,10 @@ real_t GodotSpace2D::get_param(PhysicsServer2D::SpaceParameter p_param) const { return contact_recycle_radius; case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation; - case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration; + case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + return contact_bias; case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: return body_linear_velocity_sleep_threshold; case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: @@ -1172,6 +1180,8 @@ real_t GodotSpace2D::get_param(PhysicsServer2D::SpaceParameter p_param) const { return body_time_to_sleep; case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias; + case PhysicsServer2D::SPACE_PARAM_SOLVER_ITERATIONS: + return solver_iterations; } return 0; } diff --git a/servers/physics_2d/godot_space_2d.h b/servers/physics_2d/godot_space_2d.h index b155a834b6..c0d06706d8 100644 --- a/servers/physics_2d/godot_space_2d.h +++ b/servers/physics_2d/godot_space_2d.h @@ -96,9 +96,12 @@ private: GodotArea2D *area = nullptr; + int solver_iterations = 16; + real_t contact_recycle_radius = 1.0; real_t contact_max_separation = 1.5; real_t contact_max_allowed_penetration = 0.3; + real_t contact_bias = 0.8; real_t constraint_bias = 0.2; enum { @@ -155,9 +158,11 @@ public: void remove_object(GodotCollisionObject2D *p_object); const Set<GodotCollisionObject2D *> &get_objects() const; + _FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; } _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } + _FORCE_INLINE_ real_t get_contact_bias() const { return contact_bias; } _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } diff --git a/servers/physics_2d/godot_step_2d.cpp b/servers/physics_2d/godot_step_2d.cpp index 00d11acdab..bc604e380a 100644 --- a/servers/physics_2d/godot_step_2d.cpp +++ b/servers/physics_2d/godot_step_2d.cpp @@ -124,14 +124,14 @@ void GodotStep2D::_check_suspend(LocalVector<GodotBody2D *> &p_body_island) cons } } -void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta, int p_iterations) { +void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) { p_space->lock(); // can't access space during this p_space->setup(); //update inertias, etc p_space->set_last_step(p_delta); - iterations = p_iterations; + iterations = p_space->get_solver_iterations(); delta = p_delta; const SelfList<GodotBody2D>::List *body_list = &p_space->get_active_body_list(); diff --git a/servers/physics_2d/godot_step_2d.h b/servers/physics_2d/godot_step_2d.h index efec243632..4038417854 100644 --- a/servers/physics_2d/godot_step_2d.h +++ b/servers/physics_2d/godot_step_2d.h @@ -55,7 +55,7 @@ class GodotStep2D { void _check_suspend(LocalVector<GodotBody2D *> &p_body_island) const; public: - void step(GodotSpace2D *p_space, real_t p_delta, int p_iterations); + void step(GodotSpace2D *p_space, real_t p_delta); GodotStep2D(); ~GodotStep2D(); }; diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index 8a701466ed..5c25ba9537 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -44,11 +44,6 @@ void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_in } void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { - // check if we already have the contact - - //Vector3 local_A = A->get_inv_transform().xform(p_point_A); - //Vector3 local_B = B->get_inv_transform().xform(p_point_B); - Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A); Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B); @@ -57,19 +52,14 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); Contact contact; - - contact.acc_normal_impulse = 0; - contact.acc_bias_impulse = 0; - contact.acc_bias_impulse_center_of_mass = 0; - contact.acc_tangent_impulse = Vector3(); contact.index_A = p_index_A; contact.index_B = p_index_B; contact.local_A = local_A; contact.local_B = local_B; contact.normal = (p_point_A - p_point_B).normalized(); - contact.mass_normal = 0; // will be computed in setup() + contact.used = true; - // attempt to determine if the contact will be reused + // Attempt to determine if the contact will be reused. real_t contact_recycle_radius = space->get_contact_recycle_radius(); for (int i = 0; i < contact_count; i++) { @@ -80,23 +70,34 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind contact.acc_bias_impulse = c.acc_bias_impulse; contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; contact.acc_tangent_impulse = c.acc_tangent_impulse; - new_index = i; - break; + c = contact; + return; } } - // figure out if the contact amount must be reduced to fit the new contact - + // Figure out if the contact amount must be reduced to fit the new contact. if (new_index == MAX_CONTACTS) { - // remove the contact with the minimum depth + // Remove the contact with the minimum depth. + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; int least_deep = -1; - real_t min_depth = 1e10; + real_t min_depth; + + // Start with depth for new contact. + { + Vector3 global_A = basis_A.xform(contact.local_A); + Vector3 global_B = basis_B.xform(contact.local_B) + offset_B; + + Vector3 axis = global_A - global_B; + min_depth = axis.dot(contact.normal); + } - for (int i = 0; i <= contact_count; i++) { - Contact &c = (i == contact_count) ? contact : contacts[i]; - Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; + for (int i = 0; i < contact_count; i++) { + const Contact &c = contacts[i]; + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; Vector3 axis = global_A - global_B; real_t depth = axis.dot(c.normal); @@ -107,10 +108,8 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind } } - ERR_FAIL_COND(least_deep == -1); - - if (least_deep < contact_count) { //replace the last deep contact by the new one - + if (least_deep > -1) { + // Replace the least deep contact by the new one. contacts[least_deep] = contact; } @@ -118,29 +117,41 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind } contacts[new_index] = contact; - - if (new_index == contact_count) { - contact_count++; - } + contact_count++; } void GodotBodyPair3D::validate_contacts() { - //make sure to erase contacts that are no longer valid + // Make sure to erase contacts that are no longer valid. + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation * max_separation; + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; - real_t contact_max_separation = space->get_contact_max_separation(); for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); + bool erase = false; + if (!c.used) { + // Was left behind in previous frame. + erase = true; + } else { + c.used = false; + + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); - if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { - // contact no longer needed, remove + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase = true; + } + } + if (erase) { + // Contact no longer needed, remove. if ((i + 1) < contact_count) { - // swap with the last one + // Swap with the last one. SWAP(contacts[i], contacts[contact_count - 1]); } @@ -260,7 +271,7 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) { real_t max_penetration = space->get_contact_max_allowed_penetration(); - real_t bias = (real_t)0.3; + real_t bias = 0.8; GodotShape3D *shape_A_ptr = A->get_shape(shape_A); GodotShape3D *shape_B_ptr = B->get_shape(shape_B); @@ -353,8 +364,6 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) { if (collide_B) { B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); } - c.acc_bias_impulse = 0; - c.acc_bias_impulse_center_of_mass = 0; c.bounce = combine_bounce(A, B); if (c.bounce) { @@ -538,14 +547,10 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i Contact contact; contact.index_A = p_index_A; contact.index_B = p_index_B; - contact.acc_normal_impulse = 0; - contact.acc_bias_impulse = 0; - contact.acc_bias_impulse_center_of_mass = 0; - contact.acc_tangent_impulse = Vector3(); contact.local_A = local_A; contact.local_B = local_B; contact.normal = (p_point_A - p_point_B).normalized(); - contact.mass_normal = 0; + contact.used = true; // Attempt to determine if the contact will be reused. real_t contact_recycle_radius = space->get_contact_recycle_radius(); @@ -571,20 +576,33 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i void GodotBodySoftBodyPair3D::validate_contacts() { // Make sure to erase contacts that are no longer valid. - const Transform3D &transform_A = body->get_transform(); + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation * max_separation; - real_t contact_max_separation = space->get_contact_max_separation(); + const Transform3D &transform_A = body->get_transform(); uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; - Vector3 global_A = transform_A.xform(c.local_A); - Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); + bool erase = false; + if (!c.used) { + // Was left behind in previous frame. + erase = true; + } else { + c.used = false; + + Vector3 global_A = transform_A.xform(c.local_A); + Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase = true; + } + } - if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { + if (erase) { // Contact no longer needed, remove. if ((contact_index + 1) < contact_count) { // Swap with the last one. @@ -640,7 +658,7 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) { real_t max_penetration = space->get_contact_max_allowed_penetration(); - real_t bias = (real_t)0.3; + real_t bias = space->get_contact_bias(); GodotShape3D *shape_A_ptr = body->get_shape(body_shape); @@ -723,8 +741,6 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) { if (soft_body_collides) { soft_body->apply_node_impulse(c.index_B, j_vec); } - c.acc_bias_impulse = 0; - c.acc_bias_impulse_center_of_mass = 0; c.bounce = body->get_bounce(); diff --git a/servers/physics_3d/godot_body_pair_3d.h b/servers/physics_3d/godot_body_pair_3d.h index c0a2424e05..7c2c31704b 100644 --- a/servers/physics_3d/godot_body_pair_3d.h +++ b/servers/physics_3d/godot_body_pair_3d.h @@ -54,6 +54,7 @@ protected: real_t depth = 0.0; bool active = false; + bool used = false; Vector3 rA, rB; // Offset in world orientation with respect to center of mass }; diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp index 9acae62382..573a5d373f 100644 --- a/servers/physics_3d/godot_physics_server_3d.cpp +++ b/servers/physics_3d/godot_physics_server_3d.cpp @@ -1574,20 +1574,15 @@ void GodotPhysicsServer3D::free(RID p_rid) { } else { ERR_FAIL_MSG("Invalid ID."); } -}; +} void GodotPhysicsServer3D::set_active(bool p_active) { active = p_active; -}; - -void GodotPhysicsServer3D::set_collision_iterations(int p_iterations) { - iterations = p_iterations; -}; +} void GodotPhysicsServer3D::init() { - iterations = 8; // 8? stepper = memnew(GodotStep3D); -}; +} void GodotPhysicsServer3D::step(real_t p_step) { #ifndef _3D_DISABLED @@ -1602,7 +1597,7 @@ void GodotPhysicsServer3D::step(real_t p_step) { active_objects = 0; collision_pairs = 0; for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((GodotSpace3D *)E->get(), p_step, iterations); + stepper->step((GodotSpace3D *)E->get(), p_step); island_count += E->get()->get_island_count(); active_objects += E->get()->get_active_objects(); collision_pairs += E->get()->get_collision_pairs(); @@ -1612,7 +1607,7 @@ void GodotPhysicsServer3D::step(real_t p_step) { void GodotPhysicsServer3D::sync() { doing_sync = true; -}; +} void GodotPhysicsServer3D::flush_queries() { #ifndef _3D_DISABLED @@ -1665,15 +1660,15 @@ void GodotPhysicsServer3D::flush_queries() { EngineDebugger::profiler_add_frame_data("servers", values); } #endif -}; +} void GodotPhysicsServer3D::end_sync() { doing_sync = false; -}; +} void GodotPhysicsServer3D::finish() { memdelete(stepper); -}; +} int GodotPhysicsServer3D::get_process_info(ProcessInfo p_info) { switch (p_info) { diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h index f5c8e0f60d..be9bbea76b 100644 --- a/servers/physics_3d/godot_physics_server_3d.h +++ b/servers/physics_3d/godot_physics_server_3d.h @@ -44,7 +44,6 @@ class GodotPhysicsServer3D : public PhysicsServer3D { friend class GodotPhysicsDirectSpaceState3D; bool active = true; - int iterations = 0; int island_count = 0; int active_objects = 0; @@ -364,8 +363,6 @@ 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_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp index f503273c88..b2a8b00bca 100644 --- a/servers/physics_3d/godot_space_3d.cpp +++ b/servers/physics_3d/godot_space_3d.cpp @@ -1173,9 +1173,12 @@ void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_v case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break; - case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + contact_bias = p_value; + break; case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: body_linear_velocity_sleep_threshold = p_value; break; @@ -1191,6 +1194,9 @@ void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_v case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break; + case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: + solver_iterations = p_value; + break; } } @@ -1200,8 +1206,10 @@ real_t GodotSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const { return contact_recycle_radius; case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation; - case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration; + case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + return contact_bias; case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: return body_linear_velocity_sleep_threshold; case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: @@ -1212,6 +1220,8 @@ real_t GodotSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const { return body_angular_velocity_damp_ratio; case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias; + case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: + return solver_iterations; } return 0; } diff --git a/servers/physics_3d/godot_space_3d.h b/servers/physics_3d/godot_space_3d.h index aa5e965751..b9aeee7583 100644 --- a/servers/physics_3d/godot_space_3d.h +++ b/servers/physics_3d/godot_space_3d.h @@ -93,9 +93,12 @@ private: GodotArea3D *area = nullptr; + int solver_iterations = 16; + real_t contact_recycle_radius = 0.01; real_t contact_max_separation = 0.05; real_t contact_max_allowed_penetration = 0.01; + real_t contact_bias = 0.8; real_t constraint_bias = 0.01; enum { @@ -159,9 +162,11 @@ public: void remove_object(GodotCollisionObject3D *p_object); const Set<GodotCollisionObject3D *> &get_objects() const; + _FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; } _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } + _FORCE_INLINE_ real_t get_contact_bias() const { return contact_bias; } _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } diff --git a/servers/physics_3d/godot_step_3d.cpp b/servers/physics_3d/godot_step_3d.cpp index 5453c4415c..6332532f6e 100644 --- a/servers/physics_3d/godot_step_3d.cpp +++ b/servers/physics_3d/godot_step_3d.cpp @@ -181,14 +181,14 @@ void GodotStep3D::_check_suspend(const LocalVector<GodotBody3D *> &p_body_island } } -void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) { +void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { p_space->lock(); // can't access space during this p_space->setup(); //update inertias, etc p_space->set_last_step(p_delta); - iterations = p_iterations; + iterations = p_space->get_solver_iterations(); delta = p_delta; const SelfList<GodotBody3D>::List *body_list = &p_space->get_active_body_list(); diff --git a/servers/physics_3d/godot_step_3d.h b/servers/physics_3d/godot_step_3d.h index 23ede4feff..10389713f6 100644 --- a/servers/physics_3d/godot_step_3d.h +++ b/servers/physics_3d/godot_step_3d.h @@ -56,7 +56,7 @@ class GodotStep3D { void _check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const; public: - void step(GodotSpace3D *p_space, real_t p_delta, int p_iterations); + void step(GodotSpace3D *p_space, real_t p_delta); GodotStep3D(); ~GodotStep3D(); }; diff --git a/servers/physics_server_2d.cpp b/servers/physics_server_2d.cpp index 76f0b74c72..c660bd4d69 100644 --- a/servers/physics_server_2d.cpp +++ b/servers/physics_server_2d.cpp @@ -730,17 +730,17 @@ void PhysicsServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_active", "active"), &PhysicsServer2D::set_active); - ClassDB::bind_method(D_METHOD("set_collision_iterations", "iterations"), &PhysicsServer2D::set_collision_iterations); - ClassDB::bind_method(D_METHOD("get_process_info", "process_info"), &PhysicsServer2D::get_process_info); BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_RECYCLE_RADIUS); BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_MAX_SEPARATION); - BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION); + BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION); + BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_DEFAULT_BIAS); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP); BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS); + BIND_ENUM_CONSTANT(SPACE_PARAM_SOLVER_ITERATIONS); BIND_ENUM_CONSTANT(SHAPE_WORLD_BOUNDARY); BIND_ENUM_CONSTANT(SHAPE_SEPARATION_RAY); diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h index c76ec5a5b1..6625be6d14 100644 --- a/servers/physics_server_2d.h +++ b/servers/physics_server_2d.h @@ -242,11 +242,13 @@ public: enum SpaceParameter { SPACE_PARAM_CONTACT_RECYCLE_RADIUS, SPACE_PARAM_CONTACT_MAX_SEPARATION, - SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION, + SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION, + SPACE_PARAM_CONTACT_DEFAULT_BIAS, SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD, SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD, SPACE_PARAM_BODY_TIME_TO_SLEEP, SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS, + SPACE_PARAM_SOLVER_ITERATIONS, }; virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0; @@ -566,8 +568,6 @@ public: virtual bool is_flushing_queries() const = 0; - virtual void set_collision_iterations(int p_iterations) = 0; - enum ProcessInfo { INFO_ACTIVE_OBJECTS, INFO_COLLISION_PAIRS, diff --git a/servers/physics_server_2d_wrap_mt.h b/servers/physics_server_2d_wrap_mt.h index dda4eb6ffa..f65c8921ce 100644 --- a/servers/physics_server_2d_wrap_mt.h +++ b/servers/physics_server_2d_wrap_mt.h @@ -294,7 +294,6 @@ 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_server_3d.cpp b/servers/physics_server_3d.cpp index 373f216e01..658f7daf9f 100644 --- a/servers/physics_server_3d.cpp +++ b/servers/physics_server_3d.cpp @@ -890,8 +890,6 @@ void PhysicsServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_active", "active"), &PhysicsServer3D::set_active); - ClassDB::bind_method(D_METHOD("set_collision_iterations", "iterations"), &PhysicsServer3D::set_collision_iterations); - ClassDB::bind_method(D_METHOD("get_process_info", "process_info"), &PhysicsServer3D::get_process_info); BIND_ENUM_CONSTANT(SHAPE_WORLD_BOUNDARY); @@ -963,12 +961,14 @@ void PhysicsServer3D::_bind_methods() { BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_RECYCLE_RADIUS); BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_MAX_SEPARATION); - BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION); + BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION); + BIND_ENUM_CONSTANT(SPACE_PARAM_CONTACT_DEFAULT_BIAS); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO); BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS); + BIND_ENUM_CONSTANT(SPACE_PARAM_SOLVER_ITERATIONS); BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_X); BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_Y); diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h index bd5a3eb4b7..dfaefc5fbf 100644 --- a/servers/physics_server_3d.h +++ b/servers/physics_server_3d.h @@ -263,12 +263,14 @@ public: enum SpaceParameter { SPACE_PARAM_CONTACT_RECYCLE_RADIUS, SPACE_PARAM_CONTACT_MAX_SEPARATION, - SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION, + SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION, + SPACE_PARAM_CONTACT_DEFAULT_BIAS, SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD, SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD, SPACE_PARAM_BODY_TIME_TO_SLEEP, SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO, SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS, + SPACE_PARAM_SOLVER_ITERATIONS, }; virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0; @@ -768,8 +770,6 @@ public: virtual bool is_flushing_queries() const = 0; - virtual void set_collision_iterations(int p_iterations) = 0; - enum ProcessInfo { INFO_ACTIVE_OBJECTS, INFO_COLLISION_PAIRS, diff --git a/servers/physics_server_3d_wrap_mt.h b/servers/physics_server_3d_wrap_mt.h index 507427ecec..e6dc2d8ed9 100644 --- a/servers/physics_server_3d_wrap_mt.h +++ b/servers/physics_server_3d_wrap_mt.h @@ -370,7 +370,6 @@ 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; |