diff options
Diffstat (limited to 'servers/physics_2d')
31 files changed, 477 insertions, 285 deletions
diff --git a/servers/physics_2d/godot_area_2d.cpp b/servers/physics_2d/godot_area_2d.cpp index c4060615c9..9937178550 100644 --- a/servers/physics_2d/godot_area_2d.cpp +++ b/servers/physics_2d/godot_area_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_area_2d.h b/servers/physics_2d/godot_area_2d.h index 699c1c1bc8..6e8078909b 100644 --- a/servers/physics_2d/godot_area_2d.h +++ b/servers/physics_2d/godot_area_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_area_pair_2d.cpp b/servers/physics_2d/godot_area_pair_2d.cpp index bde22aab11..72208d7dff 100644 --- a/servers/physics_2d/godot_area_pair_2d.cpp +++ b/servers/physics_2d/godot_area_pair_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_area_pair_2d.h b/servers/physics_2d/godot_area_pair_2d.h index f1290a27d0..ef6c774bc3 100644 --- a/servers/physics_2d/godot_area_pair_2d.h +++ b/servers/physics_2d/godot_area_pair_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp index 9b97583300..6873504f70 100644 --- a/servers/physics_2d/godot_body_2d.cpp +++ b/servers/physics_2d/godot_body_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -185,6 +185,9 @@ void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Varian _update_transform_dependent(); } break; case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { + if (Math::is_zero_approx(gravity_scale)) { + wakeup(); + } gravity_scale = p_value; } break; case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: { @@ -546,6 +549,9 @@ void GodotBody2D::integrate_forces(real_t p_step) { gravity *= gravity_scale; + prev_linear_velocity = linear_velocity; + prev_angular_velocity = angular_velocity; + Vector2 motion; bool do_motion = false; @@ -563,9 +569,8 @@ void GodotBody2D::integrate_forces(real_t p_step) { if (!omit_force_integration) { //overridden by direct state query - Vector2 force = gravity * mass; - force += applied_force; - real_t torque = applied_torque; + Vector2 force = gravity * mass + applied_force + constant_force; + real_t torque = applied_torque + constant_torque; real_t damp = 1.0 - p_step * total_linear_damp; @@ -592,7 +597,10 @@ void GodotBody2D::integrate_forces(real_t p_step) { } } - biased_angular_velocity = 0; + applied_force = Vector2(); + applied_torque = 0.0; + + biased_angular_velocity = 0.0; biased_linear_velocity = Vector2(); if (do_motion) { //shapes temporarily extend for raycast diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h index d1dbf92c1b..1335a19126 100644 --- a/servers/physics_2d/godot_body_2d.h +++ b/servers/physics_2d/godot_body_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -50,6 +50,9 @@ class GodotBody2D : public GodotCollisionObject2D { Vector2 linear_velocity; real_t angular_velocity = 0.0; + Vector2 prev_linear_velocity; + real_t prev_angular_velocity = 0.0; + Vector2 constant_linear_velocity; real_t constant_angular_velocity = 0.0; @@ -86,6 +89,9 @@ class GodotBody2D : public GodotCollisionObject2D { Vector2 applied_force; real_t applied_torque = 0.0; + Vector2 constant_force; + real_t constant_torque = 0.0; + SelfList<GodotBody2D> active_list; SelfList<GodotBody2D> mass_properties_update_list; SelfList<GodotBody2D> direct_state_query_list; @@ -169,7 +175,7 @@ public: if (index > -1) { areas.write[index].refCount -= 1; if (areas[index].refCount < 1) { - areas.remove(index); + areas.remove_at(index); } } } @@ -209,6 +215,9 @@ public: _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; } _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; } + _FORCE_INLINE_ Vector2 get_prev_linear_velocity() const { return prev_linear_velocity; } + _FORCE_INLINE_ real_t get_prev_angular_velocity() const { return prev_angular_velocity; } + _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; } _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; } @@ -228,11 +237,49 @@ 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; + } + } + + _FORCE_INLINE_ void apply_central_force(const Vector2 &p_force) { + applied_force += p_force; + } + + _FORCE_INLINE_ void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { + applied_force += p_force; + applied_torque += (p_position - center_of_mass).cross(p_force); } + _FORCE_INLINE_ void apply_torque(real_t p_torque) { + applied_torque += p_torque; + } + + _FORCE_INLINE_ void add_constant_central_force(const Vector2 &p_force) { + constant_force += p_force; + } + + _FORCE_INLINE_ void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { + constant_force += p_force; + constant_torque += (p_position - center_of_mass).cross(p_force); + } + + _FORCE_INLINE_ void add_constant_torque(real_t p_torque) { + constant_torque += p_torque; + } + + void set_constant_force(const Vector2 &p_force) { constant_force = p_force; } + Vector2 get_constant_force() const { return constant_force; } + + void set_constant_torque(real_t p_torque) { constant_torque = p_torque; } + real_t get_constant_torque() const { return constant_torque; } + void set_active(bool p_active); _FORCE_INLINE_ bool is_active() const { return active; } @@ -252,25 +299,6 @@ public: void set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant); Variant get_state(PhysicsServer2D::BodyState p_state) const; - void set_applied_force(const Vector2 &p_force) { applied_force = p_force; } - Vector2 get_applied_force() const { return applied_force; } - - void set_applied_torque(real_t p_torque) { applied_torque = p_torque; } - real_t get_applied_torque() const { return applied_torque; } - - _FORCE_INLINE_ void add_central_force(const Vector2 &p_force) { - applied_force += p_force; - } - - _FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { - applied_force += p_force; - applied_torque += (p_position - center_of_mass).cross(p_force); - } - - _FORCE_INLINE_ void add_torque(real_t p_torque) { - applied_torque += p_torque; - } - _FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; } _FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; } diff --git a/servers/physics_2d/godot_body_direct_state_2d.cpp b/servers/physics_2d/godot_body_direct_state_2d.cpp index 9c9bd56268..cde6e8c991 100644 --- a/servers/physics_2d/godot_body_direct_state_2d.cpp +++ b/servers/physics_2d/godot_body_direct_state_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -92,34 +92,71 @@ Vector2 GodotPhysicsDirectBodyState2D::get_velocity_at_local_position(const Vect return body->get_velocity_in_local_point(p_position); } -void GodotPhysicsDirectBodyState2D::add_central_force(const Vector2 &p_force) { +void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) { body->wakeup(); - body->add_central_force(p_force); + body->apply_central_impulse(p_impulse); } -void GodotPhysicsDirectBodyState2D::add_force(const Vector2 &p_force, const Vector2 &p_position) { +void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { body->wakeup(); - body->add_force(p_force, p_position); + body->apply_impulse(p_impulse, p_position); } -void GodotPhysicsDirectBodyState2D::add_torque(real_t p_torque) { +void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) { body->wakeup(); - body->add_torque(p_torque); + body->apply_torque_impulse(p_torque); } -void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) { +void GodotPhysicsDirectBodyState2D::apply_central_force(const Vector2 &p_force) { body->wakeup(); - body->apply_central_impulse(p_impulse); + body->apply_central_force(p_force); } -void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { +void GodotPhysicsDirectBodyState2D::apply_force(const Vector2 &p_force, const Vector2 &p_position) { body->wakeup(); - body->apply_impulse(p_impulse, p_position); + body->apply_force(p_force, p_position); } -void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) { +void GodotPhysicsDirectBodyState2D::apply_torque(real_t p_torque) { body->wakeup(); - body->apply_torque_impulse(p_torque); + body->apply_torque(p_torque); +} + +void GodotPhysicsDirectBodyState2D::add_constant_central_force(const Vector2 &p_force) { + body->wakeup(); + body->add_constant_central_force(p_force); +} + +void GodotPhysicsDirectBodyState2D::add_constant_force(const Vector2 &p_force, const Vector2 &p_position) { + body->wakeup(); + body->add_constant_force(p_force, p_position); +} + +void GodotPhysicsDirectBodyState2D::add_constant_torque(real_t p_torque) { + body->wakeup(); + body->add_constant_torque(p_torque); +} + +void GodotPhysicsDirectBodyState2D::set_constant_force(const Vector2 &p_force) { + if (!p_force.is_equal_approx(Vector2())) { + body->wakeup(); + } + body->set_constant_force(p_force); +} + +Vector2 GodotPhysicsDirectBodyState2D::get_constant_force() const { + return body->get_constant_force(); +} + +void GodotPhysicsDirectBodyState2D::set_constant_torque(real_t p_torque) { + if (!Math::is_zero_approx(p_torque)) { + body->wakeup(); + } + body->set_constant_torque(p_torque); +} + +real_t GodotPhysicsDirectBodyState2D::get_constant_torque() const { + return body->get_constant_torque(); } void GodotPhysicsDirectBodyState2D::set_sleep_state(bool p_enable) { diff --git a/servers/physics_2d/godot_body_direct_state_2d.h b/servers/physics_2d/godot_body_direct_state_2d.h index ff25205d52..a2c0a87989 100644 --- a/servers/physics_2d/godot_body_direct_state_2d.h +++ b/servers/physics_2d/godot_body_direct_state_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -61,13 +61,24 @@ public: virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override; - virtual void add_central_force(const Vector2 &p_force) override; - virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; - virtual void add_torque(real_t p_torque) override; virtual void apply_central_impulse(const Vector2 &p_impulse) override; virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; virtual void apply_torque_impulse(real_t p_torque) override; + virtual void apply_central_force(const Vector2 &p_force) override; + virtual void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void apply_torque(real_t p_torque) override; + + virtual void add_constant_central_force(const Vector2 &p_force) override; + virtual void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void add_constant_torque(real_t p_torque) override; + + virtual void set_constant_force(const Vector2 &p_force) override; + virtual Vector2 get_constant_force() const override; + + virtual void set_constant_torque(real_t p_torque) override; + virtual real_t get_constant_torque() const override; + virtual void set_sleep_state(bool p_enable) override; virtual bool is_sleeping() const override; diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp index 97eeefbfe6..1986191cc3 100644 --- a/servers/physics_2d/godot_body_pair_2d.cpp +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -32,9 +32,11 @@ #include "godot_collision_solver_2d.h" #include "godot_space_2d.h" -#define POSITION_CORRECTION #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; @@ -42,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); @@ -52,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; @@ -104,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; } @@ -115,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; @@ -134,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; @@ -151,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]); } @@ -164,7 +160,7 @@ void GodotBodyPair2D::_validate_contacts() { } } -bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) { +bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B) { Vector2 motion = p_A->get_linear_velocity() * p_step; real_t mlen = motion.length(); if (mlen < CMP_EPSILON) { @@ -175,14 +171,18 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, real_t min, max; p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max); - bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction - if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis + // Did it move enough in this direction to even attempt raycast? + // Let's say it should move more than 1/3 the size of the object in that axis. + bool fast_object = mlen > (max - min) * 0.3; + if (!fast_object) { return false; } - //cast a segment from support in motion normal, in the same direction of motion by motion length - //support is the worst case collision point, so real collision happened before + // Going too fast in that direction. + + // Cast a segment from support in motion normal, in the same direction of motion by motion length. + // Support is the worst case collision point, so real collision happened before. int a; Vector2 s[2]; p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a); @@ -191,7 +191,8 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, Transform2D from_inv = p_xform_B.affine_inverse(); - Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box + // Start from a little inside the bounding box. + Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); Vector2 local_to = from_inv.xform(to); Vector2 rpos, rnorm; @@ -199,20 +200,22 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, return false; } - //ray hit something + // Check one-way collision based on motion direction. + if (p_A->get_shape(p_shape_A)->allows_one_way_collision() && p_B->is_shape_set_as_one_way_collision(p_shape_B)) { + Vector2 direction = p_xform_B.get_axis(1).normalized(); + if (direction.dot(mnormal) < CMP_EPSILON) { + collided = false; + oneway_disabled = true; + return false; + } + } + // Shorten the linear velocity so it does not hit, but gets close enough, + // next frame will hit softly or soft enough. Vector2 hitpos = p_xform_B.xform(rpos); - Vector2 contact_A = to; - Vector2 contact_B = hitpos; - - //create a contact - - if (p_swap_result) { - _contact_added_callback(contact_B, contact_A); - } else { - _contact_added_callback(contact_A, contact_B); - } + real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01; + p_A->set_linear_velocity(mnormal * (newlen / p_step)); return true; } @@ -226,6 +229,8 @@ real_t combine_friction(GodotBody2D *A, GodotBody2D *B) { } bool GodotBodyPair2D::setup(real_t p_step) { + check_ccd = false; + if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; @@ -273,24 +278,19 @@ bool GodotBodyPair2D::setup(real_t p_step) { collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); if (!collided) { - //test ccd (currently just a raycast) + oneway_disabled = false; if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { - if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { - collided = true; - } + check_ccd = true; + return true; } if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { - if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { - collided = true; - } + check_ccd = true; + return true; } - if (!collided) { - oneway_disabled = false; - return false; - } + return false; } if (oneway_disabled) { @@ -303,9 +303,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; } @@ -324,9 +321,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; } @@ -345,13 +339,35 @@ bool GodotBodyPair2D::setup(real_t p_step) { } bool GodotBodyPair2D::pre_solve(real_t p_step) { - if (!collided || oneway_disabled) { + if (oneway_disabled) { + return false; + } + + if (!collided) { + if (check_ccd) { + 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] -= offset_A; + Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { + _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); + } + + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { + _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); + } + } + return false; } 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); @@ -390,7 +406,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; } @@ -401,8 +417,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); @@ -435,7 +451,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 { @@ -443,19 +458,19 @@ 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 c.bounce = combine_bounce(A, B); if (c.bounce) { - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); - Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); - Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + Vector2 crA(-A->get_prev_angular_velocity() * c.rA.y, A->get_prev_angular_velocity() * c.rA.x); + Vector2 crB(-B->get_prev_angular_velocity() * c.rB.y, B->get_prev_angular_velocity() * c.rB.x); + Vector2 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA; c.bounce = c.bounce * dv.dot(c.normal); } @@ -471,6 +486,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]; @@ -490,6 +510,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); @@ -500,10 +521,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; @@ -520,10 +562,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..1c0f61be26 100644 --- a/servers/physics_2d/godot_body_pair_2d.h +++ b/servers/physics_2d/godot_body_pair_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -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; }; @@ -78,10 +79,11 @@ class GodotBodyPair2D : public GodotConstraint2D { Contact contacts[MAX_CONTACTS]; int contact_count = 0; bool collided = false; + bool check_ccd = false; bool oneway_disabled = false; bool report_contacts_only = false; - bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); + bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B); void _validate_contacts(); static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self); _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); diff --git a/servers/physics_2d/godot_broad_phase_2d.cpp b/servers/physics_2d/godot_broad_phase_2d.cpp index 4b35f8d996..e734c24f88 100644 --- a/servers/physics_2d/godot_broad_phase_2d.cpp +++ b/servers/physics_2d/godot_broad_phase_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_broad_phase_2d.h b/servers/physics_2d/godot_broad_phase_2d.h index 7017a6e41f..abab087045 100644 --- a/servers/physics_2d/godot_broad_phase_2d.h +++ b/servers/physics_2d/godot_broad_phase_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp index 9ec6b0a6b7..5a96dae8ca 100644 --- a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp +++ b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.h b/servers/physics_2d/godot_broad_phase_2d_bvh.h index 19b49f3499..d77e0574eb 100644 --- a/servers/physics_2d/godot_broad_phase_2d_bvh.h +++ b/servers/physics_2d/godot_broad_phase_2d_bvh.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_collision_object_2d.cpp b/servers/physics_2d/godot_collision_object_2d.cpp index 3d4ebbedcd..35027e0f69 100644 --- a/servers/physics_2d/godot_collision_object_2d.cpp +++ b/servers/physics_2d/godot_collision_object_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -121,7 +121,7 @@ void GodotCollisionObject2D::remove_shape(int p_index) { shapes.write[i].bpid = 0; } shapes[p_index].shape->remove_owner(this); - shapes.remove(p_index); + shapes.remove_at(p_index); if (!pending_shape_update_list.in_list()) { GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); diff --git a/servers/physics_2d/godot_collision_object_2d.h b/servers/physics_2d/godot_collision_object_2d.h index 7233857808..1e9baad8d9 100644 --- a/servers/physics_2d/godot_collision_object_2d.h +++ b/servers/physics_2d/godot_collision_object_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_collision_solver_2d.cpp b/servers/physics_2d/godot_collision_solver_2d.cpp index 25371b9885..263d6d939f 100644 --- a/servers/physics_2d/godot_collision_solver_2d.cpp +++ b/servers/physics_2d/godot_collision_solver_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_collision_solver_2d.h b/servers/physics_2d/godot_collision_solver_2d.h index f10815a444..bd90641f04 100644 --- a/servers/physics_2d/godot_collision_solver_2d.h +++ b/servers/physics_2d/godot_collision_solver_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_collision_solver_2d_sat.cpp b/servers/physics_2d/godot_collision_solver_2d_sat.cpp index 63053e8259..a965795bee 100644 --- a/servers/physics_2d/godot_collision_solver_2d_sat.cpp +++ b/servers/physics_2d/godot_collision_solver_2d_sat.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_collision_solver_2d_sat.h b/servers/physics_2d/godot_collision_solver_2d_sat.h index 1517b90a19..3d8e29c41a 100644 --- a/servers/physics_2d/godot_collision_solver_2d_sat.h +++ b/servers/physics_2d/godot_collision_solver_2d_sat.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_constraint_2d.h b/servers/physics_2d/godot_constraint_2d.h index 84f975e583..d9bf035c2b 100644 --- a/servers/physics_2d/godot_constraint_2d.h +++ b/servers/physics_2d/godot_constraint_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_joints_2d.cpp b/servers/physics_2d/godot_joints_2d.cpp index 7c08c2f4b4..0876184d8c 100644 --- a/servers/physics_2d/godot_joints_2d.cpp +++ b/servers/physics_2d/godot_joints_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_joints_2d.h b/servers/physics_2d/godot_joints_2d.h index 4c97190d01..acaaf0f629 100644 --- a/servers/physics_2d/godot_joints_2d.h +++ b/servers/physics_2d/godot_joints_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_physics_server_2d.cpp b/servers/physics_2d/godot_physics_server_2d.cpp index 8ac27077fc..5e099e27ec 100644 --- a/servers/physics_2d/godot_physics_server_2d.cpp +++ b/servers/physics_2d/godot_physics_server_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -669,68 +669,68 @@ void GodotPhysicsServer2D::body_attach_object_instance_id(RID p_body, ObjectID p ERR_FAIL_COND(!body); body->set_instance_id(p_id); -}; +} ObjectID GodotPhysicsServer2D::body_get_object_instance_id(RID p_body) const { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, ObjectID()); return body->get_instance_id(); -}; +} void GodotPhysicsServer2D::body_attach_canvas_instance_id(RID p_body, ObjectID p_id) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_canvas_instance_id(p_id); -}; +} ObjectID GodotPhysicsServer2D::body_get_canvas_instance_id(RID p_body) const { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, ObjectID()); return body->get_canvas_instance_id(); -}; +} void GodotPhysicsServer2D::body_set_collision_layer(RID p_body, uint32_t p_layer) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_collision_layer(p_layer); -}; +} uint32_t GodotPhysicsServer2D::body_get_collision_layer(RID p_body) const { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return body->get_collision_layer(); -}; +} void GodotPhysicsServer2D::body_set_collision_mask(RID p_body, uint32_t p_mask) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_collision_mask(p_mask); -}; +} uint32_t GodotPhysicsServer2D::body_get_collision_mask(RID p_body) const { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return body->get_collision_mask(); -}; +} void GodotPhysicsServer2D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_param(p_param, p_value); -}; +} Variant GodotPhysicsServer2D::body_get_param(RID p_body, BodyParameter p_param) const { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return body->get_param(p_param); -}; +} void GodotPhysicsServer2D::body_reset_mass_properties(RID p_body) { GodotBody2D *body = body_owner.get_or_null(p_body); @@ -744,95 +744,123 @@ void GodotPhysicsServer2D::body_set_state(RID p_body, BodyState p_state, const V ERR_FAIL_COND(!body); body->set_state(p_state, p_variant); -}; +} Variant GodotPhysicsServer2D::body_get_state(RID p_body, BodyState p_state) const { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, Variant()); return body->get_state(p_state); -}; +} -void GodotPhysicsServer2D::body_set_applied_force(RID p_body, const Vector2 &p_force) { +void GodotPhysicsServer2D::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->set_applied_force(p_force); + body->apply_central_impulse(p_impulse); body->wakeup(); -}; +} -Vector2 GodotPhysicsServer2D::body_get_applied_force(RID p_body) const { +void GodotPhysicsServer2D::body_apply_torque_impulse(RID p_body, real_t p_torque) { GodotBody2D *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Vector2()); - return body->get_applied_force(); -}; + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_torque_impulse(p_torque); + body->wakeup(); +} -void GodotPhysicsServer2D::body_set_applied_torque(RID p_body, real_t p_torque) { +void GodotPhysicsServer2D::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->set_applied_torque(p_torque); + _update_shapes(); + + body->apply_impulse(p_impulse, p_position); body->wakeup(); -}; +} -real_t GodotPhysicsServer2D::body_get_applied_torque(RID p_body) const { +void GodotPhysicsServer2D::body_apply_central_force(RID p_body, const Vector2 &p_force) { GodotBody2D *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, 0); + ERR_FAIL_COND(!body); - return body->get_applied_torque(); -}; + body->apply_central_force(p_force); + body->wakeup(); +} -void GodotPhysicsServer2D::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) { +void GodotPhysicsServer2D::body_apply_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->apply_central_impulse(p_impulse); + body->apply_force(p_force, p_position); body->wakeup(); } -void GodotPhysicsServer2D::body_apply_torque_impulse(RID p_body, real_t p_torque) { +void GodotPhysicsServer2D::body_apply_torque(RID p_body, real_t p_torque) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - _update_shapes(); - - body->apply_torque_impulse(p_torque); + body->apply_torque(p_torque); body->wakeup(); } -void GodotPhysicsServer2D::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) { +void GodotPhysicsServer2D::body_add_constant_central_force(RID p_body, const Vector2 &p_force) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - _update_shapes(); - - body->apply_impulse(p_impulse, p_position); + body->add_constant_central_force(p_force); body->wakeup(); -}; +} -void GodotPhysicsServer2D::body_add_central_force(RID p_body, const Vector2 &p_force) { +void GodotPhysicsServer2D::body_add_constant_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->add_central_force(p_force); + body->add_constant_force(p_force, p_position); body->wakeup(); -}; +} -void GodotPhysicsServer2D::body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) { +void GodotPhysicsServer2D::body_add_constant_torque(RID p_body, real_t p_torque) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->add_force(p_force, p_position); + body->add_constant_torque(p_torque); body->wakeup(); -}; +} -void GodotPhysicsServer2D::body_add_torque(RID p_body, real_t p_torque) { +void GodotPhysicsServer2D::body_set_constant_force(RID p_body, const Vector2 &p_force) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->add_torque(p_torque); - body->wakeup(); -}; + body->set_constant_force(p_force); + if (!p_force.is_equal_approx(Vector2())) { + body->wakeup(); + } +} + +Vector2 GodotPhysicsServer2D::body_get_constant_force(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Vector2()); + return body->get_constant_force(); +} + +void GodotPhysicsServer2D::body_set_constant_torque(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_constant_torque(p_torque); + if (!Math::is_zero_approx(p_torque)) { + body->wakeup(); + } +} + +real_t GodotPhysicsServer2D::body_get_constant_torque(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_constant_torque(); +} void GodotPhysicsServer2D::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) { GodotBody2D *body = body_owner.get_or_null(p_body); @@ -1214,21 +1242,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 +1264,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 +1331,7 @@ void GodotPhysicsServer2D::end_sync() { void GodotPhysicsServer2D::finish() { memdelete(stepper); -}; +} void GodotPhysicsServer2D::_update_shapes() { while (pending_shape_update_list.first()) { @@ -1340,4 +1363,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..0a84caadc5 100644 --- a/servers/physics_2d/godot_physics_server_2d.h +++ b/servers/physics_2d/godot_physics_server_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -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; @@ -208,19 +207,24 @@ public: virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; virtual Variant body_get_state(RID p_body, BodyState p_state) const override; - virtual void body_set_applied_force(RID p_body, const Vector2 &p_force) override; - virtual Vector2 body_get_applied_force(RID p_body) const override; - - virtual void body_set_applied_torque(RID p_body, real_t p_torque) override; - virtual real_t body_get_applied_torque(RID p_body) const override; - - virtual void body_add_central_force(RID p_body, const Vector2 &p_force) override; - virtual void body_add_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; - virtual void body_add_torque(RID p_body, real_t p_torque) override; - virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) override; virtual void body_apply_torque_impulse(RID p_body, real_t p_torque) override; virtual void body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; + + virtual void body_apply_central_force(RID p_body, const Vector2 &p_force) override; + virtual void body_apply_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void body_apply_torque(RID p_body, real_t p_torque) override; + + virtual void body_add_constant_central_force(RID p_body, const Vector2 &p_force) override; + virtual void body_add_constant_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void body_add_constant_torque(RID p_body, real_t p_torque) override; + + virtual void body_set_constant_force(RID p_body, const Vector2 &p_force) override; + virtual Vector2 body_get_constant_force(RID p_body) const override; + + virtual void body_set_constant_torque(RID p_body, real_t p_torque) override; + virtual real_t body_get_constant_torque(RID p_body) const override; + virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) override; virtual void body_add_collision_exception(RID p_body, RID p_body_b) override; @@ -283,8 +287,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_shape_2d.cpp b/servers/physics_2d/godot_shape_2d.cpp index 3604004324..1e8799a727 100644 --- a/servers/physics_2d/godot_shape_2d.cpp +++ b/servers/physics_2d/godot_shape_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_shape_2d.h b/servers/physics_2d/godot_shape_2d.h index 25d113aafb..fb52cbce20 100644 --- a/servers/physics_2d/godot_shape_2d.h +++ b/servers/physics_2d/godot_shape_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ diff --git a/servers/physics_2d/godot_space_2d.cpp b/servers/physics_2d/godot_space_2d.cpp index 7a5eb26bb3..5c2bda340b 100644 --- a/servers/physics_2d/godot_space_2d.cpp +++ b/servers/physics_2d/godot_space_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -36,6 +36,7 @@ #include "core/os/os.h" #include "core/templates/pair.h" +#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001 #define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 _FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { @@ -439,9 +440,11 @@ bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_paramete GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); ERR_FAIL_COND_V(!shape, 0); + real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE); + Rect2 aabb = p_parameters.transform.xform(shape->get_aabb()); aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion - aabb = aabb.grow(p_parameters.margin); + aabb = aabb.grow(margin); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -449,7 +452,7 @@ bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_paramete // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. real_t motion_length = p_parameters.motion.length(); - real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); for (int i = 0; i < amount; i++) { @@ -469,7 +472,7 @@ bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_paramete rcd.object = col_obj; rcd.shape = shape_idx; rcd.local_shape = 0; - bool sc = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); + bool sc = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, margin); if (!sc) { continue; } @@ -540,6 +543,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: r_result->collider_id = ObjectID(); r_result->collider_shape = 0; } + Rect2 body_aabb; bool shapes_found = false; @@ -565,15 +569,17 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: return false; } + real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE); + // Undo the currently transform the physics server is aware of and apply the provided one body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_parameters.margin); + body_aabb = body_aabb.grow(margin); static const int max_excluded_shape_pairs = 32; ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs]; int excluded_shape_pair_count = 0; - real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; real_t motion_length = p_parameters.motion.length(); Vector2 motion_normal = p_parameters.motion / motion_length; @@ -630,7 +636,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); - cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work + cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work cbk.invalid_by_dir = 0; if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { @@ -655,7 +661,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: bool did_collide = false; GodotShape2D *against_shape = col_obj->get_shape(shape_idx); - if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) { + if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, margin)) { did_collide = cbk.passed > current_passed; //more passed, so collision actually existed } @@ -927,7 +933,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); - rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work + rcd.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj); @@ -949,7 +955,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: rcd.object = col_obj; rcd.shape = shape_idx; rcd.local_shape = j; - bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); + bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, margin); if (!sc) { continue; } @@ -1138,9 +1144,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 +1162,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 +1174,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 +1186,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; } @@ -1198,6 +1214,24 @@ GodotSpace2D::GodotSpace2D() { body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5); ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/2d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); + solver_iterations = GLOBAL_DEF("physics/2d/solver/solver_iterations", 16); + ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/solver_iterations", PropertyInfo(Variant::INT, "physics/2d/solver/solver_iterations", PROPERTY_HINT_RANGE, "1,32,1,or_greater")); + + contact_recycle_radius = GLOBAL_DEF("physics/2d/solver/contact_recycle_radius", 1.0); + ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/contact_recycle_radius", PropertyInfo(Variant::FLOAT, "physics/2d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,10,0.01,or_greater")); + + contact_max_separation = GLOBAL_DEF("physics/2d/solver/contact_max_separation", 1.5); + ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/contact_max_separation", PropertyInfo(Variant::FLOAT, "physics/2d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,10,0.01,or_greater")); + + contact_max_allowed_penetration = GLOBAL_DEF("physics/2d/solver/contact_max_allowed_penetration", 0.3); + ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/contact_max_allowed_penetration", PropertyInfo(Variant::FLOAT, "physics/2d/solver/contact_max_allowed_penetration", PROPERTY_HINT_RANGE, "0,10,0.01,or_greater")); + + contact_bias = GLOBAL_DEF("physics/2d/solver/default_contact_bias", 0.8); + ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/default_contact_bias", PropertyInfo(Variant::FLOAT, "physics/2d/solver/default_contact_bias", PROPERTY_HINT_RANGE, "0,1,0.01")); + + constraint_bias = GLOBAL_DEF("physics/2d/solver/default_constraint_bias", 0.2); + ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/solver/default_constraint_bias", PropertyInfo(Variant::FLOAT, "physics/2d/solver/default_constraint_bias", PROPERTY_HINT_RANGE, "0,1,0.01")); + broadphase = GodotBroadPhase2D::create_func(); broadphase->set_pair_callback(_broadphase_pair, this); broadphase->set_unpair_callback(_broadphase_unpair, this); diff --git a/servers/physics_2d/godot_space_2d.h b/servers/physics_2d/godot_space_2d.h index b155a834b6..5d97721176 100644 --- a/servers/physics_2d/godot_space_2d.h +++ b/servers/physics_2d/godot_space_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -82,7 +82,7 @@ private: GodotPhysicsDirectSpaceState2D *direct_access = nullptr; RID self; - GodotBroadPhase2D *broadphase; + GodotBroadPhase2D *broadphase = nullptr; SelfList<GodotBody2D>::List active_list; SelfList<GodotBody2D>::List mass_properties_update_list; SelfList<GodotBody2D>::List state_query_list; @@ -96,10 +96,13 @@ private: GodotArea2D *area = nullptr; - real_t contact_recycle_radius = 1.0; - real_t contact_max_separation = 1.5; - real_t contact_max_allowed_penetration = 0.3; - real_t constraint_bias = 0.2; + int solver_iterations = 0; + + real_t contact_recycle_radius = 0.0; + real_t contact_max_separation = 0.0; + real_t contact_max_allowed_penetration = 0.0; + real_t contact_bias = 0.0; + real_t constraint_bias = 0.0; enum { INTERSECTION_QUERY_MAX = 2048 @@ -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..866c415440 100644 --- a/servers/physics_2d/godot_step_2d.cpp +++ b/servers/physics_2d/godot_step_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -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..9a6d8caf9b 100644 --- a/servers/physics_2d/godot_step_2d.h +++ b/servers/physics_2d/godot_step_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -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(); }; |