diff options
Diffstat (limited to 'servers/physics_2d')
31 files changed, 830 insertions, 554 deletions
diff --git a/servers/physics_2d/godot_area_2d.cpp b/servers/physics_2d/godot_area_2d.cpp index 7cb202dd1f..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 */ @@ -77,16 +77,17 @@ void GodotArea2D::set_space(GodotSpace2D *p_space) { _set_space(p_space); } -void GodotArea2D::set_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == monitor_callback_id) { - monitor_callback_method = p_method; +void GodotArea2D::set_monitor_callback(const Callable &p_callback) { + ObjectID id = p_callback.get_object_id(); + + if (id == monitor_callback.get_object_id()) { + monitor_callback = p_callback; return; } _unregister_shapes(); - monitor_callback_id = p_id; - monitor_callback_method = p_method; + monitor_callback = p_callback; monitored_bodies.clear(); monitored_areas.clear(); @@ -98,16 +99,17 @@ void GodotArea2D::set_monitor_callback(ObjectID p_id, const StringName &p_method } } -void GodotArea2D::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == area_monitor_callback_id) { - area_monitor_callback_method = p_method; +void GodotArea2D::set_area_monitor_callback(const Callable &p_callback) { + ObjectID id = p_callback.get_object_id(); + + if (id == area_monitor_callback.get_object_id()) { + area_monitor_callback = p_callback; return; } _unregister_shapes(); - area_monitor_callback_id = p_id; - area_monitor_callback_method = p_method; + area_monitor_callback = p_callback; monitored_bodies.clear(); monitored_areas.clear(); @@ -119,18 +121,21 @@ void GodotArea2D::set_area_monitor_callback(ObjectID p_id, const StringName &p_m } } -void GodotArea2D::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode) { - bool do_override = p_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override == (space_override_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) { +void GodotArea2D::_set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode) { + bool do_override = p_new_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override == (r_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) { return; } _unregister_shapes(); - space_override_mode = p_mode; + r_mode = p_new_mode; _shape_changed(); } void GodotArea2D::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) { switch (p_param) { + case PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: + _set_space_override_mode(gravity_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value); + break; case PhysicsServer2D::AREA_PARAM_GRAVITY: gravity = p_value; break; @@ -146,9 +151,15 @@ void GodotArea2D::set_param(PhysicsServer2D::AreaParameter p_param, const Varian case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation = p_value; break; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: + _set_space_override_mode(linear_damping_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value); + break; case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: linear_damp = p_value; break; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: + _set_space_override_mode(angular_damping_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value); + break; case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: angular_damp = p_value; break; @@ -160,6 +171,8 @@ void GodotArea2D::set_param(PhysicsServer2D::AreaParameter p_param, const Varian Variant GodotArea2D::get_param(PhysicsServer2D::AreaParameter p_param) const { switch (p_param) { + case PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: + return gravity_override_mode; case PhysicsServer2D::AREA_PARAM_GRAVITY: return gravity; case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: @@ -170,8 +183,12 @@ Variant GodotArea2D::get_param(PhysicsServer2D::AreaParameter p_param) const { return gravity_distance_scale; case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: + return linear_damping_override_mode; case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: return linear_damp; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: + return angular_damping_override_mode; case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: return angular_damp; case PhysicsServer2D::AREA_PARAM_PRIORITY: @@ -196,80 +213,79 @@ void GodotArea2D::set_monitorable(bool p_monitorable) { monitorable = p_monitorable; _set_static(!monitorable); + _shapes_changed(); } void GodotArea2D::call_queries() { - if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } + if (!monitor_callback.is_null() && !monitored_bodies.is_empty()) { + if (monitor_callback.is_valid()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } - Object *obj = ObjectDB::get_instance(monitor_callback_id); - if (!obj) { - monitored_bodies.clear(); - monitor_callback_id = ObjectID(); - return; - } + for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { + if (E->get().state == 0) { // Nothing happened + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; + continue; + } + + res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; - for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { - if (E->get().state == 0) { // Nothing happened Map<BodyKey, BodyState>::Element *next = E->next(); monitored_bodies.erase(E); E = next; - continue; - } - res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; - - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; - - Callable::CallError ce; - obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce); + Callable::CallError ce; + Variant ret; + monitor_callback.call((const Variant **)resptr, 5, ret, ce); + } + } else { + monitored_bodies.clear(); + monitor_callback = Callable(); } } - if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } + if (!area_monitor_callback.is_null() && !monitored_areas.is_empty()) { + if (area_monitor_callback.is_valid()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } - Object *obj = ObjectDB::get_instance(area_monitor_callback_id); - if (!obj) { - monitored_areas.clear(); - area_monitor_callback_id = ObjectID(); - return; - } + for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { + if (E->get().state == 0) { // Nothing happened + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_areas.erase(E); + E = next; + continue; + } + + res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key().rid; + res[2] = E->key().instance_id; + res[3] = E->key().body_shape; + res[4] = E->key().area_shape; - for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { - if (E->get().state == 0) { // Nothing happened Map<BodyKey, BodyState>::Element *next = E->next(); monitored_areas.erase(E); E = next; - continue; - } - - res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; - - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); - E = next; - Callable::CallError ce; - obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce); + Callable::CallError ce; + Variant ret; + area_monitor_callback.call((const Variant **)resptr, 5, ret, ce); + } + } else { + monitored_areas.clear(); + area_monitor_callback = Callable(); } } } diff --git a/servers/physics_2d/godot_area_2d.h b/servers/physics_2d/godot_area_2d.h index daa03d39e3..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 */ @@ -41,7 +41,10 @@ class GodotBody2D; class GodotConstraint2D; class GodotArea2D : public GodotCollisionObject2D { - PhysicsServer2D::AreaSpaceOverrideMode space_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer2D::AreaSpaceOverrideMode gravity_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer2D::AreaSpaceOverrideMode linear_damping_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer2D::AreaSpaceOverrideMode angular_damping_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + real_t gravity = 9.80665; Vector2 gravity_vector = Vector2(0, -1); bool gravity_is_point = false; @@ -52,11 +55,9 @@ class GodotArea2D : public GodotCollisionObject2D { int priority = 0; bool monitorable = false; - ObjectID monitor_callback_id; - StringName monitor_callback_method; + Callable monitor_callback; - ObjectID area_monitor_callback_id; - StringName area_monitor_callback_method; + Callable area_monitor_callback; SelfList<GodotArea2D> monitor_query_list; SelfList<GodotArea2D> moved_list; @@ -98,12 +99,14 @@ class GodotArea2D : public GodotCollisionObject2D { virtual void _shapes_changed(); void _queue_monitor_update(); + void _set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode); + public: - void set_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } + void set_monitor_callback(const Callable &p_callback); + _FORCE_INLINE_ bool has_monitor_callback() const { return !monitor_callback.is_null(); } - void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } + void set_area_monitor_callback(const Callable &p_callback); + _FORCE_INLINE_ bool has_area_monitor_callback() const { return !area_monitor_callback.is_null(); } _FORCE_INLINE_ void add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); _FORCE_INLINE_ void remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); @@ -114,9 +117,6 @@ public: void set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value); Variant get_param(PhysicsServer2D::AreaParameter p_param) const; - void set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode); - PhysicsServer2D::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } - _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } _FORCE_INLINE_ real_t get_gravity() const { return gravity; } diff --git a/servers/physics_2d/godot_area_pair_2d.cpp b/servers/physics_2d/godot_area_pair_2d.cpp index fdb95aa262..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 */ @@ -38,10 +38,18 @@ bool GodotAreaPair2D::setup(real_t p_step) { } process_collision = false; + has_space_override = false; if (result != colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - process_collision = true; - } else if (area->has_monitor_callback()) { + if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } + process_collision = has_space_override; + + if (area->has_monitor_callback()) { process_collision = true; } @@ -57,7 +65,7 @@ bool GodotAreaPair2D::pre_solve(real_t p_step) { } if (colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + if (has_space_override) { body->add_area(area); } @@ -65,7 +73,7 @@ bool GodotAreaPair2D::pre_solve(real_t p_step) { area->add_body_to_query(body, body_shape, area_shape); } } else { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + if (has_space_override) { body->remove_area(area); } @@ -95,7 +103,7 @@ GodotAreaPair2D::GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotAre GodotAreaPair2D::~GodotAreaPair2D() { if (colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + if (has_space_override) { body->remove_area(area); } if (area->has_monitor_callback()) { @@ -120,7 +128,7 @@ bool GodotArea2Pair2D::setup(real_t p_step) { process_collision_a = false; if (result_a != colliding_a) { - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + if (area_a->has_area_monitor_callback() && area_b_monitorable) { process_collision_a = true; process_collision = true; } @@ -129,7 +137,7 @@ bool GodotArea2Pair2D::setup(real_t p_step) { process_collision_b = false; if (result_b != colliding_b) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + if (area_b->has_area_monitor_callback() && area_a_monitorable) { process_collision_b = true; process_collision = true; } @@ -168,19 +176,21 @@ GodotArea2Pair2D::GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotAr area_b = p_area_b; shape_a = p_shape_a; shape_b = p_shape_b; + area_a_monitorable = area_a->is_monitorable(); + area_b_monitorable = area_b->is_monitorable(); area_a->add_constraint(this); area_b->add_constraint(this); } GodotArea2Pair2D::~GodotArea2Pair2D() { if (colliding_a) { - if (area_a->has_area_monitor_callback()) { + if (area_a->has_area_monitor_callback() && area_b_monitorable) { area_a->remove_area_from_query(area_b, shape_b, shape_a); } } if (colliding_b) { - if (area_b->has_area_monitor_callback()) { + if (area_b->has_area_monitor_callback() && area_a_monitorable) { area_b->remove_area_from_query(area_a, shape_a, shape_b); } } diff --git a/servers/physics_2d/godot_area_pair_2d.h b/servers/physics_2d/godot_area_pair_2d.h index 7a9677f714..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 */ @@ -41,6 +41,7 @@ class GodotAreaPair2D : public GodotConstraint2D { int body_shape = 0; int area_shape = 0; bool colliding = false; + bool has_space_override = false; bool process_collision = false; public: @@ -61,6 +62,8 @@ class GodotArea2Pair2D : public GodotConstraint2D { bool colliding_b = false; bool process_collision_a = false; bool process_collision_b = false; + bool area_a_monitorable; + bool area_b_monitorable; public: virtual bool setup(real_t p_step) override; diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp index 56f191c203..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,8 +185,19 @@ 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: { + int mode_value = p_value; + linear_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value; + } break; + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: { + int mode_value = p_value; + angular_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value; + } break; case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { linear_damp = p_value; } break; @@ -213,11 +224,17 @@ Variant GodotBody2D::get_param(PhysicsServer2D::BodyParameter p_param) const { return inertia; } case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { - return center_of_mass; + return center_of_mass_local; } case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { return gravity_scale; } + case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: { + return linear_damp_mode; + } + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: { + return angular_damp_mode; + } case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { return linear_damp; } @@ -396,15 +413,6 @@ void GodotBody2D::set_space(GodotSpace2D *p_space) { } } -void GodotBody2D::_compute_area_gravity_and_damping(const GodotArea2D *p_area) { - Vector2 area_gravity; - p_area->compute_gravity(get_transform().get_origin(), area_gravity); - gravity += area_gravity; - - area_linear_damp += p_area->get_linear_damp(); - area_angular_damp += p_area->get_angular_damp(); -} - void GodotBody2D::_update_transform_dependent() { center_of_mass = get_transform().basis_xform(center_of_mass_local); } @@ -414,61 +422,135 @@ void GodotBody2D::integrate_forces(real_t p_step) { return; } - GodotArea2D *def_area = get_space()->get_default_area(); - // GodotArea2D *damp_area = def_area; - ERR_FAIL_COND(!def_area); + ERR_FAIL_COND(!get_space()); int ac = areas.size(); + + bool gravity_done = false; + bool linear_damp_done = false; + bool angular_damp_done = false; + bool stopped = false; + gravity = Vector2(0, 0); - area_angular_damp = 0; - area_linear_damp = 0; + + total_linear_damp = 0.0; + total_angular_damp = 0.0; + + // Combine gravity and damping from overlapping areas in priority order. if (ac) { areas.sort(); const AreaCMP *aa = &areas[0]; - // damp_area = aa[ac-1].area; for (int i = ac - 1; i >= 0 && !stopped; i--) { - PhysicsServer2D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector2(0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { + if (!gravity_done) { + PhysicsServer2D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE); + if (area_gravity_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + Vector2 area_gravity; + aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity); + switch (area_gravity_mode) { + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + gravity += area_gravity; + gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = area_gravity; + gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + if (!linear_damp_done) { + PhysicsServer2D::AreaSpaceOverrideMode area_linear_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE); + if (area_linear_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + real_t area_linear_damp = aa[i].area->get_linear_damp(); + switch (area_linear_damp_mode) { + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + total_linear_damp += area_linear_damp; + linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + total_linear_damp = area_linear_damp; + linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + if (!angular_damp_done) { + PhysicsServer2D::AreaSpaceOverrideMode area_angular_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE); + if (area_angular_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + real_t area_angular_damp = aa[i].area->get_angular_damp(); + switch (area_angular_damp_mode) { + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + total_angular_damp += area_angular_damp; + angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + total_angular_damp = area_angular_damp; + angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } } } + stopped = gravity_done && linear_damp_done && angular_damp_done; } } + + // Add default gravity and damping from space area. if (!stopped) { - _compute_area_gravity_and_damping(def_area); + GodotArea2D *default_area = get_space()->get_default_area(); + ERR_FAIL_COND(!default_area); + + if (!gravity_done) { + Vector2 default_gravity; + default_area->compute_gravity(get_transform().get_origin(), default_gravity); + gravity += default_gravity; + } + + if (!linear_damp_done) { + total_linear_damp += default_area->get_linear_damp(); + } + + if (!angular_damp_done) { + total_angular_damp += default_area->get_angular_damp(); + } } - gravity *= gravity_scale; - // If less than 0, override dampenings with that of the Body2D - if (angular_damp >= 0) { - area_angular_damp = angular_damp; + // Override linear damping with body's value. + switch (linear_damp_mode) { + case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: { + total_linear_damp += linear_damp; + } break; + case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: { + total_linear_damp = linear_damp; + } break; } - /* - else - area_angular_damp=damp_area->get_angular_damp(); - */ - if (linear_damp >= 0) { - area_linear_damp = linear_damp; + // Override angular damping with body's value. + switch (angular_damp_mode) { + case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: { + total_angular_damp += angular_damp; + } break; + case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: { + total_angular_damp = angular_damp; + } break; } - /* - else - area_linear_damp=damp_area->get_linear_damp(); - */ + + gravity *= gravity_scale; + + prev_linear_velocity = linear_velocity; + prev_angular_velocity = angular_velocity; Vector2 motion; bool do_motion = false; @@ -483,28 +565,20 @@ void GodotBody2D::integrate_forces(real_t p_step) { do_motion = true; - /* - for(int i=0;i<get_shape_count();i++) { - set_shape_kinematic_advance(i,Vector2()); - set_shape_kinematic_retreat(i,0); - } - */ - } else { 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 * area_linear_damp; + real_t damp = 1.0 - p_step * total_linear_damp; if (damp < 0) { // reached zero in the given time damp = 0; } - real_t angular_damp = 1.0 - p_step * area_angular_damp; + real_t angular_damp = 1.0 - p_step * total_angular_damp; if (angular_damp < 0) { // reached zero in the given time angular_damp = 0; @@ -523,17 +597,16 @@ void GodotBody2D::integrate_forces(real_t p_step) { } } - //motion=linear_velocity*p_step; + applied_force = Vector2(); + applied_torque = 0.0; - biased_angular_velocity = 0; + biased_angular_velocity = 0.0; biased_linear_velocity = Vector2(); if (do_motion) { //shapes temporarily extend for raycast _update_shapes_with_motion(motion); } - // damp_area=nullptr; // clear the area, so it is set in the next frame - def_area = nullptr; // clear the area, so it is set in the next frame contact_count = 0; } diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h index 5fce362fa7..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,11 +50,21 @@ 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; - real_t linear_damp = -1.0; - real_t angular_damp = -1.0; + PhysicsServer2D::BodyDampMode linear_damp_mode = PhysicsServer2D::BODY_DAMP_MODE_COMBINE; + PhysicsServer2D::BodyDampMode angular_damp_mode = PhysicsServer2D::BODY_DAMP_MODE_COMBINE; + + real_t linear_damp = 0.0; + real_t angular_damp = 0.0; + + real_t total_linear_damp = 0.0; + real_t total_angular_damp = 0.0; + real_t gravity_scale = 1.0; real_t bounce = 0.0; @@ -73,14 +83,15 @@ class GodotBody2D : public GodotCollisionObject2D { bool calculate_center_of_mass = true; Vector2 gravity; - real_t area_linear_damp = 0.0; - real_t area_angular_damp = 0.0; real_t still_time = 0.0; 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; @@ -140,8 +151,6 @@ class GodotBody2D : public GodotCollisionObject2D { uint64_t island_step = 0; - void _compute_area_gravity_and_damping(const GodotArea2D *p_area); - void _update_transform_dependent(); friend class GodotPhysicsDirectBodyState2D; // i give up, too many functions to expose @@ -166,7 +175,7 @@ public: if (index > -1) { areas.write[index].refCount -= 1; if (areas[index].refCount < 1) { - areas.remove(index); + areas.remove_at(index); } } } @@ -206,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; } @@ -225,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; } @@ -249,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; } @@ -276,14 +307,12 @@ public: void update_mass_properties(); void reset_mass_properties(); - _FORCE_INLINE_ Vector2 get_center_of_mass() const { return center_of_mass; } + _FORCE_INLINE_ const Vector2 &get_center_of_mass() const { return center_of_mass; } + _FORCE_INLINE_ const Vector2 &get_center_of_mass_local() const { return center_of_mass_local; } _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; } _FORCE_INLINE_ real_t get_friction() const { return friction; } - _FORCE_INLINE_ Vector2 get_gravity() const { return gravity; } _FORCE_INLINE_ real_t get_bounce() const { return bounce; } - _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } - _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } void integrate_forces(real_t p_step); void integrate_velocities(real_t p_step); diff --git a/servers/physics_2d/godot_body_direct_state_2d.cpp b/servers/physics_2d/godot_body_direct_state_2d.cpp index 300c302c79..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 */ @@ -39,17 +39,21 @@ Vector2 GodotPhysicsDirectBodyState2D::get_total_gravity() const { } real_t GodotPhysicsDirectBodyState2D::get_total_angular_damp() const { - return body->area_angular_damp; + return body->total_angular_damp; } real_t GodotPhysicsDirectBodyState2D::get_total_linear_damp() const { - return body->area_linear_damp; + return body->total_linear_damp; } Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass() const { return body->get_center_of_mass(); } +Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass_local() const { + return body->get_center_of_mass_local(); +} + real_t GodotPhysicsDirectBodyState2D::get_inverse_mass() const { return body->get_inv_mass(); } @@ -88,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 2f3e8e5095..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 */ @@ -46,6 +46,7 @@ public: virtual real_t get_total_linear_damp() const override; virtual Vector2 get_center_of_mass() const override; + virtual Vector2 get_center_of_mass_local() const override; virtual real_t get_inverse_mass() const override; virtual real_t get_inverse_inertia() const override; @@ -60,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 c86f87fc03..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 */ @@ -289,7 +289,7 @@ RID GodotPhysicsServer2D::area_create() { RID rid = area_owner.make_rid(area); area->set_self(rid); return rid; -}; +} void GodotPhysicsServer2D::area_set_space(RID p_area, RID p_space) { GodotArea2D *area = area_owner.get_or_null(p_area); @@ -307,7 +307,7 @@ void GodotPhysicsServer2D::area_set_space(RID p_area, RID p_space) { area->clear_constraints(); area->set_space(space); -}; +} RID GodotPhysicsServer2D::area_get_space(RID p_area) const { GodotArea2D *area = area_owner.get_or_null(p_area); @@ -318,20 +318,6 @@ RID GodotPhysicsServer2D::area_get_space(RID p_area) const { return RID(); } return space->get_self(); -}; - -void GodotPhysicsServer2D::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - GodotArea2D *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_space_override_mode(p_mode); -} - -PhysicsServer2D::AreaSpaceOverrideMode GodotPhysicsServer2D::area_get_space_override_mode(RID p_area) const { - const GodotArea2D *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); - - return area->get_space_override_mode(); } void GodotPhysicsServer2D::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform, bool p_disabled) { @@ -513,18 +499,18 @@ void GodotPhysicsServer2D::area_set_collision_layer(RID p_area, uint32_t p_layer area->set_collision_layer(p_layer); } -void GodotPhysicsServer2D::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { +void GodotPhysicsServer2D::area_set_monitor_callback(RID p_area, const Callable &p_callback) { GodotArea2D *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); - area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); + area->set_monitor_callback(p_callback.is_valid() ? p_callback : Callable()); } -void GodotPhysicsServer2D::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { +void GodotPhysicsServer2D::area_set_area_monitor_callback(RID p_area, const Callable &p_callback) { GodotArea2D *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); - area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); + area->set_area_monitor_callback(p_callback.is_valid() ? p_callback : Callable()); } /* BODY API */ @@ -683,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); @@ -758,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); -void GodotPhysicsServer2D::body_set_applied_torque(RID p_body, real_t p_torque) { + _update_shapes(); + + body->apply_torque_impulse(p_torque); + body->wakeup(); +} + +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); @@ -963,10 +977,17 @@ bool GodotPhysicsServer2D::body_test_motion(RID p_body, const MotionParameters & PhysicsDirectBodyState2D *GodotPhysicsServer2D::body_get_direct_state(RID p_body) { ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + if (!body_owner.owns(p_body)) { + return nullptr; + } + GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, nullptr); - ERR_FAIL_COND_V(!body->get_space(), nullptr); + if (!body->get_space()) { + return nullptr; + } + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); return body->get_direct_state(); @@ -1221,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) { @@ -1248,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) { @@ -1315,7 +1331,7 @@ void GodotPhysicsServer2D::end_sync() { void GodotPhysicsServer2D::finish() { memdelete(stepper); -}; +} void GodotPhysicsServer2D::_update_shapes() { while (pending_shape_update_list.first()) { @@ -1347,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 a8a1e71d13..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; @@ -124,9 +123,6 @@ public: virtual RID area_create() override; - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) override; - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const override; - virtual void area_set_space(RID p_area, RID p_space) override; virtual RID area_get_space(RID p_area) const override; @@ -158,8 +154,8 @@ public: virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; - virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; - virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; + virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override; + virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override; virtual void area_set_pickable(RID p_area, bool p_pickable) override; @@ -211,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; @@ -286,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 d72014a8ed..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) { @@ -54,13 +55,13 @@ _FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, u return true; } -int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas, ObjectID p_canvas_instance_id) { +int GodotPhysicsDirectSpaceState2D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) { if (p_result_max <= 0) { return 0; } Rect2 aabb; - aabb.position = p_point - Vector2(0.00001, 0.00001); + aabb.position = p_parameters.position - Vector2(0.00001, 0.00001); aabb.size = Vector2(0.00002, 0.00002); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -68,21 +69,21 @@ int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point int cc = 0; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { continue; } const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; - if (p_pick_point && !col_obj->is_pickable()) { + if (p_parameters.pick_point && !col_obj->is_pickable()) { continue; } - if (p_filter_by_canvas && col_obj->get_canvas_instance_id() != p_canvas_instance_id) { + if (p_parameters.canvas_instance_id.is_valid() && col_obj->get_canvas_instance_id() != p_parameters.canvas_instance_id) { continue; } @@ -90,7 +91,7 @@ int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point GodotShape2D *shape = col_obj->get_shape(shape_idx); - Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point); + Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_parameters.position); if (!shape->contains_point(local_point)) { continue; @@ -113,21 +114,13 @@ int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point return cc; } -int GodotPhysicsDirectSpaceState2D::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { - return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point); -} - -int GodotPhysicsDirectSpaceState2D::intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { - return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point, true, p_canvas_instance_id); -} - -bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +bool GodotPhysicsDirectSpaceState2D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) { ERR_FAIL_COND_V(space->locked, false); Vector2 begin, end; Vector2 normal; - begin = p_from; - end = p_to; + begin = p_parameters.from; + end = p_parameters.to; normal = (end - begin).normalized(); int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -141,11 +134,11 @@ bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const real_t min_d = 1e10; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { continue; } @@ -157,16 +150,26 @@ bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const Vector2 local_from = inv_xform.xform(begin); Vector2 local_to = inv_xform.xform(end); - /*local_from = col_obj->get_inv_transform().xform(begin); - local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from); - - local_to = col_obj->get_inv_transform().xform(end); - local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/ - const GodotShape2D *shape = col_obj->get_shape(shape_idx); Vector2 shape_point, shape_normal; + if (shape->contains_point(local_from)) { + if (p_parameters.hit_from_inside) { + // Hit shape at starting point. + min_d = 0; + res_point = local_from; + res_normal = Vector2(); + res_shape = shape_idx; + res_obj = col_obj; + collided = true; + break; + } else { + // Ignore shape when starting inside. + continue; + } + } + if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); shape_point = xform.xform(shape_point); @@ -200,16 +203,17 @@ bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const return true; } -int GodotPhysicsDirectSpaceState2D::intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +int GodotPhysicsDirectSpaceState2D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) { if (p_result_max <= 0) { return 0; } - GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); ERR_FAIL_COND_V(!shape, 0); - Rect2 aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.grow(p_margin); + 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); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -220,18 +224,18 @@ int GodotPhysicsDirectSpaceState2D::intersect_shape(const RID &p_shape, const Tr break; } - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { continue; } const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - if (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) { + if (!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(), nullptr, nullptr, nullptr, p_parameters.margin)) { continue; } @@ -248,13 +252,13 @@ int GodotPhysicsDirectSpaceState2D::intersect_shape(const RID &p_shape, const Tr return cc; } -bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); +bool GodotPhysicsDirectSpaceState2D::cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe) { + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); ERR_FAIL_COND_V(!shape, false); - Rect2 aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); + 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); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -262,11 +266,11 @@ bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Trans real_t best_unsafe = 1; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { continue; //ignore excluded } @@ -275,16 +279,16 @@ bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Trans Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? - if (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { + if (!GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) { continue; } //test initial overlap, ignore objects it's inside of. - if (GodotCollisionSolver2D::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { + if (GodotCollisionSolver2D::solve(shape, p_parameters.transform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) { continue; } - Vector2 mnormal = p_motion.normalized(); + Vector2 mnormal = p_parameters.motion.normalized(); //just do kinematic solving real_t low = 0.0; @@ -294,7 +298,7 @@ bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Trans real_t fraction = low + (hi - low) * fraction_coeff; Vector2 sep = mnormal; //important optimization for this to work fast enough - bool collided = GodotCollisionSolver2D::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); + bool collided = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_parameters.margin); if (collided) { hi = fraction; @@ -331,17 +335,17 @@ bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Trans return true; } -bool GodotPhysicsDirectSpaceState2D::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +bool GodotPhysicsDirectSpaceState2D::collide_shape(const ShapeParameters &p_parameters, Vector2 *r_results, int p_result_max, int &r_result_count) { if (p_result_max <= 0) { return false; } - GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); ERR_FAIL_COND_V(!shape, 0); - Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); + 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); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -358,13 +362,13 @@ bool GodotPhysicsDirectSpaceState2D::collide_shape(RID p_shape, const Transform2 GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { + if (p_parameters.exclude.has(col_obj->get_self())) { continue; } @@ -373,7 +377,7 @@ bool GodotPhysicsDirectSpaceState2D::collide_shape(RID p_shape, const Transform2 cbk.valid_dir = Vector2(); cbk.valid_depth = 0; - if (GodotCollisionSolver2D::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_margin)) { + if (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(), cbkres, cbkptr, nullptr, p_parameters.margin)) { collided = cbk.amount > 0; } } @@ -432,32 +436,33 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, rd->best_local_shape = rd->local_shape; } -bool GodotPhysicsDirectSpaceState2D::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape); +bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) { + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); ERR_FAIL_COND_V(!shape, 0); - real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE); - Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); + Rect2 aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion + aabb = aabb.grow(margin); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); _RestCallbackData2D rcd; - rcd.best_len = 0; - rcd.best_object = nullptr; - rcd.best_shape = 0; - rcd.min_allowed_depth = min_contact_depth; + + // 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 = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { + if (p_parameters.exclude.has(col_obj->get_self())) { continue; } @@ -467,7 +472,7 @@ bool GodotPhysicsDirectSpaceState2D::rest_info(RID p_shape, const Transform2D &p rcd.object = col_obj; rcd.shape = shape_idx; rcd.local_shape = 0; - bool sc = GodotCollisionSolver2D::solve(shape, p_shape_xform, p_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_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; } @@ -538,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; @@ -563,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; @@ -628,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) { @@ -653,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 } @@ -876,9 +884,6 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: ugt.elements[2] += p_parameters.motion * unsafe; _RestCallbackData2D rcd; - rcd.best_len = 0; - rcd.best_object = nullptr; - rcd.best_shape = 0; // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); @@ -928,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); @@ -950,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; } @@ -1139,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; @@ -1154,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; } } @@ -1163,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: @@ -1173,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; } @@ -1199,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 97e2928a9d..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 */ @@ -45,18 +45,15 @@ class GodotPhysicsDirectSpaceState2D : public PhysicsDirectSpaceState2D { GDCLASS(GodotPhysicsDirectSpaceState2D, PhysicsDirectSpaceState2D); - int _intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas = false, ObjectID p_canvas_instance_id = ObjectID()); - public: GodotSpace2D *space = nullptr; - virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; - virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; - virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual int intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; + virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override; + virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; + virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe) override; + virtual bool collide_shape(const ShapeParameters &p_parameters, Vector2 *r_results, int p_result_max, int &r_result_count) override; + virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override; GodotPhysicsDirectSpaceState2D() {} }; @@ -85,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; @@ -99,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 @@ -158,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 3010315571..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(); @@ -152,6 +152,9 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta, int p_iterations) p_space->set_active_objects(active_count); + // Update the broadphase to register collision pairs. + p_space->update(); + { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); @@ -255,11 +258,7 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta, int p_iterations) // Warning: _solve_island modifies the constraint islands for optimization purpose, // their content is not reliable after these calls and shouldn't be used anymore. - if (island_count > 1) { - work_pool.do_work(island_count, this, &GodotStep2D::_solve_island, nullptr); - } else if (island_count > 0) { - _solve_island(0); - } + work_pool.do_work(island_count, this, &GodotStep2D::_solve_island, nullptr); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -290,7 +289,6 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta, int p_iterations) all_constraints.clear(); - p_space->update(); p_space->unlock(); _step++; } 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(); }; |