diff options
Diffstat (limited to 'servers/physics_3d')
35 files changed, 0 insertions, 859 deletions
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp index 2367b653a5..59041cb5e0 100644 --- a/servers/physics_3d/area_3d_sw.cpp +++ b/servers/physics_3d/area_3d_sw.cpp @@ -46,13 +46,11 @@ 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()) get_space()->area_add_to_moved_list(&moved_list); } void Area3DSW::set_transform(const Transform &p_transform) { - if (!moved_list.in_list() && get_space()) get_space()->area_add_to_moved_list(&moved_list); @@ -61,7 +59,6 @@ void Area3DSW::set_transform(const Transform &p_transform) { } void Area3DSW::set_space(Space3DSW *p_space) { - if (get_space()) { if (monitor_query_list.in_list()) get_space()->area_remove_from_monitor_query_list(&monitor_query_list); @@ -76,7 +73,6 @@ void 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; @@ -97,7 +93,6 @@ void Area3DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { } 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; @@ -127,7 +122,6 @@ void Area3DSW::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_ } 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 +151,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,7 +174,6 @@ 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()) @@ -189,7 +181,6 @@ void Area3DSW::_queue_monitor_update() { } void Area3DSW::set_monitorable(bool p_monitorable) { - if (monitorable == p_monitorable) return; @@ -198,9 +189,7 @@ void Area3DSW::set_monitorable(bool p_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++) @@ -214,7 +203,6 @@ void Area3DSW::call_queries() { } for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) { - if (E->get().state == 0) continue; //nothing happened @@ -232,7 +220,6 @@ void 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++) @@ -246,7 +233,6 @@ void Area3DSW::call_queries() { } for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) { - if (E->get().state == 0) continue; //nothing happened @@ -269,7 +255,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..f64c84b5ab 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,18 +62,14 @@ 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 return body_shape < p_key.body_shape; @@ -88,7 +83,6 @@ class Area3DSW : public CollisionObject3DSW { }; struct BodyState { - int state; _FORCE_INLINE_ void inc() { state++; } _FORCE_INLINE_ void dec() { state--; } @@ -171,14 +165,12 @@ 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()) _queue_monitor_update(); } void Area3DSW::remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { - BodyKey bk(p_body, p_body_shape, p_area_shape); monitored_bodies[bk].dec(); if (!monitor_query_list.in_list()) @@ -186,14 +178,12 @@ void Area3DSW::remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, u } 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()) _queue_monitor_update(); } void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { - BodyKey bk(p_area, p_area_shape, p_self_shape); monitored_areas[bk].dec(); if (!monitor_query_list.in_list()) diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index fa2fb2dabb..36d460675e 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,16 +41,13 @@ bool AreaPair3DSW::setup(real_t p_step) { } if (result != colliding) { - if (result) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) body->add_area(area); if (area->has_monitor_callback()) area->add_body_to_query(body, body_shape, area_shape); } else { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) body->remove_area(area); if (area->has_monitor_callback()) @@ -68,7 +64,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; @@ -81,9 +76,7 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, } AreaPair3DSW::~AreaPair3DSW() { - if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) body->remove_area(area); if (area->has_monitor_callback()) @@ -96,7 +89,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,9 +97,7 @@ bool Area2Pair3DSW::setup(real_t p_step) { } if (result != colliding) { - if (result) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) area_b->add_area_to_query(area_a, shape_a, shape_b); @@ -115,7 +105,6 @@ bool Area2Pair3DSW::setup(real_t p_step) { area_a->add_area_to_query(area_b, shape_b, shape_a); } else { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) area_b->remove_area_from_query(area_a, shape_a, shape_b); @@ -133,7 +122,6 @@ void 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,9 +132,7 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area } Area2Pair3DSW::~Area2Pair3DSW() { - if (colliding) { - if (area_b->has_area_monitor_callback()) area_b->remove_area_from_query(area_a, shape_a, shape_b); diff --git a/servers/physics_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 2f2525bb75..c8a6a2303e 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -33,13 +33,11 @@ #include "space_3d_sw.h" void Body3DSW::_update_inertia() { - 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 +50,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 +80,6 @@ void Body3DSW::update_inertias() { inertia_tensor.set_zero(); for (int i = 0; i < get_shape_count(); i++) { - if (is_shape_disabled(i)) { continue; } @@ -121,12 +114,10 @@ void Body3DSW::update_inertias() { 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,7 +130,6 @@ void Body3DSW::update_inertias() { } void Body3DSW::set_active(bool p_active) { - if (active == p_active) return; @@ -169,14 +159,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 +176,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 +187,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 +201,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 +215,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 +222,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 +235,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); @@ -279,20 +255,16 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { */ } 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(); @@ -320,7 +292,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; @@ -361,7 +332,6 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va } } Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const { - switch (p_state) { case PhysicsServer3D::BODY_STATE_TRANSFORM: { return get_transform(); @@ -384,9 +354,7 @@ 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()) get_space()->body_remove_from_inertia_update_list(&inertia_update_list); if (active_list.in_list()) @@ -398,7 +366,6 @@ void Body3DSW::set_space(Space3DSW *p_space) { _set_space(p_space); if (get_space()) { - _update_inertia(); if (active) get_space()->body_add_to_active_list(&active_list); @@ -415,7 +382,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,7 +410,6 @@ bool Body3DSW::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const { } void Body3DSW::integrate_forces(real_t p_step) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC) return; @@ -509,7 +474,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; @@ -574,7 +538,6 @@ void Body3DSW::integrate_forces(real_t p_step) { } void Body3DSW::integrate_velocities(real_t p_step) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC) return; @@ -598,7 +561,6 @@ 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()) @@ -676,15 +638,12 @@ 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()) continue; Body3DSW *b = n[i]; @@ -698,9 +657,7 @@ void Body3DSW::wakeup_neighbours() { } void Body3DSW::call_queries() { - if (fi_callback) { - PhysicsDirectBodyState3DSW *dbs = PhysicsDirectBodyState3DSW::singleton; dbs->body = this; @@ -708,7 +665,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,7 +677,6 @@ void Body3DSW::call_queries() { } bool Body3DSW::sleep_test(real_t p_step) { - if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) return true; // else if (mode == PhysicsServer3D::BODY_MODE_CHARACTER) @@ -730,27 +685,22 @@ bool Body3DSW::sleep_test(real_t p_step) { 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; @@ -768,7 +718,6 @@ Body3DSW::Body3DSW() : active_list(this), inertia_update_list(this), direct_state_query_list(this) { - mode = PhysicsServer3D::BODY_MODE_RIGID; active = true; @@ -801,7 +750,6 @@ Body3DSW::Body3DSW() : } Body3DSW::~Body3DSW() { - if (fi_callback) memdelete(fi_callback); } @@ -809,6 +757,5 @@ Body3DSW::~Body3DSW() { 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 0308d8689e..2ce8f99428 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; @@ -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; @@ -223,18 +219,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 +239,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); } @@ -310,12 +300,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 +314,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,7 +330,6 @@ 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) @@ -356,11 +342,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,7 +352,6 @@ 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) @@ -387,7 +370,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..e7c6738698 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,7 +160,6 @@ 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) @@ -220,7 +209,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 +239,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,7 +257,6 @@ 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) bias = shape_B_ptr->get_custom_bias(); else if (shape_B_ptr->get_custom_bias() == 0) @@ -282,7 +268,6 @@ bool BodyPair3DSW::setup(real_t p_step) { 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 +326,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,12 +338,10 @@ bool BodyPair3DSW::setup(real_t p_step) { } void BodyPair3DSW::solve(real_t p_step) { - if (!collided) return; for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; if (!c.active) continue; @@ -375,7 +357,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 +373,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 +394,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 +421,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 +438,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 +453,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 +465,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..6a679a1040 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,22 @@ 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) { - 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) { - 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 +72,6 @@ void BroadPhase3DBasic::remove(ID p_id) { } } while (to_erase.size()) { - pair_map.erase(to_erase.front()->get()); to_erase.pop_front(); } @@ -86,33 +79,27 @@ 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 { - 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 { - 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++; @@ -125,14 +112,11 @@ 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++; @@ -144,14 +128,11 @@ int BroadPhase3DBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, return rc; } 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++; @@ -164,23 +145,18 @@ 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) { - 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(); @@ -200,7 +176,6 @@ void BroadPhase3DBasic::update() { } if (pair_ok && !E) { - void *data = nullptr; if (pair_callback) data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata); @@ -211,12 +186,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..b81ce31ef3 100644 --- a/servers/physics_3d/broad_phase_octree.cpp +++ b/servers/physics_3d/broad_phase_octree.cpp @@ -32,58 +32,47 @@ #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) { - 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 { - return !octree.is_pairable(p_id); } 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) return nullptr; @@ -92,7 +81,6 @@ void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, Collisio } 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) return; @@ -101,12 +89,10 @@ void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, Collisi } 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) { - unpair_callback = p_unpair_callback; unpair_userdata = p_userdata; } @@ -116,7 +102,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..f7e3b507ca 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; @@ -64,7 +62,6 @@ void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { //_shapes_changed(); } 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 +81,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,11 +91,9 @@ 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) continue; //should never get here with a null owner @@ -133,9 +126,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 +136,10 @@ void CollisionObject3DSW::_unregister_shapes() { } void CollisionObject3DSW::_update_shapes() { - 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 +161,10 @@ void CollisionObject3DSW::_update_shapes() { } void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) { - 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 +183,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 +198,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..0c320b3275 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; diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index 5096b080ab..b4902af8ec 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,7 +42,6 @@ struct _CollectorCallback { Vector3 *prev_axis; _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) { - if (swap) callback(p_point_B, p_point_A, userdata); else @@ -54,7 +52,6 @@ struct _CollectorCallback { 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 +61,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 +71,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 +82,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 +93,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; @@ -134,7 +127,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 +142,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 +149,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 +162,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 +179,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 +199,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) @@ -228,7 +215,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ } 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 +253,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 +269,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,7 +282,6 @@ class SeparatorAxisTest { public: _FORCE_INLINE_ bool test_previous_axis() { - if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) return test_axis(*callback->prev_axis); else @@ -306,7 +289,6 @@ public: } _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { - Vector3 axis = p_axis; if (Math::abs(axis.x) < CMP_EPSILON && @@ -361,7 +343,6 @@ public: } _FORCE_INLINE_ void generate_contacts() { - // nothing to do, don't generate if (best_axis == Vector3(0.0, 0.0, 0.0)) return; @@ -384,7 +365,6 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_A; i++) { supports_A[i] += -best_axis * margin_A; } @@ -398,7 +378,6 @@ public: } if (withMargin) { - for (int i = 0; i < support_count_B; i++) { supports_B[i] += best_axis * margin_B; } @@ -430,7 +409,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); @@ -449,7 +427,6 @@ static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform &p_tr 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); @@ -461,7 +438,6 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans // test faces for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); if (!separator.test_axis(axis)) @@ -487,7 +463,6 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform &p_trans // 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)) @@ -499,7 +474,6 @@ 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); @@ -542,7 +516,6 @@ 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); @@ -562,7 +535,6 @@ 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)) @@ -571,7 +543,6 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo // 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; @@ -587,7 +558,6 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo // 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; @@ -602,7 +572,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); @@ -619,7 +588,6 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran // 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,7 +608,6 @@ 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); @@ -652,7 +619,6 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor // 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)) @@ -662,7 +628,6 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor // 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)) @@ -671,9 +636,7 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform &p_transfor // 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())) @@ -718,7 +681,6 @@ 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); @@ -738,7 +700,6 @@ 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); @@ -749,7 +710,6 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i); if (!separator.test_axis(axis)) @@ -761,7 +721,6 @@ 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); @@ -798,7 +757,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); @@ -820,7 +778,6 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // 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)) @@ -837,7 +794,6 @@ 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); @@ -857,7 +813,6 @@ 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)) @@ -866,7 +821,6 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform // 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)) @@ -875,11 +829,9 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform // 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(); @@ -890,10 +842,8 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform } 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,7 +864,6 @@ 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); @@ -936,7 +885,6 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform 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); @@ -954,7 +902,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); @@ -971,7 +918,6 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo // 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)) @@ -981,11 +927,9 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo // 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())) @@ -994,10 +938,8 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo } 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,7 +959,6 @@ 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); @@ -1039,7 +980,6 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo 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]; @@ -1058,7 +998,6 @@ 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); @@ -1117,7 +1056,6 @@ 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); @@ -1136,7 +1074,6 @@ 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)) @@ -1146,7 +1083,6 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf // 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(); @@ -1158,7 +1094,6 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf // 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,7 +1101,6 @@ 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]); @@ -1182,7 +1116,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); @@ -1202,7 +1135,6 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra 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(); @@ -1214,7 +1146,6 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra return; for (int j = 0; j < 2; j++) { - // point-spheres Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis); @@ -1249,7 +1180,6 @@ 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); @@ -1278,7 +1208,6 @@ 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(); @@ -1288,7 +1217,6 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const // 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(); @@ -1298,11 +1226,9 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const // 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(); @@ -1313,14 +1239,11 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const } 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())) return; } @@ -1328,13 +1251,11 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const //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())) @@ -1343,13 +1264,11 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const } 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())) @@ -1363,7 +1282,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); @@ -1389,7 +1307,6 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform // 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(); @@ -1399,11 +1316,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform // 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(); @@ -1414,14 +1329,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform } 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())) return; } @@ -1429,13 +1341,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform //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())) @@ -1444,13 +1354,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform } 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())) @@ -1463,7 +1371,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..43e62ee452 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -37,7 +37,6 @@ //#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) return false; @@ -52,7 +51,6 @@ 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) continue; @@ -72,7 +70,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; @@ -104,7 +101,6 @@ bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform } struct _ConcaveCollisionInfo { - const Transform *transform_A; const Shape3DSW *shape_A; const Transform *transform_B; @@ -121,7 +117,6 @@ struct _ConcaveCollisionInfo { }; void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) { - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); cinfo.aabb_tests++; @@ -134,7 +129,6 @@ void CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex } 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 +152,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 +173,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,7 +187,6 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } if (type_A == PhysicsServer3D::SHAPE_PLANE) { - if (type_B == PhysicsServer3D::SHAPE_PLANE) return false; if (type_B == PhysicsServer3D::SHAPE_RAY) { @@ -209,7 +200,6 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } } else if (type_A == PhysicsServer3D::SHAPE_RAY) { - if (type_B == PhysicsServer3D::SHAPE_RAY) return false; @@ -220,7 +210,6 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } } else if (concave_B) { - if (concave_A) return false; @@ -230,13 +219,11 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo 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) @@ -248,7 +235,6 @@ void CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW 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,7 +244,6 @@ 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) return false; @@ -275,7 +260,6 @@ 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) { @@ -293,12 +277,10 @@ 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()) 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,7 +288,6 @@ 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()) return false; @@ -338,7 +319,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 +346,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 aaa7de7531..a937ebc426 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -915,11 +915,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,7 +927,6 @@ 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)) { 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 9f387ea5c5..77e6eacb69 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -155,7 +155,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 +302,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; @@ -388,7 +386,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); @@ -426,75 +423,60 @@ void Generic6DOFJoint3DSW::calcAnchorPos() { } // 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 +513,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 +598,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: { @@ -660,19 +624,15 @@ void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOF } } 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: { diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index dea1be5040..a77d78ccaa 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); @@ -397,9 +392,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 +423,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 +448,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; @@ -470,7 +460,6 @@ void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_val } } 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/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index dd6b315152..3f3c0f367d 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); @@ -127,7 +125,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 +139,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 +153,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 57ad64ca21..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); @@ -379,7 +376,6 @@ Vector3 SliderJoint3DSW::getAncorInB() { } // 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_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..d2856829ed 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,7 +142,6 @@ 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) @@ -170,7 +151,6 @@ void PhysicsServer3DSW::space_set_active(RID p_space, bool p_active) { } 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 +158,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 +165,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 +179,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 +204,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); @@ -249,7 +221,6 @@ void PhysicsServer3DSW::area_set_space(RID p_area, RID p_space) { }; RID PhysicsServer3DSW::area_get_space(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, RID()); @@ -260,7 +231,6 @@ RID PhysicsServer3DSW::area_get_space(RID p_area) const { }; 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 +238,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 +245,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 +255,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 +266,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 +273,12 @@ 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 { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, RID()); @@ -324,7 +288,6 @@ 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 { - Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND_V(!area, Transform()); @@ -332,7 +295,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,7 +302,6 @@ 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); @@ -349,7 +310,6 @@ void PhysicsServer3DSW::area_clear_shapes(RID p_area) { } 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 +318,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(); @@ -368,7 +327,6 @@ void PhysicsServer3DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id area->set_instance_id(p_id); } 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 +337,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 +347,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 +364,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 +371,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 +378,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 +385,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 +393,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 +400,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 +407,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 +414,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,7 +423,6 @@ 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) body->set_mode(p_mode); @@ -488,7 +434,6 @@ RID PhysicsServer3DSW::body_create(BodyMode p_mode, bool p_init_sleeping) { }; void PhysicsServer3DSW::body_set_space(RID p_body, RID p_space) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -506,7 +451,6 @@ void PhysicsServer3DSW::body_set_space(RID p_body, RID p_space) { }; RID PhysicsServer3DSW::body_get_space(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, RID()); @@ -517,7 +461,6 @@ RID PhysicsServer3DSW::body_get_space(RID p_body) const { }; void PhysicsServer3DSW::body_set_mode(RID p_body, BodyMode p_mode) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -525,7 +468,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 +475,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 +485,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); @@ -555,7 +495,6 @@ 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) { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -563,14 +502,12 @@ 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 { - Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, RID()); @@ -581,7 +518,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 +527,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 +534,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,7 +541,6 @@ 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); @@ -616,7 +549,6 @@ void PhysicsServer3DSW::body_clear_shapes(RID p_body) { } 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 +556,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 +563,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 +571,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 +578,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 +586,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 +593,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 +600,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 +607,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 +619,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 +626,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 +646,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 +653,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 +660,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 +668,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 +682,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 +723,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 +733,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 +743,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 +757,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 +765,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 +779,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 +787,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 +796,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 +814,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 +861,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 +872,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 +883,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,7 +903,6 @@ 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); @@ -1017,7 +910,6 @@ void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, pin_joint->set_param(p_param, p_value); } 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,7 +918,6 @@ 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); @@ -1034,7 +925,6 @@ void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { pin_joint->set_pos_a(p_A); } 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,7 +933,6 @@ 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); @@ -1051,7 +940,6 @@ void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { pin_joint->set_pos_b(p_B); } 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 +948,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 +968,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,7 +988,6 @@ 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); @@ -1110,7 +995,6 @@ void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_par hinge_joint->set_param(p_param, p_value); } 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,7 +1003,6 @@ 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); @@ -1127,7 +1010,6 @@ void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, hinge_joint->set_flag(p_flag, p_value); } 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 +1018,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 +1057,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,7 +1083,6 @@ 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); @@ -1213,7 +1090,6 @@ void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_p slider_joint->set_param(p_param, p_value); } 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 +1098,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,7 +1118,6 @@ 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); @@ -1251,7 +1125,6 @@ void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointPa cone_twist_joint->set_param(p_param, p_value); } 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 +1133,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,7 +1153,6 @@ 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); @@ -1289,7 +1160,6 @@ void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis 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) { - Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0); @@ -1298,7 +1168,6 @@ 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); @@ -1306,7 +1175,6 @@ void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p 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) { - 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 +1183,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 +1196,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 +1209,6 @@ void PhysicsServer3DSW::free(RID p_rid) { body->set_space(nullptr); while (body->get_shape_count()) { - body->remove_shape(0); } @@ -1352,7 +1216,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 +1226,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 +1246,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,7 +1272,6 @@ void PhysicsServer3DSW::init() { }; void PhysicsServer3DSW::step(real_t p_step) { - #ifndef _3D_DISABLED if (!active) @@ -1433,7 +1288,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,7 +1301,6 @@ void PhysicsServer3DSW::sync(){ }; void PhysicsServer3DSW::flush_queries() { - #ifndef _3D_DISABLED if (!active) @@ -1460,7 +1313,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 +1320,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 +1334,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 +1355,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 +1376,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,7 +1383,6 @@ 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) @@ -1550,7 +1393,6 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 & 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; @@ -1565,7 +1407,6 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 & 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 481cb667c3..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; diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index 61c32b779a..2c248a4d38 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,35 +81,29 @@ 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) r_normal = plane.normal; @@ -121,12 +111,10 @@ bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_ } 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 +123,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 +145,6 @@ PlaneShape3DSW::PlaneShape3DSW() { // real_t RayShape3DSW::get_length() const { - return length; } @@ -170,14 +153,12 @@ 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) return Vector3(0, 0, length); else @@ -185,9 +166,7 @@ Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const { } 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 +180,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 +197,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 +219,6 @@ Variant RayShape3DSW::get_data() const { } RayShape3DSW::RayShape3DSW() { - length = 1; slips_on_slope = false; } @@ -255,12 +226,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,28 +241,23 @@ 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) @@ -302,36 +266,30 @@ Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } 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 +301,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 +310,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 +338,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 +355,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 +391,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) { @@ -486,7 +433,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 +449,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 +457,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 +476,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 +487,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 +497,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 +514,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 +524,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 +573,6 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } if (collision) { - r_result = res; r_normal = n; } @@ -645,7 +580,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 +590,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), @@ -671,7 +604,6 @@ Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const { } Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const { - // use bad AABB approximation Vector3 extents = get_aabb().size * 0.5; @@ -682,14 +614,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 +627,6 @@ void CapsuleShape3DSW::set_data(const Variant &p_data) { } Variant CapsuleShape3DSW::get_data() const { - Dictionary d; d["radius"] = radius; d["height"] = height; @@ -705,14 +634,12 @@ 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) return; @@ -720,7 +647,6 @@ void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transf 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) @@ -731,7 +657,6 @@ void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transf } Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const { - Vector3 n = p_normal; int vert_support_idx = -1; @@ -744,7 +669,6 @@ Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const { 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 +681,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 +695,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 +704,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(); @@ -801,7 +721,6 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve int m = MIN(p_max, ic); for (int j = 0; j < m; j++) { - r_supports[j] = vertices[ind[j]]; } r_amount = m; @@ -810,11 +729,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 +744,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,7 +754,6 @@ 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) continue; //opposing face @@ -846,7 +761,6 @@ bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vec 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,12 +781,10 @@ 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) return false; } @@ -881,14 +793,12 @@ bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const { } 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)) continue; @@ -898,7 +808,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 +833,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 +850,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,7 +860,6 @@ 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) ERR_PRINT("Failed to build QuickHull"); @@ -961,7 +867,6 @@ void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) { AABB _aabb; for (int i = 0; i < mesh.vertices.size(); i++) { - if (i == 0) _aabb.position = mesh.vertices[i]; else @@ -972,12 +877,10 @@ void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) { } void ConvexPolygonShape3DSW::set_data(const Variant &p_data) { - _setup(p_data); } Variant ConvexPolygonShape3DSW::get_data() const { - return mesh.vertices; } @@ -987,9 +890,7 @@ 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); @@ -1002,12 +903,10 @@ void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_tr } 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 +919,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 +936,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,7 +947,6 @@ 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) continue; @@ -1061,7 +955,6 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ 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 +967,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 +979,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 +1010,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,7 +1019,6 @@ 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) @@ -1146,7 +1029,6 @@ void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Trans } Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const { - int count = vertices.size(); if (count == 0) return Vector3(); @@ -1159,7 +1041,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 +1053,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 +1062,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 +1080,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,7 +1091,6 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par } } else { - if (bvh->left >= 0) _cull_segment(bvh->left, p_params); if (bvh->right >= 0) @@ -1224,7 +1099,6 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par } bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { - if (faces.size() == 0) return false; @@ -1248,35 +1122,29 @@ 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)) 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,21 +1154,17 @@ 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) return; @@ -1328,7 +1192,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 +1202,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 +1234,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,13 +1245,11 @@ _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) aabb = p_elements[i].aabb; else @@ -1405,20 +1257,16 @@ _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_siz } 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 +1282,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 +1293,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 +1301,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 +1308,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 +1333,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(); @@ -1522,12 +1365,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 +1378,29 @@ ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() { /* HEIGHT MAP SHAPE */ Vector<real_t> HeightMapShape3DSW::get_heights() const { - return heights; } int HeightMapShape3DSW::get_width() const { - return width; } int HeightMapShape3DSW::get_depth() const { - return depth; } 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 +1409,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 +1416,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 +1426,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,9 +1436,7 @@ 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); @@ -1622,7 +1451,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 +1471,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 6b31e0e094..9193e6cbdd 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -35,7 +35,6 @@ #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; } @@ -50,7 +49,6 @@ _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint } 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,7 +56,6 @@ 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) break; @@ -94,7 +91,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,7 +110,6 @@ 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)) continue; @@ -137,14 +132,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(); @@ -172,7 +165,6 @@ 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) return 0; @@ -188,7 +180,6 @@ 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) break; @@ -223,7 +214,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,7 +236,6 @@ 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)) continue; @@ -290,10 +279,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,7 +316,6 @@ 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 false; @@ -353,7 +339,6 @@ 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)) continue; @@ -375,7 +360,6 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ } struct _RestCallbackData { - const CollisionObject3DSW *object; const CollisionObject3DSW *best_object; int shape; @@ -387,7 +371,6 @@ 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; @@ -404,7 +387,6 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, 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) { - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); @@ -420,7 +402,6 @@ 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)) continue; @@ -446,7 +427,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 +439,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,7 +453,6 @@ 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)) continue; @@ -500,18 +478,15 @@ 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) @@ -526,7 +501,6 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { keep = false; if (!keep) { - if (i < amount - 1) { SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); @@ -541,13 +515,11 @@ 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)) continue; @@ -587,7 +559,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; @@ -606,7 +577,6 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra 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 +619,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 +665,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,7 +680,6 @@ 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)) continue; @@ -747,7 +714,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; @@ -771,7 +737,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 +753,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,7 +785,6 @@ 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)) continue; @@ -843,7 +806,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 +843,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 +858,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 +867,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 +879,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,7 +903,6 @@ 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]; @@ -959,7 +914,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } 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 +936,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 +949,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 +962,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 +981,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 +988,75 @@ 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) { - active_list.add(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) { - 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) { - 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 +1064,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 +1072,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 +1108,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 +1132,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; @@ -1263,7 +1179,6 @@ Space3DSW::Space3DSW() { } 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..c8b1ad32c1 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, diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index 1a7d5f8cec..5c84eb05d0 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -34,13 +34,11 @@ #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) continue; //already processed @@ -60,7 +58,6 @@ void Step3DSW::_populate_island(Body3DSW *p_body, Body3DSW **p_island, Constrain } void Step3DSW::_setup_island(Constraint3DSW *p_island, real_t p_delta) { - Constraint3DSW *ci = p_island; while (ci) { ci->setup(p_delta); @@ -70,13 +67,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 +91,6 @@ void Step3DSW::_solve_island(Constraint3DSW *p_island, int p_iterations, real_t p_island = ci->get_island_next(); } } else { - prev = ci; } @@ -108,12 +101,10 @@ 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 @@ -129,7 +120,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 @@ -145,7 +135,6 @@ void Step3DSW::_check_suspend(Body3DSW *p_island, real_t p_delta) { } 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 +150,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 +175,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,7 +197,6 @@ 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) continue; @@ -233,7 +219,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 +261,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 +278,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); |