diff options
Diffstat (limited to 'servers/physics_2d')
33 files changed, 1069 insertions, 1408 deletions
diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 85ec2aae47..acbbb7e1e8 100644 --- a/servers/physics_2d/area_2d_sw.cpp +++ b/servers/physics_2d/area_2d_sw.cpp @@ -38,6 +38,7 @@ Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_a body_shape = p_body_shape; area_shape = p_area_shape; } + Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { rid = p_body->get_self(); instance_id = p_body->get_instance_id(); @@ -46,27 +47,28 @@ 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()) + 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()) + if (!moved_list.in_list() && get_space()) { get_space()->area_add_to_moved_list(&moved_list); + } _set_transform(p_transform); _set_inv_transform(p_transform.affine_inverse()); } void Area2DSW::set_space(Space2DSW *p_space) { - if (get_space()) { - if (monitor_query_list.in_list()) + if (monitor_query_list.in_list()) { get_space()->area_remove_from_monitor_query_list(&monitor_query_list); - if (moved_list.in_list()) + } + if (moved_list.in_list()) { get_space()->area_remove_from_moved_list(&moved_list); + } } monitored_bodies.clear(); @@ -76,7 +78,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; @@ -92,12 +93,12 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { _shape_changed(); - if (!moved_list.in_list() && get_space()) + if (!moved_list.in_list() && get_space()) { get_space()->area_add_to_moved_list(&moved_list); + } } 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; @@ -113,74 +114,97 @@ void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_meth _shape_changed(); - if (!moved_list.in_list() && get_space()) + if (!moved_list.in_list() && get_space()) { get_space()->area_add_to_moved_list(&moved_list); + } } void Area2DSW::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode) { bool do_override = p_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override == (space_override_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) + if (do_override == (space_override_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) { return; + } _unregister_shapes(); space_override_mode = p_mode; _shape_changed(); } void Area2DSW::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) { - switch (p_param) { - case PhysicsServer2D::AREA_PARAM_GRAVITY: gravity = p_value; break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: gravity_vector = p_value; break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point = p_value; break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: gravity_distance_scale = p_value; break; - case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation = p_value; break; - case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: linear_damp = p_value; break; - case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: angular_damp = p_value; break; - case PhysicsServer2D::AREA_PARAM_PRIORITY: priority = p_value; break; + case PhysicsServer2D::AREA_PARAM_GRAVITY: + gravity = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: + gravity_vector = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: + gravity_is_point = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: + gravity_distance_scale = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: + point_attenuation = p_value; + break; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: + linear_damp = p_value; + break; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: + angular_damp = p_value; + break; + case PhysicsServer2D::AREA_PARAM_PRIORITY: + priority = p_value; + break; } } Variant Area2DSW::get_param(PhysicsServer2D::AreaParameter p_param) const { - switch (p_param) { - case PhysicsServer2D::AREA_PARAM_GRAVITY: return gravity; - case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector; - case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point; - case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: return gravity_distance_scale; - case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; - case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: return linear_damp; - case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: return angular_damp; - case PhysicsServer2D::AREA_PARAM_PRIORITY: return priority; + case PhysicsServer2D::AREA_PARAM_GRAVITY: + return gravity; + case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: + return gravity_vector; + case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: + return gravity_is_point; + case PhysicsServer2D::AREA_PARAM_GRAVITY_DISTANCE_SCALE: + return gravity_distance_scale; + case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: + return point_attenuation; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: + return linear_damp; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: + return angular_damp; + case PhysicsServer2D::AREA_PARAM_PRIORITY: + return priority; } return Variant(); } void Area2DSW::_queue_monitor_update() { - ERR_FAIL_COND(!get_space()); - if (!monitor_query_list.in_list()) + if (!monitor_query_list.in_list()) { get_space()->area_add_to_monitor_query_list(&monitor_query_list); + } } void Area2DSW::set_monitorable(bool p_monitorable) { - - if (monitorable == p_monitorable) + if (monitorable == p_monitorable) { return; + } monitorable = p_monitorable; _set_static(!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++) + for (int i = 0; i < 5; i++) { resptr[i] = &res[i]; + } Object *obj = ObjectDB::get_instance(monitor_callback_id); if (!obj) { @@ -190,9 +214,9 @@ void Area2DSW::call_queries() { } for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) { - - if (E->get().state == 0) + if (E->get().state == 0) { continue; //nothing happened + } res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; res[1] = E->key().rid; @@ -208,11 +232,11 @@ 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++) + for (int i = 0; i < 5; i++) { resptr[i] = &res[i]; + } Object *obj = ObjectDB::get_instance(area_monitor_callback_id); if (!obj) { @@ -222,9 +246,9 @@ void Area2DSW::call_queries() { } for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) { - - if (E->get().state == 0) + if (E->get().state == 0) { continue; //nothing happened + } res[0] = E->get().state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; res[1] = E->key().rid; @@ -246,7 +270,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..f14466f582 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,23 +62,21 @@ 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 + } else { return body_shape < p_key.body_shape; - } else + } + } else { return rid < p_key.rid; + } } _FORCE_INLINE_ BodyKey() {} @@ -88,7 +85,6 @@ class Area2DSW : public CollisionObject2DSW { }; struct BodyState { - int state; _FORCE_INLINE_ void inc() { state++; } _FORCE_INLINE_ void dec() { state--; } @@ -170,33 +166,35 @@ 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()) + 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) { +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()) + if (!monitor_query_list.in_list()) { _queue_monitor_update(); + } } 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()) + 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) { +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()) + if (!monitor_query_list.in_list()) { _queue_monitor_update(); + } } #endif // AREA_2D_SW_H diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index 669f27bcc8..d7bceb9f02 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -32,30 +32,30 @@ #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)) { result = false; - } else if (area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), NULL, this)) { + } else if (area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { result = true; } if (result != colliding) { - if (result) { - - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { body->add_area(area); - if (area->has_monitor_callback()) + } + 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) + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { body->remove_area(area); - if (area->has_monitor_callback()) + } + if (area->has_monitor_callback()) { area->remove_body_from_query(body, body_shape, area_shape); + } } colliding = result; @@ -68,7 +68,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; @@ -76,18 +75,19 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, colliding = false; body->add_constraint(this, 0); area->add_constraint(this); - if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) //need to be active to process pair + if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair p_body->set_active(true); + } } AreaPair2DSW::~AreaPair2DSW() { - if (colliding) { - - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { body->remove_area(area); - if (area->has_monitor_callback()) + } + if (area->has_monitor_callback()) { area->remove_body_from_query(body, body_shape, area_shape); + } } body->remove_constraint(this); area->remove_constraint(this); @@ -96,31 +96,31 @@ 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; - } else if (area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), NULL, this)) { + } else if (area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { result = true; } if (result != colliding) { - if (result) { - - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { area_b->add_area_to_query(area_a, shape_a, shape_b); + } - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { area_a->add_area_to_query(area_b, shape_b, shape_a); + } } else { - - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { area_b->remove_area_from_query(area_a, shape_a, shape_b); + } - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { area_a->remove_area_from_query(area_b, shape_b, shape_a); + } } colliding = result; @@ -133,7 +133,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,14 +143,14 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area } Area2Pair2DSW::~Area2Pair2DSW() { - if (colliding) { - - if (area_b->has_area_monitor_callback()) + if (area_b->has_area_monitor_callback()) { area_b->remove_area_from_query(area_a, shape_a, shape_b); + } - if (area_a->has_area_monitor_callback()) + if (area_a->has_area_monitor_callback()) { area_a->remove_area_from_query(area_b, shape_b, shape_a); + } } area_a->remove_constraint(this); 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 11cf1339bc..856bba78f7 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -34,19 +34,16 @@ #include "space_2d_sw.h" void Body2DSW::_update_inertia() { - - if (!user_inertia && get_space() && !inertia_update_list.in_list()) + 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 +52,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; } @@ -80,20 +75,19 @@ void Body2DSW::update_inertias() { _inv_inertia = inertia > 0 ? (1.0 / inertia) : 0; - if (mass) + if (mass) { _inv_mass = 1.0 / mass; - else + } else { _inv_mass = 0; + } } 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,19 +99,22 @@ void Body2DSW::update_inertias() { } void Body2DSW::set_active(bool p_active) { - - if (active == p_active) + if (active == p_active) { return; + } active = p_active; if (!p_active) { - if (get_space()) + if (get_space()) { get_space()->body_remove_from_active_list(&active_list); + } } else { - if (mode == PhysicsServer2D::BODY_MODE_STATIC) + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { return; //static bodies can't become active - if (get_space()) + } + if (get_space()) { get_space()->body_add_to_active_list(&active_list); + } //still_time=0; } @@ -135,14 +132,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 +159,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 +170,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 +187,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 +200,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 +207,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 +219,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 +226,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); @@ -258,24 +241,20 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { _update_queries(); */ } -PhysicsServer2D::BodyMode Body2DSW::get_mode() const { +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); @@ -292,8 +271,9 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va Transform2D t = p_variant; t.orthonormalize(); new_transform = get_transform(); //used as old to compute motion - if (t == new_transform) + if (t == new_transform) { break; + } _set_transform(t); _set_inv_transform(get_transform().inverse()); } @@ -301,7 +281,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; @@ -321,8 +300,9 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va } break; case PhysicsServer2D::BODY_STATE_SLEEPING: { //? - if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) + if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { break; + } bool do_sleep = p_variant; if (do_sleep) { linear_velocity = Vector2(); @@ -331,20 +311,22 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va //biased_angular_velocity=Vector3(); set_active(false); } else { - if (mode != PhysicsServer2D::BODY_MODE_STATIC) + if (mode != PhysicsServer2D::BODY_MODE_STATIC) { set_active(true); + } } } break; case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode == PhysicsServer2D::BODY_MODE_RIGID && !active && !can_sleep) + if (mode == PhysicsServer2D::BODY_MODE_RIGID && !active && !can_sleep) { set_active(true); + } } break; } } -Variant Body2DSW::get_state(PhysicsServer2D::BodyState p_state) const { +Variant Body2DSW::get_state(PhysicsServer2D::BodyState p_state) const { switch (p_state) { case PhysicsServer2D::BODY_STATE_TRANSFORM: { return get_transform(); @@ -367,26 +349,27 @@ 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()) + if (inertia_update_list.in_list()) { get_space()->body_remove_from_inertia_update_list(&inertia_update_list); - if (active_list.in_list()) + } + if (active_list.in_list()) { get_space()->body_remove_from_active_list(&active_list); - if (direct_state_query_list.in_list()) + } + if (direct_state_query_list.in_list()) { get_space()->body_remove_from_state_query_list(&direct_state_query_list); + } } _set_space(p_space); if (get_space()) { - _update_inertia(); - if (active) + if (active) { get_space()->body_add_to_active_list(&active_list); + } /* _update_queries(); if (is_active()) { @@ -400,7 +383,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,9 +399,9 @@ 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) + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { return; + } Area2DSW *def_area = get_space()->get_default_area(); // Area2DSW *damp_area = def_area; @@ -461,15 +443,17 @@ void Body2DSW::integrate_forces(real_t p_step) { gravity *= gravity_scale; // If less than 0, override dampenings with that of the Body2D - if (angular_damp >= 0) + if (angular_damp >= 0) { area_angular_damp = angular_damp; + } /* else area_angular_damp=damp_area->get_angular_damp(); */ - if (linear_damp >= 0) + if (linear_damp >= 0) { area_linear_damp = linear_damp; + } /* else area_linear_damp=damp_area->get_linear_damp(); @@ -479,7 +463,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; @@ -506,13 +489,15 @@ void Body2DSW::integrate_forces(real_t p_step) { real_t damp = 1.0 - p_step * area_linear_damp; - if (damp < 0) // reached zero in the given time + if (damp < 0) { // reached zero in the given time damp = 0; + } real_t angular_damp = 1.0 - p_step * area_angular_damp; - if (angular_damp < 0) // reached zero in the given time + if (angular_damp < 0) { // reached zero in the given time angular_damp = 0; + } linear_velocity *= damp; angular_velocity *= angular_damp; @@ -522,7 +507,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; } @@ -538,25 +522,26 @@ void Body2DSW::integrate_forces(real_t p_step) { _update_shapes_with_motion(motion); } - // damp_area=NULL; // clear the area, so it is set in the next frame - def_area = NULL; // clear the area, so it is set in the next frame + // damp_area=nullptr; // clear the area, so it is set in the next frame + def_area = nullptr; // clear the area, so it is set in the next frame contact_count = 0; } void Body2DSW::integrate_velocities(real_t p_step) { - - if (mode == PhysicsServer2D::BODY_MODE_STATIC) + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { return; + } - if (fi_callback) + if (fi_callback) { 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) + if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) { set_active(false); //stopped moving, deactivate + } return; } @@ -569,38 +554,37 @@ void Body2DSW::integrate_velocities(real_t p_step) { _set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED); _set_inv_transform(get_transform().inverse()); - if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) + if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) { new_transform = get_transform(); + } //_update_inertia_tensor(); } 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()) + if (i == E->get()) { continue; + } Body2DSW *b = n[i]; - if (b->mode != PhysicsServer2D::BODY_MODE_RIGID) + if (b->mode != PhysicsServer2D::BODY_MODE_RIGID) { continue; + } - if (!b->is_active()) + if (!b->is_active()) { b->set_active(true); + } } } } void Body2DSW::call_queries() { - if (fi_callback) { - PhysicsDirectBodyState2DSW *dbs = PhysicsDirectBodyState2DSW::singleton; dbs->body = this; @@ -609,12 +593,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,36 +607,31 @@ void Body2DSW::call_queries() { } bool Body2DSW::sleep_test(real_t p_step) { - - if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) + if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { return true; // - else if (mode == PhysicsServer2D::BODY_MODE_CHARACTER) + } else if (mode == PhysicsServer2D::BODY_MODE_CHARACTER) { return !active; // characters and kinematic bodies don't sleep unless asked to sleep - else if (!can_sleep) + } else if (!can_sleep) { 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 = NULL; + fi_callback = nullptr; } if (p_id.is_valid()) { - fi_callback = memnew(ForceIntegrationCallback); fi_callback->id = p_id; fi_callback->method = p_method; @@ -667,7 +644,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; @@ -682,8 +658,8 @@ Body2DSW::Body2DSW() : omit_force_integration = false; applied_torque = 0; island_step = 0; - island_next = NULL; - island_list_next = NULL; + island_next = nullptr; + island_list_next = nullptr; _set_static(false); first_time_kinematic = false; linear_damp = -1; @@ -697,35 +673,31 @@ Body2DSW::Body2DSW() : still_time = 0; continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; can_sleep = true; - fi_callback = NULL; + fi_callback = nullptr; } Body2DSW::~Body2DSW() { - - if (fi_callback) + if (fi_callback) { memdelete(fi_callback); + } } -PhysicsDirectBodyState2DSW *PhysicsDirectBodyState2DSW::singleton = NULL; +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 e463377c7b..2300c9cdee 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; @@ -150,15 +146,18 @@ public: int index = areas.find(AreaCMP(p_area)); if (index > -1) { areas.write[index].refCount -= 1; - if (areas[index].refCount < 1) + if (areas[index].refCount < 1) { areas.remove(index); + } } } _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count = 0; - if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && p_size) set_active(true); + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && p_size) { + set_active(true); + } } _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } @@ -205,7 +204,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); } @@ -215,7 +213,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); } @@ -224,8 +221,9 @@ public: _FORCE_INLINE_ bool is_active() const { return active; } _FORCE_INLINE_ void wakeup() { - if ((!get_space()) || mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) + if ((!get_space()) || mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { return; + } set_active(true); } @@ -249,7 +247,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); } @@ -277,7 +274,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) { @@ -298,11 +294,11 @@ 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) + if (c_max == 0) { return; + } Contact *c = contacts.ptrw(); @@ -311,11 +307,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; @@ -323,11 +317,11 @@ 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) + if (idx == -1) { return; //none least deepe than this + } } c[idx].local_pos = p_local_pos; @@ -342,7 +336,6 @@ void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_no } class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D { - GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D); public: @@ -419,7 +412,7 @@ public: virtual real_t get_step() const { return step; } PhysicsDirectBodyState2DSW() { singleton = this; - body = NULL; + body = nullptr; } }; diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index f38a76cff6..e483ddf1cc 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,11 +159,11 @@ 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) + if (mlen < CMP_EPSILON) { return false; + } Vector2 mnormal = motion / mlen; @@ -199,8 +189,9 @@ bool BodyPair2DSW::_test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Vector2 local_to = from_inv.xform(to); Vector2 rpos, rnorm; - if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { return false; + } //ray hit something @@ -211,10 +202,11 @@ bool BodyPair2DSW::_test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const //create a contact - if (p_swap_result) + if (p_swap_result) { _contact_added_callback(contact_B, contact_A); - else + } else { _contact_added_callback(contact_A, contact_B); + } return true; } @@ -228,7 +220,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,17 +261,18 @@ 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) { - if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) + if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { collided = true; + } } if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { - if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) + if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { collided = true; + } } if (!collided) { @@ -289,22 +281,24 @@ bool BodyPair2DSW::setup(real_t p_step) { } } - if (oneway_disabled) + if (oneway_disabled) { return false; + } //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; if (B->get_linear_velocity().dot(direction) >= 0) { for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - if (!c.reused) + if (!c.reused) { continue; - if (c.normal.dot(direction) > 0) //greater (normal inverted) + } + if (c.normal.dot(direction) > 0) { //greater (normal inverted) continue; + } valid = true; break; @@ -324,10 +318,12 @@ bool BodyPair2DSW::setup(real_t p_step) { if (A->get_linear_velocity().dot(direction) >= 0) { for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - if (!c.reused) + if (!c.reused) { continue; - if (c.normal.dot(direction) < 0) //less (normal ok) + } + if (c.normal.dot(direction) < 0) { //less (normal ok) continue; + } valid = true; break; @@ -345,13 +341,13 @@ 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) + if (shape_A_ptr->get_custom_bias() == 0) { bias = shape_B_ptr->get_custom_bias(); - else if (shape_B_ptr->get_custom_bias() == 0) + } else if (shape_B_ptr->get_custom_bias() == 0) { bias = shape_A_ptr->get_custom_bias(); - else + } else { bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; + } } cc = 0; @@ -361,7 +357,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 +383,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 +393,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 +435,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,17 +448,17 @@ bool BodyPair2DSW::setup(real_t p_step) { } void BodyPair2DSW::solve(real_t p_step) { - - if (!collided) + if (!collided) { return; + } for (int i = 0; i < contact_count; ++i) { - Contact &c = contacts[i]; cc++; - if (!c.active) + if (!c.active) { continue; + } // Relative velocity at contact @@ -512,7 +504,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 +517,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 11bf8712ac..8c7e715a09 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,76 +43,71 @@ 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) { +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) { +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, NULL); + ERR_FAIL_COND_V(!E, nullptr); return E->get().owner; } -bool BroadPhase2DBasic::is_static(ID p_id) const { +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 { +int BroadPhase2DBasic::get_subindex(ID p_id) const { const Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND_V(!E, -1); return E->get().subindex; } int BroadPhase2DBasic::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { - int rc = 0; for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect2 aabb = E->get().aabb; if (aabb.intersects_segment(p_from, p_to)) { - p_results[rc] = E->get().owner; p_result_indices[rc] = E->get().subindex; rc++; - if (rc >= p_max_results) + if (rc >= p_max_results) { break; + } } } return rc; } -int BroadPhase2DBasic::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { +int BroadPhase2DBasic::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { int rc = 0; for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - const Rect2 aabb = E->get().aabb; if (aabb.intersects(p_aabb)) { - p_results[rc] = E->get().owner; p_result_indices[rc] = E->get().subindex; rc++; - if (rc >= p_max_results) + if (rc >= p_max_results) { break; + } } } @@ -121,28 +115,25 @@ 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) { +void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { unpair_userdata = p_userdata; unpair_callback = p_unpair_callback; } void BroadPhase2DBasic::update() { - // recompute pairs for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) { - for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) { - Element *elem_A = &I->get(); Element *elem_B = &J->get(); - if (elem_A->owner == elem_B->owner) + if (elem_A->owner == elem_B->owner) { continue; + } bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static); @@ -151,16 +142,17 @@ void BroadPhase2DBasic::update() { Map<PairKey, void *>::Element *E = pair_map.find(key); if (!pair_ok && E) { - if (unpair_callback) + if (unpair_callback) { unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata); + } pair_map.erase(key); } if (pair_ok && !E) { - - void *data = NULL; - if (pair_callback) + void *data = nullptr; + if (pair_callback) { data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata); + } pair_map.insert(key, data); } } @@ -168,15 +160,13 @@ void BroadPhase2DBasic::update() { } BroadPhase2DSW *BroadPhase2DBasic::_create() { - return memnew(BroadPhase2DBasic); } BroadPhase2DBasic::BroadPhase2DBasic() { - current = 1; - unpair_callback = NULL; - unpair_userdata = NULL; - pair_callback = NULL; - pair_userdata = NULL; + unpair_callback = nullptr; + unpair_userdata = nullptr; + pair_callback = nullptr; + pair_userdata = nullptr; } diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_basic.h index fea5668c89..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; @@ -91,8 +88,8 @@ public: virtual bool is_static(ID p_id) const; virtual int get_subindex(ID p_id) const; - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL); + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index 711ff9f1f7..ae549ed2e4 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,17 +89,19 @@ 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 for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) { - if (E->key() == p_elem->self) + if (E->key() == p_elem->self) { continue; // do not pair against itself - if (E->get().owner == p_elem->owner) + } + if (E->get().owner == p_elem->owner) { continue; - if (E->get()._static && p_static) + } + if (E->get()._static && p_static) { continue; + } _pair_attempt(p_elem, &E->get()); } @@ -121,9 +114,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 +123,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,26 +146,23 @@ 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) + 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) + if (E->key()->owner == p_elem->owner) { continue; + } _pair_attempt(p_elem, E->key()); } } @@ -186,23 +173,23 @@ 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) + if (E->key() == p_elem) { continue; // do not pair against itself - if (E->key()->owner == p_elem->owner) + } + if (E->key()->owner == p_elem->owner) { continue; - if (E->key()->_static && p_static) + } + if (E->key()->_static && p_static) { continue; + } _pair_attempt(E->key(), p_elem); } } void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) { - Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); if (sz.width * sz.height > large_object_min_surface) { - //unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static Map<Element *, PairData *>::Element *E = p_elem->paired.front(); while (E) { @@ -221,9 +208,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 +217,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,48 +230,41 @@ 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) + 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) + if (E->key()->owner == p_elem->owner) { continue; + } _unpair_attempt(p_elem, E->key()); } } } if (pb->object_set.empty() && pb->static_object_set.empty()) { - if (hash_table[idx] == pb) { hash_table[idx] = pb->next; } else { - PosBin *px = hash_table[idx]; while (px) { - if (px->next == pb) { px->next = pb->next; break; @@ -305,12 +282,15 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool } for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - if (E->key() == p_elem) + if (E->key() == p_elem) { continue; // do not pair against itself - if (E->key()->owner == p_elem->owner) + } + if (E->key()->owner == p_elem->owner) { continue; - if (E->key()->_static && p_static) + } + if (E->key()->_static && p_static) { continue; + } //unpair from large elements _unpair_attempt(p_elem, E->key()); @@ -318,7 +298,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,22 +312,20 @@ 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); Element &e = E->get(); - if (p_aabb == e.aabb) + if (p_aabb == e.aabb) { return; + } if (p_aabb != Rect2()) { - _enter_grid(&e, p_aabb, e._static); } if (e.aabb != Rect2()) { - _exit_grid(&e, e.aabb, e._static); } @@ -358,18 +335,20 @@ void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) { e.aabb = p_aabb; } -void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { +void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); Element &e = E->get(); - if (e._static == p_static) + if (e._static == p_static) { return; + } - if (e.aabb != Rect2()) + if (e.aabb != Rect2()) { _exit_grid(&e, e.aabb, e._static); + } e._static = p_static; @@ -378,33 +357,33 @@ void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) { _check_motion(&e); } } -void BroadPhase2DHashGrid::remove(ID p_id) { +void BroadPhase2DHashGrid::remove(ID p_id) { Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); Element &e = E->get(); - if (e.aabb != Rect2()) + if (e.aabb != Rect2()) { _exit_grid(&e, e.aabb, e._static); + } element_map.erase(p_id); } CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, NULL); + ERR_FAIL_COND_V(!E, nullptr); return E->get().owner; } -bool BroadPhase2DHashGrid::is_static(ID p_id) const { +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 { +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 +391,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 +399,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; } @@ -429,23 +406,27 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, cons pb = pb->next; } - if (!pb) + if (!pb) { return; + } for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) { - - if (index >= p_max_results) + if (index >= p_max_results) { break; - if (E->key()->pass == pass) + } + if (E->key()->pass == pass) { continue; + } E->key()->pass = pass; - if (use_aabb && !p_aabb.intersects(E->key()->aabb)) + if (use_aabb && !p_aabb.intersects(E->key()->aabb)) { continue; + } - if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) + if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) { continue; + } p_results[index] = E->key()->owner; p_result_indices[index] = E->key()->subindex; @@ -453,18 +434,20 @@ 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) + if (index >= p_max_results) { break; - if (E->key()->pass == pass) + } + if (E->key()->pass == pass) { continue; + } if (use_aabb && !p_aabb.intersects(E->key()->aabb)) { continue; } - if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) + if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) { continue; + } E->key()->pass = pass; p_results[index] = E->key()->owner; @@ -474,18 +457,20 @@ 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); - if (dir == Vector2()) + if (dir == Vector2()) { return 0; + } //avoid divisions by zero dir.normalize(); - if (dir.x == 0.0) + if (dir.x == 0.0) { dir.x = 0.000001; - if (dir.y == 0.0) + } + if (dir.y == 0.0) { dir.y = 0.000001; + } Vector2 delta = dir.abs(); delta.x = cell_size / delta.x; @@ -498,15 +483,17 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_t Vector2 max; - if (dir.x < 0) + if (dir.x < 0) { max.x = (Math::floor((double)pos.x) * cell_size - p_from.x) / dir.x; - else + } else { max.x = (Math::floor((double)pos.x + 1) * cell_size - p_from.x) / dir.x; + } - if (dir.y < 0) + if (dir.y < 0) { max.y = (Math::floor((double)pos.y) * cell_size - p_from.y) / dir.y; - else + } else { max.y = (Math::floor((double)pos.y + 1) * cell_size - p_from.y) / dir.y; + } int cullcount = 0; _cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount); @@ -515,45 +502,44 @@ 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; } if (step.x > 0) { - if (pos.x >= end.x) + if (pos.x >= end.x) { reached_x = true; + } } else if (pos.x <= end.x) { - reached_x = true; } if (step.y > 0) { - if (pos.y >= end.y) + if (pos.y >= end.y) { reached_y = true; + } } else if (pos.y <= end.y) { - reached_y = true; } _cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount); - if (reached_x && reached_y) + if (reached_x && reached_y) { break; + } } for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) { - - if (cullcount >= p_max_results) + if (cullcount >= p_max_results) { break; - if (E->key()->pass == pass) + } + if (E->key()->pass == pass) { continue; + } E->key()->pass = pass; @@ -562,8 +548,9 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_t continue; */ - if (!E->key()->aabb.intersects_segment(p_from, p_to)) + if (!E->key()->aabb.intersects_segment(p_from, p_to)) { continue; + } p_results[cullcount] = E->key()->owner; p_result_indices[cullcount] = E->key()->subindex; @@ -574,7 +561,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,24 +568,24 @@ 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) + if (cullcount >= p_max_results) { break; - if (E->key()->pass == pass) + } + if (E->key()->pass == pass) { continue; + } E->key()->pass = pass; - if (!p_aabb.intersects(E->key()->aabb)) + if (!p_aabb.intersects(E->key()->aabb)) { continue; + } /* if (!E->key()->aabb.intersects_segment(p_from,p_to)) @@ -614,12 +600,11 @@ 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) { +void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { unpair_callback = p_unpair_callback; unpair_userdata = p_userdata; } @@ -628,12 +613,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); @@ -645,15 +628,15 @@ BroadPhase2DHashGrid::BroadPhase2DHashGrid() { large_object_min_surface = GLOBAL_DEF("physics/2d/large_object_surface_threshold_in_cells", 512); ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/large_object_surface_threshold_in_cells", PropertyInfo(Variant::INT, "physics/2d/large_object_surface_threshold_in_cells", PROPERTY_HINT_RANGE, "0,1024,1,or_greater")); - for (uint32_t i = 0; i < hash_table_size; i++) - hash_table[i] = NULL; + for (uint32_t i = 0; i < hash_table_size; i++) { + hash_table[i] = nullptr; + } pass = 1; current = 0; } BroadPhase2DHashGrid::~BroadPhase2DHashGrid() { - for (uint32_t i = 0; i < hash_table_size; i++) { while (hash_table[i]) { PosBin *pb = hash_table[i]; diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h index e75b51c19d..de1ada0932 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ b/servers/physics_2d/broad_phase_2d_hash_grid.h @@ -35,21 +35,18 @@ #include "core/map.h" class BroadPhase2DHashGrid : public BroadPhase2DSW { - struct PairData { - bool colliding; int rc; void *ud; PairData() { colliding = false; rc = 1; - ud = NULL; + ud = nullptr; } }; 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; @@ -177,8 +170,8 @@ public: virtual bool is_static(ID p_id) const; virtual int get_subindex(ID p_id) const; - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL); - virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL); + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); diff --git a/servers/physics_2d/broad_phase_2d_sw.cpp b/servers/physics_2d/broad_phase_2d_sw.cpp index c9e1dd8758..5ba557e70a 100644 --- a/servers/physics_2d/broad_phase_2d_sw.cpp +++ b/servers/physics_2d/broad_phase_2d_sw.cpp @@ -30,7 +30,7 @@ #include "broad_phase_2d_sw.h" -BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func = NULL; +BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func = nullptr; BroadPhase2DSW::~BroadPhase2DSW() { } diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h index c7777d9d92..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)(); @@ -58,8 +57,8 @@ public: virtual bool is_static(ID p_id) const = 0; virtual int get_subindex(ID p_id) const = 0; - virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; - virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0; + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index a6891372c2..6931d96fe4 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; @@ -91,13 +87,15 @@ void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) { ERR_FAIL_INDEX(p_idx, shapes.size()); CollisionObject2DSW::Shape &shape = shapes.write[p_idx]; - if (shape.disabled == p_disabled) + if (shape.disabled == p_disabled) { return; + } shape.disabled = p_disabled; - if (!space) + if (!space) { return; + } if (p_disabled && shape.bpid != 0) { space->get_broadphase()->remove(shape.bpid); @@ -115,10 +113,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,13 +123,12 @@ 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) + if (shapes[i].bpid == 0) { continue; + } //should never get here with a null owner space->get_broadphase()->remove(shapes[i].bpid); shapes.write[i].bpid = 0; @@ -149,12 +144,14 @@ void CollisionObject2DSW::remove_shape(int p_index) { } void CollisionObject2DSW::_set_static(bool p_static) { - if (_static == p_static) + if (_static == p_static) { return; + } _static = p_static; - if (!space) + if (!space) { return; + } for (int i = 0; i < get_shape_count(); i++) { const Shape &s = shapes[i]; if (s.bpid > 0) { @@ -164,9 +161,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,16 +171,16 @@ void CollisionObject2DSW::_unregister_shapes() { } void CollisionObject2DSW::_update_shapes() { - - if (!space) + if (!space) { return; + } for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.disabled) + if (s.disabled) { continue; + } if (s.bpid == 0) { s.bpid = space->get_broadphase()->create(this, i); @@ -204,15 +199,15 @@ void CollisionObject2DSW::_update_shapes() { } void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { - - if (!space) + if (!space) { return; + } for (int i = 0; i < shapes.size(); i++) { - Shape &s = shapes.write[i]; - if (s.disabled) + if (s.disabled) { continue; + } if (s.bpid == 0) { s.bpid = space->get_broadphase()->create(this, i); @@ -231,13 +226,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,24 +241,21 @@ 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 = NULL; + space = nullptr; collision_mask = 1; collision_layer = 1; pickable = true; 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 217c93c6da..d993754fee 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -30,10 +30,9 @@ #include "collision_solver_2d_sat.h" -#include "core/math/geometry.h" +#include "core/math/geometry_2d.h" struct _CollectorCallback2D { - CollisionSolver2DSW::CallbackResult callback; void *userdata; bool swap; @@ -42,22 +41,21 @@ 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; */ - if (swap) + if (swap) { callback(p_point_B, p_point_A, userdata); - else + } else { callback(p_point_A, p_point_B, userdata); + } } }; 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,13 +65,12 @@ _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); #endif - Vector2 closest_B = Geometry::get_closest_point_to_segment_uncapped_2d(*p_points_A, p_points_B); + Vector2 closest_B = Geometry2D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); p_collector->call(*p_points_A, closest_B); } @@ -85,7 +82,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,25 +111,25 @@ _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); - if (n.dot(a) > n.dot(b) - CMP_EPSILON) + if (n.dot(a) > n.dot(b) - CMP_EPSILON) { continue; + } p_collector->call(a, b); } else { Vector2 b = p_points_B[dvec[i].idx]; Vector2 a = n.plane_project(dA, b); - if (n.dot(a) > n.dot(b) - CMP_EPSILON) + if (n.dot(a) > n.dot(b) - CMP_EPSILON) { continue; + } p_collector->call(a, b); } } } 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); @@ -145,7 +141,7 @@ static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_po _generate_contacts_point_edge, }, { - 0, + nullptr, _generate_contacts_edge_edge, } }; @@ -165,7 +161,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 +177,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 +193,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,30 +204,30 @@ public: } _FORCE_INLINE_ bool test_cast() { - if (castA) { - Vector2 na = motion_A.normalized(); - if (!test_axis(na)) + if (!test_axis(na)) { return false; - if (!test_axis(na.tangent())) + } + if (!test_axis(na.tangent())) { return false; + } } if (castB) { - Vector2 nb = motion_B.normalized(); - if (!test_axis(nb)) + if (!test_axis(nb)) { return false; - if (!test_axis(nb.tangent())) + } + if (!test_axis(nb.tangent())) { return false; + } } return true; } _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { - Vector2 axis = p_axis; if (Math::is_zero_approx(axis.x) && @@ -245,15 +238,17 @@ public: real_t min_A, max_A, min_B, max_B; - if (castA) + if (castA) { shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A); - else + } else { shape_A->project_range(axis, *transform_A, min_A, max_A); + } - if (castB) + if (castB) { shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B); - else + } else { shape_B->project_range(axis, *transform_B, min_B, max_B); + } if (withMargin) { min_A -= margin_A; @@ -269,8 +264,9 @@ public: real_t dmax = max_B - (min_A + max_A) * 0.5; if (dmin > 0.0 || dmax < 0.0) { - if (callback && callback->sep_axis) + if (callback && callback->sep_axis) { *callback->sep_axis = axis; + } #ifdef DEBUG_ENABLED best_axis_count++; #endif @@ -308,16 +304,17 @@ public: } _FORCE_INLINE_ void generate_contacts() { - // nothing to do, don't generate - if (best_axis == Vector2(0.0, 0.0)) + if (best_axis == Vector2(0.0, 0.0)) { return; + } if (callback) { callback->collided = true; - if (!callback->callback) + if (!callback->callback) { return; //only collide, no callback + } } static const int max_supports = 2; @@ -333,7 +330,6 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_A; i++) { supports_A[i] += -best_axis * margin_A; } @@ -351,7 +347,6 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_B; i++) { supports_B[i] += best_axis * margin_B; } @@ -360,13 +355,13 @@ public: callback->normal = best_axis; _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); - if (callback->sep_axis && *callback->sep_axis != Vector2()) + if (callback->sep_axis && *callback->sep_axis != Vector2()) { *callback->sep_axis = Vector2(); //invalidate previous axis (no test) + } } } _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,35 +392,42 @@ 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); SeparatorAxisTest2D<SegmentShape2DSW, SegmentShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } //this collision is kind of pointless - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { return; - if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) + } + if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) { return; + } if (withMargin) { //points grow to circles - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) { return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) { return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) { return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) { return; + } } separator.generate_contacts(); @@ -433,90 +435,102 @@ 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); SeparatorAxisTest2D<SegmentShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } //segment normal if (!separator.test_axis( - (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().tangent())) + (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().tangent())) { return; + } //endpoint a vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) { return; + } //endpoint b vs circle - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) { return; + } separator.generate_contacts(); } 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); SeparatorAxisTest2D<SegmentShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { return; + } - if (!separator.test_axis(p_transform_b.elements[0].normalized())) + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { return; + } - if (!separator.test_axis(p_transform_b.elements[1].normalized())) + if (!separator.test_axis(p_transform_b.elements[1].normalized())) { return; + } if (withMargin) { - Transform2D inv = p_transform_b.affine_inverse(); Vector2 a = p_transform_a.xform(segment_A->get_a()); Vector2 b = p_transform_a.xform(segment_A->get_b()); - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) { return; - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) { return; + } if (castA) { - - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) + 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))) + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) { return; + } } if (castB) { - - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) + 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))) + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) { return; + } } if (castA && castB) { - - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) + 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))) + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) { return; + } } } @@ -525,64 +539,74 @@ 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); SeparatorAxisTest2D<SegmentShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { return; + } - if (!separator.test_axis(p_transform_b.elements[0].normalized())) + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { return; + } - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) { return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) { return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) { return; - if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) { return; + } separator.generate_contacts(); } 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); SeparatorAxisTest2D<SegmentShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } - if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { 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))) + 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)))) + 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)))) + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) { return; + } } } @@ -593,74 +617,79 @@ 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); SeparatorAxisTest2D<CircleShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } - if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) { return; + } separator.generate_contacts(); } 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); SeparatorAxisTest2D<CircleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } const Vector2 &sphere = p_transform_a.elements[2]; const Vector2 *axis = &p_transform_b.elements[0]; //const Vector2& half_extents = rectangle_B->get_half_extents(); - if (!separator.test_axis(axis[0].normalized())) + if (!separator.test_axis(axis[0].normalized())) { return; + } - if (!separator.test_axis(axis[1].normalized())) + if (!separator.test_axis(axis[1].normalized())) { return; + } Transform2D binv = p_transform_b.affine_inverse(); { - - if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) + 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))) + 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))) + 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))) + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { return; + } } separator.generate_contacts(); @@ -668,53 +697,59 @@ 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); SeparatorAxisTest2D<CircleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { return; + } //capsule endpoints - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) { return; - if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) + } + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) { return; + } separator.generate_contacts(); } 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); SeparatorAxisTest2D<CircleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } //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)))) + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) { return; + } - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { return; + } } separator.generate_contacts(); @@ -724,42 +759,46 @@ 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); SeparatorAxisTest2D<RectangleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } //box faces A - if (!separator.test_axis(p_transform_a.elements[0].normalized())) + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { return; + } - if (!separator.test_axis(p_transform_a.elements[1].normalized())) + if (!separator.test_axis(p_transform_a.elements[1].normalized())) { return; + } //box faces B - if (!separator.test_axis(p_transform_b.elements[0].normalized())) + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { return; + } - if (!separator.test_axis(p_transform_b.elements[1].normalized())) + if (!separator.test_axis(p_transform_b.elements[1].normalized())) { return; + } if (withMargin) { - Transform2D invA = p_transform_a.affine_inverse(); Transform2D invB = p_transform_b.affine_inverse(); - if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) { return; + } if (castA || castB) { - Transform2D aofs = p_transform_a; aofs.elements[2] += p_motion_a; @@ -770,21 +809,21 @@ 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))) + 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))) + 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))) + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) { return; + } } } } @@ -794,56 +833,62 @@ 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); SeparatorAxisTest2D<RectangleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } //box faces - if (!separator.test_axis(p_transform_a.elements[0].normalized())) + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { return; + } - if (!separator.test_axis(p_transform_a.elements[1].normalized())) + if (!separator.test_axis(p_transform_a.elements[1].normalized())) { return; + } //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { return; + } //box endpoints to capsule circles 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); - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { return; + } } if (castA) { Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); capsule_endpoint -= p_motion_a; - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { return; + } } if (castB) { Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5); capsule_endpoint += p_motion_b; - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { return; + } } if (castA && castB) { @@ -851,8 +896,9 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D capsule_endpoint -= p_motion_a; capsule_endpoint += p_motion_b; - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { return; + } } } @@ -861,24 +907,27 @@ 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); SeparatorAxisTest2D<RectangleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } //box faces - if (!separator.test_axis(p_transform_a.elements[0].normalized())) + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { return; + } - if (!separator.test_axis(p_transform_a.elements[1].normalized())) + if (!separator.test_axis(p_transform_a.elements[1].normalized())) { return; + } //convex faces Transform2D boxinv; @@ -886,28 +935,29 @@ 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))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { return; + } if (withMargin) { //all points vs all points need to be tested if margin exist - if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) + 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))) + 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))) + 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))) + 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,38 +969,40 @@ 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); SeparatorAxisTest2D<CapsuleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } //capsule axis - if (!separator.test_axis(p_transform_b.elements[0].normalized())) + if (!separator.test_axis(p_transform_b.elements[0].normalized())) { return; + } - if (!separator.test_axis(p_transform_a.elements[0].normalized())) + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { return; + } //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)) + if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) { return; + } } } @@ -959,38 +1011,40 @@ 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); SeparatorAxisTest2D<CapsuleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { return; + } //capsule axis - if (!separator.test_axis(p_transform_a.elements[0].normalized())) + if (!separator.test_axis(p_transform_a.elements[0].normalized())) { return; + } //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)) + if (TEST_POINT(capsule_endpoint_A, cpoint)) { return; + } } - if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { return; + } } separator.generate_contacts(); @@ -1000,37 +1054,37 @@ 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); SeparatorAxisTest2D<ConvexPolygonShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_cast()) + if (!separator.test_cast()) { 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))) + 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))) + 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)))) + if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) { return; + } } } } @@ -1041,7 +1095,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); @@ -1060,25 +1113,25 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D _collision_segment_rectangle<false, false, false>, _collision_segment_capsule<false, false, false>, _collision_segment_convex_polygon<false, false, false> }, - { 0, + { nullptr, _collision_circle_circle<false, false, false>, _collision_circle_rectangle<false, false, false>, _collision_circle_capsule<false, false, false>, _collision_circle_convex_polygon<false, false, false> }, - { 0, - 0, + { nullptr, + nullptr, _collision_rectangle_rectangle<false, false, false>, _collision_rectangle_capsule<false, false, false>, _collision_rectangle_convex_polygon<false, false, false> }, - { 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, _collision_capsule_capsule<false, false, false>, _collision_capsule_convex_polygon<false, false, false> }, - { 0, - 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, + nullptr, _collision_convex_polygon_convex_polygon<false, false, false> } }; @@ -1089,25 +1142,25 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D _collision_segment_rectangle<true, false, false>, _collision_segment_capsule<true, false, false>, _collision_segment_convex_polygon<true, false, false> }, - { 0, + { nullptr, _collision_circle_circle<true, false, false>, _collision_circle_rectangle<true, false, false>, _collision_circle_capsule<true, false, false>, _collision_circle_convex_polygon<true, false, false> }, - { 0, - 0, + { nullptr, + nullptr, _collision_rectangle_rectangle<true, false, false>, _collision_rectangle_capsule<true, false, false>, _collision_rectangle_convex_polygon<true, false, false> }, - { 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, _collision_capsule_capsule<true, false, false>, _collision_capsule_convex_polygon<true, false, false> }, - { 0, - 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, + nullptr, _collision_convex_polygon_convex_polygon<true, false, false> } }; @@ -1118,25 +1171,25 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D _collision_segment_rectangle<false, true, false>, _collision_segment_capsule<false, true, false>, _collision_segment_convex_polygon<false, true, false> }, - { 0, + { nullptr, _collision_circle_circle<false, true, false>, _collision_circle_rectangle<false, true, false>, _collision_circle_capsule<false, true, false>, _collision_circle_convex_polygon<false, true, false> }, - { 0, - 0, + { nullptr, + nullptr, _collision_rectangle_rectangle<false, true, false>, _collision_rectangle_capsule<false, true, false>, _collision_rectangle_convex_polygon<false, true, false> }, - { 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, _collision_capsule_capsule<false, true, false>, _collision_capsule_convex_polygon<false, true, false> }, - { 0, - 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, + nullptr, _collision_convex_polygon_convex_polygon<false, true, false> } }; @@ -1147,25 +1200,25 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D _collision_segment_rectangle<true, true, false>, _collision_segment_capsule<true, true, false>, _collision_segment_convex_polygon<true, true, false> }, - { 0, + { nullptr, _collision_circle_circle<true, true, false>, _collision_circle_rectangle<true, true, false>, _collision_circle_capsule<true, true, false>, _collision_circle_convex_polygon<true, true, false> }, - { 0, - 0, + { nullptr, + nullptr, _collision_rectangle_rectangle<true, true, false>, _collision_rectangle_capsule<true, true, false>, _collision_rectangle_convex_polygon<true, true, false> }, - { 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, _collision_capsule_capsule<true, true, false>, _collision_capsule_convex_polygon<true, true, false> }, - { 0, - 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, + nullptr, _collision_convex_polygon_convex_polygon<true, true, false> } }; @@ -1176,25 +1229,25 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D _collision_segment_rectangle<false, false, true>, _collision_segment_capsule<false, false, true>, _collision_segment_convex_polygon<false, false, true> }, - { 0, + { nullptr, _collision_circle_circle<false, false, true>, _collision_circle_rectangle<false, false, true>, _collision_circle_capsule<false, false, true>, _collision_circle_convex_polygon<false, false, true> }, - { 0, - 0, + { nullptr, + nullptr, _collision_rectangle_rectangle<false, false, true>, _collision_rectangle_capsule<false, false, true>, _collision_rectangle_convex_polygon<false, false, true> }, - { 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, _collision_capsule_capsule<false, false, true>, _collision_capsule_convex_polygon<false, false, true> }, - { 0, - 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, + nullptr, _collision_convex_polygon_convex_polygon<false, false, true> } }; @@ -1205,25 +1258,25 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D _collision_segment_rectangle<true, false, true>, _collision_segment_capsule<true, false, true>, _collision_segment_convex_polygon<true, false, true> }, - { 0, + { nullptr, _collision_circle_circle<true, false, true>, _collision_circle_rectangle<true, false, true>, _collision_circle_capsule<true, false, true>, _collision_circle_convex_polygon<true, false, true> }, - { 0, - 0, + { nullptr, + nullptr, _collision_rectangle_rectangle<true, false, true>, _collision_rectangle_capsule<true, false, true>, _collision_rectangle_convex_polygon<true, false, true> }, - { 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, _collision_capsule_capsule<true, false, true>, _collision_capsule_convex_polygon<true, false, true> }, - { 0, - 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, + nullptr, _collision_convex_polygon_convex_polygon<true, false, true> } }; @@ -1234,25 +1287,25 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D _collision_segment_rectangle<false, true, true>, _collision_segment_capsule<false, true, true>, _collision_segment_convex_polygon<false, true, true> }, - { 0, + { nullptr, _collision_circle_circle<false, true, true>, _collision_circle_rectangle<false, true, true>, _collision_circle_capsule<false, true, true>, _collision_circle_convex_polygon<false, true, true> }, - { 0, - 0, + { nullptr, + nullptr, _collision_rectangle_rectangle<false, true, true>, _collision_rectangle_capsule<false, true, true>, _collision_rectangle_convex_polygon<false, true, true> }, - { 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, _collision_capsule_capsule<false, true, true>, _collision_capsule_convex_polygon<false, true, true> }, - { 0, - 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, + nullptr, _collision_convex_polygon_convex_polygon<false, true, true> } }; @@ -1263,25 +1316,25 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D _collision_segment_rectangle<true, true, true>, _collision_segment_capsule<true, true, true>, _collision_segment_convex_polygon<true, true, true> }, - { 0, + { nullptr, _collision_circle_circle<true, true, true>, _collision_circle_rectangle<true, true, true>, _collision_circle_capsule<true, true, true>, _collision_circle_convex_polygon<true, true, true> }, - { 0, - 0, + { nullptr, + nullptr, _collision_rectangle_rectangle<true, true, true>, _collision_rectangle_capsule<true, true, true>, _collision_rectangle_convex_polygon<true, true, true> }, - { 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, _collision_capsule_capsule<true, true, true>, _collision_capsule_convex_polygon<true, true, true> }, - { 0, - 0, - 0, - 0, + { nullptr, + nullptr, + nullptr, + nullptr, _collision_convex_polygon_convex_polygon<true, true, true> } }; @@ -1323,7 +1376,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_sat.h b/servers/physics_2d/collision_solver_2d_sat.h index 105cb9104d..6bb485f561 100644 --- a/servers/physics_2d/collision_solver_2d_sat.h +++ b/servers/physics_2d/collision_solver_2d_sat.h @@ -33,6 +33,6 @@ #include "collision_solver_2d_sw.h" -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 = false, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); +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 = false, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); #endif // COLLISION_SOLVER_2D_SAT_H diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index f117dcbfe5..beba709807 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -35,10 +35,10 @@ //#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) + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_LINE) { return false; + } Vector2 n = p_transform_A.basis_xform(line->get_normal()).normalized(); Vector2 p = p_transform_A.xform(line->get_normal() * line->get_d()); @@ -52,20 +52,21 @@ 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) + if (pd >= d) { continue; + } found = true; Vector2 support_A = supports[i] - n * (pd - d); if (p_result_callback) { - if (p_swap_result) + if (p_swap_result) { p_result_callback(supports[i], support_A, p_userdata); - else + } else { p_result_callback(support_A, supports[i], p_userdata); + } } } @@ -73,10 +74,10 @@ 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) + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_RAY) { return false; + } Vector2 from = p_transform_A.get_origin(); Vector2 to = from + p_transform_A[1] * ray->get_length(); @@ -93,9 +94,9 @@ 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) + if (sep_axis) { *sep_axis = p_transform_A[1].normalized(); + } return false; } @@ -106,16 +107,16 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector } if (p_result_callback) { - if (p_swap_result) + if (p_swap_result) { p_result_callback(support_B, support_A, p_userdata); - else + } else { p_result_callback(support_A, support_B, p_userdata); + } } return true; } struct _ConcaveCollisionInfo2D { - const Transform2D *transform_A; const Shape2DSW *shape_A; const Transform2D *transform_B; @@ -133,22 +134,22 @@ 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) + if (!cinfo.result_callback && cinfo.collided) { return; //already collided and no contacts requested, don't test anymore + } bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B); - if (!collided) + if (!collided) { return; + } cinfo.collided = true; cinfo.collisions++; } 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 +175,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 +194,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 +210,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 +221,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,17 +232,17 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p } } else if (concave_B) { - - if (concave_A) + if (concave_A) { return false; + } - if (!swap) + if (!swap) { return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B); - else + } else { return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, sep_axis, margin_A, margin_B); + } } 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/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index e73ee8fd7e..f39cfee0a9 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -40,11 +40,11 @@ public: private: static bool solve_static_line(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static void concave_callback(void *p_userdata, Shape2DSW *p_convex); - static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL); + static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = nullptr); public: - static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); }; #endif // COLLISION_SOLVER_2D_SW_H diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index b5c994cbdd..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; @@ -45,7 +44,7 @@ class Constraint2DSW { RID self; protected: - Constraint2DSW(Body2DSW **p_body_ptr = NULL, int p_body_count = 0) { + Constraint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) { _body_ptr = p_body_ptr; _body_count = p_body_count; island_step = 0; diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index 4524629d50..81e961e90d 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; @@ -78,10 +76,11 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const static inline Vector2 relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) { Vector2 sum = a->get_linear_velocity() - rA.tangent() * a->get_angular_velocity(); - if (b) + if (b) { return (b->get_linear_velocity() - rB.tangent() * b->get_angular_velocity()) - sum; - else + } else { return -sum; + } } static inline real_t @@ -90,7 +89,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 +113,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; @@ -140,53 +137,53 @@ bool PinJoint2DSW::setup(real_t p_step) { // apply accumulated impulse A->apply_impulse(rA, -P); - if (B) + if (B) { B->apply_impulse(rB, P); + } return true; } 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()); Vector2 rel_vel; - if (B) + if (B) { rel_vel = B->get_linear_velocity() - custom_cross(rB, B->get_angular_velocity()) - vA; - else + } else { rel_vel = -vA; + } Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); A->apply_impulse(rA, -impulse); - if (B) + if (B) { B->apply_impulse(rB, impulse); + } P += impulse; } void PinJoint2DSW::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { - - if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) + 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) + if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { return softness; + } ERR_FAIL_V(0); } 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); @@ -195,16 +192,18 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p softness = 0; p_body_a->add_constraint(this, 0); - if (p_body_b) + if (p_body_b) { p_body_b->add_constraint(this, 1); + } } PinJoint2DSW::~PinJoint2DSW() { - - if (A) + if (A) { A->remove_constraint(this); - if (B) + } + if (B) { B->remove_constraint(this); + } } ////////////////////////////////////////////// @@ -259,7 +258,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 +309,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 +326,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 +339,6 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_ } GrooveJoint2DSW::~GrooveJoint2DSW() { - A->remove_constraint(this); B->remove_constraint(this); } @@ -353,17 +348,17 @@ 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); Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); real_t dist = delta.length(); - if (dist) + if (dist) { n = delta / dist; - else + } else { n = Vector2(); + } real_t k = k_scalar(A, B, rA, rB, n); n_mass = 1.0f / k; @@ -382,7 +377,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; @@ -396,39 +390,29 @@ void DampedSpringJoint2DSW::solve(real_t p_step) { B->apply_impulse(rB, j); } -void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedStringParam p_param, real_t p_value) { - +void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { switch (p_param) { - - case PhysicsServer2D::DAMPED_STRING_REST_LENGTH: { - + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { rest_length = p_value; } break; - case PhysicsServer2D::DAMPED_STRING_DAMPING: { - + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { damping = p_value; } break; - case PhysicsServer2D::DAMPED_STRING_STIFFNESS: { - + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { stiffness = p_value; } break; } } -real_t DampedSpringJoint2DSW::get_param(PhysicsServer2D::DampedStringParam p_param) const { - +real_t DampedSpringJoint2DSW::get_param(PhysicsServer2D::DampedSpringParam p_param) const { switch (p_param) { - - case PhysicsServer2D::DAMPED_STRING_REST_LENGTH: { - + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { return rest_length; } break; - case PhysicsServer2D::DAMPED_STRING_DAMPING: { - + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { return damping; } break; - case PhysicsServer2D::DAMPED_STRING_STIFFNESS: { - + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { return stiffness; } break; } @@ -438,7 +422,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 +436,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 f4aeb6e3c8..3c8aab77c8 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; @@ -51,7 +50,7 @@ public: _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } virtual PhysicsServer2D::JointType get_type() const = 0; - Joint2DSW(Body2DSW **p_body_ptr = NULL, int p_body_count = 0) : + Joint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) : Constraint2DSW(p_body_ptr, p_body_count) { bias = 0; max_force = max_bias = 3.40282e+38; @@ -59,7 +58,6 @@ public: }; class PinJoint2DSW : public Joint2DSW { - union { struct { Body2DSW *A; @@ -86,12 +84,11 @@ public: void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); real_t get_param(PhysicsServer2D::PinJointParam p_param) const; - PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b = NULL); + PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b = nullptr); ~PinJoint2DSW(); }; class GrooveJoint2DSW : public Joint2DSW { - union { struct { Body2DSW *A; @@ -126,7 +123,6 @@ public: }; class DampedSpringJoint2DSW : public Joint2DSW { - union { struct { Body2DSW *A; @@ -155,8 +151,8 @@ public: virtual bool setup(real_t p_step); virtual void solve(real_t p_step); - void set_param(PhysicsServer2D::DampedStringParam p_param, real_t p_value); - real_t get_param(PhysicsServer2D::DampedStringParam p_param) const; + void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); + real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b); ~DampedSpringJoint2DSW(); diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp index e2a482e960..6983225668 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 = NULL; + 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,56 @@ 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() { +RID PhysicsServer2DSW::segment_shape_create() { return _shape_create(SHAPE_SEGMENT); } -RID PhysicsServer2DSW::circle_shape_create() { +RID PhysicsServer2DSW::circle_shape_create() { return _shape_create(SHAPE_CIRCLE); } -RID PhysicsServer2DSW::rectangle_shape_create() { +RID PhysicsServer2DSW::rectangle_shape_create() { return _shape_create(SHAPE_RECTANGLE); } -RID PhysicsServer2DSW::capsule_shape_create() { +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() { +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,18 +137,17 @@ 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) + if (cbk->max == 0) { return; + } if (cbk->valid_dir != Vector2()) { if (p_point_A.distance_squared_to(p_point_B) > cbk->valid_depth * cbk->valid_depth) { @@ -194,7 +175,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; @@ -203,14 +183,14 @@ void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 & } real_t d = p_point_A.distance_squared_to(p_point_B); - if (d < min_depth) + if (d < min_depth) { return; + } cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; cbk->passed++; } else { - cbk->ptr[cbk->amount * 2 + 0] = p_point_A; cbk->ptr[cbk->amount * 2 + 1] = p_point_B; cbk->amount++; @@ -219,15 +199,13 @@ 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, NULL, NULL); + return CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, nullptr, nullptr); } CollCbkData cbk; @@ -242,7 +220,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,17 +234,16 @@ 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) + if (p_active) { active_spaces.insert(space); - else + } else { active_spaces.erase(space); + } } 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 +251,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,44 +258,38 @@ 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, NULL); - ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), NULL, "Space state is inaccessible right now, wait for iteration or physics process notification."); + 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."); return space->get_direct_state(); } RID PhysicsServer2DSW::area_create() { - Area2DSW *area = memnew(Area2DSW); RID rid = area_owner.make_rid(area); area->set_self(rid); @@ -328,36 +297,35 @@ 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); - Space2DSW *space = NULL; + Space2DSW *space = nullptr; if (p_space.is_valid()) { space = space_owner.getornull(p_space); ERR_FAIL_COND(!space); } - if (area->get_space() == space) + if (area->get_space() == space) { return; //pointless + } area->clear_constraints(); area->set_space(space); }; RID PhysicsServer2DSW::area_get_space(RID p_area) const { - Area2DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, RID()); Space2DSW *space = area->get_space(); - if (!space) + if (!space) { return RID(); + } return space->get_self(); }; 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 +333,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 +340,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 +350,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); @@ -394,8 +359,8 @@ 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) { +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 +368,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 +377,13 @@ 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 { +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()); @@ -429,8 +392,8 @@ 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 { +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 +401,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,16 +408,15 @@ 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); - while (area->get_shape_count()) + while (area->get_shape_count()) { area->remove_shape(0); + } } 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(); @@ -464,8 +425,8 @@ void PhysicsServer2DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id ERR_FAIL_COND(!area); area->set_instance_id(p_id); } -ObjectID PhysicsServer2DSW::area_get_object_instance_id(RID p_area) const { +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 +437,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(); @@ -485,8 +445,8 @@ void PhysicsServer2DSW::area_attach_canvas_instance_id(RID p_area, ObjectID p_id ERR_FAIL_COND(!area); area->set_canvas_instance_id(p_id); } -ObjectID PhysicsServer2DSW::area_get_canvas_instance_id(RID p_area) const { +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 +457,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 +467,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 +484,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 +491,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 +505,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 +512,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 +519,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 +526,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 +535,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,35 +542,34 @@ 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 = NULL; + Space2DSW *space = nullptr; if (p_space.is_valid()) { space = space_owner.getornull(p_space); ERR_FAIL_COND(!space); } - if (body->get_space() == space) + if (body->get_space() == space) { return; //pointless + } body->clear_constraint_map(); body->set_space(space); }; RID PhysicsServer2DSW::body_get_space(RID p_body) const { - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, RID()); Space2DSW *space = body->get_space(); - if (!space) + if (!space) { return RID(); + } return space->get_self(); }; 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 +578,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 +585,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 +595,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); @@ -659,8 +604,8 @@ 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) { +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 +613,25 @@ 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 { +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()); @@ -698,8 +640,8 @@ 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 { +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 +649,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,16 +656,15 @@ 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); - while (body->get_shape_count()) + while (body->get_shape_count()) { body->remove_shape(0); + } } 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()); @@ -732,8 +672,8 @@ 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) { +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 +683,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 +696,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 +703,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 +710,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 +717,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 +724,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 +737,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 +750,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 +757,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 +764,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 +771,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 +778,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 +786,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 +800,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 +824,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 +842,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 +858,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 +872,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 +880,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 +888,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 +897,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 +915,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 +947,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 +964,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,16 +973,16 @@ 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."); - ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), NULL, "Body state is inaccessible right now, wait for iteration or physics process notification."); - - if (!body_owner.owns(p_body)) - return NULL; + if (!body_owner.owns(p_body)) { + return nullptr; + } Body2DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, NULL); - ERR_FAIL_COND_V(!body->get_space(), NULL); - ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), NULL, "Body state is inaccessible right now, wait for iteration or physics process notification."); + ERR_FAIL_COND_V(!body, nullptr); + ERR_FAIL_COND_V(!body->get_space(), nullptr); + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); direct_state->body = body; return direct_state; @@ -1086,26 +991,36 @@ 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); switch (p_param) { - case JOINT_PARAM_BIAS: joint->set_bias(p_value); break; - case JOINT_PARAM_MAX_BIAS: joint->set_max_bias(p_value); break; - case JOINT_PARAM_MAX_FORCE: joint->set_max_force(p_value); break; + case JOINT_PARAM_BIAS: + joint->set_bias(p_value); + break; + case JOINT_PARAM_MAX_BIAS: + joint->set_max_bias(p_value); + break; + case JOINT_PARAM_MAX_FORCE: + joint->set_max_force(p_value); + break; } } 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); switch (p_param) { - case JOINT_PARAM_BIAS: return joint->get_bias(); break; - case JOINT_PARAM_MAX_BIAS: return joint->get_max_bias(); break; - case JOINT_PARAM_MAX_FORCE: return joint->get_max_force(); break; + case JOINT_PARAM_BIAS: + return joint->get_bias(); + break; + case JOINT_PARAM_MAX_BIAS: + return joint->get_max_bias(); + break; + case JOINT_PARAM_MAX_FORCE: + return joint->get_max_force(); + break; } return 0; @@ -1139,10 +1054,9 @@ 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 = NULL; + Body2DSW *B = nullptr; if (body_owner.owns(p_body_b)) { B = body_owner.getornull(p_body_b); ERR_FAIL_COND_V(!B, RID()); @@ -1156,7 +1070,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()); @@ -1170,7 +1083,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()); @@ -1184,7 +1096,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); @@ -1202,8 +1113,7 @@ real_t PhysicsServer2DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param return pin_joint->get_param(p_param); } -void PhysicsServer2DSW::damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value) { - +void PhysicsServer2DSW::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) { Joint2DSW *j = joint_owner.getornull(p_joint); ERR_FAIL_COND(!j); ERR_FAIL_COND(j->get_type() != JOINT_DAMPED_SPRING); @@ -1212,8 +1122,7 @@ void PhysicsServer2DSW::damped_string_joint_set_param(RID p_joint, DampedStringP dsj->set_param(p_param, p_value); } -real_t PhysicsServer2DSW::damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const { - +real_t PhysicsServer2DSW::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const { Joint2DSW *j = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!j, 0); ERR_FAIL_COND_V(j->get_type() != JOINT_DAMPED_SPRING, 0); @@ -1223,7 +1132,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); @@ -1231,11 +1139,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()) { @@ -1246,7 +1152,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); /* @@ -1260,7 +1165,6 @@ void PhysicsServer2DSW::free(RID p_rid) { body_set_space(p_rid, RID()); while (body->get_shape_count()) { - body->remove_shape(0); } @@ -1268,7 +1172,6 @@ void PhysicsServer2DSW::free(RID p_rid) { memdelete(body); } else if (area_owner.owns(p_rid)) { - Area2DSW *area = area_owner.getornull(p_rid); /* @@ -1276,22 +1179,20 @@ void PhysicsServer2DSW::free(RID p_rid) { _clear_query(area->get_monitor_query()); */ - area->set_space(NULL); + 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()) { CollisionObject2DSW *co = (CollisionObject2DSW *)space->get_objects().front()->get(); - co->set_space(NULL); + co->set_space(nullptr); } active_spaces.erase(space); @@ -1299,25 +1200,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? @@ -1326,9 +1223,9 @@ void PhysicsServer2DSW::init() { }; void PhysicsServer2DSW::step(real_t p_step) { - - if (!active) + if (!active) { return; + } _update_shapes(); @@ -1340,7 +1237,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(); @@ -1349,21 +1245,19 @@ void PhysicsServer2DSW::step(real_t p_step) { }; void PhysicsServer2DSW::sync() { - doing_sync = true; }; void PhysicsServer2DSW::flush_queries() { - - if (!active) + if (!active) { return; + } flushing_queries = true; 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(); } @@ -1371,7 +1265,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", @@ -1386,7 +1279,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)); } @@ -1411,13 +1303,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()); @@ -1425,18 +1315,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; } @@ -1444,10 +1330,9 @@ int PhysicsServer2DSW::get_process_info(ProcessInfo p_info) { return 0; } -PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = NULL; +PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = nullptr; PhysicsServer2DSW::PhysicsServer2DSW() { - singletonsw = this; BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create; //BroadPhase2DSW::create_func=BroadPhase2DBasic::_create; @@ -1459,7 +1344,3 @@ PhysicsServer2DSW::PhysicsServer2DSW() { using_threads = int(ProjectSettings::get_singleton()->get("physics/2d/thread_model")) == 2; flushing_queries = false; }; - -PhysicsServer2DSW::~PhysicsServer2DSW(){ - -}; diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h index 9dfa8bf474..093c775cb5 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; @@ -249,7 +247,7 @@ public: virtual void body_set_pickable(RID p_body, bool p_pickable); - virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = NULL, bool p_exclude_raycast_shapes = true); + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true); virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001); // this function only works on physics process, errors and returns null otherwise @@ -268,8 +266,8 @@ public: virtual RID damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()); virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value); virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const; - virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value); - virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const; + virtual void damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value); + virtual real_t damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const; virtual JointType joint_get_type(RID p_joint) const; @@ -290,7 +288,7 @@ public: int get_process_info(ProcessInfo p_info); PhysicsServer2DSW(); - ~PhysicsServer2DSW(); + ~PhysicsServer2DSW() {} }; #endif diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.cpp b/servers/physics_2d/physics_server_2d_wrap_mt.cpp index 5f65e296fd..49c38c6ce0 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,62 +67,52 @@ 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) + if (first_frame) { first_frame = false; - else + } else { step_sem.wait(); //must not wait if a step was not issued + } } physics_2d_server->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); - thread = NULL; + thread = nullptr; } else { physics_2d_server->finish(); } @@ -147,10 +133,9 @@ 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 = NULL; + thread = nullptr; step_pending = 0; step_thread_up = false; @@ -167,7 +152,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 ffd46feebd..bc918b20f4 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(), NULL); + 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); } @@ -255,22 +250,19 @@ 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 = NULL, bool p_exclude_raycast_shapes = true) { - + 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(), NULL); + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); return physics_2d_server->body_get_direct_state(p_body); } @@ -295,8 +287,8 @@ public: FUNC3(pin_joint_set_param, RID, PinJointParam, real_t); FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam); - FUNC3(damped_string_joint_set_param, RID, DampedStringParam, real_t); - FUNC2RC(real_t, damped_string_joint_get_param, RID, DampedStringParam); + FUNC3(damped_spring_joint_set_param, RID, DampedSpringParam, real_t); + FUNC2RC(real_t, damped_spring_joint_get_param, RID, DampedSpringParam); FUNC1RC(JointType, joint_get_type, RID); @@ -325,14 +317,14 @@ public: template <class T> static PhysicsServer2D *init_server() { - int tm = GLOBAL_DEF("physics/2d/thread_model", 1); - if (tm == 0) // single unsafe + if (tm == 0) { // single unsafe return memnew(T); - else if (tm == 1) // single safe + } else if (tm == 1) { // single safe return memnew(PhysicsServer2DWrapMT(memnew(T), false)); - else // multi threaded + } else { // multi threaded return memnew(PhysicsServer2DWrapMT(memnew(T), true)); + } } #undef ServerNameWrapMT diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp index 5fefb9595f..87e22ef1c9 100644 --- a/servers/physics_2d/shape_2d_sw.cpp +++ b/servers/physics_2d/shape_2d_sw.cpp @@ -30,7 +30,7 @@ #include "shape_2d_sw.h" -#include "core/math/geometry.h" +#include "core/math/geometry_2d.h" #include "core/sort_array.h" void Shape2DSW::configure(const Rect2 &p_aabb) { @@ -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,32 +144,28 @@ 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) + if (p_normal.y > 0) { *r_supports = Vector2(0, length); - else + } else { *r_supports = Vector2(); + } } 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 +173,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 +184,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; @@ -212,22 +192,22 @@ void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports } real_t dp = p_normal.dot(b - a); - if (dp > 0) + if (dp > 0) { *r_supports = b; - else + } else { *r_supports = a; + } r_amount = 1; } 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)) + if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) { return false; + } if (n.dot(p_begin) > n.dot(a)) { r_normal = n; @@ -239,12 +219,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; @@ -255,15 +233,16 @@ void SegmentShape2DSW::set_data(const Variant &p_data) { Rect2 aabb; aabb.position = a; aabb.expand_to(b); - if (aabb.size.x == 0) + if (aabb.size.x == 0) { aabb.size.x = 0.001; - if (aabb.size.y == 0) + } + if (aabb.size.y == 0) { aabb.size.y = 0.001; + } configure(aabb); } Variant SegmentShape2DSW::get_data() const { - Rect2 r; r.position = a; r.size = b; @@ -275,18 +254,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; @@ -297,8 +273,9 @@ bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p real_t sqrtterm = b * b - 4 * a * c; - if (sqrtterm < 0) + if (sqrtterm < 0) { return false; + } sqrtterm = Math::sqrt(sqrtterm); real_t res = (-b - sqrtterm) / (2 * a); @@ -312,21 +289,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,14 +309,13 @@ 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); - if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) + if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { continue; + } real_t sgn = dp > 0 ? 1.0 : -1.0; @@ -374,18 +347,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 +363,6 @@ void RectangleShape2DSW::set_data(const Variant &p_data) { } Variant RectangleShape2DSW::get_data() const { - return half_extents; } @@ -402,13 +371,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 +388,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,25 +398,23 @@ 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; - if (p.y < 0) + if (p.y < 0) { p.y = 0; + } return p.length_squared() < radius * radius; } 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; @@ -467,8 +431,9 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 & real_t sqrtterm = b * b - 4 * a * c; - if (sqrtterm < 0) + if (sqrtterm < 0) { continue; + } sqrtterm = Math::sqrt(sqrtterm); real_t res = (-b - sqrtterm) / (2 * a); @@ -490,7 +455,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 +469,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 +482,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 +492,6 @@ void CapsuleShape2DSW::set_data(const Variant &p_data) { } Variant CapsuleShape2DSW::get_data() const { - return Point2(height, radius); } @@ -540,12 +500,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 +513,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,30 +527,27 @@ 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) + if (d > 0) { out = true; - else + } else { in = true; + } } return in != out; } 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) @@ -602,12 +556,12 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec Vector2 res; - if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) + if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) { continue; + } real_t nd = n.dot(res); if (nd < d) { - d = nd; r_point = res; r_normal = points[i].normal; @@ -616,9 +570,9 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec } if (inters) { - - if (n.dot(r_normal) > 0) + if (n.dot(r_normal) > 0) { r_normal = -r_normal; + } } //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); @@ -626,11 +580,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,12 +590,12 @@ 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) + if (points) { memdelete_arr(points); - points = NULL; + } + points = nullptr; point_count = 0; if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { @@ -658,13 +610,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 +623,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]; @@ -685,14 +634,14 @@ void ConvexPolygonShape2DSW::set_data(const Variant &p_data) { ERR_FAIL_COND(point_count == 0); Rect2 aabb; aabb.position = points[0].pos; - for (int i = 1; i < point_count; i++) + for (int i = 1; i < point_count; i++) { aabb.expand_to(points[i].pos); + } configure(aabb); } Variant ConvexPolygonShape2DSW::get_data() const { - Vector<Vector2> dvr; dvr.resize(point_count); @@ -705,25 +654,22 @@ Variant ConvexPolygonShape2DSW::get_data() const { } ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() { - - points = NULL; + points = nullptr; point_count = 0; } ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() { - - if (points) + if (points) { memdelete_arr(points); + } } ////////////////////////////////////////////////// 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 +683,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,34 +717,27 @@ 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]]; Vector2 res; - if (Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &res)) { - + if (Geometry2D::segment_intersects_segment(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,54 +748,50 @@ 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; - } else + } else { level--; + } } continue; } - if (done) + if (done) { break; + } } if (inters) { - - if (n.dot(r_normal) > 0) + if (n.dot(r_normal) > 0) { r_normal = -r_normal; + } } return inters; } 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 +805,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 +829,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 +852,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 +879,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 +886,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; @@ -974,14 +900,13 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { configure(aabb); } -Variant ConcavePolygonShape2DSW::get_data() const { +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 +915,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 +945,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,32 +966,29 @@ 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) + if (level == 0) { return; - else + } else { level--; + } } continue; } diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h index 48f3bea1e1..eca284f7a4 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,12 @@ 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++) + 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 +115,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 +142,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 +153,6 @@ public: } class LineShape2DSW : public Shape2DSW { - Vector2 normal; real_t d; @@ -195,7 +190,6 @@ public: }; class RayShape2DSW : public Shape2DSW { - real_t length; bool slips_on_slope; @@ -220,7 +214,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 +225,6 @@ public: }; class SegmentShape2DSW : public Shape2DSW { - Vector2 a; Vector2 b; Vector2 n; @@ -245,7 +237,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 +254,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 +269,6 @@ public: }; class CircleShape2DSW : public Shape2DSW { - real_t radius; public: @@ -313,7 +302,6 @@ public: }; class RectangleShape2DSW : public Shape2DSW { - Vector2 half_extents; public: @@ -336,18 +324,18 @@ 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) + if (d > r_max) { r_max = d; - if (d < r_min) + } + if (d < r_min) { r_min = d; + } } } _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 +346,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 +374,6 @@ public: }; class CapsuleShape2DSW : public Shape2DSW { - real_t radius; real_t height; @@ -419,7 +405,6 @@ public: r_min = p_normal.dot(p_transform.xform(-n)); if (r_max < r_min) { - SWAP(r_max, r_min); } @@ -430,9 +415,7 @@ public: }; class ConvexPolygonShape2DSW : public Shape2DSW { - struct Point { - Vector2 pos; Vector2 normal; //normal to next segment }; @@ -445,7 +428,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,16 +447,20 @@ 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 { - // no matter the angle, the box is mirrored anyway + if (!points || point_count <= 0) { + r_min = r_max = 0; + return; + } 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) + if (d > r_max) { r_max = d; - if (d < r_min) + } + if (d < r_min) { r_min = d; + } } } @@ -485,7 +471,6 @@ public: }; class ConcaveShape2DSW : public Shape2DSW { - public: virtual bool is_concave() const { return true; } typedef void (*Callback)(void *p_userdata, Shape2DSW *p_convex); @@ -494,9 +479,7 @@ public: }; class ConcavePolygonShape2DSW : public ConcaveShape2DSW { - struct Segment { - int points[2]; }; @@ -504,7 +487,6 @@ class ConcavePolygonShape2DSW : public ConcaveShape2DSW { Vector<Point2> points; struct BVH { - Rect2 aabb; int left, right; }; @@ -513,17 +495,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 cba190995f..f4a21da254 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -35,24 +35,25 @@ #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; } - if (p_object->get_type() == CollisionObject2DSW::TYPE_AREA && !p_collide_with_areas) + if (p_object->get_type() == CollisionObject2DSW::TYPE_AREA && !p_collide_with_areas) { return false; + } - if (p_object->get_type() == CollisionObject2DSW::TYPE_BODY && !p_collide_with_bodies) + if (p_object->get_type() == CollisionObject2DSW::TYPE_BODY && !p_collide_with_bodies) { return false; + } return true; } 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) + if (p_result_max <= 0) { return 0; + } Rect2 aabb; aabb.position = p_point - Vector2(0.00001, 0.00001); @@ -63,20 +64,23 @@ 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)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { continue; + } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { continue; + } const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; - if (p_pick_point && !col_obj->is_pickable()) + if (p_pick_point && !col_obj->is_pickable()) { continue; + } - if (p_filter_by_canvas && col_obj->get_canvas_instance_id() != p_canvas_instance_id) + if (p_filter_by_canvas && col_obj->get_canvas_instance_id() != p_canvas_instance_id) { continue; + } int shape_idx = space->intersection_query_subindex_results[i]; @@ -84,15 +88,18 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point); - if (!shape->contains_point(local_point)) + if (!shape->contains_point(local_point)) { continue; + } - if (cc >= p_result_max) + if (cc >= p_result_max) { continue; + } r_results[cc].collider_id = col_obj->get_instance_id(); - if (r_results[cc].collider_id.is_valid()) + if (r_results[cc].collider_id.is_valid()) { r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } r_results[cc].rid = col_obj->get_self(); r_results[cc].shape = shape_idx; r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx); @@ -104,17 +111,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,12 +138,13 @@ 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)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { continue; + } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { continue; + } const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; @@ -160,14 +165,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(); @@ -178,12 +181,14 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec } } - if (!collided) + if (!collided) { return false; + } r_result.collider_id = res_obj->get_instance_id(); - if (r_result.collider_id.is_valid()) + if (r_result.collider_id.is_valid()) { r_result.collider = ObjectDB::get_instance(r_result.collider_id); + } r_result.normal = res_normal; r_result.metadata = res_obj->get_shape_metadata(res_shape); r_result.position = res_point; @@ -194,9 +199,9 @@ 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) + if (p_result_max <= 0) { return 0; + } Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -209,25 +214,29 @@ 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) + if (cc >= p_result_max) { break; + } - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { continue; + } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { continue; + } const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), NULL, NULL, NULL, p_margin)) + if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) { continue; + } r_results[cc].collider_id = col_obj->get_instance_id(); - if (r_results[cc].collider_id.is_valid()) + if (r_results[cc].collider_id.is_valid()) { r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } r_results[cc].rid = col_obj->get_self(); r_results[cc].shape = shape_idx; r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx); @@ -239,7 +248,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,25 +261,25 @@ 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)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { continue; + } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { continue; //ignore excluded + } const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? - if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, p_margin)) { + if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { continue; } //test initial overlap - if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, p_margin)) { - + if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { return false; } @@ -285,13 +293,11 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor real_t ofs = (low + hi) * 0.5; Vector2 sep = mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, &sep, p_margin); + 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,9 +315,9 @@ 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 0; + if (p_result_max <= 0) { + return false; + } Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -335,20 +341,21 @@ 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)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { continue; + } const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - if (p_exclude.has(col_obj->get_self())) + if (p_exclude.has(col_obj->get_self())) { continue; + } cbk.valid_dir = Vector2(); cbk.valid_depth = 0; - if (CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) { + if (CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_margin)) { collided = cbk.amount > 0; } } @@ -359,7 +366,6 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D & } struct _RestCallbackData2D { - const CollisionObject2DSW *object; const CollisionObject2DSW *best_object; int local_shape; @@ -375,24 +381,27 @@ struct _RestCallbackData2D { }; static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { - _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata; if (rd->valid_dir != Vector2()) { - if (p_point_A.distance_squared_to(p_point_B) > rd->valid_depth * rd->valid_depth) + if (p_point_A.distance_squared_to(p_point_B) > rd->valid_depth * rd->valid_depth) { return; - if (rd->valid_dir.dot((p_point_A - p_point_B).normalized()) < Math_PI * 0.25) + } + if (rd->valid_dir.dot((p_point_A - p_point_B).normalized()) < Math_PI * 0.25) { return; + } } Vector2 contact_rel = p_point_B - p_point_A; real_t len = contact_rel.length(); - if (len < rd->min_allowed_depth) + if (len < rd->min_allowed_depth) { return; + } - if (len <= rd->best_len) + if (len <= rd->best_len) { return; + } rd->best_len = len; rd->best_contact = p_point_B; @@ -403,7 +412,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); @@ -415,33 +423,36 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh _RestCallbackData2D rcd; rcd.best_len = 0; - rcd.best_object = NULL; + rcd.best_object = nullptr; rcd.best_shape = 0; 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)) + if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { continue; + } const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - if (p_exclude.has(col_obj->get_self())) + if (p_exclude.has(col_obj->get_self())) { continue; + } rcd.valid_dir = Vector2(); rcd.valid_depth = 0; rcd.object = col_obj; rcd.shape = shape_idx; rcd.local_shape = 0; - bool sc = CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin); - if (!sc) + bool sc = CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin); + if (!sc) { continue; + } } - if (rcd.best_len == 0 || !rcd.best_object) + if (rcd.best_len == 0 || !rcd.best_object) { return false; + } r_info->collider_id = rcd.best_object->get_instance_id(); r_info->shape = rcd.best_shape; @@ -450,7 +461,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,33 +473,30 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh } PhysicsDirectSpaceState2DSW::PhysicsDirectSpaceState2DSW() { - - space = NULL; + 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) + if (intersection_query_results[i] == p_body) { keep = false; - else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) + } else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) { keep = false; - else if ((static_cast<Body2DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) + } else if ((static_cast<Body2DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) { keep = false; - else if (static_cast<Body2DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) + } else if (static_cast<Body2DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { keep = false; - else if (static_cast<Body2DSW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) + } else if (static_cast<Body2DSW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) { 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,18 +511,18 @@ 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)) + if (p_body->is_shape_set_as_disabled(i)) { continue; + } - if (p_body->get_shape(i)->get_type() != PhysicsServer2D::SHAPE_RAY) + if (p_body->get_shape(i)->get_type() != PhysicsServer2D::SHAPE_RAY) { continue; + } if (!shapes_found) { body_aabb = p_body->get_shape_aabb(i); @@ -554,7 +561,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; @@ -562,18 +568,19 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t int amount = _cull_aabb_for_body(p_body, body_aabb); for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) + if (p_body->is_shape_set_as_disabled(j)) { continue; + } Shape2DSW *body_shape = p_body->get_shape(j); - if (body_shape->get_type() != PhysicsServer2D::SHAPE_RAY) + if (body_shape->get_type() != PhysicsServer2D::SHAPE_RAY) { continue; + } Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); for (int i = 0; i < amount; i++) { - const CollisionObject2DSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; @@ -613,7 +620,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t */ Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, NULL, p_margin)) { + if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_margin)) { if (cbk.amount > 0) { collided = true; } @@ -631,7 +638,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 +648,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 +693,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,12 +709,13 @@ 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)) + if (p_body->is_shape_set_as_disabled(i)) { continue; + } - if (p_exclude_raycast_shapes && p_body->get_shape(i)->get_type() == PhysicsServer2D::SHAPE_RAY) + if (p_exclude_raycast_shapes && p_body->get_shape(i)->get_type() == PhysicsServer2D::SHAPE_RAY) { continue; + } if (!shapes_found) { body_aabb = p_body->get_shape_aabb(i); @@ -748,7 +753,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; @@ -765,8 +769,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co int amount = _cull_aabb_for_body(p_body, body_aabb); for (int j = 0; j < p_body->get_shape_count(); j++) { - if (p_body->is_shape_set_as_disabled(j)) + if (p_body->is_shape_set_as_disabled(j)) { continue; + } Shape2DSW *body_shape = p_body->get_shape(j); if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { @@ -775,7 +780,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 +793,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); @@ -818,7 +821,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co bool did_collide = false; Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, NULL, separation_margin)) { + if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, separation_margin)) { did_collide = cbk.passed > current_passed; //more passed, so collision actually existed } @@ -846,7 +849,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,9 +881,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co int amount = _cull_aabb_for_body(p_body, motion_aabb); for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) { - - if (p_body->is_shape_set_as_disabled(body_shape_idx)) + if (p_body->is_shape_set_as_disabled(body_shape_idx)) { continue; + } Shape2DSW *body_shape = p_body->get_shape(body_shape_idx); if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer2D::SHAPE_RAY) { @@ -896,7 +898,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 +912,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,19 +919,17 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } if (excluded) { - continue; } Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx); //test initial overlap, does it collide if going all the way? - if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, against_shape, col_obj_shape_xform, Vector2(), NULL, NULL, NULL, 0)) { + if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { continue; } //test initial overlap - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), NULL, NULL, NULL, 0)) { - + 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; } @@ -950,19 +948,16 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co real_t ofs = (low + hi) * 0.5; Vector2 sep = mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_shape_xform, Vector2(), NULL, NULL, &sep, 0); + 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 +982,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 +991,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,14 +1004,13 @@ 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; _RestCallbackData2D rcd; rcd.best_len = 0; - rcd.best_object = NULL; + rcd.best_object = nullptr; rcd.best_shape = 0; rcd.min_allowed_depth = test_motion_min_contact_depth; @@ -1027,9 +1019,9 @@ 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)) + if (p_body->is_shape_set_as_disabled(j)) { continue; + } Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j); Shape2DSW *body_shape = p_body->get_shape(j); @@ -1043,7 +1035,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,19 +1049,18 @@ 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; } } - if (excluded) + if (excluded) { continue; + } 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 { @@ -1081,14 +1071,14 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co rcd.object = col_obj; rcd.shape = shape_idx; rcd.local_shape = j; - bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, NULL, p_margin); - if (!sc) + bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin); + if (!sc) { continue; + } } } 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 +1102,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 +1111,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,31 +1123,26 @@ 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; } - return NULL; + return nullptr; } 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 +1150,79 @@ 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) { +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) { +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) { +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) { +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 +1230,6 @@ void Space2DSW::call_queries() { } void Space2DSW::setup() { - contact_debug_count = 0; while (inertia_update_list.first()) { @@ -1273,63 +1239,77 @@ 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; - case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break; - case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break; - case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: body_linear_velocity_sleep_threshold = p_value; break; - case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: body_angular_velocity_sleep_threshold = p_value; break; - case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break; - case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break; - case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: test_motion_min_contact_depth = p_value; break; + case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + contact_recycle_radius = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + contact_max_separation = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + contact_max_allowed_penetration = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + body_linear_velocity_sleep_threshold = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + body_angular_velocity_sleep_threshold = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + body_time_to_sleep = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + constraint_bias = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: + test_motion_min_contact_depth = p_value; + break; } } 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: return contact_max_separation; - case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration; - case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: return body_linear_velocity_sleep_threshold; - case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: return body_angular_velocity_sleep_threshold; - case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep; - case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias; - case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: return test_motion_min_contact_depth; + case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + return contact_recycle_radius; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + return contact_max_separation; + case PhysicsServer2D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + return contact_max_allowed_penetration; + case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + return body_linear_velocity_sleep_threshold; + case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + return body_angular_velocity_sleep_threshold; + case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + return body_time_to_sleep; + case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + return constraint_bias; + case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: + return test_motion_min_contact_depth; } return 0; } 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; @@ -1351,17 +1331,17 @@ Space2DSW::Space2DSW() { broadphase = BroadPhase2DSW::create_func(); broadphase->set_pair_callback(_broadphase_pair, this); broadphase->set_unpair_callback(_broadphase_unpair, this); - area = NULL; + area = nullptr; direct_access = memnew(PhysicsDirectSpaceState2DSW); direct_access->space = this; - for (int i = 0; i < ELAPSED_TIME_MAX; i++) + for (int i = 0; i < ELAPSED_TIME_MAX; i++) { elapsed_time[i] = 0; + } } 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 c6b324c928..0a96f2f495 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, @@ -192,7 +190,9 @@ public: void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); } _FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) { - if (contact_debug_count < contact_debug.size()) contact_debug.write[contact_debug_count++] = p_contact; + if (contact_debug_count < contact_debug.size()) { + contact_debug.write[contact_debug_count++] = p_contact; + } } _FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; } _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 198fccdb80..c7711bcd1d 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -32,35 +32,35 @@ #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) + if (c->get_island_step() == _step) { continue; //already processed + } c->set_island_step(_step); c->set_island_next(*p_constraint_island); *p_constraint_island = c; for (int i = 0; i < c->get_body_count(); i++) { - if (i == E->get()) + if (i == E->get()) { continue; + } Body2DSW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) + if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { continue; //no go + } _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); } } } bool Step2DSW::_setup_island(Constraint2DSW *p_island, real_t p_delta) { - Constraint2DSW *ci = p_island; - Constraint2DSW *prev_ci = NULL; + Constraint2DSW *prev_ci = nullptr; bool removed_root = false; while (ci) { bool process = ci->setup(p_delta); @@ -83,9 +83,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,19 +93,18 @@ 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 } - if (!b->sleep_test(p_delta)) + if (!b->sleep_test(p_delta)) { can_sleep = false; + } b = b->get_island_next(); } @@ -116,7 +113,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 @@ -124,15 +120,15 @@ void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { bool active = b->is_active(); - if (active == can_sleep) + if (active == can_sleep) { b->set_active(!can_sleep); + } b = b->get_island_next(); } } 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 +144,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++; @@ -164,8 +159,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { /* GENERATE CONSTRAINT ISLANDS */ - Body2DSW *island_list = NULL; - Constraint2DSW *constraint_island_list = NULL; + Body2DSW *island_list = nullptr; + Constraint2DSW *constraint_island_list = nullptr; b = body_list->first(); int island_count = 0; @@ -174,9 +169,8 @@ 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 = NULL; - Constraint2DSW *constraint_island = NULL; + Body2DSW *island = nullptr; + Constraint2DSW *constraint_island = nullptr; _populate_island(body, &island, &constraint_island); island->set_island_list_next(island_list); @@ -197,12 +191,12 @@ 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) + if (c->get_island_step() == _step) { continue; + } c->set_island_step(_step); - c->set_island_next(NULL); + c->set_island_next(nullptr); c->set_island_list_next(constraint_island_list); constraint_island_list = c; } @@ -219,11 +213,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { { Constraint2DSW *ci = constraint_island_list; - Constraint2DSW *prev_ci = NULL; + 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 +230,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 +273,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 +283,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 +300,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); |