diff options
Diffstat (limited to 'servers/physics_3d')
36 files changed, 614 insertions, 450 deletions
diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index 23c8079538..ba362740b2 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -113,7 +113,7 @@ struct MinkowskiDiff { real_t margin_A = 0.0; real_t margin_B = 0.0; - Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t); + Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t) = nullptr; void Initialize(const GodotShape3D* shape0, const Transform3D& wtrs0, const real_t margin0, const GodotShape3D* shape1, const Transform3D& wtrs1, const real_t margin1) { @@ -191,13 +191,13 @@ struct GJK /* Fields */ tShape m_shape; Vector3 m_ray; - real_t m_distance; + real_t m_distance = 0.0f; sSimplex m_simplices[2]; sSV m_store[4]; sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; + U m_nfree = 0; + U m_current = 0; + sSimplex* m_simplex = nullptr; eStatus::_ m_status; /* Methods */ GJK() @@ -548,12 +548,12 @@ struct GJK struct sFace { Vector3 n; - real_t d; + real_t d = 0.0f; sSV* c[3]; sFace* f[3]; sFace* l[2]; U1 e[3]; - U1 pass; + U1 pass = 0; }; struct sList { @@ -583,10 +583,10 @@ struct GJK eStatus::_ m_status; GJK::sSimplex m_result; Vector3 m_normal; - real_t m_depth; + real_t m_depth = 0.0f; sSV m_sv_store[EPA_MAX_VERTICES]; sFace m_fc_store[EPA_MAX_FACES]; - U m_nextsv; + U m_nextsv = 0; sList m_hull; sList m_stock; /* Methods */ diff --git a/servers/physics_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h index 01a47f222e..309af76561 100644 --- a/servers/physics_3d/gjk_epa.h +++ b/servers/physics_3d/gjk_epa.h @@ -37,4 +37,4 @@ bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); -#endif +#endif // GJK_EPA_H diff --git a/servers/physics_3d/godot_area_3d.cpp b/servers/physics_3d/godot_area_3d.cpp index e7df23d0d8..d4d3b3e6aa 100644 --- a/servers/physics_3d/godot_area_3d.cpp +++ b/servers/physics_3d/godot_area_3d.cpp @@ -254,27 +254,33 @@ void GodotArea3D::call_queries() { resptr[i] = &res[i]; } - for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_bodies.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); E = next; continue; } - res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; + res[0] = E->value.state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); E = next; Callable::CallError ce; Variant ret; - monitor_callback.call((const Variant **)resptr, 5, ret, ce); + monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling monitor callback method " + Variant::get_callable_error_text(monitor_callback, (const Variant **)resptr, 5, ce)); + } } } else { monitored_bodies.clear(); @@ -290,27 +296,33 @@ void GodotArea3D::call_queries() { resptr[i] = &res[i]; } - for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_areas.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); E = next; continue; } - res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; + res[0] = E->value.state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); E = next; Callable::CallError ce; Variant ret; - area_monitor_callback.call((const Variant **)resptr, 5, ret, ce); + area_monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling area monitor callback method " + Variant::get_callable_error_text(area_monitor_callback, (const Variant **)resptr, 5, ce)); + } } } else { monitored_areas.clear(); @@ -321,12 +333,12 @@ void GodotArea3D::call_queries() { void GodotArea3D::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const { if (is_gravity_point()) { - const real_t gravity_distance_scale = get_gravity_distance_scale(); + const real_t gr_distance_scale = get_gravity_distance_scale(); Vector3 v = get_transform().xform(get_gravity_vector()) - p_position; - if (gravity_distance_scale > 0) { + if (gr_distance_scale > 0) { const real_t v_length = v.length(); if (v_length > 0) { - const real_t v_scaled = v_length * gravity_distance_scale; + const real_t v_scaled = v_length * gr_distance_scale; r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled))); } else { r_gravity = Vector3(); diff --git a/servers/physics_3d/godot_area_3d.h b/servers/physics_3d/godot_area_3d.h index ce64fc802a..51b435eb00 100644 --- a/servers/physics_3d/godot_area_3d.h +++ b/servers/physics_3d/godot_area_3d.h @@ -72,16 +72,15 @@ class GodotArea3D : public GodotCollisionObject3D { uint32_t body_shape = 0; uint32_t area_shape = 0; - _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; - } - } else { - return rid < p_key.rid; - } + static uint32_t hash(const BodyKey &p_key) { + uint32_t h = hash_one_uint64(p_key.rid.get_id()); + h = hash_murmur3_one_64(p_key.instance_id, h); + h = hash_murmur3_one_32(p_key.area_shape, h); + return hash_fmix32(hash_murmur3_one_32(p_key.body_shape, h)); + } + + _FORCE_INLINE_ bool operator==(const BodyKey &p_key) const { + return rid == p_key.rid && instance_id == p_key.instance_id && body_shape == p_key.body_shape && area_shape == p_key.area_shape; } _FORCE_INLINE_ BodyKey() {} @@ -96,13 +95,13 @@ class GodotArea3D : public GodotCollisionObject3D { _FORCE_INLINE_ void dec() { state--; } }; - Map<BodyKey, BodyState> monitored_soft_bodies; - Map<BodyKey, BodyState> monitored_bodies; - Map<BodyKey, BodyState> monitored_areas; + HashMap<BodyKey, BodyState, BodyKey> monitored_soft_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_areas; - Set<GodotConstraint3D *> constraints; + HashSet<GodotConstraint3D *> constraints; - virtual void _shapes_changed(); + virtual void _shapes_changed() override; void _queue_monitor_update(); void _set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::AreaSpaceOverrideMode p_new_mode); @@ -164,7 +163,7 @@ public: _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } void set_monitorable(bool p_monitorable); @@ -172,7 +171,7 @@ public: void set_transform(const Transform3D &p_transform); - void set_space(GodotSpace3D *p_space); + void set_space(GodotSpace3D *p_space) override; void call_queries(); diff --git a/servers/physics_3d/godot_area_pair_3d.h b/servers/physics_3d/godot_area_pair_3d.h index c416477204..64b43a3b51 100644 --- a/servers/physics_3d/godot_area_pair_3d.h +++ b/servers/physics_3d/godot_area_pair_3d.h @@ -37,8 +37,8 @@ #include "godot_soft_body_3d.h" class GodotAreaPair3D : public GodotConstraint3D { - GodotBody3D *body; - GodotArea3D *area; + GodotBody3D *body = nullptr; + GodotArea3D *area = nullptr; int body_shape; int area_shape; bool colliding = false; @@ -55,8 +55,8 @@ public: }; class GodotArea2Pair3D : public GodotConstraint3D { - GodotArea3D *area_a; - GodotArea3D *area_b; + GodotArea3D *area_a = nullptr; + GodotArea3D *area_b = nullptr; int shape_a; int shape_b; bool colliding_a = false; @@ -76,8 +76,8 @@ public: }; class GodotAreaSoftBodyPair3D : public GodotConstraint3D { - GodotSoftBody3D *soft_body; - GodotArea3D *area; + GodotSoftBody3D *soft_body = nullptr; + GodotArea3D *area = nullptr; int soft_body_shape; int area_shape; bool colliding = false; diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp index ad97533f44..53f4ab86f9 100644 --- a/servers/physics_3d/godot_body_3d.cpp +++ b/servers/physics_3d/godot_body_3d.cpp @@ -56,7 +56,7 @@ void GodotBody3D::update_mass_properties() { // Update shapes and motions. switch (mode) { - case PhysicsServer3D::BODY_MODE_DYNAMIC: { + case PhysicsServer3D::BODY_MODE_RIGID: { real_t total_area = 0; for (int i = 0; i < get_shape_count(); i++) { if (is_shape_disabled(i)) { @@ -78,10 +78,10 @@ void GodotBody3D::update_mass_properties() { real_t area = get_shape_area(i); - real_t mass = area * this->mass / total_area; + real_t mass_new = area * mass / total_area; // NOTE: we assume that the shape origin is also its center of mass. - center_of_mass_local += mass * get_shape_transform(i).origin; + center_of_mass_local += mass_new * get_shape_transform(i).origin; } center_of_mass_local /= mass; @@ -108,9 +108,9 @@ void GodotBody3D::update_mass_properties() { const GodotShape3D *shape = get_shape(i); - real_t mass = area * this->mass / total_area; + real_t mass_new = area * mass / total_area; - Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass)); + Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass_new)); Transform3D shape_transform = get_shape_transform(i); Basis shape_basis = shape_transform.basis.orthonormalized(); @@ -118,7 +118,7 @@ void GodotBody3D::update_mass_properties() { shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed(); Vector3 shape_origin = shape_transform.origin - center_of_mass_local; - inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass; + inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass_new; } // Set the inertia to a valid value when there are no valid shapes. @@ -154,7 +154,7 @@ void GodotBody3D::update_mass_properties() { _inv_inertia = Vector3(); _inv_mass = 0; } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { + case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { _inv_inertia_tensor.set_zero(); _inv_mass = 1.0 / mass; @@ -201,7 +201,7 @@ void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Varian real_t mass_value = p_value; ERR_FAIL_COND(mass_value <= 0); mass = mass_value; - if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (mode >= PhysicsServer3D::BODY_MODE_RIGID) { _mass_properties_changed(); } } break; @@ -209,12 +209,12 @@ void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Varian inertia = p_value; if ((inertia.x <= 0.0) || (inertia.y <= 0.0) || (inertia.z <= 0.0)) { calculate_inertia = true; - if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (mode == PhysicsServer3D::BODY_MODE_RIGID) { _mass_properties_changed(); } } else { calculate_inertia = false; - if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (mode == PhysicsServer3D::BODY_MODE_RIGID) { principal_inertia_axes_local = Basis(); _inv_inertia = inertia.inverse(); _update_transform_dependent(); @@ -263,7 +263,7 @@ Variant GodotBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const { return mass; } break; case PhysicsServer3D::BODY_PARAM_INERTIA: { - if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (mode == PhysicsServer3D::BODY_MODE_RIGID) { return _inv_inertia.inverse(); } else { return Vector3(); @@ -315,7 +315,7 @@ void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) { _update_transform_dependent(); } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC: { + case PhysicsServer3D::BODY_MODE_RIGID: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; if (!calculate_inertia) { principal_inertia_axes_local = Basis(); @@ -327,7 +327,7 @@ void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) { set_active(true); } break; - case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: { + case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { _inv_mass = mass > 0 ? (1.0 / mass) : 0; _inv_inertia = Vector3(); angular_velocity = Vector3(); @@ -407,7 +407,7 @@ void GodotBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p } break; case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { can_sleep = p_variant; - if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) { + if (mode >= PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) { set_active(true); } @@ -637,14 +637,14 @@ void GodotBody3D::integrate_forces(real_t p_step) { damp = 0; } - real_t angular_damp = 1.0 - p_step * total_angular_damp; + real_t angular_damp_new = 1.0 - p_step * total_angular_damp; - if (angular_damp < 0) { // reached zero in the given time - angular_damp = 0; + if (angular_damp_new < 0) { // reached zero in the given time + angular_damp_new = 0; } linear_velocity *= damp; - angular_velocity *= angular_damp; + angular_velocity *= angular_damp_new; linear_velocity += _inv_mass * force * p_step; angular_velocity += _inv_inertia_tensor.xform(torque) * p_step; @@ -674,7 +674,7 @@ void GodotBody3D::integrate_velocities(real_t p_step) { return; } - if (fi_callback_data || body_state_callback) { + if (fi_callback_data || body_state_callback.get_object()) { get_space()->body_add_to_state_query_list(&direct_state_query_list); } @@ -707,27 +707,27 @@ void GodotBody3D::integrate_velocities(real_t p_step) { Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; real_t ang_vel = total_angular_velocity.length(); - Transform3D transform = get_transform(); + Transform3D transform_new = get_transform(); if (!Math::is_zero_approx(ang_vel)) { Vector3 ang_vel_axis = total_angular_velocity / ang_vel; Basis rot(ang_vel_axis, ang_vel * p_step); Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1); - transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local); - transform.basis = rot * transform.basis; - transform.orthonormalize(); + transform_new.origin += ((identity3 - rot) * transform_new.basis).xform(center_of_mass_local); + transform_new.basis = rot * transform_new.basis; + transform_new.orthonormalize(); } Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity; /*for(int i=0;i<3;i++) { if (axis_lock&(1<<i)) { - transform.origin[i]=0.0; + transform_new.origin[i]=0.0; } }*/ - transform.origin += total_linear_velocity * p_step; + transform_new.origin += total_linear_velocity * p_step; - _set_transform(transform); + _set_transform(transform_new); _set_inv_transform(get_transform().inverse()); _update_transform_dependent(); @@ -744,7 +744,7 @@ void GodotBody3D::wakeup_neighbours() { continue; } GodotBody3D *b = n[i]; - if (b->mode < PhysicsServer3D::BODY_MODE_DYNAMIC) { + if (b->mode < PhysicsServer3D::BODY_MODE_RIGID) { continue; } @@ -756,22 +756,26 @@ void GodotBody3D::wakeup_neighbours() { } void GodotBody3D::call_queries() { + Variant direct_state_variant = get_direct_state(); + if (fi_callback_data) { if (!fi_callback_data->callable.get_object()) { set_force_integration_callback(Callable()); } else { - Variant direct_state_variant = get_direct_state(); const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; Callable::CallError ce; int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2; Variant rv; - fi_callback_data->callable.call(vp, argc, rv, ce); + fi_callback_data->callable.callp(vp, argc, rv, ce); } } - if (body_state_callback_instance) { - (body_state_callback)(body_state_callback_instance, get_direct_state()); + if (body_state_callback.get_object()) { + const Variant *vp[1] = { &direct_state_variant }; + Callable::CallError ce; + Variant rv; + body_state_callback.callp(vp, 1, rv, ce); } } @@ -792,9 +796,8 @@ bool GodotBody3D::sleep_test(real_t p_step) { } } -void GodotBody3D::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) { - body_state_callback_instance = p_instance; - body_state_callback = p_callback; +void GodotBody3D::set_state_sync_callback(const Callable &p_callable) { + body_state_callback = p_callable; } void GodotBody3D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h index 1906e8aab1..412cbebc7d 100644 --- a/servers/physics_3d/godot_body_3d.h +++ b/servers/physics_3d/godot_body_3d.h @@ -40,7 +40,7 @@ class GodotConstraint3D; class GodotPhysicsDirectBodyState3D; class GodotBody3D : public GodotCollisionObject3D { - PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_DYNAMIC; + PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_RIGID; Vector3 linear_velocity; Vector3 angular_velocity; @@ -109,10 +109,10 @@ class GodotBody3D : public GodotCollisionObject3D { bool first_time_kinematic = false; void _mass_properties_changed(); - virtual void _shapes_changed(); + virtual void _shapes_changed() override; Transform3D new_transform; - Map<GodotConstraint3D *, int> constraint_map; + HashMap<GodotConstraint3D *, int> constraint_map; Vector<AreaCMP> areas; @@ -131,8 +131,7 @@ class GodotBody3D : public GodotCollisionObject3D { Vector<Contact> contacts; //no contacts by default int contact_count = 0; - void *body_state_callback_instance = nullptr; - PhysicsServer3D::BodyStateCallback body_state_callback = nullptr; + Callable body_state_callback; struct ForceIntegrationCallbackData { Callable callable; @@ -150,7 +149,7 @@ class GodotBody3D : public GodotCollisionObject3D { friend class GodotPhysicsDirectBodyState3D; // i give up, too many functions to expose public: - void set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback); + void set_state_sync_callback(const Callable &p_callable); void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); GodotPhysicsDirectBodyState3D *get_direct_state(); @@ -196,7 +195,7 @@ public: _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraint_map.erase(p_constraint); } - const Map<GodotConstraint3D *, int> &get_constraint_map() const { return constraint_map; } + const HashMap<GodotConstraint3D *, int> &get_constraint_map() const { return constraint_map; } _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } @@ -301,7 +300,7 @@ public: _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; } _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; } - void set_space(GodotSpace3D *p_space); + void set_space(GodotSpace3D *p_space) override; void update_mass_properties(); void reset_mass_properties(); diff --git a/servers/physics_3d/godot_body_direct_state_3d.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp index a8c6086e1c..25088d33f3 100644 --- a/servers/physics_3d/godot_body_direct_state_3d.cpp +++ b/servers/physics_3d/godot_body_direct_state_3d.cpp @@ -145,7 +145,7 @@ void GodotPhysicsDirectBodyState3D::add_constant_torque(const Vector3 &p_torque) } void GodotPhysicsDirectBodyState3D::set_constant_force(const Vector3 &p_force) { - if (!p_force.is_equal_approx(Vector3())) { + if (!p_force.is_zero_approx()) { body->wakeup(); } body->set_constant_force(p_force); @@ -156,7 +156,7 @@ Vector3 GodotPhysicsDirectBodyState3D::get_constant_force() const { } void GodotPhysicsDirectBodyState3D::set_constant_torque(const Vector3 &p_torque) { - if (!p_torque.is_equal_approx(Vector3())) { + if (!p_torque.is_zero_approx()) { body->wakeup(); } body->set_constant_torque(p_torque); diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index 89d5d59161..7e6cc6f834 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -39,7 +39,7 @@ #define MAX_BIAS_ROTATION (Math_PI / 8) void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - GodotBodyPair3D *pair = (GodotBodyPair3D *)p_userdata; + GodotBodyPair3D *pair = static_cast<GodotBodyPair3D *>(p_userdata); pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); } @@ -170,7 +170,7 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, Vector3 mnormal = motion / mlen; - real_t min, max; + real_t min = 0.0, max = 0.0; p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max); // Did it move enough in this direction to even attempt raycast? @@ -562,7 +562,7 @@ GodotBodyPair3D::~GodotBodyPair3D() { } void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - GodotBodySoftBodyPair3D *pair = (GodotBodySoftBodyPair3D *)p_userdata; + GodotBodySoftBodyPair3D *pair = static_cast<GodotBodySoftBodyPair3D *>(p_userdata); pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); } diff --git a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp index b34f9d214f..435c1e8aec 100644 --- a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp +++ b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp @@ -87,7 +87,7 @@ int GodotBroadPhase3DBVH::cull_aabb(const AABB &p_aabb, GodotCollisionObject3D * } void *GodotBroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B) { - GodotBroadPhase3DBVH *bpo = (GodotBroadPhase3DBVH *)(self); + GodotBroadPhase3DBVH *bpo = static_cast<GodotBroadPhase3DBVH *>(self); if (!bpo->pair_callback) { return nullptr; } @@ -96,7 +96,7 @@ void *GodotBroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollis } void GodotBroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B, void *pairdata) { - GodotBroadPhase3DBVH *bpo = (GodotBroadPhase3DBVH *)(self); + GodotBroadPhase3DBVH *bpo = static_cast<GodotBroadPhase3DBVH *>(self); if (!bpo->unpair_callback) { return; } diff --git a/servers/physics_3d/godot_broad_phase_3d_bvh.h b/servers/physics_3d/godot_broad_phase_3d_bvh.h index 7660030195..ae5a18d955 100644 --- a/servers/physics_3d/godot_broad_phase_3d_bvh.h +++ b/servers/physics_3d/godot_broad_phase_3d_bvh.h @@ -75,23 +75,23 @@ class GodotBroadPhase3DBVH : public GodotBroadPhase3D { public: // 0 is an invalid ID - virtual ID create(GodotCollisionObject3D *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false); - virtual void move(ID p_id, const AABB &p_aabb); - virtual void set_static(ID p_id, bool p_static); - virtual void remove(ID p_id); + virtual ID create(GodotCollisionObject3D *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) override; + virtual void move(ID p_id, const AABB &p_aabb) override; + virtual void set_static(ID p_id, bool p_static) override; + virtual void remove(ID p_id) override; - virtual GodotCollisionObject3D *get_object(ID p_id) const; - virtual bool is_static(ID p_id) const; - virtual int get_subindex(ID p_id) const; + virtual GodotCollisionObject3D *get_object(ID p_id) const override; + virtual bool is_static(ID p_id) const override; + virtual int get_subindex(ID p_id) const override; - virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr); - virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr); + virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) override; + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) override; + virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) override; - virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); - virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) override; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) override; - virtual void update(); + virtual void update() override; static GodotBroadPhase3D *_create(); GodotBroadPhase3DBVH(); diff --git a/servers/physics_3d/godot_collision_object_3d.h b/servers/physics_3d/godot_collision_object_3d.h index 515b945564..2d342f65f3 100644 --- a/servers/physics_3d/godot_collision_object_3d.h +++ b/servers/physics_3d/godot_collision_object_3d.h @@ -59,6 +59,7 @@ private: ObjectID instance_id; uint32_t collision_layer = 1; uint32_t collision_mask = 1; + real_t collision_priority = 1.0; struct Shape { Transform3D xform; @@ -112,7 +113,7 @@ public: _FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; } _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } - void _shape_changed(); + void _shape_changed() override; _FORCE_INLINE_ Type get_type() const { return type; } void add_shape(GodotShape3D *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false); @@ -165,6 +166,13 @@ public: } _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } + _FORCE_INLINE_ void set_collision_priority(real_t p_priority) { + ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0."); + collision_priority = p_priority; + _shape_changed(); + } + _FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; } + _FORCE_INLINE_ bool collides_with(GodotCollisionObject3D *p_other) const { return p_other->collision_layer & collision_mask; } @@ -173,7 +181,7 @@ public: return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; } - void remove_shape(GodotShape3D *p_shape); + void remove_shape(GodotShape3D *p_shape) override; void remove_shape(int p_index); virtual void set_space(GodotSpace3D *p_space) = 0; diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp index 81e1a88366..ca76a819ec 100644 --- a/servers/physics_3d/godot_collision_solver_3d.cpp +++ b/servers/physics_3d/godot_collision_solver_3d.cpp @@ -29,6 +29,7 @@ /*************************************************************************/ #include "godot_collision_solver_3d.h" + #include "godot_collision_solver_3d_sat.h" #include "godot_soft_body_3d.h" @@ -37,7 +38,7 @@ #define collision_solver sat_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A); if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { return false; @@ -47,7 +48,7 @@ bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_s static const int max_supports = 16; Vector3 supports[max_supports]; int support_count; - GodotShape3D::FeatureType support_type; + GodotShape3D::FeatureType support_type = GodotShape3D::FeatureType::FEATURE_POINT; p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); if (support_type == GodotShape3D::FEATURE_CIRCLE) { @@ -69,6 +70,7 @@ bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_s bool found = false; for (int i = 0; i < support_count; i++) { + supports[i] += p_margin * supports[i].normalized(); supports[i] = p_transform_B.xform(supports[i]); if (p.distance_to(supports[i]) >= 0) { continue; @@ -93,7 +95,7 @@ bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A, const GodotSeparationRayShape3D *ray = static_cast<const GodotSeparationRayShape3D *>(p_shape_A); Vector3 from = p_transform_A.origin; - Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin); + Vector3 to = from + p_transform_A.basis.get_column(2) * (ray->get_length() + p_margin); Vector3 support_A = to; Transform3D ai = p_transform_B.affine_inverse(); @@ -141,7 +143,7 @@ struct _SoftBodyContactCollisionInfo { }; void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - _SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata); + _SoftBodyContactCollisionInfo &cinfo = *(static_cast<_SoftBodyContactCollisionInfo *>(p_userdata)); ++cinfo.contact_count; @@ -170,7 +172,7 @@ struct _SoftBodyQueryInfo { }; bool GodotCollisionSolver3D::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) { - _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); + _SoftBodyQueryInfo &query_cinfo = *(static_cast<_SoftBodyQueryInfo *>(p_userdata)); Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index); @@ -189,7 +191,7 @@ bool GodotCollisionSolver3D::soft_body_query_callback(uint32_t p_node_index, voi } bool GodotCollisionSolver3D::soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex) { - _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); + _SoftBodyQueryInfo &query_cinfo = *(static_cast<_SoftBodyQueryInfo *>(p_userdata)); query_cinfo.shape_A = p_convex; @@ -251,7 +253,7 @@ bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, cons // Calculate AABB for internal concave shape query (in local space). AABB local_aabb; for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_A.basis.get_axis(i)); + Vector3 axis(p_transform_A.basis.get_column(i)); real_t axis_scale = 1.0 / axis.length(); real_t smin = soft_body_aabb.position[i]; @@ -276,23 +278,24 @@ bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, cons } struct _ConcaveCollisionInfo { - const Transform3D *transform_A; - const GodotShape3D *shape_A; - const Transform3D *transform_B; - GodotCollisionSolver3D::CallbackResult result_callback; - void *userdata; - bool swap_result; - bool collided; - int aabb_tests; - int collisions; - bool tested; - real_t margin_A; - real_t margin_B; - Vector3 close_A, close_B; + const Transform3D *transform_A = nullptr; + const GodotShape3D *shape_A = nullptr; + const Transform3D *transform_B = nullptr; + GodotCollisionSolver3D::CallbackResult result_callback = nullptr; + void *userdata = nullptr; + bool swap_result = false; + bool collided = false; + int aabb_tests = 0; + int collisions = 0; + bool tested = false; + real_t margin_A = 0.0f; + real_t margin_B = 0.0f; + Vector3 close_A; + Vector3 close_B; }; bool GodotCollisionSolver3D::concave_callback(void *p_userdata, GodotShape3D *p_convex) { - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); + _ConcaveCollisionInfo &cinfo = *(static_cast<_ConcaveCollisionInfo *>(p_userdata)); cinfo.aabb_tests++; bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, nullptr, cinfo.margin_A, cinfo.margin_B); @@ -331,11 +334,11 @@ bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const AABB local_aabb; for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_B.basis.get_axis(i)); + Vector3 axis(p_transform_B.basis.get_column(i)); real_t axis_scale = 1.0 / axis.length(); axis *= axis_scale; - real_t smin, smax; + real_t smin = 0.0, smax = 0.0; p_shape_A->project_range(axis, rel_transform, smin, smax); smin -= p_margin_A; smax += p_margin_A; @@ -367,23 +370,27 @@ bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const T if (type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { if (type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + WARN_PRINT_ONCE("Collisions between world boundaries are not supported."); return false; } if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + WARN_PRINT_ONCE("Collisions between world boundaries and rays are not supported."); return false; } if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { + WARN_PRINT_ONCE("Collisions between world boundaries and soft bodies are not supported."); return false; } if (swap) { - return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A); } else { - return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_B); } } else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) { if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + WARN_PRINT_ONCE("Collisions between rays are not supported."); return false; } @@ -395,7 +402,7 @@ bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const T } else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) { - // Soft Body / Soft Body not supported. + WARN_PRINT_ONCE("Collisions between soft bodies are not supported."); return false; } @@ -407,6 +414,7 @@ bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const T } else if (concave_B) { if (concave_A) { + WARN_PRINT_ONCE("Collisions between two concave shapes are not supported."); return false; } @@ -422,7 +430,7 @@ bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const T } bool GodotCollisionSolver3D::concave_distance_callback(void *p_userdata, GodotShape3D *p_convex) { - _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); + _ConcaveCollisionInfo &cinfo = *(static_cast<_ConcaveCollisionInfo *>(p_userdata)); cinfo.aabb_tests++; Vector3 close_A, close_B; @@ -454,8 +462,17 @@ bool GodotCollisionSolver3D::solve_distance_world_boundary(const GodotShape3D *p Vector3 supports[max_supports]; int support_count; GodotShape3D::FeatureType support_type; + Vector3 support_direction = p_transform_B.basis.xform_inv(-p.normal).normalized(); - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); + p_shape_B->get_supports(support_direction, max_supports, supports, support_count, support_type); + + if (support_count == 0) { // This is a poor man's way to detect shapes that don't implement get_supports, such as GodotMotionShape3D. + Vector3 support_B = p_transform_B.xform(p_shape_B->get_support(support_direction)); + r_point_A = p.project(support_B); + r_point_B = support_B; + bool collided = p.distance_to(support_B) <= 0; + return collided; + } if (support_type == GodotShape3D::FEATURE_CIRCLE) { ERR_FAIL_COND_V(support_count != 3, false); @@ -540,7 +557,7 @@ bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const AABB local_aabb; for (int i = 0; i < 3; i++) { - Vector3 axis(p_transform_B.basis.get_axis(i)); + Vector3 axis(p_transform_B.basis.get_column(i)); real_t axis_scale = ((real_t)1.0) / axis.length(); axis *= axis_scale; diff --git a/servers/physics_3d/godot_collision_solver_3d.h b/servers/physics_3d/godot_collision_solver_3d.h index a6a0ebfead..e7d67903e9 100644 --- a/servers/physics_3d/godot_collision_solver_3d.h +++ b/servers/physics_3d/godot_collision_solver_3d.h @@ -42,7 +42,7 @@ private: static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex); static bool concave_callback(void *p_userdata, GodotShape3D *p_convex); - static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static bool solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp index ca429040f5..96253cb452 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp +++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp @@ -68,7 +68,7 @@ *************************************************************************/ struct _CollectorCallback { - GodotCollisionSolver3D::CallbackResult callback; + GodotCollisionSolver3D::CallbackResult callback = nullptr; void *userdata = nullptr; bool swap = false; bool collided = false; @@ -629,12 +629,12 @@ public: _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { Vector3 axis = p_axis; - if (axis.is_equal_approx(Vector3())) { + if (axis.is_zero_approx()) { // strange case, try an upwards separator axis = Vector3(0.0, 1.0, 0.0); } - real_t min_A, max_A, min_B, max_B; + real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0; shape_A->project_range(axis, *transform_A, min_A, max_A); shape_B->project_range(axis, *transform_B, min_B, max_B); @@ -792,7 +792,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_ // test faces for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_b.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -819,7 +819,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_ // 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(); + Vector3 axis = point_axis.cross(p_transform_b.basis.get_column(i)).cross(p_transform_b.basis.get_column(i)).normalized(); if (!separator.test_axis(axis)) { return; @@ -842,7 +842,7 @@ static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D //capsule sphere 1, sphere - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; @@ -883,7 +883,7 @@ static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3 } // Cylinder B end caps. - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1).normalized(); if (!separator.test_axis(cylinder_B_axis)) { return; } @@ -897,8 +897,8 @@ static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3 // Closest point to cylinder caps. const Vector3 &sphere_center = p_transform_a.origin; - Vector3 cyl_axis = p_transform_b.basis.get_axis(1); - Vector3 cap_axis = p_transform_b.basis.get_axis(0); + Vector3 cyl_axis = p_transform_b.basis.get_column(1); + Vector3 cap_axis = p_transform_b.basis.get_column(0); real_t height_scale = cyl_axis.length(); real_t cap_dist = cylinder_B->get_height() * 0.5 * height_scale; cyl_axis /= height_scale; @@ -964,8 +964,8 @@ static void _collision_sphere_convex_polygon(const GodotShape3D *p_a, const Tran // 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 v1 = p_transform_b.xform(vertices[edges[i].vertex_a]); + Vector3 v2 = p_transform_b.xform(vertices[edges[i].vertex_b]); Vector3 v3 = p_transform_a.origin; Vector3 n1 = v2 - v1; @@ -1063,7 +1063,7 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra // test faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -1073,7 +1073,7 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra // test faces of B for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_b.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -1083,7 +1083,7 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra // 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)); + Vector3 axis = p_transform_a.basis.get_column(i).cross(p_transform_b.basis.get_column(j)); if (Math::is_zero_approx(axis.length_squared())) { continue; @@ -1129,14 +1129,14 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra for (int i = 0; i < 3; i++) { //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); + Vector3 axis_a = p_transform_a.basis.get_column(i); if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { return; } //b ->a - Vector3 axis_b = p_transform_b.basis.get_axis(i); + Vector3 axis_b = p_transform_b.basis.get_column(i); if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) { return; @@ -1160,20 +1160,20 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; } } - Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized(); // edges of A, capsule cylinder for (int i = 0; i < 3; i++) { // cylinder - Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 box_axis = p_transform_a.basis.get_column(i); Vector3 axis = box_axis.cross(cyl_axis); if (Math::is_zero_approx(axis.length_squared())) { continue; @@ -1196,7 +1196,7 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; + point += p_transform_a.basis.get_column(l) * he[l]; } //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); @@ -1212,7 +1212,7 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p // capsule balls, edges of A for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -1234,7 +1234,7 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p // 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(); + Vector3 axis = point_axis.cross(p_transform_a.basis.get_column(j)).cross(p_transform_a.basis.get_column(j)).normalized(); if (!separator.test_axis(axis)) { return; @@ -1258,14 +1258,14 @@ static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D & // Faces of A. for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; } } - Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized(); // Cylinder end caps. { @@ -1276,7 +1276,7 @@ static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D & // Edges of A, cylinder lateral surface. for (int i = 0; i < 3; i++) { - Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 box_axis = p_transform_a.basis.get_column(i); Vector3 axis = box_axis.cross(cyl_axis); if (Math::is_zero_approx(axis.length_squared())) { continue; @@ -1300,7 +1300,7 @@ static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D & Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k]; point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * extent[l]; + point += p_transform_a.basis.get_column(l) * extent[l]; } } } @@ -1380,7 +1380,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -1401,10 +1401,10 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo // A<->B edges for (int i = 0; i < 3; i++) { - Vector3 e1 = p_transform_a.basis.get_axis(i); + Vector3 e1 = p_transform_a.basis.get_column(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 e2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]); Vector3 axis = e1.cross(e2).normalized(); @@ -1438,7 +1438,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo for (int i = 0; i < 3; i++) { //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); + Vector3 axis_a = p_transform_a.basis.get_column(i); if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { return; @@ -1456,12 +1456,12 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; + point += p_transform_a.basis.get_column(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 p1 = p_transform_b.xform(vertices[edges[e].vertex_a]); + Vector3 p2 = p_transform_b.xform(vertices[edges[e].vertex_b]); Vector3 n = (p2 - p1); if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) { @@ -1497,7 +1497,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (axis.dot(normal) < 0.0) { axis *= -1.0; } @@ -1513,7 +1513,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr Vector3 e = vertex[i] - vertex[(i + 1) % 3]; for (int j = 0; j < 3; j++) { - Vector3 axis = e.cross(p_transform_a.basis.get_axis(j)).normalized(); + Vector3 axis = e.cross(p_transform_a.basis.get_column(j)).normalized(); if (axis.dot(normal) < 0.0) { axis *= -1.0; } @@ -1550,7 +1550,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr for (int i = 0; i < 3; i++) { //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); + Vector3 axis_a = p_transform_a.basis.get_column(i); Vector3 axis = axis_ab.cross(axis_a).cross(axis_a).normalized(); if (axis.dot(normal) < 0.0) { @@ -1573,7 +1573,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; + point += p_transform_a.basis.get_column(l) * he[l]; } for (int e = 0; e < 3; e++) { @@ -1623,8 +1623,8 @@ static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3 // some values - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); - Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; @@ -1686,14 +1686,14 @@ static void _collision_capsule_cylinder(const GodotShape3D *p_a, const Transform } // Cylinder B end caps. - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1).normalized(); if (!separator.test_axis(cylinder_B_axis)) { return; } // Cylinder edge against capsule balls. - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1); + Vector3 capsule_A_axis = p_transform_a.basis.get_column(1); Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); @@ -1771,8 +1771,8 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra 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(1)).normalized(); + Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].vertex_a]) - p_transform_b.basis.xform(vertices[edges[i].vertex_b]); + Vector3 axis = edge_axis.cross(p_transform_a.basis.get_column(1)).normalized(); if (!separator.test_axis(axis)) { return; @@ -1784,13 +1784,13 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra for (int i = 0; i < 2; i++) { // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); for (int j = 0; j < edge_count; j++) { - Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]); - Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); + Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].vertex_a]); + Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]); Vector3 axis = n1.cross(n2).cross(n2).normalized(); @@ -1824,7 +1824,7 @@ static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D & // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); for (int i = 0; i < 3; i++) { // edge-cylinder @@ -1895,8 +1895,8 @@ static void _collision_cylinder_cylinder(const GodotShape3D *p_a, const Transfor SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1); - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1); + Vector3 cylinder_A_axis = p_transform_a.basis.get_column(1); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1); if (!separator.test_previous_axis()) { return; @@ -1983,7 +1983,7 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D return; } - Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized(); + Vector3 cyl_axis = p_transform_a.basis.get_column(1).normalized(); if (cyl_axis.dot(normal) < 0.0) { cyl_axis *= -1.0; } @@ -2075,6 +2075,16 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D separator.generate_contacts(); } +static _FORCE_INLINE_ bool is_minkowski_face(const Vector3 &A, const Vector3 &B, const Vector3 &B_x_A, const Vector3 &C, const Vector3 &D, const Vector3 &D_x_C) { + // Test if arcs AB and CD intersect on the unit sphere + real_t CBA = C.dot(B_x_A); + real_t DBA = D.dot(B_x_A); + real_t ADC = A.dot(D_x_C); + real_t BDC = B.dot(D_x_C); + + return (CBA * DBA < 0.0f) && (ADC * BDC < 0.0f) && (CBA * BDC > 0.0f); +} + template <bool withMargin> static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a); @@ -2129,16 +2139,27 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co } // 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]); + Vector3 p1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_a]); + Vector3 q1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_b]); + Vector3 e1 = q1 - p1; + Vector3 u1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_a].plane.normal).normalized(); + Vector3 v1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_b].plane.normal).normalized(); 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 p2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_a]); + Vector3 q2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_b]); + Vector3 e2 = q2 - p2; + Vector3 u2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_a].plane.normal).normalized(); + Vector3 v2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_b].plane.normal).normalized(); - Vector3 axis = e1.cross(e2).normalized(); + if (is_minkowski_face(u1, v1, -e1, -u2, -v2, -e2)) { + Vector3 axis = e1.cross(e2).normalized(); - if (!separator.test_axis(axis)) { - return; + if (!separator.test_axis(axis)) { + return; + } } } } @@ -2157,8 +2178,8 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co //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 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_a]); + Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_b]); Vector3 n = (e2 - e1); for (int j = 0; j < vertex_count_B; j++) { @@ -2171,8 +2192,8 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co } 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 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_a]); + Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_b]); Vector3 n = (e2 - e1); for (int j = 0; j < vertex_count_A; j++) { @@ -2231,7 +2252,7 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf // 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]); + Vector3 e1 = p_transform_a.xform(vertices[edges[i].vertex_a]) - p_transform_a.xform(vertices[edges[i].vertex_b]); for (int j = 0; j < 3; j++) { Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; @@ -2266,8 +2287,8 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf //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 e1 = p_transform_a.basis.xform(vertices[edges[i].vertex_a]); + Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].vertex_b]); Vector3 n = (e2 - e1); for (int j = 0; j < 3; j++) { diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.h b/servers/physics_3d/godot_collision_solver_3d_sat.h index 3eb7aa4c9e..46c5ec3254 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.h +++ b/servers/physics_3d/godot_collision_solver_3d_sat.h @@ -28,11 +28,11 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef GODOT_COLLISION_SOLVER_SAT_H -#define GODOT_COLLISION_SOLVER_SAT_H +#ifndef GODOT_COLLISION_SOLVER_3D_SAT_H +#define GODOT_COLLISION_SOLVER_3D_SAT_H #include "godot_collision_solver_3d.h" bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); -#endif // GODOT_COLLISION_SOLVER_SAT_H +#endif // GODOT_COLLISION_SOLVER_3D_SAT_H diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp index f7270713e0..68db5df144 100644 --- a/servers/physics_3d/godot_physics_server_3d.cpp +++ b/servers/physics_3d/godot_physics_server_3d.cpp @@ -387,6 +387,13 @@ void GodotPhysicsServer3D::area_set_collision_layer(RID p_area, uint32_t p_layer area->set_collision_layer(p_layer); } +uint32_t GodotPhysicsServer3D::area_get_collision_layer(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, 0); + + return area->get_collision_layer(); +} + void GodotPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) { GodotArea3D *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); @@ -394,6 +401,13 @@ void GodotPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) area->set_collision_mask(p_mask); } +uint32_t GodotPhysicsServer3D::area_get_collision_mask(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_COND_V(!area, 0); + + return area->get_collision_mask(); +} + void GodotPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) { GodotArea3D *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); @@ -593,6 +607,20 @@ uint32_t GodotPhysicsServer3D::body_get_collision_mask(RID p_body) const { return body->get_collision_mask(); } +void GodotPhysicsServer3D::body_set_collision_priority(RID p_body, real_t p_priority) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->set_collision_priority(p_priority); +} + +real_t GodotPhysicsServer3D::body_get_collision_priority(RID p_body) const { + const GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, 0); + + return body->get_collision_priority(); +} + void GodotPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p_id) { GodotBody3D *body = body_owner.get_or_null(p_body); if (body) { @@ -746,7 +774,7 @@ void GodotPhysicsServer3D::body_set_constant_force(RID p_body, const Vector3 &p_ ERR_FAIL_COND(!body); body->set_constant_force(p_force); - if (!p_force.is_equal_approx(Vector3())) { + if (!p_force.is_zero_approx()) { body->wakeup(); } } @@ -762,7 +790,7 @@ void GodotPhysicsServer3D::body_set_constant_torque(RID p_body, const Vector3 &p ERR_FAIL_COND(!body); body->set_constant_torque(p_torque); - if (!p_torque.is_equal_approx(Vector3())) { + if (!p_torque.is_zero_approx()) { body->wakeup(); } } @@ -863,10 +891,10 @@ int GodotPhysicsServer3D::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } -void GodotPhysicsServer3D::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { +void GodotPhysicsServer3D::body_set_state_sync_callback(RID p_body, const Callable &p_callable) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->set_state_sync_callback(p_instance, p_callback); + body->set_state_sync_callback(p_callable); } void GodotPhysicsServer3D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { @@ -1182,6 +1210,7 @@ RID GodotPhysicsServer3D::joint_create() { void GodotPhysicsServer3D::joint_clear(RID p_joint) { GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); if (joint->get_type() != JOINT_TYPE_MAX) { GodotJoint3D *empty_joint = memnew(GodotJoint3D); empty_joint->copy_settings_from(joint); @@ -1531,7 +1560,7 @@ void GodotPhysicsServer3D::free(RID p_rid) { GodotShape3D *shape = shape_owner.get_or_null(p_rid); while (shape->get_owners().size()) { - GodotShapeOwner3D *so = shape->get_owners().front()->key(); + GodotShapeOwner3D *so = shape->get_owners().begin()->key; so->remove_shape(shape); } @@ -1570,7 +1599,7 @@ void GodotPhysicsServer3D::free(RID p_rid) { GodotSpace3D *space = space_owner.get_or_null(p_rid); while (space->get_objects().size()) { - GodotCollisionObject3D *co = (GodotCollisionObject3D *)space->get_objects().front()->get(); + GodotCollisionObject3D *co = static_cast<GodotCollisionObject3D *>(*space->get_objects().begin()); co->set_space(nullptr); } @@ -1611,11 +1640,11 @@ void GodotPhysicsServer3D::step(real_t p_step) { island_count = 0; active_objects = 0; collision_pairs = 0; - for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((GodotSpace3D *)E->get(), p_step); - island_count += E->get()->get_island_count(); - active_objects += E->get()->get_active_objects(); - collision_pairs += E->get()->get_collision_pairs(); + for (const GodotSpace3D *E : active_spaces) { + stepper->step(const_cast<GodotSpace3D *>(E), p_step); + island_count += E->get_island_count(); + active_objects += E->get_active_objects(); + collision_pairs += E->get_collision_pairs(); } #endif } @@ -1635,8 +1664,8 @@ void GodotPhysicsServer3D::flush_queries() { uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { - GodotSpace3D *space = (GodotSpace3D *)E->get(); + for (const GodotSpace3D *E : active_spaces) { + GodotSpace3D *space = const_cast<GodotSpace3D *>(E); space->call_queries(); } @@ -1656,9 +1685,9 @@ void GodotPhysicsServer3D::flush_queries() { total_time[i] = 0; } - for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { + for (const GodotSpace3D *E : active_spaces) { for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) { - total_time[i] += E->get()->get_elapsed_time(GodotSpace3D::ElapsedTime(i)); + total_time[i] += E->get_elapsed_time(GodotSpace3D::ElapsedTime(i)); } } @@ -1671,7 +1700,7 @@ void GodotPhysicsServer3D::flush_queries() { values.push_back("flush_queries"); values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); - values.push_front("physics"); + values.push_front("physics_3d"); EngineDebugger::profiler_add_frame_data("servers", values); } #endif @@ -1709,7 +1738,7 @@ void GodotPhysicsServer3D::_update_shapes() { } void GodotPhysicsServer3D::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - CollCbkData *cbk = (CollCbkData *)p_userdata; + CollCbkData *cbk = static_cast<CollCbkData *>(p_userdata); if (cbk->max == 0) { return; diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h index 00ed763e01..e3e649da57 100644 --- a/servers/physics_3d/godot_physics_server_3d.h +++ b/servers/physics_3d/godot_physics_server_3d.h @@ -54,7 +54,7 @@ class GodotPhysicsServer3D : public PhysicsServer3D { bool flushing_queries = false; GodotStep3D *stepper = nullptr; - Set<const GodotSpace3D *> active_spaces; + HashSet<const GodotSpace3D *> active_spaces; mutable RID_PtrOwner<GodotShape3D, true> shape_owner; mutable RID_PtrOwner<GodotSpace3D, true> space_owner; @@ -74,7 +74,7 @@ public: struct CollCbkData { int max; int amount; - Vector3 *ptr; + Vector3 *ptr = nullptr; }; static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); @@ -148,8 +148,11 @@ public: virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; - virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; + virtual uint32_t area_get_collision_layer(RID p_area) const override; + + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual uint32_t area_get_collision_mask(RID p_area) const override; virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; @@ -192,6 +195,9 @@ public: virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override; virtual uint32_t body_get_collision_mask(RID p_body) const override; + virtual void body_set_collision_priority(RID p_body, real_t p_priority) override; + virtual real_t body_get_collision_priority(RID p_body) const override; + virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override; virtual uint32_t body_get_user_flags(RID p_body) const override; @@ -239,7 +245,7 @@ public: virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; virtual int body_get_max_contacts_reported(RID p_body) const override; - virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; + virtual void body_set_state_sync_callback(RID p_body, const Callable &p_callable) override; virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp index 7762c4829e..1443cd166b 100644 --- a/servers/physics_3d/godot_shape_3d.cpp +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -62,7 +62,7 @@ void GodotShape3D::configure(const AABB &p_aabb) { aabb = p_aabb; configured = true; for (const KeyValue<GodotShapeOwner3D *, int> &E : owners) { - GodotShapeOwner3D *co = (GodotShapeOwner3D *)E.key; + GodotShapeOwner3D *co = const_cast<GodotShapeOwner3D *>(E.key); co->_shape_changed(); } } @@ -76,20 +76,20 @@ Vector3 GodotShape3D::get_support(const Vector3 &p_normal) const { } void GodotShape3D::add_owner(GodotShapeOwner3D *p_owner) { - Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner); + HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner); if (E) { - E->get()++; + E->value++; } else { owners[p_owner] = 1; } } void GodotShape3D::remove_owner(GodotShapeOwner3D *p_owner) { - Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner); + HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner); ERR_FAIL_COND(!E); - E->get()--; - if (E->get() == 0) { - owners.erase(E); + E->value--; + if (E->value == 0) { + owners.remove(E); } } @@ -97,7 +97,7 @@ bool GodotShape3D::is_owner(GodotShapeOwner3D *p_owner) const { return owners.has(p_owner); } -const Map<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const { +const HashMap<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const { return owners; } @@ -411,9 +411,9 @@ void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 * } bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { - AABB aabb(-half_extents, half_extents * 2.0); + AABB aabb_ext(-half_extents, half_extents * 2.0); - return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); + return aabb_ext.intersects_segment(p_begin, p_end, &r_result, &r_normal); } bool GodotBoxShape3D::intersect_point(const Vector3 &p_point) const { @@ -662,7 +662,7 @@ GodotCapsuleShape3D::GodotCapsuleShape3D() {} /********** CYLINDER *************/ void GodotCylinderShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized(); + Vector3 cylinder_axis = p_transform.basis.get_column(1).normalized(); real_t axis_dot = cylinder_axis.dot(p_normal); Vector3 local_normal = p_transform.basis.xform_inv(p_normal); @@ -735,29 +735,6 @@ void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vect r_amount = 1; r_type = FEATURE_POINT; r_supports[0] = get_support(p_normal); - return; - - Vector3 n = p_normal; - real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius); - if (Math::abs(h) > 1.0) { - // Top or bottom surface. - n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5; - } else { - // Lateral surface. - n.y = height * 0.5 * h; - } - - real_t s = Math::sqrt(n.x * n.x + n.z * n.z); - if (Math::is_zero_approx(s)) { - n.x = 0.0; - n.z = 0.0; - } else { - real_t scaled_radius = radius / s; - n.x = n.x * scaled_radius; - n.z = n.z * scaled_radius; - } - - r_supports[0] = n; } } @@ -840,48 +817,78 @@ GodotCylinderShape3D::GodotCylinderShape3D() {} /********** CONVEX POLYGON *************/ void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { - int vertex_count = mesh.vertices.size(); + uint32_t vertex_count = mesh.vertices.size(); if (vertex_count == 0) { return; } const Vector3 *vrts = &mesh.vertices[0]; - for (int i = 0; i < vertex_count; i++) { - real_t d = p_normal.dot(p_transform.xform(vrts[i])); + if (vertex_count > 3 * extreme_vertices.size()) { + // For a large mesh, two calls to get_support() is faster than a full + // scan over all vertices. - if (i == 0 || d > r_max) { - r_max = d; - } - if (i == 0 || d < r_min) { - r_min = d; + Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); + r_min = p_normal.dot(p_transform.xform(get_support(-n))); + r_max = p_normal.dot(p_transform.xform(get_support(n))); + } else { + for (uint32_t i = 0; i < vertex_count; i++) { + real_t d = p_normal.dot(p_transform.xform(vrts[i])); + + if (i == 0 || d > r_max) { + r_max = d; + } + if (i == 0 || d < r_min) { + r_min = d; + } } } } Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const { - Vector3 n = p_normal; - - int vert_support_idx = -1; - real_t support_max = 0; - - int vertex_count = mesh.vertices.size(); - if (vertex_count == 0) { + if (mesh.vertices.size() == 0) { return Vector3(); } - const Vector3 *vrts = &mesh.vertices[0]; + // Find an initial guess for the support vertex by checking the ones we + // found in _setup(). - for (int i = 0; i < vertex_count; i++) { - real_t d = n.dot(vrts[i]); - - if (i == 0 || d > support_max) { - support_max = d; - vert_support_idx = i; + int best_vertex = -1; + real_t max_support = 0.0; + for (uint32_t i = 0; i < extreme_vertices.size(); i++) { + real_t s = p_normal.dot(mesh.vertices[extreme_vertices[i]]); + if (best_vertex == -1 || s > max_support) { + best_vertex = extreme_vertices[i]; + max_support = s; } } + if (extreme_vertices.size() == mesh.vertices.size()) { + // We've already checked every vertex, so we can return now. + return mesh.vertices[best_vertex]; + } + + // Move along the surface until we reach the true support vertex. - return vrts[vert_support_idx]; + int last_vertex = -1; + while (true) { + int next_vertex = -1; + for (uint32_t i = 0; i < vertex_neighbors[best_vertex].size(); i++) { + int vert = vertex_neighbors[best_vertex][i]; + if (vert != last_vertex) { + real_t s = p_normal.dot(mesh.vertices[vert]); + if (s > max_support) { + next_vertex = vert; + max_support = s; + break; + } + } + } + if (next_vertex == -1) { + return mesh.vertices[best_vertex]; + } + last_vertex = best_vertex; + best_vertex = next_vertex; + } } void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { @@ -938,13 +945,13 @@ void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, } for (int i = 0; i < ec; i++) { - real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal); + real_t dot = (vertices[edges[i].vertex_a] - vertices[edges[i].vertex_b]).normalized().dot(p_normal); dot = ABS(dot); - if (dot < edge_support_threshold && (edges[i].a == vtx || edges[i].b == vtx)) { + if (dot < edge_support_threshold && (edges[i].vertex_a == vtx || edges[i].vertex_b == vtx)) { r_amount = 2; r_type = FEATURE_EDGE; - r_supports[0] = vertices[edges[i].a]; - r_supports[1] = vertices[edges[i].b]; + r_supports[0] = vertices[edges[i].vertex_a]; + r_supports[1] = vertices[edges[i].vertex_b]; return; } } @@ -1048,8 +1055,8 @@ Vector3 GodotConvexPolygonShape3D::get_closest_point_to(const Vector3 &p_point) int ec = mesh.edges.size(); for (int i = 0; i < ec; i++) { Vector3 s[2] = { - vertices[edges[i].a], - vertices[edges[i].b] + vertices[edges[i].vertex_a], + vertices[edges[i].vertex_b] }; Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s); @@ -1081,7 +1088,7 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) { AABB _aabb; - for (int i = 0; i < mesh.vertices.size(); i++) { + for (uint32_t i = 0; i < mesh.vertices.size(); i++) { if (i == 0) { _aabb.position = mesh.vertices[i]; } else { @@ -1090,6 +1097,43 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) { } configure(_aabb); + + // Pre-compute the extreme vertices in 26 directions. This will be used + // to speed up get_support() by letting us quickly get a good guess for + // the support vertex. + + for (int x = -1; x < 2; x++) { + for (int y = -1; y < 2; y++) { + for (int z = -1; z < 2; z++) { + if (x != 0 || y != 0 || z != 0) { + Vector3 dir(x, y, z); + dir.normalize(); + real_t max_support = 0.0; + int best_vertex = -1; + for (uint32_t i = 0; i < mesh.vertices.size(); i++) { + real_t s = dir.dot(mesh.vertices[i]); + if (best_vertex == -1 || s > max_support) { + best_vertex = i; + max_support = s; + } + } + if (extreme_vertices.find(best_vertex) == -1) + extreme_vertices.push_back(best_vertex); + } + } + } + } + + // Record all the neighbors of each vertex. This is used in get_support(). + + if (extreme_vertices.size() < mesh.vertices.size()) { + vertex_neighbors.resize(mesh.vertices.size()); + for (uint32_t i = 0; i < mesh.edges.size(); i++) { + Geometry3D::MeshData::Edge &edge = mesh.edges[i]; + vertex_neighbors[edge.vertex_a].push_back(edge.vertex_b); + vertex_neighbors[edge.vertex_b].push_back(edge.vertex_a); + } + } } void GodotConvexPolygonShape3D::set_data(const Variant &p_data) { @@ -1097,7 +1141,12 @@ void GodotConvexPolygonShape3D::set_data(const Variant &p_data) { } Variant GodotConvexPolygonShape3D::get_data() const { - return mesh.vertices; + Vector<Vector3> vertices; + vertices.resize(mesh.vertices.size()); + for (uint32_t i = 0; i < mesh.vertices.size(); i++) { + vertices.write[i] = mesh.vertices[i]; + } + return vertices; } GodotConvexPolygonShape3D::GodotConvexPolygonShape3D() { @@ -1282,14 +1331,14 @@ Vector3 GodotConcavePolygonShape3D::get_support(const Vector3 &p_normal) const { } void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_params) const { - const BVH *bvh = &p_params->bvh[p_idx]; + const BVH *params_bvh = &p_params->bvh[p_idx]; - if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { + if (!params_bvh->aabb.intersects_segment(p_params->from, p_params->to)) { return; } - if (bvh->face_index >= 0) { - const Face *f = &p_params->faces[bvh->face_index]; + if (params_bvh->face_index >= 0) { + const Face *f = &p_params->faces[params_bvh->face_index]; GodotFaceShape3D *face = p_params->face; face->normal = f->normal; face->vertex[0] = p_params->vertices[f->indices[0]]; @@ -1308,11 +1357,11 @@ void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_ } } } else { - if (bvh->left >= 0) { - _cull_segment(bvh->left, p_params); + if (params_bvh->left >= 0) { + _cull_segment(params_bvh->left, p_params); } - if (bvh->right >= 0) { - _cull_segment(bvh->right, p_params); + if (params_bvh->right >= 0) { + _cull_segment(params_bvh->right, p_params); } } } @@ -1362,14 +1411,14 @@ Vector3 GodotConcavePolygonShape3D::get_closest_point_to(const Vector3 &p_point) } bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const { - const BVH *bvh = &p_params->bvh[p_idx]; + const BVH *params_bvh = &p_params->bvh[p_idx]; - if (!p_params->aabb.intersects(bvh->aabb)) { + if (!p_params->aabb.intersects(params_bvh->aabb)) { return false; } - if (bvh->face_index >= 0) { - const Face *f = &p_params->faces[bvh->face_index]; + if (params_bvh->face_index >= 0) { + const Face *f = &p_params->faces[params_bvh->face_index]; GodotFaceShape3D *face = p_params->face; face->normal = f->normal; face->vertex[0] = p_params->vertices[f->indices[0]]; @@ -1379,14 +1428,14 @@ bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const { return true; } } else { - if (bvh->left >= 0) { - if (_cull(bvh->left, p_params)) { + if (params_bvh->left >= 0) { + if (_cull(params_bvh->left, p_params)) { return true; } } - if (bvh->right >= 0) { - if (_cull(bvh->right, p_params)) { + if (params_bvh->right >= 0) { + if (_cull(params_bvh->right, p_params)) { return true; } } @@ -1438,7 +1487,7 @@ Vector3 GodotConcavePolygonShape3D::get_moment_of_inertia(real_t p_mass) const { struct _Volume_BVH_Element { AABB aabb; Vector3 center; - int face_index; + int face_index = 0; }; struct _Volume_BVH_CompareX { @@ -1461,10 +1510,10 @@ struct _Volume_BVH_CompareZ { struct _Volume_BVH { AABB aabb; - _Volume_BVH *left; - _Volume_BVH *right; + _Volume_BVH *left = nullptr; + _Volume_BVH *right = nullptr; - int face_index; + int face_index = 0; }; _Volume_BVH *_volume_build_bvh(_Volume_BVH_Element *p_elements, int p_size, int &count) { @@ -1942,14 +1991,14 @@ Vector3 GodotHeightMapShape3D::get_closest_point_to(const Vector3 &p_point) cons } void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const { - const AABB &aabb = get_aabb(); + const AABB &shape_aabb = get_aabb(); - Vector3 pos_local = aabb.position + local_origin; + Vector3 pos_local = shape_aabb.position + local_origin; Vector3 clamped_point(p_point); - clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x); - clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y); - clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z); + clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + shape_aabb.size.x); + clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + shape_aabb.size.y); + clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + shape_aabb.size.z); r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5); r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5); @@ -2093,19 +2142,19 @@ void GodotHeightMapShape3D::_setup(const Vector<real_t> &p_heights, int p_width, depth = p_depth; // Initialize aabb. - AABB aabb; - aabb.position = Vector3(0.0, p_min_height, 0.0); - aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); + AABB aabb_new; + aabb_new.position = Vector3(0.0, p_min_height, 0.0); + aabb_new.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); // Initialize origin as the aabb center. - local_origin = aabb.position + 0.5 * aabb.size; + local_origin = aabb_new.position + 0.5 * aabb_new.size; local_origin.y = 0.0; - aabb.position -= local_origin; + aabb_new.position -= local_origin; _build_accelerator(); - configure(aabb); + configure(aabb_new); } void GodotHeightMapShape3D::set_data(const Variant &p_data) { @@ -2116,11 +2165,11 @@ void GodotHeightMapShape3D::set_data(const Variant &p_data) { ERR_FAIL_COND(!d.has("depth")); ERR_FAIL_COND(!d.has("heights")); - int width = d["width"]; - int depth = d["depth"]; + int width_new = d["width"]; + int depth_new = d["depth"]; - ERR_FAIL_COND(width <= 0.0); - ERR_FAIL_COND(depth <= 0.0); + ERR_FAIL_COND(width_new <= 0.0); + ERR_FAIL_COND(depth_new <= 0.0); Variant heights_variant = d["heights"]; Vector<real_t> heights_buffer; @@ -2174,10 +2223,10 @@ void GodotHeightMapShape3D::set_data(const Variant &p_data) { ERR_FAIL_COND(min_height > max_height); - ERR_FAIL_COND(heights_buffer.size() != (width * depth)); + ERR_FAIL_COND(heights_buffer.size() != (width_new * depth_new)); // If specified, min and max height will be used as precomputed values. - _setup(heights_buffer, width, depth, min_height, max_height); + _setup(heights_buffer, width_new, depth_new, min_height, max_height); } Variant GodotHeightMapShape3D::get_data() const { @@ -2185,9 +2234,9 @@ Variant GodotHeightMapShape3D::get_data() const { d["width"] = width; d["depth"] = depth; - const AABB &aabb = get_aabb(); - d["min_height"] = aabb.position.y; - d["max_height"] = aabb.position.y + aabb.size.y; + const AABB &shape_aabb = get_aabb(); + d["min_height"] = shape_aabb.position.y; + d["max_height"] = shape_aabb.position.y + shape_aabb.size.y; d["heights"] = heights; diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h index 9d171c3928..dc8e34e2bc 100644 --- a/servers/physics_3d/godot_shape_3d.h +++ b/servers/physics_3d/godot_shape_3d.h @@ -51,7 +51,7 @@ class GodotShape3D { bool configured = false; real_t custom_bias = 0.0; - Map<GodotShapeOwner3D *, int> owners; + HashMap<GodotShapeOwner3D *, int> owners; protected: void configure(const AABB &p_aabb); @@ -93,7 +93,7 @@ public: void add_owner(GodotShapeOwner3D *p_owner); void remove_owner(GodotShapeOwner3D *p_owner); bool is_owner(GodotShapeOwner3D *p_owner) const; - const Map<GodotShapeOwner3D *, int> &get_owners() const; + const HashMap<GodotShapeOwner3D *, int> &get_owners() const; GodotShape3D() {} virtual ~GodotShape3D(); @@ -277,6 +277,8 @@ public: struct GodotConvexPolygonShape3D : public GodotShape3D { Geometry3D::MeshData mesh; + LocalVector<int> extreme_vertices; + LocalVector<LocalVector<int>> vertex_neighbors; void _setup(const Vector<Vector3> &p_vertices); diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp index f09da92cae..77110c5fbc 100644 --- a/servers/physics_3d/godot_soft_body_3d.cpp +++ b/servers/physics_3d/godot_soft_body_3d.cpp @@ -33,7 +33,7 @@ #include "godot_space_3d.h" #include "core/math/geometry_3d.h" -#include "core/templates/map.h" +#include "core/templates/rb_map.h" #include "servers/rendering_server.h" // Based on Bullet soft body. @@ -429,33 +429,33 @@ uint32_t GodotSoftBody3D::get_node_count() const { } real_t GodotSoftBody3D::get_node_inv_mass(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0); + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), 0.0); return nodes[p_node_index].im; } Vector3 GodotSoftBody3D::get_node_position(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3()); return nodes[p_node_index].x; } Vector3 GodotSoftBody3D::get_node_velocity(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3()); return nodes[p_node_index].v; } Vector3 GodotSoftBody3D::get_node_biased_velocity(uint32_t p_node_index) const { - ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3()); return nodes[p_node_index].bv; } void GodotSoftBody3D::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { - ERR_FAIL_COND(p_node_index >= nodes.size()); + ERR_FAIL_UNSIGNED_INDEX(p_node_index, nodes.size()); Node &node = nodes[p_node_index]; node.v += p_impulse * node.im; } void GodotSoftBody3D::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { - ERR_FAIL_COND(p_node_index >= nodes.size()); + ERR_FAIL_UNSIGNED_INDEX(p_node_index, nodes.size()); Node &node = nodes[p_node_index]; node.bv += p_impulse * node.im; } @@ -465,7 +465,7 @@ uint32_t GodotSoftBody3D::get_face_count() const { } void GodotSoftBody3D::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const { - ERR_FAIL_COND(p_face_index >= faces.size()); + ERR_FAIL_UNSIGNED_INDEX(p_face_index, faces.size()); const Face &face = faces[p_face_index]; r_point_1 = face.n[0]->x; r_point_2 = face.n[1]->x; @@ -473,7 +473,7 @@ void GodotSoftBody3D::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, } Vector3 GodotSoftBody3D::get_face_normal(uint32_t p_face_index) const { - ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3()); + ERR_FAIL_UNSIGNED_INDEX_V(p_face_index, faces.size(), Vector3()); return faces[p_face_index].normal; } @@ -494,7 +494,7 @@ bool GodotSoftBody3D::create_from_trimesh(const Vector<int> &p_indices, const Ve // Process vertices. { uint32_t vertex_count = 0; - Map<Vector3, uint32_t> unique_vertices; + HashMap<Vector3, uint32_t> unique_vertices; vertices.resize(visual_vertex_count); map_visual_to_physics.resize(visual_vertex_count); @@ -502,11 +502,11 @@ bool GodotSoftBody3D::create_from_trimesh(const Vector<int> &p_indices, const Ve for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) { const Vector3 &vertex = p_vertices[visual_vertex_index]; - Map<Vector3, uint32_t>::Element *e = unique_vertices.find(vertex); + HashMap<Vector3, uint32_t>::Iterator e = unique_vertices.find(vertex); uint32_t vertex_id; if (e) { // Already existing. - vertex_id = e->value(); + vertex_id = e->value; } else { // Create new one. vertex_id = vertex_count++; @@ -722,7 +722,14 @@ void GodotSoftBody3D::reoptimize_link_order() { const int reop_not_dependent = -1; const int reop_node_complete = -2; - uint32_t i, link_count = links.size(), node_count = nodes.size(); + uint32_t link_count = links.size(); + uint32_t node_count = nodes.size(); + + if (link_count < 1 || node_count < 2) { + return; + } + + uint32_t i; Link *lr; int ar, br; Node *node0 = &(nodes[0]); @@ -1272,7 +1279,7 @@ struct _SoftBodyIntersectSegmentInfo { real_t hit_dist_sq = INFINITY; static bool process_hit(uint32_t p_face_index, void *p_userdata) { - _SoftBodyIntersectSegmentInfo &query_info = *(_SoftBodyIntersectSegmentInfo *)(p_userdata); + _SoftBodyIntersectSegmentInfo &query_info = *(static_cast<_SoftBodyIntersectSegmentInfo *>(p_userdata)); Vector3 points[3]; query_info.soft_body->get_face_points(p_face_index, points[0], points[1], points[2]); diff --git a/servers/physics_3d/godot_soft_body_3d.h b/servers/physics_3d/godot_soft_body_3d.h index d9f5078313..86f73c366b 100644 --- a/servers/physics_3d/godot_soft_body_3d.h +++ b/servers/physics_3d/godot_soft_body_3d.h @@ -37,8 +37,8 @@ #include "core/math/aabb.h" #include "core/math/dynamic_bvh.h" #include "core/math/vector3.h" +#include "core/templates/hash_set.h" #include "core/templates/local_vector.h" -#include "core/templates/set.h" #include "core/templates/vset.h" class GodotConstraint3D; @@ -103,7 +103,7 @@ class GodotSoftBody3D : public GodotCollisionObject3D { SelfList<GodotSoftBody3D> active_list; - Set<GodotConstraint3D *> constraints; + HashSet<GodotConstraint3D *> constraints; Vector<AreaCMP> areas; @@ -123,7 +123,7 @@ public: _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } @@ -153,7 +153,7 @@ public: } } - virtual void set_space(GodotSpace3D *p_space); + virtual void set_space(GodotSpace3D *p_space) override; void set_mesh(RID p_mesh); @@ -204,8 +204,8 @@ public: void predict_motion(real_t p_delta); void solve_constraints(real_t p_delta); - _FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return ((Node *)p_node)->index; } - _FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return ((Face *)p_face)->index; } + _FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return static_cast<Node *>(p_node)->index; } + _FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return static_cast<Face *>(p_face)->index; } // Return true to stop the query. // p_index is the node index for AABB query, face index for Ray query. @@ -215,7 +215,7 @@ public: void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata); protected: - virtual void _shapes_changed(); + virtual void _shapes_changed() override; private: void update_normals_and_centroids(); diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp index e8af2d7283..c23485279d 100644 --- a/servers/physics_3d/godot_space_3d.cpp +++ b/servers/physics_3d/godot_space_3d.cpp @@ -120,8 +120,8 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parame bool collided = false; Vector3 res_point, res_normal; - int res_shape; - const GodotCollisionObject3D *res_obj; + int res_shape = -1; + const GodotCollisionObject3D *res_obj = nullptr; real_t min_d = 1e10; for (int i = 0; i < amount; i++) { @@ -185,6 +185,7 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parame if (!collided) { return false; } + ERR_FAIL_NULL_V(res_obj, false); // Shouldn't happen but silences warning. r_result.collider_id = res_obj->get_instance_id(); if (r_result.collider_id.is_valid()) { @@ -445,7 +446,7 @@ struct _RestCallbackData { }; static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - _RestCallbackData *rd = (_RestCallbackData *)p_userdata; + _RestCallbackData *rd = static_cast<_RestCallbackData *>(p_userdata); Vector3 contact_rel = p_point_B - p_point_A; real_t len = contact_rel.length(); @@ -701,6 +702,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: const int max_results = 32; int recover_attempts = 4; Vector3 sr[max_results * 2]; + real_t priorities[max_results]; do { GodotPhysicsServer3D::CollCbkData cbk; @@ -710,6 +712,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk; GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk; + int priority_amount = 0; bool collided = false; @@ -737,6 +740,10 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: if (GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, margin)) { collided = cbk.amount > 0; } + while (cbk.amount > priority_amount) { + priorities[priority_amount] = col_obj->get_collision_priority(); + priority_amount++; + } } } @@ -744,6 +751,12 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: break; } + real_t inv_total_weight = 0.0; + for (int i = 0; i < cbk.amount; i++) { + inv_total_weight += priorities[i]; + } + inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight; + recovered = true; Vector3 recover_motion; @@ -759,7 +772,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: real_t depth = n.dot(a + recover_motion) - d; if (depth > min_contact_depth + CMP_EPSILON) { // Only recover if there is penetration. - recover_motion -= n * (depth - min_contact_depth) * 0.4; + recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight; } } @@ -906,7 +919,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: } bool collided = false; - if (recovered || (safe < 1)) { + if ((p_parameters.recovery_as_collision && recovered) || (safe < 1)) { if (safe >= 1) { best_shape = -1; //no best shape with cast, reset to -1 } @@ -989,6 +1002,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: r_result->collision_unsafe_fraction = unsafe; r_result->collision_count = rcd.result_count; + r_result->collision_depth = rcd.best_result.len; } collided = true; @@ -1002,6 +1016,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: r_result->collision_safe_fraction = 1.0; r_result->collision_unsafe_fraction = 1.0; + r_result->collision_depth = 0.0; } return collided; @@ -1017,7 +1032,7 @@ void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A SWAP(type_A, type_B); } - GodotSpace3D *self = (GodotSpace3D *)p_self; + GodotSpace3D *self = static_cast<GodotSpace3D *>(p_self); self->collision_pairs++; @@ -1038,10 +1053,10 @@ void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A } } else if (type_A == GodotCollisionObject3D::TYPE_BODY) { if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) { - GodotBodySoftBodyPair3D *soft_pair = memnew(GodotBodySoftBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotSoftBody3D *)B)); + GodotBodySoftBodyPair3D *soft_pair = memnew(GodotBodySoftBodyPair3D(static_cast<GodotBody3D *>(A), p_subindex_A, static_cast<GodotSoftBody3D *>(B))); return soft_pair; } else { - GodotBodyPair3D *b = memnew(GodotBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotBody3D *)B, p_subindex_B)); + GodotBodyPair3D *b = memnew(GodotBodyPair3D(static_cast<GodotBody3D *>(A), p_subindex_A, static_cast<GodotBody3D *>(B), p_subindex_B)); return b; } } else { @@ -1056,9 +1071,9 @@ void GodotSpace3D::_broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_ return; } - GodotSpace3D *self = (GodotSpace3D *)p_self; + GodotSpace3D *self = static_cast<GodotSpace3D *>(p_self); self->collision_pairs--; - GodotConstraint3D *c = (GodotConstraint3D *)p_data; + GodotConstraint3D *c = static_cast<GodotConstraint3D *>(p_data); memdelete(c); } @@ -1096,7 +1111,7 @@ void GodotSpace3D::remove_object(GodotCollisionObject3D *p_object) { objects.erase(p_object); } -const Set<GodotCollisionObject3D *> &GodotSpace3D::get_objects() const { +const HashSet<GodotCollisionObject3D *> &GodotSpace3D::get_objects() const { return objects; } @@ -1235,7 +1250,7 @@ GodotPhysicsDirectSpaceState3D *GodotSpace3D::get_direct_state() { GodotSpace3D::GodotSpace3D() { body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); - body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0)); + body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg_to_rad(8.0)); body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); diff --git a/servers/physics_3d/godot_space_3d.h b/servers/physics_3d/godot_space_3d.h index ac54c8bf69..df7315e96d 100644 --- a/servers/physics_3d/godot_space_3d.h +++ b/servers/physics_3d/godot_space_3d.h @@ -47,7 +47,7 @@ class GodotPhysicsDirectSpaceState3D : public PhysicsDirectSpaceState3D { GDCLASS(GodotPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D); public: - GodotSpace3D *space; + GodotSpace3D *space = nullptr; virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override; @@ -89,7 +89,7 @@ private: static void *_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self); static void _broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self); - Set<GodotCollisionObject3D *> objects; + HashSet<GodotCollisionObject3D *> objects; GodotArea3D *area = nullptr; @@ -158,7 +158,7 @@ public: void add_object(GodotCollisionObject3D *p_object); void remove_object(GodotCollisionObject3D *p_object); - const Set<GodotCollisionObject3D *> &get_objects() const; + const HashSet<GodotCollisionObject3D *> &get_objects() const; _FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; } _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } diff --git a/servers/physics_3d/godot_step_3d.cpp b/servers/physics_3d/godot_step_3d.cpp index 63635b224b..bfedcd29c0 100644 --- a/servers/physics_3d/godot_step_3d.cpp +++ b/servers/physics_3d/godot_step_3d.cpp @@ -44,12 +44,12 @@ void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D p_body->set_island_step(_step); if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { - // Only dynamic bodies are tested for activation. + // Only rigid bodies are tested for activation. p_body_island.push_back(p_body); } for (const KeyValue<GodotConstraint3D *, int> &E : p_body->get_constraint_map()) { - GodotConstraint3D *constraint = (GodotConstraint3D *)E.key; + GodotConstraint3D *constraint = const_cast<GodotConstraint3D *>(E.key); if (constraint->get_island_step() == _step) { continue; // Already processed. } @@ -87,8 +87,8 @@ void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D void GodotStep3D::_populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) { p_soft_body->set_island_step(_step); - for (Set<GodotConstraint3D *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) { - GodotConstraint3D *constraint = (GodotConstraint3D *)E->get(); + for (const GodotConstraint3D *E : p_soft_body->get_constraints()) { + GodotConstraint3D *constraint = const_cast<GodotConstraint3D *>(E); if (constraint->get_island_step() == _step) { continue; // Already processed. } @@ -236,8 +236,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { const SelfList<GodotArea3D>::List &aml = p_space->get_moved_area_list(); while (aml.first()) { - for (const Set<GodotConstraint3D *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - GodotConstraint3D *constraint = E->get(); + for (GodotConstraint3D *E : aml.first()->self()->get_constraints()) { + GodotConstraint3D *constraint = E; if (constraint->get_island_step() == _step) { continue; } @@ -343,7 +343,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ uint32_t total_contraint_count = all_constraints.size(); - work_pool.do_work(total_contraint_count, this, &GodotStep3D::_setup_contraint, nullptr); + WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep3D::_setup_contraint, nullptr, total_contraint_count, -1, true, SNAME("Physics3DConstraintSetup")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -362,7 +363,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { // Warning: _solve_island modifies the constraint islands for optimization purpose, // their content is not reliable after these calls and shouldn't be used anymore. - work_pool.do_work(island_count, this, &GodotStep3D::_solve_island, nullptr); + group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep3D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics3DConstraintSolveIslands")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -409,10 +411,7 @@ GodotStep3D::GodotStep3D() { body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE); all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); - - work_pool.init(); } GodotStep3D::~GodotStep3D() { - work_pool.finish(); } diff --git a/servers/physics_3d/godot_step_3d.h b/servers/physics_3d/godot_step_3d.h index 6d975b0dd3..189487757f 100644 --- a/servers/physics_3d/godot_step_3d.h +++ b/servers/physics_3d/godot_step_3d.h @@ -33,8 +33,8 @@ #include "godot_space_3d.h" +#include "core/object/worker_thread_pool.h" #include "core/templates/local_vector.h" -#include "core/templates/thread_work_pool.h" class GodotStep3D { uint64_t _step = 1; @@ -42,8 +42,6 @@ class GodotStep3D { int iterations = 0; real_t delta = 0.0; - ThreadWorkPool work_pool; - LocalVector<LocalVector<GodotBody3D *>> body_islands; LocalVector<LocalVector<GodotConstraint3D *>> constraint_islands; LocalVector<GodotConstraint3D *> all_constraints; diff --git a/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp index dc4858ff9a..28cc064a1e 100644 --- a/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp @@ -147,8 +147,8 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { Vector3 b1Axis1, b1Axis2, b1Axis3; Vector3 b2Axis1, b2Axis2; - b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0)); - b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0)); + b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(0)); + b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_column(0)); real_t swing1 = real_t(0.), swing2 = real_t(0.); @@ -158,7 +158,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { // Get Frame into world space if (m_swingSpan1 >= real_t(0.05f)) { - b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1)); + b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(1)); //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); @@ -169,7 +169,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { } if (m_swingSpan2 >= real_t(0.05f)) { - b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2)); + b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(2)); //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); @@ -199,7 +199,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { // Twist limits if (m_twistSpan >= real_t(0.)) { - Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); + Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_column(1)); Quaternion rotationArc = Quaternion(b2Axis1, b1Axis1); Vector3 TwistRef = rotationArc.xform(b2Axis22); real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); diff --git a/servers/physics_3d/joints/godot_cone_twist_joint_3d.h b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h index fdcc2ceea3..1651d34916 100644 --- a/servers/physics_3d/joints/godot_cone_twist_joint_3d.h +++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h @@ -28,6 +28,9 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_CONE_TWIST_JOINT_3D_H +#define GODOT_CONE_TWIST_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ @@ -49,9 +52,6 @@ subject to the following restrictions: Written by: Marcus Hennix */ -#ifndef GODOT_CONE_TWIST_JOINT_3D_H -#define GODOT_CONE_TWIST_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp index 41e9bf06bb..e0fa940104 100644 --- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp @@ -249,8 +249,8 @@ void GodotGeneric6DOFJoint3D::calculateAngleInfo() { // easier to take the euler rate expression for d(angle[2])/dt with respect // to the components of w and set that to 0. - Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0); - Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2); + Vector3 axis0 = m_calculatedTransformB.basis.get_column(0); + Vector3 axis2 = m_calculatedTransformA.basis.get_column(2); m_calculatedAxis[1] = axis2.cross(axis0); m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); @@ -345,9 +345,9 @@ bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) { for (i = 0; i < 3; i++) { if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { if (m_useLinearReferenceFrameA) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); } else { - normalWorld = m_calculatedTransformB.basis.get_axis(i); + normalWorld = m_calculatedTransformB.basis.get_column(i); } buildLinearJacobian( @@ -388,9 +388,9 @@ void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) { jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); if (m_useLinearReferenceFrameA) { - linear_axis = m_calculatedTransformA.basis.get_axis(i); + linear_axis = m_calculatedTransformA.basis.get_column(i); } else { - linear_axis = m_calculatedTransformB.basis.get_axis(i); + linear_axis = m_calculatedTransformB.basis.get_column(i); } m_linearLimits.solveLinearAxis( diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h index bcf2d18647..6a0ab81461 100644 --- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H +#define GODOT_GENERIC_6DOF_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H -#define GODOT_GENERIC_6DOF_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp index 1c4d5dec23..01c59395c8 100644 --- a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp @@ -92,16 +92,16 @@ GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const V m_rbAFrame.origin = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis - Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0); + Vector3 rbAxisA1 = rbA->get_transform().basis.get_column(0); Vector3 rbAxisA2; real_t projection = axisInA.dot(rbAxisA1); if (projection >= 1.0f - CMP_EPSILON) { - rbAxisA1 = -rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); + rbAxisA1 = -rbA->get_transform().basis.get_column(2); + rbAxisA2 = rbA->get_transform().basis.get_column(1); } else if (projection <= -1.0f + CMP_EPSILON) { - rbAxisA1 = rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); + rbAxisA1 = rbA->get_transform().basis.get_column(2); + rbAxisA2 = rbA->get_transform().basis.get_column(1); } else { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); @@ -171,11 +171,11 @@ bool GodotHingeJoint3D::setup(real_t p_step) { Vector3 jointAxis0local; Vector3 jointAxis1local; - plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local); + plane_space(m_rbAFrame.basis.get_column(2), jointAxis0local, jointAxis1local); Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local); Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); - Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); memnew_placement( &m_jacAng[0], @@ -226,7 +226,7 @@ bool GodotHingeJoint3D::setup(real_t p_step) { } //Compute K = J*W*J' for hinge axis - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); return true; @@ -271,8 +271,8 @@ void GodotHingeJoint3D::solve(real_t p_step) { ///solve angular part // get axes in world space - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2)); + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); + Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(2)); const Vector3 &angVelA = A->get_angular_velocity(); const Vector3 &angVelB = B->get_angular_velocity(); @@ -384,9 +384,9 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { } real_t GodotHingeJoint3D::get_hinge_angle() { - const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0)); - const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1)); - const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1)); + const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0)); + const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(1)); + const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(1)); return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); } diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.h b/servers/physics_3d/joints/godot_hinge_joint_3d.h index b934540e8d..f1187771af 100644 --- a/servers/physics_3d/joints/godot_hinge_joint_3d.h +++ b/servers/physics_3d/joints/godot_hinge_joint_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_HINGE_JOINT_3D_H +#define GODOT_HINGE_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_HINGE_JOINT_3D_H -#define GODOT_HINGE_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" diff --git a/servers/physics_3d/joints/godot_jacobian_entry_3d.h b/servers/physics_3d/joints/godot_jacobian_entry_3d.h index 0fe15751d5..a46ce830ec 100644 --- a/servers/physics_3d/joints/godot_jacobian_entry_3d.h +++ b/servers/physics_3d/joints/godot_jacobian_entry_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_JACOBIAN_ENTRY_3D_H +#define GODOT_JACOBIAN_ENTRY_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_JACOBIAN_ENTRY_3D_H -#define GODOT_JACOBIAN_ENTRY_3D_H - /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ diff --git a/servers/physics_3d/joints/godot_pin_joint_3d.h b/servers/physics_3d/joints/godot_pin_joint_3d.h index eeeaa650bd..b3e2389d5a 100644 --- a/servers/physics_3d/joints/godot_pin_joint_3d.h +++ b/servers/physics_3d/joints/godot_pin_joint_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_PIN_JOINT_3D_H +#define GODOT_PIN_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_PIN_JOINT_3D_H -#define GODOT_PIN_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" diff --git a/servers/physics_3d/joints/godot_slider_joint_3d.cpp b/servers/physics_3d/joints/godot_slider_joint_3d.cpp index f175421304..4539be21e3 100644 --- a/servers/physics_3d/joints/godot_slider_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_slider_joint_3d.cpp @@ -102,7 +102,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; m_relPosA = m_projPivotInW - A->get_transform().origin; @@ -111,7 +111,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { int i; //linear part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); memnew_placement( &m_jacLin[i], GodotJacobianEntry3D( @@ -130,7 +130,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { testLinLimits(); // angular part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); memnew_placement( &m_jacAng[i], GodotJacobianEntry3D( @@ -141,7 +141,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { B->get_inv_inertia())); } testAngLimits(); - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); + Vector3 axisA = m_calculatedTransformA.basis.get_column(0); m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); // clear accumulator for motors m_accumulatedLinMotorImpulse = real_t(0.0); @@ -206,8 +206,8 @@ void GodotSliderJoint3D::solve(real_t p_step) { } // angular // get axes in world space - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); + Vector3 axisA = m_calculatedTransformA.basis.get_column(0); + Vector3 axisB = m_calculatedTransformB.basis.get_column(0); const Vector3 &angVelA = A->get_angular_velocity(); const Vector3 &angVelB = B->get_angular_velocity(); @@ -297,14 +297,14 @@ void GodotSliderJoint3D::calculateTransforms() { m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; Vector3 normalWorld; int i; //linear part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); m_depth[i] = m_delta.dot(normalWorld); } } @@ -335,9 +335,9 @@ void GodotSliderJoint3D::testAngLimits() { m_angDepth = real_t(0.); m_solveAngLim = false; if (m_lowerAngLimit <= m_upperAngLimit) { - const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1); - const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2); - const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1); + const Vector3 axisA0 = m_calculatedTransformA.basis.get_column(1); + const Vector3 axisA1 = m_calculatedTransformA.basis.get_column(2); + const Vector3 axisB0 = m_calculatedTransformB.basis.get_column(1); real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); if (rot < m_lowerAngLimit) { m_angDepth = rot - m_lowerAngLimit; diff --git a/servers/physics_3d/joints/godot_slider_joint_3d.h b/servers/physics_3d/joints/godot_slider_joint_3d.h index f596c9ff75..29d19be0d7 100644 --- a/servers/physics_3d/joints/godot_slider_joint_3d.h +++ b/servers/physics_3d/joints/godot_slider_joint_3d.h @@ -28,13 +28,13 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ +#ifndef GODOT_SLIDER_JOINT_3D_H +#define GODOT_SLIDER_JOINT_3D_H + /* Adapted to Godot from the Bullet library. */ -#ifndef GODOT_SLIDER_JOINT_3D_H -#define GODOT_SLIDER_JOINT_3D_H - #include "servers/physics_3d/godot_joint_3d.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h" |