diff options
Diffstat (limited to 'servers/physics_3d')
38 files changed, 702 insertions, 1205 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index 2367b653a5..98237dd91c 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -38,6 +38,7 @@ Area3DSW::BodyKey::BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_a body_shape = p_body_shape; area_shape = p_area_shape; } + Area3DSW::BodyKey::BodyKey(Area3DSW *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 @@ 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()); } 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(); @@ -76,7 +78,6 @@ void Area3DSW::set_space(Space3DSW *p_space) { } void Area3DSW::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 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) { - if (p_id == area_monitor_callback_id) { area_monitor_callback_method = p_method; return; @@ -113,21 +114,22 @@ 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(); } void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) { - switch (p_param) { case PhysicsServer3D::AREA_PARAM_GRAVITY: gravity = p_value; @@ -157,7 +159,6 @@ void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant & } Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const { - switch (p_param) { case PhysicsServer3D::AREA_PARAM_GRAVITY: return gravity; @@ -181,30 +182,29 @@ 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); } 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) { @@ -214,9 +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; @@ -232,11 +232,11 @@ void Area3DSW::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) { @@ -246,9 +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; @@ -269,7 +269,6 @@ Area3DSW::Area3DSW() : CollisionObject3DSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { - _set_static(true); //areas are never active space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; gravity = 9.80665; diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h index 05e74e63dc..6af3976167 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -41,7 +41,6 @@ class Body3DSW; class Constraint3DSW; class Area3DSW : public CollisionObject3DSW { - PhysicsServer3D::AreaSpaceOverrideMode space_override_mode; real_t gravity; Vector3 gravity_vector; @@ -63,23 +62,21 @@ class Area3DSW : public CollisionObject3DSW { SelfList<Area3DSW> 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 Area3DSW : public CollisionObject3DSW { }; struct BodyState { - int state; _FORCE_INLINE_ void inc() { state++; } _FORCE_INLINE_ void dec() { state--; } @@ -171,33 +167,35 @@ 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) { +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) { +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 fa2fb2dabb..a5fb20fe2b 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -32,7 +32,6 @@ #include "collision_solver_3d_sw.h" bool AreaPair3DSW::setup(real_t p_step) { - bool result = false; if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) { @@ -42,20 +41,21 @@ 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; @@ -68,7 +68,6 @@ void AreaPair3DSW::solve(real_t p_step) { } AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) { - body = p_body; area = p_area; body_shape = p_body_shape; @@ -76,18 +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); @@ -96,7 +96,6 @@ AreaPair3DSW::~AreaPair3DSW() { //////////////////////////////////////////////////// bool Area2Pair3DSW::setup(real_t p_step) { - bool result = false; if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) { result = false; @@ -105,22 +104,23 @@ 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,7 +133,6 @@ void Area2Pair3DSW::solve(real_t p_step) { } Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *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 @@ 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/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h index 3490f41c26..992d4747b9 100644 --- a/servers/physics_3d/area_pair_3d_sw.h +++ b/servers/physics_3d/area_pair_3d_sw.h @@ -36,7 +36,6 @@ #include "constraint_3d_sw.h" class AreaPair3DSW : public Constraint3DSW { - Body3DSW *body; Area3DSW *area; int body_shape; @@ -52,7 +51,6 @@ public: }; class Area2Pair3DSW : public Constraint3DSW { - Area3DSW *area_a; Area3DSW *area_b; int shape_a; diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index fea5aed6ad..a3bdc96c9f 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -33,13 +33,12 @@ #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() { - center_of_mass = get_transform().basis.xform(center_of_mass_local); principal_inertia_axes = get_transform().basis * principal_inertia_axes_local; @@ -52,18 +51,14 @@ void Body3DSW::_update_transform_dependant() { } void Body3DSW::update_inertias() { - //update shapes and motions switch (mode) { - case PhysicsServer3D::BODY_MODE_RIGID: { - //update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet) real_t total_area = 0; for (int i = 0; i < get_shape_count(); i++) { - total_area += get_shape_area(i); } @@ -86,7 +81,6 @@ void Body3DSW::update_inertias() { inertia_tensor.set_zero(); for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { continue; } @@ -112,21 +106,20 @@ 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; case PhysicsServer3D::BODY_MODE_KINEMATIC: case PhysicsServer3D::BODY_MODE_STATIC: { - _inv_inertia_tensor.set_zero(); _inv_mass = 0; } break; case PhysicsServer3D::BODY_MODE_CHARACTER: { - _inv_inertia_tensor.set_zero(); _inv_mass = 1.0 / mass; @@ -139,19 +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; } @@ -169,14 +165,11 @@ void Body3DSW::set_active(bool p_active) { } void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) { - switch (p_param) { case PhysicsServer3D::BODY_PARAM_BOUNCE: { - bounce = p_value; } break; case PhysicsServer3D::BODY_PARAM_FRICTION: { - friction = p_value; } break; case PhysicsServer3D::BODY_PARAM_MASS: { @@ -189,11 +182,9 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) gravity_scale = p_value; } break; case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { - linear_damp = p_value; } break; case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { - angular_damp = p_value; } break; default: { @@ -202,14 +193,11 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) } real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const { - switch (p_param) { case PhysicsServer3D::BODY_PARAM_BOUNCE: { - return bounce; } break; case PhysicsServer3D::BODY_PARAM_FRICTION: { - return friction; } break; case PhysicsServer3D::BODY_PARAM_MASS: { @@ -219,11 +207,9 @@ real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const { return gravity_scale; } break; case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { - return linear_damp; } break; case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { - return angular_damp; } break; @@ -235,7 +221,6 @@ real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const { } void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { - PhysicsServer3D::BodyMode prev = mode; mode = p_mode; @@ -243,7 +228,6 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! case PhysicsServer3D::BODY_MODE_STATIC: case PhysicsServer3D::BODY_MODE_KINEMATIC: { - _set_inv_transform(get_transform().affine_inverse()); _inv_mass = 0; _set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC); @@ -257,14 +241,12 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { } break; case PhysicsServer3D::BODY_MODE_RIGID: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); set_active(true); } break; case PhysicsServer3D::BODY_MODE_CHARACTER: { - _inv_mass = mass > 0 ? (1.0 / mass) : 0; _set_static(false); set_active(true); @@ -278,21 +260,18 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { _update_queries(); */ } -PhysicsServer3D::BodyMode Body3DSW::get_mode() const { +PhysicsServer3D::BodyMode Body3DSW::get_mode() const { return mode; } void Body3DSW::_shapes_changed() { - _update_inertia(); } void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { - switch (p_state) { case PhysicsServer3D::BODY_STATE_TRANSFORM: { - if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { new_transform = p_variant; //wakeup_neighbours(); @@ -311,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()); } @@ -320,7 +300,6 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va } break; case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { - /* if (mode==PhysicsServer3D::BODY_MODE_STATIC) break; @@ -339,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(); @@ -354,14 +334,15 @@ 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; } } -Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const { +Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const { switch (p_state) { case PhysicsServer3D::BODY_STATE_TRANSFORM: { return get_transform(); @@ -384,24 +365,25 @@ 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()) { @@ -415,7 +397,6 @@ void Body3DSW::set_space(Space3DSW *p_space) { } void Body3DSW::_compute_area_gravity_and_dampenings(const Area3DSW *p_area) { - if (p_area->is_gravity_point()) { if (p_area->get_gravity_distance_scale() > 0) { Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin(); @@ -444,9 +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; @@ -491,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(); @@ -509,7 +492,6 @@ void Body3DSW::integrate_forces(real_t p_step) { bool do_motion = false; if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { - //compute motion, angular and etc. velocities from prev transform linear_velocity = (new_transform.origin - get_transform().origin) / p_step; @@ -535,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; @@ -574,12 +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++) { @@ -598,11 +583,11 @@ 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; } @@ -673,34 +658,33 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { } + */ void Body3DSW::wakeup_neighbours() { - for (Map<Constraint3DSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) { - const Constraint3DSW *c = E->key(); Body3DSW **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; + } 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); + } } } } void Body3DSW::call_queries() { - if (fi_callback) { - PhysicsDirectBodyState3DSW *dbs = PhysicsDirectBodyState3DSW::singleton; dbs->body = this; @@ -708,7 +692,6 @@ void Body3DSW::call_queries() { Object *obj = ObjectDB::get_instance(fi_callback->id); if (!obj) { - set_force_integration_callback(ObjectID(), StringName()); } else { const Variant *vp[2] = { &v, &fi_callback->udata }; @@ -721,36 +704,31 @@ 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; 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 Body3DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { - if (fi_callback) { - memdelete(fi_callback); fi_callback = nullptr; } if (p_id.is_valid()) { - fi_callback = memnew(ForceIntegrationCallback); fi_callback->id = p_id; fi_callback->method = p_method; @@ -764,11 +742,10 @@ void Body3DSW::set_kinematic_margin(real_t p_margin) { Body3DSW::Body3DSW() : CollisionObject3DSW(TYPE_BODY), - locked_axis(0), + active_list(this), inertia_update_list(this), direct_state_query_list(this) { - mode = PhysicsServer3D::BODY_MODE_RIGID; active = true; @@ -801,14 +778,13 @@ Body3DSW::Body3DSW() : } Body3DSW::~Body3DSW() { - - if (fi_callback) + if (fi_callback) { memdelete(fi_callback); + } } PhysicsDirectBodyState3DSW *PhysicsDirectBodyState3DSW::singleton = nullptr; PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() { - return body->get_space()->get_direct_state(); } diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index a67894b64b..483ea58620 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -38,7 +38,6 @@ class Constraint3DSW; class Body3DSW : public CollisionObject3DSW { - PhysicsServer3D::BodyMode mode; Vector3 linear_velocity; @@ -54,7 +53,7 @@ class Body3DSW : public CollisionObject3DSW { real_t angular_damp; real_t gravity_scale; - uint16_t locked_axis; + uint16_t locked_axis = 0; real_t kinematic_safe_margin; real_t _inv_mass; @@ -99,7 +98,6 @@ class Body3DSW : public CollisionObject3DSW { Map<Constraint3DSW *, int> constraint_map; struct AreaCMP { - Area3DSW *area; int refCount; _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } @@ -114,7 +112,6 @@ class Body3DSW : public CollisionObject3DSW { Vector<AreaCMP> areas; struct Contact { - Vector3 local_pos; Vector3 local_normal; real_t depth; @@ -130,7 +127,6 @@ class Body3DSW : public CollisionObject3DSW { int contact_count; struct ForceIntegrationCallback { - ObjectID id; StringName method; Variant udata; @@ -167,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(); } @@ -223,18 +221,15 @@ public: } _FORCE_INLINE_ void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { - linear_velocity += p_j * _inv_mass; angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); } _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_j) { - angular_velocity += _inv_inertia_tensor.xform(p_j); } _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_pos, const Vector3 &p_j, real_t p_max_delta_av = -1.0) { - biased_linear_velocity += p_j * _inv_mass; if (p_max_delta_av != 0.0) { Vector3 delta_av = _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j)); @@ -246,17 +241,14 @@ public: } _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_j) { - biased_angular_velocity += _inv_inertia_tensor.xform(p_j); } _FORCE_INLINE_ void add_central_force(const Vector3 &p_force) { - applied_force += p_force; } _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_pos) { - applied_force += p_force; applied_torque += (p_pos - center_of_mass).cross(p_force); } @@ -269,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); } @@ -310,12 +303,10 @@ public: void integrate_velocities(real_t p_step); _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3 &rel_pos) const { - return linear_velocity + angular_velocity.cross(rel_pos - center_of_mass); } _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3 &p_pos, const Vector3 &p_normal) const { - Vector3 r0 = p_pos - get_transform().origin - center_of_mass; Vector3 c0 = (r0).cross(p_normal); @@ -326,7 +317,6 @@ public: } _FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3 &p_axis) const { - return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis)); } @@ -343,11 +333,11 @@ public: //add contact inline 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(); @@ -356,11 +346,9 @@ void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &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; @@ -368,11 +356,11 @@ 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; @@ -387,7 +375,6 @@ void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_no } class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D { - GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D); public: diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 245fb3449c..a4f86badbe 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -50,13 +50,11 @@ #define MAX_BIAS_ROTATION (Math_PI / 8) void BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { - BodyPair3DSW *pair = (BodyPair3DSW *)p_userdata; pair->contact_added_callback(p_point_A, p_point_B); } void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B) { - // check if we already have the contact //Vector3 local_A = A->get_inv_transform().xform(p_point_A); @@ -84,11 +82,9 @@ void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector real_t 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) < (contact_recycle_radius * contact_recycle_radius) && c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { - contact.acc_normal_impulse = c.acc_normal_impulse; contact.acc_bias_impulse = c.acc_bias_impulse; contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; @@ -101,14 +97,12 @@ void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector // 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]; Vector3 global_A = A->get_transform().basis.xform(c.local_A); Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; @@ -117,7 +111,6 @@ void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector real_t depth = axis.dot(c.normal); if (depth < min_depth) { - min_depth = depth; least_deep = i; } @@ -136,18 +129,15 @@ void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector contacts[new_index] = contact; if (new_index == contact_count) { - contact_count++; } } void BodyPair3DSW::validate_contacts() { - //make sure to erase contacts that are no longer valid real_t contact_max_separation = space->get_contact_max_separation(); for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; Vector3 global_A = A->get_transform().basis.xform(c.local_A); @@ -170,11 +160,11 @@ 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; @@ -220,7 +210,6 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) { } bool BodyPair3DSW::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() <= PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) { collided = false; @@ -251,7 +240,6 @@ bool BodyPair3DSW::setup(real_t p_step) { this->collided = collided; if (!collided) { - //test ccd (currently just a raycast) if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) { @@ -270,19 +258,18 @@ 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; for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; c.active = false; @@ -341,7 +328,6 @@ bool BodyPair3DSW::setup(real_t p_step) { c.bounce = combine_bounce(A, B); if (c.bounce) { - Vector3 crA = A->get_angular_velocity().cross(c.rA); Vector3 crB = B->get_angular_velocity().cross(c.rB); Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; @@ -354,15 +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 @@ -375,7 +361,6 @@ void BodyPair3DSW::solve(real_t p_step) { real_t vbn = dbv.dot(c.normal); if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - real_t jbn = (-vbn + c.bias) * c.mass_normal; real_t jbnOld = c.acc_bias_impulse; c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); @@ -392,7 +377,6 @@ void BodyPair3DSW::solve(real_t p_step) { vbn = dbv.dot(c.normal); if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { - real_t jbn_com = (-vbn + c.bias) / (A->get_inv_mass() + B->get_inv_mass()); real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); @@ -414,7 +398,6 @@ void BodyPair3DSW::solve(real_t p_step) { real_t vn = dv.dot(c.normal); if (Math::abs(vn) > MIN_VELOCITY) { - real_t jn = -(c.bounce + vn) * c.mass_normal; real_t jnOld = c.acc_normal_impulse; c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); @@ -442,7 +425,6 @@ void BodyPair3DSW::solve(real_t p_step) { real_t tvl = tv.length(); if (tvl > MIN_VELOCITY) { - tv /= tvl; Vector3 temp1 = A->get_inv_inertia_tensor().xform(c.rA.cross(tv)); @@ -460,7 +442,6 @@ void BodyPair3DSW::solve(real_t p_step) { real_t jtMax = c.acc_normal_impulse * friction; if (fi_len > CMP_EPSILON && fi_len > jtMax) { - c.acc_tangent_impulse *= jtMax / fi_len; } @@ -476,7 +457,6 @@ void BodyPair3DSW::solve(real_t p_step) { BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B) : Constraint3DSW(_arr, 2) { - A = p_A; B = p_B; shape_A = p_shape_A; @@ -489,7 +469,6 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh } BodyPair3DSW::~BodyPair3DSW() { - A->remove_constraint(this); B->remove_constraint(this); } diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 7f4afb9dca..59e36e7ea5 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -53,7 +53,6 @@ class BodyPair3DSW : public Constraint3DSW { int shape_B; struct Contact { - Vector3 position; Vector3 normal; Vector3 local_A, local_B; diff --git a/servers/physics_3d/broad_phase_3d_basic.cpp b/servers/physics_3d/broad_phase_3d_basic.cpp index 08ea219869..0f271b33af 100644 --- a/servers/physics_3d/broad_phase_3d_basic.cpp +++ b/servers/physics_3d/broad_phase_3d_basic.cpp @@ -33,7 +33,6 @@ #include "core/print_string.h" BroadPhase3DSW::ID BroadPhase3DBasic::create(CollisionObject3DSW *p_object, int p_subindex) { - ERR_FAIL_COND_V(p_object == nullptr, 0); current++; @@ -48,27 +47,24 @@ BroadPhase3DSW::ID BroadPhase3DBasic::create(CollisionObject3DSW *p_object, int } void BroadPhase3DBasic::move(ID p_id, const AABB &p_aabb) { - Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); E->get().aabb = p_aabb; } -void BroadPhase3DBasic::set_static(ID p_id, bool p_static) { +void BroadPhase3DBasic::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 BroadPhase3DBasic::remove(ID p_id) { +void BroadPhase3DBasic::remove(ID p_id) { Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); List<PairKey> to_erase; //unpair must be done immediately on removal to avoid potential invalid pointers for (Map<PairKey, void *>::Element *F = pair_map.front(); F; F = F->next()) { - if (F->key().a == p_id || F->key().b == p_id) { - if (unpair_callback) { Element *elem_A = &element_map[F->key().a]; Element *elem_B = &element_map[F->key().b]; @@ -78,7 +74,6 @@ void BroadPhase3DBasic::remove(ID p_id) { } } while (to_erase.size()) { - pair_map.erase(to_erase.front()->get()); to_erase.pop_front(); } @@ -86,38 +81,35 @@ void BroadPhase3DBasic::remove(ID p_id) { } CollisionObject3DSW *BroadPhase3DBasic::get_object(ID p_id) const { - const Map<ID, Element>::Element *E = element_map.find(p_id); ERR_FAIL_COND_V(!E, nullptr); return E->get().owner; } -bool BroadPhase3DBasic::is_static(ID p_id) const { +bool BroadPhase3DBasic::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 BroadPhase3DBasic::get_subindex(ID p_id) const { +int BroadPhase3DBasic::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 BroadPhase3DBasic::cull_point(const Vector3 &p_point, CollisionObject3DSW **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 AABB aabb = E->get().aabb; if (aabb.has_point(p_point)) { - 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; + } } } @@ -125,38 +117,35 @@ int BroadPhase3DBasic::cull_point(const Vector3 &p_point, CollisionObject3DSW ** } int BroadPhase3DBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **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 AABB 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 BroadPhase3DBasic::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { +int BroadPhase3DBasic::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **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 AABB 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; + } } } @@ -164,28 +153,25 @@ int BroadPhase3DBasic::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_res } void BroadPhase3DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_userdata = p_userdata; pair_callback = p_pair_callback; } -void BroadPhase3DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { +void BroadPhase3DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { unpair_userdata = p_userdata; unpair_callback = p_unpair_callback; } void BroadPhase3DBasic::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); @@ -194,16 +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); } } @@ -211,12 +198,10 @@ void BroadPhase3DBasic::update() { } BroadPhase3DSW *BroadPhase3DBasic::_create() { - return memnew(BroadPhase3DBasic); } BroadPhase3DBasic::BroadPhase3DBasic() { - current = 1; unpair_callback = nullptr; unpair_userdata = nullptr; diff --git a/servers/physics_3d/broad_phase_3d_basic.h b/servers/physics_3d/broad_phase_3d_basic.h index 563dda6931..4b644bf818 100644 --- a/servers/physics_3d/broad_phase_3d_basic.h +++ b/servers/physics_3d/broad_phase_3d_basic.h @@ -35,9 +35,7 @@ #include "core/map.h" class BroadPhase3DBasic : public BroadPhase3DSW { - struct Element { - CollisionObject3DSW *owner; bool _static; AABB aabb; @@ -49,7 +47,6 @@ class BroadPhase3DBasic : public BroadPhase3DSW { ID current; struct PairKey { - union { struct { ID a; diff --git a/servers/physics_3d/broad_phase_3d_sw.h b/servers/physics_3d/broad_phase_3d_sw.h index 5950489619..081e75810f 100644 --- a/servers/physics_3d/broad_phase_3d_sw.h +++ b/servers/physics_3d/broad_phase_3d_sw.h @@ -37,7 +37,6 @@ class CollisionObject3DSW; class BroadPhase3DSW { - public: typedef BroadPhase3DSW *(*CreateFunction)(); diff --git a/servers/physics_3d/broad_phase_octree.cpp b/servers/physics_3d/broad_phase_octree.cpp index 264ab21e1e..1ace1a4fcf 100644 --- a/servers/physics_3d/broad_phase_octree.cpp +++ b/servers/physics_3d/broad_phase_octree.cpp @@ -32,81 +32,73 @@ #include "collision_object_3d_sw.h" BroadPhase3DSW::ID BroadPhaseOctree::create(CollisionObject3DSW *p_object, int p_subindex) { - ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0); return oid; } void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) { - octree.move(p_id, p_aabb); } void BroadPhaseOctree::set_static(ID p_id, bool p_static) { - CollisionObject3DSW *it = octree.get(p_id); octree.set_pairable(p_id, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1? } -void BroadPhaseOctree::remove(ID p_id) { +void BroadPhaseOctree::remove(ID p_id) { octree.erase(p_id); } CollisionObject3DSW *BroadPhaseOctree::get_object(ID p_id) const { - CollisionObject3DSW *it = octree.get(p_id); ERR_FAIL_COND_V(!it, nullptr); return it; } -bool BroadPhaseOctree::is_static(ID p_id) const { +bool BroadPhaseOctree::is_static(ID p_id) const { return !octree.is_pairable(p_id); } -int BroadPhaseOctree::get_subindex(ID p_id) const { +int BroadPhaseOctree::get_subindex(ID p_id) const { return octree.get_subindex(p_id); } int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_point(p_point, p_results, p_max_results, p_result_indices); } int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); } int BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { - return octree.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); } 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); } void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { - pair_callback = p_pair_callback; pair_userdata = p_userdata; } -void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { +void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { unpair_callback = p_unpair_callback; unpair_userdata = p_userdata; } @@ -116,7 +108,6 @@ void BroadPhaseOctree::update() { } BroadPhase3DSW *BroadPhaseOctree::_create() { - return memnew(BroadPhaseOctree); } diff --git a/servers/physics_3d/broad_phase_octree.h b/servers/physics_3d/broad_phase_octree.h index 0ad59d8b0c..761a90a051 100644 --- a/servers/physics_3d/broad_phase_octree.h +++ b/servers/physics_3d/broad_phase_octree.h @@ -35,7 +35,6 @@ #include "core/math/octree.h" class BroadPhaseOctree : public BroadPhase3DSW { - Octree<CollisionObject3DSW, true> octree; static void *_pair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int); diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp index 24715d211d..e12f0659e2 100644 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -33,7 +33,6 @@ #include "space_3d_sw.h" void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform &p_transform, bool p_disabled) { - Shape s; s.shape = p_shape; s.xform = p_transform; @@ -51,7 +50,6 @@ void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform &p_trans } void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { - ERR_FAIL_INDEX(p_index, shapes.size()); shapes[p_index].shape->remove_owner(this); shapes.write[p_index].shape = p_shape; @@ -63,8 +61,8 @@ void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { //_update_shapes(); //_shapes_changed(); } -void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_transform) { +void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_transform) { ERR_FAIL_INDEX(p_index, shapes.size()); shapes.write[p_index].xform = p_transform; @@ -84,10 +82,8 @@ void CollisionObject3DSW::set_shape_as_disabled(int p_idx, bool p_enable) { } void CollisionObject3DSW::remove_shape(Shape3DSW *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--; @@ -96,13 +92,12 @@ void CollisionObject3DSW::remove_shape(Shape3DSW *p_shape) { } 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; @@ -118,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) { @@ -133,9 +130,7 @@ void CollisionObject3DSW::_set_static(bool p_static) { } void CollisionObject3DSW::_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); @@ -145,12 +140,11 @@ 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]; if (s.bpid == 0) { s.bpid = space->get_broadphase()->create(this, i); @@ -172,12 +166,11 @@ 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]; if (s.bpid == 0) { s.bpid = space->get_broadphase()->create(this, i); @@ -196,13 +189,10 @@ void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) { } void CollisionObject3DSW::_set_space(Space3DSW *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); @@ -214,21 +204,18 @@ void CollisionObject3DSW::_set_space(Space3DSW *p_space) { space = p_space; if (space) { - space->add_object(this); _update_shapes(); } } void CollisionObject3DSW::_shape_changed() { - _update_shapes(); _shapes_changed(); } CollisionObject3DSW::CollisionObject3DSW(Type p_type) : pending_shape_update_list(this) { - _static = true; type = p_type; space = nullptr; diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 9dd798af26..9506f14402 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -59,7 +59,6 @@ private: uint32_t collision_mask; struct Shape { - Transform xform; Transform xform_inv; BroadPhase3DSW::ID bpid; @@ -92,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 5096b080ab..736222c7d9 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -34,7 +34,6 @@ #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.02 struct _CollectorCallback { - CollisionSolver3DSW::CallbackResult callback; void *userdata; bool swap; @@ -43,18 +42,17 @@ 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); + } } }; typedef void (*GenerateContactsFunc)(const Vector3 *, int, const Vector3 *, int, _CollectorCallback *); static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 1); ERR_FAIL_COND(p_point_count_B != 1); @@ -64,7 +62,6 @@ static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_poin } static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 1); ERR_FAIL_COND(p_point_count_B != 2); @@ -75,7 +72,6 @@ static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point } static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 1); ERR_FAIL_COND(p_point_count_B < 3); @@ -87,7 +83,6 @@ static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point } static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #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 @@ -99,7 +94,6 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_ Vector3 c = rel_A.cross(rel_B).cross(rel_B); if (Math::is_zero_approx(rel_A.dot(c))) { - // should handle somehow.. //ERR_PRINT("TODO FIX"); //return; @@ -123,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); @@ -134,7 +129,6 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_ } static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 2); ERR_FAIL_COND(p_point_count_B < 3); @@ -150,7 +144,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ // copy A points to clipbuf_src for (int i = 0; i < p_point_count_A; i++) { - clipbuf_src[i] = p_points_A[i]; } @@ -158,7 +151,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ // go through all of B points for (int i = 0; i < p_point_count_B; i++) { - int i_n = (i + 1) % p_point_count_B; Vector3 edge0_B = p_points_B[i]; @@ -172,7 +164,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ int dst_idx = 0; bool edge = clipbuf_len == 2; for (int j = 0; j < clipbuf_len; j++) { - int j_n = (j + 1) % clipbuf_len; Vector3 edge0_A = clipbuf_src[j]; @@ -190,7 +181,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ // check for different sides and non coplanar //if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) { if ((dist0 * dist1) < 0 && !(edge && j)) { - // calculate intersection Vector3 rel = edge1_A - edge0_A; real_t den = clip.normal.dot(rel); @@ -211,7 +201,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]); for (int i = 0; i < clipbuf_len; i++) { - real_t d = plane_B.distance_to(clipbuf_src[i]); /* if (d>CMP_EPSILON) @@ -220,15 +209,15 @@ 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); } } static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { - #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 1); ERR_FAIL_COND(p_point_count_B < 1); @@ -267,7 +256,6 @@ static void _generate_contacts_from_supports(const Vector3 *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; @@ -284,7 +272,6 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po template <class ShapeA, class ShapeB, bool withMargin = false> class SeparatorAxisTest { - const ShapeA *shape_A; const ShapeB *shape_B; const Transform *transform_A; @@ -298,15 +285,14 @@ 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) { - Vector3 axis = p_axis; if (Math::abs(axis.x) < CMP_EPSILON && @@ -361,16 +347,17 @@ 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,7 +371,6 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_A; i++) { supports_A[i] += -best_axis * margin_A; } @@ -398,15 +384,15 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_B; i++) { supports_B[i] += best_axis * margin_B; } } 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; @@ -430,7 +416,6 @@ typedef void (*CollisionFunc)(const Shape3DSW *, const Transform &, const Shape3 template <bool withMargin> static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const SphereShape3DSW *sphere_B = static_cast<const SphereShape3DSW *>(p_b); @@ -438,34 +423,36 @@ 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(); } template <bool withMargin> static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); 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 @@ -481,17 +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(); @@ -499,14 +487,14 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans template <bool withMargin> static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); 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 @@ -514,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 @@ -530,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(); } @@ -542,14 +533,14 @@ static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform &p_ template <bool withMargin> static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); 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(); @@ -562,16 +553,15 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo // 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; + } } // edges of B for (int i = 0; i < edge_count; i++) { - Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]); Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]); Vector3 v3 = p_transform_a.origin; @@ -581,20 +571,21 @@ 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 for (int i = 0; i < vertex_count; i++) { - Vector3 v1 = p_transform_b.xform(vertices[i]); Vector3 v2 = p_transform_a.origin; Vector3 axis = (v2 - v1).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } separator.generate_contacts(); @@ -602,7 +593,6 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo template <bool withMargin> static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -614,12 +604,12 @@ 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++) { - Vector3 n1 = vertex[i] - p_transform_a.origin; if (!separator.test_axis(n1.normalized())) { @@ -640,44 +630,43 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran template <bool withMargin> static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); 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 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 for (int i = 0; i < 3; i++) { - 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)) { @@ -718,18 +707,19 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor //now try edges, which become cylinders! for (int i = 0; i < 3; i++) { - //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; + } } } @@ -738,22 +728,22 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor template <bool withMargin> static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); 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(); @@ -761,15 +751,16 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // edges of A, capsule cylinder for (int i = 0; i < 3; i++) { - // 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 @@ -783,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; + } } } } @@ -798,7 +791,6 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // capsule balls, edges of A for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -814,17 +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; + } } } @@ -837,14 +830,14 @@ static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform &p_tra template <bool withMargin> static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); 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(); @@ -857,43 +850,40 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform // 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; + } } // 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 for (int i = 0; i < 3; i++) { - Vector3 e1 = p_transform_a.basis.get_axis(i); for (int j = 0; j < edge_count; j++) { - Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } if (withMargin) { - // calculate closest points between vertices and box edges for (int v = 0; v < vertex_count; v++) { - Vector3 vtxb = p_transform_b.xform(vertices[v]); Vector3 ab_vec = vtxb - p_transform_a.origin; @@ -914,12 +904,12 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform //now try edges, which become cylinders! for (int i = 0; i < 3; i++) { - //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; + } } } @@ -932,17 +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; + } } } } @@ -954,7 +945,6 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform template <bool withMargin> static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -966,38 +956,36 @@ 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 for (int i = 0; i < 3; i++) { - Vector3 e = vertex[i] - vertex[(i + 1) % 3]; 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; + } } } if (withMargin) { - // calculate closest points between vertices and box edges for (int v = 0; v < 3; v++) { - Vector3 ab_vec = vertex[v] - p_transform_a.origin; Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); @@ -1017,12 +1005,12 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo //now try edges, which become cylinders! for (int i = 0; i < 3; i++) { - //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; + } } } @@ -1035,18 +1023,19 @@ 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]; Vector3 p2 = vertex[(e + 1) % 3]; 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; + } } } } @@ -1058,14 +1047,14 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo template <bool withMargin> static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); 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 @@ -1079,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(); } @@ -1117,14 +1115,14 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p template <bool withMargin> static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); 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(); @@ -1136,29 +1134,28 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf // 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; + } } // edges of B, capsule cylinder for (int i = 0; i < edge_count; i++) { - // cylinder 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 for (int i = 0; i < 2; i++) { - // edges of B, capsule cylinder Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); @@ -1166,14 +1163,14 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); for (int j = 0; j < edge_count; j++) { - Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]); Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); Vector3 axis = n1.cross(n2).cross(n2).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } @@ -1182,7 +1179,6 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf template <bool withMargin> static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1194,41 +1190,44 @@ 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 Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); for (int i = 0; i < 3; i++) { - // edge-cylinder 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 Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis); 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; + } } } @@ -1249,14 +1248,14 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr template <bool withMargin> static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); 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(); @@ -1278,82 +1277,77 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const // faces of A for (int i = 0; i < face_count_A; i++) { - 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 for (int i = 0; i < face_count_B; i++) { - 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 for (int i = 0; i < edge_count_A; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]); for (int j = 0; j < edge_count_B; j++) { - Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]); Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } if (withMargin) { - //vertex-vertex for (int i = 0; i < vertex_count_A; i++) { - 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) for (int i = 0; i < edge_count_A; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]); Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]); Vector3 n = (e2 - e1); 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; + } } } for (int i = 0; i < edge_count_B; i++) { - Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]); Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]); Vector3 n = (e2 - e1); 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; + } } } } @@ -1363,7 +1357,6 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const template <bool withMargin> static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { - const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); @@ -1384,77 +1377,73 @@ 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 for (int i = 0; i < edge_count; i++) { - Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]); for (int j = 0; j < 3; j++) { - Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis(axis)) + if (!separator.test_axis(axis)) { return; + } } } if (withMargin) { - //vertex-vertex for (int i = 0; i < vertex_count; i++) { - 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) for (int i = 0; i < edge_count; i++) { - Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]); Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]); Vector3 n = (e2 - e1); 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; + } } } for (int i = 0; i < 3; i++) { - Vector3 e1 = vertex[i]; Vector3 e2 = vertex[(i + 1) % 3]; Vector3 n = (e2 - e1); 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; + } } } } @@ -1463,7 +1452,6 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform } bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { - PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_PLANE, false); diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index 5d31e1f546..e2bfaf990d 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -37,10 +37,10 @@ //#define collision_solver gjk_epa_calculate_penetration 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,19 +52,20 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T bool found = false; 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); + } } } @@ -72,7 +73,6 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T } bool CollisionSolver3DSW::solve_ray(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 RayShape3DSW *ray = static_cast<const RayShape3DSW *>(p_shape_A); Vector3 from = p_transform_A.origin; @@ -85,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()) { @@ -95,16 +96,16 @@ 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; } struct _ConcaveCollisionInfo { - const Transform *transform_A; const Shape3DSW *shape_A; const Transform *transform_B; @@ -121,20 +122,19 @@ struct _ConcaveCollisionInfo { }; void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) { - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); 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++; } bool CollisionSolver3DSW::solve_concave(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, real_t p_margin_A, real_t p_margin_B) { - const ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(p_shape_B); _ConcaveCollisionInfo cinfo; @@ -158,7 +158,6 @@ bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transf AABB local_aabb; for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_B.basis.get_axis(i)); real_t axis_scale = 1.0 / axis.length(); axis *= axis_scale; @@ -180,7 +179,6 @@ bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transf } bool CollisionSolver3DSW::solve_static(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, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { - PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); bool concave_A = p_shape_A->is_concave(); @@ -195,9 +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; } @@ -209,9 +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); @@ -220,35 +218,35 @@ 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); } } 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; cinfo.tested = true; @@ -258,10 +256,10 @@ 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; @@ -275,14 +273,14 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const real_t closest_d = 0; for (int i = 0; i < support_count; i++) { - supports[i] = p_transform_B.xform(supports[i]); real_t d = p.distance_to(supports[i]); if (i == 0 || d < closest_d) { closest = supports[i]; closest_d = d; - if (d <= 0) + if (d <= 0) { collided = true; + } } } @@ -293,12 +291,11 @@ 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; bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); r_point_A = b; @@ -306,9 +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); @@ -338,7 +335,6 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans AABB local_aabb; for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_B.basis.get_axis(i)); real_t axis_scale = ((real_t)1.0) / axis.length(); axis *= axis_scale; @@ -366,7 +362,6 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans return !cinfo.collided; } else { - return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis.. } } diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h index 5e2b00404b..081ddb0382 100644 --- a/servers/physics_3d/constraint_3d_sw.h +++ b/servers/physics_3d/constraint_3d_sw.h @@ -34,7 +34,6 @@ #include "body_3d_sw.h" class Constraint3DSW { - Body3DSW **_body_ptr; int _body_count; uint64_t island_step; diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index db37f261ce..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; } @@ -513,16 +531,16 @@ struct GJK }; struct sList { - sFace* root; - U count; - sList() : root(nullptr),count(0) {} + sFace* root = nullptr; + U count = 0; + sList() {} }; struct sHorizon { - sFace* cf; - sFace* ff; - U nf; - sHorizon() : cf(nullptr),ff(nullptr),nf(0) {} + sFace* cf = nullptr; + sFace* ff = nullptr; + U nf = 0; + sHorizon() {} }; struct eStatus { enum _ { Valid, @@ -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); @@ -847,6 +883,7 @@ bool Distance( const Shape3DSW* shape0, } } + // bool Penetration( const Shape3DSW* shape0, const Transform& wtrs0, @@ -879,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: @@ -891,6 +930,7 @@ bool Penetration( const Shape3DSW* shape0, } + /* Symbols cleanup */ #undef GJK_MAX_ITERATIONS @@ -915,11 +955,9 @@ bool Penetration( const Shape3DSW* shape0, /* clang-format on */ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { - GjkEpa2::sResults res; if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { - r_result_A = res.witnesses[0]; r_result_B = res.witnesses[1]; return true; @@ -929,15 +967,15 @@ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_t } bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) { - GjkEpa2::sResults res; 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/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp index 56dd8044e4..9d10ede608 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -52,7 +52,6 @@ Written by: Marcus Hennix #include "cone_twist_joint_3d_sw.h" static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - if (Math::abs(n.z) > Math_SQRT12) { // choose p in y-z plane real_t a = n[1] * n[1] + n[2] * n[2]; @@ -87,7 +86,6 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame) : Joint3DSW(_arr, 2) { - A = rbA; B = rbB; @@ -238,7 +236,6 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) { } void ConeTwistJoint3DSW::solve(real_t p_timestep) { - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); @@ -309,27 +306,21 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { } void ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) { - switch (p_param) { case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { - m_swingSpan1 = p_value; m_swingSpan2 = p_value; } break; case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { - m_twistSpan = p_value; } break; case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { - m_biasFactor = p_value; } break; case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { - m_limitSoftness = p_value; } break; case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { - m_relaxationFactor = p_value; } break; case PhysicsServer3D::CONE_TWIST_MAX: @@ -338,26 +329,20 @@ void ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param, } real_t ConeTwistJoint3DSW::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const { - switch (p_param) { case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { - return m_swingSpan1; } break; case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { - return m_twistSpan; } break; case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { - return m_biasFactor; } break; case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { - return m_limitSoftness; } break; case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { - return m_relaxationFactor; } break; case PhysicsServer3D::CONE_TWIST_MAX: 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 ae5cffdfa8..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; } @@ -155,7 +157,6 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( int limit_index, const Vector3 &axis_normal_on_a, const Vector3 &anchorPos) { - ///find relative velocity // Vector3 rel_pos1 = pointInA - body1->get_transform().origin; // Vector3 rel_pos2 = pointInB - body2->get_transform().origin; @@ -303,7 +304,6 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) { } bool Generic6DOFJoint3DSW::setup(real_t p_timestep) { - // Clear accumulated impulses for the next simulation step m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.)); int i; @@ -327,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, @@ -369,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, @@ -388,7 +390,6 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) { real_t angularJacDiagABInv; for (i = 0; i < 3; i++) { if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) { - // get axis angular_axis = getAxis(i); @@ -411,7 +412,7 @@ real_t Generic6DOFJoint3DSW::getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; } -void Generic6DOFJoint3DSW::calcAnchorPos(void) { +void Generic6DOFJoint3DSW::calcAnchorPos() { real_t imA = A->get_inv_mass(); real_t imB = B->get_inv_mass(); real_t weight; @@ -426,75 +427,60 @@ void Generic6DOFJoint3DSW::calcAnchorPos(void) { } // Generic6DOFJointSW::calcAnchorPos() void Generic6DOFJoint3DSW::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) { - ERR_FAIL_INDEX(p_axis, 3); switch (p_param) { case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { - m_linearLimits.m_lowerLimit[p_axis] = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { - m_linearLimits.m_upperLimit[p_axis] = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { - m_linearLimits.m_limitSoftness[p_axis] = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { - m_linearLimits.m_restitution[p_axis] = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { - m_linearLimits.m_damping[p_axis] = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { - m_angularLimits[p_axis].m_loLimit = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { - m_angularLimits[p_axis].m_hiLimit = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { - m_angularLimits[p_axis].m_limitSoftness = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { - m_angularLimits[p_axis].m_damping = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { - m_angularLimits[p_axis].m_bounce = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { - m_angularLimits[p_axis].m_maxLimitForce = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { - m_angularLimits[p_axis].m_ERP = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { - m_angularLimits[p_axis].m_targetVelocity = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { - m_angularLimits[p_axis].m_maxLimitForce = p_value; } break; @@ -531,71 +517,57 @@ real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6 ERR_FAIL_INDEX_V(p_axis, 3, 0); switch (p_param) { case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { - return m_linearLimits.m_lowerLimit[p_axis]; } break; case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { - return m_linearLimits.m_upperLimit[p_axis]; } break; case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { - return m_linearLimits.m_limitSoftness[p_axis]; } break; case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { - return m_linearLimits.m_restitution[p_axis]; } break; case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { - return m_linearLimits.m_damping[p_axis]; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { - return m_angularLimits[p_axis].m_loLimit; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { - return m_angularLimits[p_axis].m_hiLimit; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { - return m_angularLimits[p_axis].m_limitSoftness; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { - return m_angularLimits[p_axis].m_damping; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { - return m_angularLimits[p_axis].m_bounce; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { - return m_angularLimits[p_axis].m_maxLimitForce; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { - return m_angularLimits[p_axis].m_ERP; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { - return m_angularLimits[p_axis].m_targetVelocity; } break; case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { - return m_angularLimits[p_axis].m_maxMotorForce; } break; @@ -630,20 +602,16 @@ real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6 } void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) { - ERR_FAIL_INDEX(p_axis, 3); switch (p_flag) { case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { - m_linearLimits.enable_limit[p_axis] = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { - m_angularLimits[p_axis].m_enableLimit = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { - m_angularLimits[p_axis].m_enableMotor = p_value; } break; case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { @@ -659,20 +627,17 @@ void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOF break; // Can't happen, but silences warning } } -bool Generic6DOFJoint3DSW::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const { +bool Generic6DOFJoint3DSW::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const { ERR_FAIL_INDEX_V(p_axis, 3, 0); switch (p_flag) { case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { - return m_linearLimits.enable_limit[p_axis]; } break; case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { - return m_angularLimits[p_axis].m_enableLimit; } break; case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { - return m_angularLimits[p_axis].m_enableMotor; } break; case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { @@ -688,5 +653,5 @@ bool Generic6DOFJoint3DSW::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOF break; // Can't happen, but silences warning } - return 0; + return false; } diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h index f7aa607901..cc1423a1cb 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -389,7 +389,7 @@ public: return B; } - virtual void calcAnchorPos(void); // overridable + virtual void calcAnchorPos(); // overridable void set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value); real_t get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const; diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index dea1be5040..a879b4ca7f 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -50,7 +50,6 @@ subject to the following restrictions: #include "hinge_joint_3d_sw.h" static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { - if (Math::abs(n.z) > Math_SQRT12) { // choose p in y-z plane real_t a = n[1] * n[1] + n[2] * n[2]; @@ -70,7 +69,6 @@ static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameA, const Transform &frameB) : Joint3DSW(_arr, 2) { - A = rbA; B = rbB; @@ -103,7 +101,6 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &fr HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB) : Joint3DSW(_arr, 2) { - A = rbA; B = rbB; @@ -158,7 +155,6 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo } bool HingeJoint3DSW::setup(real_t p_step) { - m_appliedImpulse = real_t(0.); if (!m_angularOnly) { @@ -254,7 +250,6 @@ bool HingeJoint3DSW::setup(real_t p_step) { } void HingeJoint3DSW::solve(real_t p_step) { - Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); @@ -365,12 +360,14 @@ void HingeJoint3DSW::solve(real_t p_step) { } } } + /* void HingeJointSW::updateRHS(real_t timeStep) { (void)timeStep; } + */ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { @@ -397,9 +394,7 @@ real_t HingeJoint3DSW::get_hinge_angle() { } void HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer3D::HINGE_JOINT_BIAS: tau = p_value; break; @@ -430,9 +425,7 @@ void HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t } real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const { - switch (p_param) { - case PhysicsServer3D::HINGE_JOINT_BIAS: return tau; case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: @@ -457,7 +450,6 @@ real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const } void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) { - switch (p_flag) { case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit = p_value; @@ -469,8 +461,8 @@ void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_val break; // Can't happen, but silences warning } } -bool HingeJoint3DSW::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const { +bool HingeJoint3DSW::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const { switch (p_flag) { case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit; diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index eebead20b8..c5af888eca 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -54,7 +54,6 @@ subject to the following restrictions: */ class HingeJoint3DSW : public Joint3DSW { - union { struct { Body3DSW *A; diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/jacobian_entry_3d_sw.h index 7e605ab173..1737c21b3d 100644 --- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h +++ b/servers/physics_3d/joints/jacobian_entry_3d_sw.h @@ -54,7 +54,7 @@ subject to the following restrictions: class JacobianEntry3DSW { public: - JacobianEntry3DSW(){}; + JacobianEntry3DSW() {} //constraint between two different rigidbodies JacobianEntry3DSW( const Basis &world2A, diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index dd6b315152..230904408b 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -50,7 +50,6 @@ subject to the following restrictions: #include "pin_joint_3d_sw.h" bool PinJoint3DSW::setup(real_t p_step) { - m_appliedImpulse = real_t(0.); Vector3 normal(0, 0, 0); @@ -74,7 +73,6 @@ bool PinJoint3DSW::setup(real_t p_step) { } void PinJoint3DSW::solve(real_t p_step) { - Vector3 pivotAInW = A->get_transform().xform(m_pivotInA); Vector3 pivotBInW = B->get_transform().xform(m_pivotInB); @@ -111,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; @@ -127,7 +127,6 @@ void PinJoint3DSW::solve(real_t p_step) { } void PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) { - switch (p_param) { case PhysicsServer3D::PIN_JOINT_BIAS: m_tau = p_value; @@ -142,7 +141,6 @@ void PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_va } real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const { - switch (p_param) { case PhysicsServer3D::PIN_JOINT_BIAS: return m_tau; @@ -157,7 +155,6 @@ real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const { PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b) : Joint3DSW(_arr, 2) { - A = p_body_a; B = p_body_b; m_pivotInA = p_pos_a; diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index 8e81ccf5e0..0181a4455b 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -54,7 +54,6 @@ subject to the following restrictions: */ class PinJoint3DSW : public Joint3DSW { - union { struct { Body3DSW *A; diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index b133f3d9ad..5b4609f24e 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -116,7 +116,6 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform & Joint3DSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB) { - A = rbA; B = rbB; @@ -129,7 +128,6 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform & //----------------------------------------------------------------------------- bool SliderJoint3DSW::setup(real_t p_step) { - //calculate transforms m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformB = B->get_transform() * m_frameInB; @@ -182,7 +180,6 @@ bool SliderJoint3DSW::setup(real_t p_step) { //----------------------------------------------------------------------------- void SliderJoint3DSW::solve(real_t p_step) { - int i; // linear Vector3 velA = A->get_velocity_in_local_point(m_relPosA); @@ -304,7 +301,7 @@ void SliderJoint3DSW::solve(real_t p_step) { //----------------------------------------------------------------------------- -void SliderJoint3DSW::calculateTransforms(void) { +void SliderJoint3DSW::calculateTransforms() { m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; @@ -323,7 +320,7 @@ void SliderJoint3DSW::calculateTransforms(void) { //----------------------------------------------------------------------------- -void SliderJoint3DSW::testLinLimits(void) { +void SliderJoint3DSW::testLinLimits() { m_solveLinLim = false; m_linPos = m_depth[0]; if (m_lowerLinLimit <= m_upperLinLimit) { @@ -343,7 +340,7 @@ void SliderJoint3DSW::testLinLimits(void) { //----------------------------------------------------------------------------- -void SliderJoint3DSW::testAngLimits(void) { +void SliderJoint3DSW::testAngLimits() { m_angDepth = real_t(0.); m_solveAngLim = false; if (m_lowerAngLimit <= m_upperAngLimit) { @@ -363,7 +360,7 @@ void SliderJoint3DSW::testAngLimits(void) { //----------------------------------------------------------------------------- -Vector3 SliderJoint3DSW::getAncorInA(void) { +Vector3 SliderJoint3DSW::getAncorInA() { Vector3 ancorInA; ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis; ancorInA = A->get_transform().inverse().xform(ancorInA); @@ -372,14 +369,13 @@ Vector3 SliderJoint3DSW::getAncorInA(void) { //----------------------------------------------------------------------------- -Vector3 SliderJoint3DSW::getAncorInB(void) { +Vector3 SliderJoint3DSW::getAncorInB() { Vector3 ancorInB; ancorInB = m_frameInB.origin; return ancorInB; } // SliderJointSW::getAncorInB(); void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) { - switch (p_param) { case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit = p_value; @@ -455,7 +451,6 @@ void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_ } real_t SliderJoint3DSW::get_param(PhysicsServer3D::SliderJointParam p_param) const { - switch (p_param) { case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit; diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index 18287db9c2..37394a1580 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -230,12 +230,12 @@ public: bool getSolveAngLimit() { return m_solveAngLim; } real_t getAngDepth() { return m_angDepth; } // shared code used by ODE solver - void calculateTransforms(void); - void testLinLimits(void); - void testAngLimits(void); + void calculateTransforms(); + void testLinLimits(); + void testAngLimits(); // access for PE Solver - Vector3 getAncorInA(void); - Vector3 getAncorInB(void); + Vector3 getAncorInA(); + Vector3 getAncorInB(); void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value); real_t get_param(PhysicsServer3D::SliderJointParam p_param) const; diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/joints_3d_sw.h index 0f2d4892a8..6a010ee771 100644 --- a/servers/physics_3d/joints_3d_sw.h +++ b/servers/physics_3d/joints_3d_sw.h @@ -35,7 +35,6 @@ #include "constraint_3d_sw.h" class Joint3DSW : public Constraint3DSW { - public: virtual PhysicsServer3D::JointType get_type() const = 0; _FORCE_INLINE_ Joint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index b454dc54af..b625ae2271 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -44,48 +44,36 @@ 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 PhysicsServer3DSW::shape_create(ShapeType p_shape) { - Shape3DSW *shape = nullptr; switch (p_shape) { - case SHAPE_PLANE: { - shape = memnew(PlaneShape3DSW); } break; case SHAPE_RAY: { - shape = memnew(RayShape3DSW); } break; case SHAPE_SPHERE: { - shape = memnew(SphereShape3DSW); } break; case SHAPE_BOX: { - shape = memnew(BoxShape3DSW); } break; case SHAPE_CAPSULE: { - shape = memnew(CapsuleShape3DSW); } break; case SHAPE_CYLINDER: { - ERR_FAIL_V_MSG(RID(), "CylinderShape3D is not supported in GodotPhysics3D. Please switch to Bullet in the Project Settings."); } break; case SHAPE_CONVEX_POLYGON: { - shape = memnew(ConvexPolygonShape3DSW); } break; case SHAPE_CONCAVE_POLYGON: { - shape = memnew(ConcavePolygonShape3DSW); } break; case SHAPE_HEIGHTMAP: { - shape = memnew(HeightMapShape3DSW); } break; case SHAPE_CUSTOM: { - ERR_FAIL_V(RID()); } break; @@ -98,28 +86,24 @@ RID PhysicsServer3DSW::shape_create(ShapeType p_shape) { }; void PhysicsServer3DSW::shape_set_data(RID p_shape, const Variant &p_data) { - Shape3DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND(!shape); shape->set_data(p_data); }; void PhysicsServer3DSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { - Shape3DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND(!shape); shape->set_custom_bias(p_bias); } PhysicsServer3D::ShapeType PhysicsServer3DSW::shape_get_type(RID p_shape) const { - const Shape3DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM); return shape->get_type(); }; Variant PhysicsServer3DSW::shape_get_data(RID p_shape) const { - const Shape3DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, Variant()); ERR_FAIL_COND_V(!shape->is_configured(), Variant()); @@ -134,14 +118,12 @@ real_t PhysicsServer3DSW::shape_get_margin(RID p_shape) const { } real_t PhysicsServer3DSW::shape_get_custom_solver_bias(RID p_shape) const { - const Shape3DSW *shape = shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); return shape->get_custom_bias(); } RID PhysicsServer3DSW::space_create() { - Space3DSW *space = memnew(Space3DSW); RID id = space_owner.make_rid(space); space->set_self(id); @@ -160,17 +142,16 @@ 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 { - const Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, false); @@ -178,7 +159,6 @@ bool PhysicsServer3DSW::space_is_active(RID p_space) const { } void PhysicsServer3DSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { - Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND(!space); @@ -186,14 +166,12 @@ void PhysicsServer3DSW::space_set_param(RID p_space, SpaceParameter p_param, rea } real_t PhysicsServer3DSW::space_get_param(RID p_space, SpaceParameter p_param) const { - const Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, 0); return space->get_param(p_param); } PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space) { - Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, nullptr); ERR_FAIL_COND_V_MSG(!doing_sync || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); @@ -202,28 +180,24 @@ PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space } void PhysicsServer3DSW::space_set_debug_contacts(RID p_space, int p_max_contacts) { - Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND(!space); space->set_debug_contacts(p_max_contacts); } Vector<Vector3> PhysicsServer3DSW::space_get_contacts(RID p_space) const { - Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, Vector<Vector3>()); return space->get_debug_contacts(); } int PhysicsServer3DSW::space_get_contact_count(RID p_space) const { - Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, 0); return space->get_debug_contact_count(); } RID PhysicsServer3DSW::area_create() { - Area3DSW *area = memnew(Area3DSW); RID rid = area_owner.make_rid(area); area->set_self(rid); @@ -231,7 +205,6 @@ RID PhysicsServer3DSW::area_create() { }; void PhysicsServer3DSW::area_set_space(RID p_area, RID p_space) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -241,26 +214,26 @@ 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); }; RID PhysicsServer3DSW::area_get_space(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, RID()); Space3DSW *space = area->get_space(); - if (!space) + if (!space) { return RID(); + } return space->get_self(); }; void PhysicsServer3DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -268,7 +241,6 @@ void PhysicsServer3DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverri } PhysicsServer3D::AreaSpaceOverrideMode PhysicsServer3DSW::area_get_space_override_mode(RID p_area) const { - const Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); @@ -276,7 +248,6 @@ PhysicsServer3D::AreaSpaceOverrideMode PhysicsServer3DSW::area_get_space_overrid } void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform &p_transform, bool p_disabled) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -287,7 +258,6 @@ void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform } void PhysicsServer3DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -299,7 +269,6 @@ void PhysicsServer3DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) } void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -307,14 +276,13 @@ void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, co } int PhysicsServer3DSW::area_get_shape_count(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, -1); return area->get_shape_count(); } -RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const { +RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, RID()); @@ -323,8 +291,8 @@ RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const { return shape->get_self(); } -Transform PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { +Transform PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, Transform()); @@ -332,7 +300,6 @@ Transform PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_id } void PhysicsServer3DSW::area_remove_shape(RID p_area, int p_shape_idx) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -340,16 +307,15 @@ void PhysicsServer3DSW::area_remove_shape(RID p_area, int p_shape_idx) { } 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) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count()); @@ -358,7 +324,6 @@ void PhysicsServer3DSW::area_set_shape_disabled(RID p_area, int p_shape_idx, boo } void PhysicsServer3DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) { - if (space_owner.owns(p_area)) { Space3DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -367,8 +332,8 @@ void PhysicsServer3DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id ERR_FAIL_COND(!area); area->set_instance_id(p_id); } -ObjectID PhysicsServer3DSW::area_get_object_instance_id(RID p_area) const { +ObjectID PhysicsServer3DSW::area_get_object_instance_id(RID p_area) const { if (space_owner.owns(p_area)) { Space3DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -379,7 +344,6 @@ ObjectID PhysicsServer3DSW::area_get_object_instance_id(RID p_area) const { } void PhysicsServer3DSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { - if (space_owner.owns(p_area)) { Space3DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -390,14 +354,12 @@ void PhysicsServer3DSW::area_set_param(RID p_area, AreaParameter p_param, const }; void PhysicsServer3DSW::area_set_transform(RID p_area, const Transform &p_transform) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); area->set_transform(p_transform); }; Variant PhysicsServer3DSW::area_get_param(RID p_area, AreaParameter p_param) const { - if (space_owner.owns(p_area)) { Space3DSW *space = space_owner.getornull(p_area); p_area = space->get_default_area()->get_self(); @@ -409,7 +371,6 @@ Variant PhysicsServer3DSW::area_get_param(RID p_area, AreaParameter p_param) con }; Transform PhysicsServer3DSW::area_get_transform(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, Transform()); @@ -417,7 +378,6 @@ Transform PhysicsServer3DSW::area_get_transform(RID p_area) const { }; void PhysicsServer3DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -425,7 +385,6 @@ void PhysicsServer3DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) { } void PhysicsServer3DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -433,7 +392,6 @@ void PhysicsServer3DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) { } void PhysicsServer3DSW::area_set_monitorable(RID p_area, bool p_monitorable) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); FLUSH_QUERY_CHECK(area); @@ -442,7 +400,6 @@ void PhysicsServer3DSW::area_set_monitorable(RID p_area, bool p_monitorable) { } void PhysicsServer3DSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -450,7 +407,6 @@ void PhysicsServer3DSW::area_set_monitor_callback(RID p_area, Object *p_receiver } void PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -458,7 +414,6 @@ void PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) { } bool PhysicsServer3DSW::area_is_ray_pickable(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, false); @@ -466,7 +421,6 @@ bool PhysicsServer3DSW::area_is_ray_pickable(RID p_area) const { } void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -476,19 +430,19 @@ void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_rec /* BODY API */ 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; }; void PhysicsServer3DSW::body_set_space(RID p_body, RID p_space) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -498,26 +452,26 @@ 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); }; RID PhysicsServer3DSW::body_get_space(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, RID()); Space3DSW *space = body->get_space(); - if (!space) + if (!space) { return RID(); + } return space->get_self(); }; void PhysicsServer3DSW::body_set_mode(RID p_body, BodyMode p_mode) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -525,7 +479,6 @@ void PhysicsServer3DSW::body_set_mode(RID p_body, BodyMode p_mode) { }; PhysicsServer3D::BodyMode PhysicsServer3DSW::body_get_mode(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, BODY_MODE_STATIC); @@ -533,7 +486,6 @@ PhysicsServer3D::BodyMode PhysicsServer3DSW::body_get_mode(RID p_body) const { }; void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform &p_transform, bool p_disabled) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -544,7 +496,6 @@ void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform } void PhysicsServer3DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -554,8 +505,8 @@ void PhysicsServer3DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) body->set_shape(p_shape_idx, shape); } -void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) { +void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -563,14 +514,13 @@ void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, co } int PhysicsServer3DSW::body_get_shape_count(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, -1); return body->get_shape_count(); } -RID PhysicsServer3DSW::body_get_shape(RID p_body, int p_shape_idx) const { +RID PhysicsServer3DSW::body_get_shape(RID p_body, int p_shape_idx) const { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, RID()); @@ -581,7 +531,6 @@ RID PhysicsServer3DSW::body_get_shape(RID p_body, int p_shape_idx) const { } void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); @@ -591,7 +540,6 @@ void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo } Transform PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, Transform()); @@ -599,7 +547,6 @@ Transform PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_id } void PhysicsServer3DSW::body_remove_shape(RID p_body, int p_shape_idx) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -607,16 +554,15 @@ void PhysicsServer3DSW::body_remove_shape(RID p_body, int p_shape_idx) { } 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) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -624,7 +570,6 @@ void PhysicsServer3DSW::body_set_enable_continuous_collision_detection(RID p_bod } bool PhysicsServer3DSW::body_is_continuous_collision_detection_enabled(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); @@ -632,7 +577,6 @@ bool PhysicsServer3DSW::body_is_continuous_collision_detection_enabled(RID p_bod } void PhysicsServer3DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -641,7 +585,6 @@ void PhysicsServer3DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) { } uint32_t PhysicsServer3DSW::body_get_collision_layer(RID p_body) const { - const Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); @@ -649,7 +592,6 @@ uint32_t PhysicsServer3DSW::body_get_collision_layer(RID p_body) const { } void PhysicsServer3DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -658,7 +600,6 @@ void PhysicsServer3DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) { } uint32_t PhysicsServer3DSW::body_get_collision_mask(RID p_body) const { - const Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); @@ -666,7 +607,6 @@ uint32_t PhysicsServer3DSW::body_get_collision_mask(RID p_body) const { } void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -674,7 +614,6 @@ void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id }; ObjectID PhysicsServer3DSW::body_get_object_instance_id(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, ObjectID()); @@ -682,13 +621,11 @@ ObjectID PhysicsServer3DSW::body_get_object_instance_id(RID p_body) const { }; void PhysicsServer3DSW::body_set_user_flags(RID p_body, uint32_t p_flags) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); }; uint32_t PhysicsServer3DSW::body_get_user_flags(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); @@ -696,7 +633,6 @@ uint32_t PhysicsServer3DSW::body_get_user_flags(RID p_body) const { }; void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -704,7 +640,6 @@ void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, real_t }; real_t PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); @@ -725,7 +660,6 @@ real_t PhysicsServer3DSW::body_get_kinematic_safe_margin(RID p_body) const { } void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -733,7 +667,6 @@ void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Vari }; Variant PhysicsServer3DSW::body_get_state(RID p_body, BodyState p_state) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, Variant()); @@ -741,7 +674,6 @@ Variant PhysicsServer3DSW::body_get_state(RID p_body, BodyState p_state) const { }; void PhysicsServer3DSW::body_set_applied_force(RID p_body, const Vector3 &p_force) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -750,14 +682,12 @@ void PhysicsServer3DSW::body_set_applied_force(RID p_body, const Vector3 &p_forc }; Vector3 PhysicsServer3DSW::body_get_applied_force(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, Vector3()); return body->get_applied_force(); }; void PhysicsServer3DSW::body_set_applied_torque(RID p_body, const Vector3 &p_torque) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -766,7 +696,6 @@ void PhysicsServer3DSW::body_set_applied_torque(RID p_body, const Vector3 &p_tor }; Vector3 PhysicsServer3DSW::body_get_applied_torque(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, Vector3()); @@ -808,7 +737,6 @@ void PhysicsServer3DSW::body_apply_central_impulse(RID p_body, const Vector3 &p_ } void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -819,7 +747,6 @@ void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, con }; void PhysicsServer3DSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -830,7 +757,6 @@ void PhysicsServer3DSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_i }; void PhysicsServer3DSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -845,7 +771,6 @@ void PhysicsServer3DSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis }; void PhysicsServer3DSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -854,14 +779,12 @@ void PhysicsServer3DSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_l } bool PhysicsServer3DSW::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { - const Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); return body->is_axis_locked(p_axis); } void PhysicsServer3DSW::body_add_collision_exception(RID p_body, RID p_body_b) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -870,7 +793,6 @@ void PhysicsServer3DSW::body_add_collision_exception(RID p_body, RID p_body_b) { }; void PhysicsServer3DSW::body_remove_collision_exception(RID p_body, RID p_body_b) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -879,7 +801,6 @@ void PhysicsServer3DSW::body_remove_collision_exception(RID p_body, RID p_body_b }; void PhysicsServer3DSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -889,20 +810,17 @@ void PhysicsServer3DSW::body_get_collision_exceptions(RID p_body, List<RID> *p_e }; void PhysicsServer3DSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); }; real_t PhysicsServer3DSW::body_get_contacts_reported_depth_threshold(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, 0); return 0; }; void PhysicsServer3DSW::body_set_omit_force_integration(RID p_body, bool p_omit) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -910,49 +828,42 @@ void PhysicsServer3DSW::body_set_omit_force_integration(RID p_body, bool p_omit) }; bool PhysicsServer3DSW::body_is_omitting_force_integration(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); return body->get_omit_force_integration(); }; void PhysicsServer3DSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); body->set_max_contacts_reported(p_contacts); } int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, -1); return body->get_max_contacts_reported(); } void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) { - Body3DSW *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); } void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); body->set_ray_pickable(p_enable); } bool PhysicsServer3DSW::body_is_ray_pickable(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); return body->is_ray_pickable(); } bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -964,7 +875,6 @@ bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, co } int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -976,7 +886,6 @@ int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_t } PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, nullptr); ERR_FAIL_COND_V_MSG(!doing_sync || body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); @@ -988,7 +897,6 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { /* JOINT API */ RID PhysicsServer3DSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND_V(!body_A, RID()); @@ -1009,15 +917,14 @@ RID PhysicsServer3DSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, } void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_param(p_param, p_value); } -real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { +real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0); @@ -1026,15 +933,14 @@ real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param } void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_pos_a(p_A); } -Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { +Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); @@ -1043,15 +949,14 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { } void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_pos_b(p_B); } -Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { +Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); @@ -1060,7 +965,6 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { } RID PhysicsServer3DSW::joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND_V(!body_A, RID()); @@ -1081,7 +985,6 @@ RID PhysicsServer3DSW::joint_create_hinge(RID p_body_A, const Transform &p_frame } RID PhysicsServer3DSW::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND_V(!body_A, RID()); @@ -1102,15 +1005,14 @@ RID PhysicsServer3DSW::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_ } void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); hinge_joint->set_param(p_param, p_value); } -real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { +real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0); @@ -1119,15 +1021,14 @@ real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_p } void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); hinge_joint->set_flag(p_flag, p_value); } -bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { +bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, false); ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false); @@ -1136,14 +1037,12 @@ bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) } void PhysicsServer3DSW::joint_set_solver_priority(RID p_joint, int p_priority) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); joint->set_priority(p_priority); } int PhysicsServer3DSW::joint_get_solver_priority(RID p_joint) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); return joint->get_priority(); @@ -1177,14 +1076,12 @@ bool PhysicsServer3DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) } PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) const { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, JOINT_PIN); return joint->get_type(); } RID PhysicsServer3DSW::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND_V(!body_A, RID()); @@ -1205,15 +1102,14 @@ RID PhysicsServer3DSW::joint_create_slider(RID p_body_A, const Transform &p_loca } void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER); SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); slider_joint->set_param(p_param, p_value); } -real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { +real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); @@ -1222,7 +1118,6 @@ real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p } RID PhysicsServer3DSW::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND_V(!body_A, RID()); @@ -1243,15 +1138,14 @@ RID PhysicsServer3DSW::joint_create_cone_twist(RID p_body_A, const Transform &p_ } void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST); ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); cone_twist_joint->set_param(p_param, p_value); } -real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { +real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); @@ -1260,7 +1154,6 @@ real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJoint } RID PhysicsServer3DSW::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { - Body3DSW *body_A = body_owner.getornull(p_body_A); ERR_FAIL_COND_V(!body_A, RID()); @@ -1281,15 +1174,14 @@ RID PhysicsServer3DSW::joint_create_generic_6dof(RID p_body_A, const Transform & } void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); generic_6dof_joint->set_param(p_axis, p_param, p_value); } -real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) { +real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0); @@ -1298,15 +1190,14 @@ real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axi } void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); } -bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) { +bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, false); ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false); @@ -1315,11 +1206,9 @@ bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p } void PhysicsServer3DSW::free(RID p_rid) { - _update_shapes(); //just in case if (shape_owner.owns(p_rid)) { - Shape3DSW *shape = shape_owner.getornull(p_rid); while (shape->get_owners().size()) { @@ -1330,7 +1219,6 @@ void PhysicsServer3DSW::free(RID p_rid) { shape_owner.free(p_rid); memdelete(shape); } else if (body_owner.owns(p_rid)) { - Body3DSW *body = body_owner.getornull(p_rid); /* @@ -1344,7 +1232,6 @@ void PhysicsServer3DSW::free(RID p_rid) { body->set_space(nullptr); while (body->get_shape_count()) { - body->remove_shape(0); } @@ -1352,7 +1239,6 @@ void PhysicsServer3DSW::free(RID p_rid) { memdelete(body); } else if (area_owner.owns(p_rid)) { - Area3DSW *area = area_owner.getornull(p_rid); /* @@ -1363,14 +1249,12 @@ void PhysicsServer3DSW::free(RID p_rid) { area->set_space(nullptr); while (area->get_shape_count()) { - area->remove_shape(0); } area_owner.free(p_rid); memdelete(area); } else if (space_owner.owns(p_rid)) { - Space3DSW *space = space_owner.getornull(p_rid); while (space->get_objects().size()) { @@ -1385,29 +1269,24 @@ void PhysicsServer3DSW::free(RID p_rid) { space_owner.free(p_rid); memdelete(space); } else if (joint_owner.owns(p_rid)) { - Joint3DSW *joint = joint_owner.getornull(p_rid); for (int i = 0; i < joint->get_body_count(); i++) { - joint->get_body_ptr()[i]->remove_constraint(joint); } joint_owner.free(p_rid); memdelete(joint); } else { - ERR_FAIL_MSG("Invalid ID."); } }; void PhysicsServer3DSW::set_active(bool p_active) { - active = p_active; }; void PhysicsServer3DSW::init() { - doing_sync = true; last_step = 0.001; iterations = 8; // 8? @@ -1416,11 +1295,11 @@ void PhysicsServer3DSW::init() { }; void PhysicsServer3DSW::step(real_t p_step) { - #ifndef _3D_DISABLED - if (!active) + if (!active) { return; + } _update_shapes(); @@ -1433,7 +1312,6 @@ void PhysicsServer3DSW::step(real_t p_step) { active_objects = 0; collision_pairs = 0; for (Set<const Space3DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((Space3DSW *)E->get(), p_step, iterations); island_count += E->get()->get_island_count(); active_objects += E->get()->get_active_objects(); @@ -1447,11 +1325,11 @@ void PhysicsServer3DSW::sync(){ }; void PhysicsServer3DSW::flush_queries() { - #ifndef _3D_DISABLED - if (!active) + if (!active) { return; + } doing_sync = true; @@ -1460,7 +1338,6 @@ void PhysicsServer3DSW::flush_queries() { uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); for (Set<const Space3DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - Space3DSW *space = (Space3DSW *)E->get(); space->call_queries(); } @@ -1468,7 +1345,6 @@ void PhysicsServer3DSW::flush_queries() { flushing_queries = false; if (EngineDebugger::is_profiling("servers")) { - uint64_t total_time[Space3DSW::ELAPSED_TIME_MAX]; static const char *time_name[Space3DSW::ELAPSED_TIME_MAX] = { "integrate_forces", @@ -1483,7 +1359,6 @@ void PhysicsServer3DSW::flush_queries() { } for (Set<const Space3DSW *>::Element *E = active_spaces.front(); E; E = E->next()) { - for (int i = 0; i < Space3DSW::ELAPSED_TIME_MAX; i++) { total_time[i] += E->get()->get_elapsed_time(Space3DSW::ElapsedTime(i)); } @@ -1505,24 +1380,19 @@ void PhysicsServer3DSW::flush_queries() { }; void PhysicsServer3DSW::finish() { - memdelete(stepper); memdelete(direct_state); }; int PhysicsServer3DSW::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; } @@ -1531,7 +1401,6 @@ int PhysicsServer3DSW::get_process_info(ProcessInfo p_info) { } void PhysicsServer3DSW::_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()); @@ -1539,18 +1408,17 @@ 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 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; @@ -1559,13 +1427,13 @@ 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; } else { - cbk->ptr[cbk->amount * 2 + 0] = p_point_A; cbk->ptr[cbk->amount * 2 + 1] = p_point_B; cbk->amount++; diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index 6e79d9eceb..46341bb380 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -39,7 +39,6 @@ #include "step_3d_sw.h" class PhysicsServer3DSW : public PhysicsServer3D { - GDCLASS(PhysicsServer3DSW, PhysicsServer3D); friend class PhysicsDirectSpaceState3DSW; @@ -74,7 +73,6 @@ public: static PhysicsServer3DSW *singleton; struct CollCbkData { - int max; int amount; Vector3 *ptr; @@ -307,7 +305,7 @@ public: virtual void soft_body_remove_all_pinned_points(RID p_body) {} virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {} - virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) { return 0; } + virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) { return false; } /* JOINT API */ diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index 61c32b779a..eb0e87cec0 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -48,7 +48,6 @@ void Shape3DSW::configure(const AABB &p_aabb) { } Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 res; int amnt; get_supports(p_normal, 1, &res, amnt); @@ -56,7 +55,6 @@ Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const { } void Shape3DSW::add_owner(ShapeOwner3DSW *p_owner) { - Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner); if (E) { E->get()++; @@ -66,7 +64,6 @@ void Shape3DSW::add_owner(ShapeOwner3DSW *p_owner) { } void Shape3DSW::remove_owner(ShapeOwner3DSW *p_owner) { - Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner); ERR_FAIL_COND(!E); E->get()--; @@ -76,7 +73,6 @@ void Shape3DSW::remove_owner(ShapeOwner3DSW *p_owner) { } bool Shape3DSW::is_owner(ShapeOwner3DSW *p_owner) const { - return owners.has(p_owner); } @@ -85,48 +81,41 @@ const Map<ShapeOwner3DSW *, int> &Shape3DSW::get_owners() const { } Shape3DSW::Shape3DSW() { - custom_bias = 0; configured = false; } Shape3DSW::~Shape3DSW() { - ERR_FAIL_COND(owners.size()); } Plane PlaneShape3DSW::get_plane() const { - return plane; } void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - // gibberish, a plane is infinity r_min = -1e7; r_max = 1e7; } Vector3 PlaneShape3DSW::get_support(const Vector3 &p_normal) const { - return p_normal * 1e15; } 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; } bool PlaneShape3DSW::intersect_point(const Vector3 &p_point) const { - return plane.distance_to(p_point) < 0; } Vector3 PlaneShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - if (plane.is_point_over(p_point)) { return plane.project(p_point); } else { @@ -135,23 +124,19 @@ Vector3 PlaneShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } Vector3 PlaneShape3DSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); //wtf } void PlaneShape3DSW::_setup(const Plane &p_plane) { - plane = p_plane; configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2))); } void PlaneShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); } Variant PlaneShape3DSW::get_data() const { - return plane; } @@ -161,7 +146,6 @@ PlaneShape3DSW::PlaneShape3DSW() { // real_t RayShape3DSW::get_length() const { - return length; } @@ -170,24 +154,21 @@ bool RayShape3DSW::get_slips_on_slope() const { } void RayShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - // don't think this will be even used r_min = 0; r_max = 1; } 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 { - if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 2; r_supports[0] = Vector3(0, 0, 0); r_supports[1] = Vector3(0, 0, length); @@ -201,17 +182,14 @@ void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s } bool RayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return false; //simply not possible } bool RayShape3DSW::intersect_point(const Vector3 &p_point) const { - return false; //simply not possible } Vector3 RayShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - Vector3 s[2] = { Vector3(0, 0, 0), Vector3(0, 0, length) @@ -221,25 +199,21 @@ Vector3 RayShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } Vector3 RayShape3DSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); } void RayShape3DSW::_setup(real_t p_length, bool p_slips_on_slope) { - length = p_length; slips_on_slope = p_slips_on_slope; configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); } void RayShape3DSW::set_data(const Variant &p_data) { - Dictionary d = p_data; _setup(d["length"], d["slips_on_slope"]); } Variant RayShape3DSW::get_data() const { - Dictionary d; d["length"] = length; d["slips_on_slope"] = slips_on_slope; @@ -247,7 +221,6 @@ Variant RayShape3DSW::get_data() const { } RayShape3DSW::RayShape3DSW() { - length = 1; slips_on_slope = false; } @@ -255,12 +228,10 @@ RayShape3DSW::RayShape3DSW() { /********** SPHERE *************/ real_t SphereShape3DSW::get_radius() const { - return radius; } void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - real_t d = p_normal.dot(p_transform.origin); // figure out scale at point @@ -272,66 +243,56 @@ void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_ } Vector3 SphereShape3DSW::get_support(const Vector3 &p_normal) const { - return p_normal * radius; } void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - *r_supports = p_normal * radius; r_amount = 1; } bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); } bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const { - return p_point.length() < radius; } 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; } Vector3 SphereShape3DSW::get_moment_of_inertia(real_t p_mass) const { - real_t s = 0.4 * p_mass * radius * radius; return Vector3(s, s, s); } void SphereShape3DSW::_setup(real_t p_radius) { - radius = p_radius; configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); } void SphereShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); } Variant SphereShape3DSW::get_data() const { - return radius; } SphereShape3DSW::SphereShape3DSW() { - radius = 0; } /********** BOX *************/ void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - // no matter the angle, the box is mirrored anyway Vector3 local_normal = p_transform.basis.xform_inv(p_normal); @@ -343,7 +304,6 @@ void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_tra } Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 point( (p_normal.x < 0) ? -half_extents.x : half_extents.x, (p_normal.y < 0) ? -half_extents.y : half_extents.y, @@ -353,17 +313,14 @@ Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const { } void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - static const int next[3] = { 1, 2, 0 }; static const int next2[3] = { 2, 0, 1 }; for (int i = 0; i < 3; i++) { - Vector3 axis; axis[i] = 1.0; real_t dot = p_normal.dot(axis); if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - //Vector3 axis_b; bool neg = dot < 0; @@ -384,7 +341,6 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s }; for (int j = 0; j < 4; j++) { - point[i_n] = sign[j][0] * half_extents[i_n]; point[i_n2] = sign[j][1] * half_extents[i_n2]; r_supports[j] = neg ? -point : point; @@ -402,12 +358,10 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s } for (int i = 0; i < 3; i++) { - Vector3 axis; axis[i] = 1.0; if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 2; int i_n = next[i]; @@ -440,24 +394,20 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s } bool BoxShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - AABB aabb(-half_extents, half_extents * 2.0); return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); } bool BoxShape3DSW::intersect_point(const Vector3 &p_point) const { - return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z); } Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - int outside = 0; Vector3 min_point; for (int i = 0; i < 3; i++) { - if (Math::abs(p_point[i]) > half_extents[i]) { outside++; if (outside == 1) { @@ -471,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; @@ -486,7 +438,6 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const { }; for (int i = 0; i < 3; i++) { - s[1] = closest_vertex; s[1][i] = -s[1][i]; //edge @@ -503,7 +454,6 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } Vector3 BoxShape3DSW::get_moment_of_inertia(real_t p_mass) const { - real_t lx = half_extents.x; real_t ly = half_extents.y; real_t lz = half_extents.z; @@ -512,19 +462,16 @@ Vector3 BoxShape3DSW::get_moment_of_inertia(real_t p_mass) const { } void BoxShape3DSW::_setup(const Vector3 &p_half_extents) { - half_extents = p_half_extents.abs(); configure(AABB(-half_extents, half_extents * 2)); } void BoxShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); } Variant BoxShape3DSW::get_data() const { - return half_extents; } @@ -534,7 +481,6 @@ BoxShape3DSW::BoxShape3DSW() { /********** CAPSULE *************/ void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); real_t h = (n.z > 0) ? height : -height; @@ -546,7 +492,6 @@ void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform &p } Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 n = p_normal; real_t h = (n.z > 0) ? height : -height; @@ -557,13 +502,11 @@ Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const { } void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - Vector3 n = p_normal; real_t d = n.z; if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - // make it flat n.z = 0.0; n.normalize(); @@ -576,7 +519,6 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 r_supports[1].z -= height * 0.5; } else { - real_t h = (d > 0) ? height : -height; n *= radius; @@ -587,7 +529,6 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 } bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - Vector3 norm = (p_end - p_begin).normalized(); real_t min_d = 1e20; @@ -637,7 +578,6 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } if (collision) { - r_result = res; r_normal = n; } @@ -645,7 +585,6 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const { - if (Math::abs(p_point.z) < height * 0.5) { return Vector3(p_point.x, p_point.y, 0).length() < radius; } else { @@ -656,7 +595,6 @@ bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const { } Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - Vector3 s[2] = { Vector3(0, 0, -height * 0.5), Vector3(0, 0, height * 0.5), @@ -664,14 +602,14 @@ 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; } Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation Vector3 extents = get_aabb().size * 0.5; @@ -682,14 +620,12 @@ Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const { } void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) { - height = p_height; radius = p_radius; configure(AABB(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0))); } void CapsuleShape3DSW::set_data(const Variant &p_data) { - Dictionary d = p_data; ERR_FAIL_COND(!d.has("radius")); ERR_FAIL_COND(!d.has("height")); @@ -697,7 +633,6 @@ void CapsuleShape3DSW::set_data(const Variant &p_data) { } Variant CapsuleShape3DSW::get_data() const { - Dictionary d; d["radius"] = radius; d["height"] = height; @@ -705,46 +640,45 @@ Variant CapsuleShape3DSW::get_data() const { } CapsuleShape3DSW::CapsuleShape3DSW() { - height = radius = 0; } /********** CONVEX POLYGON *************/ 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; + } } } Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 n = p_normal; int vert_support_idx = -1; 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]; for (int i = 0; i < vertex_count; i++) { - real_t d = n.dot(vrts[i]); if (i == 0 || d > support_max) { @@ -757,7 +691,6 @@ Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const { } void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); @@ -772,7 +705,6 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve int vtx = 0; for (int i = 0; i < vc; i++) { - real_t d = p_normal.dot(vertices[i]); if (i == 0 || d > max) { @@ -782,9 +714,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve } for (int i = 0; i < fc; i++) { - if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - int ic = faces[i].indices.size(); const int *ind = faces[i].indices.ptr(); @@ -796,12 +726,12 @@ 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++) { - r_supports[j] = vertices[ind[j]]; } r_amount = m; @@ -810,11 +740,9 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve } for (int i = 0; i < ec; i++) { - real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal); dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { - r_amount = 2; r_supports[0] = vertices[edges[i].a]; r_supports[1] = vertices[edges[i].b]; @@ -827,7 +755,6 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve } bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); @@ -838,15 +765,14 @@ 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(); for (int j = 1; j < ic - 1; j++) { - Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]); Vector3 result; if (f.intersects_segment(p_begin, p_end, &result)) { @@ -867,30 +793,28 @@ bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vec } bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const { - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); 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; } Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - const Geometry::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); const Vector3 *vertices = mesh.vertices.ptr(); 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; @@ -898,7 +822,6 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con const int *indices = faces[i].indices.ptr(); for (int j = 0; j < ic; j++) { - Vector3 a = vertices[indices[j]]; Vector3 b = vertices[indices[(j + 1) % ic]]; Vector3 n = (a - b).cross(faces[i].plane.normal).normalized(); @@ -924,7 +847,6 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); int ec = mesh.edges.size(); for (int i = 0; i < ec; i++) { - Vector3 s[2] = { vertices[edges[i].a], vertices[edges[i].b] @@ -942,7 +864,6 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con } Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation Vector3 extents = get_aabb().size * 0.5; @@ -953,31 +874,29 @@ 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); } void ConvexPolygonShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); } Variant ConvexPolygonShape3DSW::get_data() const { - return mesh.vertices; } @@ -987,27 +906,25 @@ ConvexPolygonShape3DSW::ConvexPolygonShape3DSW() { /********** FACE POLYGON *************/ void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - for (int i = 0; i < 3; i++) { - 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; + } } } Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const { - int vert_support_idx = -1; real_t support_max = 0; for (int i = 0; i < 3; i++) { - real_t d = p_normal.dot(vertex[i]); if (i == 0 || d > support_max) { @@ -1020,15 +937,12 @@ Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const { } void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - Vector3 n = p_normal; /** TEST FACE AS SUPPORT **/ if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 3; for (int i = 0; i < 3; i++) { - r_supports[i] = vertex[i]; } return; @@ -1040,7 +954,6 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ real_t support_max = 0; for (int i = 0; i < 3; i++) { - real_t d = n.dot(vertex[i]); if (i == 0 || d > support_max) { @@ -1052,16 +965,15 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ /** TEST EDGES AS SUPPORT **/ 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); dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { - r_amount = 2; r_supports[0] = vertex[i]; r_supports[1] = vertex[nx]; @@ -1074,7 +986,6 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ } bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - bool c = Geometry::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); if (c) { r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; @@ -1087,36 +998,29 @@ bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_e } bool FaceShape3DSW::intersect_point(const Vector3 &p_point) const { - return false; //face is flat } Vector3 FaceShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); } Vector3 FaceShape3DSW::get_moment_of_inertia(real_t p_mass) const { - return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! } FaceShape3DSW::FaceShape3DSW() { - configure(AABB()); } Vector<Vector3> ConcavePolygonShape3DSW::get_faces() const { - Vector<Vector3> rfaces; rfaces.resize(faces.size() * 3); for (int i = 0; i < faces.size(); i++) { - Face f = faces.get(i); for (int j = 0; j < 3; j++) { - rfaces.set(i * 3 + j, vertices.get(f.indices[j])); } } @@ -1125,7 +1029,6 @@ Vector<Vector3> ConcavePolygonShape3DSW::get_faces() const { } void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - int count = vertices.size(); if (count == 0) { r_min = 0; @@ -1135,21 +1038,22 @@ void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Trans const Vector3 *vptr = vertices.ptr(); 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(); @@ -1159,7 +1063,6 @@ Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const { real_t support_max = 0; for (int i = 0; i < count; i++) { - real_t d = n.dot(vptr[i]); if (i == 0 || d > support_max) { @@ -1172,7 +1075,6 @@ Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const { } void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_params) const { - const BVH *bvh = &p_params->bvh[p_idx]; /* @@ -1182,12 +1084,10 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par //printf("addr: %p\n",bvh); if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { - return; } if (bvh->face_index >= 0) { - Vector3 res; Vector3 vertices[3] = { p_params->vertices[p_params->faces[bvh->face_index].indices[0]], @@ -1202,11 +1102,9 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par vertices[1], vertices[2], &res)) { - real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); //TODO, seems segmen/triangle intersection is broken :( if (d > 0 && d < p_params->min_d) { - p_params->min_d = d; p_params->result = res; p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).normal; @@ -1215,18 +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(); @@ -1248,35 +1147,30 @@ bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Ve _cull_segment(0, ¶ms); if (params.collisions > 0) { - r_result = params.result; r_normal = params.normal; return true; } else { - return false; } } bool ConcavePolygonShape3DSW::intersect_point(const Vector3 &p_point) const { - return false; //face is flat } Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - return Vector3(); } 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]; FaceShape3DSW *face = p_params->face; face->normal = f->normal; @@ -1286,24 +1180,21 @@ void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const { p_params->callback(p_params->userdata, face); } else { - if (bvh->left >= 0) { - _cull(bvh->left, p_params); } if (bvh->right >= 0) { - _cull(bvh->right, p_params); } } } 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; @@ -1328,7 +1219,6 @@ void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback } Vector3 ConcavePolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation Vector3 extents = get_aabb().size * 0.5; @@ -1339,38 +1229,30 @@ Vector3 ConcavePolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const { } struct _VolumeSW_BVH_Element { - AABB aabb; Vector3 center; int face_index; }; struct _VolumeSW_BVH_CompareX { - _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.x < b.center.x; } }; struct _VolumeSW_BVH_CompareY { - _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.y < b.center.y; } }; struct _VolumeSW_BVH_CompareZ { - _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const { - return a.center.z < b.center.z; } }; struct _VolumeSW_BVH { - AABB aabb; _VolumeSW_BVH *left; _VolumeSW_BVH *right; @@ -1379,7 +1261,6 @@ struct _VolumeSW_BVH { }; _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) { - _VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH); if (p_size == 1) { @@ -1391,34 +1272,29 @@ _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_siz count++; return bvh; } else { - bvh->face_index = -1; } 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()) { - case 0: { - SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x; sort_x.sort(p_elements, p_size); } break; case 1: { - SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y; sort_y.sort(p_elements, p_size); } break; case 2: { - SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z; sort_z.sort(p_elements, p_size); } break; @@ -1434,7 +1310,6 @@ _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_siz } void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) { - int idx = p_idx; p_bvh_array[idx].aabb = p_bvh_tree->aabb; @@ -1446,7 +1321,6 @@ void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_ar _fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx); } else { - p_bvh_array[p_idx].left = -1; } @@ -1455,7 +1329,6 @@ void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_ar _fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx); } else { - p_bvh_array[p_idx].right = -1; } @@ -1463,7 +1336,6 @@ void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_ar } void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) { - int src_face_count = p_faces.size(); if (src_face_count == 0) { configure(AABB()); @@ -1489,7 +1361,6 @@ void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) { AABB _aabb; for (int i = 0; i < src_face_count; i++) { - Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); bvh_arrayw[i].aabb = face.get_aabb(); @@ -1502,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; @@ -1522,12 +1394,10 @@ void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) { } void ConcavePolygonShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); } Variant ConcavePolygonShape3DSW::get_data() const { - return get_faces(); } @@ -1537,36 +1407,32 @@ ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { /* HEIGHT MAP SHAPE */ Vector<real_t> HeightMapShape3DSW::get_heights() const { - return heights; } -int HeightMapShape3DSW::get_width() const { +int HeightMapShape3DSW::get_width() const { return width; } -int HeightMapShape3DSW::get_depth() const { +int HeightMapShape3DSW::get_depth() const { return depth; } -real_t HeightMapShape3DSW::get_cell_size() const { +real_t HeightMapShape3DSW::get_cell_size() const { return cell_size; } void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - //not very useful, but not very used either p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max); } Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const { - //not very useful, but not very used either return get_aabb().get_support(p_normal); } bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { - return false; } @@ -1575,7 +1441,6 @@ bool HeightMapShape3DSW::intersect_point(const Vector3 &p_point) const { } Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const { - return Vector3(); } @@ -1583,7 +1448,6 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, voi } Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation Vector3 extents = get_aabb().size * 0.5; @@ -1594,7 +1458,6 @@ Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const { } void HeightMapShape3DSW::_setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) { - heights = p_heights; width = p_width; depth = p_depth; @@ -1605,16 +1468,15 @@ void HeightMapShape3DSW::_setup(Vector<real_t> p_heights, int p_width, int p_dep AABB aabb; for (int i = 0; i < depth; i++) { - for (int j = 0; j < width; j++) { - 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); + } } } @@ -1622,7 +1484,6 @@ void HeightMapShape3DSW::_setup(Vector<real_t> p_heights, int p_width, int p_dep } void HeightMapShape3DSW::set_data(const Variant &p_data) { - ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); Dictionary d = p_data; ERR_FAIL_COND(!d.has("width")); @@ -1643,12 +1504,10 @@ void HeightMapShape3DSW::set_data(const Variant &p_data) { } Variant HeightMapShape3DSW::get_data() const { - ERR_FAIL_V(Variant()); } HeightMapShape3DSW::HeightMapShape3DSW() { - width = 0; depth = 0; cell_size = 0; diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index dd29ec849b..848a1135ad 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -56,7 +56,6 @@ public: }; class Shape3DSW { - RID self; AABB aabb; bool configured; @@ -108,7 +107,6 @@ public: }; class ConcaveShape3DSW : public Shape3DSW { - public: virtual bool is_concave() const { return true; } typedef void (*Callback)(void *p_userdata, Shape3DSW *p_convex); @@ -120,7 +118,6 @@ public: }; class PlaneShape3DSW : public Shape3DSW { - Plane plane; void _setup(const Plane &p_plane); @@ -146,7 +143,6 @@ public: }; class RayShape3DSW : public Shape3DSW { - real_t length; bool slips_on_slope; @@ -175,7 +171,6 @@ public: }; class SphereShape3DSW : public Shape3DSW { - real_t radius; void _setup(real_t p_radius); @@ -203,7 +198,6 @@ public: }; class BoxShape3DSW : public Shape3DSW { - Vector3 half_extents; void _setup(const Vector3 &p_half_extents); @@ -229,7 +223,6 @@ public: }; class CapsuleShape3DSW : public Shape3DSW { - real_t height; real_t radius; @@ -259,7 +252,6 @@ public: }; struct ConvexPolygonShape3DSW : public Shape3DSW { - Geometry::MeshData mesh; void _setup(const Vector<Vector3> &p_vertices); @@ -291,7 +283,6 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { // always a trimesh struct Face { - Vector3 normal; int indices[3]; }; @@ -300,7 +291,6 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { Vector<Vector3> vertices; struct BVH { - AABB aabb; int left; int right; @@ -311,7 +301,6 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { Vector<BVH> bvh; struct _CullParams { - AABB aabb; Callback callback; void *userdata; @@ -322,7 +311,6 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { }; struct _SegmentCullParams { - Vector3 from; Vector3 to; const Face *faces; @@ -366,7 +354,6 @@ public: }; struct HeightMapShape3DSW : public ConcaveShape3DSW { - Vector<real_t> heights; int width; int depth; @@ -403,7 +390,6 @@ public: //used internally struct FaceShape3DSW : public Shape3DSW { - Vector3 normal; //cache Vector3 vertex[3]; @@ -427,14 +413,12 @@ struct FaceShape3DSW : public Shape3DSW { }; struct MotionShape3DSW : public Shape3DSW { - Shape3DSW *shape; Vector3 motion; virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { - Vector3 cast = p_transform.basis.xform(motion); real_t mina, maxa; real_t minb, maxb; @@ -447,7 +431,6 @@ struct MotionShape3DSW : public Shape3DSW { } Vector3 get_support(const Vector3 &p_normal) const { - Vector3 support = shape->get_support(p_normal); if (p_normal.dot(motion) > 0) { support += motion; diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 66d17b3182..4d272bdabd 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -35,22 +35,22 @@ #include "physics_server_3d_sw.h" _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *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() == 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; } int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &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) { - ERR_FAIL_COND_V(space->locked, false); int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int cc = 0; @@ -58,17 +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]; @@ -76,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; @@ -94,7 +98,6 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe } bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vector3 &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, bool p_pick_ray) { - ERR_FAIL_COND_V(space->locked, false); Vector3 begin, end; @@ -114,15 +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]; @@ -137,14 +142,12 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec Vector3 shape_point, shape_normal; if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { - Transform 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(); @@ -155,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(); @@ -172,9 +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); @@ -188,30 +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; } @@ -223,7 +232,6 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans } bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &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, ShapeRestInfo *r_info) { - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, false); @@ -246,12 +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]; @@ -290,10 +299,8 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor bool collided = !CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); if (collided) { - hi = ofs; } else { - point_A = lA; point_B = lB; low = ofs; @@ -329,9 +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) - return 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); @@ -353,9 +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,7 +382,6 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ } struct _RestCallbackData { - const CollisionObject3DSW *object; const CollisionObject3DSW *best_object; int shape; @@ -387,15 +393,16 @@ struct _RestCallbackData { }; static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { - _RestCallbackData *rd = (_RestCallbackData *)p_userdata; 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,8 +410,8 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, rd->best_object = rd->object; rd->best_shape = rd->shape; } -bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, 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) { +bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, 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) { Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -420,25 +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; @@ -446,7 +456,6 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap r_info->point = rcd.best_contact; r_info->rid = rcd.best_object->get_self(); if (rcd.best_object->get_type() == CollisionObject3DSW::TYPE_BODY) { - const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object); r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos); @@ -459,7 +468,6 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap } Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { - CollisionObject3DSW *obj = PhysicsServer3DSW::singleton->area_owner.getornull(p_object); if (!obj) { obj = PhysicsServer3DSW::singleton->body_owner.getornull(p_object); @@ -474,9 +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); @@ -500,33 +508,30 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob } PhysicsDirectSpaceState3DSW::PhysicsDirectSpaceState3DSW() { - space = nullptr; } //////////////////////////////////////////////////////////////////////////////////////////////////////////// int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &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() == 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) { SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); @@ -541,15 +546,14 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { } int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) { - AABB 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 (!shapes_found) { body_aabb = p_body->get_shape_aabb(i); @@ -587,7 +591,6 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; do { - Vector3 recover_motion; bool collided = false; @@ -595,18 +598,19 @@ 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); for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; @@ -649,7 +653,6 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra 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(); @@ -696,7 +699,6 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra } bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::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 @@ -712,9 +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); @@ -747,7 +749,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons Vector3 sr[max_results * 2]; do { - PhysicsServer3DSW::CollCbkData cbk; cbk.max = max_results; cbk.amount = 0; @@ -761,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); @@ -771,7 +773,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; @@ -788,7 +789,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons Vector3 recover_motion; for (int i = 0; i < cbk.amount; i++) { - Vector3 a = sr[i * 2 + 0]; Vector3 b = sr[i * 2 + 1]; recover_motion += (b - a) * 0.4; @@ -821,9 +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); @@ -843,7 +843,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons real_t best_unsafe = 1; for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; @@ -881,10 +880,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons bool collided = !CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep); if (collided) { - hi = ofs; } else { - point_A = lA; point_B = lB; low = ofs; @@ -898,7 +895,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } if (stuck) { - safe = 0; unsafe = 0; best_shape = j; //sadly it's the best @@ -908,7 +904,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons continue; } if (best_safe < safe) { - safe = best_safe; unsafe = best_unsafe; best_shape = j; @@ -921,14 +916,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons //not collided collided = false; if (r_result) { - r_result->motion = p_motion; r_result->remainder = Vector3(); r_result->motion += (body_transform.get_origin() - p_from.get_origin()); } } else { - //it collided, let's get the rest info in unsafe advance Transform ugt = body_transform; ugt.origin += p_motion * unsafe; @@ -947,19 +940,18 @@ 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 i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; 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) { - if (r_result) { r_result->collider = rcd.best_object->get_self(); r_result->collider_id = rcd.best_object->get_instance_id(); @@ -982,7 +974,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons collided = true; } else { if (r_result) { - r_result->motion = p_motion; r_result->remainder = Vector3(); r_result->motion += (body_transform.get_origin() - p_from.get_origin()); @@ -996,11 +987,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self) { - CollisionObject3DSW::Type type_A = A->get_type(); CollisionObject3DSW::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); @@ -1011,21 +1000,17 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll self->collision_pairs++; if (type_A == CollisionObject3DSW::TYPE_AREA) { - Area3DSW *area = static_cast<Area3DSW *>(A); if (type_B == CollisionObject3DSW::TYPE_AREA) { - Area3DSW *area_b = static_cast<Area3DSW *>(B); Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A)); return area2_pair; } else { - Body3DSW *body = static_cast<Body3DSW *>(B); AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A)); return area_pair; } } else { - BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B)); return b; } @@ -1034,7 +1019,6 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll } void Space3DSW::_broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self) { - Space3DSW *self = (Space3DSW *)p_self; self->collision_pairs--; Constraint3DSW *c = (Constraint3DSW *)p_data; @@ -1042,94 +1026,79 @@ void Space3DSW::_broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, Col } const SelfList<Body3DSW>::List &Space3DSW::get_active_body_list() const { - return active_list; } -void Space3DSW::body_add_to_active_list(SelfList<Body3DSW> *p_body) { +void Space3DSW::body_add_to_active_list(SelfList<Body3DSW> *p_body) { active_list.add(p_body); } -void Space3DSW::body_remove_from_active_list(SelfList<Body3DSW> *p_body) { +void Space3DSW::body_remove_from_active_list(SelfList<Body3DSW> *p_body) { active_list.remove(p_body); } void Space3DSW::body_add_to_inertia_update_list(SelfList<Body3DSW> *p_body) { - inertia_update_list.add(p_body); } void Space3DSW::body_remove_from_inertia_update_list(SelfList<Body3DSW> *p_body) { - inertia_update_list.remove(p_body); } BroadPhase3DSW *Space3DSW::get_broadphase() { - return broadphase; } void Space3DSW::add_object(CollisionObject3DSW *p_object) { - ERR_FAIL_COND(objects.has(p_object)); objects.insert(p_object); } void Space3DSW::remove_object(CollisionObject3DSW *p_object) { - ERR_FAIL_COND(!objects.has(p_object)); objects.erase(p_object); } const Set<CollisionObject3DSW *> &Space3DSW::get_objects() const { - return objects; } void Space3DSW::body_add_to_state_query_list(SelfList<Body3DSW> *p_body) { - state_query_list.add(p_body); } -void Space3DSW::body_remove_from_state_query_list(SelfList<Body3DSW> *p_body) { +void Space3DSW::body_remove_from_state_query_list(SelfList<Body3DSW> *p_body) { state_query_list.remove(p_body); } void Space3DSW::area_add_to_monitor_query_list(SelfList<Area3DSW> *p_area) { - monitor_query_list.add(p_area); } -void Space3DSW::area_remove_from_monitor_query_list(SelfList<Area3DSW> *p_area) { +void Space3DSW::area_remove_from_monitor_query_list(SelfList<Area3DSW> *p_area) { monitor_query_list.remove(p_area); } void Space3DSW::area_add_to_moved_list(SelfList<Area3DSW> *p_area) { - area_moved_list.add(p_area); } void Space3DSW::area_remove_from_moved_list(SelfList<Area3DSW> *p_area) { - area_moved_list.remove(p_area); } const SelfList<Area3DSW>::List &Space3DSW::get_moved_area_list() const { - return area_moved_list; } void Space3DSW::call_queries() { - while (state_query_list.first()) { - Body3DSW *b = state_query_list.first()->self(); state_query_list.remove(state_query_list.first()); b->call_queries(); } while (monitor_query_list.first()) { - Area3DSW *a = monitor_query_list.first()->self(); monitor_query_list.remove(monitor_query_list.first()); a->call_queries(); @@ -1137,7 +1106,6 @@ void Space3DSW::call_queries() { } void Space3DSW::setup() { - contact_debug_count = 0; while (inertia_update_list.first()) { inertia_update_list.first()->self()->update_inertias(); @@ -1146,14 +1114,11 @@ void Space3DSW::setup() { } void Space3DSW::update() { - broadphase->update(); } void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) { - switch (p_param) { - case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius = p_value; break; @@ -1185,9 +1150,7 @@ void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_valu } real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const { - switch (p_param) { - case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius; case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: @@ -1211,27 +1174,22 @@ real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const { } void Space3DSW::lock() { - locked = true; } void Space3DSW::unlock() { - locked = false; } bool Space3DSW::is_locked() const { - return locked; } PhysicsDirectSpaceState3DSW *Space3DSW::get_direct_state() { - return direct_access; } Space3DSW::Space3DSW() { - collision_pairs = 0; active_objects = 0; island_count = 0; @@ -1258,12 +1216,12 @@ 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() { - memdelete(broadphase); memdelete(direct_access); } diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index a0b4379376..f9198e3d40 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -42,7 +42,6 @@ #include "core/typedefs.h" class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D { - GDCLASS(PhysicsDirectSpaceState3DSW, PhysicsDirectSpaceState3D); public: @@ -60,7 +59,6 @@ public: }; class Space3DSW { - public: enum ElapsedTime { ELAPSED_TIME_INTEGRATE_FORCES, @@ -187,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 1a7d5f8cec..9a2a0073a1 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -34,33 +34,33 @@ #include "core/os/os.h" void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island) { - p_body->set_island_step(_step); p_body->set_island_next(*p_island); *p_island = p_body; 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); } } } void Step3DSW::_setup_island(Constraint3DSW *p_island, real_t p_delta) { - Constraint3DSW *ci = p_island; while (ci) { ci->setup(p_delta); @@ -70,13 +70,10 @@ void Step3DSW::_setup_island(Constraint3DSW *p_island, real_t p_delta) { } void Step3DSW::_solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_delta) { - int at_priority = 1; while (p_island) { - for (int i = 0; i < p_iterations; i++) { - Constraint3DSW *ci = p_island; while (ci) { ci->solve(p_delta); @@ -97,7 +94,6 @@ void Step3DSW::_solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_island = ci->get_island_next(); } } else { - prev = ci; } @@ -108,19 +104,18 @@ void Step3DSW::_solve_island(Constraint3DSW *p_island, int p_iterations, real_t } void Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) { - bool can_sleep = true; Body3DSW *b = p_island; while (b) { - if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::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(); } @@ -129,7 +124,6 @@ void Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) { b = p_island; while (b) { - if (b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { b = b->get_island_next(); continue; //ignore for static @@ -137,15 +131,15 @@ 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(); } } void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { - p_space->lock(); // can't access space during this p_space->setup(); //update inertias, etc @@ -161,7 +155,6 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { const SelfList<Body3DSW> *b = body_list->first(); while (b) { - b->self()->integrate_forces(p_delta); b = b->next(); active_count++; @@ -187,7 +180,6 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { Body3DSW *body = b->self(); if (body->get_island_step() != _step) { - Body3DSW *island = nullptr; Constraint3DSW *constraint_island = nullptr; _populate_island(body, &island, &constraint_island); @@ -210,10 +202,10 @@ 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); @@ -233,7 +225,6 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { { Constraint3DSW *ci = constraint_island_list; while (ci) { - _setup_island(ci, p_delta); ci = ci->get_island_list_next(); } @@ -276,7 +267,6 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { { Body3DSW *bi = island_list; while (bi) { - _check_suspend(bi, p_delta); bi = bi->get_island_list_next(); } @@ -294,6 +284,5 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { } Step3DSW::Step3DSW() { - _step = 1; } diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h index c735688a9e..9dbb61308f 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -34,7 +34,6 @@ #include "space_3d_sw.h" class Step3DSW { - uint64_t _step; void _populate_island(Body3DSW *p_body, Body3DSW **p_island, Constraint3DSW **p_constraint_island); |