diff options
Diffstat (limited to 'servers/physics_2d')
30 files changed, 0 insertions, 795 deletions
diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 6ae7c90c89..b566e75851 100644 --- a/servers/physics_2d/area_2d_sw.cpp +++ b/servers/physics_2d/area_2d_sw.cpp @@ -46,13 +46,11 @@ Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_a } void Area2DSW::_shapes_changed() { - if (!moved_list.in_list() && get_space()) get_space()->area_add_to_moved_list(&moved_list); } void Area2DSW::set_transform(const Transform2D &p_transform) { - if (!moved_list.in_list() && get_space()) get_space()->area_add_to_moved_list(&moved_list); @@ -61,7 +59,6 @@ void Area2DSW::set_transform(const Transform2D &p_transform) { } void Area2DSW::set_space(Space2DSW *p_space) { - if (get_space()) { if (monitor_query_list.in_list()) get_space()->area_remove_from_monitor_query_list(&monitor_query_list); @@ -76,7 +73,6 @@ void Area2DSW::set_space(Space2DSW *p_space) { } void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == monitor_callback_id) { monitor_callback_method = p_method; return; @@ -97,7 +93,6 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { } void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == area_monitor_callback_id) { area_monitor_callback_method = p_method; return; @@ -127,7 +122,6 @@ void Area2DSW::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_ } void Area2DSW::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) { - switch (p_param) { case PhysicsServer2D::AREA_PARAM_GRAVITY: gravity = p_value; @@ -157,7 +151,6 @@ void Area2DSW::set_param(PhysicsServer2D::AreaParameter p_param, const Variant & } Variant Area2DSW::get_param(PhysicsServer2D::AreaParameter p_param) const { - switch (p_param) { case PhysicsServer2D::AREA_PARAM_GRAVITY: return gravity; @@ -181,7 +174,6 @@ Variant Area2DSW::get_param(PhysicsServer2D::AreaParameter p_param) const { } void Area2DSW::_queue_monitor_update() { - ERR_FAIL_COND(!get_space()); if (!monitor_query_list.in_list()) @@ -189,7 +181,6 @@ void Area2DSW::_queue_monitor_update() { } void Area2DSW::set_monitorable(bool p_monitorable) { - if (monitorable == p_monitorable) return; @@ -198,9 +189,7 @@ void Area2DSW::set_monitorable(bool p_monitorable) { } void Area2DSW::call_queries() { - if (monitor_callback_id.is_valid() && !monitored_bodies.empty()) { - Variant res[5]; Variant *resptr[5]; for (int i = 0; i < 5; i++) @@ -214,7 +203,6 @@ void Area2DSW::call_queries() { } for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) { - if (E->get().state == 0) continue; //nothing happened @@ -232,7 +220,6 @@ void Area2DSW::call_queries() { monitored_bodies.clear(); if (area_monitor_callback_id.is_valid() && !monitored_areas.empty()) { - Variant res[5]; Variant *resptr[5]; for (int i = 0; i < 5; i++) @@ -246,7 +233,6 @@ void Area2DSW::call_queries() { } for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) { - if (E->get().state == 0) continue; //nothing happened @@ -270,7 +256,6 @@ Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { - _set_static(true); //areas are not active by default space_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; gravity = 9.80665; diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h index ae2a8ff995..4910167952 100644 --- a/servers/physics_2d/area_2d_sw.h +++ b/servers/physics_2d/area_2d_sw.h @@ -41,7 +41,6 @@ class Body2DSW; class Constraint2DSW; class Area2DSW : public CollisionObject2DSW { - PhysicsServer2D::AreaSpaceOverrideMode space_override_mode; real_t gravity; Vector2 gravity_vector; @@ -63,18 +62,14 @@ class Area2DSW : public CollisionObject2DSW { SelfList<Area2DSW> moved_list; struct BodyKey { - RID rid; ObjectID instance_id; uint32_t body_shape; uint32_t area_shape; _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; @@ -88,7 +83,6 @@ class Area2DSW : public CollisionObject2DSW { }; struct BodyState { - int state; _FORCE_INLINE_ void inc() { state++; } _FORCE_INLINE_ void dec() { state--; } @@ -170,14 +164,12 @@ public: }; void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body, p_body_shape, p_area_shape); monitored_bodies[bk].inc(); if (!monitor_query_list.in_list()) _queue_monitor_update(); } void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body, p_body_shape, p_area_shape); monitored_bodies[bk].dec(); if (!monitor_query_list.in_list()) @@ -185,14 +177,12 @@ void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, u } void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area, p_area_shape, p_self_shape); monitored_areas[bk].inc(); if (!monitor_query_list.in_list()) _queue_monitor_update(); } void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area, p_area_shape, p_self_shape); monitored_areas[bk].dec(); if (!monitor_query_list.in_list()) diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index 0e70a626c2..a3d5e403c1 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -32,7 +32,6 @@ #include "collision_solver_2d_sw.h" 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)) { @@ -42,16 +41,13 @@ bool AreaPair2DSW::setup(real_t p_step) { } 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()) @@ -68,7 +64,6 @@ void AreaPair2DSW::solve(real_t p_step) { } AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { - body = p_body; area = p_area; body_shape = p_body_shape; @@ -81,9 +76,7 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, } AreaPair2DSW::~AreaPair2DSW() { - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) body->remove_area(area); if (area->has_monitor_callback()) @@ -96,7 +89,6 @@ AreaPair2DSW::~AreaPair2DSW() { ////////////////////////////////// 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; @@ -105,9 +97,7 @@ bool Area2Pair2DSW::setup(real_t p_step) { } 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); @@ -115,7 +105,6 @@ bool Area2Pair2DSW::setup(real_t p_step) { area_a->add_area_to_query(area_b, shape_b, shape_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); @@ -133,7 +122,6 @@ void Area2Pair2DSW::solve(real_t p_step) { } Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { - area_a = p_area_a; area_b = p_area_b; shape_a = p_shape_a; @@ -144,9 +132,7 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area } Area2Pair2DSW::~Area2Pair2DSW() { - if (colliding) { - if (area_b->has_area_monitor_callback()) area_b->remove_area_from_query(area_a, shape_a, shape_b); diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h index 78173d98b2..5e8670b464 100644 --- a/servers/physics_2d/area_pair_2d_sw.h +++ b/servers/physics_2d/area_pair_2d_sw.h @@ -36,7 +36,6 @@ #include "constraint_2d_sw.h" class AreaPair2DSW : public Constraint2DSW { - Body2DSW *body; Area2DSW *area; int body_shape; @@ -52,7 +51,6 @@ public: }; class Area2Pair2DSW : public Constraint2DSW { - Area2DSW *area_a; Area2DSW *area_b; int shape_a; diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index 39e28fd002..12c7a49288 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -34,19 +34,15 @@ #include "space_2d_sw.h" void Body2DSW::_update_inertia() { - if (!user_inertia && get_space() && !inertia_update_list.in_list()) get_space()->body_add_to_inertia_update_list(&inertia_update_list); } void Body2DSW::update_inertias() { - //update shapes and motions switch (mode) { - case PhysicsServer2D::BODY_MODE_RIGID: { - if (user_inertia) { _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; break; @@ -55,14 +51,12 @@ void Body2DSW::update_inertias() { real_t total_area = 0; for (int i = 0; i < get_shape_count(); i++) { - total_area += get_shape_aabb(i).get_area(); } inertia = 0; for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { continue; } @@ -88,12 +82,10 @@ void Body2DSW::update_inertias() { } break; case PhysicsServer2D::BODY_MODE_KINEMATIC: case PhysicsServer2D::BODY_MODE_STATIC: { - _inv_inertia = 0; _inv_mass = 0; } break; case PhysicsServer2D::BODY_MODE_CHARACTER: { - _inv_inertia = 0; _inv_mass = 1.0 / mass; @@ -105,7 +97,6 @@ void Body2DSW::update_inertias() { } void Body2DSW::set_active(bool p_active) { - if (active == p_active) return; @@ -135,14 +126,11 @@ void Body2DSW::set_active(bool p_active) { } void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) { - switch (p_param) { case PhysicsServer2D::BODY_PARAM_BOUNCE: { - bounce = p_value; } break; case PhysicsServer2D::BODY_PARAM_FRICTION: { - friction = p_value; } break; case PhysicsServer2D::BODY_PARAM_MASS: { @@ -165,11 +153,9 @@ void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) gravity_scale = p_value; } break; case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { - linear_damp = p_value; } break; case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { - angular_damp = p_value; } break; default: { @@ -178,14 +164,11 @@ void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) } real_t Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const { - switch (p_param) { case PhysicsServer2D::BODY_PARAM_BOUNCE: { - return bounce; } case PhysicsServer2D::BODY_PARAM_FRICTION: { - return friction; } case PhysicsServer2D::BODY_PARAM_MASS: { @@ -198,11 +181,9 @@ real_t Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const { return gravity_scale; } case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { - return linear_damp; } case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { - return angular_damp; } default: { @@ -213,7 +194,6 @@ real_t Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const { } void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { - PhysicsServer2D::BodyMode prev = mode; mode = p_mode; @@ -221,7 +201,6 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! case PhysicsServer2D::BODY_MODE_STATIC: case PhysicsServer2D::BODY_MODE_KINEMATIC: { - _set_inv_transform(get_transform().affine_inverse()); _inv_mass = 0; _inv_inertia = 0; @@ -234,7 +213,6 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { } } break; case PhysicsServer2D::BODY_MODE_RIGID: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; _set_static(false); @@ -242,7 +220,6 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { } break; case PhysicsServer2D::BODY_MODE_CHARACTER: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = 0; _set_static(false); @@ -259,23 +236,18 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { */ } PhysicsServer2D::BodyMode Body2DSW::get_mode() const { - return mode; } void Body2DSW::_shapes_changed() { - _update_inertia(); wakeup_neighbours(); } void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) { - switch (p_state) { case PhysicsServer2D::BODY_STATE_TRANSFORM: { - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - new_transform = p_variant; //wakeup_neighbours(); set_active(true); @@ -301,7 +273,6 @@ 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; @@ -344,7 +315,6 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va } } Variant Body2DSW::get_state(PhysicsServer2D::BodyState p_state) const { - switch (p_state) { case PhysicsServer2D::BODY_STATE_TRANSFORM: { return get_transform(); @@ -367,9 +337,7 @@ Variant Body2DSW::get_state(PhysicsServer2D::BodyState p_state) const { } void Body2DSW::set_space(Space2DSW *p_space) { - if (get_space()) { - wakeup_neighbours(); if (inertia_update_list.in_list()) @@ -383,7 +351,6 @@ void Body2DSW::set_space(Space2DSW *p_space) { _set_space(p_space); if (get_space()) { - _update_inertia(); if (active) get_space()->body_add_to_active_list(&active_list); @@ -400,7 +367,6 @@ void Body2DSW::set_space(Space2DSW *p_space) { } 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(); @@ -417,7 +383,6 @@ void Body2DSW::_compute_area_gravity_and_dampenings(const Area2DSW *p_area) { } void Body2DSW::integrate_forces(real_t p_step) { - if (mode == PhysicsServer2D::BODY_MODE_STATIC) return; @@ -479,7 +444,6 @@ void Body2DSW::integrate_forces(real_t p_step) { bool do_motion = false; if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - //compute motion, angular and etc. velocities from prev transform motion = new_transform.get_origin() - get_transform().get_origin(); linear_velocity = motion / p_step; @@ -522,7 +486,6 @@ void Body2DSW::integrate_forces(real_t p_step) { } if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) { - motion = linear_velocity * p_step; do_motion = true; } @@ -544,7 +507,6 @@ void Body2DSW::integrate_forces(real_t p_step) { } void Body2DSW::integrate_velocities(real_t p_step) { - if (mode == PhysicsServer2D::BODY_MODE_STATIC) return; @@ -552,7 +514,6 @@ void Body2DSW::integrate_velocities(real_t p_step) { get_space()->body_add_to_state_query_list(&direct_state_query_list); if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { - _set_transform(new_transform, false); _set_inv_transform(new_transform.affine_inverse()); if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) @@ -576,15 +537,12 @@ 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(); Body2DSW **n = c->get_body_ptr(); int bc = c->get_body_count(); for (int i = 0; i < bc; i++) { - if (i == E->get()) continue; Body2DSW *b = n[i]; @@ -598,9 +556,7 @@ void Body2DSW::wakeup_neighbours() { } void Body2DSW::call_queries() { - if (fi_callback) { - PhysicsDirectBodyState2DSW *dbs = PhysicsDirectBodyState2DSW::singleton; dbs->body = this; @@ -609,12 +565,10 @@ void Body2DSW::call_queries() { Object *obj = ObjectDB::get_instance(fi_callback->id); if (!obj) { - set_force_integration_callback(ObjectID(), StringName()); } else { Callable::CallError ce; if (fi_callback->callback_udata.get_type() != Variant::NIL) { - obj->call(fi_callback->method, vp, 2, ce); } else { @@ -625,7 +579,6 @@ 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) @@ -634,27 +587,22 @@ bool Body2DSW::sleep_test(real_t p_step) { return false; if (Math::abs(angular_velocity) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { - still_time += p_step; return still_time > get_space()->get_body_time_to_sleep(); } else { - still_time = 0; //maybe this should be set to 0 on set_active? return false; } } void Body2DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { - if (fi_callback) { - memdelete(fi_callback); fi_callback = nullptr; } if (p_id.is_valid()) { - fi_callback = memnew(ForceIntegrationCallback); fi_callback->id = p_id; fi_callback->method = p_method; @@ -667,7 +615,6 @@ Body2DSW::Body2DSW() : active_list(this), inertia_update_list(this), direct_state_query_list(this) { - mode = PhysicsServer2D::BODY_MODE_RIGID; active = true; angular_velocity = 0; @@ -701,7 +648,6 @@ Body2DSW::Body2DSW() : } Body2DSW::~Body2DSW() { - if (fi_callback) memdelete(fi_callback); } @@ -709,23 +655,19 @@ Body2DSW::~Body2DSW() { PhysicsDirectBodyState2DSW *PhysicsDirectBodyState2DSW::singleton = nullptr; PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() { - return body->get_space()->get_direct_state(); } Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant()); if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) { - return Variant(); } Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider); int sidx = body->contacts[p_contact_idx].collider_shape; if (sidx < 0 || sidx >= other->get_shape_count()) { - return Variant(); } diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index a36dc3bfe2..c1fc4e2bb5 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -38,7 +38,6 @@ class Constraint2DSW; class Body2DSW : public CollisionObject2DSW { - PhysicsServer2D::BodyMode mode; Vector2 biased_linear_velocity; @@ -87,7 +86,6 @@ class Body2DSW : public CollisionObject2DSW { Map<Constraint2DSW *, int> constraint_map; struct AreaCMP { - Area2DSW *area; int refCount; _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } @@ -102,7 +100,6 @@ class Body2DSW : public CollisionObject2DSW { Vector<AreaCMP> areas; struct Contact { - Vector2 local_pos; Vector2 local_normal; real_t depth; @@ -118,7 +115,6 @@ class Body2DSW : public CollisionObject2DSW { int contact_count; struct ForceIntegrationCallback { - ObjectID id; StringName method; Variant callback_udata; @@ -206,7 +202,6 @@ public: } _FORCE_INLINE_ void apply_impulse(const Vector2 &p_offset, const Vector2 &p_impulse) { - linear_velocity += p_impulse * _inv_mass; angular_velocity += _inv_inertia * p_offset.cross(p_impulse); } @@ -216,7 +211,6 @@ public: } _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_pos, const Vector2 &p_j) { - biased_linear_velocity += p_j * _inv_mass; biased_angular_velocity += _inv_inertia * p_pos.cross(p_j); } @@ -250,7 +244,6 @@ public: } _FORCE_INLINE_ void add_force(const Vector2 &p_offset, const Vector2 &p_force) { - applied_force += p_force; applied_torque += p_offset.cross(p_force); } @@ -278,7 +271,6 @@ public: void integrate_velocities(real_t p_step); _FORCE_INLINE_ Vector2 get_motion() const { - if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) { return new_transform.get_origin() - get_transform().get_origin(); } else if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { @@ -299,7 +291,6 @@ public: //add contact inline void Body2DSW::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) { - int c_max = contacts.size(); if (c_max == 0) @@ -312,11 +303,9 @@ void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_no if (contact_count < c_max) { idx = contact_count++; } else { - real_t least_depth = 1e20; int least_deep = -1; for (int i = 0; i < c_max; i++) { - if (i == 0 || c[i].depth < least_depth) { least_deep = i; least_depth = c[i].depth; @@ -324,7 +313,6 @@ void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_no } if (least_deep >= 0 && least_depth < p_depth) { - idx = least_deep; } if (idx == -1) @@ -343,7 +331,6 @@ void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_no } class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D { - GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D); public: diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index f38a76cff6..32a3dd2504 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -36,14 +36,12 @@ #define ACCUMULATE_IMPULSES void BodyPair2DSW::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) { - BodyPair2DSW *self = (BodyPair2DSW *)p_self; self->_contact_added_callback(p_point_A, p_point_B); } void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) { - // check if we already have the contact Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); @@ -69,12 +67,10 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius(); for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; if ( c.local_A.distance_squared_to(local_A) < (recycle_radius_2) && c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) { - contact.acc_normal_impulse = c.acc_normal_impulse; contact.acc_tangent_impulse = c.acc_tangent_impulse; contact.acc_bias_impulse = c.acc_bias_impulse; @@ -86,14 +82,12 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto // figure out if the contact amount must be reduced to fit the new contact if (new_index == MAX_CONTACTS) { - // remove the contact with the minimum depth int least_deep = -1; real_t min_depth = 1e10; 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; @@ -102,7 +96,6 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto real_t depth = axis.dot(c.normal); if (depth < min_depth) { - min_depth = depth; least_deep = i; } @@ -121,20 +114,17 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto contacts[new_index] = contact; if (new_index == contact_count) { - contact_count++; } } void BodyPair2DSW::_validate_contacts() { - //make sure to erase contacts that are no longer valid real_t max_separation = space->get_contact_max_separation(); real_t max_separation2 = max_separation * max_separation; for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; bool erase = false; @@ -169,7 +159,6 @@ void BodyPair2DSW::_validate_contacts() { } bool BodyPair2DSW::_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) { - Vector2 motion = p_A->get_linear_velocity() * p_step; real_t mlen = motion.length(); if (mlen < CMP_EPSILON) @@ -228,7 +217,6 @@ 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)) { collided = false; @@ -270,7 +258,6 @@ bool BodyPair2DSW::setup(real_t p_step) { collided = CollisionSolver2DSW::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); if (!collided) { - //test ccd (currently just a raycast) if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { @@ -294,7 +281,6 @@ bool BodyPair2DSW::setup(real_t p_step) { //if (!prev_collided) { { - if (A->is_shape_set_as_one_way_collision(shape_A)) { Vector2 direction = xform_A.get_axis(1).normalized(); bool valid = false; @@ -345,7 +331,6 @@ bool BodyPair2DSW::setup(real_t p_step) { real_t bias = 0.3; 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(); else if (shape_B_ptr->get_custom_bias() == 0) @@ -361,7 +346,6 @@ bool BodyPair2DSW::setup(real_t p_step) { bool do_process = false; for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; Vector2 global_A = xform_Au.xform(c.local_A); @@ -388,7 +372,6 @@ bool BodyPair2DSW::setup(real_t p_step) { 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; @@ -399,7 +382,6 @@ bool BodyPair2DSW::setup(real_t p_step) { 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()); } @@ -442,7 +424,6 @@ bool BodyPair2DSW::setup(real_t p_step) { c.bounce = combine_bounce(A, B); if (c.bounce) { - Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; @@ -456,12 +437,10 @@ bool BodyPair2DSW::setup(real_t p_step) { } void BodyPair2DSW::solve(real_t p_step) { - if (!collided) return; for (int i = 0; i < contact_count; ++i) { - Contact &c = contacts[i]; cc++; @@ -512,7 +491,6 @@ void BodyPair2DSW::solve(real_t p_step) { BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B) : Constraint2DSW(_arr, 2) { - A = p_A; B = p_B; shape_A = p_shape_A; @@ -526,7 +504,6 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh } BodyPair2DSW::~BodyPair2DSW() { - A->remove_constraint(this); B->remove_constraint(this); } diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h index e46ecbc8eb..ea4d55841a 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -35,7 +35,6 @@ #include "constraint_2d_sw.h" class BodyPair2DSW : public Constraint2DSW { - enum { MAX_CONTACTS = 2 }; @@ -54,7 +53,6 @@ class BodyPair2DSW : public Constraint2DSW { Space2DSW *space; struct Contact { - Vector2 position; Vector2 normal; Vector2 local_A, local_B; diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp index 5e3a13f4dd..9f4960c4f6 100644 --- a/servers/physics_2d/broad_phase_2d_basic.cpp +++ b/servers/physics_2d/broad_phase_2d_basic.cpp @@ -31,7 +31,6 @@ #include "broad_phase_2d_basic.h" BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) { - current++; Element e; @@ -44,52 +43,43 @@ BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, } 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++; @@ -101,14 +91,11 @@ int BroadPhase2DBasic::cull_segment(const Vector2 &p_from, const Vector2 &p_to, 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++; @@ -121,23 +108,18 @@ int BroadPhase2DBasic::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_re } 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(); @@ -157,7 +139,6 @@ void BroadPhase2DBasic::update() { } 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); @@ -168,12 +149,10 @@ void BroadPhase2DBasic::update() { } BroadPhase2DSW *BroadPhase2DBasic::_create() { - return memnew(BroadPhase2DBasic); } BroadPhase2DBasic::BroadPhase2DBasic() { - current = 1; unpair_callback = nullptr; unpair_userdata = nullptr; diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_basic.h index 7d02590af9..ec5cfdbf1d 100644 --- a/servers/physics_2d/broad_phase_2d_basic.h +++ b/servers/physics_2d/broad_phase_2d_basic.h @@ -34,9 +34,7 @@ #include "core/map.h" #include "space_2d_sw.h" class BroadPhase2DBasic : public BroadPhase2DSW { - struct Element { - CollisionObject2DSW *owner; bool _static; Rect2 aabb; @@ -48,7 +46,6 @@ class BroadPhase2DBasic : public BroadPhase2DSW { ID current; struct PairKey { - union { struct { ID a; diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index 2cb021258a..b78dcf2866 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -34,13 +34,11 @@ #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; @@ -50,7 +48,6 @@ void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) { } 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.. @@ -58,7 +55,6 @@ void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) { E->get()->rc--; if (E->get()->rc == 0) { - if (E->get()->colliding) { //uncollide if (unpair_callback) { @@ -73,20 +69,15 @@ void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) { } void BroadPhase2DHashGrid::_check_motion(Element *p_elem) { - for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) { - bool pairing = p_elem->aabb.intersects(E->key()->aabb); if (pairing != E->get()->colliding) { - if (pairing) { - if (pair_callback) { E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata); } } else { - if (unpair_callback) { unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata); } @@ -98,7 +89,6 @@ void BroadPhase2DHashGrid::_check_motion(Element *p_elem) { } 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 @@ -121,9 +111,7 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo 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; @@ -132,7 +120,6 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo PosBin *pb = hash_table[idx]; while (pb) { - if (pb->key == pk) { break; } @@ -156,24 +143,19 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo } } 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()); @@ -186,7 +168,6 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo //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) @@ -199,10 +180,8 @@ void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, boo } 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) { @@ -221,9 +200,7 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool 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; @@ -232,7 +209,6 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool PosBin *pb = hash_table[idx]; while (pb) { - if (pb->key == pk) { break; } @@ -246,31 +222,25 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool 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()); @@ -279,15 +249,12 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool } 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; @@ -318,7 +285,6 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool } BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) { - current++; Element e; @@ -333,7 +299,6 @@ BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_obj } void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) { - Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); @@ -343,12 +308,10 @@ void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) { return; if (p_aabb != Rect2()) { - _enter_grid(&e, p_aabb, e._static); } if (e.aabb != Rect2()) { - _exit_grid(&e, e.aabb, e._static); } @@ -359,7 +322,6 @@ void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) { e.aabb = p_aabb; } void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { - Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); @@ -379,7 +341,6 @@ void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { } } void BroadPhase2DHashGrid::remove(ID p_id) { - Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); @@ -392,19 +353,16 @@ void BroadPhase2DHashGrid::remove(ID 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; @@ -412,7 +370,6 @@ int BroadPhase2DHashGrid::get_subindex(ID p_id) const { 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; @@ -421,7 +378,6 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, cons PosBin *pb = hash_table[idx]; while (pb) { - if (pb->key == pk) { break; } @@ -433,7 +389,6 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, cons 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) @@ -453,7 +408,6 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, cons } 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) @@ -474,7 +428,6 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, cons } 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); @@ -515,13 +468,10 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_t 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; } @@ -530,7 +480,6 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_t if (pos.x >= end.x) reached_x = true; } else if (pos.x <= end.x) { - reached_x = true; } @@ -538,7 +487,6 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_t if (pos.y >= end.y) reached_y = true; } else if (pos.y <= end.y) { - reached_y = true; } @@ -549,7 +497,6 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_t } for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (cullcount >= p_max_results) break; if (E->key()->pass == pass) @@ -574,7 +521,6 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_t } 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(); @@ -582,15 +528,12 @@ int BroadPhase2DHashGrid::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p 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) @@ -614,12 +557,10 @@ int BroadPhase2DHashGrid::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p } 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; } @@ -628,12 +569,10 @@ 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); @@ -653,7 +592,6 @@ BroadPhase2DHashGrid::BroadPhase2DHashGrid() { } BroadPhase2DHashGrid::~BroadPhase2DHashGrid() { - for (uint32_t i = 0; i < hash_table_size; i++) { while (hash_table[i]) { PosBin *pb = hash_table[i]; diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h index dc29d0c619..de1ada0932 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ b/servers/physics_2d/broad_phase_2d_hash_grid.h @@ -35,9 +35,7 @@ #include "core/map.h" class BroadPhase2DHashGrid : public BroadPhase2DSW { - struct PairData { - bool colliding; int rc; void *ud; @@ -49,7 +47,6 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { }; struct Element { - ID self; CollisionObject2DSW *owner; bool _static; @@ -60,7 +57,6 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { }; struct RC { - int ref; _FORCE_INLINE_ int inc() { @@ -85,7 +81,6 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { uint64_t pass; struct PairKey { - union { struct { ID a; @@ -126,7 +121,6 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { _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; @@ -153,7 +147,6 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW { }; struct PosBin { - PosKey key; Map<Element *, RC> object_set; Map<Element *, RC> static_object_set; diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h index 5e42c72d83..e4444cd180 100644 --- a/servers/physics_2d/broad_phase_2d_sw.h +++ b/servers/physics_2d/broad_phase_2d_sw.h @@ -37,7 +37,6 @@ class CollisionObject2DSW; class BroadPhase2DSW { - public: typedef BroadPhase2DSW *(*CreateFunction)(); diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index 0ec293c042..696100ccc3 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -33,7 +33,6 @@ #include "space_2d_sw.h" void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_transform, bool p_disabled) { - Shape s; s.shape = p_shape; s.xform = p_transform; @@ -53,7 +52,6 @@ void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_tra } void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) { - ERR_FAIL_INDEX(p_index, shapes.size()); shapes[p_index].shape->remove_owner(this); shapes.write[p_index].shape = p_shape; @@ -68,13 +66,11 @@ void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) { } void CollisionObject2DSW::set_shape_metadata(int p_index, const Variant &p_metadata) { - ERR_FAIL_INDEX(p_index, shapes.size()); shapes.write[p_index].metadata = p_metadata; } void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_transform) { - ERR_FAIL_INDEX(p_index, shapes.size()); shapes.write[p_index].xform = p_transform; @@ -115,10 +111,8 @@ void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) { } void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) { - //remove a shape, all the times it appears for (int i = 0; i < shapes.size(); i++) { - if (shapes[i].shape == p_shape) { remove_shape(i); i--; @@ -127,11 +121,9 @@ void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) { } void CollisionObject2DSW::remove_shape(int p_index) { - //remove anything from shape to be erased to end, so subindices don't change ERR_FAIL_INDEX(p_index, shapes.size()); for (int i = p_index; i < shapes.size(); i++) { - if (shapes[i].bpid == 0) continue; //should never get here with a null owner @@ -164,9 +156,7 @@ void CollisionObject2DSW::_set_static(bool p_static) { } void CollisionObject2DSW::_unregister_shapes() { - for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; if (s.bpid > 0) { space->get_broadphase()->remove(s.bpid); @@ -176,12 +166,10 @@ void CollisionObject2DSW::_unregister_shapes() { } void CollisionObject2DSW::_update_shapes() { - if (!space) return; for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; if (s.disabled) @@ -204,12 +192,10 @@ void CollisionObject2DSW::_update_shapes() { } void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { - if (!space) return; for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; if (s.disabled) continue; @@ -231,13 +217,10 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { } void CollisionObject2DSW::_set_space(Space2DSW *p_space) { - if (space) { - space->remove_object(this); for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; if (s.bpid) { space->get_broadphase()->remove(s.bpid); @@ -249,21 +232,18 @@ void CollisionObject2DSW::_set_space(Space2DSW *p_space) { space = p_space; if (space) { - space->add_object(this); _update_shapes(); } } void CollisionObject2DSW::_shape_changed() { - _update_shapes(); _shapes_changed(); } CollisionObject2DSW::CollisionObject2DSW(Type p_type) : pending_shape_update_list(this) { - _static = true; type = p_type; space = nullptr; diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 98105a7c0e..84a2baaa74 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -53,7 +53,6 @@ private: bool pickable; struct Shape { - Transform2D xform; Transform2D xform_inv; BroadPhase2DSW::ID bpid; @@ -186,7 +185,6 @@ public: _FORCE_INLINE_ bool is_pickable() const { return pickable; } _FORCE_INLINE_ bool test_collision_mask(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 a954cb3de3..58cba28fe1 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -33,7 +33,6 @@ #include "core/math/geometry.h" struct _CollectorCallback2D { - CollisionSolver2DSW::CallbackResult callback; void *userdata; bool swap; @@ -42,7 +41,6 @@ struct _CollectorCallback2D { Vector2 *sep_axis; _FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) { - /* if (normal.dot(p_point_A) >= normal.dot(p_point_B)) return; @@ -57,7 +55,6 @@ struct _CollectorCallback2D { typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *); _FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 1); ERR_FAIL_COND(p_point_count_B != 1); @@ -67,7 +64,6 @@ _FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_point } _FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 1); ERR_FAIL_COND(p_point_count_B != 2); @@ -85,7 +81,6 @@ struct _generate_contacts_Pair { }; _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 2); ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix @@ -115,7 +110,6 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_ sa.sort(dvec, 4); for (int i = 1; i <= 2; i++) { - if (dvec[i].a) { Vector2 a = p_points_A[dvec[i].idx]; Vector2 b = n.plane_project(dB, a); @@ -133,7 +127,6 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_ } static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 1); ERR_FAIL_COND(p_point_count_B < 1); @@ -165,7 +158,6 @@ static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_po points_A = p_points_B; points_B = p_points_A; } else { - pointcount_B = p_point_count_B; pointcount_A = p_point_count_A; points_A = p_points_A; @@ -182,7 +174,6 @@ static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_po template <class ShapeA, class ShapeB, bool castA = false, bool castB = false, bool withMargin = false> class SeparatorAxisTest2D { - const ShapeA *shape_A; const ShapeB *shape_B; const Transform2D *transform_A; @@ -199,7 +190,6 @@ class SeparatorAxisTest2D { public: _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) { return test_axis(*callback->sep_axis); } else { @@ -211,9 +201,7 @@ public: } _FORCE_INLINE_ bool test_cast() { - if (castA) { - Vector2 na = motion_A.normalized(); if (!test_axis(na)) return false; @@ -222,7 +210,6 @@ public: } if (castB) { - Vector2 nb = motion_B.normalized(); if (!test_axis(nb)) return false; @@ -234,7 +221,6 @@ public: } _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { - Vector2 axis = p_axis; if (Math::is_zero_approx(axis.x) && @@ -308,7 +294,6 @@ public: } _FORCE_INLINE_ void generate_contacts() { - // nothing to do, don't generate if (best_axis == Vector2(0.0, 0.0)) return; @@ -333,7 +318,6 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_A; i++) { supports_A[i] += -best_axis * margin_A; } @@ -351,7 +335,6 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_B; i++) { supports_B[i] += best_axis * margin_B; } @@ -366,7 +349,6 @@ public: } _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) { - margin_A = p_margin_A; margin_B = p_margin_B; best_depth = 1e15; @@ -397,7 +379,6 @@ typedef void (*CollisionFunc)(const Shape2DSW *, const Transform2D &, const Shap template <bool castA, bool castB, bool withMargin> static void _collision_segment_segment(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW *>(p_b); @@ -433,7 +414,6 @@ static void _collision_segment_segment(const Shape2DSW *p_a, const Transform2D & template <bool castA, bool castB, bool withMargin> static void _collision_segment_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b); @@ -462,7 +442,6 @@ static void _collision_segment_circle(const Shape2DSW *p_a, const Transform2D &p template <bool castA, bool castB, bool withMargin> static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); @@ -484,7 +463,6 @@ static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D return; if (withMargin) { - Transform2D inv = p_transform_b.affine_inverse(); Vector2 a = p_transform_a.xform(segment_A->get_a()); @@ -496,7 +474,6 @@ static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D return; if (castA) { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) return; if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) @@ -504,7 +481,6 @@ static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D } if (castB) { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) return; if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) @@ -512,7 +488,6 @@ static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D } if (castA && castB) { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) return; if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) @@ -525,7 +500,6 @@ static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D template <bool castA, bool castB, bool withMargin> static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); @@ -557,7 +531,6 @@ static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D & template <bool castA, bool castB, bool withMargin> static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); @@ -573,12 +546,10 @@ static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Transf return; for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; if (withMargin) { - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) return; if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) @@ -593,7 +564,6 @@ static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Transf template <bool castA, bool castB, bool withMargin> static void _collision_circle_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b); @@ -613,7 +583,6 @@ static void _collision_circle_circle(const Shape2DSW *p_a, const Transform2D &p_ template <bool castA, bool castB, bool withMargin> static void _collision_circle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); @@ -637,27 +606,23 @@ static void _collision_circle_rectangle(const Shape2DSW *p_a, const Transform2D Transform2D binv = p_transform_b.affine_inverse(); { - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) return; } if (castA) { - Vector2 sphereofs = sphere + p_motion_a; if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) return; } if (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) { - Vector2 sphereofs = sphere - p_motion_b + p_motion_a; if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) return; @@ -668,7 +633,6 @@ static void _collision_circle_rectangle(const Shape2DSW *p_a, const Transform2D template <bool castA, bool castB, bool withMargin> static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); @@ -695,7 +659,6 @@ static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p template <bool castA, bool castB, bool withMargin> static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); @@ -709,7 +672,6 @@ static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Transfo //poly faces and poly points vs circle for (int i = 0; i < convex_B->get_point_count(); i++) { - if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) return; @@ -724,7 +686,6 @@ static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Transfo template <bool castA, bool castB, bool withMargin> static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); @@ -751,7 +712,6 @@ static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform return; if (withMargin) { - Transform2D invA = p_transform_a.affine_inverse(); Transform2D invB = p_transform_b.affine_inverse(); @@ -759,7 +719,6 @@ static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform return; if (castA || castB) { - Transform2D aofs = p_transform_a; aofs.elements[2] += p_motion_a; @@ -770,19 +729,16 @@ static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform Transform2D bofsinv = bofs.affine_inverse(); if (castA) { - if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) return; } if (castB) { - if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) return; } if (castA && castB) { - if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) return; } @@ -794,7 +750,6 @@ static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform template <bool castA, bool castB, bool withMargin> static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); @@ -822,7 +777,6 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D Transform2D boxinv = p_transform_a.affine_inverse(); 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); @@ -861,7 +815,6 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D template <bool castA, bool castB, bool withMargin> static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); @@ -886,7 +839,6 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Tran boxinv = p_transform_a.affine_inverse(); } for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; @@ -895,17 +847,14 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Tran 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 (!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 (!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 (!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; } @@ -919,7 +868,6 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Tran template <bool castA, bool castB, bool withMargin> static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a); const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); @@ -942,11 +890,9 @@ static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D & //capsule endpoints 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); 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); if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) @@ -959,7 +905,6 @@ static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D & template <bool castA, bool castB, bool withMargin> static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); @@ -978,11 +923,9 @@ static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transf //poly vs capsule for (int i = 0; i < convex_B->get_point_count(); i++) { - Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); 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); if (TEST_POINT(capsule_endpoint_A, cpoint)) @@ -1000,7 +943,6 @@ static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transf template <bool castA, bool castB, bool withMargin> static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { - const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW *>(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); @@ -1013,22 +955,18 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const return; for (int i = 0; i < convex_A->get_point_count(); i++) { - if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) return; } for (int i = 0; i < convex_B->get_point_count(); i++) { - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) return; } if (withMargin) { - for (int i = 0; i < convex_A->get_point_count(); i++) { for (int j = 0; j < convex_B->get_point_count(); j++) { - if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) return; } @@ -1041,7 +979,6 @@ 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); @@ -1323,7 +1260,6 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2]; } } else { - if (*motion_A == Vector2() && *motion_B == Vector2()) { collision_func = collision_table[type_A - 2][type_B - 2]; } else if (*motion_A != Vector2() && *motion_B == Vector2()) { diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index f117dcbfe5..636b288358 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -35,7 +35,6 @@ //#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) return false; @@ -52,7 +51,6 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Tr bool found = false; for (int i = 0; i < support_count; i++) { - supports[i] = p_transform_B.xform(supports[i]); real_t pd = n.dot(supports[i]); if (pd >= d) @@ -73,7 +71,6 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Tr } 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) return false; @@ -93,7 +90,6 @@ 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(); return false; @@ -115,7 +111,6 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector } struct _ConcaveCollisionInfo2D { - const Transform2D *transform_A; const Shape2DSW *shape_A; const Transform2D *transform_B; @@ -133,7 +128,6 @@ struct _ConcaveCollisionInfo2D { }; void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { - _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata); cinfo.aabb_tests++; if (!cinfo.result_callback && cinfo.collided) @@ -148,7 +142,6 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex } 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) { - const ConcaveShape2DSW *concave_B = static_cast<const ConcaveShape2DSW *>(p_shape_B); _ConcaveCollisionInfo2D cinfo; @@ -174,7 +167,6 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transf Rect2 local_aabb; for (int i = 0; i < 2; i++) { - Vector2 axis(p_transform_B.elements[i]); real_t axis_scale = 1.0 / axis.length(); axis *= axis_scale; @@ -194,7 +186,6 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transf } 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) { - 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(); @@ -211,7 +202,6 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p } if (type_A == PhysicsServer2D::SHAPE_LINE) { - if (type_B == PhysicsServer2D::SHAPE_LINE || type_B == PhysicsServer2D::SHAPE_RAY) { return false; } @@ -223,9 +213,7 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p } } else if (type_A == PhysicsServer2D::SHAPE_RAY) { - if (type_B == PhysicsServer2D::SHAPE_RAY) { - return false; //no ray-ray } @@ -236,7 +224,6 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p } } else if (concave_B) { - if (concave_A) return false; @@ -246,7 +233,6 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p 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); } 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); } } diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index f8eb16214f..d8751f588e 100644 --- a/servers/physics_2d/constraint_2d_sw.h +++ b/servers/physics_2d/constraint_2d_sw.h @@ -34,7 +34,6 @@ #include "body_2d_sw.h" class Constraint2DSW { - Body2DSW **_body_ptr; int _body_count; uint64_t island_step; diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index 4524629d50..73d916138b 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -56,7 +56,6 @@ */ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) { - real_t value = 0; { @@ -66,7 +65,6 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const } if (b) { - value += b->get_inv_mass(); real_t rcn = rB.cross(n); value += b->get_inv_inertia() * rcn * rcn; @@ -90,7 +88,6 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto } bool PinJoint2DSW::setup(real_t p_step) { - Space2DSW *space = A->get_space(); ERR_FAIL_COND_V(!space, false); rA = A->get_transform().basis_xform(anchor_A); @@ -115,7 +112,6 @@ bool PinJoint2DSW::setup(real_t p_step) { K[1] = K1[1] + K2[1]; if (B) { - Transform2D K3; K3[0].x = B->get_inv_inertia() * rB.y * rB.y; K3[1].x = -B->get_inv_inertia() * rB.x * rB.y; @@ -147,12 +143,10 @@ bool PinJoint2DSW::setup(real_t p_step) { } inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) { - return Vector2(p_other * p_vec.y, -p_other * p_vec.x); } void PinJoint2DSW::solve(real_t p_step) { - // compute relative velocity Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity()); @@ -172,13 +166,11 @@ void PinJoint2DSW::solve(real_t p_step) { } void PinJoint2DSW::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { - if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) softness = p_value; } real_t PinJoint2DSW::get_param(PhysicsServer2D::PinJointParam p_param) const { - if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) return softness; ERR_FAIL_V(0); @@ -186,7 +178,6 @@ real_t PinJoint2DSW::get_param(PhysicsServer2D::PinJointParam p_param) const { PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b) : Joint2DSW(_arr, p_body_b ? 2 : 1) { - A = p_body_a; B = p_body_b; anchor_A = p_body_a->get_inv_transform().xform(p_pos); @@ -200,7 +191,6 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p } PinJoint2DSW::~PinJoint2DSW() { - if (A) A->remove_constraint(this); if (B) @@ -259,7 +249,6 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { } bool GrooveJoint2DSW::setup(real_t p_step) { - // calculate endpoints in worldspace Vector2 ta = A->get_transform().xform(A_groove_1); Vector2 tb = A->get_transform().xform(A_groove_2); @@ -311,7 +300,6 @@ bool GrooveJoint2DSW::setup(real_t p_step) { } void GrooveJoint2DSW::solve(real_t p_step) { - // compute impulse Vector2 vr = relative_velocity(A, B, rA, rB); @@ -329,7 +317,6 @@ void GrooveJoint2DSW::solve(real_t p_step) { 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) : Joint2DSW(_arr, 2) { - A = p_body_a; B = p_body_b; @@ -343,7 +330,6 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_ } GrooveJoint2DSW::~GrooveJoint2DSW() { - A->remove_constraint(this); B->remove_constraint(this); } @@ -353,7 +339,6 @@ GrooveJoint2DSW::~GrooveJoint2DSW() { ////////////////////////////////////////////// bool DampedSpringJoint2DSW::setup(real_t p_step) { - rA = A->get_transform().basis_xform(anchor_A); rB = B->get_transform().basis_xform(anchor_B); @@ -382,7 +367,6 @@ bool DampedSpringJoint2DSW::setup(real_t p_step) { } void DampedSpringJoint2DSW::solve(real_t p_step) { - // compute relative velocity real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn; @@ -397,38 +381,28 @@ void DampedSpringJoint2DSW::solve(real_t p_step) { } void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedStringParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer2D::DAMPED_STRING_REST_LENGTH: { - rest_length = p_value; } break; case PhysicsServer2D::DAMPED_STRING_DAMPING: { - damping = p_value; } break; case PhysicsServer2D::DAMPED_STRING_STIFFNESS: { - stiffness = p_value; } break; } } real_t DampedSpringJoint2DSW::get_param(PhysicsServer2D::DampedStringParam p_param) const { - switch (p_param) { - case PhysicsServer2D::DAMPED_STRING_REST_LENGTH: { - return rest_length; } break; case PhysicsServer2D::DAMPED_STRING_DAMPING: { - return damping; } break; case PhysicsServer2D::DAMPED_STRING_STIFFNESS: { - return stiffness; } break; } @@ -438,7 +412,6 @@ real_t DampedSpringJoint2DSW::get_param(PhysicsServer2D::DampedStringParam p_par DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b) : Joint2DSW(_arr, 2) { - A = p_body_a; B = p_body_b; anchor_A = A->get_inv_transform().xform(p_anchor_a); @@ -453,7 +426,6 @@ DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Ve } 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 a0d25dc70d..87556ccea1 100644 --- a/servers/physics_2d/joints_2d_sw.h +++ b/servers/physics_2d/joints_2d_sw.h @@ -35,7 +35,6 @@ #include "constraint_2d_sw.h" class Joint2DSW : public Constraint2DSW { - real_t max_force; real_t bias; real_t max_bias; @@ -59,7 +58,6 @@ public: }; class PinJoint2DSW : public Joint2DSW { - union { struct { Body2DSW *A; @@ -91,7 +89,6 @@ public: }; class GrooveJoint2DSW : public Joint2DSW { - union { struct { Body2DSW *A; @@ -126,7 +123,6 @@ public: }; class DampedSpringJoint2DSW : public Joint2DSW { - union { struct { Body2DSW *A; diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp index bc3b468cea..645ee5ff45 100644 --- a/servers/physics_2d/physics_server_2d_sw.cpp +++ b/servers/physics_2d/physics_server_2d_sw.cpp @@ -41,44 +41,33 @@ 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."); RID PhysicsServer2DSW::_shape_create(ShapeType p_shape) { - Shape2DSW *shape = nullptr; switch (p_shape) { - case SHAPE_LINE: { - shape = memnew(LineShape2DSW); } break; case SHAPE_RAY: { - shape = memnew(RayShape2DSW); } break; case SHAPE_SEGMENT: { - shape = memnew(SegmentShape2DSW); } break; case SHAPE_CIRCLE: { - shape = memnew(CircleShape2DSW); } break; case SHAPE_RECTANGLE: { - shape = memnew(RectangleShape2DSW); } break; case SHAPE_CAPSULE: { - shape = memnew(CapsuleShape2DSW); } break; case SHAPE_CONVEX_POLYGON: { - shape = memnew(ConvexPolygonShape2DSW); } break; case SHAPE_CONCAVE_POLYGON: { - shape = memnew(ConcavePolygonShape2DSW); } break; case SHAPE_CUSTOM: { - ERR_FAIL_V(RID()); } break; @@ -91,63 +80,51 @@ RID PhysicsServer2DSW::_shape_create(ShapeType p_shape) { } RID PhysicsServer2DSW::line_shape_create() { - return _shape_create(SHAPE_LINE); } RID PhysicsServer2DSW::ray_shape_create() { - return _shape_create(SHAPE_RAY); } RID PhysicsServer2DSW::segment_shape_create() { - return _shape_create(SHAPE_SEGMENT); } RID PhysicsServer2DSW::circle_shape_create() { - return _shape_create(SHAPE_CIRCLE); } RID PhysicsServer2DSW::rectangle_shape_create() { - return _shape_create(SHAPE_RECTANGLE); } RID PhysicsServer2DSW::capsule_shape_create() { - return _shape_create(SHAPE_CAPSULE); } RID PhysicsServer2DSW::convex_polygon_shape_create() { - return _shape_create(SHAPE_CONVEX_POLYGON); } RID PhysicsServer2DSW::concave_polygon_shape_create() { - return _shape_create(SHAPE_CONCAVE_POLYGON); } void PhysicsServer2DSW::shape_set_data(RID p_shape, const Variant &p_data) { - Shape2DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND(!shape); shape->set_data(p_data); }; void PhysicsServer2DSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { - Shape2DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND(!shape); shape->set_custom_bias(p_bias); } PhysicsServer2D::ShapeType PhysicsServer2DSW::shape_get_type(RID p_shape) const { - const Shape2DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); return shape->get_type(); }; Variant PhysicsServer2DSW::shape_get_data(RID p_shape) const { - const Shape2DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, Variant()); ERR_FAIL_COND_V(!shape->is_configured(), Variant()); @@ -155,14 +132,12 @@ Variant PhysicsServer2DSW::shape_get_data(RID p_shape) const { }; real_t PhysicsServer2DSW::shape_get_custom_solver_bias(RID p_shape) const { - const Shape2DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); return shape->get_custom_bias(); } void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { - CollCbkData *cbk = (CollCbkData *)p_userdata; if (cbk->max == 0) @@ -194,7 +169,6 @@ void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 & real_t min_depth = 1e20; int min_depth_idx = 0; for (int i = 0; i < cbk->amount; i++) { - real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); if (d < min_depth) { min_depth = d; @@ -210,7 +184,6 @@ void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 & cbk->passed++; } else { - cbk->ptr[cbk->amount * 2 + 0] = p_point_A; cbk->ptr[cbk->amount * 2 + 1] = p_point_B; cbk->amount++; @@ -219,14 +192,12 @@ void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 & } bool PhysicsServer2DSW::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) { - Shape2DSW *shape_A = shape_owner.getornull(p_shape_A); ERR_FAIL_COND_V(!shape_A, false); Shape2DSW *shape_B = shape_owner.getornull(p_shape_B); ERR_FAIL_COND_V(!shape_B, false); if (p_result_max == 0) { - return CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, nullptr, nullptr); } @@ -242,7 +213,6 @@ bool PhysicsServer2DSW::shape_collide(RID p_shape_A, const Transform2D &p_xform_ } RID PhysicsServer2DSW::space_create() { - Space2DSW *space = memnew(Space2DSW); RID id = space_owner.make_rid(space); space->set_self(id); @@ -257,7 +227,6 @@ RID PhysicsServer2DSW::space_create() { }; void PhysicsServer2DSW::space_set_active(RID p_space, bool p_active) { - Space2DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND(!space); if (p_active) @@ -267,7 +236,6 @@ void PhysicsServer2DSW::space_set_active(RID p_space, bool p_active) { } bool PhysicsServer2DSW::space_is_active(RID p_space) const { - const Space2DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, false); @@ -275,7 +243,6 @@ bool PhysicsServer2DSW::space_is_active(RID p_space) const { } void PhysicsServer2DSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { - Space2DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND(!space); @@ -283,35 +250,30 @@ void PhysicsServer2DSW::space_set_param(RID p_space, SpaceParameter p_param, rea } real_t PhysicsServer2DSW::space_get_param(RID p_space, SpaceParameter p_param) const { - const Space2DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, 0); return space->get_param(p_param); } void PhysicsServer2DSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { - Space2DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND(!space); space->set_debug_contacts(p_max_contacts); } Vector<Vector2> PhysicsServer2DSW::space_get_contacts(RID p_space) const { - Space2DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, Vector<Vector2>()); return space->get_debug_contacts(); } int PhysicsServer2DSW::space_get_contact_count(RID p_space) const { - Space2DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, 0); return space->get_debug_contact_count(); } PhysicsDirectSpaceState2D *PhysicsServer2DSW::space_get_direct_state(RID p_space) { - Space2DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, nullptr); ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); @@ -320,7 +282,6 @@ PhysicsDirectSpaceState2D *PhysicsServer2DSW::space_get_direct_state(RID p_space } RID PhysicsServer2DSW::area_create() { - Area2DSW *area = memnew(Area2DSW); RID rid = area_owner.make_rid(area); area->set_self(rid); @@ -328,7 +289,6 @@ RID PhysicsServer2DSW::area_create() { }; void PhysicsServer2DSW::area_set_space(RID p_area, RID p_space) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -346,7 +306,6 @@ void PhysicsServer2DSW::area_set_space(RID p_area, RID p_space) { }; RID PhysicsServer2DSW::area_get_space(RID p_area) const { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, RID()); @@ -357,7 +316,6 @@ RID PhysicsServer2DSW::area_get_space(RID p_area) const { }; void PhysicsServer2DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -365,7 +323,6 @@ void PhysicsServer2DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverri } PhysicsServer2D::AreaSpaceOverrideMode PhysicsServer2DSW::area_get_space_override_mode(RID p_area) const { - const Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); @@ -373,7 +330,6 @@ PhysicsServer2D::AreaSpaceOverrideMode PhysicsServer2DSW::area_get_space_overrid } void PhysicsServer2DSW::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform, bool p_disabled) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -384,7 +340,6 @@ void PhysicsServer2DSW::area_add_shape(RID p_area, RID p_shape, const Transform2 } void PhysicsServer2DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -395,7 +350,6 @@ void PhysicsServer2DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) area->set_shape(p_shape_idx, shape); } void PhysicsServer2DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -403,7 +357,6 @@ void PhysicsServer2DSW::area_set_shape_transform(RID p_area, int p_shape_idx, co } void PhysicsServer2DSW::area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); ERR_FAIL_INDEX(p_shape, area->get_shape_count()); @@ -413,14 +366,12 @@ void PhysicsServer2DSW::area_set_shape_disabled(RID p_area, int p_shape, bool p_ } int PhysicsServer2DSW::area_get_shape_count(RID p_area) const { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, -1); return area->get_shape_count(); } RID PhysicsServer2DSW::area_get_shape(RID p_area, int p_shape_idx) const { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, RID()); @@ -430,7 +381,6 @@ RID PhysicsServer2DSW::area_get_shape(RID p_area, int p_shape_idx) const { return shape->get_self(); } Transform2D PhysicsServer2DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, Transform2D()); @@ -438,7 +388,6 @@ Transform2D PhysicsServer2DSW::area_get_shape_transform(RID p_area, int p_shape_ } void PhysicsServer2DSW::area_remove_shape(RID p_area, int p_shape_idx) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -446,7 +395,6 @@ void PhysicsServer2DSW::area_remove_shape(RID p_area, int p_shape_idx) { } void PhysicsServer2DSW::area_clear_shapes(RID p_area) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -455,7 +403,6 @@ void PhysicsServer2DSW::area_clear_shapes(RID p_area) { } void PhysicsServer2DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) { - if (space_owner.owns(p_area)) { Space2DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -465,7 +412,6 @@ void PhysicsServer2DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id area->set_instance_id(p_id); } ObjectID PhysicsServer2DSW::area_get_object_instance_id(RID p_area) const { - if (space_owner.owns(p_area)) { Space2DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -476,7 +422,6 @@ ObjectID PhysicsServer2DSW::area_get_object_instance_id(RID p_area) const { } void PhysicsServer2DSW::area_attach_canvas_instance_id(RID p_area, ObjectID p_id) { - if (space_owner.owns(p_area)) { Space2DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -486,7 +431,6 @@ void PhysicsServer2DSW::area_attach_canvas_instance_id(RID p_area, ObjectID p_id area->set_canvas_instance_id(p_id); } ObjectID PhysicsServer2DSW::area_get_canvas_instance_id(RID p_area) const { - if (space_owner.owns(p_area)) { Space2DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -497,7 +441,6 @@ ObjectID PhysicsServer2DSW::area_get_canvas_instance_id(RID p_area) const { } void PhysicsServer2DSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { - if (space_owner.owns(p_area)) { Space2DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -508,14 +451,12 @@ void PhysicsServer2DSW::area_set_param(RID p_area, AreaParameter p_param, const }; void PhysicsServer2DSW::area_set_transform(RID p_area, const Transform2D &p_transform) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); area->set_transform(p_transform); }; Variant PhysicsServer2DSW::area_get_param(RID p_area, AreaParameter p_param) const { - if (space_owner.owns(p_area)) { Space2DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -527,7 +468,6 @@ Variant PhysicsServer2DSW::area_get_param(RID p_area, AreaParameter p_param) con }; Transform2D PhysicsServer2DSW::area_get_transform(RID p_area) const { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, Transform2D()); @@ -535,14 +475,12 @@ Transform2D PhysicsServer2DSW::area_get_transform(RID p_area) const { }; void PhysicsServer2DSW::area_set_pickable(RID p_area, bool p_pickable) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); area->set_pickable(p_pickable); } void PhysicsServer2DSW::area_set_monitorable(RID p_area, bool p_monitorable) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); FLUSH_QUERY_CHECK(area); @@ -551,7 +489,6 @@ void PhysicsServer2DSW::area_set_monitorable(RID p_area, bool p_monitorable) { } void PhysicsServer2DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -559,7 +496,6 @@ void PhysicsServer2DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { } void PhysicsServer2DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -567,7 +503,6 @@ void PhysicsServer2DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { } void PhysicsServer2DSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -575,7 +510,6 @@ void PhysicsServer2DSW::area_set_monitor_callback(RID p_area, Object *p_receiver } void PhysicsServer2DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -585,7 +519,6 @@ void PhysicsServer2DSW::area_set_area_monitor_callback(RID p_area, Object *p_rec /* BODY API */ RID PhysicsServer2DSW::body_create() { - Body2DSW *body = memnew(Body2DSW); RID rid = body_owner.make_rid(body); body->set_self(rid); @@ -593,7 +526,6 @@ RID PhysicsServer2DSW::body_create() { } void PhysicsServer2DSW::body_set_space(RID p_body, RID p_space) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); Space2DSW *space = nullptr; @@ -610,7 +542,6 @@ void PhysicsServer2DSW::body_set_space(RID p_body, RID p_space) { }; RID PhysicsServer2DSW::body_get_space(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, RID()); @@ -621,7 +552,6 @@ RID PhysicsServer2DSW::body_get_space(RID p_body) const { }; void PhysicsServer2DSW::body_set_mode(RID p_body, BodyMode p_mode) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); FLUSH_QUERY_CHECK(body); @@ -630,7 +560,6 @@ void PhysicsServer2DSW::body_set_mode(RID p_body, BodyMode p_mode) { }; PhysicsServer2D::BodyMode PhysicsServer2DSW::body_get_mode(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); @@ -638,7 +567,6 @@ PhysicsServer2D::BodyMode PhysicsServer2DSW::body_get_mode(RID p_body) const { }; void PhysicsServer2DSW::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform, bool p_disabled) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -649,7 +577,6 @@ void PhysicsServer2DSW::body_add_shape(RID p_body, RID p_shape, const Transform2 } void PhysicsServer2DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -660,7 +587,6 @@ void PhysicsServer2DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) body->set_shape(p_shape_idx, shape); } void PhysicsServer2DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -668,28 +594,24 @@ void PhysicsServer2DSW::body_set_shape_transform(RID p_body, int p_shape_idx, co } void PhysicsServer2DSW::body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant &p_metadata) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); body->set_shape_metadata(p_shape_idx, p_metadata); } Variant PhysicsServer2DSW::body_get_shape_metadata(RID p_body, int p_shape_idx) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, Variant()); return body->get_shape_metadata(p_shape_idx); } int PhysicsServer2DSW::body_get_shape_count(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, -1); return body->get_shape_count(); } RID PhysicsServer2DSW::body_get_shape(RID p_body, int p_shape_idx) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, RID()); @@ -699,7 +621,6 @@ RID PhysicsServer2DSW::body_get_shape(RID p_body, int p_shape_idx) const { return shape->get_self(); } Transform2D PhysicsServer2DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, Transform2D()); @@ -707,7 +628,6 @@ Transform2D PhysicsServer2DSW::body_get_shape_transform(RID p_body, int p_shape_ } void PhysicsServer2DSW::body_remove_shape(RID p_body, int p_shape_idx) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -715,7 +635,6 @@ void PhysicsServer2DSW::body_remove_shape(RID p_body, int p_shape_idx) { } void PhysicsServer2DSW::body_clear_shapes(RID p_body) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -724,7 +643,6 @@ void PhysicsServer2DSW::body_clear_shapes(RID p_body) { } void PhysicsServer2DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); @@ -733,7 +651,6 @@ void PhysicsServer2DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo body->set_shape_as_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) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); @@ -743,14 +660,12 @@ void PhysicsServer2DSW::body_set_shape_as_one_way_collision(RID p_body, int p_sh } void PhysicsServer2DSW::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); body->set_continuous_collision_detection_mode(p_mode); } PhysicsServer2DSW::CCDMode PhysicsServer2DSW::body_get_continuous_collision_detection_mode(RID p_body) const { - const Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, CCD_MODE_DISABLED); @@ -758,7 +673,6 @@ PhysicsServer2DSW::CCDMode PhysicsServer2DSW::body_get_continuous_collision_dete } void PhysicsServer2DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -766,7 +680,6 @@ void PhysicsServer2DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id }; ObjectID PhysicsServer2DSW::body_get_object_instance_id(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, ObjectID()); @@ -774,7 +687,6 @@ ObjectID PhysicsServer2DSW::body_get_object_instance_id(RID p_body) const { }; void PhysicsServer2DSW::body_attach_canvas_instance_id(RID p_body, ObjectID p_id) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -782,7 +694,6 @@ void PhysicsServer2DSW::body_attach_canvas_instance_id(RID p_body, ObjectID p_id }; ObjectID PhysicsServer2DSW::body_get_canvas_instance_id(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, ObjectID()); @@ -790,14 +701,12 @@ ObjectID PhysicsServer2DSW::body_get_canvas_instance_id(RID p_body) const { }; void PhysicsServer2DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); body->set_collision_layer(p_layer); }; uint32_t PhysicsServer2DSW::body_get_collision_layer(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); @@ -805,14 +714,12 @@ uint32_t PhysicsServer2DSW::body_get_collision_layer(RID p_body) const { }; void PhysicsServer2DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); body->set_collision_mask(p_mask); }; uint32_t PhysicsServer2DSW::body_get_collision_mask(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); @@ -820,7 +727,6 @@ uint32_t PhysicsServer2DSW::body_get_collision_mask(RID p_body) const { }; void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -828,7 +734,6 @@ void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, real_t }; real_t PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); @@ -836,7 +741,6 @@ real_t PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) cons }; void PhysicsServer2DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -844,7 +748,6 @@ void PhysicsServer2DSW::body_set_state(RID p_body, BodyState p_state, const Vari }; Variant PhysicsServer2DSW::body_get_state(RID p_body, BodyState p_state) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, Variant()); @@ -852,7 +755,6 @@ Variant PhysicsServer2DSW::body_get_state(RID p_body, BodyState p_state) const { }; void PhysicsServer2DSW::body_set_applied_force(RID p_body, const Vector2 &p_force) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -861,14 +763,12 @@ void PhysicsServer2DSW::body_set_applied_force(RID p_body, const Vector2 &p_forc }; Vector2 PhysicsServer2DSW::body_get_applied_force(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, Vector2()); return body->get_applied_force(); }; void PhysicsServer2DSW::body_set_applied_torque(RID p_body, real_t p_torque) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -877,7 +777,6 @@ void PhysicsServer2DSW::body_set_applied_torque(RID p_body, real_t p_torque) { }; real_t PhysicsServer2DSW::body_get_applied_torque(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); @@ -902,7 +801,6 @@ void PhysicsServer2DSW::body_apply_torque_impulse(RID p_body, real_t p_torque) { } void PhysicsServer2DSW::body_apply_impulse(RID p_body, const Vector2 &p_pos, const Vector2 &p_impulse) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -921,7 +819,6 @@ void PhysicsServer2DSW::body_add_central_force(RID p_body, const Vector2 &p_forc }; void PhysicsServer2DSW::body_add_force(RID p_body, const Vector2 &p_offset, const Vector2 &p_force) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -938,7 +835,6 @@ void PhysicsServer2DSW::body_add_torque(RID p_body, real_t p_torque) { }; void PhysicsServer2DSW::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -953,7 +849,6 @@ void PhysicsServer2DSW::body_set_axis_velocity(RID p_body, const Vector2 &p_axis }; void PhysicsServer2DSW::body_add_collision_exception(RID p_body, RID p_body_b) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -962,7 +857,6 @@ void PhysicsServer2DSW::body_add_collision_exception(RID p_body, RID p_body_b) { }; void PhysicsServer2DSW::body_remove_collision_exception(RID p_body, RID p_body_b) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -971,7 +865,6 @@ void PhysicsServer2DSW::body_remove_collision_exception(RID p_body, RID p_body_b }; void PhysicsServer2DSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -981,20 +874,17 @@ void PhysicsServer2DSW::body_get_collision_exceptions(RID p_body, List<RID> *p_e }; void PhysicsServer2DSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); }; real_t PhysicsServer2DSW::body_get_contacts_reported_depth_threshold(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); return 0; }; void PhysicsServer2DSW::body_set_omit_force_integration(RID p_body, bool p_omit) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -1002,35 +892,30 @@ void PhysicsServer2DSW::body_set_omit_force_integration(RID p_body, bool p_omit) }; bool PhysicsServer2DSW::body_is_omitting_force_integration(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); return body->get_omit_force_integration(); }; void PhysicsServer2DSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); body->set_max_contacts_reported(p_contacts); } int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, -1); 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) { - 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); } 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) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_INDEX_V(p_body_shape, body->get_shape_count(), false); @@ -1039,14 +924,12 @@ bool PhysicsServer2DSW::body_collide_shape(RID p_body, int p_body_shape, RID p_s } void PhysicsServer2DSW::body_set_pickable(RID p_body, bool p_pickable) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); 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) { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -1058,7 +941,6 @@ bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from, } 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); @@ -1068,7 +950,6 @@ int PhysicsServer2DSW::body_test_ray_separation(RID p_body, const Transform2D &p } PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) { - ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); if (!body_owner.owns(p_body)) @@ -1086,7 +967,6 @@ PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) { /* JOINT API */ 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); @@ -1104,7 +984,6 @@ void PhysicsServer2DSW::joint_set_param(RID p_joint, JointParam p_param, real_t } real_t PhysicsServer2DSW::joint_get_param(RID p_joint, JointParam p_param) const { - const Joint2DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, -1); @@ -1151,7 +1030,6 @@ bool PhysicsServer2DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) } RID PhysicsServer2DSW::pin_joint_create(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()); Body2DSW *B = nullptr; @@ -1168,7 +1046,6 @@ RID PhysicsServer2DSW::pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID } 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) { - Body2DSW *A = body_owner.getornull(p_body_a); ERR_FAIL_COND_V(!A, RID()); @@ -1182,7 +1059,6 @@ RID PhysicsServer2DSW::groove_joint_create(const Vector2 &p_a_groove1, const Vec } RID PhysicsServer2DSW::damped_spring_joint_create(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()); @@ -1196,7 +1072,6 @@ RID PhysicsServer2DSW::damped_spring_joint_create(const Vector2 &p_anchor_a, con } 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); @@ -1215,7 +1090,6 @@ real_t PhysicsServer2DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param } void PhysicsServer2DSW::damped_string_joint_set_param(RID p_joint, DampedStringParam 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); @@ -1225,7 +1099,6 @@ void PhysicsServer2DSW::damped_string_joint_set_param(RID p_joint, DampedStringP } real_t PhysicsServer2DSW::damped_string_joint_get_param(RID p_joint, DampedStringParam 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); @@ -1235,7 +1108,6 @@ real_t PhysicsServer2DSW::damped_string_joint_get_param(RID p_joint, DampedStrin } PhysicsServer2D::JointType PhysicsServer2DSW::joint_get_type(RID p_joint) const { - Joint2DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, JOINT_PIN); @@ -1243,11 +1115,9 @@ PhysicsServer2D::JointType PhysicsServer2DSW::joint_get_type(RID p_joint) const } void PhysicsServer2DSW::free(RID p_rid) { - _update_shapes(); // just in case if (shape_owner.owns(p_rid)) { - Shape2DSW *shape = shape_owner.getornull(p_rid); while (shape->get_owners().size()) { @@ -1258,7 +1128,6 @@ void PhysicsServer2DSW::free(RID p_rid) { shape_owner.free(p_rid); memdelete(shape); } else if (body_owner.owns(p_rid)) { - Body2DSW *body = body_owner.getornull(p_rid); /* @@ -1272,7 +1141,6 @@ void PhysicsServer2DSW::free(RID p_rid) { body_set_space(p_rid, RID()); while (body->get_shape_count()) { - body->remove_shape(0); } @@ -1280,7 +1148,6 @@ void PhysicsServer2DSW::free(RID p_rid) { memdelete(body); } else if (area_owner.owns(p_rid)) { - Area2DSW *area = area_owner.getornull(p_rid); /* @@ -1291,14 +1158,12 @@ void PhysicsServer2DSW::free(RID p_rid) { area->set_space(nullptr); while (area->get_shape_count()) { - area->remove_shape(0); } area_owner.free(p_rid); memdelete(area); } else if (space_owner.owns(p_rid)) { - Space2DSW *space = space_owner.getornull(p_rid); while (space->get_objects().size()) { @@ -1311,25 +1176,21 @@ void PhysicsServer2DSW::free(RID p_rid) { space_owner.free(p_rid); memdelete(space); } else if (joint_owner.owns(p_rid)) { - Joint2DSW *joint = joint_owner.getornull(p_rid); joint_owner.free(p_rid); memdelete(joint); } else { - ERR_FAIL_MSG("Invalid ID."); } }; void PhysicsServer2DSW::set_active(bool p_active) { - active = p_active; }; void PhysicsServer2DSW::init() { - doing_sync = false; last_step = 0.001; iterations = 8; // 8? @@ -1338,7 +1199,6 @@ void PhysicsServer2DSW::init() { }; void PhysicsServer2DSW::step(real_t p_step) { - if (!active) return; @@ -1352,7 +1212,6 @@ void PhysicsServer2DSW::step(real_t p_step) { active_objects = 0; collision_pairs = 0; for (Set<const Space2DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((Space2DSW *)E->get(), p_step, iterations); island_count += E->get()->get_island_count(); active_objects += E->get()->get_active_objects(); @@ -1361,12 +1220,10 @@ void PhysicsServer2DSW::step(real_t p_step) { }; void PhysicsServer2DSW::sync() { - doing_sync = true; }; void PhysicsServer2DSW::flush_queries() { - if (!active) return; @@ -1375,7 +1232,6 @@ void PhysicsServer2DSW::flush_queries() { uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); for (Set<const Space2DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - Space2DSW *space = (Space2DSW *)E->get(); space->call_queries(); } @@ -1383,7 +1239,6 @@ void PhysicsServer2DSW::flush_queries() { flushing_queries = false; if (EngineDebugger::is_profiling("servers")) { - uint64_t total_time[Space2DSW::ELAPSED_TIME_MAX]; static const char *time_name[Space2DSW::ELAPSED_TIME_MAX] = { "integrate_forces", @@ -1398,7 +1253,6 @@ void PhysicsServer2DSW::flush_queries() { } for (Set<const Space2DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) { total_time[i] += E->get()->get_elapsed_time(Space2DSW::ElapsedTime(i)); } @@ -1423,13 +1277,11 @@ void PhysicsServer2DSW::end_sync() { } void PhysicsServer2DSW::finish() { - memdelete(stepper); memdelete(direct_state); }; void PhysicsServer2DSW::_update_shapes() { - while (pending_shape_update_list.first()) { pending_shape_update_list.first()->self()->_shape_changed(); pending_shape_update_list.remove(pending_shape_update_list.first()); @@ -1437,18 +1289,14 @@ void PhysicsServer2DSW::_update_shapes() { } int PhysicsServer2DSW::get_process_info(ProcessInfo p_info) { - switch (p_info) { - case INFO_ACTIVE_OBJECTS: { - return active_objects; } break; case INFO_COLLISION_PAIRS: { return collision_pairs; } break; case INFO_ISLAND_COUNT: { - return island_count; } break; } @@ -1459,7 +1307,6 @@ int PhysicsServer2DSW::get_process_info(ProcessInfo p_info) { PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = nullptr; PhysicsServer2DSW::PhysicsServer2DSW() { - singletonsw = this; BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create; //BroadPhase2DSW::create_func=BroadPhase2DBasic::_create; diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h index 918958ffe2..d08f836fa9 100644 --- a/servers/physics_2d/physics_server_2d_sw.h +++ b/servers/physics_2d/physics_server_2d_sw.h @@ -39,7 +39,6 @@ #include "step_2d_sw.h" class PhysicsServer2DSW : public PhysicsServer2D { - GDCLASS(PhysicsServer2DSW, PhysicsServer2D); friend class PhysicsDirectSpaceState2DSW; @@ -79,7 +78,6 @@ class PhysicsServer2DSW : public PhysicsServer2D { public: struct CollCbkData { - Vector2 valid_dir; real_t valid_depth; int max; diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.cpp b/servers/physics_2d/physics_server_2d_wrap_mt.cpp index 0a89a76615..86d5755ec5 100644 --- a/servers/physics_2d/physics_server_2d_wrap_mt.cpp +++ b/servers/physics_2d/physics_server_2d_wrap_mt.cpp @@ -33,25 +33,21 @@ #include "core/os/os.h" void PhysicsServer2DWrapMT::thread_exit() { - exit = true; } void PhysicsServer2DWrapMT::thread_step(real_t p_delta) { - physics_2d_server->step(p_delta); step_sem.post(); } void PhysicsServer2DWrapMT::_thread_callback(void *_instance) { - PhysicsServer2DWrapMT *vsmt = reinterpret_cast<PhysicsServer2DWrapMT *>(_instance); vsmt->thread_loop(); } void PhysicsServer2DWrapMT::thread_loop() { - server_thread = Thread::get_caller_id(); physics_2d_server->init(); @@ -71,19 +67,15 @@ void PhysicsServer2DWrapMT::thread_loop() { /* EVENT QUEUING */ void PhysicsServer2DWrapMT::step(real_t p_step) { - if (create_thread) { - command_queue.push(this, &PhysicsServer2DWrapMT::thread_step, p_step); } else { - command_queue.flush_all(); //flush all pending from other threads physics_2d_server->step(p_step); } } void PhysicsServer2DWrapMT::sync() { - if (thread) { if (first_frame) first_frame = false; @@ -94,34 +86,27 @@ void PhysicsServer2DWrapMT::sync() { } void PhysicsServer2DWrapMT::flush_queries() { - physics_2d_server->flush_queries(); } void PhysicsServer2DWrapMT::end_sync() { - physics_2d_server->end_sync(); } void PhysicsServer2DWrapMT::init() { - if (create_thread) { - //OS::get_singleton()->release_rendering_thread(); thread = Thread::create(_thread_callback, this); while (!step_thread_up) { OS::get_singleton()->delay_usec(1000); } } else { - physics_2d_server->init(); } } void PhysicsServer2DWrapMT::finish() { - if (thread) { - command_queue.push(this, &PhysicsServer2DWrapMT::thread_exit); Thread::wait_to_finish(thread); memdelete(thread); @@ -147,7 +132,6 @@ void PhysicsServer2DWrapMT::finish() { 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; @@ -167,7 +151,6 @@ PhysicsServer2DWrapMT::PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool } PhysicsServer2DWrapMT::~PhysicsServer2DWrapMT() { - memdelete(physics_2d_server); //finish(); } diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h index 7e61927378..dbfdb077ab 100644 --- a/servers/physics_2d/physics_server_2d_wrap_mt.h +++ b/servers/physics_2d/physics_server_2d_wrap_mt.h @@ -43,7 +43,6 @@ #endif class PhysicsServer2DWrapMT : public PhysicsServer2D { - mutable PhysicsServer2D *physics_2d_server; mutable CommandQueueMT command_queue; @@ -95,7 +94,6 @@ public: //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) { - 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); } @@ -111,20 +109,17 @@ public: // this function only works on physics process, errors and returns null otherwise PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) { - 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 { - 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 { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0); return physics_2d_server->space_get_contact_count(p_space); } @@ -256,20 +251,17 @@ public: 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) { - 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); } // this function only works on physics process, errors and returns null otherwise PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) { - ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); return physics_2d_server->body_get_direct_state(p_body); } @@ -325,7 +317,6 @@ public: template <class T> static PhysicsServer2D *init_server() { - int tm = GLOBAL_DEF("physics/2d/thread_model", 1); if (tm == 0) // single unsafe return memnew(T); diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp index 06096d674a..4976e3ede4 100644 --- a/servers/physics_2d/shape_2d_sw.cpp +++ b/servers/physics_2d/shape_2d_sw.cpp @@ -43,7 +43,6 @@ void Shape2DSW::configure(const Rect2 &p_aabb) { } Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const { - Vector2 res[2]; int amnt; get_supports(p_normal, res, amnt); @@ -51,7 +50,6 @@ Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const { } void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) { - Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner); if (E) { E->get()++; @@ -61,7 +59,6 @@ void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) { } void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) { - Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner); ERR_FAIL_COND(!E); E->get()--; @@ -71,7 +68,6 @@ void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) { } bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const { - return owners.has(p_owner); } @@ -80,13 +76,11 @@ const Map<ShapeOwner2DSW *, int> &Shape2DSW::get_owners() const { } Shape2DSW::Shape2DSW() { - custom_bias = 0; configured = false; } Shape2DSW::~Shape2DSW() { - ERR_FAIL_COND(owners.size()); } @@ -95,23 +89,19 @@ Shape2DSW::~Shape2DSW() { /*********************************************************/ void LineShape2DSW::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 { - 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 { - Vector2 segment = p_begin - p_end; real_t den = normal.dot(segment); //printf("den is %i\n",den); if (Math::abs(den) <= CMP_EPSILON) { - return false; } @@ -119,7 +109,6 @@ bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_e //printf("dist is %i\n",dist); if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { - return false; } @@ -130,12 +119,10 @@ bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_e } real_t LineShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return 0; } void LineShape2DSW::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); @@ -145,7 +132,6 @@ void LineShape2DSW::set_data(const Variant &p_data) { } Variant LineShape2DSW::get_data() const { - Array arr; arr.resize(2); arr[0] = normal; @@ -158,7 +144,6 @@ Variant LineShape2DSW::get_data() const { /*********************************************************/ void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - r_amount = 1; if (p_normal.y > 0) @@ -168,22 +153,18 @@ void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, in } bool RayShape2DSW::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 { - return false; //rays can't be intersected } real_t RayShape2DSW::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) { - Dictionary d = p_data; length = d["length"]; slips_on_slope = d["slips_on_slope"]; @@ -191,7 +172,6 @@ void RayShape2DSW::set_data(const Variant &p_data) { } Variant RayShape2DSW::get_data() const { - Dictionary d; d["length"] = length; d["slips_on_slope"] = slips_on_slope; @@ -203,7 +183,6 @@ Variant RayShape2DSW::get_data() const { /*********************************************************/ void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { r_supports[0] = a; r_supports[1] = b; @@ -220,12 +199,10 @@ void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports } bool SegmentShape2DSW::contains_point(const Vector2 &p_point) const { - return false; } bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &r_point)) return false; @@ -239,12 +216,10 @@ bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 & } real_t SegmentShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12; } void SegmentShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); Rect2 r = p_data; @@ -263,7 +238,6 @@ void SegmentShape2DSW::set_data(const Variant &p_data) { } Variant SegmentShape2DSW::get_data() const { - Rect2 r; r.position = a; r.size = b; @@ -275,18 +249,15 @@ Variant SegmentShape2DSW::get_data() const { /*********************************************************/ void CircleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - r_amount = 1; *r_supports = p_normal * radius; } bool CircleShape2DSW::contains_point(const Vector2 &p_point) const { - return p_point.length_squared() < radius * radius; } bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - Vector2 line_vec = p_end - p_begin; real_t a, b, c; @@ -312,21 +283,18 @@ bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p } real_t CircleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - real_t a = radius * p_scale.x; real_t b = radius * p_scale.y; return p_mass * (a * a + b * b) / 4; } void CircleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(!p_data.is_num()); radius = p_data; configure(Rect2(-radius, -radius, radius * 2, radius * 2)); } Variant CircleShape2DSW::get_data() const { - return radius; } @@ -335,9 +303,7 @@ Variant CircleShape2DSW::get_data() const { /*********************************************************/ void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - for (int i = 0; i < 2; i++) { - Vector2 ag; ag[i] = 1.0; real_t dp = ag.dot(p_normal); @@ -374,18 +340,15 @@ bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const { } bool RectangleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal); } real_t RectangleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Vector2 he2 = half_extents * 2 * p_scale; return p_mass * he2.dot(he2) / 12.0; } void RectangleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); half_extents = p_data; @@ -393,7 +356,6 @@ void RectangleShape2DSW::set_data(const Variant &p_data) { } Variant RectangleShape2DSW::get_data() const { - return half_extents; } @@ -402,13 +364,11 @@ Variant RectangleShape2DSW::get_data() const { /*********************************************************/ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - Vector2 n = p_normal; real_t d = n.y; if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { - // make it flat n.y = 0.0; n.normalize(); @@ -421,7 +381,6 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports r_supports[1].y -= height * 0.5; } else { - real_t h = (d > 0) ? height : -height; n *= radius; @@ -432,7 +391,6 @@ 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; @@ -443,14 +401,12 @@ bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const { } bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - real_t d = 1e10; Vector2 n = (p_end - p_begin).normalized(); bool collided = false; //try spheres 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; @@ -490,7 +446,6 @@ 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)) { - real_t pd = n.dot(rpos); if (pd < d) { r_point = rpos; @@ -505,13 +460,11 @@ 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; return p_mass * he2.dot(he2) / 12.0; } void CapsuleShape2DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); if (p_data.get_type() == Variant::ARRAY) { @@ -520,7 +473,6 @@ void CapsuleShape2DSW::set_data(const Variant &p_data) { height = arr[0]; radius = arr[1]; } else { - Point2 p = p_data; radius = p.x; height = p.y; @@ -531,7 +483,6 @@ void CapsuleShape2DSW::set_data(const Variant &p_data) { } Variant CapsuleShape2DSW::get_data() const { - return Point2(height, radius); } @@ -540,12 +491,10 @@ 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; for (int i = 0; i < point_count; i++) { - //test point real_t ld = p_normal.dot(points[i].pos); if (ld > d) { @@ -555,7 +504,6 @@ void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_su //test segment if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 2; r_supports[0] = points[i].pos; r_supports[1] = points[(i + 1) % point_count].pos; @@ -570,12 +518,10 @@ void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_su } bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const { - bool out = false; bool in = false; for (int i = 0; i < point_count; i++) { - real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos); if (d > 0) out = true; @@ -587,13 +533,11 @@ bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const { } bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - Vector2 n = (p_end - p_begin).normalized(); real_t d = 1e10; bool inters = false; for (int i = 0; i < point_count; i++) { - //hmm.. no can do.. /* if (d.dot(points[i].normal)>=0) @@ -607,7 +551,6 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec real_t nd = n.dot(res); if (nd < d) { - d = nd; r_point = res; r_normal = points[i].normal; @@ -616,7 +559,6 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec } if (inters) { - if (n.dot(r_normal) > 0) r_normal = -r_normal; } @@ -626,11 +568,9 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec } real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { - Rect2 aabb; aabb.position = points[0].pos * p_scale; for (int i = 0; i < point_count; i++) { - aabb.expand_to(points[i].pos * p_scale); } @@ -638,7 +578,6 @@ real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 } void ConvexPolygonShape2DSW::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); if (points) @@ -658,13 +597,11 @@ 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(); } } else { - Vector<real_t> dvr = p_data; point_count = dvr.size() / 4; ERR_FAIL_COND(point_count == 0); @@ -673,7 +610,6 @@ void ConvexPolygonShape2DSW::set_data(const Variant &p_data) { const real_t *r = dvr.ptr(); for (int i = 0; i < point_count; i++) { - int idx = i << 2; points[i].pos.x = r[idx + 0]; points[i].pos.y = r[idx + 1]; @@ -692,7 +628,6 @@ void ConvexPolygonShape2DSW::set_data(const Variant &p_data) { } Variant ConvexPolygonShape2DSW::get_data() const { - Vector<Vector2> dvr; dvr.resize(point_count); @@ -705,13 +640,11 @@ Variant ConvexPolygonShape2DSW::get_data() const { } ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() { - points = nullptr; point_count = 0; } ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() { - if (points) memdelete_arr(points); } @@ -719,11 +652,9 @@ ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() { ////////////////////////////////////////////////// void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - real_t d = -1e10; int idx = -1; for (int i = 0; i < points.size(); i++) { - real_t ld = p_normal.dot(points[i]); if (ld > d) { d = ld; @@ -737,12 +668,10 @@ void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_s } bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const { - return false; //sorry } bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); enum { @@ -773,23 +702,18 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve stack[0] = 0; while (true) { - uint32_t node = stack[level] & NODE_IDX_MASK; const BVH &bvh = 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); if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (bvh.left < 0) { - const Segment &s = segmentptr[bvh.right]; Vector2 a = pointptr[s.points[0]]; Vector2 b = pointptr[s.points[1]]; @@ -797,10 +721,8 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve Vector2 res; if (Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &res)) { - real_t nd = n.dot(res); if (nd < d) { - d = nd; r_point = res; r_normal = (b - a).tangent().normalized(); @@ -811,28 +733,24 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } } continue; case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = bvh.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; level++; } continue; case VISIT_DONE_BIT: { - if (level == 0) { done = true; break; @@ -847,7 +765,6 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve } if (inters) { - if (n.dot(r_normal) > 0) r_normal = -r_normal; } @@ -856,9 +773,7 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve } int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { - if (p_len == 1) { - bvh_depth = MAX(p_depth, bvh_depth); bvh.push_back(*p_bvh); return bvh.size() - 1; @@ -872,12 +787,10 @@ int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { } if (global_aabb.size.x > global_aabb.size.y) { - SortArray<BVH, BVH_CompareX> sort; sort.sort(p_bvh, p_len); } else { - SortArray<BVH, BVH_CompareY> sort; sort.sort(p_bvh, p_len); } @@ -898,13 +811,11 @@ int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { } void ConcavePolygonShape2DSW::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); Rect2 aabb; if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { - Vector<Vector2> p2arr = p_data; int len = p2arr.size(); ERR_FAIL_COND(len % 2); @@ -923,7 +834,6 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { Map<Point2, int> pointmap; for (int i = 0; i < len; i += 2) { - Point2 p1 = arr[i]; Point2 p2 = arr[i + 1]; int idx_p1, idx_p2; @@ -951,7 +861,6 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { points.resize(pointmap.size()); aabb.position = pointmap.front()->key(); for (Map<Point2, int>::Element *E = pointmap.front(); E; E = E->next()) { - aabb.expand_to(E->key()); points.write[E->get()] = E->key(); } @@ -959,7 +868,6 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { Vector<BVH> main_vbh; main_vbh.resize(segments.size()); for (int i = 0; i < main_vbh.size(); i++) { - main_vbh.write[i].aabb.position = points[segments[i].points[0]]; main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]); main_vbh.write[i].left = -1; @@ -975,13 +883,11 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { configure(aabb); } Variant ConcavePolygonShape2DSW::get_data() const { - Vector<Vector2> rsegments; int len = segments.size(); rsegments.resize(len * 2); Vector2 *w = rsegments.ptrw(); for (int i = 0; i < len; i++) { - w[(i << 1) + 0] = points[segments[i].points[0]]; w[(i << 1) + 1] = points[segments[i].points[1]]; } @@ -990,7 +896,6 @@ Variant ConcavePolygonShape2DSW::get_data() const { } void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const { - uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); enum { @@ -1021,22 +926,17 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac stack[0] = 0; while (true) { - uint32_t node = stack[level] & NODE_IDX_MASK; const BVH &bvh = bvhptr[node]; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { - bool valid = p_local_aabb.intersects(bvh.aabb); if (!valid) { - stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - if (bvh.left < 0) { - const Segment &s = segmentptr[bvh.right]; Vector2 a = pointptr[s.points[0]]; Vector2 b = pointptr[s.points[1]]; @@ -1047,28 +947,24 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { - stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } } continue; case VISIT_LEFT_BIT: { - stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = bvh.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; level++; } continue; case VISIT_DONE_BIT: { - if (level == 0) return; else diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index ca001e6dd9..97a051ca65 100644 --- a/servers/physics_2d/shape_2d_sw.h +++ b/servers/physics_2d/shape_2d_sw.h @@ -57,7 +57,6 @@ public: }; class Shape2DSW { - RID self; Rect2 aabb; bool configured; @@ -100,13 +99,11 @@ public: const Map<ShapeOwner2DSW *, 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); for (int i = 0; i < r_amount; i++) r_supports[i] = p_xform.xform(r_supports[i]); if (r_amount == 1) { - if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { //make line because they are parallel r_amount = 2; @@ -117,7 +114,6 @@ public: } } else { - if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { //optimize line and make it larger because they are parallel if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) { @@ -145,7 +141,6 @@ public: 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; \ @@ -157,7 +152,6 @@ public: } class LineShape2DSW : public Shape2DSW { - Vector2 normal; real_t d; @@ -195,7 +189,6 @@ public: }; class RayShape2DSW : public Shape2DSW { - real_t length; bool slips_on_slope; @@ -220,7 +213,6 @@ public: r_max = p_normal.dot(p_transform.get_origin()); r_min = p_normal.dot(p_transform.xform(Vector2(0, length))); if (r_max < r_min) { - SWAP(r_max, r_min); } } @@ -232,7 +224,6 @@ public: }; class SegmentShape2DSW : public Shape2DSW { - Vector2 a; Vector2 b; Vector2 n; @@ -245,7 +236,6 @@ public: virtual PhysicsServer2D::ShapeType get_type() const { 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(); } 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); } @@ -263,7 +253,6 @@ public: r_max = p_normal.dot(p_transform.xform(a)); r_min = p_normal.dot(p_transform.xform(b)); if (r_max < r_min) { - SWAP(r_max, r_min); } } @@ -279,7 +268,6 @@ public: }; class CircleShape2DSW : public Shape2DSW { - real_t radius; public: @@ -313,7 +301,6 @@ public: }; class RectangleShape2DSW : public Shape2DSW { - Vector2 half_extents; public: @@ -336,7 +323,6 @@ public: r_max = -1e20; r_min = 1e20; for (int i = 0; i < 4; i++) { - real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y))); if (d > r_max) @@ -347,7 +333,6 @@ public: } _FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const Vector2 &p_circle) const { - Vector2 local_v = p_xform_inv.xform(p_circle); Vector2 he( @@ -358,7 +343,6 @@ public: } _FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const RectangleShape2DSW *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const { - Vector2 a, b; { @@ -387,7 +371,6 @@ public: }; class CapsuleShape2DSW : public Shape2DSW { - real_t radius; real_t height; @@ -419,7 +402,6 @@ public: r_min = p_normal.dot(p_transform.xform(-n)); if (r_max < r_min) { - SWAP(r_max, r_min); } @@ -430,9 +412,7 @@ public: }; class ConvexPolygonShape2DSW : public Shape2DSW { - struct Point { - Vector2 pos; Vector2 normal; //normal to next segment }; @@ -445,7 +425,6 @@ public: _FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; } _FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; } _FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D &p_xform, int p_idx) const { - Vector2 a = points[p_idx].pos; p_idx++; Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos; @@ -465,7 +444,6 @@ public: virtual Variant get_data() const; _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) { r_min = r_max = 0; return; @@ -473,7 +451,6 @@ public: r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos)); for (int i = 1; i < point_count; i++) { - real_t d = p_normal.dot(p_transform.xform(points[i].pos)); if (d > r_max) r_max = d; @@ -489,7 +466,6 @@ public: }; class ConcaveShape2DSW : public Shape2DSW { - public: virtual bool is_concave() const { return true; } typedef void (*Callback)(void *p_userdata, Shape2DSW *p_convex); @@ -498,9 +474,7 @@ public: }; class ConcavePolygonShape2DSW : public ConcaveShape2DSW { - struct Segment { - int points[2]; }; @@ -508,7 +482,6 @@ class ConcavePolygonShape2DSW : public ConcaveShape2DSW { Vector<Point2> points; struct BVH { - Rect2 aabb; int left, right; }; @@ -517,17 +490,13 @@ class ConcavePolygonShape2DSW : public ConcaveShape2DSW { int bvh_depth; struct BVH_CompareX { - _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { - return (a.aabb.position.x + a.aabb.size.x * 0.5) < (b.aabb.position.x + b.aabb.size.x * 0.5); } }; struct BVH_CompareY { - _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { - return (a.aabb.position.y + a.aabb.size.y * 0.5) < (b.aabb.position.y + b.aabb.size.y * 0.5); } }; diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 46fe0afda6..5fcb31221f 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -35,7 +35,6 @@ #include "core/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)) { return false; } @@ -50,7 +49,6 @@ _FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint } int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas, ObjectID p_canvas_instance_id) { - if (p_result_max <= 0) return 0; @@ -63,7 +61,6 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S int cc = 0; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; @@ -104,17 +101,14 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S } int PhysicsDirectSpaceState2DSW::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { - return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point); } int PhysicsDirectSpaceState2DSW::intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { - return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point, true, p_canvas_instance_id); } bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - ERR_FAIL_COND_V(space->locked, false); Vector2 begin, end; @@ -134,7 +128,6 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec real_t min_d = 1e10; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; @@ -160,14 +153,12 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec Vector2 shape_point, shape_normal; if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { - Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); shape_point = xform.xform(shape_point); real_t ld = normal.dot(shape_point); if (ld < min_d) { - min_d = ld; res_point = shape_point; res_normal = inv_xform.basis_xform_inv(shape_normal).normalized(); @@ -194,7 +185,6 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec } int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) return 0; @@ -209,7 +199,6 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans int cc = 0; for (int i = 0; i < amount; i++) { - if (cc >= p_result_max) break; @@ -239,7 +228,6 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans } bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, false); @@ -253,7 +241,6 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor real_t best_unsafe = 1; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; @@ -271,7 +258,6 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor //test initial overlap if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { - return false; } @@ -288,10 +274,8 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor 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); if (collided) { - hi = ofs; } else { - low = ofs; } } @@ -309,7 +293,6 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor } bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - if (p_result_max <= 0) return false; @@ -335,7 +318,6 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D & PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; @@ -359,7 +341,6 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D & } struct _RestCallbackData2D { - const CollisionObject2DSW *object; const CollisionObject2DSW *best_object; int local_shape; @@ -375,7 +356,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()) { @@ -403,7 +383,6 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, } bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -420,7 +399,6 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh rcd.min_allowed_depth = space->test_motion_min_contact_depth; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) continue; @@ -450,7 +428,6 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh r_info->rid = rcd.best_object->get_self(); r_info->metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) { - const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object); Vector2 rel_vec = r_info->point - body->get_transform().get_origin(); r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); @@ -463,18 +440,15 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh } PhysicsDirectSpaceState2DSW::PhysicsDirectSpaceState2DSW() { - space = nullptr; } //////////////////////////////////////////////////////////////////////////////////////////////////////////// int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { - int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); for (int i = 0; i < amount; i++) { - bool keep = true; if (intersection_query_results[i] == p_body) @@ -489,7 +463,6 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { keep = false; if (!keep) { - if (i < amount - 1) { SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); @@ -504,13 +477,11 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { } 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; @@ -554,7 +525,6 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; do { - Vector2 recover_motion; bool collided = false; @@ -573,7 +543,6 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t 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]; @@ -631,7 +600,6 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t } if (ray_index != -1) { - PhysicsServer2D::SeparationResult &result = r_results[ray_index]; for (int k = 0; k < cbk.amount; k++) { @@ -642,7 +610,6 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t 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(); @@ -688,7 +655,6 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t } 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) { - //give me back regular physics engine logic //this is madness //and most people using this function will think @@ -705,7 +671,6 @@ 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; @@ -748,7 +713,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co Vector2 sr[max_results * 2]; do { - PhysicsServer2DSW::CollCbkData cbk; cbk.max = max_results; cbk.amount = 0; @@ -775,7 +739,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co 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]; @@ -789,7 +752,6 @@ 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)) { - cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); float owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); @@ -846,7 +808,6 @@ 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; @@ -879,7 +840,6 @@ 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)) continue; @@ -896,7 +856,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co real_t best_unsafe = 1; for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; int col_shape_idx = intersection_query_subindex_results[i]; Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx); @@ -911,7 +870,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co bool excluded = false; for (int k = 0; k < excluded_shape_pair_count; k++) { - if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == col_shape_idx) { excluded = true; break; @@ -919,7 +877,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } if (excluded) { - continue; } @@ -931,7 +888,6 @@ 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; } @@ -953,16 +909,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); if (collided) { - hi = ofs; } else { - low = ofs; } } if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { - Vector2 cd[2]; PhysicsServer2DSW::CollCbkData cbk; cbk.max = 1; @@ -987,7 +940,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } if (stuck) { - safe = 0; unsafe = 0; best_shape = body_shape_idx; //sadly it's the best @@ -997,7 +949,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co continue; } if (best_safe < safe) { - safe = best_safe; unsafe = best_unsafe; best_shape = body_shape_idx; @@ -1011,7 +962,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } { - //it collided, let's get the rest info in unsafe advance Transform2D ugt = body_transform; ugt.elements[2] += p_motion * unsafe; @@ -1027,7 +977,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co 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)) continue; @@ -1043,7 +992,6 @@ 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 i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; @@ -1058,7 +1006,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co bool excluded = false; for (int k = 0; k < excluded_shape_pair_count; k++) { - if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == shape_idx) { excluded = true; break; @@ -1070,7 +1017,6 @@ 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)) { - rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); rcd.valid_depth = 10e20; } else { @@ -1088,7 +1034,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } if (rcd.best_len != 0) { - if (r_result) { r_result->collider = rcd.best_object->get_self(); r_result->collider_id = rcd.best_object->get_instance_id(); @@ -1112,7 +1057,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } if (!collided && r_result) { - r_result->motion = p_motion; r_result->remainder = Vector2(); r_result->motion += (body_transform.get_origin() - p_from.get_origin()); @@ -1122,11 +1066,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) { - CollisionObject2DSW::Type type_A = A->get_type(); CollisionObject2DSW::Type type_B = B->get_type(); if (type_A > type_B) { - SWAP(A, B); SWAP(p_subindex_A, p_subindex_B); SWAP(type_A, type_B); @@ -1136,22 +1078,18 @@ void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, Coll self->collision_pairs++; if (type_A == CollisionObject2DSW::TYPE_AREA) { - Area2DSW *area = static_cast<Area2DSW *>(A); if (type_B == CollisionObject2DSW::TYPE_AREA) { - Area2DSW *area_b = static_cast<Area2DSW *>(B); Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area, p_subindex_A)); return area2_pair; } else { - Body2DSW *body = static_cast<Body2DSW *>(B); AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body, p_subindex_B, area, p_subindex_A)); return area_pair; } } else { - BodyPair2DSW *b = memnew(BodyPair2DSW((Body2DSW *)A, p_subindex_A, (Body2DSW *)B, p_subindex_B)); return b; } @@ -1160,7 +1098,6 @@ void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, Coll } void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self) { - Space2DSW *self = (Space2DSW *)p_self; self->collision_pairs--; Constraint2DSW *c = (Constraint2DSW *)p_data; @@ -1168,94 +1105,75 @@ void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, Col } const SelfList<Body2DSW>::List &Space2DSW::get_active_body_list() const { - return active_list; } void Space2DSW::body_add_to_active_list(SelfList<Body2DSW> *p_body) { - active_list.add(p_body); } void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW> *p_body) { - active_list.remove(p_body); } void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body) { - inertia_update_list.add(p_body); } void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body) { - inertia_update_list.remove(p_body); } BroadPhase2DSW *Space2DSW::get_broadphase() { - return broadphase; } void Space2DSW::add_object(CollisionObject2DSW *p_object) { - ERR_FAIL_COND(objects.has(p_object)); objects.insert(p_object); } void Space2DSW::remove_object(CollisionObject2DSW *p_object) { - ERR_FAIL_COND(!objects.has(p_object)); objects.erase(p_object); } const Set<CollisionObject2DSW *> &Space2DSW::get_objects() const { - return objects; } void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW> *p_body) { - state_query_list.add(p_body); } void Space2DSW::body_remove_from_state_query_list(SelfList<Body2DSW> *p_body) { - state_query_list.remove(p_body); } void Space2DSW::area_add_to_monitor_query_list(SelfList<Area2DSW> *p_area) { - monitor_query_list.add(p_area); } void Space2DSW::area_remove_from_monitor_query_list(SelfList<Area2DSW> *p_area) { - monitor_query_list.remove(p_area); } void Space2DSW::area_add_to_moved_list(SelfList<Area2DSW> *p_area) { - area_moved_list.add(p_area); } void Space2DSW::area_remove_from_moved_list(SelfList<Area2DSW> *p_area) { - area_moved_list.remove(p_area); } const SelfList<Area2DSW>::List &Space2DSW::get_moved_area_list() const { - return area_moved_list; } void Space2DSW::call_queries() { - while (state_query_list.first()) { - Body2DSW *b = state_query_list.first()->self(); state_query_list.remove(state_query_list.first()); b->call_queries(); } while (monitor_query_list.first()) { - Area2DSW *a = monitor_query_list.first()->self(); monitor_query_list.remove(monitor_query_list.first()); a->call_queries(); @@ -1263,7 +1181,6 @@ void Space2DSW::call_queries() { } void Space2DSW::setup() { - contact_debug_count = 0; while (inertia_update_list.first()) { @@ -1273,14 +1190,11 @@ void Space2DSW::setup() { } void Space2DSW::update() { - broadphase->update(); } void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius = p_value; break; @@ -1309,9 +1223,7 @@ void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_valu } real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const { - switch (p_param) { - case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius; case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: @@ -1333,27 +1245,22 @@ real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const { } void Space2DSW::lock() { - locked = true; } void Space2DSW::unlock() { - locked = false; } bool Space2DSW::is_locked() const { - return locked; } PhysicsDirectSpaceState2DSW *Space2DSW::get_direct_state() { - return direct_access; } Space2DSW::Space2DSW() { - collision_pairs = 0; active_objects = 0; island_count = 0; @@ -1385,7 +1292,6 @@ Space2DSW::Space2DSW() { } Space2DSW::~Space2DSW() { - memdelete(broadphase); memdelete(direct_access); } diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index e87d842ef1..79c38fd638 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -42,7 +42,6 @@ #include "core/typedefs.h" class PhysicsDirectSpaceState2DSW : public PhysicsDirectSpaceState2D { - GDCLASS(PhysicsDirectSpaceState2DSW, PhysicsDirectSpaceState2D); int _intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas = false, ObjectID p_canvas_instance_id = ObjectID()); @@ -62,7 +61,6 @@ public: }; class Space2DSW { - public: enum ElapsedTime { ELAPSED_TIME_INTEGRATE_FORCES, diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 6f3bcfec13..b184e652c7 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -32,13 +32,11 @@ #include "core/os/os.h" void Step2DSW::_populate_island(Body2DSW *p_body, Body2DSW **p_island, 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 @@ -58,7 +56,6 @@ void Step2DSW::_populate_island(Body2DSW *p_body, Body2DSW **p_island, Constrain } bool Step2DSW::_setup_island(Constraint2DSW *p_island, real_t p_delta) { - Constraint2DSW *ci = p_island; Constraint2DSW *prev_ci = nullptr; bool removed_root = false; @@ -83,9 +80,7 @@ bool Step2DSW::_setup_island(Constraint2DSW *p_island, real_t p_delta) { } 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); @@ -95,12 +90,10 @@ void Step2DSW::_solve_island(Constraint2DSW *p_island, int p_iterations, real_t } void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { - 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 @@ -116,7 +109,6 @@ void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { 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 @@ -132,7 +124,6 @@ void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { } void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { - p_space->lock(); // can't access space during this p_space->setup(); //update inertias, etc @@ -148,7 +139,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { const SelfList<Body2DSW> *b = body_list->first(); while (b) { - b->self()->integrate_forces(p_delta); b = b->next(); active_count++; @@ -174,7 +164,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { Body2DSW *body = b->self(); if (body->get_island_step() != _step) { - Body2DSW *island = nullptr; Constraint2DSW *constraint_island = nullptr; _populate_island(body, &island, &constraint_island); @@ -197,7 +186,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { 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; @@ -221,9 +209,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { 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(); @@ -238,7 +224,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { } prev_ci = next; } else { - //list is empty, just skip if (prev_ci) { prev_ci->set_island_list_next(ci->get_island_list_next()); @@ -282,7 +267,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { b = body_list->first(); while (b) { - const SelfList<Body2DSW> *n = b->next(); b->self()->integrate_velocities(p_delta); b = n; // in case it shuts itself down @@ -293,7 +277,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { { Body2DSW *bi = island_list; while (bi) { - _check_suspend(bi, p_delta); bi = bi->get_island_list_next(); } @@ -311,6 +294,5 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { } Step2DSW::Step2DSW() { - _step = 1; } diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h index 22d59b729b..c1b2d01fb4 100644 --- a/servers/physics_2d/step_2d_sw.h +++ b/servers/physics_2d/step_2d_sw.h @@ -34,7 +34,6 @@ #include "space_2d_sw.h" class Step2DSW { - uint64_t _step; void _populate_island(Body2DSW *p_body, Body2DSW **p_island, Constraint2DSW **p_constraint_island); |