diff options
Diffstat (limited to 'servers/physics_2d')
21 files changed, 354 insertions, 284 deletions
diff --git a/servers/physics_2d/godot_area_2d.cpp b/servers/physics_2d/godot_area_2d.cpp index 9937178550..3fe062de57 100644 --- a/servers/physics_2d/godot_area_2d.cpp +++ b/servers/physics_2d/godot_area_2d.cpp @@ -225,27 +225,33 @@ void GodotArea2D::call_queries() { resptr[i] = &res[i]; } - for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_bodies.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); E = next; continue; } - res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; + res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); E = next; Callable::CallError ce; Variant ret; - monitor_callback.call((const Variant **)resptr, 5, ret, ce); + monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(monitor_callback, (const Variant **)resptr, 5, ce)); + } } } else { monitored_bodies.clear(); @@ -261,27 +267,33 @@ void GodotArea2D::call_queries() { resptr[i] = &res[i]; } - for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_areas.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); E = next; continue; } - res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; + res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); E = next; Callable::CallError ce; Variant ret; - area_monitor_callback.call((const Variant **)resptr, 5, ret, ce); + area_monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(area_monitor_callback, (const Variant **)resptr, 5, ce)); + } } } else { monitored_areas.clear(); @@ -292,12 +304,12 @@ void GodotArea2D::call_queries() { void GodotArea2D::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const { if (is_gravity_point()) { - const real_t gravity_distance_scale = get_gravity_distance_scale(); + const real_t gr_distance_scale = get_gravity_distance_scale(); Vector2 v = get_transform().xform(get_gravity_vector()) - p_position; - if (gravity_distance_scale > 0) { + if (gr_distance_scale > 0) { const real_t v_length = v.length(); if (v_length > 0) { - const real_t v_scaled = v_length * gravity_distance_scale; + const real_t v_scaled = v_length * gr_distance_scale; r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); } else { r_gravity = Vector2(); diff --git a/servers/physics_2d/godot_area_2d.h b/servers/physics_2d/godot_area_2d.h index 6e8078909b..221982cf78 100644 --- a/servers/physics_2d/godot_area_2d.h +++ b/servers/physics_2d/godot_area_2d.h @@ -68,16 +68,15 @@ class GodotArea2D : public GodotCollisionObject2D { uint32_t body_shape = 0; uint32_t area_shape = 0; - _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { - if (rid == p_key.rid) { - if (body_shape == p_key.body_shape) { - return area_shape < p_key.area_shape; - } else { - return body_shape < p_key.body_shape; - } - } else { - return rid < p_key.rid; - } + static uint32_t hash(const BodyKey &p_key) { + uint32_t h = hash_one_uint64(p_key.rid.get_id()); + h = hash_murmur3_one_64(p_key.instance_id, h); + h = hash_murmur3_one_32(p_key.area_shape, h); + return hash_fmix32(hash_murmur3_one_32(p_key.body_shape, h)); + } + + _FORCE_INLINE_ bool operator==(const BodyKey &p_key) const { + return rid == p_key.rid && instance_id == p_key.instance_id && body_shape == p_key.body_shape && area_shape == p_key.area_shape; } _FORCE_INLINE_ BodyKey() {} @@ -91,12 +90,12 @@ class GodotArea2D : public GodotCollisionObject2D { _FORCE_INLINE_ void dec() { state--; } }; - Map<BodyKey, BodyState> monitored_bodies; - Map<BodyKey, BodyState> monitored_areas; + HashMap<BodyKey, BodyState, BodyKey> monitored_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_areas; - Set<GodotConstraint2D *> constraints; + HashSet<GodotConstraint2D *> constraints; - virtual void _shapes_changed(); + virtual void _shapes_changed() override; void _queue_monitor_update(); void _set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode); @@ -143,7 +142,7 @@ public: _FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set<GodotConstraint2D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ const HashSet<GodotConstraint2D *> &get_constraints() const { return constraints; } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } void set_monitorable(bool p_monitorable); @@ -151,7 +150,7 @@ public: void set_transform(const Transform2D &p_transform); - void set_space(GodotSpace2D *p_space); + void set_space(GodotSpace2D *p_space) override; void call_queries(); diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp index 6873504f70..4422be959a 100644 --- a/servers/physics_2d/godot_body_2d.cpp +++ b/servers/physics_2d/godot_body_2d.cpp @@ -44,7 +44,7 @@ void GodotBody2D::update_mass_properties() { //update shapes and motions switch (mode) { - case PhysicsServer2D::BODY_MODE_DYNAMIC: { + case PhysicsServer2D::BODY_MODE_RIGID: { real_t total_area = 0; for (int i = 0; i < get_shape_count(); i++) { if (is_shape_disabled(i)) { @@ -65,10 +65,10 @@ void GodotBody2D::update_mass_properties() { real_t area = get_shape_aabb(i).get_area(); - real_t mass = area * this->mass / total_area; + real_t mass_new = area * mass / total_area; // NOTE: we assume that the shape origin is also its center of mass. - center_of_mass_local += mass * get_shape_transform(i).get_origin(); + center_of_mass_local += mass_new * get_shape_transform(i).get_origin(); } center_of_mass_local /= mass; @@ -90,12 +90,12 @@ void GodotBody2D::update_mass_properties() { continue; } - real_t mass = area * this->mass / total_area; + real_t mass_new = area * mass / total_area; Transform2D mtx = get_shape_transform(i); Vector2 scale = mtx.get_scale(); Vector2 shape_origin = mtx.get_origin() - center_of_mass_local; - inertia += shape->get_moment_of_inertia(mass, scale) + mass * shape_origin.length_squared(); + inertia += shape->get_moment_of_inertia(mass_new, scale) + mass_new * shape_origin.length_squared(); } } @@ -113,7 +113,7 @@ void GodotBody2D::update_mass_properties() { _inv_inertia = 0; _inv_mass = 0; } break; - case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { + case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: { _inv_inertia = 0; _inv_mass = 1.0 / mass; @@ -160,7 +160,7 @@ void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Varian real_t mass_value = p_value; ERR_FAIL_COND(mass_value <= 0); mass = mass_value; - if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC) { + if (mode >= PhysicsServer2D::BODY_MODE_RIGID) { _mass_properties_changed(); } } break; @@ -168,13 +168,13 @@ void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Varian real_t inertia_value = p_value; if (inertia_value <= 0.0) { calculate_inertia = true; - if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { + if (mode == PhysicsServer2D::BODY_MODE_RIGID) { _mass_properties_changed(); } } else { calculate_inertia = false; inertia = inertia_value; - if (mode == PhysicsServer2D::BODY_MODE_DYNAMIC) { + if (mode == PhysicsServer2D::BODY_MODE_RIGID) { _inv_inertia = 1.0 / inertia; } } @@ -267,7 +267,7 @@ void GodotBody2D::set_mode(PhysicsServer2D::BodyMode p_mode) { first_time_kinematic = true; } } break; - case PhysicsServer2D::BODY_MODE_DYNAMIC: { + case PhysicsServer2D::BODY_MODE_RIGID: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; if (!calculate_inertia) { _inv_inertia = 1.0 / inertia; @@ -277,7 +277,7 @@ void GodotBody2D::set_mode(PhysicsServer2D::BodyMode p_mode) { set_active(true); } break; - case PhysicsServer2D::BODY_MODE_DYNAMIC_LINEAR: { + case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = 0; angular_velocity = 0; @@ -358,7 +358,7 @@ void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p } break; case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode >= PhysicsServer2D::BODY_MODE_DYNAMIC && !active && !can_sleep) { + if (mode >= PhysicsServer2D::BODY_MODE_RIGID && !active && !can_sleep) { set_active(true); } @@ -578,14 +578,14 @@ void GodotBody2D::integrate_forces(real_t p_step) { damp = 0; } - real_t angular_damp = 1.0 - p_step * total_angular_damp; + real_t angular_damp_new = 1.0 - p_step * total_angular_damp; - if (angular_damp < 0) { // reached zero in the given time - angular_damp = 0; + if (angular_damp_new < 0) { // reached zero in the given time + angular_damp_new = 0; } linear_velocity *= damp; - angular_velocity *= angular_damp; + angular_velocity *= angular_damp_new; linear_velocity += _inv_mass * force * p_step; angular_velocity += _inv_inertia * torque * p_step; @@ -615,7 +615,7 @@ void GodotBody2D::integrate_velocities(real_t p_step) { return; } - if (fi_callback_data || body_state_callback) { + if (fi_callback_data || body_state_callback.get_object()) { get_space()->body_add_to_state_query_list(&direct_state_query_list); } @@ -661,7 +661,7 @@ void GodotBody2D::wakeup_neighbours() { continue; } GodotBody2D *b = n[i]; - if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) { + if (b->mode < PhysicsServer2D::BODY_MODE_RIGID) { continue; } @@ -673,26 +673,30 @@ void GodotBody2D::wakeup_neighbours() { } void GodotBody2D::call_queries() { + Variant direct_state_variant = get_direct_state(); + if (fi_callback_data) { if (!fi_callback_data->callable.get_object()) { set_force_integration_callback(Callable()); } else { - Variant direct_state_variant = get_direct_state(); const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; Callable::CallError ce; Variant rv; if (fi_callback_data->udata.get_type() != Variant::NIL) { - fi_callback_data->callable.call(vp, 2, rv, ce); + fi_callback_data->callable.callp(vp, 2, rv, ce); } else { - fi_callback_data->callable.call(vp, 1, rv, ce); + fi_callback_data->callable.callp(vp, 1, rv, ce); } } } - if (body_state_callback) { - (body_state_callback)(body_state_callback_instance, get_direct_state()); + if (body_state_callback.get_object()) { + const Variant *vp[1] = { &direct_state_variant }; + Callable::CallError ce; + Variant rv; + body_state_callback.callp(vp, 1, rv, ce); } } @@ -713,9 +717,8 @@ bool GodotBody2D::sleep_test(real_t p_step) { } } -void GodotBody2D::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) { - body_state_callback_instance = p_instance; - body_state_callback = p_callback; +void GodotBody2D::set_state_sync_callback(const Callable &p_callable) { + body_state_callback = p_callable; } void GodotBody2D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h index 1335a19126..86d42ae7f8 100644 --- a/servers/physics_2d/godot_body_2d.h +++ b/servers/physics_2d/godot_body_2d.h @@ -42,7 +42,7 @@ class GodotConstraint2D; class GodotPhysicsDirectBodyState2D; class GodotBody2D : public GodotCollisionObject2D { - PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC; + PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_RIGID; Vector2 biased_linear_velocity; real_t biased_angular_velocity = 0.0; @@ -103,7 +103,7 @@ class GodotBody2D : public GodotCollisionObject2D { bool can_sleep = true; bool first_time_kinematic = false; void _mass_properties_changed(); - virtual void _shapes_changed(); + virtual void _shapes_changed() override; Transform2D new_transform; List<Pair<GodotConstraint2D *, int>> constraint_list; @@ -137,8 +137,7 @@ class GodotBody2D : public GodotCollisionObject2D { Vector<Contact> contacts; //no contacts by default int contact_count = 0; - void *body_state_callback_instance = nullptr; - PhysicsServer2D::BodyStateCallback body_state_callback = nullptr; + Callable body_state_callback; struct ForceIntegrationCallbackData { Callable callable; @@ -156,7 +155,7 @@ class GodotBody2D : public GodotCollisionObject2D { friend class GodotPhysicsDirectBodyState2D; // i give up, too many functions to expose public: - void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback); + void set_state_sync_callback(const Callable &p_callable); void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); GodotPhysicsDirectBodyState2D *get_direct_state(); @@ -302,7 +301,7 @@ public: _FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; } _FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; } - void set_space(GodotSpace2D *p_space); + void set_space(GodotSpace2D *p_space) override; void update_mass_properties(); void reset_mass_properties(); diff --git a/servers/physics_2d/godot_body_direct_state_2d.cpp b/servers/physics_2d/godot_body_direct_state_2d.cpp index cde6e8c991..d413e03be6 100644 --- a/servers/physics_2d/godot_body_direct_state_2d.cpp +++ b/servers/physics_2d/godot_body_direct_state_2d.cpp @@ -138,7 +138,7 @@ void GodotPhysicsDirectBodyState2D::add_constant_torque(real_t p_torque) { } void GodotPhysicsDirectBodyState2D::set_constant_force(const Vector2 &p_force) { - if (!p_force.is_equal_approx(Vector2())) { + if (!p_force.is_zero_approx()) { body->wakeup(); } body->set_constant_force(p_force); diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp index 2bf1e5a1d4..7b7c67bbc2 100644 --- a/servers/physics_2d/godot_body_pair_2d.cpp +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -39,7 +39,7 @@ #define MAX_BIAS_ROTATION (Math_PI / 8) void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) { - GodotBodyPair2D *self = (GodotBodyPair2D *)p_self; + GodotBodyPair2D *self = static_cast<GodotBodyPair2D *>(p_self); self->_contact_added_callback(p_point_A, p_point_B); } @@ -170,7 +170,7 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, Vector2 mnormal = motion / mlen; - real_t min, max; + real_t min = 0.0, max = 0.0; p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max); // Did it move enough in this direction to even attempt raycast? @@ -203,7 +203,7 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, // Check one-way collision based on motion direction. if (p_A->get_shape(p_shape_A)->allows_one_way_collision() && p_B->is_shape_set_as_one_way_collision(p_shape_B)) { - Vector2 direction = p_xform_B.get_axis(1).normalized(); + Vector2 direction = p_xform_B.columns[1].normalized(); if (direction.dot(mnormal) < CMP_EPSILON) { collided = false; oneway_disabled = true; @@ -260,7 +260,7 @@ bool GodotBodyPair2D::setup(real_t p_step) { Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); Transform2D xform_Bu = B->get_transform(); - xform_Bu.elements[2] -= offset_A; + xform_Bu.columns[2] -= offset_A; Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); GodotShape2D *shape_A_ptr = A->get_shape(shape_A); @@ -300,7 +300,7 @@ bool GodotBodyPair2D::setup(real_t p_step) { if (!prev_collided) { 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(); + Vector2 direction = xform_A.columns[1].normalized(); bool valid = false; for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; @@ -318,7 +318,7 @@ bool GodotBodyPair2D::setup(real_t p_step) { } 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(); + Vector2 direction = xform_B.columns[1].normalized(); bool valid = false; for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; @@ -351,7 +351,7 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) { Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); Transform2D xform_Bu = B->get_transform(); - xform_Bu.elements[2] -= offset_A; + xform_Bu.columns[2] -= offset_A; Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp index 2cc60fc765..c545fec113 100644 --- a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp +++ b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp @@ -82,7 +82,7 @@ int GodotBroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D } void *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B) { - GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self); + GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self); if (!bpo->pair_callback) { return nullptr; } @@ -91,7 +91,7 @@ void *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollis } void GodotBroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B, void *pairdata) { - GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self); + GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self); if (!bpo->unpair_callback) { return; } diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.h b/servers/physics_2d/godot_broad_phase_2d_bvh.h index b11ad0e75e..512111f948 100644 --- a/servers/physics_2d/godot_broad_phase_2d_bvh.h +++ b/servers/physics_2d/godot_broad_phase_2d_bvh.h @@ -77,22 +77,22 @@ class GodotBroadPhase2DBVH : public GodotBroadPhase2D { public: // 0 is an invalid ID - virtual ID create(GodotCollisionObject2D *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); + virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) override; + virtual void move(ID p_id, const Rect2 &p_aabb) override; + virtual void set_static(ID p_id, bool p_static) override; + virtual void remove(ID p_id) override; - virtual GodotCollisionObject2D *get_object(ID p_id) const; - virtual bool is_static(ID p_id) const; - virtual int get_subindex(ID p_id) const; + virtual GodotCollisionObject2D *get_object(ID p_id) const override; + virtual bool is_static(ID p_id) const override; + virtual int get_subindex(ID p_id) const override; - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override; + virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override; - 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 set_pair_callback(PairCallback p_pair_callback, void *p_userdata) override; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) override; - virtual void update(); + virtual void update() override; static GodotBroadPhase2D *_create(); GodotBroadPhase2DBVH(); diff --git a/servers/physics_2d/godot_collision_object_2d.h b/servers/physics_2d/godot_collision_object_2d.h index 19d6e91561..7965e8a94d 100644 --- a/servers/physics_2d/godot_collision_object_2d.h +++ b/servers/physics_2d/godot_collision_object_2d.h @@ -70,6 +70,7 @@ private: Transform2D inv_transform; uint32_t collision_mask = 1; uint32_t collision_layer = 1; + real_t collision_priority = 1.0; bool _static = true; SelfList<GodotCollisionObject2D> pending_shape_update_list; @@ -104,7 +105,7 @@ public: _FORCE_INLINE_ void set_canvas_instance_id(const ObjectID &p_canvas_instance_id) { canvas_instance_id = p_canvas_instance_id; } _FORCE_INLINE_ ObjectID get_canvas_instance_id() const { return canvas_instance_id; } - void _shape_changed(); + void _shape_changed() override; _FORCE_INLINE_ Type get_type() const { return type; } void add_shape(GodotShape2D *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false); @@ -166,7 +167,14 @@ public: } _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } - void remove_shape(GodotShape2D *p_shape); + _FORCE_INLINE_ void set_collision_priority(real_t p_priority) { + ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0."); + collision_priority = p_priority; + _shape_changed(); + } + _FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; } + + void remove_shape(GodotShape2D *p_shape) override; void remove_shape(int p_index); virtual void set_space(GodotSpace2D *p_space) = 0; diff --git a/servers/physics_2d/godot_collision_solver_2d.cpp b/servers/physics_2d/godot_collision_solver_2d.cpp index 263d6d939f..9c7b8fbe53 100644 --- a/servers/physics_2d/godot_collision_solver_2d.cpp +++ b/servers/physics_2d/godot_collision_solver_2d.cpp @@ -34,7 +34,7 @@ #define collision_solver sat_2d_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { const GodotWorldBoundaryShape2D *world_boundary = static_cast<const GodotWorldBoundaryShape2D *>(p_shape_A); if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { return false; @@ -52,7 +52,9 @@ bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_s bool found = false; for (int i = 0; i < support_count; i++) { + supports[i] += p_margin * supports[i].normalized(); supports[i] = p_transform_B.xform(supports[i]); + supports[i] += p_motion_B; real_t pd = n.dot(supports[i]); if (pd >= d) { continue; @@ -140,7 +142,7 @@ struct _ConcaveCollisionInfo2D { Vector2 motion_B; real_t margin_A = 0.0; real_t margin_B = 0.0; - GodotCollisionSolver2D::CallbackResult result_callback; + GodotCollisionSolver2D::CallbackResult result_callback = nullptr; void *userdata = nullptr; bool swap_result = false; bool collided = false; @@ -150,7 +152,7 @@ struct _ConcaveCollisionInfo2D { }; bool GodotCollisionSolver2D::concave_callback(void *p_userdata, GodotShape2D *p_convex) { - _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata); + _ConcaveCollisionInfo2D &cinfo = *(static_cast<_ConcaveCollisionInfo2D *>(p_userdata)); cinfo.aabb_tests++; 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); @@ -185,17 +187,17 @@ bool GodotCollisionSolver2D::solve_concave(const GodotShape2D *p_shape_A, const cinfo.aabb_tests = 0; Transform2D rel_transform = p_transform_A; - rel_transform.elements[2] -= p_transform_B.get_origin(); + rel_transform.columns[2] -= p_transform_B.get_origin(); //quickly compute a local Rect2 Rect2 local_aabb; for (int i = 0; i < 2; i++) { - Vector2 axis(p_transform_B.elements[i]); + Vector2 axis(p_transform_B.columns[i]); real_t axis_scale = 1.0 / axis.length(); axis *= axis_scale; - real_t smin, smax; + real_t smin = 0.0, smax = 0.0; p_shape_A->project_rangev(axis, rel_transform, smin, smax); smin *= axis_scale; smax *= axis_scale; @@ -227,17 +229,19 @@ bool GodotCollisionSolver2D::solve(const GodotShape2D *p_shape_A, const Transfor if (type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { if (type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + WARN_PRINT_ONCE("Collisions between world boundaries are not supported."); return false; } if (swap) { - return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, p_margin_A); } else { - return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, p_margin_B); } } else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) { if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + WARN_PRINT_ONCE("Collisions between two rays are not supported."); return false; //no ray-ray } @@ -249,6 +253,7 @@ bool GodotCollisionSolver2D::solve(const GodotShape2D *p_shape_A, const Transfor } else if (concave_B) { if (concave_A) { + WARN_PRINT_ONCE("Collisions between two concave shapes are not supported."); return false; } diff --git a/servers/physics_2d/godot_collision_solver_2d.h b/servers/physics_2d/godot_collision_solver_2d.h index bd90641f04..3aac2751e0 100644 --- a/servers/physics_2d/godot_collision_solver_2d.h +++ b/servers/physics_2d/godot_collision_solver_2d.h @@ -38,7 +38,7 @@ public: typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); private: - static bool solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool concave_callback(void *p_userdata, GodotShape2D *p_convex); static bool solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *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 GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *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); diff --git a/servers/physics_2d/godot_collision_solver_2d_sat.cpp b/servers/physics_2d/godot_collision_solver_2d_sat.cpp index ded3ff356b..f8924239ea 100644 --- a/servers/physics_2d/godot_collision_solver_2d_sat.cpp +++ b/servers/physics_2d/godot_collision_solver_2d_sat.cpp @@ -33,7 +33,7 @@ #include "core/math/geometry_2d.h" struct _CollectorCallback2D { - GodotCollisionSolver2D::CallbackResult callback; + GodotCollisionSolver2D::CallbackResult callback = nullptr; void *userdata = nullptr; bool swap = false; bool collided = false; @@ -234,7 +234,7 @@ public: axis = Vector2(0.0, 1.0); } - real_t min_A, max_A, min_B, max_B; + real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0; if (castA) { shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A); @@ -477,11 +477,11 @@ static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transfor return; } - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { return; } - if (!separator.test_axis(p_transform_b.elements[1].normalized())) { + if (!separator.test_axis(p_transform_b.columns[1].normalized())) { return; } @@ -498,7 +498,7 @@ static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transfor return; } - if (castA) { + if constexpr (castA) { if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) { return; } @@ -507,7 +507,7 @@ static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transfor } } - if (castB) { + if constexpr (castB) { if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) { return; } @@ -516,7 +516,7 @@ static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transfor } } - if (castA && castB) { + if constexpr (castA && castB) { if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) { return; } @@ -548,22 +548,22 @@ static void _collision_segment_capsule(const GodotShape2D *p_a, const Transform2 return; } - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { return; } 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))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.columns[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_dir))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.columns[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_dir))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.columns[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_dir))) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) { return; } @@ -646,8 +646,8 @@ static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform return; } - const Vector2 &sphere = p_transform_a.elements[2]; - const Vector2 *axis = &p_transform_b.elements[0]; + const Vector2 &sphere = p_transform_a.columns[2]; + const Vector2 *axis = &p_transform_b.columns[0]; //const Vector2& half_extents = rectangle_B->get_half_extents(); if (!separator.test_axis(axis[0].normalized())) { @@ -665,21 +665,21 @@ static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform } } - if (castA) { + if constexpr (castA) { Vector2 sphereofs = sphere + p_motion_a; if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { return; } } - if (castB) { + if constexpr (castB) { Vector2 sphereofs = sphere - p_motion_b; if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { return; } } - if (castA && castB) { + if constexpr (castA && castB) { Vector2 sphereofs = sphere - p_motion_b + p_motion_a; if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { return; @@ -705,17 +705,17 @@ static void _collision_circle_capsule(const GodotShape2D *p_a, const Transform2D } //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { 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_dir))) { + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) { return; } - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) { + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) { return; } @@ -769,24 +769,24 @@ static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transf } //box faces A - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { return; } - if (!separator.test_axis(p_transform_a.elements[1].normalized())) { + if (!separator.test_axis(p_transform_a.columns[1].normalized())) { return; } //box faces B - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { return; } - if (!separator.test_axis(p_transform_b.elements[1].normalized())) { + if (!separator.test_axis(p_transform_b.columns[1].normalized())) { return; } - if (withMargin) { + if constexpr (withMargin) { Transform2D invA = p_transform_a.affine_inverse(); Transform2D invB = p_transform_b.affine_inverse(); @@ -794,29 +794,29 @@ static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transf return; } - if (castA || castB) { + if constexpr (castA || castB) { Transform2D aofs = p_transform_a; - aofs.elements[2] += p_motion_a; + aofs.columns[2] += p_motion_a; Transform2D bofs = p_transform_b; - bofs.elements[2] += p_motion_b; + bofs.columns[2] += p_motion_b; - Transform2D aofsinv = aofs.affine_inverse(); - Transform2D bofsinv = bofs.affine_inverse(); + [[maybe_unused]] Transform2D aofsinv = aofs.affine_inverse(); + [[maybe_unused]] Transform2D bofsinv = bofs.affine_inverse(); - if (castA) { + if constexpr (castA) { if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) { return; } } - if (castB) { + if constexpr (castB) { if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) { return; } } - if (castA && castB) { + if constexpr (castA && castB) { if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) { return; } @@ -843,16 +843,16 @@ static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transfor } //box faces - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { return; } - if (!separator.test_axis(p_transform_a.elements[1].normalized())) { + if (!separator.test_axis(p_transform_a.columns[1].normalized())) { return; } //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { return; } @@ -864,15 +864,15 @@ static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transfor for (int i = 0; i < 2; i++) { { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir; if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { return; } } - if (castA) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + if constexpr (castA) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir; capsule_endpoint -= p_motion_a; if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { @@ -880,8 +880,8 @@ static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transfor } } - if (castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + if constexpr (castB) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir; capsule_endpoint += p_motion_b; if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { @@ -889,8 +889,8 @@ static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transfor } } - if (castA && castB) { - Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir; + if constexpr (castA && castB) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir; capsule_endpoint -= p_motion_a; capsule_endpoint += p_motion_b; @@ -921,17 +921,17 @@ static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const T } //box faces - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { return; } - if (!separator.test_axis(p_transform_a.elements[1].normalized())) { + if (!separator.test_axis(p_transform_a.columns[1].normalized())) { return; } //convex faces Transform2D boxinv; - if (withMargin) { + if constexpr (withMargin) { boxinv = p_transform_a.affine_inverse(); } for (int i = 0; i < convex_B->get_point_count(); i++) { @@ -939,22 +939,22 @@ static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const T return; } - if (withMargin) { + if constexpr (withMargin) { //all points vs all points need to be tested if margin exist if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) { return; } - if (castA) { + if constexpr (castA) { if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) { return; } } - if (castB) { + if constexpr (castB) { if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) { return; } } - if (castA && castB) { + if constexpr (castA && castB) { if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) { return; } @@ -984,11 +984,11 @@ static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2 //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) { + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { return; } - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { return; } @@ -996,11 +996,11 @@ static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2 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_dir_A; + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[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_dir_B; + Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir_B; if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) { return; @@ -1032,7 +1032,7 @@ static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Tra //capsule axis - if (!separator.test_axis(p_transform_a.elements[0].normalized())) { + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { return; } @@ -1042,7 +1042,7 @@ static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Tra 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_dir; + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir; if (TEST_POINT(capsule_endpoint_A, cpoint)) { return; diff --git a/servers/physics_2d/godot_joints_2d.cpp b/servers/physics_2d/godot_joints_2d.cpp index 0c21b08ea9..34eb66500d 100644 --- a/servers/physics_2d/godot_joints_2d.cpp +++ b/servers/physics_2d/godot_joints_2d.cpp @@ -436,13 +436,13 @@ void GodotDampedSpringJoint2D::solve(real_t p_step) { // not 100% certain this is derived correctly, though it makes sense real_t v_damp = -vrn * v_coef; target_vrn = vrn + v_damp; - Vector2 j = n * v_damp * n_mass; + Vector2 j_new = n * v_damp * n_mass; if (dynamic_A) { - A->apply_impulse(-j, rA); + A->apply_impulse(-j_new, rA); } if (dynamic_B) { - B->apply_impulse(j, rB); + B->apply_impulse(j_new, rB); } } diff --git a/servers/physics_2d/godot_physics_server_2d.cpp b/servers/physics_2d/godot_physics_server_2d.cpp index a9b499c6b5..f1551df2e3 100644 --- a/servers/physics_2d/godot_physics_server_2d.cpp +++ b/servers/physics_2d/godot_physics_server_2d.cpp @@ -144,7 +144,7 @@ real_t GodotPhysicsServer2D::shape_get_custom_solver_bias(RID p_shape) const { } void GodotPhysicsServer2D::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { - CollCbkData *cbk = (CollCbkData *)p_userdata; + CollCbkData *cbk = static_cast<CollCbkData *>(p_userdata); if (cbk->max == 0) { return; @@ -485,6 +485,20 @@ void GodotPhysicsServer2D::area_set_monitorable(RID p_area, bool p_monitorable) area->set_monitorable(p_monitorable); } +void GodotPhysicsServer2D::area_set_collision_layer(RID p_area, uint32_t p_layer) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND(!area); + + area->set_collision_layer(p_layer); +} + +uint32_t GodotPhysicsServer2D::area_get_collision_layer(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, 0); + + return area->get_collision_layer(); +} + void GodotPhysicsServer2D::area_set_collision_mask(RID p_area, uint32_t p_mask) { GodotArea2D *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); @@ -492,11 +506,11 @@ void GodotPhysicsServer2D::area_set_collision_mask(RID p_area, uint32_t p_mask) area->set_collision_mask(p_mask); } -void GodotPhysicsServer2D::area_set_collision_layer(RID p_area, uint32_t p_layer) { +uint32_t GodotPhysicsServer2D::area_get_collision_mask(RID p_area) const { GodotArea2D *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); + ERR_FAIL_COND_V(!area, 0); - area->set_collision_layer(p_layer); + return area->get_collision_mask(); } void GodotPhysicsServer2D::area_set_monitor_callback(RID p_area, const Callable &p_callback) { @@ -718,6 +732,20 @@ uint32_t GodotPhysicsServer2D::body_get_collision_mask(RID p_body) const { return body->get_collision_mask(); } +void GodotPhysicsServer2D::body_set_collision_priority(RID p_body, real_t p_priority) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_collision_priority(p_priority); +} + +real_t GodotPhysicsServer2D::body_get_collision_priority(RID p_body) const { + const GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_priority(); +} + void GodotPhysicsServer2D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); @@ -834,7 +862,7 @@ void GodotPhysicsServer2D::body_set_constant_force(RID p_body, const Vector2 &p_ ERR_FAIL_COND(!body); body->set_constant_force(p_force); - if (!p_force.is_equal_approx(Vector2())) { + if (!p_force.is_zero_approx()) { body->wakeup(); } } @@ -937,10 +965,10 @@ int GodotPhysicsServer2D::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } -void GodotPhysicsServer2D::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { +void GodotPhysicsServer2D::body_set_state_sync_callback(RID p_body, const Callable &p_callable) { GodotBody2D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->set_state_sync_callback(p_instance, p_callback); + body->set_state_sync_callback(p_callable); } void GodotPhysicsServer2D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { @@ -1004,6 +1032,7 @@ RID GodotPhysicsServer2D::joint_create() { void GodotPhysicsServer2D::joint_clear(RID p_joint) { GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); if (joint->get_type() != JOINT_TYPE_MAX) { GodotJoint2D *empty_joint = memnew(GodotJoint2D); empty_joint->copy_settings_from(joint); @@ -1130,38 +1159,38 @@ void GodotPhysicsServer2D::joint_make_damped_spring(RID p_joint, const Vector2 & } void GodotPhysicsServer2D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { - GodotJoint2D *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!j); - ERR_FAIL_COND(j->get_type() != JOINT_TYPE_PIN); + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); - GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(j); + GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint); pin_joint->set_param(p_param, p_value); } real_t GodotPhysicsServer2D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { - GodotJoint2D *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!j, 0); - ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_PIN, 0); + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); - GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(j); + GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint); return pin_joint->get_param(p_param); } void GodotPhysicsServer2D::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) { - GodotJoint2D *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND(!j); - ERR_FAIL_COND(j->get_type() != JOINT_TYPE_DAMPED_SPRING); + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_DAMPED_SPRING); - GodotDampedSpringJoint2D *dsj = static_cast<GodotDampedSpringJoint2D *>(j); + GodotDampedSpringJoint2D *dsj = static_cast<GodotDampedSpringJoint2D *>(joint); dsj->set_param(p_param, p_value); } real_t GodotPhysicsServer2D::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const { - GodotJoint2D *j = joint_owner.get_or_null(p_joint); - ERR_FAIL_COND_V(!j, 0); - ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); - GodotDampedSpringJoint2D *dsj = static_cast<GodotDampedSpringJoint2D *>(j); + GodotDampedSpringJoint2D *dsj = static_cast<GodotDampedSpringJoint2D *>(joint); return dsj->get_param(p_param); } @@ -1179,7 +1208,7 @@ void GodotPhysicsServer2D::free(RID p_rid) { GodotShape2D *shape = shape_owner.get_or_null(p_rid); while (shape->get_owners().size()) { - GodotShapeOwner2D *so = shape->get_owners().front()->key(); + GodotShapeOwner2D *so = shape->get_owners().begin()->key; so->remove_shape(shape); } @@ -1212,7 +1241,7 @@ void GodotPhysicsServer2D::free(RID p_rid) { GodotSpace2D *space = space_owner.get_or_null(p_rid); while (space->get_objects().size()) { - GodotCollisionObject2D *co = (GodotCollisionObject2D *)space->get_objects().front()->get(); + GodotCollisionObject2D *co = static_cast<GodotCollisionObject2D *>(*space->get_objects().begin()); co->set_space(nullptr); } @@ -1250,11 +1279,11 @@ void GodotPhysicsServer2D::step(real_t p_step) { island_count = 0; active_objects = 0; collision_pairs = 0; - for (Set<const GodotSpace2D *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((GodotSpace2D *)E->get(), p_step); - island_count += E->get()->get_island_count(); - active_objects += E->get()->get_active_objects(); - collision_pairs += E->get()->get_collision_pairs(); + for (const GodotSpace2D *E : active_spaces) { + stepper->step(const_cast<GodotSpace2D *>(E), p_step); + island_count += E->get_island_count(); + active_objects += E->get_active_objects(); + collision_pairs += E->get_collision_pairs(); } } @@ -1271,8 +1300,8 @@ void GodotPhysicsServer2D::flush_queries() { uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - for (Set<const GodotSpace2D *>::Element *E = active_spaces.front(); E; E = E->next()) { - GodotSpace2D *space = (GodotSpace2D *)E->get(); + for (const GodotSpace2D *E : active_spaces) { + GodotSpace2D *space = const_cast<GodotSpace2D *>(E); space->call_queries(); } @@ -1292,9 +1321,9 @@ void GodotPhysicsServer2D::flush_queries() { total_time[i] = 0; } - for (Set<const GodotSpace2D *>::Element *E = active_spaces.front(); E; E = E->next()) { + for (const GodotSpace2D *E : active_spaces) { for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { - total_time[i] += E->get()->get_elapsed_time(GodotSpace2D::ElapsedTime(i)); + total_time[i] += E->get_elapsed_time(GodotSpace2D::ElapsedTime(i)); } } diff --git a/servers/physics_2d/godot_physics_server_2d.h b/servers/physics_2d/godot_physics_server_2d.h index 0a84caadc5..b96677700c 100644 --- a/servers/physics_2d/godot_physics_server_2d.h +++ b/servers/physics_2d/godot_physics_server_2d.h @@ -56,7 +56,7 @@ class GodotPhysicsServer2D : public PhysicsServer2D { bool flushing_queries = false; GodotStep2D *stepper = nullptr; - Set<const GodotSpace2D *> active_spaces; + HashSet<const GodotSpace2D *> active_spaces; mutable RID_PtrOwner<GodotShape2D, true> shape_owner; mutable RID_PtrOwner<GodotSpace2D, true> space_owner; @@ -151,8 +151,12 @@ public: virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; virtual Transform2D area_get_transform(RID p_area) const override; virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; - virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; + virtual uint32_t area_get_collision_layer(RID p_area) const override; + + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual uint32_t area_get_collision_mask(RID p_area) const override; virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override; virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override; @@ -199,6 +203,9 @@ public: virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override; virtual uint32_t body_get_collision_mask(RID p_body) const override; + virtual void body_set_collision_priority(RID p_body, real_t p_priority) override; + virtual real_t body_get_collision_priority(RID p_body) const override; + virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; @@ -240,7 +247,7 @@ 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_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; + virtual void body_set_state_sync_callback(RID p_body, const Callable &p_callable) 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; diff --git a/servers/physics_2d/godot_shape_2d.cpp b/servers/physics_2d/godot_shape_2d.cpp index b5dbb8a2dd..da414ae233 100644 --- a/servers/physics_2d/godot_shape_2d.cpp +++ b/servers/physics_2d/godot_shape_2d.cpp @@ -37,7 +37,7 @@ void GodotShape2D::configure(const Rect2 &p_aabb) { aabb = p_aabb; configured = true; for (const KeyValue<GodotShapeOwner2D *, int> &E : owners) { - GodotShapeOwner2D *co = (GodotShapeOwner2D *)E.key; + GodotShapeOwner2D *co = const_cast<GodotShapeOwner2D *>(E.key); co->_shape_changed(); } } @@ -50,20 +50,20 @@ Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const { } void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) { - Map<GodotShapeOwner2D *, int>::Element *E = owners.find(p_owner); + HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner); if (E) { - E->get()++; + E->value++; } else { owners[p_owner] = 1; } } void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) { - Map<GodotShapeOwner2D *, int>::Element *E = owners.find(p_owner); + HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner); ERR_FAIL_COND(!E); - E->get()--; - if (E->get() == 0) { - owners.erase(E); + E->value--; + if (E->value == 0) { + owners.remove(E); } } @@ -71,7 +71,7 @@ bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const { return owners.has(p_owner); } -const Map<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const { +const HashMap<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const { return owners; } @@ -225,16 +225,16 @@ void GodotSegmentShape2D::set_data(const Variant &p_data) { b = r.size; n = (b - a).orthogonal(); - Rect2 aabb; - aabb.position = a; - aabb.expand_to(b); - if (aabb.size.x == 0) { - aabb.size.x = 0.001; + Rect2 aabb_new; + aabb_new.position = a; + aabb_new.expand_to(b); + if (aabb_new.size.x == 0) { + aabb_new.size.x = 0.001; } - if (aabb.size.y == 0) { - aabb.size.y = 0.001; + if (aabb_new.size.y == 0) { + aabb_new.size.y = 0.001; } - configure(aabb); + configure(aabb_new); } Variant GodotSegmentShape2D::get_data() const { @@ -564,13 +564,13 @@ bool GodotConvexPolygonShape2D::intersect_segment(const Vector2 &p_begin, const real_t GodotConvexPolygonShape2D::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; + Rect2 aabb_new; + aabb_new.position = points[0].pos * p_scale; for (int i = 0; i < point_count; i++) { - aabb.expand_to(points[i].pos * p_scale); + aabb_new.expand_to(points[i].pos * p_scale); } - return p_mass * aabb.size.dot(aabb.size) / 12.0; + return p_mass * aabb_new.size.dot(aabb_new.size) / 12.0; } void GodotConvexPolygonShape2D::set_data(const Variant &p_data) { @@ -620,13 +620,13 @@ void GodotConvexPolygonShape2D::set_data(const Variant &p_data) { } ERR_FAIL_COND(point_count == 0); - Rect2 aabb; - aabb.position = points[0].pos; + Rect2 aabb_new; + aabb_new.position = points[0].pos; for (int i = 1; i < point_count; i++) { - aabb.expand_to(points[i].pos); + aabb_new.expand_to(points[i].pos); } - configure(aabb); + configure(aabb_new); } Variant GodotConvexPolygonShape2D::get_data() const { @@ -705,18 +705,18 @@ bool GodotConcavePolygonShape2D::intersect_segment(const Vector2 &p_begin, const stack[0] = 0; while (true) { uint32_t node = stack[level] & NODE_IDX_MASK; - const BVH &bvh = bvhptr[node]; + const BVH &bvh2 = bvhptr[node]; bool done = false; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = bvh.aabb.intersects_segment(p_begin, p_end); + bool valid = bvh2.aabb.intersects_segment(p_begin, p_end); if (!valid) { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (bvh.left < 0) { - const Segment &s = segmentptr[bvh.right]; + if (bvh2.left < 0) { + const Segment &s = segmentptr[bvh2.right]; Vector2 a = pointptr[s.points[0]]; Vector2 b = pointptr[s.points[1]]; @@ -742,13 +742,13 @@ bool GodotConcavePolygonShape2D::intersect_segment(const Vector2 &p_begin, const continue; case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.left | TEST_AABB_BIT; + stack[level + 1] = bvh2.left | TEST_AABB_BIT; level++; } continue; case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.right | TEST_AABB_BIT; + stack[level + 1] = bvh2.right | TEST_AABB_BIT; level++; } continue; @@ -822,7 +822,7 @@ void GodotConcavePolygonShape2D::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); #endif - Rect2 aabb; + Rect2 aabb_new; if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { Vector<Vector2> p2arr = p_data; @@ -835,13 +835,13 @@ void GodotConcavePolygonShape2D::set_data(const Variant &p_data) { bvh_depth = 1; if (len == 0) { - configure(aabb); + configure(aabb_new); return; } const Vector2 *arr = p2arr.ptr(); - Map<Point2, int> pointmap; + HashMap<Point2, int> pointmap; for (int i = 0; i < len; i += 2) { Point2 p1 = arr[i]; Point2 p2 = arr[i + 1]; @@ -868,9 +868,9 @@ void GodotConcavePolygonShape2D::set_data(const Variant &p_data) { } points.resize(pointmap.size()); - aabb.position = pointmap.front()->key(); + aabb_new.position = pointmap.begin()->key; for (const KeyValue<Point2, int> &E : pointmap) { - aabb.expand_to(E.key); + aabb_new.expand_to(E.key); points.write[E.value] = E.key; } @@ -889,7 +889,7 @@ void GodotConcavePolygonShape2D::set_data(const Variant &p_data) { //dictionary with arrays } - configure(aabb); + configure(aabb_new); } Variant GodotConcavePolygonShape2D::get_data() const { @@ -937,17 +937,17 @@ void GodotConcavePolygonShape2D::cull(const Rect2 &p_local_aabb, QueryCallback p stack[0] = 0; while (true) { uint32_t node = stack[level] & NODE_IDX_MASK; - const BVH &bvh = bvhptr[node]; + const BVH &bvh2 = bvhptr[node]; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = p_local_aabb.intersects(bvh.aabb); + bool valid = p_local_aabb.intersects(bvh2.aabb); if (!valid) { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (bvh.left < 0) { - const Segment &s = segmentptr[bvh.right]; + if (bvh2.left < 0) { + const Segment &s = segmentptr[bvh2.right]; Vector2 a = pointptr[s.points[0]]; Vector2 b = pointptr[s.points[1]]; @@ -966,13 +966,13 @@ void GodotConcavePolygonShape2D::cull(const Rect2 &p_local_aabb, QueryCallback p continue; case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.left | TEST_AABB_BIT; + stack[level + 1] = bvh2.left | TEST_AABB_BIT; level++; } continue; case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; - stack[level + 1] = bvh.right | TEST_AABB_BIT; + stack[level + 1] = bvh2.right | TEST_AABB_BIT; level++; } continue; diff --git a/servers/physics_2d/godot_shape_2d.h b/servers/physics_2d/godot_shape_2d.h index fb52cbce20..cede01e18d 100644 --- a/servers/physics_2d/godot_shape_2d.h +++ b/servers/physics_2d/godot_shape_2d.h @@ -50,7 +50,7 @@ class GodotShape2D { bool configured = false; real_t custom_bias = 0.0; - Map<GodotShapeOwner2D *, int> owners; + HashMap<GodotShapeOwner2D *, int> owners; protected: void configure(const Rect2 &p_aabb); @@ -86,7 +86,7 @@ public: void add_owner(GodotShapeOwner2D *p_owner); void remove_owner(GodotShapeOwner2D *p_owner); bool is_owner(GodotShapeOwner2D *p_owner) const; - const Map<GodotShapeOwner2D *, int> &get_owners() const; + const HashMap<GodotShapeOwner2D *, int> &get_owners() const; _FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const { get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount); @@ -134,7 +134,7 @@ public: real_t mina, maxa; \ real_t minb, maxb; \ Transform2D ofsb = p_transform; \ - ofsb.elements[2] += p_cast; \ + ofsb.columns[2] += p_cast; \ project_range(p_normal, p_transform, mina, maxa); \ project_range(p_normal, ofsb, minb, maxb); \ r_min = MIN(mina, minb); \ diff --git a/servers/physics_2d/godot_space_2d.cpp b/servers/physics_2d/godot_space_2d.cpp index 99c0566fb1..a82d7dbbc4 100644 --- a/servers/physics_2d/godot_space_2d.cpp +++ b/servers/physics_2d/godot_space_2d.cpp @@ -129,8 +129,8 @@ bool GodotPhysicsDirectSpaceState2D::intersect_ray(const RayParameters &p_parame bool collided = false; Vector2 res_point, res_normal; - int res_shape; - const GodotCollisionObject2D *res_obj; + int res_shape = -1; + const GodotCollisionObject2D *res_obj = nullptr; real_t min_d = 1e10; for (int i = 0; i < amount; i++) { @@ -190,6 +190,7 @@ bool GodotPhysicsDirectSpaceState2D::intersect_ray(const RayParameters &p_parame if (!collided) { return false; } + ERR_FAIL_NULL_V(res_obj, false); // Shouldn't happen but silences warning. r_result.collider_id = res_obj->get_instance_id(); if (r_result.collider_id.is_valid()) { @@ -403,7 +404,7 @@ 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; + _RestCallbackData2D *rd = static_cast<_RestCallbackData2D *>(p_userdata); Vector2 contact_rel = p_point_B - p_point_A; real_t len = contact_rel.length(); @@ -594,6 +595,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: const int max_results = 32; int recover_attempts = 4; Vector2 sr[max_results * 2]; + real_t priorities[max_results]; do { GodotPhysicsServer2D::CollCbkData cbk; @@ -606,6 +608,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk; GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk; + int priority_amount = 0; bool collided = false; @@ -633,7 +636,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(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(); + cbk.valid_dir = col_obj_shape_xform.columns[1].normalized(); real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work @@ -641,7 +644,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { //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 @@ -664,6 +667,10 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, margin)) { did_collide = cbk.passed > current_passed; //more passed, so collision actually existed } + while (cbk.amount > priority_amount) { + priorities[priority_amount] = col_obj->get_collision_priority(); + priority_amount++; + } if (!did_collide && cbk.invalid_by_dir > 0) { //this shape must be excluded @@ -686,6 +693,12 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: break; } + real_t inv_total_weight = 0.0; + for (int i = 0; i < cbk.amount; i++) { + inv_total_weight += priorities[i]; + } + inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight; + recovered = true; Vector2 recover_motion; @@ -701,7 +714,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: real_t depth = n.dot(a + recover_motion) - d; if (depth > min_contact_depth + CMP_EPSILON) { // Only recover if there is penetration. - recover_motion -= n * (depth - min_contact_depth) * 0.4; + recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight; } } @@ -710,7 +723,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: break; } - body_transform.elements[2] += recover_motion; + body_transform.columns[2] += recover_motion; body_aabb.position += recover_motion; recover_attempts--; @@ -788,7 +801,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: //test initial overlap if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { 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(); + Vector2 direction = col_obj_shape_xform.columns[1].normalized(); if (motion_normal.dot(direction) < 0) { continue; } @@ -838,7 +851,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: cbk.amount = 0; cbk.passed = 0; cbk.ptr = cd; - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); + cbk.valid_dir = col_obj_shape_xform.columns[1].normalized(); cbk.valid_depth = 10e20; @@ -874,14 +887,14 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: bool collided = false; - if (recovered || (safe < 1)) { + if ((p_parameters.recovery_as_collision && 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_parameters.motion * unsafe; + ugt.columns[2] += p_parameters.motion * unsafe; _RestCallbackData2D rcd; @@ -929,14 +942,14 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D:: Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(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_dir = col_obj_shape_xform.columns[1].normalized(); real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); rcd.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj); - if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { //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 @@ -1005,7 +1018,7 @@ void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A SWAP(type_A, type_B); } - GodotSpace2D *self = (GodotSpace2D *)p_self; + GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self); self->collision_pairs++; if (type_A == GodotCollisionObject2D::TYPE_AREA) { @@ -1021,11 +1034,9 @@ void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A } } else { - GodotBodyPair2D *b = memnew(GodotBodyPair2D((GodotBody2D *)A, p_subindex_A, (GodotBody2D *)B, p_subindex_B)); + GodotBodyPair2D *b = memnew(GodotBodyPair2D(static_cast<GodotBody2D *>(A), p_subindex_A, static_cast<GodotBody2D *>(B), p_subindex_B)); return b; } - - return nullptr; } void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self) { @@ -1033,9 +1044,9 @@ void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_ return; } - GodotSpace2D *self = (GodotSpace2D *)p_self; + GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self); self->collision_pairs--; - GodotConstraint2D *c = (GodotConstraint2D *)p_data; + GodotConstraint2D *c = static_cast<GodotConstraint2D *>(p_data); memdelete(c); } @@ -1073,7 +1084,7 @@ void GodotSpace2D::remove_object(GodotCollisionObject2D *p_object) { objects.erase(p_object); } -const Set<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const { +const HashSet<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const { return objects; } @@ -1206,7 +1217,7 @@ GodotPhysicsDirectSpaceState2D *GodotSpace2D::get_direct_state() { GodotSpace2D::GodotSpace2D() { 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", Math::deg2rad(8.0)); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg_to_rad(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/godot_space_2d.h b/servers/physics_2d/godot_space_2d.h index 5d97721176..b6fc48bd70 100644 --- a/servers/physics_2d/godot_space_2d.h +++ b/servers/physics_2d/godot_space_2d.h @@ -92,7 +92,7 @@ private: static void *_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self); static void _broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self); - Set<GodotCollisionObject2D *> objects; + HashSet<GodotCollisionObject2D *> objects; GodotArea2D *area = nullptr; @@ -156,7 +156,7 @@ public: void add_object(GodotCollisionObject2D *p_object); void remove_object(GodotCollisionObject2D *p_object); - const Set<GodotCollisionObject2D *> &get_objects() const; + const HashSet<GodotCollisionObject2D *> &get_objects() const; _FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; } _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } diff --git a/servers/physics_2d/godot_step_2d.cpp b/servers/physics_2d/godot_step_2d.cpp index 866c415440..46718c8819 100644 --- a/servers/physics_2d/godot_step_2d.cpp +++ b/servers/physics_2d/godot_step_2d.cpp @@ -42,12 +42,12 @@ void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D p_body->set_island_step(_step); if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { - // Only dynamic bodies are tested for activation. + // Only rigid bodies are tested for activation. p_body_island.push_back(p_body); } for (const Pair<GodotConstraint2D *, int> &E : p_body->get_constraint_list()) { - GodotConstraint2D *constraint = (GodotConstraint2D *)E.first; + GodotConstraint2D *constraint = const_cast<GodotConstraint2D *>(E.first); if (constraint->get_island_step() == _step) { continue; // Already processed. } @@ -168,8 +168,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) { const SelfList<GodotArea2D>::List &aml = p_space->get_moved_area_list(); while (aml.first()) { - for (const Set<GodotConstraint2D *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - GodotConstraint2D *constraint = E->get(); + for (GodotConstraint2D *E : aml.first()->self()->get_constraints()) { + GodotConstraint2D *constraint = E; if (constraint->get_island_step() == _step) { continue; } @@ -239,7 +239,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) { /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ uint32_t total_contraint_count = all_constraints.size(); - work_pool.do_work(total_contraint_count, this, &GodotStep2D::_setup_contraint, nullptr); + WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_setup_contraint, nullptr, total_contraint_count, -1, true, SNAME("Physics2DConstraintSetup")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -258,7 +259,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) { // Warning: _solve_island modifies the constraint islands for optimization purpose, // their content is not reliable after these calls and shouldn't be used anymore. - work_pool.do_work(island_count, this, &GodotStep2D::_solve_island, nullptr); + group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics2DConstraintSolveIslands")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -297,10 +299,7 @@ GodotStep2D::GodotStep2D() { body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE); all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); - - work_pool.init(); } GodotStep2D::~GodotStep2D() { - work_pool.finish(); } diff --git a/servers/physics_2d/godot_step_2d.h b/servers/physics_2d/godot_step_2d.h index 9a6d8caf9b..9f8fdd6ce3 100644 --- a/servers/physics_2d/godot_step_2d.h +++ b/servers/physics_2d/godot_step_2d.h @@ -33,8 +33,8 @@ #include "godot_space_2d.h" +#include "core/object/worker_thread_pool.h" #include "core/templates/local_vector.h" -#include "core/templates/thread_work_pool.h" class GodotStep2D { uint64_t _step = 1; @@ -42,8 +42,6 @@ class GodotStep2D { int iterations = 0; real_t delta = 0.0; - ThreadWorkPool work_pool; - LocalVector<LocalVector<GodotBody2D *>> body_islands; LocalVector<LocalVector<GodotConstraint2D *>> constraint_islands; LocalVector<GodotConstraint2D *> all_constraints; |