diff options
Diffstat (limited to 'servers/physics_3d')
20 files changed, 632 insertions, 321 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index de4399330d..98237dd91c 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -47,13 +47,15 @@ Area3DSW::BodyKey::BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_a } void Area3DSW::_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 Area3DSW::set_transform(const Transform &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()); @@ -61,10 +63,12 @@ void Area3DSW::set_transform(const Transform &p_transform) { void Area3DSW::set_space(Space3DSW *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(); @@ -89,8 +93,9 @@ void Area3DSW::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 Area3DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { @@ -109,14 +114,16 @@ void Area3DSW::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 Area3DSW::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode) { bool do_override = p_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override == (space_override_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) + if (do_override == (space_override_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) { return; + } _unregister_shapes(); space_override_mode = p_mode; _shape_changed(); @@ -177,13 +184,15 @@ Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const { void Area3DSW::_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 Area3DSW::set_monitorable(bool p_monitorable) { - if (monitorable == p_monitorable) + if (monitorable == p_monitorable) { return; + } monitorable = p_monitorable; _set_static(!monitorable); @@ -193,8 +202,9 @@ void Area3DSW::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) { @@ -204,8 +214,9 @@ void Area3DSW::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 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; res[1] = E->key().rid; @@ -223,8 +234,9 @@ void Area3DSW::call_queries() { 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) { @@ -234,8 +246,9 @@ void Area3DSW::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 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; res[1] = E->key().rid; diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h index d85a7fc509..6af3976167 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -71,10 +71,12 @@ class Area3DSW : public CollisionObject3DSW { 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() {} @@ -167,29 +169,33 @@ public: void Area3DSW::add_body_to_query(Body3DSW *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 Area3DSW::remove_body_from_query(Body3DSW *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 Area3DSW::add_area_to_query(Area3DSW *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 Area3DSW::remove_area_from_query(Area3DSW *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__SW_H diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index 36d460675e..a5fb20fe2b 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -42,16 +42,20 @@ bool AreaPair3DSW::setup(real_t p_step) { if (result != colliding) { if (result) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) + if (area->get_space_override_mode() != PhysicsServer3D::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() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) + if (area->get_space_override_mode() != PhysicsServer3D::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; @@ -71,16 +75,19 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, colliding = false; body->add_constraint(this, 0); area->add_constraint(this); - if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) + if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { p_body->set_active(true); + } } AreaPair3DSW::~AreaPair3DSW() { if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) + if (area->get_space_override_mode() != PhysicsServer3D::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); @@ -98,18 +105,22 @@ bool Area2Pair3DSW::setup(real_t p_step) { 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,11 +144,13 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area Area2Pair3DSW::~Area2Pair3DSW() { 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_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index c01e55c4dd..a3bdc96c9f 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -33,8 +33,9 @@ #include "space_3d_sw.h" void Body3DSW::_update_inertia() { - if (get_space() && !inertia_update_list.in_list()) + if (get_space() && !inertia_update_list.in_list()) { get_space()->body_add_to_inertia_update_list(&inertia_update_list); + } } void Body3DSW::_update_transform_dependant() { @@ -105,10 +106,11 @@ void Body3DSW::update_inertias() { principal_inertia_axes_local = inertia_tensor.diagonalize().transposed(); _inv_inertia = inertia_tensor.get_main_diagonal().inverse(); - if (mass) + if (mass) { _inv_mass = 1.0 / mass; - else + } else { _inv_mass = 0; + } } break; @@ -130,18 +132,22 @@ void Body3DSW::update_inertias() { } void Body3DSW::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 == PhysicsServer3D::BODY_MODE_STATIC) + if (mode == PhysicsServer3D::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; } @@ -284,8 +290,9 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va Transform t = p_variant; t.orthonormalize(); new_transform = get_transform(); //used as old to compute motion - if (new_transform == t) + if (new_transform == t) { break; + } _set_transform(t); _set_inv_transform(get_transform().inverse()); } @@ -311,8 +318,9 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va } break; case PhysicsServer3D::BODY_STATE_SLEEPING: { //? - if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) + if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { break; + } bool do_sleep = p_variant; if (do_sleep) { linear_velocity = Vector3(); @@ -326,8 +334,9 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va } break; case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode == PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) + if (mode == PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) { set_active(true); + } } break; } @@ -357,20 +366,24 @@ Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const { void Body3DSW::set_space(Space3DSW *p_space) { if (get_space()) { - 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()) { @@ -412,8 +425,9 @@ bool Body3DSW::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const { } void Body3DSW::integrate_forces(real_t p_step) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC) + if (mode == PhysicsServer3D::BODY_MODE_STATIC) { return; + } Area3DSW *def_area = get_space()->get_default_area(); // AreaSW *damp_area = def_area; @@ -458,15 +472,17 @@ void Body3DSW::integrate_forces(real_t p_step) { gravity *= gravity_scale; // If less than 0, override dampenings with that of the Body - 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(); @@ -501,13 +517,15 @@ void Body3DSW::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; @@ -540,11 +558,13 @@ void Body3DSW::integrate_forces(real_t p_step) { } void Body3DSW::integrate_velocities(real_t p_step) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC) + if (mode == PhysicsServer3D::BODY_MODE_STATIC) { return; + } - if (fi_callback) + if (fi_callback) { get_space()->body_add_to_state_query_list(&direct_state_query_list); + } //apply axis lock linear for (int i = 0; i < 3; i++) { @@ -565,8 +585,9 @@ void Body3DSW::integrate_velocities(real_t p_step) { if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { _set_transform(new_transform, false); _set_inv_transform(new_transform.affine_inverse()); - if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3()) + if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3()) { set_active(false); //stopped moving, deactivate + } return; } @@ -647,14 +668,17 @@ void Body3DSW::wakeup_neighbours() { int bc = c->get_body_count(); for (int i = 0; i < bc; i++) { - if (i == E->get()) + if (i == E->get()) { continue; + } Body3DSW *b = n[i]; - if (b->mode != PhysicsServer3D::BODY_MODE_RIGID) + if (b->mode != PhysicsServer3D::BODY_MODE_RIGID) { continue; + } - if (!b->is_active()) + if (!b->is_active()) { b->set_active(true); + } } } } @@ -680,12 +704,13 @@ void Body3DSW::call_queries() { } bool Body3DSW::sleep_test(real_t p_step) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) + if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { return true; // - else if (mode == PhysicsServer3D::BODY_MODE_CHARACTER) + } else if (mode == PhysicsServer3D::BODY_MODE_CHARACTER) { return !active; // characters don't sleep unless asked to sleep - else if (!can_sleep) + } else if (!can_sleep) { return false; + } if (Math::abs(angular_velocity.length()) < 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; @@ -753,8 +778,9 @@ Body3DSW::Body3DSW() : } Body3DSW::~Body3DSW() { - if (fi_callback) + if (fi_callback) { memdelete(fi_callback); + } } PhysicsDirectBodyState3DSW *PhysicsDirectBodyState3DSW::singleton = nullptr; diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index 2ce8f99428..483ea58620 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -163,16 +163,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 == PhysicsServer3D::BODY_MODE_KINEMATIC && p_size) + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && p_size) { set_active(true); + } } _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } @@ -259,8 +261,9 @@ public: _FORCE_INLINE_ bool is_active() const { return active; } _FORCE_INLINE_ void wakeup() { - if ((!get_space()) || mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) + if ((!get_space()) || mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { return; + } set_active(true); } @@ -332,8 +335,9 @@ public: void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) { int c_max = contacts.size(); - if (c_max == 0) + if (c_max == 0) { return; + } Contact *c = contacts.ptrw(); @@ -354,8 +358,9 @@ void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &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; diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index e7c6738698..a4f86badbe 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -162,8 +162,9 @@ void BodyPair3DSW::validate_contacts() { bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B) { Vector3 motion = p_A->get_linear_velocity() * p_step; real_t mlen = motion.length(); - if (mlen < CMP_EPSILON) + if (mlen < CMP_EPSILON) { return false; + } Vector3 mnormal = motion / mlen; @@ -257,12 +258,13 @@ bool BodyPair3DSW::setup(real_t p_step) { real_t bias = (real_t)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; + } } real_t inv_dt = 1.0 / p_step; @@ -338,13 +340,15 @@ bool BodyPair3DSW::setup(real_t p_step) { } void BodyPair3DSW::solve(real_t p_step) { - if (!collided) + if (!collided) { return; + } for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - if (!c.active) + if (!c.active) { continue; + } c.active = false; //try to deactivate, will activate itself if still needed diff --git a/servers/physics_3d/broad_phase_3d_basic.cpp b/servers/physics_3d/broad_phase_3d_basic.cpp index 68bf0da53e..0f271b33af 100644 --- a/servers/physics_3d/broad_phase_3d_basic.cpp +++ b/servers/physics_3d/broad_phase_3d_basic.cpp @@ -107,8 +107,9 @@ int BroadPhase3DBasic::cull_point(const Vector3 &p_point, CollisionObject3DSW ** 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; + } } } @@ -124,8 +125,9 @@ int BroadPhase3DBasic::cull_segment(const Vector3 &p_from, const Vector3 &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; + } } } @@ -141,8 +143,9 @@ int BroadPhase3DBasic::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_res 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; + } } } @@ -166,8 +169,9 @@ void BroadPhase3DBasic::update() { 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); @@ -176,15 +180,17 @@ void BroadPhase3DBasic::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 = nullptr; - if (pair_callback) + 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); } } diff --git a/servers/physics_3d/broad_phase_octree.cpp b/servers/physics_3d/broad_phase_octree.cpp index ffa1802bc8..1ace1a4fcf 100644 --- a/servers/physics_3d/broad_phase_octree.cpp +++ b/servers/physics_3d/broad_phase_octree.cpp @@ -77,16 +77,18 @@ int BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_resu void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B) { BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); - if (!bpo->pair_callback) + if (!bpo->pair_callback) { return nullptr; + } return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); } void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) { BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self); - if (!bpo->unpair_callback) + if (!bpo->unpair_callback) { return; + } bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); } diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp index 22a8235744..e12f0659e2 100644 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -95,8 +95,9 @@ void CollisionObject3DSW::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; @@ -112,12 +113,14 @@ void CollisionObject3DSW::remove_shape(int p_index) { } void CollisionObject3DSW::_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) { @@ -137,8 +140,9 @@ void CollisionObject3DSW::_unregister_shapes() { } void CollisionObject3DSW::_update_shapes() { - if (!space) + if (!space) { return; + } for (int i = 0; i < shapes.size(); i++) { Shape &s = shapes.write[i]; @@ -162,8 +166,9 @@ void CollisionObject3DSW::_update_shapes() { } void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) { - if (!space) + if (!space) { return; + } for (int i = 0; i < shapes.size(); i++) { Shape &s = shapes.write[i]; diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 0c320b3275..9506f14402 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -91,8 +91,9 @@ protected: #endif transform = p_transform; - if (p_update_shapes) + if (p_update_shapes) { _update_shapes(); + } } _FORCE_INLINE_ void _set_inv_transform(const Transform &p_transform) { inv_transform = p_transform; } void _set_static(bool p_static); diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index b4902af8ec..736222c7d9 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -42,10 +42,11 @@ struct _CollectorCallback { Vector3 *prev_axis; _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { - if (swap) + if (swap) { callback(p_point_B, p_point_A, userdata); - else + } else { callback(p_point_A, p_point_B, userdata); + } } }; @@ -116,10 +117,11 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_ real_t d = (c.dot(p_points_B[0]) - p_points_A[0].dot(c)) / rel_A.dot(c); - if (d < 0.0) + if (d < 0.0) { d = 0.0; - else if (d > 1.0) + } else if (d > 1.0) { d = 1.0; + } Vector3 closest_A = p_points_A[0] + rel_A * d; Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B); @@ -207,8 +209,9 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d; - if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) + if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) { continue; + } p_callback->call(clipbuf_src[i], closest_B); } @@ -282,10 +285,11 @@ class SeparatorAxisTest { public: _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) + if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) { return test_axis(*callback->prev_axis); - else + } else { return true; + } } _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { @@ -344,14 +348,16 @@ public: _FORCE_INLINE_ void generate_contacts() { // nothing to do, don't generate - if (best_axis == Vector3(0.0, 0.0, 0.0)) + if (best_axis == Vector3(0.0, 0.0, 0.0)) { return; + } if (!callback->callback) { //just was checking intersection? callback->collided = true; - if (callback->prev_axis) + if (callback->prev_axis) { *callback->prev_axis = best_axis; + } return; } @@ -384,8 +390,9 @@ public: } callback->normal = best_axis; - if (callback->prev_axis) + if (callback->prev_axis) { *callback->prev_axis = best_axis; + } _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); callback->collided = true; @@ -416,11 +423,13 @@ static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_tr // previous axis - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } - if (!separator.test_axis((p_transform_a.origin - p_transform_b.origin).normalized())) + if (!separator.test_axis((p_transform_a.origin - p_transform_b.origin).normalized())) { return; + } separator.generate_contacts(); } @@ -432,16 +441,18 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans SeparatorAxisTest<SphereShape3DSW, BoxShape3DSW, withMargin> separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } // test faces for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // calculate closest point to sphere @@ -457,16 +468,18 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans // use point to test axis Vector3 point_axis = (p_transform_a.origin - cpoint).normalized(); - if (!separator.test_axis(point_axis)) + if (!separator.test_axis(point_axis)) { return; + } // test edges for (int i = 0; i < 3; i++) { Vector3 axis = point_axis.cross(p_transform_b.basis.get_axis(i)).cross(p_transform_b.basis.get_axis(i)).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } separator.generate_contacts(); @@ -479,8 +492,9 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t SeparatorAxisTest<SphereShape3DSW, CapsuleShape3DSW, withMargin> separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } //capsule sphere 1, sphere @@ -488,15 +502,17 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; - if (!separator.test_axis((capsule_ball_1 - p_transform_a.origin).normalized())) + if (!separator.test_axis((capsule_ball_1 - p_transform_a.origin).normalized())) { return; + } //capsule sphere 2, sphere Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis; - if (!separator.test_axis((capsule_ball_2 - p_transform_a.origin).normalized())) + if (!separator.test_axis((capsule_ball_2 - p_transform_a.origin).normalized())) { return; + } //capsule edge, sphere @@ -504,8 +520,9 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t Vector3 axis = b2a.cross(capsule_axis).cross(capsule_axis).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } separator.generate_contacts(); } @@ -521,8 +538,9 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo SeparatorAxisTest<SphereShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); @@ -537,8 +555,9 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo for (int i = 0; i < face_count; i++) { Vector3 axis = p_transform_b.xform(faces[i].plane).normal; - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // edges of B @@ -552,8 +571,9 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo Vector3 axis = n1.cross(n2).cross(n1).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // vertices of B @@ -563,8 +583,9 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo Vector3 axis = (v2 - v1).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } separator.generate_contacts(); @@ -583,8 +604,9 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { return; + } // edges and points of B for (int i = 0; i < 3; i++) { @@ -613,16 +635,18 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor SeparatorAxisTest<BoxShape3DSW, BoxShape3DSW, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } // test faces of A for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // test faces of B @@ -630,8 +654,9 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // test combined edges @@ -639,8 +664,9 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor for (int j = 0; j < 3; j++) { Vector3 axis = p_transform_a.basis.get_axis(i).cross(p_transform_b.basis.get_axis(j)); - if (Math::is_zero_approx(axis.length_squared())) + if (Math::is_zero_approx(axis.length_squared())) { continue; + } axis.normalize(); if (!separator.test_axis(axis)) { @@ -684,14 +710,16 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); - if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { return; + } //b ->a Vector3 axis_b = p_transform_b.basis.get_axis(i); - if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) + if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) { return; + } } } @@ -705,15 +733,17 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran SeparatorAxisTest<BoxShape3DSW, CapsuleShape3DSW, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } // faces of A for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_a.basis.get_axis(i); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } Vector3 cyl_axis = p_transform_b.basis.get_axis(2).normalized(); @@ -724,11 +754,13 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // cylinder Vector3 box_axis = p_transform_a.basis.get_axis(i); Vector3 axis = box_axis.cross(cyl_axis); - if (Math::is_zero_approx(axis.length_squared())) + if (Math::is_zero_approx(axis.length_squared())) { continue; + } - if (!separator.test_axis(axis.normalized())) + if (!separator.test_axis(axis.normalized())) { return; + } } // points of A, capsule cylinder @@ -742,14 +774,16 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran he.y *= (j * 2 - 1); he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; - for (int l = 0; l < 3; l++) + for (int l = 0; l < 3; l++) { point += p_transform_a.basis.get_axis(l) * he[l]; + } //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); Vector3 axis = Plane(cyl_axis, 0).project(point).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } } @@ -772,16 +806,18 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // use point to test axis Vector3 point_axis = (sphere_pos - cpoint).normalized(); - if (!separator.test_axis(point_axis)) + if (!separator.test_axis(point_axis)) { return; + } // test edges of A for (int j = 0; j < 3; j++) { Vector3 axis = point_axis.cross(p_transform_a.basis.get_axis(j)).cross(p_transform_a.basis.get_axis(j)).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } @@ -799,8 +835,9 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform SeparatorAxisTest<BoxShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); @@ -815,16 +852,18 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // faces of B for (int i = 0; i < face_count; i++) { Vector3 axis = p_transform_b.xform(faces[i].plane).normal; - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // A<->B edges @@ -836,8 +875,9 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } @@ -867,8 +907,9 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); - if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { return; + } } } @@ -881,16 +922,18 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform he.y *= (j * 2 - 1); he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; - for (int l = 0; l < 3; l++) + for (int l = 0; l < 3; l++) { point += p_transform_a.basis.get_axis(l) * he[l]; + } for (int e = 0; e < edge_count; e++) { Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]); Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]); Vector3 n = (p2 - p1); - if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) + if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) { return; + } } } } @@ -913,15 +956,17 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { return; + } // faces of A for (int i = 0; i < 3; i++) { Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // combined edges @@ -932,8 +977,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo for (int j = 0; j < 3; j++) { Vector3 axis = p_transform_a.basis.get_axis(j); - if (!separator.test_axis(e.cross(axis).normalized())) + if (!separator.test_axis(e.cross(axis).normalized())) { return; + } } } @@ -962,8 +1008,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo //a ->b Vector3 axis_a = p_transform_a.basis.get_axis(i); - if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { return; + } } } @@ -976,8 +1023,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo he.y *= (j * 2 - 1); he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; - for (int l = 0; l < 3; l++) + for (int l = 0; l < 3; l++) { point += p_transform_a.basis.get_axis(l) * he[l]; + } for (int e = 0; e < 3; e++) { Vector3 p1 = vertex[e]; @@ -985,8 +1033,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo Vector3 n = (p2 - p1); - if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) + if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) { return; + } } } } @@ -1003,8 +1052,9 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_ SeparatorAxisTest<CapsuleShape3DSW, CapsuleShape3DSW, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } // some values @@ -1018,34 +1068,43 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_ //balls-balls - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).normalized())) + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).normalized())) { return; - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).normalized())) + } + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).normalized())) { return; + } - if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_1).normalized())) + if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_1).normalized())) { return; - if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_2).normalized())) + } + if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_2).normalized())) { return; + } // edges-balls - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) { return; + } - if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) + if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).cross(capsule_A_axis).cross(capsule_A_axis).normalized())) { return; + } - if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_1).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) + if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_1).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) { return; + } - if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_2).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) + if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_2).cross(capsule_B_axis).cross(capsule_B_axis).normalized())) { return; + } // edges - if (!separator.test_axis(capsule_A_axis.cross(capsule_B_axis).normalized())) + if (!separator.test_axis(capsule_A_axis.cross(capsule_B_axis).normalized())) { return; + } separator.generate_contacts(); } @@ -1061,8 +1120,9 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf SeparatorAxisTest<CapsuleShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); @@ -1076,8 +1136,9 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf for (int i = 0; i < face_count; i++) { Vector3 axis = p_transform_b.xform(faces[i].plane).normal; - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // edges of B, capsule cylinder @@ -1087,8 +1148,9 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]); Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(2)).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // capsule balls, edges of B @@ -1106,8 +1168,9 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf Vector3 axis = n1.cross(n2).cross(n2).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } @@ -1127,8 +1190,9 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { return; + } // edges of B, capsule cylinder @@ -1139,11 +1203,13 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; Vector3 axis = edge_axis.cross(capsule_axis).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } - if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized())) + if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized())) { return; + } for (int j = 0; j < 2; j++) { // point-spheres @@ -1151,15 +1217,17 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra Vector3 n1 = sphere_pos - vertex[i]; - if (!separator.test_axis(n1.normalized())) + if (!separator.test_axis(n1.normalized())) { return; + } Vector3 n2 = edge_axis; axis = n1.cross(n2).cross(n2); - if (!separator.test_axis(axis.normalized())) + if (!separator.test_axis(axis.normalized())) { return; + } } } @@ -1185,8 +1253,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const SeparatorAxisTest<ConvexPolygonShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - if (!separator.test_previous_axis()) + if (!separator.test_previous_axis()) { return; + } const Geometry::MeshData &mesh_A = convex_polygon_A->get_mesh(); @@ -1211,8 +1280,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal; //Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // faces of B @@ -1220,8 +1290,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal; //Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // A<->B edges @@ -1233,8 +1304,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } @@ -1244,8 +1316,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Vector3 va = p_transform_a.xform(vertices_A[i]); for (int j = 0; j < vertex_count_B; j++) { - if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) + if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) { return; + } } } //edge-vertex (shell) @@ -1258,8 +1331,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const for (int j = 0; j < vertex_count_B; j++) { Vector3 e3 = p_transform_b.xform(vertices_B[j]); - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { return; + } } } @@ -1271,8 +1345,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const for (int j = 0; j < vertex_count_A; j++) { Vector3 e3 = p_transform_a.xform(vertices_A[j]); - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { return; + } } } } @@ -1302,16 +1377,18 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform p_transform_b.xform(face_B->vertex[2]), }; - if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { return; + } // faces of A for (int i = 0; i < face_count; i++) { //Vector3 axis = p_transform_a.xform( faces[i].plane ).normal; Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } // A<->B edges @@ -1323,8 +1400,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } @@ -1334,8 +1412,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform Vector3 va = p_transform_a.xform(vertices[i]); for (int j = 0; j < 3; j++) { - if (!separator.test_axis((va - vertex[j]).normalized())) + if (!separator.test_axis((va - vertex[j]).normalized())) { return; + } } } //edge-vertex (shell) @@ -1348,8 +1427,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform for (int j = 0; j < 3; j++) { Vector3 e3 = vertex[j]; - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { return; + } } } @@ -1361,8 +1441,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform for (int j = 0; j < vertex_count; j++) { Vector3 e3 = p_transform_a.xform(vertices[j]); - if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { return; + } } } } diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index 43e62ee452..e2bfaf990d 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -38,8 +38,9 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { return false; + } Plane p = p_transform_A.xform(plane->get_plane()); static const int max_supports = 16; @@ -52,17 +53,19 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T for (int i = 0; i < support_count; i++) { supports[i] = p_transform_B.xform(supports[i]); - if (p.distance_to(supports[i]) >= 0) + if (p.distance_to(supports[i]) >= 0) { continue; + } found = true; Vector3 support_A = p.project(supports[i]); 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); + } } } @@ -82,8 +85,9 @@ bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform to = ai.xform(to); Vector3 p, n; - if (!p_shape_B->intersect_segment(from, to, p, n)) + if (!p_shape_B->intersect_segment(from, to, p, n)) { return false; + } Vector3 support_B = p_transform_B.xform(p); if (ray->get_slips_on_slope()) { @@ -92,10 +96,11 @@ bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform } 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; } @@ -121,8 +126,9 @@ void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex cinfo.aabb_tests++; bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, nullptr, cinfo.margin_A, cinfo.margin_B); - if (!collided) + if (!collided) { return; + } cinfo.collided = true; cinfo.collisions++; @@ -187,8 +193,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } if (type_A == PhysicsServer3D::SHAPE_PLANE) { - if (type_B == PhysicsServer3D::SHAPE_PLANE) + if (type_B == PhysicsServer3D::SHAPE_PLANE) { return false; + } if (type_B == PhysicsServer3D::SHAPE_RAY) { return false; } @@ -200,8 +207,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } } else if (type_A == PhysicsServer3D::SHAPE_RAY) { - if (type_B == PhysicsServer3D::SHAPE_RAY) + if (type_B == PhysicsServer3D::SHAPE_RAY) { return false; + } if (swap) { return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); @@ -210,13 +218,15 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } } 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_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A, p_margin_B); - else + } else { return solve_concave(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A, p_margin_B); + } } else { return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B); @@ -226,14 +236,16 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) { _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); cinfo.aabb_tests++; - if (cinfo.collided) + if (cinfo.collided) { return; + } Vector3 close_A, close_B; cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B); - if (cinfo.collided) + if (cinfo.collided) { return; + } if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { cinfo.close_A = close_A; cinfo.close_B = close_B; @@ -245,8 +257,9 @@ void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A); - if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { return false; + } Plane p = p_transform_A.xform(plane->get_plane()); static const int max_supports = 16; @@ -265,8 +278,9 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const if (i == 0 || d < closest_d) { closest = supports[i]; closest_d = d; - if (d <= 0) + if (d <= 0) { collided = true; + } } } @@ -277,8 +291,9 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const } bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { - if (p_shape_A->is_concave()) + if (p_shape_A->is_concave()) { return false; + } if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) { Vector3 a, b; @@ -288,8 +303,9 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans return !col; } else if (p_shape_B->is_concave()) { - if (p_shape_A->is_concave()) + if (p_shape_A->is_concave()) { return false; + } const ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(p_shape_B); diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index 7f7fc0f55b..d99a2532f8 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -123,10 +123,12 @@ struct MinkowskiDiff { _FORCE_INLINE_ Vector3 Support ( const Vector3& d,U index ) const { - if ( index ) + if ( index ) { return ( Support1 ( d ) ); - else + } else { return ( Support0 ( d ) ); + +} } }; @@ -278,7 +280,9 @@ struct GJK m_free[m_nfree++] = cs.c[i]; } } - if(mask==15) m_status=eStatus::Inside; + if(mask==15) { m_status=eStatus::Inside; + +} } else {/* Return old simplex */ @@ -307,10 +311,14 @@ struct GJK Vector3 axis=Vector3(0,0,0); axis[i]=1; appendvertice(*m_simplex, axis); - if(EncloseOrigin()) return(true); + if(EncloseOrigin()) { return(true); + +} removevertice(*m_simplex); appendvertice(*m_simplex,-axis); - if(EncloseOrigin()) return(true); + if(EncloseOrigin()) { return(true); + +} removevertice(*m_simplex); } } @@ -326,10 +334,14 @@ struct GJK if(p.length_squared()>0) { appendvertice(*m_simplex, p); - if(EncloseOrigin()) return(true); + if(EncloseOrigin()) { return(true); + +} removevertice(*m_simplex); appendvertice(*m_simplex,-p); - if(EncloseOrigin()) return(true); + if(EncloseOrigin()) { return(true); + +} removevertice(*m_simplex); } } @@ -342,10 +354,14 @@ struct GJK if(n.length_squared()>0) { appendvertice(*m_simplex,n); - if(EncloseOrigin()) return(true); + if(EncloseOrigin()) { return(true); + +} removevertice(*m_simplex); appendvertice(*m_simplex,-n); - if(EncloseOrigin()) return(true); + if(EncloseOrigin()) { return(true); + +} removevertice(*m_simplex); } } @@ -354,8 +370,10 @@ struct GJK { if(Math::abs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, m_simplex->c[1]->w-m_simplex->c[3]->w, - m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) { return(true); + +} } break; } @@ -561,15 +579,23 @@ struct GJK { face->l[0] = nullptr; face->l[1] = list.root; - if(list.root) list.root->l[0]=face; + if(list.root) { list.root->l[0]=face; + +} list.root = face; ++list.count; } static inline void remove(sList& list,sFace* face) { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; + if(face->l[1]) { face->l[1]->l[0]=face->l[0]; + +} + if(face->l[0]) { face->l[0]->l[1]=face->l[1]; + +} + if(face==list.root) { list.root=face->l[1]; + +} --list.count; } @@ -650,7 +676,9 @@ struct GJK remove(m_hull,best); append(m_stock,best); best=findbest(); - if(best->p>=outer.p) outer=*best; + if(best->p>=outer.p) { outer=*best; + +} } else { m_status=eStatus::InvalidHull;break; } } else { m_status=eStatus::AccuraryReached;break; } } else { m_status=eStatus::OutOfVertices;break; } @@ -679,10 +707,12 @@ struct GJK m_status = eStatus::FallBack; m_normal = -guess; const real_t nl=m_normal.length(); - if(nl>0) + if(nl>0) { m_normal = m_normal/nl; - else + } else { m_normal = Vector3(1,0,0); + +} m_depth = 0; m_result.rank=1; m_result.c[0]=simplex.c[0]; @@ -716,8 +746,12 @@ struct GJK if(forced||(face->d>=-EPA_PLANE_EPS)) { return(face); - } else m_status=eStatus::NonConvex; - } else m_status=eStatus::Degenerated; + } else { m_status=eStatus::NonConvex; + +} + } else { m_status=eStatus::Degenerated; + +} remove(m_hull,face); append(m_stock,face); return(nullptr); @@ -758,7 +792,9 @@ struct GJK if(nf) { bind(nf,0,f,e); - if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + if(horizon.cf) { bind(horizon.cf,1,nf,2); } else { horizon.ff=nf; + +} horizon.cf=nf; ++horizon.nf; return(true); @@ -880,7 +916,9 @@ bool Penetration( const Shape3DSW* shape0, results.normal = -epa.m_normal; results.distance = -epa.m_depth; return(true); - } else results.status=sResults::EPA_Failed; + } else { results.status=sResults::EPA_Failed; + +} } break; case GJK::eStatus::Failed: @@ -933,10 +971,11 @@ bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform & if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { if (p_result_callback) { - if (p_swap) + if (p_swap) { p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata); - else + } else { p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata); + } } return true; } diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp index 97310a5924..423bbc0dfd 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -83,8 +83,9 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) { real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1) { - if (!needApplyTorques()) + if (!needApplyTorques()) { return 0.0f; + } real_t target_velocity = m_targetVelocity; real_t maxMotorForce = m_maxMotorForce; @@ -138,8 +139,9 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( Vector3 motorImp = clippedMotorImpulse * axis; body0->apply_torque_impulse(motorImp); - if (body1) + if (body1) { body1->apply_torque_impulse(-motorImp); + } return clippedMotorImpulse; } @@ -325,10 +327,11 @@ bool Generic6DOFJoint3DSW::setup(real_t p_timestep) { //linear part for (i = 0; i < 3; i++) { if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { - if (m_useLinearReferenceFrameA) + if (m_useLinearReferenceFrameA) { normalWorld = m_calculatedTransformA.basis.get_axis(i); - else + } else { normalWorld = m_calculatedTransformB.basis.get_axis(i); + } buildLinearJacobian( m_jacLinear[i], normalWorld, @@ -367,10 +370,11 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) { if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); - if (m_useLinearReferenceFrameA) + if (m_useLinearReferenceFrameA) { linear_axis = m_calculatedTransformA.basis.get_axis(i); - else + } else { linear_axis = m_calculatedTransformB.basis.get_axis(i); + } m_linearLimits.solveLinearAxis( m_timeStep, diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index 3f3c0f367d..230904408b 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -109,10 +109,12 @@ void PinJoint3DSW::solve(real_t p_step) { real_t impulseClamp = m_impulseClamp; if (impulseClamp > 0) { - if (impulse < -impulseClamp) + if (impulse < -impulseClamp) { impulse = -impulseClamp; - if (impulse > impulseClamp) + } + if (impulse > impulseClamp) { impulse = impulseClamp; + } } m_appliedImpulse += impulse; diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index d05001e636..b625ae2271 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -144,10 +144,11 @@ RID PhysicsServer3DSW::space_create() { void PhysicsServer3DSW::space_set_active(RID p_space, bool p_active) { Space3DSW *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 PhysicsServer3DSW::space_is_active(RID p_space) const { @@ -213,8 +214,9 @@ void PhysicsServer3DSW::area_set_space(RID p_area, RID 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); @@ -225,8 +227,9 @@ RID PhysicsServer3DSW::area_get_space(RID p_area) const { ERR_FAIL_COND_V(!area, RID()); Space3DSW *space = area->get_space(); - if (!space) + if (!space) { return RID(); + } return space->get_self(); }; @@ -307,8 +310,9 @@ void PhysicsServer3DSW::area_clear_shapes(RID p_area) { Area3DSW *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 PhysicsServer3DSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { @@ -427,10 +431,12 @@ void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_rec RID PhysicsServer3DSW::body_create(BodyMode p_mode, bool p_init_sleeping) { Body3DSW *body = memnew(Body3DSW); - if (p_mode != BODY_MODE_RIGID) + if (p_mode != BODY_MODE_RIGID) { body->set_mode(p_mode); - if (p_init_sleeping) + } + if (p_init_sleeping) { body->set_state(BODY_STATE_SLEEPING, p_init_sleeping); + } RID rid = body_owner.make_rid(body); body->set_self(rid); return rid; @@ -446,8 +452,9 @@ void PhysicsServer3DSW::body_set_space(RID p_body, RID 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); @@ -458,8 +465,9 @@ RID PhysicsServer3DSW::body_get_space(RID p_body) const { ERR_FAIL_COND_V(!body, RID()); Space3DSW *space = body->get_space(); - if (!space) + if (!space) { return RID(); + } return space->get_self(); }; @@ -549,8 +557,9 @@ void PhysicsServer3DSW::body_clear_shapes(RID p_body) { Body3DSW *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 PhysicsServer3DSW::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { @@ -1288,8 +1297,9 @@ void PhysicsServer3DSW::init() { void PhysicsServer3DSW::step(real_t p_step) { #ifndef _3D_DISABLED - if (!active) + if (!active) { return; + } _update_shapes(); @@ -1317,8 +1327,9 @@ void PhysicsServer3DSW::sync(){ void PhysicsServer3DSW::flush_queries() { #ifndef _3D_DISABLED - if (!active) + if (!active) { return; + } doing_sync = true; @@ -1399,8 +1410,9 @@ void PhysicsServer3DSW::_update_shapes() { void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { CollCbkData *cbk = (CollCbkData *)p_userdata; - if (cbk->max == 0) + if (cbk->max == 0) { return; + } if (cbk->amount == cbk->max) { //find least deep @@ -1415,8 +1427,9 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 & } 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; diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index a44244f7d5..eb0e87cec0 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -105,8 +105,9 @@ Vector3 PlaneShape3DSW::get_support(const Vector3 &p_normal) const { bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { bool inters = plane.intersects_segment(p_begin, p_end, &r_result); - if (inters) + if (inters) { r_normal = plane.normal; + } return inters; } @@ -159,10 +160,11 @@ void RayShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_tra } Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const { - if (p_normal.z > 0) + if (p_normal.z > 0) { return Vector3(0, 0, length); - else + } else { return Vector3(0, 0, 0); + } } void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { @@ -260,8 +262,9 @@ bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const { Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 p = p_point; float l = p.length(); - if (l < radius) + if (l < radius) { return p_point; + } return (p / l) * radius; } @@ -418,11 +421,13 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } } - if (!outside) + if (!outside) { return p_point; //it's inside, don't do anything else + } - if (outside == 1) //if only above one plane, this plane clearly wins + if (outside == 1) { //if only above one plane, this plane clearly wins return min_point; + } //check segments float min_distance = 1e20; @@ -597,8 +602,9 @@ Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 p = Geometry::get_closest_point_to_segment(p_point, s); - if (p.distance_to(p_point) < radius) + if (p.distance_to(p_point) < radius) { return p_point; + } return p + (p_point - p).normalized() * radius; } @@ -641,18 +647,21 @@ CapsuleShape3DSW::CapsuleShape3DSW() { void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { int vertex_count = mesh.vertices.size(); - if (vertex_count == 0) + if (vertex_count == 0) { return; + } const Vector3 *vrts = &mesh.vertices[0]; for (int i = 0; i < vertex_count; i++) { real_t d = p_normal.dot(p_transform.xform(vrts[i])); - if (i == 0 || d > r_max) + if (i == 0 || d > r_max) { r_max = d; - if (i == 0 || d < r_min) + } + if (i == 0 || d < r_min) { r_min = d; + } } } @@ -663,8 +672,9 @@ Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const { real_t support_max = 0; int vertex_count = mesh.vertices.size(); - if (vertex_count == 0) + if (vertex_count == 0) { return Vector3(); + } const Vector3 *vrts = &mesh.vertices[0]; @@ -716,8 +726,9 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve } } - if (!valid) + if (!valid) { continue; + } int m = MIN(p_max, ic); for (int j = 0; j < m; j++) { @@ -754,8 +765,9 @@ bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vec bool col = false; for (int i = 0; i < fc; i++) { - if (faces[i].plane.normal.dot(n) > 0) + if (faces[i].plane.normal.dot(n) > 0) { continue; //opposing face + } int ic = faces[i].indices.size(); const int *ind = faces[i].indices.ptr(); @@ -785,8 +797,9 @@ bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const { int fc = mesh.faces.size(); for (int i = 0; i < fc; i++) { - if (faces[i].plane.distance_to(p_point) >= 0) + if (faces[i].plane.distance_to(p_point) >= 0) { return false; + } } return true; @@ -799,8 +812,9 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con bool all_inside = true; for (int i = 0; i < fc; i++) { - if (!faces[i].plane.is_point_over(p_point)) + if (!faces[i].plane.is_point_over(p_point)) { continue; + } all_inside = false; bool is_inside = true; @@ -861,16 +875,18 @@ Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const { void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) { Error err = QuickHull::build(p_vertices, mesh); - if (err != OK) + if (err != OK) { ERR_PRINT("Failed to build QuickHull"); + } AABB _aabb; for (int i = 0; i < mesh.vertices.size(); i++) { - if (i == 0) + if (i == 0) { _aabb.position = mesh.vertices[i]; - else + } else { _aabb.expand_to(mesh.vertices[i]); + } } configure(_aabb); @@ -894,11 +910,13 @@ void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_tr Vector3 v = p_transform.xform(vertex[i]); real_t d = p_normal.dot(v); - if (i == 0 || d > r_max) + if (i == 0 || d > r_max) { r_max = d; + } - if (i == 0 || d < r_min) + if (i == 0 || d < r_min) { r_min = d; + } } } @@ -948,8 +966,9 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ for (int i = 0; i < 3; i++) { int nx = (i + 1) % 3; - if (i != vert_support_idx && nx != vert_support_idx) + if (i != vert_support_idx && nx != vert_support_idx) { continue; + } // check if edge is valid as a support real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); @@ -1021,17 +1040,20 @@ void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Trans for (int i = 0; i < count; i++) { real_t d = p_normal.dot(p_transform.xform(vptr[i])); - if (i == 0 || d > r_max) + if (i == 0 || d > r_max) { r_max = d; - if (i == 0 || d < r_min) + } + if (i == 0 || d < r_min) { r_min = d; + } } } Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const { int count = vertices.size(); - if (count == 0) + if (count == 0) { return Vector3(); + } const Vector3 *vptr = vertices.ptr(); @@ -1091,16 +1113,19 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par } } else { - if (bvh->left >= 0) + if (bvh->left >= 0) { _cull_segment(bvh->left, p_params); - if (bvh->right >= 0) + } + if (bvh->right >= 0) { _cull_segment(bvh->right, p_params); + } } } bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - if (faces.size() == 0) + if (faces.size() == 0) { return false; + } // unlock data const Face *fr = faces.ptr(); @@ -1141,8 +1166,9 @@ Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) co void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { const BVH *bvh = &p_params->bvh[p_idx]; - if (!p_params->aabb.intersects(bvh->aabb)) + if (!p_params->aabb.intersects(bvh->aabb)) { return; + } if (bvh->face_index >= 0) { const Face *f = &p_params->faces[bvh->face_index]; @@ -1166,8 +1192,9 @@ void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const { // make matrix local to concave - if (faces.size() == 0) + if (faces.size() == 0) { return; + } AABB local_aabb = p_local_aabb; @@ -1250,10 +1277,11 @@ _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_siz AABB aabb; for (int i = 0; i < p_size; i++) { - if (i == 0) + if (i == 0) { aabb = p_elements[i].aabb; - else + } else { aabb.merge_with(p_elements[i].aabb); + } } bvh->aabb = aabb; switch (aabb.get_longest_axis_index()) { @@ -1345,10 +1373,11 @@ void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) { verticesw[i * 3 + 0] = face.vertex[0]; verticesw[i * 3 + 1] = face.vertex[1]; verticesw[i * 3 + 2] = face.vertex[2]; - if (i == 0) + if (i == 0) { _aabb = bvh_arrayw[i].aabb; - else + } else { _aabb.merge_with(bvh_arrayw[i].aabb); + } } int count = 0; @@ -1443,10 +1472,11 @@ void HeightMapShape3DSW::_setup(Vector<real_t> p_heights, int p_width, int p_dep real_t h = r[i * width + j]; Vector3 pos(j * cell_size, h, i * cell_size); - if (i == 0 || j == 0) + if (i == 0 || j == 0) { aabb.position = pos; - else + } else { aabb.expand_to(pos); + } } } diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 066945d808..4d272bdabd 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -39,11 +39,13 @@ _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint return false; } - if (p_object->get_type() == CollisionObject3DSW::TYPE_AREA && !p_collide_with_areas) + if (p_object->get_type() == CollisionObject3DSW::TYPE_AREA && !p_collide_with_areas) { return false; + } - if (p_object->get_type() == CollisionObject3DSW::TYPE_BODY && !p_collide_with_bodies) + if (p_object->get_type() == CollisionObject3DSW::TYPE_BODY && !p_collide_with_bodies) { return false; + } return true; } @@ -56,16 +58,19 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe //Transform ai = p_xform.affine_inverse(); 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; + } //area can't be picked by ray (default) - if (p_exclude.has(space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { continue; + } const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; @@ -73,14 +78,16 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe Transform inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); inv_xform.affine_invert(); - if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) + if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) { 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); - else + } else { r_results[cc].collider = nullptr; + } r_results[cc].rid = col_obj->get_self(); r_results[cc].shape = shape_idx; @@ -110,14 +117,17 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &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_pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) + if (p_pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) { 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 CollisionObject3DSW *col_obj = space->intersection_query_results[i]; @@ -148,14 +158,16 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &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); - else + } else { r_result.collider = nullptr; + } r_result.normal = res_normal; r_result.position = res_point; r_result.rid = res_obj->get_self(); @@ -165,8 +177,9 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec } int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Transform &p_xform, 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; + } Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -180,29 +193,34 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans //Transform ai = p_xform.affine_inverse(); 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; + } //area can't be picked by ray (default) - if (p_exclude.has(space->intersection_query_results[i]->get_self())) + if (p_exclude.has(space->intersection_query_results[i]->get_self())) { continue; + } const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - if (!CollisionSolver3DSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) + if (!CollisionSolver3DSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) { continue; + } if (r_results) { 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); - else + } else { r_results[cc].collider = nullptr; + } r_results[cc].rid = col_obj->get_self(); r_results[cc].shape = shape_idx; } @@ -236,11 +254,13 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor Vector3 closest_A, closest_B; 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 CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; @@ -316,8 +336,9 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor } bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *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) + if (p_result_max <= 0) { return false; + } Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -339,8 +360,9 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ PhysicsServer3DSW::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 CollisionObject3DSW *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; @@ -375,10 +397,12 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, Vector3 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,24 +427,28 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap 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 CollisionObject3DSW *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.object = col_obj; rcd.shape = shape_idx; bool sc = CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); - if (!sc) + 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; @@ -454,8 +482,9 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob bool shapes_found = false; for (int i = 0; i < obj->get_shape_count(); i++) { - if (obj->is_shape_set_as_disabled(i)) + if (obj->is_shape_set_as_disabled(i)) { continue; + } Transform shape_xform = obj->get_transform() * obj->get_shape_transform(i); Shape3DSW *shape = obj->get_shape(i); @@ -490,16 +519,17 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { 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() == CollisionObject3DSW::TYPE_AREA) + } else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_AREA) { keep = false; - else if ((static_cast<Body3DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) + } else if ((static_cast<Body3DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) { keep = false; - else if (static_cast<Body3DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) + } else if (static_cast<Body3DSW *>(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<Body3DSW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) + } else if (static_cast<Body3DSW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) { keep = false; + } if (!keep) { if (i < amount - 1) { @@ -521,8 +551,9 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra 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 (!shapes_found) { body_aabb = p_body->get_shape_aabb(i); @@ -567,13 +598,15 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra 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; + } Shape3DSW *body_shape = p_body->get_shape(j); - if (body_shape->get_type() != PhysicsServer3D::SHAPE_RAY) + if (body_shape->get_type() != PhysicsServer3D::SHAPE_RAY) { continue; + } Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); @@ -681,8 +714,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons 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 (!shapes_found) { body_aabb = p_body->get_shape_aabb(i); @@ -728,8 +762,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons 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; + } Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); @@ -786,8 +821,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons int amount = _cull_aabb_for_body(p_body, motion_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; + } Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); Shape3DSW *body_shape = p_body->get_shape(j); @@ -910,8 +946,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons rcd.object = col_obj; rcd.shape = shape_idx; bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); - if (!sc) + if (!sc) { continue; + } } if (rcd.best_len != 0) { @@ -1179,8 +1216,9 @@ Space3DSW::Space3DSW() { direct_access = memnew(PhysicsDirectSpaceState3DSW); 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; + } } Space3DSW::~Space3DSW() { diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index c8b1ad32c1..f9198e3d40 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -185,8 +185,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 Vector3 &p_contact) { - if (contact_debug_count < contact_debug.size()) + if (contact_debug_count < contact_debug.size()) { contact_debug.write[contact_debug_count++] = p_contact; + } } _FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contact_debug; } _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index 5c84eb05d0..9a2a0073a1 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -40,18 +40,21 @@ void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constrain for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { Constraint3DSW *c = (Constraint3DSW *)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; + } Body3DSW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) + if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { continue; //no go + } _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); } } @@ -110,8 +113,9 @@ void Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) { continue; //ignore for static } - if (!b->sleep_test(p_delta)) + if (!b->sleep_test(p_delta)) { can_sleep = false; + } b = b->get_island_next(); } @@ -127,8 +131,9 @@ void Step3DSW::_check_suspend(Body3DSW *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(); } @@ -198,8 +203,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { while (aml.first()) { for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { Constraint3DSW *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(nullptr); c->set_island_list_next(constraint_island_list); |