diff options
Diffstat (limited to 'servers/physics_2d')
34 files changed, 1388 insertions, 2384 deletions
diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 7485f31afc..663a47f273 100644 --- a/servers/physics_2d/area_2d_sw.cpp +++ b/servers/physics_2d/area_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -199,7 +199,7 @@ void Area2DSW::set_monitorable(bool p_monitorable) { } void Area2DSW::call_queries() { - if (monitor_callback_id.is_valid() && !monitored_bodies.empty()) { + if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { Variant res[5]; Variant *resptr[5]; for (int i = 0; i < 5; i++) { @@ -215,7 +215,9 @@ void Area2DSW::call_queries() { for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { if (E->get().state == 0) { // Nothing happened - E = E->next(); + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_bodies.erase(E); + E = next; continue; } @@ -234,7 +236,7 @@ void Area2DSW::call_queries() { } } - if (area_monitor_callback_id.is_valid() && !monitored_areas.empty()) { + if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { Variant res[5]; Variant *resptr[5]; for (int i = 0; i < 5; i++) { @@ -250,7 +252,9 @@ void Area2DSW::call_queries() { for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { if (E->get().state == 0) { // Nothing happened - E = E->next(); + Map<BodyKey, BodyState>::Element *next = E->next(); + monitored_areas.erase(E); + E = next; continue; } @@ -270,6 +274,26 @@ void Area2DSW::call_queries() { } } +void Area2DSW::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const { + if (is_gravity_point()) { + const real_t gravity_distance_scale = get_gravity_distance_scale(); + Vector2 v = get_transform().xform(get_gravity_vector()) - p_position; + if (gravity_distance_scale > 0) { + const real_t v_length = v.length(); + if (v_length > 0) { + const real_t v_scaled = v_length * gravity_distance_scale; + r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); + } else { + r_gravity = Vector2(); + } + } else { + r_gravity = v.normalized() * get_gravity(); + } + } else { + r_gravity = get_gravity_vector() * get_gravity(); + } +} + Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h index f14466f582..d9147d6f1d 100644 --- a/servers/physics_2d/area_2d_sw.h +++ b/servers/physics_2d/area_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,9 +32,8 @@ #define AREA_2D_SW_H #include "collision_object_2d_sw.h" -#include "core/self_list.h" +#include "core/templates/self_list.h" #include "servers/physics_server_2d.h" -//#include "servers/physics_3d/query_sw.h" class Space2DSW; class Body2DSW; @@ -94,17 +93,12 @@ class Area2DSW : public CollisionObject2DSW { Map<BodyKey, BodyState> monitored_bodies; Map<BodyKey, BodyState> monitored_areas; - //virtual void shape_changed_notify(Shape2DSW *p_shape); - //virtual void shape_deleted_notify(Shape2DSW *p_shape); Set<Constraint2DSW *> constraints; virtual void _shapes_changed(); void _queue_monitor_update(); public: - //_FORCE_INLINE_ const Matrix32& get_inverse_transform() const { return inverse_transform; } - //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } - void set_monitor_callback(ObjectID p_id, const StringName &p_method); _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } @@ -161,6 +155,8 @@ public: void call_queries(); + void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const; + Area2DSW(); ~Area2DSW(); }; diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index d7bceb9f02..4f1148c26f 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,38 +33,52 @@ bool AreaPair2DSW::setup(real_t p_step) { bool result = false; - - if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) { - result = false; - } else if (area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { + if (area->collides_with(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->add_area(area); - } - if (area->has_monitor_callback()) { - area->add_body_to_query(body, body_shape, area_shape); - } - - } else { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + process_collision = true; + } else if (area->has_monitor_callback()) { + process_collision = true; } colliding = result; } - return false; //never do any post solving + return process_collision; +} + +bool AreaPair2DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. } void AreaPair2DSW::solve(real_t p_step) { + // Nothing to do. } AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { @@ -72,7 +86,6 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, area = p_area; body_shape = p_body_shape; area_shape = p_area_shape; - colliding = false; body->add_constraint(this, 0); area->add_constraint(this); if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair @@ -89,47 +102,65 @@ AreaPair2DSW::~AreaPair2DSW() { area->remove_body_from_query(body, body_shape, area_shape); } } - body->remove_constraint(this); + body->remove_constraint(this, 0); area->remove_constraint(this); } ////////////////////////////////// bool Area2Pair2DSW::setup(real_t p_step) { - bool result = false; - if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) { - result = false; - } else if (area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { - result = true; + bool result_a = area_a->collides_with(area_b); + bool result_b = area_b->collides_with(area_a); + if ((result_a || result_b) && !CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { + result_a = false; + result_b = false; } - if (result != colliding) { - if (result) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - area_b->add_area_to_query(area_a, shape_a, shape_b); - } + bool process_collision = false; - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } + process_collision_a = false; + if (result_a != colliding_a) { + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + process_collision_a = true; + process_collision = true; + } + colliding_a = result_a; + } - } else { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } + process_collision_b = false; + if (result_b != colliding_b) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + process_collision_b = true; + process_collision = true; + } + colliding_b = result_b; + } - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } + return process_collision; +} + +bool Area2Pair2DSW::pre_solve(real_t p_step) { + if (process_collision_a) { + if (colliding_a) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } else { + area_a->remove_area_from_query(area_b, shape_b, shape_a); } + } - colliding = result; + if (process_collision_b) { + if (colliding_b) { + area_b->add_area_to_query(area_a, shape_a, shape_b); + } else { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } } - return false; //never do any post solving + return false; // Never do any post solving. } void Area2Pair2DSW::solve(real_t p_step) { + // Nothing to do. } Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { @@ -137,22 +168,23 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area area_b = p_area_b; shape_a = p_shape_a; shape_b = p_shape_b; - colliding = false; area_a->add_constraint(this); area_b->add_constraint(this); } Area2Pair2DSW::~Area2Pair2DSW() { - if (colliding) { - if (area_b->has_area_monitor_callback()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - + if (colliding_a) { if (area_a->has_area_monitor_callback()) { area_a->remove_area_from_query(area_b, shape_b, shape_a); } } + if (colliding_b) { + if (area_b->has_area_monitor_callback()) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + area_a->remove_constraint(this); area_b->remove_constraint(this); } diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h index 5e8670b464..66e9f1afee 100644 --- a/servers/physics_2d/area_pair_2d_sw.h +++ b/servers/physics_2d/area_pair_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -36,30 +36,36 @@ #include "constraint_2d_sw.h" class AreaPair2DSW : public Constraint2DSW { - Body2DSW *body; - Area2DSW *area; - int body_shape; - int area_shape; - bool colliding; + Body2DSW *body = nullptr; + Area2DSW *area = nullptr; + int body_shape = 0; + int area_shape = 0; + bool colliding = false; + bool process_collision = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape); ~AreaPair2DSW(); }; class Area2Pair2DSW : public Constraint2DSW { - Area2DSW *area_a; - Area2DSW *area_b; - int shape_a; - int shape_b; - bool colliding; + Area2DSW *area_a = nullptr; + Area2DSW *area_b = nullptr; + int shape_a = 0; + int shape_b = 0; + bool colliding_a = false; + bool colliding_b = false; + bool process_collision_a = false; + bool process_collision_b = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b); ~Area2Pair2DSW(); diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index 75c9a95739..ad47912b82 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -43,7 +43,7 @@ void Body2DSW::update_inertias() { //update shapes and motions switch (mode) { - case PhysicsServer2D::BODY_MODE_RIGID: { + case PhysicsServer2D::BODY_MODE_DYNAMIC: { if (user_inertia) { _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; break; @@ -87,7 +87,7 @@ void Body2DSW::update_inertias() { _inv_inertia = 0; _inv_mass = 0; } break; - case PhysicsServer2D::BODY_MODE_CHARACTER: { + case PhysicsServer2D::BODY_MODE_DYNAMIC_LOCKED: { _inv_inertia = 0; _inv_mass = 1.0 / mass; @@ -104,31 +104,17 @@ void Body2DSW::set_active(bool p_active) { } active = p_active; - if (!p_active) { - if (get_space()) { - get_space()->body_remove_from_active_list(&active_list); - } - } else { + + if (active) { if (mode == PhysicsServer2D::BODY_MODE_STATIC) { - return; //static bodies can't become active - } - if (get_space()) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { get_space()->body_add_to_active_list(&active_list); } - - //still_time=0; - } - /* - if (!space) - return; - - for(int i=0;i<get_shape_count();i++) { - Shape &s=shapes[i]; - if (s.bpid>0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); } -*/ } void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) { @@ -218,14 +204,14 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { first_time_kinematic = true; } } break; - case PhysicsServer2D::BODY_MODE_RIGID: { + case PhysicsServer2D::BODY_MODE_DYNAMIC: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; _set_static(false); set_active(true); } break; - case PhysicsServer2D::BODY_MODE_CHARACTER: { + case PhysicsServer2D::BODY_MODE_DYNAMIC_LOCKED: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = 0; _set_static(false); @@ -233,7 +219,7 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { angular_velocity = 0; } break; } - if (p_mode == PhysicsServer2D::BODY_MODE_RIGID && _inv_inertia == 0) { + if (p_mode == PhysicsServer2D::BODY_MODE_DYNAMIC && _inv_inertia == 0) { _update_inertia(); } /* @@ -281,25 +267,16 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va } break; case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { - /* - if (mode==PhysicsServer2D::BODY_MODE_STATIC) - break; - */ linear_velocity = p_variant; wakeup(); } break; case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { - /* - if (mode!=PhysicsServer2D::BODY_MODE_RIGID) - break; - */ angular_velocity = p_variant; wakeup(); } break; case PhysicsServer2D::BODY_STATE_SLEEPING: { - //? if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { break; } @@ -318,7 +295,7 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va } break; case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode == PhysicsServer2D::BODY_MODE_RIGID && !active && !can_sleep) { + if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) { set_active(true); } @@ -370,29 +347,15 @@ void Body2DSW::set_space(Space2DSW *p_space) { if (active) { get_space()->body_add_to_active_list(&active_list); } - /* - _update_queries(); - if (is_active()) { - active=false; - set_active(true); - } - */ } first_integration = false; } -void Body2DSW::_compute_area_gravity_and_dampenings(const Area2DSW *p_area) { - if (p_area->is_gravity_point()) { - if (p_area->get_gravity_distance_scale() > 0) { - Vector2 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin(); - gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2)); - } else { - gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity(); - } - } else { - gravity += p_area->get_gravity_vector() * p_area->get_gravity(); - } +void Body2DSW::_compute_area_gravity_and_damping(const Area2DSW *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(); @@ -421,7 +384,7 @@ void Body2DSW::integrate_forces(real_t p_step) { switch (mode) { case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_dampenings(aa[i].area); + _compute_area_gravity_and_damping(aa[i].area); stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; } break; case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: @@ -429,7 +392,7 @@ void Body2DSW::integrate_forces(real_t p_step) { gravity = Vector2(0, 0); area_angular_damp = 0; area_linear_damp = 0; - _compute_area_gravity_and_dampenings(aa[i].area); + _compute_area_gravity_and_damping(aa[i].area); stopped = mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; } break; default: { @@ -438,7 +401,7 @@ void Body2DSW::integrate_forces(real_t p_step) { } } if (!stopped) { - _compute_area_gravity_and_dampenings(def_area); + _compute_area_gravity_and_damping(def_area); } gravity *= gravity_scale; @@ -562,17 +525,17 @@ void Body2DSW::integrate_velocities(real_t p_step) { } void Body2DSW::wakeup_neighbours() { - for (Map<Constraint2DSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) { - const Constraint2DSW *c = E->key(); + for (const Pair<Constraint2DSW *, int> &E : constraint_list) { + const Constraint2DSW *c = E.first; Body2DSW **n = c->get_body_ptr(); int bc = c->get_body_count(); for (int i = 0; i < bc; i++) { - if (i == E->get()) { + if (i == E.second) { continue; } Body2DSW *b = n[i]; - if (b->mode != PhysicsServer2D::BODY_MODE_RIGID) { + if (b->mode != PhysicsServer2D::BODY_MODE_DYNAMIC) { continue; } @@ -591,16 +554,17 @@ void Body2DSW::call_queries() { Variant v = dbs; const Variant *vp[2] = { &v, &fi_callback->callback_udata }; - Object *obj = ObjectDB::get_instance(fi_callback->id); + Object *obj = fi_callback->callable.get_object(); if (!obj) { - set_force_integration_callback(ObjectID(), StringName()); + set_force_integration_callback(Callable()); } else { Callable::CallError ce; + Variant rv; if (fi_callback->callback_udata.get_type() != Variant::NIL) { - obj->call(fi_callback->method, vp, 2, ce); + fi_callback->callable.call(vp, 2, rv, ce); } else { - obj->call(fi_callback->method, vp, 1, ce); + fi_callback->callable.call(vp, 1, rv, ce); } } } @@ -608,9 +572,7 @@ void Body2DSW::call_queries() { bool Body2DSW::sleep_test(real_t p_step) { if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - return true; // - } else if (mode == PhysicsServer2D::BODY_MODE_CHARACTER) { - return !active; // characters and kinematic bodies don't sleep unless asked to sleep + return true; } else if (!can_sleep) { return false; } @@ -625,16 +587,15 @@ bool Body2DSW::sleep_test(real_t p_step) { } } -void Body2DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { +void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { if (fi_callback) { memdelete(fi_callback); fi_callback = nullptr; } - if (p_id.is_valid()) { + if (p_callable.get_object()) { fi_callback = memnew(ForceIntegrationCallback); - fi_callback->id = p_id; - fi_callback->method = p_method; + fi_callback->callable = p_callable; fi_callback->callback_udata = p_udata; } } @@ -644,7 +605,7 @@ Body2DSW::Body2DSW() : active_list(this), inertia_update_list(this), direct_state_query_list(this) { - mode = PhysicsServer2D::BODY_MODE_RIGID; + mode = PhysicsServer2D::BODY_MODE_DYNAMIC; active = true; angular_velocity = 0; biased_angular_velocity = 0; @@ -658,8 +619,6 @@ Body2DSW::Body2DSW() : omit_force_integration = false; applied_torque = 0; island_step = 0; - island_next = nullptr; - island_list_next = nullptr; _set_static(false); first_time_kinematic = false; linear_damp = -1; diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index 4a3ef718ec..30e14eb9c7 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,7 +33,9 @@ #include "area_2d_sw.h" #include "collision_object_2d_sw.h" -#include "core/vset.h" +#include "core/templates/list.h" +#include "core/templates/pair.h" +#include "core/templates/vset.h" class Constraint2DSW; @@ -83,7 +85,7 @@ class Body2DSW : public CollisionObject2DSW { virtual void _shapes_changed(); Transform2D new_transform; - Map<Constraint2DSW *, int> constraint_map; + List<Pair<Constraint2DSW *, int>> constraint_list; struct AreaCMP { Area2DSW *area; @@ -115,23 +117,20 @@ class Body2DSW : public CollisionObject2DSW { int contact_count; struct ForceIntegrationCallback { - ObjectID id; - StringName method; + Callable callable; Variant callback_udata; }; ForceIntegrationCallback *fi_callback; uint64_t island_step; - Body2DSW *island_next; - Body2DSW *island_list_next; - _FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area2DSW *p_area); + _FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area2DSW *p_area); friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose public: - void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); + void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); _FORCE_INLINE_ void add_area(Area2DSW *p_area) { int index = areas.find(AreaCMP(p_area)); @@ -162,7 +161,7 @@ public: _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } - _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); } + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } _FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos); _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } @@ -173,16 +172,10 @@ public: _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ Body2DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Body2DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Body2DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Body2DSW *p_next) { island_list_next = p_next; } - - _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } - _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraint_map.erase(p_constraint); } - const Map<Constraint2DSW *, int> &get_constraint_map() const { return constraint_map; } - _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } + _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); } + _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); } + const List<Pair<Constraint2DSW *, int>> &get_constraint_list() const { return constraint_list; } + _FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); } _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } @@ -273,6 +266,10 @@ public: void integrate_forces(real_t p_step); void integrate_velocities(real_t p_step); + _FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const { + return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x); + } + _FORCE_INLINE_ Vector2 get_motion() const { if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) { return new_transform.get_origin() - get_transform().get_origin(); @@ -359,6 +356,8 @@ public: virtual void set_transform(const Transform2D &p_transform) override { body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); } virtual Transform2D get_transform() const override { return body->get_transform(); } + virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override { return body->get_velocity_in_local_point(p_position); } + virtual void add_central_force(const Vector2 &p_force) override { body->add_central_force(p_force); } virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override { body->add_force(p_force, p_position); } virtual void add_torque(real_t p_torque) override { body->add_torque(p_torque); } diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index 2021aab17c..8bcc4609f4 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -87,10 +87,13 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto int least_deep = -1; real_t min_depth = 1e10; + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + for (int i = 0; i <= contact_count; i++) { Contact &c = (i == contact_count) ? contact : contacts[i]; - Vector2 global_A = A->get_transform().basis_xform(c.local_A); - Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; Vector2 axis = global_A - global_B; real_t depth = axis.dot(c.normal); @@ -124,6 +127,9 @@ void BodyPair2DSW::_validate_contacts() { real_t max_separation = space->get_contact_max_separation(); real_t max_separation2 = max_separation * max_separation; + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; @@ -134,8 +140,8 @@ void BodyPair2DSW::_validate_contacts() { } else { c.reused = false; - Vector2 global_A = A->get_transform().basis_xform(c.local_A); - Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; Vector2 axis = global_A - global_B; real_t depth = axis.dot(c.normal); @@ -220,15 +226,22 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) { } bool BodyPair2DSW::setup(real_t p_step) { - //cannot collide - if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) { + if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; } - if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) { - collided = false; - return false; + collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B); + collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A); + + report_contacts_only = false; + if (!collide_A && !collide_B) { + if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { + report_contacts_only = true; + } else { + collided = false; + return false; + } } //use local A coordinates to avoid numerical issues on collision detection @@ -236,12 +249,12 @@ bool BodyPair2DSW::setup(real_t p_step) { _validate_contacts(); - Vector2 offset_A = A->get_transform().get_origin(); + const Vector2 &offset_A = A->get_transform().get_origin(); Transform2D xform_Au = A->get_transform().untranslated(); Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); Transform2D xform_Bu = B->get_transform(); - xform_Bu.elements[2] -= A->get_transform().get_origin(); + xform_Bu.elements[2] -= offset_A; Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); Shape2DSW *shape_A_ptr = A->get_shape(shape_A); @@ -262,13 +275,13 @@ bool BodyPair2DSW::setup(real_t p_step) { if (!collided) { //test ccd (currently just a raycast) - if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { collided = true; } } - if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { collided = true; } @@ -285,24 +298,20 @@ bool BodyPair2DSW::setup(real_t p_step) { } if (!prev_collided) { - if (A->is_shape_set_as_one_way_collision(shape_A)) { + if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) { Vector2 direction = xform_A.get_axis(1).normalized(); bool valid = false; - if (B->get_linear_velocity().dot(direction) >= 0) { - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (!c.reused) { - continue; - } - if (c.normal.dot(direction) > 0) { //greater (normal inverted) - continue; - } - - valid = true; - break; + 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; + } + valid = true; + break; } - if (!valid) { collided = false; oneway_disabled = true; @@ -310,22 +319,19 @@ bool BodyPair2DSW::setup(real_t p_step) { } } - if (B->is_shape_set_as_one_way_collision(shape_B)) { + if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) { Vector2 direction = xform_B.get_axis(1).normalized(); bool valid = false; - if (A->get_linear_velocity().dot(direction) >= 0) { - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (!c.reused) { - continue; - } - if (c.normal.dot(direction) < 0) { //less (normal ok) - continue; - } - - valid = true; - break; + 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; + } + valid = true; + break; } if (!valid) { collided = false; @@ -335,9 +341,21 @@ bool BodyPair2DSW::setup(real_t p_step) { } } + return true; +} + +bool BodyPair2DSW::pre_solve(real_t p_step) { + if (!collided || oneway_disabled) { + return false; + } + real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = 0.3; + + Shape2DSW *shape_A_ptr = A->get_shape(shape_A); + Shape2DSW *shape_B_ptr = B->get_shape(shape_B); + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias() == 0) { bias = shape_B_ptr->get_custom_bias(); @@ -348,56 +366,55 @@ bool BodyPair2DSW::setup(real_t p_step) { } } - cc = 0; - real_t inv_dt = 1.0 / p_step; bool do_process = false; + const Vector2 &offset_A = A->get_transform().get_origin(); + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0; + real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0; + + 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]; + c.active = false; - Vector2 global_A = xform_Au.xform(c.local_A); - Vector2 global_B = xform_Bu.xform(c.local_B); + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; - real_t depth = c.normal.dot(global_A - global_B); + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); - if (depth <= 0 || !c.reused) { - c.active = false; + if (depth <= 0.0 || !c.reused) { continue; } - c.active = true; #ifdef DEBUG_ENABLED if (space->is_debugging_contacts()) { space->add_debug_contact(global_A + offset_A); space->add_debug_contact(global_B + offset_A); } #endif - int gather_A = A->can_report_contacts(); - int gather_B = B->can_report_contacts(); c.rA = global_A; c.rB = global_B - offset_B; - if (gather_A | gather_B) { - //Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x ); - - global_A += offset_A; - global_B += offset_A; + if (A->can_report_contacts()) { + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); + A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity()); + } - if (gather_A) { - Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); - A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity()); - } - if (gather_B) { - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); - B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity()); - } + if (B->can_report_contacts()) { + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity()); } - if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { - c.active = false; + if (report_contacts_only) { collided = false; continue; } @@ -405,15 +422,15 @@ bool BodyPair2DSW::setup(real_t p_step) { // Precompute normal mass, tangent mass, and bias. real_t rnA = c.rA.dot(c.normal); real_t rnB = c.rB.dot(c.normal); - real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); - kNormal += A->get_inv_inertia() * (c.rA.dot(c.rA) - rnA * rnA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rnB * rnB); + real_t kNormal = inv_mass_A + inv_mass_B; + kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB); c.mass_normal = 1.0f / kNormal; - Vector2 tangent = c.normal.tangent(); + Vector2 tangent = c.normal.orthogonal(); real_t rtA = c.rA.dot(tangent); real_t rtB = c.rB.dot(tangent); - real_t kTangent = A->get_inv_mass() + B->get_inv_mass(); - kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB); + real_t kTangent = inv_mass_A + inv_mass_B; + kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB); c.mass_tangent = 1.0f / kTangent; c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); @@ -425,8 +442,12 @@ bool BodyPair2DSW::setup(real_t p_step) { // Apply normal + friction impulse Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; - A->apply_impulse(-P, c.rA); - B->apply_impulse(P, c.rB); + if (collide_A) { + A->apply_impulse(-P, c.rA); + } + if (collide_B) { + B->apply_impulse(P, c.rB); + } } #endif @@ -438,6 +459,7 @@ bool BodyPair2DSW::setup(real_t p_step) { c.bounce = c.bounce * dv.dot(c.normal); } + c.active = true; do_process = true; } @@ -445,13 +467,12 @@ bool BodyPair2DSW::setup(real_t p_step) { } void BodyPair2DSW::solve(real_t p_step) { - if (!collided) { + if (!collided || oneway_disabled) { return; } for (int i = 0; i < contact_count; ++i) { Contact &c = contacts[i]; - cc++; if (!c.active) { continue; @@ -469,7 +490,7 @@ void BodyPair2DSW::solve(real_t p_step) { real_t vn = dv.dot(c.normal); real_t vbn = dbv.dot(c.normal); - Vector2 tangent = c.normal.tangent(); + Vector2 tangent = c.normal.orthogonal(); real_t vt = dv.dot(tangent); real_t jbn = (c.bias - vbn) * c.mass_normal; @@ -478,8 +499,12 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(-jb, c.rA); - B->apply_bias_impulse(jb, c.rB); + if (collide_A) { + A->apply_bias_impulse(-jb, c.rA); + } + if (collide_B) { + B->apply_bias_impulse(jb, c.rB); + } real_t jn = -(c.bounce + vn) * c.mass_normal; real_t jnOld = c.acc_normal_impulse; @@ -494,8 +519,12 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); - A->apply_impulse(-j, c.rA); - B->apply_impulse(j, c.rB); + if (collide_A) { + A->apply_impulse(-j, c.rA); + } + if (collide_B) { + B->apply_impulse(j, c.rB); + } } } @@ -508,12 +537,9 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh space = A->get_space(); A->add_constraint(this, 0); B->add_constraint(this, 1); - contact_count = 0; - collided = false; - oneway_disabled = false; } BodyPair2DSW::~BodyPair2DSW() { - A->remove_constraint(this); - B->remove_constraint(this); + A->remove_constraint(this, 0); + B->remove_constraint(this, 1); } diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h index ea4d55841a..849a7e2430 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -44,13 +44,16 @@ class BodyPair2DSW : public Constraint2DSW { Body2DSW *B; }; - Body2DSW *_arr[2]; + Body2DSW *_arr[2] = { nullptr, nullptr }; }; - int shape_A; - int shape_B; + int shape_A = 0; + int shape_B = 0; - Space2DSW *space; + bool collide_A = false; + bool collide_B = false; + + Space2DSW *space = nullptr; struct Contact { Vector2 position; @@ -73,10 +76,10 @@ class BodyPair2DSW : public Constraint2DSW { Vector2 sep_axis; Contact contacts[MAX_CONTACTS]; - int contact_count; - bool collided; - bool oneway_disabled; - int cc; + int contact_count = 0; + bool collided = false; + bool oneway_disabled = false; + bool report_contacts_only = false; bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); void _validate_contacts(); @@ -84,8 +87,9 @@ class BodyPair2DSW : public Constraint2DSW { _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B); ~BodyPair2DSW(); diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp deleted file mode 100644 index 3bdfc1a973..0000000000 --- a/servers/physics_2d/broad_phase_2d_basic.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_basic.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "broad_phase_2d_basic.h" - -BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) { - current++; - - Element e; - e.owner = p_object_; - e._static = false; - e.subindex = p_subindex; - - element_map[current] = e; - return current; -} - -void BroadPhase2DBasic::move(ID p_id, const Rect2 &p_aabb) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get().aabb = p_aabb; -} - -void BroadPhase2DBasic::set_static(ID p_id, bool p_static) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - E->get()._static = p_static; -} - -void BroadPhase2DBasic::remove(ID p_id) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - element_map.erase(E); -} - -CollisionObject2DSW *BroadPhase2DBasic::get_object(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, nullptr); - return E->get().owner; -} - -bool BroadPhase2DBasic::is_static(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, false); - return E->get()._static; -} - -int BroadPhase2DBasic::get_subindex(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, -1); - return E->get().subindex; -} - -int BroadPhase2DBasic::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect2 aabb = E->get().aabb; - if (aabb.intersects_segment(p_from, p_to)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} - -int BroadPhase2DBasic::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; - - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect2 aabb = E->get().aabb; - if (aabb.intersects(p_aabb)) { - p_results[rc] = E->get().owner; - p_result_indices[rc] = E->get().subindex; - rc++; - if (rc >= p_max_results) { - break; - } - } - } - - return rc; -} - -void BroadPhase2DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_userdata = p_userdata; - pair_callback = p_pair_callback; -} - -void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_userdata = p_userdata; - unpair_callback = p_unpair_callback; -} - -void BroadPhase2DBasic::update() { - // recompute pairs - for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) { - for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) { - Element *elem_A = &I->get(); - Element *elem_B = &J->get(); - - if (elem_A->owner == elem_B->owner) { - continue; - } - - bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static); - - PairKey key(I->key(), J->key()); - - Map<PairKey, void *>::Element *E = pair_map.find(key); - - if (!pair_ok && E) { - if (unpair_callback) { - unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata); - } - pair_map.erase(key); - } - - if (pair_ok && !E) { - void *data = nullptr; - if (pair_callback) { - data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata); - if (data) { - pair_map.insert(key, data); - } - } - } - } - } -} - -BroadPhase2DSW *BroadPhase2DBasic::_create() { - return memnew(BroadPhase2DBasic); -} - -BroadPhase2DBasic::BroadPhase2DBasic() { - current = 1; - unpair_callback = nullptr; - unpair_userdata = nullptr; - pair_callback = nullptr; - pair_userdata = nullptr; -} diff --git a/servers/physics_2d/broad_phase_2d_bvh.cpp b/servers/physics_2d/broad_phase_2d_bvh.cpp new file mode 100644 index 0000000000..5f53f4a012 --- /dev/null +++ b/servers/physics_2d/broad_phase_2d_bvh.cpp @@ -0,0 +1,116 @@ +/*************************************************************************/ +/* broad_phase_2d_bvh.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "broad_phase_2d_bvh.h" +#include "collision_object_2d_sw.h" + +BroadPhase2DSW::ID BroadPhase2DBVH::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { + ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? + return oid + 1; +} + +void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) { + bvh.move(p_id - 1, p_aabb); +} + +void BroadPhase2DBVH::set_static(ID p_id, bool p_static) { + CollisionObject2DSW *it = bvh.get(p_id - 1); + bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? +} + +void BroadPhase2DBVH::remove(ID p_id) { + bvh.erase(p_id - 1); +} + +CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const { + CollisionObject2DSW *it = bvh.get(p_id - 1); + ERR_FAIL_COND_V(!it, nullptr); + return it; +} + +bool BroadPhase2DBVH::is_static(ID p_id) const { + return !bvh.is_pairable(p_id - 1); +} + +int BroadPhase2DBVH::get_subindex(ID p_id) const { + return bvh.get_subindex(p_id - 1); +} + +int BroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); +} + +int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); +} + +void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B) { + BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); + if (!bpo->pair_callback) { + return nullptr; + } + + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); +} + +void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B, void *pairdata) { + BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); + if (!bpo->unpair_callback) { + return; + } + + bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); +} + +void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + pair_callback = p_pair_callback; + pair_userdata = p_userdata; +} + +void BroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + unpair_callback = p_unpair_callback; + unpair_userdata = p_userdata; +} + +void BroadPhase2DBVH::update() { + bvh.update(); +} + +BroadPhase2DSW *BroadPhase2DBVH::_create() { + return memnew(BroadPhase2DBVH); +} + +BroadPhase2DBVH::BroadPhase2DBVH() { + bvh.set_pair_callback(_pair_callback, this); + bvh.set_unpair_callback(_unpair_callback, this); + pair_callback = nullptr; + pair_userdata = nullptr; + unpair_userdata = nullptr; +} diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_bvh.h index ec5cfdbf1d..6c11d2561b 100644 --- a/servers/physics_2d/broad_phase_2d_basic.h +++ b/servers/physics_2d/broad_phase_2d_bvh.h @@ -1,12 +1,12 @@ /*************************************************************************/ -/* broad_phase_2d_basic.h */ +/* broad_phase_2d_bvh.h */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -28,49 +28,19 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef BROAD_PHASE_2D_BASIC_H -#define BROAD_PHASE_2D_BASIC_H +#ifndef BROAD_PHASE_2D_BVH_H +#define BROAD_PHASE_2D_BVH_H -#include "core/map.h" -#include "space_2d_sw.h" -class BroadPhase2DBasic : public BroadPhase2DSW { - struct Element { - CollisionObject2DSW *owner; - bool _static; - Rect2 aabb; - int subindex; - }; +#include "broad_phase_2d_sw.h" +#include "core/math/bvh.h" +#include "core/math/rect2.h" +#include "core/math/vector2.h" - Map<ID, Element> element_map; +class BroadPhase2DBVH : public BroadPhase2DSW { + BVH_Manager<CollisionObject2DSW, true, 128, Rect2, Vector2> bvh; - ID current; - - struct PairKey { - union { - struct { - ID a; - ID b; - }; - uint64_t key; - }; - - _FORCE_INLINE_ bool operator<(const PairKey &p_key) const { - return key < p_key.key; - } - - PairKey() { key = 0; } - PairKey(ID p_a, ID p_b) { - if (p_a > p_b) { - a = p_b; - b = p_a; - } else { - a = p_a; - b = p_b; - } - } - }; - - Map<PairKey, void *> pair_map; + static void *_pair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int); + static void _unpair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int, void *); PairCallback pair_callback; void *pair_userdata; @@ -79,7 +49,7 @@ class BroadPhase2DBasic : public BroadPhase2DSW { public: // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0); + virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); virtual void move(ID p_id, const Rect2 &p_aabb); virtual void set_static(ID p_id, bool p_static); virtual void remove(ID p_id); @@ -97,7 +67,7 @@ public: virtual void update(); static BroadPhase2DSW *_create(); - BroadPhase2DBasic(); + BroadPhase2DBVH(); }; -#endif // BROAD_PHASE_2D_BASIC_H +#endif // BROAD_PHASE_2D_BVH_H diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp deleted file mode 100644 index ec74507e03..0000000000 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ /dev/null @@ -1,735 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_hash_grid.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "broad_phase_2d_hash_grid.h" -#include "collision_object_2d_sw.h" -#include "core/project_settings.h" - -#define LARGE_ELEMENT_FI 1.01239812 - -void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) { - Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with); - - ERR_FAIL_COND(p_elem->_static && p_with->_static); - - if (!E) { - PairData *pd = memnew(PairData); - p_elem->paired[p_with] = pd; - p_with->paired[p_elem] = pd; - } else { - E->get()->rc++; - } -} - -void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) { - Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with); - - ERR_FAIL_COND(!E); //this should really be paired.. - - E->get()->rc--; - - if (E->get()->rc == 0) { - if (E->get()->colliding) { - //uncollide - if (unpair_callback) { - unpair_callback(p_elem->owner, p_elem->subindex, p_with->owner, p_with->subindex, E->get()->ud, unpair_userdata); - } - } - - memdelete(E->get()); - p_elem->paired.erase(E); - p_with->paired.erase(p_elem); - } -} - -void BroadPhase2DHashGrid::_check_motion(Element *p_elem) { - for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) { - bool physical_collision = p_elem->aabb.intersects(E->key()->aabb); - bool logical_collision = p_elem->owner->test_collision_mask(E->key()->owner); - - if (physical_collision) { - if (!E->get()->colliding || (logical_collision && !E->get()->ud && pair_callback)) { - E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata); - } else if (E->get()->colliding && !logical_collision && E->get()->ud && unpair_callback) { - unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata); - E->get()->ud = nullptr; - } - E->get()->colliding = true; - } else { // No physcial_collision - if (E->get()->colliding && unpair_callback) { - unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata); - } - E->get()->colliding = false; - } - } -} - -void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) { - Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues - if (sz.width * sz.height > large_object_min_surface) { - //large object, do not use grid, must check against all elements - for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - if (E->key() == p_elem->self) { - continue; // do not pair against itself - } - if (E->get().owner == p_elem->owner) { - continue; - } - if (E->get()._static && p_static) { - continue; - } - - _pair_attempt(p_elem, &E->get()); - } - - large_elements[p_elem].inc(); - return; - } - - Point2i from = (p_rect.position / cell_size).floor(); - Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor(); - - for (int i = from.x; i <= to.x; i++) { - for (int j = from.y; j <= to.y; j++) { - PosKey pk; - pk.x = i; - pk.y = j; - - uint32_t idx = pk.hash() % hash_table_size; - PosBin *pb = hash_table[idx]; - - while (pb) { - if (pb->key == pk) { - break; - } - - pb = pb->next; - } - - bool entered = false; - - if (!pb) { - //does not exist, create! - pb = memnew(PosBin); - pb->key = pk; - pb->next = hash_table[idx]; - hash_table[idx] = pb; - } - - if (p_static) { - if (pb->static_object_set[p_elem].inc() == 1) { - entered = true; - } - } else { - if (pb->object_set[p_elem].inc() == 1) { - entered = true; - } - } - - if (entered) { - for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - if (E->key()->owner == p_elem->owner) { - continue; - } - _pair_attempt(p_elem, E->key()); - } - - if (!p_static) { - for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) { - if (E->key()->owner == p_elem->owner) { - continue; - } - _pair_attempt(p_elem, E->key()); - } - } - } - } - } - - //pair separatedly with large elements - - for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (E->key() == p_elem) { - continue; // do not pair against itself - } - if (E->key()->owner == p_elem->owner) { - continue; - } - if (E->key()->_static && p_static) { - continue; - } - - _pair_attempt(E->key(), p_elem); - } -} - -void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) { - Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); - if (sz.width * sz.height > large_object_min_surface) { - //unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static - Map<Element *, PairData *>::Element *E = p_elem->paired.front(); - while (E) { - Map<Element *, PairData *>::Element *next = E->next(); - _unpair_attempt(p_elem, E->key()); - E = next; - } - - if (large_elements[p_elem].dec() == 0) { - large_elements.erase(p_elem); - } - return; - } - - Point2i from = (p_rect.position / cell_size).floor(); - Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor(); - - for (int i = from.x; i <= to.x; i++) { - for (int j = from.y; j <= to.y; j++) { - PosKey pk; - pk.x = i; - pk.y = j; - - uint32_t idx = pk.hash() % hash_table_size; - PosBin *pb = hash_table[idx]; - - while (pb) { - if (pb->key == pk) { - break; - } - - pb = pb->next; - } - - ERR_CONTINUE(!pb); //should exist!! - - bool exited = false; - - if (p_static) { - if (pb->static_object_set[p_elem].dec() == 0) { - pb->static_object_set.erase(p_elem); - exited = true; - } - } else { - if (pb->object_set[p_elem].dec() == 0) { - pb->object_set.erase(p_elem); - exited = true; - } - } - - if (exited) { - for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - if (E->key()->owner == p_elem->owner) { - continue; - } - _unpair_attempt(p_elem, E->key()); - } - - if (!p_static) { - for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) { - if (E->key()->owner == p_elem->owner) { - continue; - } - _unpair_attempt(p_elem, E->key()); - } - } - } - - if (pb->object_set.empty() && pb->static_object_set.empty()) { - if (hash_table[idx] == pb) { - hash_table[idx] = pb->next; - } else { - PosBin *px = hash_table[idx]; - - while (px) { - if (px->next == pb) { - px->next = pb->next; - break; - } - - px = px->next; - } - - ERR_CONTINUE(!px); - } - - memdelete(pb); - } - } - } - - for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (E->key() == p_elem) { - continue; // do not pair against itself - } - if (E->key()->owner == p_elem->owner) { - continue; - } - if (E->key()->_static && p_static) { - continue; - } - - //unpair from large elements - _unpair_attempt(p_elem, E->key()); - } -} - -BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) { - current++; - - Element e; - e.owner = p_object; - e._static = false; - e.subindex = p_subindex; - e.self = current; - e.pass = 0; - - element_map[current] = e; - return current; -} - -void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - - Element &e = E->get(); - - if (p_aabb != e.aabb) { - if (p_aabb != Rect2()) { - _enter_grid(&e, p_aabb, e._static); - } - if (e.aabb != Rect2()) { - _exit_grid(&e, e.aabb, e._static); - } - e.aabb = p_aabb; - } - - _check_motion(&e); -} - -void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - - Element &e = E->get(); - - if (e._static == p_static) { - return; - } - - if (e.aabb != Rect2()) { - _exit_grid(&e, e.aabb, e._static); - } - - e._static = p_static; - - if (e.aabb != Rect2()) { - _enter_grid(&e, e.aabb, e._static); - _check_motion(&e); - } -} - -void BroadPhase2DHashGrid::remove(ID p_id) { - Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - - Element &e = E->get(); - - if (e.aabb != Rect2()) { - _exit_grid(&e, e.aabb, e._static); - } - - element_map.erase(p_id); -} - -CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, nullptr); - return E->get().owner; -} - -bool BroadPhase2DHashGrid::is_static(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, false); - return E->get()._static; -} - -int BroadPhase2DHashGrid::get_subindex(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, -1); - return E->get().subindex; -} - -template <bool use_aabb, bool use_segment> -void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index) { - PosKey pk; - pk.x = p_cell.x; - pk.y = p_cell.y; - - uint32_t idx = pk.hash() % hash_table_size; - PosBin *pb = hash_table[idx]; - - while (pb) { - if (pb->key == pk) { - break; - } - - pb = pb->next; - } - - if (!pb) { - return; - } - - for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - if (index >= p_max_results) { - break; - } - if (E->key()->pass == pass) { - continue; - } - - E->key()->pass = pass; - - if (use_aabb && !p_aabb.intersects(E->key()->aabb)) { - continue; - } - - if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) { - continue; - } - - p_results[index] = E->key()->owner; - p_result_indices[index] = E->key()->subindex; - index++; - } - - for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) { - if (index >= p_max_results) { - break; - } - if (E->key()->pass == pass) { - continue; - } - - if (use_aabb && !p_aabb.intersects(E->key()->aabb)) { - continue; - } - - if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) { - continue; - } - - E->key()->pass = pass; - p_results[index] = E->key()->owner; - p_result_indices[index] = E->key()->subindex; - index++; - } -} - -int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - pass++; - - Vector2 dir = (p_to - p_from); - if (dir == Vector2()) { - return 0; - } - //avoid divisions by zero - dir.normalize(); - if (dir.x == 0.0) { - dir.x = 0.000001; - } - if (dir.y == 0.0) { - dir.y = 0.000001; - } - Vector2 delta = dir.abs(); - - delta.x = cell_size / delta.x; - delta.y = cell_size / delta.y; - - Point2i pos = (p_from / cell_size).floor(); - Point2i end = (p_to / cell_size).floor(); - - Point2i step = Vector2(SGN(dir.x), SGN(dir.y)); - - Vector2 max; - - if (dir.x < 0) { - max.x = (Math::floor((double)pos.x) * cell_size - p_from.x) / dir.x; - } else { - max.x = (Math::floor((double)pos.x + 1) * cell_size - p_from.x) / dir.x; - } - - if (dir.y < 0) { - max.y = (Math::floor((double)pos.y) * cell_size - p_from.y) / dir.y; - } else { - max.y = (Math::floor((double)pos.y + 1) * cell_size - p_from.y) / dir.y; - } - - int cullcount = 0; - _cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount); - - bool reached_x = false; - bool reached_y = false; - - while (true) { - if (max.x < max.y) { - max.x += delta.x; - pos.x += step.x; - } else { - max.y += delta.y; - pos.y += step.y; - } - - if (step.x > 0) { - if (pos.x >= end.x) { - reached_x = true; - } - } else if (pos.x <= end.x) { - reached_x = true; - } - - if (step.y > 0) { - if (pos.y >= end.y) { - reached_y = true; - } - } else if (pos.y <= end.y) { - reached_y = true; - } - - _cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount); - - if (reached_x && reached_y) { - break; - } - } - - for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (cullcount >= p_max_results) { - break; - } - if (E->key()->pass == pass) { - continue; - } - - E->key()->pass = pass; - - /* - if (use_aabb && !p_aabb.intersects(E->key()->aabb)) - continue; - */ - - if (!E->key()->aabb.intersects_segment(p_from, p_to)) { - continue; - } - - p_results[cullcount] = E->key()->owner; - p_result_indices[cullcount] = E->key()->subindex; - cullcount++; - } - - return cullcount; -} - -int BroadPhase2DHashGrid::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - pass++; - - Point2i from = (p_aabb.position / cell_size).floor(); - Point2i to = ((p_aabb.position + p_aabb.size) / cell_size).floor(); - int cullcount = 0; - - for (int i = from.x; i <= to.x; i++) { - for (int j = from.y; j <= to.y; j++) { - _cull<true, false>(Point2i(i, j), p_aabb, Point2(), Point2(), p_results, p_max_results, p_result_indices, cullcount); - } - } - - for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (cullcount >= p_max_results) { - break; - } - if (E->key()->pass == pass) { - continue; - } - - E->key()->pass = pass; - - if (!p_aabb.intersects(E->key()->aabb)) { - continue; - } - - /* - if (!E->key()->aabb.intersects_segment(p_from,p_to)) - continue; - */ - - p_results[cullcount] = E->key()->owner; - p_result_indices[cullcount] = E->key()->subindex; - cullcount++; - } - return cullcount; -} - -void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_callback = p_pair_callback; - pair_userdata = p_userdata; -} - -void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { - unpair_callback = p_unpair_callback; - unpair_userdata = p_userdata; -} - -void BroadPhase2DHashGrid::update() { -} - -BroadPhase2DSW *BroadPhase2DHashGrid::_create() { - return memnew(BroadPhase2DHashGrid); -} - -BroadPhase2DHashGrid::BroadPhase2DHashGrid() { - hash_table_size = GLOBAL_DEF("physics/2d/bp_hash_table_size", 4096); - ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/bp_hash_table_size", PropertyInfo(Variant::INT, "physics/2d/bp_hash_table_size", PROPERTY_HINT_RANGE, "0,8192,1,or_greater")); - hash_table_size = Math::larger_prime(hash_table_size); - hash_table = memnew_arr(PosBin *, hash_table_size); - - cell_size = GLOBAL_DEF("physics/2d/cell_size", 128); - ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/cell_size", PropertyInfo(Variant::INT, "physics/2d/cell_size", PROPERTY_HINT_RANGE, "0,512,1,or_greater")); - - large_object_min_surface = GLOBAL_DEF("physics/2d/large_object_surface_threshold_in_cells", 512); - ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/large_object_surface_threshold_in_cells", PropertyInfo(Variant::INT, "physics/2d/large_object_surface_threshold_in_cells", PROPERTY_HINT_RANGE, "0,1024,1,or_greater")); - - for (uint32_t i = 0; i < hash_table_size; i++) { - hash_table[i] = nullptr; - } - pass = 1; - - current = 0; -} - -BroadPhase2DHashGrid::~BroadPhase2DHashGrid() { - for (uint32_t i = 0; i < hash_table_size; i++) { - while (hash_table[i]) { - PosBin *pb = hash_table[i]; - hash_table[i] = pb->next; - memdelete(pb); - } - } - - memdelete_arr(hash_table); -} - -/* 3D version of voxel traversal: - -public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth) -{ - // Implementation is based on: - // "A Fast Voxel Traversal Algorithm for Ray Tracing" - // John Amanatides, Andrew Woo - // http://www.cse.yorku.ca/~amana/research/grid.pdf - // https://web.archive.org/web/20100616193049/http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf - - // NOTES: - // * This code assumes that the ray's position and direction are in 'cell coordinates', which means - // that one unit equals one cell in all directions. - // * When the ray doesn't start within the voxel grid, calculate the first position at which the - // ray could enter the grid. If it never enters the grid, there is nothing more to do here. - // * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite. - // * The Point3D structure is a simple structure having three integer fields (X, Y and Z). - - // The cell in which the ray starts. - Point3D start = GetCellAt(ray.Position); // Rounds the position's X, Y and Z down to the nearest integer values. - int x = start.X; - int y = start.Y; - int z = start.Z; - - // Determine which way we go. - int stepX = Math.Sign(ray.Direction.X); - int stepY = Math.Sign(ray.Direction.Y); - int stepZ = Math.Sign(ray.Direction.Z); - - // Calculate cell boundaries. When the step (i.e. direction sign) is positive, - // the next boundary is AFTER our current position, meaning that we have to add 1. - // Otherwise, it is BEFORE our current position, in which case we add nothing. - Point3D cellBoundary = new Point3D( - x + (stepX > 0 ? 1 : 0), - y + (stepY > 0 ? 1 : 0), - z + (stepZ > 0 ? 1 : 0)); - - // NOTE: For the following calculations, the result will be Single.PositiveInfinity - // when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand - // value of the division also equals zero, the result is Single.NaN, which is not OK. - - // Determine how far we can travel along the ray before we hit a voxel boundary. - Vector3 tMax = new Vector3( - (cellBoundary.X - ray.Position.X) / ray.Direction.X, // Boundary is a plane on the YZ axis. - (cellBoundary.Y - ray.Position.Y) / ray.Direction.Y, // Boundary is a plane on the XZ axis. - (cellBoundary.Z - ray.Position.Z) / ray.Direction.Z); // Boundary is a plane on the XY axis. - if (Single.IsNaN(tMax.X)) tMax.X = Single.PositiveInfinity; - if (Single.IsNaN(tMax.Y)) tMax.Y = Single.PositiveInfinity; - if (Single.IsNaN(tMax.Z)) tMax.Z = Single.PositiveInfinity; - - // Determine how far we must travel along the ray before we have crossed a gridcell. - Vector3 tDelta = new Vector3( - stepX / ray.Direction.X, // Crossing the width of a cell. - stepY / ray.Direction.Y, // Crossing the height of a cell. - stepZ / ray.Direction.Z); // Crossing the depth of a cell. - if (Single.IsNaN(tDelta.X)) tDelta.X = Single.PositiveInfinity; - if (Single.IsNaN(tDelta.Y)) tDelta.Y = Single.PositiveInfinity; - if (Single.IsNaN(tDelta.Z)) tDelta.Z = Single.PositiveInfinity; - - // For each step, determine which distance to the next voxel boundary is lowest (i.e. - // which voxel boundary is nearest) and walk that way. - for (int i = 0; i < maxDepth; i++) - { - // Return it. - yield return new Point3D(x, y, z); - - // Do the next step. - if (tMax.X < tMax.Y && tMax.X < tMax.Z) - { - // tMax.X is the lowest, an YZ cell boundary plane is nearest. - x += stepX; - tMax.X += tDelta.X; - } - else if (tMax.Y < tMax.Z) - { - // tMax.Y is the lowest, an XZ cell boundary plane is nearest. - y += stepY; - tMax.Y += tDelta.Y; - } - else - { - // tMax.Z is the lowest, an XY cell boundary plane is nearest. - z += stepZ; - tMax.Z += tDelta.Z; - } - } - - */ diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h deleted file mode 100644 index de1ada0932..0000000000 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ /dev/null @@ -1,187 +0,0 @@ -/*************************************************************************/ -/* broad_phase_2d_hash_grid.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef BROAD_PHASE_2D_HASH_GRID_H -#define BROAD_PHASE_2D_HASH_GRID_H - -#include "broad_phase_2d_sw.h" -#include "core/map.h" - -class BroadPhase2DHashGrid : public BroadPhase2DSW { - struct PairData { - bool colliding; - int rc; - void *ud; - PairData() { - colliding = false; - rc = 1; - ud = nullptr; - } - }; - - struct Element { - ID self; - CollisionObject2DSW *owner; - bool _static; - Rect2 aabb; - int subindex; - uint64_t pass; - Map<Element *, PairData *> paired; - }; - - struct RC { - int ref; - - _FORCE_INLINE_ int inc() { - ref++; - return ref; - } - _FORCE_INLINE_ int dec() { - ref--; - return ref; - } - - _FORCE_INLINE_ RC() { - ref = 0; - } - }; - - Map<ID, Element> element_map; - Map<Element *, RC> large_elements; - - ID current; - - uint64_t pass; - - struct PairKey { - union { - struct { - ID a; - ID b; - }; - uint64_t key; - }; - - _FORCE_INLINE_ bool operator<(const PairKey &p_key) const { - return key < p_key.key; - } - - PairKey() { key = 0; } - PairKey(ID p_a, ID p_b) { - if (p_a > p_b) { - a = p_b; - b = p_a; - } else { - a = p_a; - b = p_b; - } - } - }; - - Map<PairKey, PairData> pair_map; - - int cell_size; - int large_object_min_surface; - - PairCallback pair_callback; - void *pair_userdata; - UnpairCallback unpair_callback; - void *unpair_userdata; - - void _enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static); - void _exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static); - template <bool use_aabb, bool use_segment> - _FORCE_INLINE_ void _cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index); - - struct PosKey { - union { - struct { - int32_t x; - int32_t y; - }; - uint64_t key; - }; - - _FORCE_INLINE_ uint32_t hash() const { - uint64_t k = key; - k = (~k) + (k << 18); // k = (k << 18) - k - 1; - k = k ^ (k >> 31); - k = k * 21; // k = (k + (k << 2)) + (k << 4); - k = k ^ (k >> 11); - k = k + (k << 6); - k = k ^ (k >> 22); - return k; - } - - bool operator==(const PosKey &p_key) const { return key == p_key.key; } - _FORCE_INLINE_ bool operator<(const PosKey &p_key) const { - return key < p_key.key; - } - }; - - struct PosBin { - PosKey key; - Map<Element *, RC> object_set; - Map<Element *, RC> static_object_set; - PosBin *next; - }; - - uint32_t hash_table_size; - PosBin **hash_table; - - void _pair_attempt(Element *p_elem, Element *p_with); - void _unpair_attempt(Element *p_elem, Element *p_with); - void _check_motion(Element *p_elem); - -public: - virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0); - virtual void move(ID p_id, const Rect2 &p_aabb); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); - - virtual CollisionObject2DSW *get_object(ID p_id) const; - virtual bool is_static(ID p_id) const; - virtual int get_subindex(ID p_id) const; - - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); - - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); - - virtual void update(); - - static BroadPhase2DSW *_create(); - - BroadPhase2DHashGrid(); - ~BroadPhase2DHashGrid(); -}; - -#endif // BROAD_PHASE_2D_HASH_GRID_H diff --git a/servers/physics_2d/broad_phase_2d_sw.cpp b/servers/physics_2d/broad_phase_2d_sw.cpp index 5ba557e70a..7f0af48b1f 100644 --- a/servers/physics_2d/broad_phase_2d_sw.cpp +++ b/servers/physics_2d/broad_phase_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h index e4444cd180..0f82f06b9c 100644 --- a/servers/physics_2d/broad_phase_2d_sw.h +++ b/servers/physics_2d/broad_phase_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -48,7 +48,7 @@ public: typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata); // 0 is an invalid ID - virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0) = 0; + virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0; virtual void move(ID p_id, const Rect2 &p_aabb) = 0; virtual void set_static(ID p_id, bool p_static) = 0; virtual void remove(ID p_id) = 0; diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index 6931d96fe4..5d1ef83165 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -47,8 +47,6 @@ void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_tra if (!pending_shape_update_list.in_list()) { PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } - // _update_shapes(); - // _shapes_changed(); } void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) { @@ -61,8 +59,6 @@ void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) { if (!pending_shape_update_list.in_list()) { PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } - // _update_shapes(); - // _shapes_changed(); } void CollisionObject2DSW::set_shape_metadata(int p_index, const Variant &p_metadata) { @@ -79,11 +75,9 @@ void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_ if (!pending_shape_update_list.in_list()) { PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } - // _update_shapes(); - // _shapes_changed(); } -void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) { +void CollisionObject2DSW::set_shape_disabled(int p_idx, bool p_disabled) { ERR_FAIL_INDEX(p_idx, shapes.size()); CollisionObject2DSW::Shape &shape = shapes.write[p_idx]; @@ -103,12 +97,10 @@ void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) { if (!pending_shape_update_list.in_list()) { PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } - //_update_shapes(); } else if (!p_disabled && shape.bpid == 0) { if (!pending_shape_update_list.in_list()) { PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } - //_update_shapes(); // automatically adds shape with bpid == 0 } } @@ -177,24 +169,23 @@ void CollisionObject2DSW::_update_shapes() { for (int i = 0; i < shapes.size(); i++) { Shape &s = shapes.write[i]; - if (s.disabled) { continue; } - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } - //not quite correct, should compute the next matrix.. Rect2 shape_aabb = s.shape->get_aabb(); Transform2D xform = transform * s.xform; shape_aabb = xform.xform(shape_aabb); + shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); s.aabb_cache = shape_aabb; - s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); - space->get_broadphase()->move(s.bpid, s.aabb_cache); + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); } } @@ -209,11 +200,6 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { continue; } - if (s.bpid == 0) { - s.bpid = space->get_broadphase()->create(this, i); - space->get_broadphase()->set_static(s.bpid, _static); - } - //not quite correct, should compute the next matrix.. Rect2 shape_aabb = s.shape->get_aabb(); Transform2D xform = transform * s.xform; @@ -221,6 +207,11 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion s.aabb_cache = shape_aabb; + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + space->get_broadphase()->move(s.bpid, shape_aabb); } } diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 8caa53680d..55ffa9b1b8 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,7 +32,7 @@ #define COLLISION_OBJECT_2D_SW_H #include "broad_phase_2d_sw.h" -#include "core/self_list.h" +#include "core/templates/self_list.h" #include "servers/physics_server_2d.h" #include "shape_2d_sw.h" @@ -61,7 +61,7 @@ private: Variant metadata; bool disabled; bool one_way_collision; - float one_way_collision_margin; + real_t one_way_collision_margin; Shape() { disabled = false; one_way_collision = false; @@ -118,10 +118,6 @@ public: void set_shape_metadata(int p_index, const Variant &p_metadata); _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } - _FORCE_INLINE_ bool is_shape_disabled(int p_index) const { - CRASH_BAD_INDEX(p_index, shapes.size()); - return shapes[p_index].disabled; - } _FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const { CRASH_BAD_INDEX(p_index, shapes.size()); return shapes[p_index].shape; @@ -143,17 +139,17 @@ public: return shapes[p_index].metadata; } - _FORCE_INLINE_ Transform2D get_transform() const { return transform; } - _FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; } _FORCE_INLINE_ Space2DSW *get_space() const { return space; } - void set_shape_as_disabled(int p_idx, bool p_disabled); - _FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const { - CRASH_BAD_INDEX(p_idx, shapes.size()); + void set_shape_disabled(int p_idx, bool p_disabled); + _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { + ERR_FAIL_INDEX_V(p_idx, shapes.size(), false); return shapes[p_idx].disabled; } - _FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, float p_margin) { + _FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, real_t p_margin) { CRASH_BAD_INDEX(p_idx, shapes.size()); shapes.write[p_idx].one_way_collision = p_one_way_collision; shapes.write[p_idx].one_way_collision_margin = p_margin; @@ -163,7 +159,7 @@ public: return shapes[p_idx].one_way_collision; } - _FORCE_INLINE_ float get_shape_one_way_collision_margin(int p_idx) const { + _FORCE_INLINE_ real_t get_shape_one_way_collision_margin(int p_idx) const { CRASH_BAD_INDEX(p_idx, shapes.size()); return shapes[p_idx].one_way_collision_margin; } @@ -190,7 +186,11 @@ public: void set_pickable(bool p_pickable) { pickable = p_pickable; } _FORCE_INLINE_ bool is_pickable() const { return pickable; } - _FORCE_INLINE_ bool test_collision_mask(CollisionObject2DSW *p_other) const { + _FORCE_INLINE_ bool collides_with(CollisionObject2DSW *p_other) const { + return p_other->collision_layer & collision_mask; + } + + _FORCE_INLINE_ bool interacts_with(CollisionObject2DSW *p_other) const { return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; } diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp index d993754fee..b1aee01bde 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -88,7 +88,7 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_ #endif Vector2 n = p_collector->normal; - Vector2 t = n.tangent(); + Vector2 t = n.orthogonal(); real_t dA = n.dot(p_points_A[0]); real_t dB = n.dot(p_points_B[0]); @@ -209,7 +209,7 @@ public: if (!test_axis(na)) { return false; } - if (!test_axis(na.tangent())) { + if (!test_axis(na.orthogonal())) { return false; } } @@ -219,7 +219,7 @@ public: if (!test_axis(nb)) { return false; } - if (!test_axis(nb.tangent())) { + if (!test_axis(nb.orthogonal())) { return false; } } @@ -450,7 +450,7 @@ static void _collision_segment_circle(const Shape2DSW *p_a, const Transform2D &p //segment normal if (!separator.test_axis( - (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().tangent())) { + (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().orthogonal())) { return; } @@ -560,16 +560,18 @@ static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D & return; } - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) { + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { return; } - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { return; } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { return; } - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { return; } @@ -715,11 +717,13 @@ static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p return; } + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + //capsule endpoints - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) { + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) { return; } - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) { + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { return; } @@ -864,9 +868,11 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D Transform2D boxinv = p_transform_a.affine_inverse(); + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + for (int i = 0; i < 2; i++) { { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { return; @@ -874,7 +880,7 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D } if (castA) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; capsule_endpoint -= p_motion_a; if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { @@ -883,7 +889,7 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D } if (castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; capsule_endpoint += p_motion_b; if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { @@ -892,7 +898,7 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D } if (castA && castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; capsule_endpoint -= p_motion_a; capsule_endpoint += p_motion_b; @@ -900,6 +906,8 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D return; } } + + capsule_dir *= -1.0; } separator.generate_contacts(); @@ -994,16 +1002,22 @@ static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D & //capsule endpoints + real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); for (int i = 0; i < 2; i++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (i == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir_A; + real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (j == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir_B; if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) { return; } + + capsule_dir_B *= -1.0; } + + capsule_dir_A *= -1.0; } separator.generate_contacts(); @@ -1034,12 +1048,15 @@ static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transf for (int i = 0; i < convex_B->get_point_count(); i++) { Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); + real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); for (int j = 0; j < 2; j++) { - Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (j == 0 ? 0.5 : -0.5); + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir; if (TEST_POINT(capsule_endpoint_A, cpoint)) { return; } + + capsule_dir *= -1.0; } if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { @@ -1097,14 +1114,14 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); - ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_LINE, false); - //ERR_FAIL_COND_V(type_A==PhysicsServer2D::SHAPE_RAY,false); + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_MARGIN, false); + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_A->is_concave(), false); PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); - ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_LINE, false); - //ERR_FAIL_COND_V(type_B==PhysicsServer2D::SHAPE_RAY,false); + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_MARGIN, false); + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); ERR_FAIL_COND_V(p_shape_B->is_concave(), false); static const CollisionFunc collision_table[5][5] = { diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h index 6bb485f561..49cc5176f9 100644 --- a/servers/physics_2d/collision_solver_2d_sat.h +++ b/servers/physics_2d/collision_solver_2d_sat.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index beba709807..ae50615953 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -34,20 +34,20 @@ #define collision_solver sat_2d_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { - const LineShape2DSW *line = static_cast<const LineShape2DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_LINE) { +bool CollisionSolver2DSW::solve_static_world_margin(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + const WorldMarginShape2DSW *world_margin = static_cast<const WorldMarginShape2DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_MARGIN) { return false; } - Vector2 n = p_transform_A.basis_xform(line->get_normal()).normalized(); - Vector2 p = p_transform_A.xform(line->get_normal() * line->get_d()); + Vector2 n = p_transform_A.basis_xform(world_margin->get_normal()).normalized(); + Vector2 p = p_transform_A.xform(world_margin->get_normal() * world_margin->get_d()); real_t d = n.dot(p); Vector2 supports[2]; int support_count; - p_shape_B->get_supports(p_transform_A.affine_inverse().basis_xform(-n).normalized(), supports, support_count); + p_shape_B->get_supports(p_transform_B.affine_inverse().basis_xform(-n).normalized(), supports, support_count); bool found = false; @@ -73,14 +73,14 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Tr return found; } -bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis) { - const RayShape2DSW *ray = static_cast<const RayShape2DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_RAY) { +bool CollisionSolver2DSW::solve_separation_ray(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin) { + const SeparationRayShape2DSW *ray = static_cast<const SeparationRayShape2DSW *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) { return false; } Vector2 from = p_transform_A.get_origin(); - Vector2 to = from + p_transform_A[1] * ray->get_length(); + Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin); if (p_motion_A != Vector2()) { //not the best but should be enough Vector2 normal = (to - from).normalized(); @@ -94,14 +94,30 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector Vector2 p, n; if (!p_shape_B->intersect_segment(from, to, p, n)) { - if (sep_axis) { - *sep_axis = p_transform_A[1].normalized(); + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts when the ray is fully contained inside the shape. + if (n == Vector2()) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts in the wrong direction. + if (n.dot(from - to) < CMP_EPSILON) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); } return false; } Vector2 support_B = p_transform_B.xform(p); - if (ray->get_slips_on_slope()) { + if (ray->get_slide_on_slope()) { Vector2 global_n = invb.basis_xform_inv(n).normalized(); support_B = support_A + (support_B - support_A).length() * global_n; } @@ -133,23 +149,23 @@ struct _ConcaveCollisionInfo2D { Vector2 *sep_axis; }; -void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { +bool CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata); cinfo.aabb_tests++; - if (!cinfo.result_callback && cinfo.collided) { - return; //already collided and no contacts requested, don't test anymore - } bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B); if (!collided) { - return; + return false; } cinfo.collided = true; cinfo.collisions++; + + // Stop at first collision if contacts are not needed. + return !cinfo.result_callback; } -bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { +bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { const ConcaveShape2DSW *concave_B = static_cast<const ConcaveShape2DSW *>(p_shape_B); _ConcaveCollisionInfo2D cinfo; @@ -162,7 +178,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transf cinfo.swap_result = p_swap_result; cinfo.collided = false; cinfo.collisions = 0; - cinfo.sep_axis = sep_axis; + cinfo.sep_axis = r_sep_axis; cinfo.margin_A = p_margin_A; cinfo.margin_B = p_margin_B; @@ -193,7 +209,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transf return cinfo.collided; } -bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { +bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); bool concave_A = p_shape_A->is_concave(); @@ -209,26 +225,26 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p swap = true; } - if (type_A == PhysicsServer2D::SHAPE_LINE) { - if (type_B == PhysicsServer2D::SHAPE_LINE || type_B == PhysicsServer2D::SHAPE_RAY) { + if (type_A == PhysicsServer2D::SHAPE_WORLD_MARGIN) { + if (type_B == PhysicsServer2D::SHAPE_WORLD_MARGIN) { return false; } if (swap) { - return solve_static_line(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_static_world_margin(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); } else { - return solve_static_line(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_static_world_margin(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); } - } else if (type_A == PhysicsServer2D::SHAPE_RAY) { - if (type_B == PhysicsServer2D::SHAPE_RAY) { + } else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) { return false; //no ray-ray } if (swap) { - return solve_raycast(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis); + return solve_separation_ray(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, r_sep_axis, p_margin_B); } else { - return solve_raycast(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis); + return solve_separation_ray(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A); } } else if (concave_B) { @@ -237,12 +253,12 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p } if (!swap) { - return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B); + return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); } else { - return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, sep_axis, margin_A, margin_B); + return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B); } } else { - return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B); + return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); } } diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index f39cfee0a9..62fccc4ff3 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -38,13 +38,13 @@ public: typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); private: - static bool solve_static_line(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static void concave_callback(void *p_userdata, Shape2DSW *p_convex); - static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = nullptr); + static bool solve_static_world_margin(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool concave_callback(void *p_userdata, Shape2DSW *p_convex); + static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_separation_ray(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin = 0); public: - static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); }; #endif // COLLISION_SOLVER_2D_SW_H diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index d8751f588e..5e8dfbe570 100644 --- a/servers/physics_2d/constraint_2d_sw.h +++ b/servers/physics_2d/constraint_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -37,8 +37,6 @@ class Constraint2DSW { Body2DSW **_body_ptr; int _body_count; uint64_t island_step; - Constraint2DSW *island_next; - Constraint2DSW *island_list_next; bool disabled_collisions_between_bodies; RID self; @@ -58,12 +56,6 @@ public: _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ Constraint2DSW *get_island_next() const { return island_next; } - _FORCE_INLINE_ void set_island_next(Constraint2DSW *p_next) { island_next = p_next; } - - _FORCE_INLINE_ Constraint2DSW *get_island_list_next() const { return island_list_next; } - _FORCE_INLINE_ void set_island_list_next(Constraint2DSW *p_next) { island_list_next = p_next; } - _FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ int get_body_count() const { return _body_count; } @@ -71,6 +63,7 @@ public: _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0; virtual ~Constraint2DSW() {} diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index e7d26645e9..5a0a628fbc 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -55,6 +55,14 @@ * SOFTWARE. */ +void Joint2DSW::copy_settings_from(Joint2DSW *p_joint) { + set_self(p_joint->get_self()); + set_max_force(p_joint->get_max_force()); + set_bias(p_joint->get_bias()); + set_max_bias(p_joint->get_max_bias()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); +} + static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) { real_t value = 0; @@ -75,9 +83,9 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const static inline Vector2 relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) { - Vector2 sum = a->get_linear_velocity() - rA.tangent() * a->get_angular_velocity(); + Vector2 sum = a->get_linear_velocity() - rA.orthogonal() * a->get_angular_velocity(); if (b) { - return (b->get_linear_velocity() - rB.tangent() * b->get_angular_velocity()) - sum; + return (b->get_linear_velocity() - rB.orthogonal() * b->get_angular_velocity()) - sum; } else { return -sum; } @@ -89,8 +97,16 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto } bool PinJoint2DSW::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + Space2DSW *space = A->get_space(); ERR_FAIL_COND_V(!space, false); + rA = A->get_transform().basis_xform(anchor_A); rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B; @@ -135,12 +151,6 @@ bool PinJoint2DSW::setup(real_t p_step) { bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); - // apply accumulated impulse - A->apply_impulse(-P, rA); - if (B) { - B->apply_impulse(P, rB); - } - return true; } @@ -148,6 +158,18 @@ inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) { return Vector2(p_other * p_vec.y, -p_other * p_vec.x); } +bool PinJoint2DSW::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-P, rA); + } + if (B && dynamic_B) { + B->apply_impulse(P, rB); + } + + return true; +} + void PinJoint2DSW::solve(real_t p_step) { // compute relative velocity Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity()); @@ -161,8 +183,10 @@ void PinJoint2DSW::solve(real_t p_step) { Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); - A->apply_impulse(-impulse, rA); - if (B) { + if (dynamic_A) { + A->apply_impulse(-impulse, rA); + } + if (B && dynamic_B) { B->apply_impulse(impulse, rB); } @@ -197,15 +221,6 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p } } -PinJoint2DSW::~PinJoint2DSW() { - if (A) { - A->remove_constraint(this); - } - if (B) { - B->remove_constraint(this); - } -} - ////////////////////////////////////////////// ////////////////////////////////////////////// ////////////////////////////////////////////// @@ -258,13 +273,22 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { } bool GrooveJoint2DSW::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + Space2DSW *space = A->get_space(); + ERR_FAIL_COND_V(!space, false); + // calculate endpoints in worldspace Vector2 ta = A->get_transform().xform(A_groove_1); Vector2 tb = A->get_transform().xform(A_groove_2); - Space2DSW *space = A->get_space(); // calculate axis - Vector2 n = -(tb - ta).tangent().normalized(); + Vector2 n = -(tb - ta).orthogonal().normalized(); real_t d = ta.dot(n); xf_normal = n; @@ -282,7 +306,7 @@ bool GrooveJoint2DSW::setup(real_t p_step) { } else { clamp = 0.0f; //joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); - rA = ((-n.tangent() * -td) + n * d) - A->get_transform().get_origin(); + rA = ((-n.orthogonal() * -td) + n * d) - A->get_transform().get_origin(); } // Calculate mass tensor @@ -298,16 +322,24 @@ bool GrooveJoint2DSW::setup(real_t p_step) { Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); real_t _b = get_bias(); - gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).clamped(get_max_bias()); - - // apply accumulated impulse - A->apply_impulse(-jn_acc, rA); - B->apply_impulse(jn_acc, rB); + gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias()); correct = true; return true; } +bool GrooveJoint2DSW::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-jn_acc, rA); + } + if (dynamic_B) { + B->apply_impulse(jn_acc, rB); + } + + return true; +} + void GrooveJoint2DSW::solve(real_t p_step) { // compute impulse Vector2 vr = relative_velocity(A, B, rA, rB); @@ -316,12 +348,16 @@ void GrooveJoint2DSW::solve(real_t p_step) { Vector2 jOld = jn_acc; j += jOld; - jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).clamped(jn_max); + jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max); j = jn_acc - jOld; - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } } GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) : @@ -332,22 +368,24 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_ A_groove_1 = A->get_inv_transform().xform(p_a_groove1); A_groove_2 = A->get_inv_transform().xform(p_a_groove2); B_anchor = B->get_inv_transform().xform(p_b_anchor); - A_groove_normal = -(A_groove_2 - A_groove_1).normalized().tangent(); + A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal(); A->add_constraint(this, 0); B->add_constraint(this, 1); } -GrooveJoint2DSW::~GrooveJoint2DSW() { - A->remove_constraint(this); - B->remove_constraint(this); -} - ////////////////////////////////////////////// ////////////////////////////////////////////// ////////////////////////////////////////////// bool DampedSpringJoint2DSW::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + rA = A->get_transform().basis_xform(anchor_A); rB = B->get_transform().basis_xform(anchor_B); @@ -366,12 +404,21 @@ bool DampedSpringJoint2DSW::setup(real_t p_step) { target_vrn = 0.0f; v_coef = 1.0f - Math::exp(-damping * (p_step)*k); - // apply spring force + // Calculate spring force. real_t f_spring = (rest_length - dist) * stiffness; - Vector2 j = n * f_spring * (p_step); + j = n * f_spring * (p_step); - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); + return true; +} + +bool DampedSpringJoint2DSW::pre_solve(real_t p_step) { + // Apply spring force. + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } return true; } @@ -386,8 +433,12 @@ void DampedSpringJoint2DSW::solve(real_t p_step) { target_vrn = vrn + v_damp; Vector2 j = n * v_damp * n_mass; - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } } void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { @@ -434,8 +485,3 @@ DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Ve A->add_constraint(this, 0); B->add_constraint(this, 1); } - -DampedSpringJoint2DSW::~DampedSpringJoint2DSW() { - A->remove_constraint(this); - B->remove_constraint(this); -} diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h index 3c8aab77c8..ccc5c585a0 100644 --- a/servers/physics_2d/joints_2d_sw.h +++ b/servers/physics_2d/joints_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -39,6 +39,10 @@ class Joint2DSW : public Constraint2DSW { real_t bias; real_t max_bias; +protected: + bool dynamic_A = false; + bool dynamic_B = false; + public: _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } _FORCE_INLINE_ real_t get_max_force() const { return max_force; } @@ -49,12 +53,27 @@ public: _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } - virtual PhysicsServer2D::JointType get_type() const = 0; + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return false; } + virtual void solve(real_t p_step) override {} + + void copy_settings_from(Joint2DSW *p_joint); + + virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; } Joint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) : Constraint2DSW(p_body_ptr, p_body_count) { bias = 0; max_force = max_bias = 3.40282e+38; }; + + virtual ~Joint2DSW() { + for (int i = 0; i < get_body_count(); i++) { + Body2DSW *body = get_body_ptr()[i]; + if (body) { + body->remove_constraint(this, i); + } + } + }; }; class PinJoint2DSW : public Joint2DSW { @@ -76,16 +95,16 @@ class PinJoint2DSW : public Joint2DSW { real_t softness; public: - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_PIN; } + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); real_t get_param(PhysicsServer2D::PinJointParam p_param) const; PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b = nullptr); - ~PinJoint2DSW(); }; class GrooveJoint2DSW : public Joint2DSW { @@ -113,13 +132,13 @@ class GrooveJoint2DSW : public Joint2DSW { bool correct; public: - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_GROOVE; } + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b); - ~GrooveJoint2DSW(); }; class DampedSpringJoint2DSW : public Joint2DSW { @@ -141,21 +160,22 @@ class DampedSpringJoint2DSW : public Joint2DSW { Vector2 rA, rB; Vector2 n; + Vector2 j; real_t n_mass; real_t target_vrn; real_t v_coef; public: - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_DAMPED_SPRING; } + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b); - ~DampedSpringJoint2DSW(); }; #endif // JOINTS_2D_SW_H diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp index 1b396190e9..813e5ddc17 100644 --- a/servers/physics_2d/physics_server_2d_sw.cpp +++ b/servers/physics_2d/physics_server_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,12 +30,11 @@ #include "physics_server_2d_sw.h" -#include "broad_phase_2d_basic.h" -#include "broad_phase_2d_hash_grid.h" +#include "broad_phase_2d_bvh.h" #include "collision_solver_2d_sw.h" +#include "core/config/project_settings.h" #include "core/debugger/engine_debugger.h" #include "core/os/os.h" -#include "core/project_settings.h" #define FLUSH_QUERY_CHECK(m_object) \ ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); @@ -43,11 +42,11 @@ RID PhysicsServer2DSW::_shape_create(ShapeType p_shape) { Shape2DSW *shape = nullptr; switch (p_shape) { - case SHAPE_LINE: { - shape = memnew(LineShape2DSW); + case SHAPE_WORLD_MARGIN: { + shape = memnew(WorldMarginShape2DSW); } break; - case SHAPE_RAY: { - shape = memnew(RayShape2DSW); + case SHAPE_SEPARATION_RAY: { + shape = memnew(SeparationRayShape2DSW); } break; case SHAPE_SEGMENT: { shape = memnew(SegmentShape2DSW); @@ -79,12 +78,12 @@ RID PhysicsServer2DSW::_shape_create(ShapeType p_shape) { return id; } -RID PhysicsServer2DSW::line_shape_create() { - return _shape_create(SHAPE_LINE); +RID PhysicsServer2DSW::world_margin_shape_create() { + return _shape_create(SHAPE_WORLD_MARGIN); } -RID PhysicsServer2DSW::ray_shape_create() { - return _shape_create(SHAPE_RAY); +RID PhysicsServer2DSW::separation_ray_shape_create() { + return _shape_create(SHAPE_SEPARATION_RAY); } RID PhysicsServer2DSW::segment_shape_create() { @@ -149,24 +148,19 @@ void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 & return; } + Vector2 rel_dir = (p_point_A - p_point_B); + real_t rel_length2 = rel_dir.length_squared(); if (cbk->valid_dir != Vector2()) { - if (p_point_A.distance_squared_to(p_point_B) > cbk->valid_depth * cbk->valid_depth) { - cbk->invalid_by_dir++; - return; - } - Vector2 rel_dir = (p_point_A - p_point_B).normalized(); - - if (cbk->valid_dir.dot(rel_dir) < Math_SQRT12) { //sqrt(2)/2.0 - 45 degrees - cbk->invalid_by_dir++; - - /* - print_line("A: "+p_point_A); - print_line("B: "+p_point_B); - print_line("discard too angled "+rtos(cbk->valid_dir.dot((p_point_A-p_point_B)))); - print_line("resnorm: "+(p_point_A-p_point_B).normalized()); - print_line("distance: "+rtos(p_point_A.distance_to(p_point_B))); - */ - return; + if (cbk->valid_depth < 10e20) { + if (rel_length2 > cbk->valid_depth * cbk->valid_depth || + (rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) { + cbk->invalid_by_dir++; + return; + } + } else { + if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) { + return; + } } } @@ -182,8 +176,7 @@ void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 & } } - real_t d = p_point_A.distance_squared_to(p_point_B); - if (d < min_depth) { + if (rel_length2 < min_depth) { return; } cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; @@ -373,7 +366,7 @@ void PhysicsServer2DSW::area_set_shape_disabled(RID p_area, int p_shape, bool p_ ERR_FAIL_INDEX(p_shape, area->get_shape_count()); FLUSH_QUERY_CHECK(area); - area->set_shape_as_disabled(p_shape, p_disabled); + area->set_shape_disabled(p_shape, p_disabled); } int PhysicsServer2DSW::area_get_shape_count(RID p_area) const { @@ -554,7 +547,7 @@ void PhysicsServer2DSW::body_set_space(RID p_body, RID p_space) { return; //pointless } - body->clear_constraint_map(); + body->clear_constraint_list(); body->set_space(space); }; @@ -670,10 +663,10 @@ void PhysicsServer2DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); FLUSH_QUERY_CHECK(body); - body->set_shape_as_disabled(p_shape_idx, p_disabled); + body->set_shape_disabled(p_shape_idx, p_disabled); } -void PhysicsServer2DSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, float p_margin) { +void PhysicsServer2DSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); @@ -933,10 +926,10 @@ int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } -void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) { +void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); - body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method, p_udata); + body->set_force_integration_callback(p_callable, p_udata); } bool PhysicsServer2DSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { @@ -953,7 +946,7 @@ void PhysicsServer2DSW::body_set_pickable(RID p_body, bool p_pickable) { body->set_pickable(p_pickable); } -bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, MotionResult *r_result, bool p_exclude_raycast_shapes) { +bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -961,16 +954,7 @@ bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from, _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); -} - -int PhysicsServer2DSW::body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { - Body2DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - ERR_FAIL_COND_V(!body->get_space(), false); - ERR_FAIL_COND_V(body->get_space()->is_locked(), false); - - return body->get_space()->test_body_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); } PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) { @@ -991,6 +975,24 @@ PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) { /* JOINT API */ +RID PhysicsServer2DSW::joint_create() { + Joint2DSW *joint = memnew(Joint2DSW); + RID joint_rid = joint_owner.make_rid(joint); + joint->set_self(joint_rid); + return joint_rid; +} + +void PhysicsServer2DSW::joint_clear(RID p_joint) { + Joint2DSW *joint = joint_owner.getornull(p_joint); + if (joint->get_type() != JOINT_TYPE_MAX) { + Joint2DSW *empty_joint = memnew(Joint2DSW); + empty_joint->copy_settings_from(joint); + + joint_owner.replace(p_joint, empty_joint); + memdelete(joint); + } +} + void PhysicsServer2DSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { Joint2DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); @@ -1054,52 +1056,63 @@ bool PhysicsServer2DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) return joint->is_disabled_collisions_between_bodies(); } -RID PhysicsServer2DSW::pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID p_body_b) { +void PhysicsServer2DSW::joint_make_pin(RID p_joint, const Vector2 &p_pos, RID p_body_a, RID p_body_b) { Body2DSW *A = body_owner.getornull(p_body_a); - ERR_FAIL_COND_V(!A, RID()); + ERR_FAIL_COND(!A); Body2DSW *B = nullptr; if (body_owner.owns(p_body_b)) { B = body_owner.getornull(p_body_b); - ERR_FAIL_COND_V(!B, RID()); + ERR_FAIL_COND(!B); } + Joint2DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); + Joint2DSW *joint = memnew(PinJoint2DSW(p_pos, A, B)); - RID self = joint_owner.make_rid(joint); - joint->set_self(self); - return self; + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); } -RID PhysicsServer2DSW::groove_joint_create(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) { +void PhysicsServer2DSW::joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) { Body2DSW *A = body_owner.getornull(p_body_a); - ERR_FAIL_COND_V(!A, RID()); + ERR_FAIL_COND(!A); Body2DSW *B = body_owner.getornull(p_body_b); - ERR_FAIL_COND_V(!B, RID()); + ERR_FAIL_COND(!B); + + Joint2DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint2DSW *joint = memnew(GrooveJoint2DSW(p_a_groove1, p_a_groove2, p_b_anchor, A, B)); - RID self = joint_owner.make_rid(joint); - joint->set_self(self); - return self; + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); } -RID PhysicsServer2DSW::damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) { +void PhysicsServer2DSW::joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) { Body2DSW *A = body_owner.getornull(p_body_a); - ERR_FAIL_COND_V(!A, RID()); + ERR_FAIL_COND(!A); Body2DSW *B = body_owner.getornull(p_body_b); - ERR_FAIL_COND_V(!B, RID()); + ERR_FAIL_COND(!B); + + Joint2DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint2DSW *joint = memnew(DampedSpringJoint2DSW(p_anchor_a, p_anchor_b, A, B)); - RID self = joint_owner.make_rid(joint); - joint->set_self(self); - return self; + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); } void PhysicsServer2DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { Joint2DSW *j = joint_owner.getornull(p_joint); ERR_FAIL_COND(!j); - ERR_FAIL_COND(j->get_type() != JOINT_PIN); + ERR_FAIL_COND(j->get_type() != JOINT_TYPE_PIN); PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW *>(j); pin_joint->set_param(p_param, p_value); @@ -1108,7 +1121,7 @@ void PhysicsServer2DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t PhysicsServer2DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { Joint2DSW *j = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!j, 0); - ERR_FAIL_COND_V(j->get_type() != JOINT_PIN, 0); + ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_PIN, 0); PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW *>(j); return pin_joint->get_param(p_param); @@ -1117,7 +1130,7 @@ real_t PhysicsServer2DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param void PhysicsServer2DSW::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) { Joint2DSW *j = joint_owner.getornull(p_joint); ERR_FAIL_COND(!j); - ERR_FAIL_COND(j->get_type() != JOINT_DAMPED_SPRING); + ERR_FAIL_COND(j->get_type() != JOINT_TYPE_DAMPED_SPRING); DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW *>(j); dsj->set_param(p_param, p_value); @@ -1126,7 +1139,7 @@ void PhysicsServer2DSW::damped_spring_joint_set_param(RID p_joint, DampedSpringP real_t PhysicsServer2DSW::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const { Joint2DSW *j = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!j, 0); - ERR_FAIL_COND_V(j->get_type() != JOINT_DAMPED_SPRING, 0); + ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW *>(j); return dsj->get_param(p_param); @@ -1134,7 +1147,7 @@ real_t PhysicsServer2DSW::damped_spring_joint_get_param(RID p_joint, DampedSprin PhysicsServer2D::JointType PhysicsServer2DSW::joint_get_type(RID p_joint) const { Joint2DSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, JOINT_PIN); + ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); return joint->get_type(); } @@ -1215,6 +1228,10 @@ void PhysicsServer2DSW::set_active(bool p_active) { active = p_active; }; +void PhysicsServer2DSW::set_collision_iterations(int p_iterations) { + iterations = p_iterations; +}; + void PhysicsServer2DSW::init() { doing_sync = false; last_step = 0.001; @@ -1230,8 +1247,6 @@ void PhysicsServer2DSW::step(real_t p_step) { _update_shapes(); - doing_sync = false; - last_step = p_step; PhysicsDirectBodyState2DSW::singleton->step = p_step; island_count = 0; @@ -1333,19 +1348,14 @@ int PhysicsServer2DSW::get_process_info(ProcessInfo p_info) { PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = nullptr; -PhysicsServer2DSW::PhysicsServer2DSW() { +PhysicsServer2DSW::PhysicsServer2DSW(bool p_using_threads) { singletonsw = this; - BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create; - //BroadPhase2DSW::create_func=BroadPhase2DBasic::_create; + BroadPhase2DSW::create_func = BroadPhase2DBVH::_create; active = true; island_count = 0; active_objects = 0; collision_pairs = 0; -#ifdef NO_THREADS - using_threads = false; -#else - using_threads = int(ProjectSettings::get_singleton()->get("physics/2d/thread_model")) == 2; -#endif + using_threads = p_using_threads; flushing_queries = false; }; diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h index e88db28056..c5b6d550d7 100644 --- a/servers/physics_2d/physics_server_2d_sw.h +++ b/servers/physics_2d/physics_server_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,7 +31,7 @@ #ifndef PHYSICS_2D_SERVER_SW #define PHYSICS_2D_SERVER_SW -#include "core/rid_owner.h" +#include "core/templates/rid_owner.h" #include "joints_2d_sw.h" #include "servers/physics_server_2d.h" #include "shape_2d_sw.h" @@ -61,11 +61,11 @@ class PhysicsServer2DSW : public PhysicsServer2D { PhysicsDirectBodyState2DSW *direct_state; - mutable RID_PtrOwner<Shape2DSW> shape_owner; - mutable RID_PtrOwner<Space2DSW> space_owner; - mutable RID_PtrOwner<Area2DSW> area_owner; - mutable RID_PtrOwner<Body2DSW> body_owner; - mutable RID_PtrOwner<Joint2DSW> joint_owner; + mutable RID_PtrOwner<Shape2DSW, true> shape_owner; + mutable RID_PtrOwner<Space2DSW, true> space_owner; + mutable RID_PtrOwner<Area2DSW, true> area_owner; + mutable RID_PtrOwner<Body2DSW, true> body_owner; + mutable RID_PtrOwner<Joint2DSW, true> joint_owner; static PhysicsServer2DSW *singletonsw; @@ -87,8 +87,8 @@ public: Vector2 *ptr; }; - virtual RID line_shape_create() override; - virtual RID ray_shape_create() override; + virtual RID world_margin_shape_create() override; + virtual RID separation_ray_shape_create() override; virtual RID segment_shape_create() override; virtual RID circle_shape_create() override; virtual RID rectangle_shape_create() override; @@ -191,7 +191,7 @@ public: virtual void body_clear_shapes(RID p_body) override; virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; - virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, float p_margin) override; + virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) override; virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override; virtual ObjectID body_get_object_instance_id(RID p_body) const override; @@ -242,28 +242,32 @@ public: virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; virtual int body_get_max_contacts_reported(RID p_body) const override; - virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) override; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override; virtual void body_set_pickable(RID p_body, bool p_pickable) override; - virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override; - virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) override; + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override; // this function only works on physics process, errors and returns null otherwise virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override; /* JOINT API */ + virtual RID joint_create() override; + + virtual void joint_clear(RID p_joint) override; + virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) override; virtual real_t joint_get_param(RID p_joint, JointParam p_param) const override; virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled) override; virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; - virtual RID pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID p_body_b = RID()) override; - virtual RID groove_joint_create(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override; - virtual RID damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override; + virtual void joint_make_pin(RID p_joint, const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) override; + virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override; + virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override; + virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; virtual void damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override; @@ -283,11 +287,13 @@ 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; - PhysicsServer2DSW(); + PhysicsServer2DSW(bool p_using_threads = false); ~PhysicsServer2DSW() {} }; diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.cpp b/servers/physics_2d/physics_server_2d_wrap_mt.cpp index 49c38c6ce0..930b19c2cb 100644 --- a/servers/physics_2d/physics_server_2d_wrap_mt.cpp +++ b/servers/physics_2d/physics_server_2d_wrap_mt.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,7 +33,7 @@ #include "core/os/os.h" void PhysicsServer2DWrapMT::thread_exit() { - exit = true; + exit.set(); } void PhysicsServer2DWrapMT::thread_step(real_t p_delta) { @@ -52,11 +52,11 @@ void PhysicsServer2DWrapMT::thread_loop() { physics_2d_server->init(); - exit = false; - step_thread_up = true; - while (!exit) { + exit.clear(); + step_thread_up.set(); + while (!exit.is_set()) { // flush commands one by one, until exit is requested - command_queue.wait_and_flush_one(); + command_queue.wait_and_flush(); } command_queue.flush_all(); // flush all @@ -76,7 +76,7 @@ void PhysicsServer2DWrapMT::step(real_t p_step) { } void PhysicsServer2DWrapMT::sync() { - if (thread) { + if (create_thread) { if (first_frame) { first_frame = false; } else { @@ -97,8 +97,8 @@ void PhysicsServer2DWrapMT::end_sync() { void PhysicsServer2DWrapMT::init() { if (create_thread) { //OS::get_singleton()->release_rendering_thread(); - thread = Thread::create(_thread_callback, this); - while (!step_thread_up) { + thread.start(_thread_callback, this); + while (!step_thread_up.is_set()) { OS::get_singleton()->delay_usec(1000); } } else { @@ -107,37 +107,19 @@ void PhysicsServer2DWrapMT::init() { } void PhysicsServer2DWrapMT::finish() { - if (thread) { + if (thread.is_started()) { command_queue.push(this, &PhysicsServer2DWrapMT::thread_exit); - Thread::wait_to_finish(thread); - memdelete(thread); - - thread = nullptr; + thread.wait_to_finish(); } else { physics_2d_server->finish(); } - - line_shape_free_cached_ids(); - ray_shape_free_cached_ids(); - segment_shape_free_cached_ids(); - circle_shape_free_cached_ids(); - rectangle_shape_free_cached_ids(); - capsule_shape_free_cached_ids(); - convex_polygon_shape_free_cached_ids(); - concave_polygon_shape_free_cached_ids(); - - space_free_cached_ids(); - area_free_cached_ids(); - body_free_cached_ids(); } PhysicsServer2DWrapMT::PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread) : command_queue(p_create_thread) { physics_2d_server = p_contained; create_thread = p_create_thread; - thread = nullptr; step_pending = 0; - step_thread_up = false; pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h index 586dbe9e12..05d4ac64b0 100644 --- a/servers/physics_2d/physics_server_2d_wrap_mt.h +++ b/servers/physics_2d/physics_server_2d_wrap_mt.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,9 +31,10 @@ #ifndef PHYSICS2DSERVERWRAPMT_H #define PHYSICS2DSERVERWRAPMT_H -#include "core/command_queue_mt.h" +#include "core/config/project_settings.h" #include "core/os/thread.h" -#include "core/project_settings.h" +#include "core/templates/command_queue_mt.h" +#include "core/templates/safe_refcount.h" #include "servers/physics_server_2d.h" #ifdef DEBUG_SYNC @@ -52,9 +53,9 @@ class PhysicsServer2DWrapMT : public PhysicsServer2D { Thread::ID server_thread; Thread::ID main_thread; - volatile bool exit; - Thread *thread; - volatile bool step_thread_up; + SafeFlag exit; + Thread thread; + SafeFlag step_thread_up; bool create_thread; Semaphore step_sem; @@ -73,11 +74,13 @@ public: #define ServerName PhysicsServer2D #define ServerNameWrapMT PhysicsServer2DWrapMT #define server_name physics_2d_server +#define WRITE_ACTION + #include "servers/server_wrap_mt_common.h" //FUNC1RID(shape,ShapeType); todo fix - FUNCRID(line_shape) - FUNCRID(ray_shape) + FUNCRID(world_margin_shape) + FUNCRID(separation_ray_shape) FUNCRID(segment_shape) FUNCRID(circle_shape) FUNCRID(rectangle_shape) @@ -93,7 +96,7 @@ public: FUNC1RC(real_t, shape_get_custom_solver_bias, RID); //these work well, but should be used from the main thread only - bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) { + bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); return physics_2d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count); } @@ -108,18 +111,18 @@ public: FUNC2RC(real_t, space_get_param, RID, SpaceParameter); // this function only works on physics process, errors and returns null otherwise - PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) { + PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); return physics_2d_server->space_get_direct_state(p_space); } FUNC2(space_set_debug_contacts, RID, int); - virtual Vector<Vector2> space_get_contacts(RID p_space) const { + virtual Vector<Vector2> space_get_contacts(RID p_space) const override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector<Vector2>()); return physics_2d_server->space_get_contacts(p_space); } - virtual int space_get_contact_count(RID p_space) const { + virtual int space_get_contact_count(RID p_space) const override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0); return physics_2d_server->space_get_contact_count(p_space); } @@ -189,7 +192,7 @@ public: FUNC2RC(RID, body_get_shape, RID, int); FUNC3(body_set_shape_disabled, RID, int, bool); - FUNC4(body_set_shape_as_one_way_collision, RID, int, bool, float); + FUNC4(body_set_shape_as_one_way_collision, RID, int, bool, real_t); FUNC2(body_remove_shape, RID, int); FUNC1(body_clear_shapes, RID); @@ -242,32 +245,31 @@ public: FUNC2(body_set_omit_force_integration, RID, bool); FUNC1RC(bool, body_is_omitting_force_integration, RID); - FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &); + FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); - bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { + bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override { return physics_2d_server->body_collide_shape(p_body, p_body_shape, p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); } FUNC2(body_set_pickable, RID, bool); - bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) { + bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); - } - - int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_2d_server->body_test_ray_separation(p_body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); + return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude); } // this function only works on physics process, errors and returns null otherwise - PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) { + PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); return physics_2d_server->body_get_direct_state(p_body); } /* JOINT API */ + FUNCRID(joint) + + FUNC1(joint_clear, RID) + FUNC3(joint_set_param, RID, JointParam, real_t); FUNC2RC(real_t, joint_get_param, RID, JointParam); @@ -280,9 +282,9 @@ public: //TODO need to convert this to FUNCRID, but it's a hassle.. - FUNC3R(RID, pin_joint_create, const Vector2 &, RID, RID); - FUNC5R(RID, groove_joint_create, const Vector2 &, const Vector2 &, const Vector2 &, RID, RID); - FUNC4R(RID, damped_spring_joint_create, const Vector2 &, const Vector2 &, RID, RID); + FUNC4(joint_make_pin, RID, const Vector2 &, RID, RID); + FUNC6(joint_make_groove, RID, const Vector2 &, const Vector2 &, const Vector2 &, RID, RID); + FUNC5(joint_make_damped_spring, RID, const Vector2 &, const Vector2 &, RID, RID); FUNC3(pin_joint_set_param, RID, PinJointParam, real_t); FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam); @@ -296,44 +298,30 @@ public: FUNC1(free, RID); FUNC1(set_active, bool); + FUNC1(set_collision_iterations, int); - virtual void init(); - virtual void step(real_t p_step); - virtual void sync(); - virtual void end_sync(); - virtual void flush_queries(); - virtual void finish(); + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void end_sync() override; + virtual void flush_queries() override; + virtual void finish() override; - virtual bool is_flushing_queries() const { + virtual bool is_flushing_queries() const override { return physics_2d_server->is_flushing_queries(); } - int get_process_info(ProcessInfo p_info) { + int get_process_info(ProcessInfo p_info) override { return physics_2d_server->get_process_info(p_info); } PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread); ~PhysicsServer2DWrapMT(); - template <class T> - static PhysicsServer2D *init_server() { -#ifdef NO_THREADS - return memnew(T); // Always single unsafe when no threads are available. -#else - int tm = GLOBAL_DEF("physics/2d/thread_model", 1); - if (tm == 0) { // single unsafe - return memnew(T); - } else if (tm == 1) { // single safe - return memnew(PhysicsServer2DWrapMT(memnew(T), false)); - } else { // multi threaded - return memnew(PhysicsServer2DWrapMT(memnew(T), true)); - } -#endif - } - #undef ServerNameWrapMT #undef ServerName #undef server_name +#undef WRITE_ACTION }; #ifdef DEBUG_SYNC diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp index 87e22ef1c9..064c4afe52 100644 --- a/servers/physics_2d/shape_2d_sw.cpp +++ b/servers/physics_2d/shape_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,7 +31,7 @@ #include "shape_2d_sw.h" #include "core/math/geometry_2d.h" -#include "core/sort_array.h" +#include "core/templates/sort_array.h" void Shape2DSW::configure(const Rect2 &p_aabb) { aabb = p_aabb; @@ -88,15 +88,15 @@ Shape2DSW::~Shape2DSW() { /*********************************************************/ /*********************************************************/ -void LineShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { +void WorldMarginShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { r_amount = 0; } -bool LineShape2DSW::contains_point(const Vector2 &p_point) const { +bool WorldMarginShape2DSW::contains_point(const Vector2 &p_point) const { return normal.dot(p_point) < d; } -bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { +bool WorldMarginShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { Vector2 segment = p_begin - p_end; real_t den = normal.dot(segment); @@ -118,11 +118,11 @@ bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_e return true; } -real_t LineShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { +real_t WorldMarginShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return 0; } -void LineShape2DSW::set_data(const Variant &p_data) { +void WorldMarginShape2DSW::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); Array arr = p_data; ERR_FAIL_COND(arr.size() != 2); @@ -131,7 +131,7 @@ void LineShape2DSW::set_data(const Variant &p_data) { configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2))); } -Variant LineShape2DSW::get_data() const { +Variant WorldMarginShape2DSW::get_data() const { Array arr; arr.resize(2); arr[0] = normal; @@ -143,7 +143,7 @@ Variant LineShape2DSW::get_data() const { /*********************************************************/ /*********************************************************/ -void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { +void SeparationRayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { r_amount = 1; if (p_normal.y > 0) { @@ -153,29 +153,29 @@ void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, in } } -bool RayShape2DSW::contains_point(const Vector2 &p_point) const { +bool SeparationRayShape2DSW::contains_point(const Vector2 &p_point) const { return false; } -bool RayShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { +bool SeparationRayShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { return false; //rays can't be intersected } -real_t RayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { +real_t SeparationRayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return 0; //rays are mass-less } -void RayShape2DSW::set_data(const Variant &p_data) { +void SeparationRayShape2DSW::set_data(const Variant &p_data) { Dictionary d = p_data; length = d["length"]; - slips_on_slope = d["slips_on_slope"]; + slide_on_slope = d["slide_on_slope"]; configure(Rect2(0, 0, 0.001, length)); } -Variant RayShape2DSW::get_data() const { +Variant SeparationRayShape2DSW::get_data() const { Dictionary d; d["length"] = length; - d["slips_on_slope"] = slips_on_slope; + d["slide_on_slope"] = slide_on_slope; return d; } @@ -228,7 +228,7 @@ void SegmentShape2DSW::set_data(const Variant &p_data) { Rect2 r = p_data; a = r.position; b = r.size; - n = (b - a).tangent(); + n = (b - a).orthogonal(); Rect2 aabb; aabb.position = a; @@ -339,10 +339,10 @@ void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_suppor } bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const { - float x = p_point.x; - float y = p_point.y; - float edge_x = half_extents.x; - float edge_y = half_extents.y; + real_t x = p_point.x; + real_t y = p_point.y; + real_t edge_x = half_extents.x; + real_t edge_y = half_extents.y; return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y); } @@ -383,15 +383,15 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports r_amount = 2; r_supports[0] = n; - r_supports[0].y += height * 0.5; + r_supports[0].y += height * 0.5 - radius; r_supports[1] = n; - r_supports[1].y -= height * 0.5; + r_supports[1].y -= height * 0.5 - radius; } else { - real_t h = (d > 0) ? height : -height; + real_t h = height * 0.5 - radius; n *= radius; - n.y += h * 0.5; + n.y += (d > 0) ? h : -h; r_amount = 1; *r_supports = n; } @@ -400,7 +400,7 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const { Vector2 p = p_point; p.y = Math::abs(p.y); - p.y -= height * 0.5; + p.y -= height * 0.5 - radius; if (p.y < 0) { p.y = 0; } @@ -417,7 +417,7 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 & for (int i = 0; i < 2; i++) { Vector2 begin = p_begin; Vector2 end = p_end; - real_t ofs = (i == 0) ? -height * 0.5 : height * 0.5; + real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius; begin.y += ofs; end.y += ofs; @@ -454,7 +454,7 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 & } Vector2 rpos, rnorm; - if (Rect2(Point2(-radius, -height * 0.5), Size2(radius * 2.0, height)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { + if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { real_t pd = n.dot(rpos); if (pd < d) { r_point = rpos; @@ -469,7 +469,7 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 & } real_t CapsuleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2 = Vector2(radius * 2, height + radius * 2) * p_scale; + Vector2 he2 = Vector2(radius * 2, height) * p_scale; return p_mass * he2.dot(he2) / 12.0; } @@ -487,7 +487,7 @@ void CapsuleShape2DSW::set_data(const Variant &p_data) { height = p.y; } - Point2 he(radius, height * 0.5 + radius); + Point2 he(radius, height * 0.5); configure(Rect2(-he, he * 2)); } @@ -502,6 +502,7 @@ Variant CapsuleShape2DSW::get_data() const { void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { int support_idx = -1; real_t d = -1e10; + r_amount = 0; for (int i = 0; i < point_count; i++) { //test point @@ -520,7 +521,7 @@ void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_su } } - ERR_FAIL_COND(support_idx == -1); + ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found."); r_amount = 1; r_supports[0] = points[support_idx].pos; @@ -569,17 +570,11 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec } } - if (inters) { - if (n.dot(r_normal) > 0) { - r_normal = -r_normal; - } - } - - //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); - return inters; //todo + return inters; } real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points."); Rect2 aabb; aabb.position = points[0].pos * p_scale; for (int i = 0; i < point_count; i++) { @@ -590,7 +585,11 @@ real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 } void ConvexPolygonShape2DSW::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif if (points) { memdelete_arr(points); @@ -612,7 +611,7 @@ void ConvexPolygonShape2DSW::set_data(const Variant &p_data) { for (int i = 0; i < point_count; i++) { Vector2 p = points[i].pos; Vector2 pn = points[(i + 1) % point_count].pos; - points[i].normal = (pn - p).tangent().normalized(); + points[i].normal = (pn - p).orthogonal().normalized(); } } else { Vector<real_t> dvr = p_data; @@ -687,6 +686,10 @@ bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const { } bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + if (segments.size() == 0 || points.size() == 0) { + return false; + } + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); enum { @@ -740,7 +743,7 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve if (nd < d) { d = nd; r_point = res; - r_normal = (b - a).tangent().normalized(); + r_normal = (b - a).orthogonal().normalized(); inters = true; } } @@ -829,7 +832,11 @@ int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { } void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif Rect2 aabb; @@ -914,7 +921,7 @@ Variant ConcavePolygonShape2DSW::get_data() const { return rsegments; } -void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const { +void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); enum { @@ -960,9 +967,11 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac Vector2 a = pointptr[s.points[0]]; Vector2 b = pointptr[s.points[1]]; - SegmentShape2DSW ss(a, b, (b - a).tangent().normalized()); + SegmentShape2DSW ss(a, b, (b - a).orthogonal().normalized()); - p_callback(p_userdata, &ss); + if (p_callback(p_userdata, &ss)) { + return; + } stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index eca284f7a4..1185d343ee 100644 --- a/servers/physics_2d/shape_2d_sw.h +++ b/servers/physics_2d/shape_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -34,18 +34,6 @@ #include "servers/physics_server_2d.h" #define _SEGMENT_IS_VALID_SUPPORT_THRESHOLD 0.99998 -/* - -SHAPE_LINE, ///< plane:"plane" -SHAPE_SEGMENT, ///< real_t:"length" -SHAPE_CIRCLE, ///< real_t:"radius" -SHAPE_RECTANGLE, ///< vec3:"extents" -SHAPE_CONVEX_POLYGON, ///< array of planes:"planes" -SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array) -SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error - -*/ - class Shape2DSW; class ShapeOwner2DSW { @@ -76,6 +64,8 @@ public: _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } _FORCE_INLINE_ bool is_configured() const { return configured; } + virtual bool allows_one_way_collision() const { return true; } + virtual bool is_concave() const { return false; } virtual bool contains_point(const Vector2 &p_point) const = 0; @@ -137,22 +127,22 @@ public: }; //let the optimizer do the magic -#define DEFAULT_PROJECT_RANGE_CAST \ - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ - project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ - } \ - _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ - real_t mina, maxa; \ - real_t minb, maxb; \ - Transform2D ofsb = p_transform; \ - ofsb.elements[2] += p_cast; \ - project_range(p_normal, p_transform, mina, maxa); \ - project_range(p_normal, ofsb, minb, maxb); \ - r_min = MIN(mina, minb); \ - r_max = MAX(maxa, maxb); \ +#define DEFAULT_PROJECT_RANGE_CAST \ + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { \ + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ + } \ + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ + real_t mina, maxa; \ + real_t minb, maxb; \ + Transform2D ofsb = p_transform; \ + ofsb.elements[2] += p_cast; \ + project_range(p_normal, p_transform, mina, maxa); \ + project_range(p_normal, ofsb, minb, maxb); \ + r_min = MIN(mina, minb); \ + r_max = MAX(maxa, maxb); \ } -class LineShape2DSW : public Shape2DSW { +class WorldMarginShape2DSW : public Shape2DSW { Vector2 normal; real_t d; @@ -160,17 +150,17 @@ public: _FORCE_INLINE_ Vector2 get_normal() const { return normal; } _FORCE_INLINE_ real_t get_d() const { return d; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_LINE; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_MARGIN; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large @@ -178,7 +168,7 @@ public: r_max = 1e10; } - virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); } @@ -189,25 +179,27 @@ public: } }; -class RayShape2DSW : public Shape2DSW { +class SeparationRayShape2DSW : public Shape2DSW { real_t length; - bool slips_on_slope; + bool slide_on_slope; public: _FORCE_INLINE_ real_t get_length() const { return length; } - _FORCE_INLINE_ bool get_slips_on_slope() const { return slips_on_slope; } + _FORCE_INLINE_ bool get_slide_on_slope() const { return slide_on_slope; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_RAY; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual bool allows_one_way_collision() const override { return false; } - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large @@ -220,8 +212,8 @@ public: DEFAULT_PROJECT_RANGE_CAST - _FORCE_INLINE_ RayShape2DSW() {} - _FORCE_INLINE_ RayShape2DSW(real_t p_length) { length = p_length; } + _FORCE_INLINE_ SeparationRayShape2DSW() {} + _FORCE_INLINE_ SeparationRayShape2DSW(real_t p_length) { length = p_length; } }; class SegmentShape2DSW : public Shape2DSW { @@ -234,20 +226,20 @@ public: _FORCE_INLINE_ const Vector2 &get_b() const { return b; } _FORCE_INLINE_ const Vector2 &get_normal() const { return n; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_SEGMENT; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; } _FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const { - return (p_xform.xform(b) - p_xform.xform(a)).normalized().tangent(); + return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large @@ -274,17 +266,17 @@ class CircleShape2DSW : public Shape2DSW { public: _FORCE_INLINE_ const real_t &get_radius() const { return radius; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_CIRCLE; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { //real large @@ -307,17 +299,17 @@ class RectangleShape2DSW : public Shape2DSW { public: _FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_RECTANGLE; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway @@ -381,25 +373,25 @@ public: _FORCE_INLINE_ const real_t &get_radius() const { return radius; } _FORCE_INLINE_ const real_t &get_height() const { return height; } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_CAPSULE; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { // no matter the angle, the box is mirrored anyway Vector2 n = p_transform.basis_xform_inv(p_normal).normalized(); - real_t h = (n.y > 0) ? height : -height; + real_t h = height * 0.5 - radius; n *= radius; - n.y += h * 0.5; + n.y += (n.y > 0) ? h : -h; r_max = p_normal.dot(p_transform.xform(n)); r_min = p_normal.dot(p_transform.xform(-n)); @@ -431,20 +423,20 @@ public: Vector2 a = points[p_idx].pos; p_idx++; Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos; - return (p_xform.xform(b) - p_xform.xform(a)).normalized().tangent(); + return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); } - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const; + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { if (!points || point_count <= 0) { @@ -472,10 +464,12 @@ public: class ConcaveShape2DSW : public Shape2DSW { public: - virtual bool is_concave() const { return true; } - typedef void (*Callback)(void *p_userdata, Shape2DSW *p_convex); + virtual bool is_concave() const override { return true; } + + // Returns true to stop the query. + typedef bool (*QueryCallback)(void *p_userdata, Shape2DSW *p_convex); - virtual void cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; }; class ConcavePolygonShape2DSW : public ConcaveShape2DSW { @@ -509,23 +503,31 @@ class ConcavePolygonShape2DSW : public ConcaveShape2DSW { int _generate_bvh(BVH *p_bvh, int p_len, int p_depth); public: - virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } - virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_rangev in ConcavePolygonShape2DSW"); } - virtual void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ + + void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_range in ConcavePolygonShape2DSW"); } - virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const; - virtual bool contains_point(const Vector2 &p_point) const; - virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const; + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; - virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return 0; } + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0; } - virtual void set_data(const Variant &p_data); - virtual Variant get_data() const; + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; - virtual void cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const; + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; DEFAULT_PROJECT_RANGE_CAST }; diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 966dcbd651..c0493749e0 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,7 +32,7 @@ #include "collision_solver_2d_sw.h" #include "core/os/os.h" -#include "core/pair.h" +#include "core/templates/pair.h" #include "physics_server_2d_sw.h" _FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { if (!(p_object->get_collision_layer() & p_collision_mask)) { @@ -278,27 +278,43 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor continue; } - //test initial overlap + //test initial overlap, ignore objects it's inside of. if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { - return false; + continue; } - //just do kinematic solving - real_t low = 0; - real_t hi = 1; Vector2 mnormal = p_motion.normalized(); + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; for (int j = 0; j < 8; j++) { //steps should be customizable.. - - real_t ofs = (low + hi) * 0.5; + real_t fraction = low + (hi - low) * fraction_coeff; Vector2 sep = mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); + bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); if (collided) { - hi = ofs; + hi = fraction; + if ((j == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } } else { - low = ofs; + low = fraction; + if ((j == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } } } @@ -346,12 +362,13 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D & } const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; if (p_exclude.has(col_obj->get_self())) { continue; } + int shape_idx = space->intersection_query_subindex_results[i]; + cbk.valid_dir = Vector2(); cbk.valid_depth = 0; @@ -383,15 +400,6 @@ struct _RestCallbackData2D { static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata; - if (rd->valid_dir != Vector2()) { - if (p_point_A.distance_squared_to(p_point_B) > rd->valid_depth * rd->valid_depth) { - return; - } - if (rd->valid_dir.dot((p_point_A - p_point_B).normalized()) < Math_PI * 0.25) { - return; - } - } - Vector2 contact_rel = p_point_B - p_point_A; real_t len = contact_rel.length(); @@ -403,9 +411,21 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, return; } + Vector2 normal = contact_rel / len; + + if (rd->valid_dir != Vector2()) { + if (len > rd->valid_depth) { + return; + } + + if (rd->valid_dir.dot(normal) > -CMP_EPSILON) { + return; + } + } + rd->best_len = len; rd->best_contact = p_point_B; - rd->best_normal = contact_rel / len; + rd->best_normal = normal; rd->best_object = rd->object; rd->best_shape = rd->shape; rd->best_local_shape = rd->local_shape; @@ -433,14 +453,14 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh } const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - int shape_idx = space->intersection_query_subindex_results[i]; if (p_exclude.has(col_obj->get_self())) { continue; } + int shape_idx = space->intersection_query_subindex_results[i]; + rcd.valid_dir = Vector2(); - rcd.valid_depth = 0; rcd.object = col_obj; rcd.shape = shape_idx; rcd.local_shape = 0; @@ -488,12 +508,10 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { keep = false; } else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) { keep = false; - } else if ((static_cast<Body2DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) { + } else if (!p_body->collides_with(static_cast<Body2DSW *>(intersection_query_results[i]))) { keep = false; } else if (static_cast<Body2DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { keep = false; - } else if (static_cast<Body2DSW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) { - keep = false; } if (!keep) { @@ -510,189 +528,7 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { return amount; } -int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, PhysicsServer2D::SeparationResult *r_results, int p_result_max, real_t p_margin) { - Rect2 body_aabb; - - bool shapes_found = false; - - for (int i = 0; i < p_body->get_shape_count(); i++) { - if (p_body->is_shape_set_as_disabled(i)) { - continue; - } - - if (p_body->get_shape(i)->get_type() != PhysicsServer2D::SHAPE_RAY) { - continue; - } - - if (!shapes_found) { - body_aabb = p_body->get_shape_aabb(i); - shapes_found = true; - } else { - body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); - } - } - - if (!shapes_found) { - return 0; - } - - // Undo the currently transform the physics server is aware of and apply the provided one - body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_margin); - - Transform2D body_transform = p_transform; - - for (int i = 0; i < p_result_max; i++) { - //reset results - r_results[i].collision_depth = 0; - } - - int rays_found = 0; - - { - // raycast AND separate - - const int max_results = 32; - int recover_attempts = 4; - Vector2 sr[max_results * 2]; - PhysicsServer2DSW::CollCbkData cbk; - cbk.max = max_results; - PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; - CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; - - do { - Vector2 recover_motion; - - bool collided = false; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - - for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) { - continue; - } - - Shape2DSW *body_shape = p_body->get_shape(j); - - if (body_shape->get_type() != PhysicsServer2D::SHAPE_RAY) { - continue; - } - - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); - - for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - cbk.amount = 0; - cbk.passed = 0; - cbk.ptr = sr; - cbk.invalid_by_dir = 0; - - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - - Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - - /* - * There is no point in supporting one way collisions with ray shapes, as they will always collide in the desired - * direction. Use a short ray shape if you want to achieve a similar effect. - * - if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { - - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - cbk.valid_depth = p_margin; //only valid depth is the collision margin - cbk.invalid_by_dir = 0; - - } else { -*/ - - cbk.valid_dir = Vector2(); - cbk.valid_depth = 0; - cbk.invalid_by_dir = 0; - - /* - } - */ - - Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_margin)) { - if (cbk.amount > 0) { - collided = true; - } - - int ray_index = -1; //reuse shape - for (int k = 0; k < rays_found; k++) { - if (r_results[ray_index].collision_local_shape == j) { - ray_index = k; - } - } - - if (ray_index == -1 && rays_found < p_result_max) { - ray_index = rays_found; - rays_found++; - } - - if (ray_index != -1) { - PhysicsServer2D::SeparationResult &result = r_results[ray_index]; - - for (int k = 0; k < cbk.amount; k++) { - Vector2 a = sr[k * 2 + 0]; - Vector2 b = sr[k * 2 + 1]; - - recover_motion += (b - a) * 0.4; - - float depth = a.distance_to(b); - if (depth > result.collision_depth) { - result.collision_depth = depth; - result.collision_point = b; - result.collision_normal = (b - a).normalized(); - result.collision_local_shape = j; - result.collider_shape = shape_idx; - result.collider = col_obj->get_self(); - result.collider_id = col_obj->get_instance_id(); - result.collider_metadata = col_obj->get_shape_metadata(shape_idx); - if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { - Body2DSW *body = (Body2DSW *)col_obj; - - Vector2 rel_vec = b - body->get_transform().get_origin(); - result.collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - } - } - } - } - } - } - } - - if (!collided || recover_motion == Vector2()) { - break; - } - - body_transform.elements[2] += recover_motion; - body_aabb.position += recover_motion; - - recover_attempts--; - } while (recover_attempts); - } - - //optimize results (remove non colliding) - for (int i = 0; i < rays_found; i++) { - if (r_results[i].collision_depth == 0) { - rays_found--; - SWAP(r_results[i], r_results[rays_found]); - } - } - - r_recover_motion = body_transform.elements[2] - p_transform.elements[2]; - return rays_found; -} - -bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer2D::MotionResult *r_result, bool p_exclude_raycast_shapes) { +bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, PhysicsServer2D::MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) { //give me back regular physics engine logic //this is madness //and most people using this function will think @@ -709,11 +545,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co bool shapes_found = false; for (int i = 0; i < p_body->get_shape_count(); i++) { - if (p_body->is_shape_set_as_disabled(i)) { - continue; - } - - if (p_exclude_raycast_shapes && p_body->get_shape(i)->get_type() == PhysicsServer2D::SHAPE_RAY) { + if (p_body->is_shape_disabled(i)) { continue; } @@ -728,7 +560,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co if (!shapes_found) { if (r_result) { *r_result = PhysicsServer2D::MotionResult(); - r_result->motion = p_motion; + r_result->travel = p_motion; } return false; } @@ -741,10 +573,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs]; int excluded_shape_pair_count = 0; - float separation_margin = MIN(p_margin, MAX(0.0, p_motion.length() - CMP_EPSILON)); //don't separate by more than the intended motion + real_t motion_length = p_motion.length(); + Vector2 motion_normal = p_motion / motion_length; Transform2D body_transform = p_from; + bool recovered = false; + { //STEP 1, FREE BODY IF STUCK @@ -769,44 +604,38 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co int amount = _cull_aabb_for_body(p_body, body_aabb); for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) { + if (p_body->is_shape_disabled(j)) { continue; } Shape2DSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { - continue; - } - Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); + for (int i = 0; i < amount; i++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } + if (p_exclude.has(col_obj->get_self())) { + continue; } + int shape_idx = intersection_query_subindex_results[i]; + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - float owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); cbk.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work cbk.invalid_by_dir = 0; if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction Vector2 lv = b->get_linear_velocity(); //compute displacement from linear velocity Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step; - float motion_len = motion.length(); + real_t motion_len = motion.length(); motion.normalize(); cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); } @@ -821,7 +650,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co bool did_collide = false; Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, separation_margin)) { + if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_margin)) { did_collide = cbk.passed > current_passed; //more passed, so collision actually existed } @@ -847,11 +676,20 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } Vector2 recover_motion; - for (int i = 0; i < cbk.amount; i++) { Vector2 a = sr[i * 2 + 0]; Vector2 b = sr[i * 2 + 1]; - recover_motion += (b - a) * 0.4; + + // Compute plane on b towards a. + Vector2 n = (a - b).normalized(); + real_t d = n.dot(b); + + // Compute depth on recovered motion. + real_t depth = n.dot(a + recover_motion) - d; + if (depth > 0.0) { + // Only recover if there is penetration. + recover_motion -= n * depth * 0.4; + } } if (recover_motion == Vector2()) { @@ -859,6 +697,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co break; } + recovered = true; + body_transform.elements[2] += recover_motion; body_aabb.position += recover_motion; @@ -881,13 +721,19 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co int amount = _cull_aabb_for_body(p_body, motion_aabb); for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) { - if (p_body->is_shape_set_as_disabled(body_shape_idx)) { + if (p_body->is_shape_disabled(body_shape_idx)) { continue; } Shape2DSW *body_shape = p_body->get_shape(body_shape_idx); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { - continue; + + // Colliding separation rays allows to properly snap to the ground, + // otherwise it's not needed in regular motion. + if (!p_collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) { + // When slide on slope is on, separation ray shape acts like a regular shape. + if (!static_cast<SeparationRayShape2DSW *>(body_shape)->get_slide_on_slope()) { + continue; + } } Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx); @@ -899,16 +745,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co for (int i = 0; i < amount; i++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; + if (p_exclude.has(col_obj->get_self())) { + continue; + } int col_shape_idx = intersection_query_subindex_results[i]; Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx); - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } - } - bool excluded = false; for (int k = 0; k < excluded_shape_pair_count; k++) { @@ -930,8 +772,11 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //test initial overlap if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { - if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { - continue; + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + Vector2 direction = col_obj_shape_xform.get_axis(1).normalized(); + if (motion_normal.dot(direction) < 0) { + continue; + } } stuck = true; @@ -939,25 +784,39 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } //just do kinematic solving - real_t low = 0; - real_t hi = 1; - Vector2 mnormal = p_motion.normalized(); - + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; for (int k = 0; k < 8; k++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; - real_t ofs = (low + hi) * 0.5; - - Vector2 sep = mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); + Vector2 sep = motion_normal; //important optimization for this to work fast enough + bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); if (collided) { - hi = ofs; + hi = fraction; + if ((k == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } } else { - low = ofs; + low = fraction; + if ((k == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } } } - if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { Vector2 cd[2]; PhysicsServer2DSW::CollCbkData cbk; cbk.max = 1; @@ -968,7 +827,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co cbk.valid_depth = 10e20; - Vector2 sep = mnormal; //important optimization for this to work fast enough + Vector2 sep = motion_normal; //important optimization for this to work fast enough bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), PhysicsServer2DSW::_shape_col_cbk, &cbk, &sep, 0); if (!collided || cbk.amount == 0) { continue; @@ -999,11 +858,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } bool collided = false; - if (safe >= 1) { - best_shape = -1; //no best shape with cast, reset to -1 - } - { + if (recovered || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 + } + //it collided, let's get the rest info in unsafe advance Transform2D ugt = body_transform; ugt.elements[2] += p_motion * unsafe; @@ -1012,39 +872,33 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co rcd.best_len = 0; rcd.best_object = nullptr; rcd.best_shape = 0; - rcd.min_allowed_depth = test_motion_min_contact_depth; - //optimization + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth); + int from_shape = best_shape != -1 ? best_shape : 0; int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); for (int j = from_shape; j < to_shape; j++) { - if (p_body->is_shape_set_as_disabled(j)) { + if (p_body->is_shape_disabled(j)) { continue; } Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j); Shape2DSW *body_shape = p_body->get_shape(j); - if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { - continue; - } - body_aabb.position += p_motion * unsafe; int amount = _cull_aabb_for_body(p_body, body_aabb); for (int i = 0; i < amount; i++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; - - if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { - const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); - if (p_infinite_inertia && PhysicsServer2D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer2D::BODY_MODE_KINEMATIC != b->get_mode()) { - continue; - } + if (p_exclude.has(col_obj->get_self())) { + continue; } + int shape_idx = intersection_query_subindex_results[i]; + Shape2DSW *against_shape = col_obj->get_shape(shape_idx); bool excluded = false; @@ -1060,9 +914,24 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); - if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - rcd.valid_depth = 10e20; + + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + rcd.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work + + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { + const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { + //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction + Vector2 lv = b->get_linear_velocity(); + //compute displacement from linear velocity + Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step; + real_t motion_len = motion.length(); + motion.normalize(); + rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0); + } + } } else { rcd.valid_dir = Vector2(); rcd.valid_depth = 0; @@ -1086,15 +955,18 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co r_result->collision_local_shape = rcd.best_local_shape; r_result->collision_normal = rcd.best_normal; r_result->collision_point = rcd.best_contact; + r_result->collision_depth = rcd.best_len; + r_result->collision_safe_fraction = safe; + r_result->collision_unsafe_fraction = unsafe; r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object); Vector2 rel_vec = r_result->collision_point - body->get_transform().get_origin(); r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); - r_result->motion = safe * p_motion; + r_result->travel = safe * p_motion; r_result->remainder = p_motion - safe * p_motion; - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + r_result->travel += (body_transform.get_origin() - p_from.get_origin()); } collided = true; @@ -1102,16 +974,16 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } if (!collided && r_result) { - r_result->motion = p_motion; + r_result->travel = p_motion; r_result->remainder = Vector2(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + r_result->travel += (body_transform.get_origin() - p_from.get_origin()); } return collided; } void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) { - if (!A->test_collision_mask(B)) { + if (!A->interacts_with(B)) { return nullptr; } @@ -1332,7 +1204,7 @@ Space2DSW::Space2DSW() { constraint_bias = 0.2; body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0); - body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI)); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0)); 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")); diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index 1eee83dfe9..392df9b025 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -37,8 +37,8 @@ #include "body_pair_2d_sw.h" #include "broad_phase_2d_sw.h" #include "collision_object_2d_sw.h" -#include "core/hash_map.h" -#include "core/project_settings.h" +#include "core/config/project_settings.h" +#include "core/templates/hash_map.h" #include "core/typedefs.h" class PhysicsDirectSpaceState2DSW : public PhysicsDirectSpaceState2D { @@ -105,7 +105,6 @@ private: real_t test_motion_min_contact_depth; enum { - INTERSECTION_QUERY_MAX = 2048 }; @@ -184,11 +183,10 @@ public: int get_collision_pairs() const { return collision_pairs; } - bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer2D::MotionResult *r_result, bool p_exclude_raycast_shapes = true); - int test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, PhysicsServer2D::SeparationResult *r_results, int p_result_max, real_t p_margin); + bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, PhysicsServer2D::MotionResult *r_result, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()); void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } - _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } + _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } _FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) { if (contact_debug_count < contact_debug.size()) { contact_debug.write[contact_debug_count++] = p_contact; diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index c7711bcd1d..8b30160cc1 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -29,102 +29,98 @@ /*************************************************************************/ #include "step_2d_sw.h" + #include "core/os/os.h" -void Step2DSW::_populate_island(Body2DSW *p_body, Body2DSW **p_island, Constraint2DSW **p_constraint_island) { +#define BODY_ISLAND_COUNT_RESERVE 128 +#define BODY_ISLAND_SIZE_RESERVE 512 +#define ISLAND_COUNT_RESERVE 128 +#define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 + +void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island) { p_body->set_island_step(_step); - p_body->set_island_next(*p_island); - *p_island = p_body; - for (Map<Constraint2DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - Constraint2DSW *c = (Constraint2DSW *)E->key(); - if (c->get_island_step() == _step) { - continue; //already processed + if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + // Only dynamic bodies are tested for activation. + p_body_island.push_back(p_body); + } + + for (const Pair<Constraint2DSW *, int> &E : p_body->get_constraint_list()) { + Constraint2DSW *constraint = (Constraint2DSW *)E.first; + if (constraint->get_island_step() == _step) { + continue; // Already processed. } - c->set_island_step(_step); - c->set_island_next(*p_constraint_island); - *p_constraint_island = c; + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + all_constraints.push_back(constraint); - for (int i = 0; i < c->get_body_count(); i++) { - if (i == E->get()) { + for (int i = 0; i < constraint->get_body_count(); i++) { + if (i == E.second) { continue; } - Body2DSW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { - continue; //no go + Body2DSW *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. } - _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); + if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(other_body, p_body_island, p_constraint_island); } } } -bool Step2DSW::_setup_island(Constraint2DSW *p_island, real_t p_delta) { - Constraint2DSW *ci = p_island; - Constraint2DSW *prev_ci = nullptr; - bool removed_root = false; - while (ci) { - bool process = ci->setup(p_delta); - - if (!process) { - //remove from island if process fails - if (prev_ci) { - prev_ci->set_island_next(ci->get_island_next()); - } else { - removed_root = true; - prev_ci = ci; - } - } else { - prev_ci = ci; +void Step2DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { + Constraint2DSW *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void Step2DSW::_pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const { + uint32_t constraint_count = p_constraint_island.size(); + uint32_t valid_constraint_count = 0; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + Constraint2DSW *constraint = p_constraint_island[constraint_index]; + if (p_constraint_island[constraint_index]->pre_solve(delta)) { + // Keep this constraint for solving. + p_constraint_island[valid_constraint_count++] = constraint; } - ci = ci->get_island_next(); } - - return removed_root; + p_constraint_island.resize(valid_constraint_count); } -void Step2DSW::_solve_island(Constraint2DSW *p_island, int p_iterations, real_t p_delta) { - for (int i = 0; i < p_iterations; i++) { - Constraint2DSW *ci = p_island; - while (ci) { - ci->solve(p_delta); - ci = ci->get_island_next(); +void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const { + const LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[p_island_index]; + + for (int i = 0; i < iterations; i++) { + uint32_t constraint_count = constraint_island.size(); + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + constraint_island[constraint_index]->solve(delta); } } } -void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { +void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const { bool can_sleep = true; - Body2DSW *b = p_island; - while (b) { - if (b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } + uint32_t body_count = p_body_island.size(); + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + Body2DSW *body = p_body_island[body_index]; - if (!b->sleep_test(p_delta)) { + if (!body->sleep_test(delta)) { can_sleep = false; } - - b = b->get_island_next(); } - //put all to sleep or wake up everyoen - - b = p_island; - while (b) { - if (b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { - b = b->get_island_next(); - continue; //ignore for static - } + // Put all to sleep or wake up everyone. + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + Body2DSW *body = p_body_island[body_index]; - bool active = b->is_active(); + bool active = body->is_active(); if (active == can_sleep) { - b->set_active(!can_sleep); + body->set_active(!can_sleep); } - - b = b->get_island_next(); } } @@ -133,6 +129,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + iterations = p_iterations; + delta = p_delta; + const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list(); /* INTEGRATE FORCES */ @@ -157,94 +156,85 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - /* GENERATE CONSTRAINT ISLANDS */ + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + Constraint2DSW *constraint = E->get(); + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ - Body2DSW *island_list = nullptr; - Constraint2DSW *constraint_island_list = nullptr; b = body_list->first(); - int island_count = 0; + uint32_t body_island_count = 0; while (b) { Body2DSW *body = b->self(); if (body->get_island_step() != _step) { - Body2DSW *island = nullptr; - Constraint2DSW *constraint_island = nullptr; - _populate_island(body, &island, &constraint_island); - - island->set_island_list_next(island_list); - island_list = island; + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector<Body2DSW *> &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); - if (constraint_island) { - constraint_island->set_island_list_next(constraint_island_list); - constraint_island_list = constraint_island; - island_count++; + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); } - } - b = b->next(); - } + LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); - p_space->set_island_count(island_count); + _populate_island(body, body_island, constraint_island); - const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list(); + if (body_island.is_empty()) { + --body_island_count; + } - while (aml.first()) { - for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint2DSW *c = E->get(); - if (c->get_island_step() == _step) { - continue; + if (constraint_island.is_empty()) { + --island_count; } - c->set_island_step(_step); - c->set_island_next(nullptr); - c->set_island_list_next(constraint_island_list); - constraint_island_list = c; } - p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here + b = b->next(); } + p_space->set_island_count((int)island_count); + { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); profile_begtime = profile_endtime; } - /* SETUP CONSTRAINT ISLANDS */ - - { - Constraint2DSW *ci = constraint_island_list; - Constraint2DSW *prev_ci = nullptr; - while (ci) { - if (_setup_island(ci, p_delta)) { - //removed the root from the island graph because it is not to be processed - - Constraint2DSW *next = ci->get_island_next(); - - if (next) { - //root from list being deleted no longer exists, replace by next - next->set_island_list_next(ci->get_island_list_next()); - if (prev_ci) { - prev_ci->set_island_list_next(next); - } else { - constraint_island_list = next; - } - prev_ci = next; - } else { - //list is empty, just skip - if (prev_ci) { - prev_ci->set_island_list_next(ci->get_island_list_next()); - - } else { - constraint_island_list = ci->get_island_list_next(); - } - } - } else { - prev_ci = ci; - } + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ - ci = ci->get_island_list_next(); - } - } + uint32_t total_contraint_count = all_constraints.size(); + work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -252,15 +242,21 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } + /* PRE-SOLVE CONSTRAINT ISLANDS */ + + // Warning: This doesn't run on threads, because it involves thread-unsafe processing. + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _pre_solve_island(constraint_islands[island_index]); + } + /* SOLVE CONSTRAINT ISLANDS */ - { - Constraint2DSW *ci = constraint_island_list; - while (ci) { - //iterating each island separatedly improves cache efficiency - _solve_island(ci, p_iterations, p_delta); - ci = ci->get_island_list_next(); - } + // Warning: _solve_island modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + if (island_count > 1) { + work_pool.do_work(island_count, this, &Step2DSW::_solve_island, nullptr); + } else if (island_count > 0) { + _solve_island(0); } { //profile @@ -280,12 +276,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { /* SLEEP / WAKE UP ISLANDS */ - { - Body2DSW *bi = island_list; - while (bi) { - _check_suspend(bi, p_delta); - bi = bi->get_island_list_next(); - } + for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { + _check_suspend(body_islands[island_index]); } { //profile @@ -294,6 +286,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { //profile_begtime=profile_endtime; } + all_constraints.clear(); + p_space->update(); p_space->unlock(); _step++; @@ -301,4 +295,14 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { Step2DSW::Step2DSW() { _step = 1; + + body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); + constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); + + work_pool.init(); +} + +Step2DSW::~Step2DSW() { + work_pool.finish(); } diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h index c1b2d01fb4..c51fd73a79 100644 --- a/servers/physics_2d/step_2d_sw.h +++ b/servers/physics_2d/step_2d_sw.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,17 +33,31 @@ #include "space_2d_sw.h" +#include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" + class Step2DSW { uint64_t _step; - void _populate_island(Body2DSW *p_body, Body2DSW **p_island, Constraint2DSW **p_constraint_island); - bool _setup_island(Constraint2DSW *p_island, real_t p_delta); - void _solve_island(Constraint2DSW *p_island, int p_iterations, real_t p_delta); - void _check_suspend(Body2DSW *p_island, real_t p_delta); + int iterations = 0; + real_t delta = 0.0; + + ThreadWorkPool work_pool; + + LocalVector<LocalVector<Body2DSW *>> body_islands; + LocalVector<LocalVector<Constraint2DSW *>> constraint_islands; + LocalVector<Constraint2DSW *> all_constraints; + + void _populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; + void _check_suspend(LocalVector<Body2DSW *> &p_body_island) const; public: void step(Space2DSW *p_space, real_t p_delta, int p_iterations); Step2DSW(); + ~Step2DSW(); }; #endif // STEP_2D_SW_H |