diff options
Diffstat (limited to 'servers/physics_3d')
45 files changed, 1476 insertions, 1089 deletions
diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index ef6535a878..ba362740b2 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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 */ @@ -918,7 +918,7 @@ bool Distance( const GodotShape3D* shape0, { results.status = gjk_status==GJK::eStatus::Inside? sResults::Penetrating : - sResults::GJK_Failed ; + sResults::GJK_Failed; return(false); } } diff --git a/servers/physics_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h index 39a7d03435..309af76561 100644 --- a/servers/physics_3d/gjk_epa.h +++ b/servers/physics_3d/gjk_epa.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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 e115e17061..9765d0bf58 100644 --- a/servers/physics_3d/godot_area_3d.cpp +++ b/servers/physics_3d/godot_area_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -86,16 +86,16 @@ void GodotArea3D::set_space(GodotSpace3D *p_space) { _set_space(p_space); } -void GodotArea3D::set_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == monitor_callback_id) { - monitor_callback_method = p_method; +void GodotArea3D::set_monitor_callback(const Callable &p_callback) { + ObjectID id = p_callback.get_object_id(); + if (id == monitor_callback.get_object_id()) { + monitor_callback = p_callback; return; } _unregister_shapes(); - monitor_callback_id = p_id; - monitor_callback_method = p_method; + monitor_callback = p_callback; monitored_bodies.clear(); monitored_areas.clear(); @@ -107,16 +107,16 @@ void GodotArea3D::set_monitor_callback(ObjectID p_id, const StringName &p_method } } -void GodotArea3D::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { - if (p_id == area_monitor_callback_id) { - area_monitor_callback_method = p_method; +void GodotArea3D::set_area_monitor_callback(const Callable &p_callback) { + ObjectID id = p_callback.get_object_id(); + if (id == area_monitor_callback.get_object_id()) { + area_monitor_callback = p_callback; return; } _unregister_shapes(); - area_monitor_callback_id = p_id; - area_monitor_callback_method = p_method; + area_monitor_callback = p_callback; monitored_bodies.clear(); monitored_areas.clear(); @@ -128,18 +128,21 @@ void GodotArea3D::set_area_monitor_callback(ObjectID p_id, const StringName &p_m } } -void GodotArea3D::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode) { - bool do_override = p_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; - if (do_override == (space_override_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) { +void GodotArea3D::_set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::AreaSpaceOverrideMode p_new_mode) { + bool do_override = p_new_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override == (r_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) { return; } _unregister_shapes(); - space_override_mode = p_mode; + r_mode = p_new_mode; _shape_changed(); } void GodotArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) { switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: + _set_space_override_mode(gravity_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value); + break; case PhysicsServer3D::AREA_PARAM_GRAVITY: gravity = p_value; break; @@ -155,9 +158,15 @@ void GodotArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Varian case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation = p_value; break; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: + _set_space_override_mode(linear_damping_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value); + break; case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: linear_damp = p_value; break; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: + _set_space_override_mode(angular_damping_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value); + break; case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: angular_damp = p_value; break; @@ -183,6 +192,8 @@ void GodotArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Varian Variant GodotArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const { switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: + return gravity_override_mode; case PhysicsServer3D::AREA_PARAM_GRAVITY: return gravity; case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: @@ -193,8 +204,12 @@ Variant GodotArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const { return gravity_distance_scale; case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: + return linear_damping_override_mode; case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: return linear_damp; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: + return angular_damping_override_mode; case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: return angular_damp; case PhysicsServer3D::AREA_PARAM_PRIORITY: @@ -227,80 +242,91 @@ void GodotArea3D::set_monitorable(bool p_monitorable) { monitorable = p_monitorable; _set_static(!monitorable); + _shapes_changed(); } void GodotArea3D::call_queries() { - if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - Object *obj = ObjectDB::get_instance(monitor_callback_id); - if (!obj) { - monitored_bodies.clear(); - monitor_callback_id = ObjectID(); - return; - } - - 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); - E = next; - continue; + if (!monitor_callback.is_null() && !monitored_bodies.is_empty()) { + if (monitor_callback.is_valid()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; } - 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; + 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->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; + + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); + E = next; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); - E = next; + Callable::CallError ce; + Variant ret; + monitor_callback.callp((const Variant **)resptr, 5, ret, ce); - Callable::CallError ce; - obj->call(monitor_callback_method, (const Variant **)resptr, 5, 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(); + monitor_callback = Callable(); } } - if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) { - Variant res[5]; - Variant *resptr[5]; - for (int i = 0; i < 5; i++) { - resptr[i] = &res[i]; - } - - Object *obj = ObjectDB::get_instance(area_monitor_callback_id); - if (!obj) { - monitored_areas.clear(); - area_monitor_callback_id = ObjectID(); - return; - } - - 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); - E = next; - continue; + if (!area_monitor_callback.is_null() && !monitored_areas.is_empty()) { + if (area_monitor_callback.is_valid()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; } - 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; + 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->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; + + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); + E = next; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); - E = next; + Callable::CallError ce; + Variant ret; + area_monitor_callback.callp((const Variant **)resptr, 5, ret, ce); - Callable::CallError ce; - obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, 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(); + area_monitor_callback = Callable(); } } } diff --git a/servers/physics_3d/godot_area_3d.h b/servers/physics_3d/godot_area_3d.h index e8caa9221b..51b435eb00 100644 --- a/servers/physics_3d/godot_area_3d.h +++ b/servers/physics_3d/godot_area_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -42,7 +42,10 @@ class GodotSoftBody3D; class GodotConstraint3D; class GodotArea3D : public GodotCollisionObject3D { - PhysicsServer3D::AreaSpaceOverrideMode space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer3D::AreaSpaceOverrideMode gravity_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer3D::AreaSpaceOverrideMode linear_damping_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer3D::AreaSpaceOverrideMode angular_damping_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + real_t gravity = 9.80665; Vector3 gravity_vector = Vector3(0, -1, 0); bool gravity_is_point = false; @@ -57,11 +60,8 @@ class GodotArea3D : public GodotCollisionObject3D { int priority = 0; bool monitorable = false; - ObjectID monitor_callback_id; - StringName monitor_callback_method; - - ObjectID area_monitor_callback_id; - StringName area_monitor_callback_method; + Callable monitor_callback; + Callable area_monitor_callback; SelfList<GodotArea3D> monitor_query_list; SelfList<GodotArea3D> moved_list; @@ -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,21 +95,23 @@ 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); + public: - void set_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } + void set_monitor_callback(const Callable &p_callback); + _FORCE_INLINE_ bool has_monitor_callback() const { return !monitor_callback.is_null(); } - void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); - _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } + void set_area_monitor_callback(const Callable &p_callback); + _FORCE_INLINE_ bool has_area_monitor_callback() const { return !area_monitor_callback.is_null(); } _FORCE_INLINE_ void add_body_to_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); _FORCE_INLINE_ void remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); @@ -124,9 +125,6 @@ public: void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value); Variant get_param(PhysicsServer3D::AreaParameter p_param) const; - void set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode); - PhysicsServer3D::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } - _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } _FORCE_INLINE_ real_t get_gravity() const { return gravity; } @@ -165,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); @@ -173,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.cpp b/servers/physics_3d/godot_area_pair_3d.cpp index 7453153de6..a75cb74f60 100644 --- a/servers/physics_3d/godot_area_pair_3d.cpp +++ b/servers/physics_3d/godot_area_pair_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -39,10 +39,18 @@ bool GodotAreaPair3D::setup(real_t p_step) { } process_collision = false; + has_space_override = false; if (result != colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - process_collision = true; - } else if (area->has_monitor_callback()) { + if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } + process_collision = has_space_override; + + if (area->has_monitor_callback()) { process_collision = true; } @@ -58,7 +66,7 @@ bool GodotAreaPair3D::pre_solve(real_t p_step) { } if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + if (has_space_override) { body->add_area(area); } @@ -66,7 +74,7 @@ bool GodotAreaPair3D::pre_solve(real_t p_step) { area->add_body_to_query(body, body_shape, area_shape); } } else { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + if (has_space_override) { body->remove_area(area); } @@ -96,7 +104,7 @@ GodotAreaPair3D::GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotAre GodotAreaPair3D::~GodotAreaPair3D() { if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + if (has_space_override) { body->remove_area(area); } if (area->has_monitor_callback()) { @@ -121,7 +129,7 @@ bool GodotArea2Pair3D::setup(real_t p_step) { process_collision_a = false; if (result_a != colliding_a) { - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + if (area_a->has_area_monitor_callback() && area_b_monitorable) { process_collision_a = true; process_collision = true; } @@ -130,7 +138,7 @@ bool GodotArea2Pair3D::setup(real_t p_step) { process_collision_b = false; if (result_b != colliding_b) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + if (area_b->has_area_monitor_callback() && area_a_monitorable) { process_collision_b = true; process_collision = true; } @@ -169,19 +177,21 @@ GodotArea2Pair3D::GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotAr area_b = p_area_b; shape_a = p_shape_a; shape_b = p_shape_b; + area_a_monitorable = area_a->is_monitorable(); + area_b_monitorable = area_b->is_monitorable(); area_a->add_constraint(this); area_b->add_constraint(this); } GodotArea2Pair3D::~GodotArea2Pair3D() { if (colliding_a) { - if (area_a->has_area_monitor_callback()) { + if (area_a->has_area_monitor_callback() && area_b_monitorable) { area_a->remove_area_from_query(area_b, shape_b, shape_a); } } if (colliding_b) { - if (area_b->has_area_monitor_callback()) { + if (area_b->has_area_monitor_callback() && area_a_monitorable) { area_b->remove_area_from_query(area_a, shape_a, shape_b); } } @@ -207,10 +217,15 @@ bool GodotAreaSoftBodyPair3D::setup(real_t p_step) { } process_collision = false; + has_space_override = false; if (result != colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - process_collision = true; - } else if (area->has_monitor_callback()) { + if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if (area->get_wind_force_magnitude() > CMP_EPSILON) { + has_space_override = true; + } + + if (area->has_monitor_callback()) { process_collision = true; } @@ -226,7 +241,7 @@ bool GodotAreaSoftBodyPair3D::pre_solve(real_t p_step) { } if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + if (has_space_override) { soft_body->add_area(area); } @@ -234,7 +249,7 @@ bool GodotAreaSoftBodyPair3D::pre_solve(real_t p_step) { area->add_soft_body_to_query(soft_body, soft_body_shape, area_shape); } } else { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + if (has_space_override) { soft_body->remove_area(area); } @@ -261,7 +276,7 @@ GodotAreaSoftBodyPair3D::GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_soft_body, i GodotAreaSoftBodyPair3D::~GodotAreaSoftBodyPair3D() { if (colliding) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + if (has_space_override) { soft_body->remove_area(area); } if (area->has_monitor_callback()) { diff --git a/servers/physics_3d/godot_area_pair_3d.h b/servers/physics_3d/godot_area_pair_3d.h index f55c03be03..64b43a3b51 100644 --- a/servers/physics_3d/godot_area_pair_3d.h +++ b/servers/physics_3d/godot_area_pair_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -37,12 +37,13 @@ #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; bool process_collision = false; + bool has_space_override = false; public: virtual bool setup(real_t p_step) override; @@ -54,14 +55,16 @@ 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; bool colliding_b = false; bool process_collision_a = false; bool process_collision_b = false; + bool area_a_monitorable; + bool area_b_monitorable; public: virtual bool setup(real_t p_step) override; @@ -73,12 +76,13 @@ 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; bool process_collision = false; + bool has_space_override = false; public: virtual bool setup(real_t p_step) override; diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp index 02929eeaed..4c89106839 100644 --- a/servers/physics_3d/godot_body_3d.cpp +++ b/servers/physics_3d/godot_body_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -227,8 +227,19 @@ void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Varian _update_transform_dependent(); } break; case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { + if (Math::is_zero_approx(gravity_scale)) { + wakeup(); + } gravity_scale = p_value; } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: { + int mode_value = p_value; + linear_damp_mode = (PhysicsServer3D::BodyDampMode)mode_value; + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: { + int mode_value = p_value; + angular_damp_mode = (PhysicsServer3D::BodyDampMode)mode_value; + } break; case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { linear_damp = p_value; } break; @@ -259,11 +270,17 @@ Variant GodotBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const { } } break; case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: { - return center_of_mass; + return center_of_mass_local; } break; case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { return gravity_scale; } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: { + return linear_damp_mode; + } + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: { + return angular_damp_mode; + } case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { return linear_damp; } break; @@ -443,15 +460,6 @@ void GodotBody3D::set_space(GodotSpace3D *p_space) { } } -void GodotBody3D::_compute_area_gravity_and_damping(const GodotArea3D *p_area) { - Vector3 area_gravity; - p_area->compute_gravity(get_transform().get_origin(), area_gravity); - gravity += area_gravity; - - area_linear_damp += p_area->get_linear_damp(); - area_angular_damp += p_area->get_angular_damp(); -} - void GodotBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) { if (lock) { locked_axis |= p_axis; @@ -469,63 +477,135 @@ void GodotBody3D::integrate_forces(real_t p_step) { return; } - GodotArea3D *def_area = get_space()->get_default_area(); - - ERR_FAIL_COND(!def_area); + ERR_FAIL_COND(!get_space()); int ac = areas.size(); + + bool gravity_done = false; + bool linear_damp_done = false; + bool angular_damp_done = false; + bool stopped = false; + gravity = Vector3(0, 0, 0); - area_linear_damp = 0; - area_angular_damp = 0; + + total_linear_damp = 0.0; + total_angular_damp = 0.0; + + // Combine gravity and damping from overlapping areas in priority order. if (ac) { areas.sort(); const AreaCMP *aa = &areas[0]; - // damp_area = aa[ac-1].area; for (int i = ac - 1; i >= 0 && !stopped; i--) { - PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector3(0, 0, 0); - area_angular_damp = 0; - area_linear_damp = 0; - _compute_area_gravity_and_damping(aa[i].area); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { + if (!gravity_done) { + PhysicsServer3D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE); + if (area_gravity_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + Vector3 area_gravity; + aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity); + switch (area_gravity_mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + gravity += area_gravity; + gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = area_gravity; + gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + if (!linear_damp_done) { + PhysicsServer3D::AreaSpaceOverrideMode area_linear_damp_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE); + if (area_linear_damp_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + real_t area_linear_damp = aa[i].area->get_linear_damp(); + switch (area_linear_damp_mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + total_linear_damp += area_linear_damp; + linear_damp_done = area_linear_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + total_linear_damp = area_linear_damp; + linear_damp_done = area_linear_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } } } + if (!angular_damp_done) { + PhysicsServer3D::AreaSpaceOverrideMode area_angular_damp_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE); + if (area_angular_damp_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + real_t area_angular_damp = aa[i].area->get_angular_damp(); + switch (area_angular_damp_mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + total_angular_damp += area_angular_damp; + angular_damp_done = area_angular_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + total_angular_damp = area_angular_damp; + angular_damp_done = area_angular_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + stopped = gravity_done && linear_damp_done && angular_damp_done; } } + // Add default gravity and damping from space area. if (!stopped) { - _compute_area_gravity_and_damping(def_area); - } + GodotArea3D *default_area = get_space()->get_default_area(); + ERR_FAIL_COND(!default_area); - gravity *= gravity_scale; + if (!gravity_done) { + Vector3 default_gravity; + default_area->compute_gravity(get_transform().get_origin(), default_gravity); + gravity += default_gravity; + } + + if (!linear_damp_done) { + total_linear_damp += default_area->get_linear_damp(); + } + + if (!angular_damp_done) { + total_angular_damp += default_area->get_angular_damp(); + } + } - // If less than 0, override dampenings with that of the Body - if (angular_damp >= 0) { - area_angular_damp = angular_damp; + // Override linear damping with body's value. + switch (linear_damp_mode) { + case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: { + total_linear_damp += linear_damp; + } break; + case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: { + total_linear_damp = linear_damp; + } break; } - /* - else - area_angular_damp=damp_area->get_angular_damp(); - */ - if (linear_damp >= 0) { - area_linear_damp = linear_damp; + // Override angular damping with body's value. + switch (angular_damp_mode) { + case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: { + total_angular_damp += angular_damp; + } break; + case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: { + total_angular_damp = angular_damp; + } break; } - /* - else - area_linear_damp=damp_area->get_linear_damp(); - */ + + gravity *= gravity_scale; + + prev_linear_velocity = linear_velocity; + prev_angular_velocity = angular_velocity; Vector3 motion; bool do_motion = false; @@ -548,17 +628,16 @@ void GodotBody3D::integrate_forces(real_t p_step) { if (!omit_force_integration) { //overridden by direct state query - Vector3 force = gravity * mass; - force += applied_force; - Vector3 torque = applied_torque; + Vector3 force = gravity * mass + applied_force + constant_force; + Vector3 torque = applied_torque + constant_torque; - real_t damp = 1.0 - p_step * area_linear_damp; + real_t damp = 1.0 - p_step * total_linear_damp; if (damp < 0) { // reached zero in the given time damp = 0; } - real_t angular_damp = 1.0 - p_step * area_angular_damp; + real_t angular_damp = 1.0 - p_step * total_angular_damp; if (angular_damp < 0) { // reached zero in the given time angular_damp = 0; @@ -580,8 +659,6 @@ void GodotBody3D::integrate_forces(real_t p_step) { applied_force = Vector3(); applied_torque = Vector3(); - //motion=linear_velocity*p_step; - biased_angular_velocity = Vector3(); biased_linear_velocity = Vector3(); @@ -589,7 +666,6 @@ void GodotBody3D::integrate_forces(real_t p_step) { _update_shapes_with_motion(motion); } - def_area = nullptr; // clear the area, so it is set in the next frame contact_count = 0; } @@ -690,7 +766,7 @@ void GodotBody3D::call_queries() { 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); } } diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h index 5acdab9d13..93bd5a0071 100644 --- a/servers/physics_3d/godot_body_3d.h +++ b/servers/physics_3d/godot_body_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -45,6 +45,9 @@ class GodotBody3D : public GodotCollisionObject3D { Vector3 linear_velocity; Vector3 angular_velocity; + Vector3 prev_linear_velocity; + Vector3 prev_angular_velocity; + Vector3 constant_linear_velocity; Vector3 constant_angular_velocity; @@ -55,8 +58,15 @@ class GodotBody3D : public GodotCollisionObject3D { real_t friction = 1.0; Vector3 inertia; - real_t linear_damp = -1.0; - real_t angular_damp = -1.0; + PhysicsServer3D::BodyDampMode linear_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE; + PhysicsServer3D::BodyDampMode angular_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE; + + real_t linear_damp = 0.0; + real_t angular_damp = 0.0; + + real_t total_linear_damp = 0.0; + real_t total_angular_damp = 0.0; + real_t gravity_scale = 1.0; uint16_t locked_axis = 0; @@ -83,8 +93,8 @@ class GodotBody3D : public GodotCollisionObject3D { Vector3 applied_force; Vector3 applied_torque; - real_t area_angular_damp = 0.0; - real_t area_linear_damp = 0.0; + Vector3 constant_force; + Vector3 constant_torque; SelfList<GodotBody3D> active_list; SelfList<GodotBody3D> mass_properties_update_list; @@ -99,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; @@ -135,8 +145,6 @@ class GodotBody3D : public GodotCollisionObject3D { uint64_t island_step = 0; - void _compute_area_gravity_and_damping(const GodotArea3D *p_area); - void _update_transform_dependent(); friend class GodotPhysicsDirectBodyState3D; // i give up, too many functions to expose @@ -161,7 +169,7 @@ public: if (index > -1) { areas.write[index].refCount -= 1; if (areas[index].refCount < 1) { - areas.remove(index); + areas.remove_at(index); } } } @@ -188,7 +196,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; } @@ -196,6 +204,7 @@ public: _FORCE_INLINE_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; } _FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; } + _FORCE_INLINE_ Vector3 get_center_of_mass_local() const { return center_of_mass_local; } _FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } _FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; } @@ -204,6 +213,9 @@ public: _FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; } _FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; } + _FORCE_INLINE_ Vector3 get_prev_linear_velocity() const { return prev_linear_velocity; } + _FORCE_INLINE_ Vector3 get_prev_angular_velocity() const { return prev_angular_velocity; } + _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; } _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; } @@ -235,19 +247,38 @@ public: biased_angular_velocity += _inv_inertia_tensor.xform(p_impulse); } - _FORCE_INLINE_ void add_central_force(const Vector3 &p_force) { + _FORCE_INLINE_ void apply_central_force(const Vector3 &p_force) { applied_force += p_force; } - _FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) { + _FORCE_INLINE_ void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) { applied_force += p_force; applied_torque += (p_position - center_of_mass).cross(p_force); } - _FORCE_INLINE_ void add_torque(const Vector3 &p_torque) { + _FORCE_INLINE_ void apply_torque(const Vector3 &p_torque) { applied_torque += p_torque; } + _FORCE_INLINE_ void add_constant_central_force(const Vector3 &p_force) { + constant_force += p_force; + } + + _FORCE_INLINE_ void add_constant_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) { + constant_force += p_force; + constant_torque += (p_position - center_of_mass).cross(p_force); + } + + _FORCE_INLINE_ void add_constant_torque(const Vector3 &p_torque) { + constant_torque += p_torque; + } + + void set_constant_force(const Vector3 &p_force) { constant_force = p_force; } + Vector3 get_constant_force() const { return constant_force; } + + void set_constant_torque(const Vector3 &p_torque) { constant_torque = p_torque; } + Vector3 get_constant_torque() const { return constant_torque; } + void set_active(bool p_active); _FORCE_INLINE_ bool is_active() const { return active; } @@ -267,16 +298,10 @@ public: void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); Variant get_state(PhysicsServer3D::BodyState p_state) const; - void set_applied_force(const Vector3 &p_force) { applied_force = p_force; } - Vector3 get_applied_force() const { return applied_force; } - - void set_applied_torque(const Vector3 &p_torque) { applied_torque = p_torque; } - Vector3 get_applied_torque() const { return applied_torque; } - _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(); @@ -285,7 +310,6 @@ public: _FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; } _FORCE_INLINE_ const Basis &get_inv_inertia_tensor() const { return _inv_inertia_tensor; } _FORCE_INLINE_ real_t get_friction() const { return friction; } - _FORCE_INLINE_ const Vector3 &get_gravity() const { return gravity; } _FORCE_INLINE_ real_t get_bounce() const { return bounce; } void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock); diff --git a/servers/physics_3d/godot_body_direct_state_3d.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp index db09657f8a..a8c6086e1c 100644 --- a/servers/physics_3d/godot_body_direct_state_3d.cpp +++ b/servers/physics_3d/godot_body_direct_state_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -38,17 +38,21 @@ Vector3 GodotPhysicsDirectBodyState3D::get_total_gravity() const { } real_t GodotPhysicsDirectBodyState3D::get_total_angular_damp() const { - return body->area_angular_damp; + return body->total_angular_damp; } real_t GodotPhysicsDirectBodyState3D::get_total_linear_damp() const { - return body->area_linear_damp; + return body->total_linear_damp; } Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass() const { return body->get_center_of_mass(); } +Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass_local() const { + return body->get_center_of_mass_local(); +} + Basis GodotPhysicsDirectBodyState3D::get_principal_inertia_axes() const { return body->get_principal_inertia_axes(); } @@ -95,34 +99,71 @@ Vector3 GodotPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vect return body->get_velocity_in_local_point(p_position); } -void GodotPhysicsDirectBodyState3D::add_central_force(const Vector3 &p_force) { +void GodotPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) { body->wakeup(); - body->add_central_force(p_force); + body->apply_central_impulse(p_impulse); } -void GodotPhysicsDirectBodyState3D::add_force(const Vector3 &p_force, const Vector3 &p_position) { +void GodotPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { body->wakeup(); - body->add_force(p_force, p_position); + body->apply_impulse(p_impulse, p_position); } -void GodotPhysicsDirectBodyState3D::add_torque(const Vector3 &p_torque) { +void GodotPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) { body->wakeup(); - body->add_torque(p_torque); + body->apply_torque_impulse(p_impulse); } -void GodotPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) { +void GodotPhysicsDirectBodyState3D::apply_central_force(const Vector3 &p_force) { body->wakeup(); - body->apply_central_impulse(p_impulse); + body->apply_central_force(p_force); } -void GodotPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { +void GodotPhysicsDirectBodyState3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) { body->wakeup(); - body->apply_impulse(p_impulse, p_position); + body->apply_force(p_force, p_position); } -void GodotPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) { +void GodotPhysicsDirectBodyState3D::apply_torque(const Vector3 &p_torque) { body->wakeup(); - body->apply_torque_impulse(p_impulse); + body->apply_torque(p_torque); +} + +void GodotPhysicsDirectBodyState3D::add_constant_central_force(const Vector3 &p_force) { + body->wakeup(); + body->add_constant_central_force(p_force); +} + +void GodotPhysicsDirectBodyState3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_position) { + body->wakeup(); + body->add_constant_force(p_force, p_position); +} + +void GodotPhysicsDirectBodyState3D::add_constant_torque(const Vector3 &p_torque) { + body->wakeup(); + body->add_constant_torque(p_torque); +} + +void GodotPhysicsDirectBodyState3D::set_constant_force(const Vector3 &p_force) { + if (!p_force.is_equal_approx(Vector3())) { + body->wakeup(); + } + body->set_constant_force(p_force); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_constant_force() const { + return body->get_constant_force(); +} + +void GodotPhysicsDirectBodyState3D::set_constant_torque(const Vector3 &p_torque) { + if (!p_torque.is_equal_approx(Vector3())) { + body->wakeup(); + } + body->set_constant_torque(p_torque); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_constant_torque() const { + return body->get_constant_torque(); } void GodotPhysicsDirectBodyState3D::set_sleep_state(bool p_sleep) { diff --git a/servers/physics_3d/godot_body_direct_state_3d.h b/servers/physics_3d/godot_body_direct_state_3d.h index 6c584a2634..483cfb9298 100644 --- a/servers/physics_3d/godot_body_direct_state_3d.h +++ b/servers/physics_3d/godot_body_direct_state_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -46,6 +46,7 @@ public: virtual real_t get_total_linear_damp() const override; virtual Vector3 get_center_of_mass() const override; + virtual Vector3 get_center_of_mass_local() const override; virtual Basis get_principal_inertia_axes() const override; virtual real_t get_inverse_mass() const override; @@ -63,13 +64,24 @@ public: virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override; - virtual void add_central_force(const Vector3 &p_force) override; - virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; - virtual void add_torque(const Vector3 &p_torque) override; virtual void apply_central_impulse(const Vector3 &p_impulse) override; virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; virtual void apply_torque_impulse(const Vector3 &p_impulse) override; + virtual void apply_central_force(const Vector3 &p_force) override; + virtual void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void apply_torque(const Vector3 &p_torque) override; + + virtual void add_constant_central_force(const Vector3 &p_force) override; + virtual void add_constant_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void add_constant_torque(const Vector3 &p_torque) override; + + virtual void set_constant_force(const Vector3 &p_force) override; + virtual Vector3 get_constant_force() const override; + + virtual void set_constant_torque(const Vector3 &p_torque) override; + virtual Vector3 get_constant_torque() const override; + virtual void set_sleep_state(bool p_sleep) override; virtual bool is_sleeping() const override; diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index 457abfb71a..eebbe0196d 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -35,32 +35,15 @@ #include "core/os/os.h" -/* -#define NO_ACCUMULATE_IMPULSES -#define NO_SPLIT_IMPULSES - -#define NO_FRICTION -*/ - -#define NO_TANGENTIALS -/* BODY PAIR */ - -//#define ALLOWED_PENETRATION 0.01 -#define RELAXATION_TIMESTEPS 3 #define MIN_VELOCITY 0.0001 #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); } void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { - // check if we already have the contact - - //Vector3 local_A = A->get_inv_transform().xform(p_point_A); - //Vector3 local_B = B->get_inv_transform().xform(p_point_B); - Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A); Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B); @@ -69,19 +52,14 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); Contact contact; - - contact.acc_normal_impulse = 0; - contact.acc_bias_impulse = 0; - contact.acc_bias_impulse_center_of_mass = 0; - contact.acc_tangent_impulse = Vector3(); contact.index_A = p_index_A; contact.index_B = p_index_B; contact.local_A = local_A; contact.local_B = local_B; contact.normal = (p_point_A - p_point_B).normalized(); - contact.mass_normal = 0; // will be computed in setup() + contact.used = true; - // attempt to determine if the contact will be reused + // Attempt to determine if the contact will be reused. real_t contact_recycle_radius = space->get_contact_recycle_radius(); for (int i = 0; i < contact_count; i++) { @@ -92,23 +70,34 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind contact.acc_bias_impulse = c.acc_bias_impulse; contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; contact.acc_tangent_impulse = c.acc_tangent_impulse; - new_index = i; - break; + c = contact; + return; } } - // figure out if the contact amount must be reduced to fit the new contact - + // Figure out if the contact amount must be reduced to fit the new contact. if (new_index == MAX_CONTACTS) { - // remove the contact with the minimum depth + // Remove the contact with the minimum depth. + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; int least_deep = -1; - real_t min_depth = 1e10; + real_t min_depth; + + // Start with depth for new contact. + { + Vector3 global_A = basis_A.xform(contact.local_A); + Vector3 global_B = basis_B.xform(contact.local_B) + offset_B; + + Vector3 axis = global_A - global_B; + min_depth = axis.dot(contact.normal); + } - for (int i = 0; i <= contact_count; i++) { - Contact &c = (i == contact_count) ? contact : contacts[i]; - Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; + for (int i = 0; i < contact_count; i++) { + const Contact &c = contacts[i]; + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; Vector3 axis = global_A - global_B; real_t depth = axis.dot(c.normal); @@ -119,10 +108,8 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind } } - ERR_FAIL_COND(least_deep == -1); - - if (least_deep < contact_count) { //replace the last deep contact by the new one - + if (least_deep > -1) { + // Replace the least deep contact by the new one. contacts[least_deep] = contact; } @@ -130,29 +117,41 @@ void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_ind } contacts[new_index] = contact; - - if (new_index == contact_count) { - contact_count++; - } + contact_count++; } void GodotBodyPair3D::validate_contacts() { - //make sure to erase contacts that are no longer valid + // Make sure to erase contacts that are no longer valid. + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation * max_separation; + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; - real_t contact_max_separation = space->get_contact_max_separation(); for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - Vector3 global_A = A->get_transform().basis.xform(c.local_A); - Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B; - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); + bool erase = false; + if (!c.used) { + // Was left behind in previous frame. + erase = true; + } else { + c.used = false; - if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { - // contact no longer needed, remove + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase = true; + } + } + + if (erase) { + // Contact no longer needed, remove. if ((i + 1) < contact_count) { - // swap with the last one + // Swap with the last one. SWAP(contacts[i], contacts[contact_count - 1]); } @@ -173,29 +172,35 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, real_t min, max; p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max); - bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction - if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis + // Did it move enough in this direction to even attempt raycast? + // Let's say it should move more than 1/3 the size of the object in that axis. + bool fast_object = mlen > (max - min) * 0.3; + if (!fast_object) { return false; } - //cast a segment from support in motion normal, in the same direction of motion by motion length - //support is the worst case collision point, so real collision happened before + // Going too fast in that direction. + + // Cast a segment from support in motion normal, in the same direction of motion by motion length. + // Support is the worst case collision point, so real collision happened before. Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized()); Vector3 from = p_xform_A.xform(s); Vector3 to = from + motion; Transform3D from_inv = p_xform_B.affine_inverse(); - Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box + // Start from a little inside the bounding box. + Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); Vector3 local_to = from_inv.xform(to); Vector3 rpos, rnorm; - if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm, true)) { return false; } - //shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough + // Shorten the linear velocity so it does not hit, but gets close enough, + // next frame will hit softly or soft enough. Vector3 hitpos = p_xform_B.xform(rpos); real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01; @@ -213,6 +218,8 @@ real_t combine_friction(GodotBody3D *A, GodotBody3D *B) { } bool GodotBodyPair3D::setup(real_t p_step) { + check_ccd = false; + if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; @@ -249,14 +256,14 @@ bool GodotBodyPair3D::setup(real_t p_step) { collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); if (!collided) { - //test ccd (currently just a raycast) - if (A->is_continuous_collision_detection_enabled() && collide_A) { - _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); + check_ccd = true; + return true; } if (B->is_continuous_collision_detection_enabled() && collide_B) { - _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); + check_ccd = true; + return true; } return false; @@ -267,12 +274,30 @@ bool GodotBodyPair3D::setup(real_t p_step) { bool GodotBodyPair3D::pre_solve(real_t p_step) { if (!collided) { + if (check_ccd) { + const Vector3 &offset_A = A->get_transform().get_origin(); + Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3()); + Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform3D xform_Bu = B->get_transform(); + xform_Bu.origin -= offset_A; + Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + if (A->is_continuous_collision_detection_enabled() && collide_A) { + _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); + } + + if (B->is_continuous_collision_detection_enabled() && collide_B) { + _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); + } + } + return false; } real_t max_penetration = space->get_contact_max_allowed_penetration(); - real_t bias = (real_t)0.3; + real_t bias = 0.8; GodotShape3D *shape_A_ptr = A->get_shape(shape_A); GodotShape3D *shape_B_ptr = B->get_shape(shape_B); @@ -365,15 +390,12 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) { if (collide_B) { B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); } - c.acc_bias_impulse = 0; - c.acc_bias_impulse_center_of_mass = 0; c.bounce = combine_bounce(A, B); if (c.bounce) { - Vector3 crA = A->get_angular_velocity().cross(c.rA); - Vector3 crB = B->get_angular_velocity().cross(c.rB); - Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; - //normal impule + Vector3 crA = A->get_prev_angular_velocity().cross(c.rA); + Vector3 crB = B->get_prev_angular_velocity().cross(c.rB); + Vector3 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA; c.bounce = c.bounce * dv.dot(c.normal); } } @@ -495,8 +517,7 @@ void GodotBodyPair3D::solve(real_t p_step) { Vector3 temp1 = inv_inertia_tensor_A.xform(c.rA.cross(tv)); Vector3 temp2 = inv_inertia_tensor_B.xform(c.rB.cross(tv)); - real_t t = -tvl / - (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); + real_t t = -tvl / (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); Vector3 jt = t * tv; @@ -541,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); } @@ -552,14 +573,10 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i Contact contact; contact.index_A = p_index_A; contact.index_B = p_index_B; - contact.acc_normal_impulse = 0; - contact.acc_bias_impulse = 0; - contact.acc_bias_impulse_center_of_mass = 0; - contact.acc_tangent_impulse = Vector3(); contact.local_A = local_A; contact.local_B = local_B; contact.normal = (p_point_A - p_point_B).normalized(); - contact.mass_normal = 0; + contact.used = true; // Attempt to determine if the contact will be reused. real_t contact_recycle_radius = space->get_contact_recycle_radius(); @@ -585,20 +602,33 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i void GodotBodySoftBodyPair3D::validate_contacts() { // Make sure to erase contacts that are no longer valid. - const Transform3D &transform_A = body->get_transform(); + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation * max_separation; - real_t contact_max_separation = space->get_contact_max_separation(); + const Transform3D &transform_A = body->get_transform(); uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; - Vector3 global_A = transform_A.xform(c.local_A); - Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; - Vector3 axis = global_A - global_B; - real_t depth = axis.dot(c.normal); + bool erase = false; + if (!c.used) { + // Was left behind in previous frame. + erase = true; + } else { + c.used = false; + + Vector3 global_A = transform_A.xform(c.local_A); + Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase = true; + } + } - if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { + if (erase) { // Contact no longer needed, remove. if ((contact_index + 1) < contact_count) { // Swap with the last one. @@ -654,7 +684,7 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) { real_t max_penetration = space->get_contact_max_allowed_penetration(); - real_t bias = (real_t)0.3; + real_t bias = space->get_contact_bias(); GodotShape3D *shape_A_ptr = body->get_shape(body_shape); @@ -737,8 +767,6 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) { if (soft_body_collides) { soft_body->apply_node_impulse(c.index_B, j_vec); } - c.acc_bias_impulse = 0; - c.acc_bias_impulse_center_of_mass = 0; c.bounce = body->get_bounce(); @@ -863,8 +891,7 @@ void GodotBodySoftBodyPair3D::solve(real_t p_step) { Vector3 temp1 = body_inv_inertia_tensor.xform(c.rA.cross(tv)); - real_t t = -tvl / - (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA))); + real_t t = -tvl / (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA))); Vector3 jt = t * tv; diff --git a/servers/physics_3d/godot_body_pair_3d.h b/servers/physics_3d/godot_body_pair_3d.h index c0a2424e05..7a7309f9d5 100644 --- a/servers/physics_3d/godot_body_pair_3d.h +++ b/servers/physics_3d/godot_body_pair_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -54,11 +54,13 @@ protected: real_t depth = 0.0; bool active = false; + bool used = false; Vector3 rA, rB; // Offset in world orientation with respect to center of mass }; Vector3 sep_axis; bool collided = false; + bool check_ccd = false; GodotSpace3D *space = nullptr; diff --git a/servers/physics_3d/godot_broad_phase_3d.cpp b/servers/physics_3d/godot_broad_phase_3d.cpp index db51dfb2b6..115a01a0c5 100644 --- a/servers/physics_3d/godot_broad_phase_3d.cpp +++ b/servers/physics_3d/godot_broad_phase_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/godot_broad_phase_3d.h b/servers/physics_3d/godot_broad_phase_3d.h index 65423f293c..75196cbd1b 100644 --- a/servers/physics_3d/godot_broad_phase_3d.h +++ b/servers/physics_3d/godot_broad_phase_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp index 0f2061a1ea..435c1e8aec 100644 --- a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp +++ b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,51 +33,61 @@ #include "godot_collision_object_3d.h" GodotBroadPhase3DBVH::ID GodotBroadPhase3DBVH::create(GodotCollisionObject3D *p_object, int p_subindex, const AABB &p_aabb, bool p_static) { - ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? + uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC; + uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC); + ID oid = bvh.create(p_object, true, tree_id, tree_collision_mask, p_aabb, p_subindex); // Pair everything, don't care? return oid + 1; } void GodotBroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) { + ERR_FAIL_COND(!p_id); bvh.move(p_id - 1, p_aabb); } void GodotBroadPhase3DBVH::set_static(ID p_id, bool p_static) { - GodotCollisionObject3D *it = bvh.get(p_id - 1); - bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? + ERR_FAIL_COND(!p_id); + uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC; + uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC); + bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false); } void GodotBroadPhase3DBVH::remove(ID p_id) { + ERR_FAIL_COND(!p_id); bvh.erase(p_id - 1); } GodotCollisionObject3D *GodotBroadPhase3DBVH::get_object(ID p_id) const { + ERR_FAIL_COND_V(!p_id, nullptr); GodotCollisionObject3D *it = bvh.get(p_id - 1); ERR_FAIL_COND_V(!it, nullptr); return it; } bool GodotBroadPhase3DBVH::is_static(ID p_id) const { - return !bvh.is_pairable(p_id - 1); + ERR_FAIL_COND_V(!p_id, false); + uint32_t tree_id = bvh.get_tree_id(p_id - 1); + return tree_id == 0; } int GodotBroadPhase3DBVH::get_subindex(ID p_id) const { + ERR_FAIL_COND_V(!p_id, 0); return bvh.get_subindex(p_id - 1); } int GodotBroadPhase3DBVH::cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices); + return bvh.cull_point(p_point, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); } int GodotBroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); } int GodotBroadPhase3DBVH::cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) { - return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); + return bvh.cull_aabb(p_aabb, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); } 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; } @@ -86,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 61127e52c1..ae5a18d955 100644 --- a/servers/physics_3d/godot_broad_phase_3d_bvh.h +++ b/servers/physics_3d/godot_broad_phase_3d_bvh.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -36,7 +36,34 @@ #include "core/math/bvh.h" class GodotBroadPhase3DBVH : public GodotBroadPhase3D { - BVH_Manager<GodotCollisionObject3D, true, 128> bvh; + template <class T> + class UserPairTestFunction { + public: + static bool user_pair_check(const T *p_a, const T *p_b) { + // return false if no collision, decided by masks etc + return p_a->interacts_with(p_b); + } + }; + + template <class T> + class UserCullTestFunction { + public: + static bool user_cull_check(const T *p_a, const T *p_b) { + return true; + } + }; + + enum Tree { + TREE_STATIC = 0, + TREE_DYNAMIC = 1, + }; + + enum TreeFlag { + TREE_FLAG_STATIC = 1 << TREE_STATIC, + TREE_FLAG_DYNAMIC = 1 << TREE_DYNAMIC, + }; + + BVH_Manager<GodotCollisionObject3D, 2, true, 128, UserPairTestFunction<GodotCollisionObject3D>, UserCullTestFunction<GodotCollisionObject3D>> bvh; static void *_pair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int); static void _unpair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int, void *); @@ -48,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.cpp b/servers/physics_3d/godot_collision_object_3d.cpp index 80a3d18ce0..63284aeca5 100644 --- a/servers/physics_3d/godot_collision_object_3d.cpp +++ b/servers/physics_3d/godot_collision_object_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -118,7 +118,7 @@ void GodotCollisionObject3D::remove_shape(int p_index) { shapes.write[i].bpid = 0; } shapes[p_index].shape->remove_owner(this); - shapes.remove(p_index); + shapes.remove_at(p_index); if (!pending_shape_update_list.in_list()) { GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); @@ -171,7 +171,7 @@ void GodotCollisionObject3D::_update_shapes() { s.aabb_cache = shape_aabb; Vector3 scale = xform.get_basis().get_scale(); - s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z; + s.area_cache = s.shape->get_volume() * scale.x * scale.y * scale.z; if (s.bpid == 0) { s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); diff --git a/servers/physics_3d/godot_collision_object_3d.h b/servers/physics_3d/godot_collision_object_3d.h index 43558034e0..2d342f65f3 100644 --- a/servers/physics_3d/godot_collision_object_3d.h +++ b/servers/physics_3d/godot_collision_object_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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,15 +166,22 @@ 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; } - _FORCE_INLINE_ bool interacts_with(GodotCollisionObject3D *p_other) const { + _FORCE_INLINE_ bool interacts_with(const GodotCollisionObject3D *p_other) const { 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 b9f2f7506b..b2d3e4d876 100644 --- a/servers/physics_3d/godot_collision_solver_3d.cpp +++ b/servers/physics_3d/godot_collision_solver_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -29,6 +29,7 @@ /*************************************************************************/ #include "godot_collision_solver_3d.h" + #include "godot_collision_solver_3d_sat.h" #include "godot_soft_body_3d.h" @@ -93,7 +94,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(); @@ -102,7 +103,7 @@ bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A, to = ai.xform(to); Vector3 p, n; - if (!p_shape_B->intersect_segment(from, to, p, n)) { + if (!p_shape_B->intersect_segment(from, to, p, n, true)) { return false; } @@ -141,7 +142,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 +171,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 +190,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 +252,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]; @@ -264,7 +265,7 @@ bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, cons local_aabb.size[i] = smax - smin; } - concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo); + concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo, true); } else { AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb()); shape_aabb.grow_by(collision_margin); @@ -276,23 +277,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,7 +333,7 @@ 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; @@ -346,7 +348,7 @@ bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const local_aabb.size[i] = smax - smin; } - concave_B->cull(local_aabb, concave_callback, &cinfo); + concave_B->cull(local_aabb, concave_callback, &cinfo, false); return cinfo.collided; } @@ -422,7 +424,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; @@ -540,7 +542,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; @@ -559,7 +561,7 @@ bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const local_aabb.size[i] = smax - smin; } - concave_B->cull(local_aabb, concave_distance_callback, &cinfo); + concave_B->cull(local_aabb, concave_distance_callback, &cinfo, false); if (!cinfo.collided) { r_point_A = cinfo.close_A; r_point_B = cinfo.close_B; diff --git a/servers/physics_3d/godot_collision_solver_3d.h b/servers/physics_3d/godot_collision_solver_3d.h index 133635ca7e..a6a0ebfead 100644 --- a/servers/physics_3d/godot_collision_solver_3d.h +++ b/servers/physics_3d/godot_collision_solver_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp index 0790333f65..20e9300778 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp +++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -36,6 +36,8 @@ #define fallback_collision_solver gjk_epa_calculate_penetration +#define _BACKFACE_NORMAL_THRESHOLD -0.0002 + // Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders. /* @@ -66,7 +68,7 @@ *************************************************************************/ struct _CollectorCallback { - GodotCollisionSolver3D::CallbackResult callback; + GodotCollisionSolver3D::CallbackResult callback = nullptr; void *userdata = nullptr; bool swap = false; bool collided = false; @@ -343,10 +345,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ for (int i = 0; i < clipbuf_len; i++) { real_t d = plane_B.distance_to(clipbuf_src[i]); - /* - if (d>CMP_EPSILON) - continue; - */ Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d; @@ -612,13 +610,14 @@ class SeparatorAxisTest { const Transform3D *transform_A = nullptr; const Transform3D *transform_B = nullptr; real_t best_depth = 1e15; - Vector3 best_axis; _CollectorCallback *callback = nullptr; real_t margin_A = 0.0; real_t margin_B = 0.0; Vector3 separator_axis; public: + Vector3 best_axis; + _FORCE_INLINE_ bool test_previous_axis() { if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) { return test_axis(*callback->prev_axis); @@ -627,7 +626,7 @@ public: } } - _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis, bool p_directional = false) { + _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { Vector3 axis = p_axis; if (axis.is_equal_approx(Vector3())) { @@ -661,12 +660,7 @@ public: //use the smallest depth if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0 - if (p_directional) { - min_B = max_B; - axis = -axis; - } else { - min_B = -min_B; - } + min_B = -min_B; } if (max_B < min_B) { @@ -798,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; @@ -825,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; @@ -848,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; @@ -889,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; } @@ -903,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; @@ -1014,7 +1008,7 @@ static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { return; } @@ -1041,6 +1035,17 @@ static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } @@ -1058,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; @@ -1068,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; @@ -1078,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; @@ -1124,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; @@ -1155,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; @@ -1191,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(); @@ -1207,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); @@ -1229,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; @@ -1253,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. { @@ -1271,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; @@ -1295,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]; } } } @@ -1375,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; @@ -1396,7 +1401,7 @@ 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]); @@ -1433,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; @@ -1451,7 +1456,7 @@ 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++) { @@ -1486,13 +1491,13 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { return; } // 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; } @@ -1508,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; } @@ -1545,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) { @@ -1568,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++) { @@ -1591,6 +1596,17 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } @@ -1607,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; @@ -1670,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()); @@ -1756,7 +1772,7 @@ 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 axis = edge_axis.cross(p_transform_a.basis.get_column(1)).normalized(); if (!separator.test_axis(axis)) { return; @@ -1768,7 +1784,7 @@ 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); @@ -1802,13 +1818,13 @@ static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D & Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { return; } // 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 @@ -1858,6 +1874,17 @@ static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D & } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } @@ -1868,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; @@ -1952,11 +1979,11 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); // Face B normal. - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { 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; } @@ -2034,6 +2061,17 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } @@ -2174,7 +2212,7 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { return; } @@ -2266,6 +2304,17 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.h b/servers/physics_3d/godot_collision_solver_3d_sat.h index 069a701cba..46c5ec3254 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.h +++ b/servers/physics_3d/godot_collision_solver_3d_sat.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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_constraint_3d.h b/servers/physics_3d/godot_constraint_3d.h index 840c81716c..217955047f 100644 --- a/servers/physics_3d/godot_constraint_3d.h +++ b/servers/physics_3d/godot_constraint_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/godot_joint_3d.h b/servers/physics_3d/godot_joint_3d.h index 4086bb53e1..d4b719fc5e 100644 --- a/servers/physics_3d/godot_joint_3d.h +++ b/servers/physics_3d/godot_joint_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp index 79a2e0b0ea..9c1535f561 100644 --- a/servers/physics_3d/godot_physics_server_3d.cpp +++ b/servers/physics_3d/godot_physics_server_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -219,7 +219,7 @@ RID GodotPhysicsServer3D::area_create() { RID rid = area_owner.make_rid(area); area->set_self(rid); return rid; -}; +} void GodotPhysicsServer3D::area_set_space(RID p_area, RID p_space) { GodotArea3D *area = area_owner.get_or_null(p_area); @@ -237,7 +237,7 @@ void GodotPhysicsServer3D::area_set_space(RID p_area, RID p_space) { area->clear_constraints(); area->set_space(space); -}; +} RID GodotPhysicsServer3D::area_get_space(RID p_area) const { GodotArea3D *area = area_owner.get_or_null(p_area); @@ -248,20 +248,6 @@ RID GodotPhysicsServer3D::area_get_space(RID p_area) const { return RID(); } return space->get_self(); -}; - -void GodotPhysicsServer3D::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { - GodotArea3D *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND(!area); - - area->set_space_override_mode(p_mode); -} - -PhysicsServer3D::AreaSpaceOverrideMode GodotPhysicsServer3D::area_get_space_override_mode(RID p_area) const { - const GodotArea3D *area = area_owner.get_or_null(p_area); - ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED); - - return area->get_space_override_mode(); } void GodotPhysicsServer3D::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) { @@ -416,11 +402,11 @@ void GodotPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) area->set_monitorable(p_monitorable); } -void GodotPhysicsServer3D::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { +void GodotPhysicsServer3D::area_set_monitor_callback(RID p_area, const Callable &p_callback) { GodotArea3D *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); - area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); + area->set_monitor_callback(p_callback.is_valid() ? p_callback : Callable()); } void GodotPhysicsServer3D::area_set_ray_pickable(RID p_area, bool p_enable) { @@ -430,11 +416,11 @@ void GodotPhysicsServer3D::area_set_ray_pickable(RID p_area, bool p_enable) { area->set_ray_pickable(p_enable); } -void GodotPhysicsServer3D::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { +void GodotPhysicsServer3D::area_set_area_monitor_callback(RID p_area, const Callable &p_callback) { GodotArea3D *area = area_owner.get_or_null(p_area); ERR_FAIL_COND(!area); - area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method); + area->set_area_monitor_callback(p_callback.is_valid() ? p_callback : Callable()); } /* BODY API */ @@ -607,6 +593,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) { @@ -621,40 +621,40 @@ void GodotPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p } ERR_FAIL_MSG("Invalid ID."); -}; +} ObjectID GodotPhysicsServer3D::body_get_object_instance_id(RID p_body) const { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, ObjectID()); return body->get_instance_id(); -}; +} void GodotPhysicsServer3D::body_set_user_flags(RID p_body, uint32_t p_flags) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); -}; +} uint32_t GodotPhysicsServer3D::body_get_user_flags(RID p_body) const { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return 0; -}; +} void GodotPhysicsServer3D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); body->set_param(p_param, p_value); -}; +} Variant GodotPhysicsServer3D::body_get_param(RID p_body, BodyParameter p_param) const { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, 0); return body->get_param(p_param); -}; +} void GodotPhysicsServer3D::body_reset_mass_properties(RID p_body) { GodotBody3D *body = body_owner.get_or_null(p_body); @@ -668,97 +668,125 @@ void GodotPhysicsServer3D::body_set_state(RID p_body, BodyState p_state, const V ERR_FAIL_COND(!body); body->set_state(p_state, p_variant); -}; +} Variant GodotPhysicsServer3D::body_get_state(RID p_body, BodyState p_state) const { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND_V(!body, Variant()); return body->get_state(p_state); -}; +} -void GodotPhysicsServer3D::body_set_applied_force(RID p_body, const Vector3 &p_force) { +void GodotPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->set_applied_force(p_force); + _update_shapes(); + + body->apply_central_impulse(p_impulse); body->wakeup(); -}; +} -Vector3 GodotPhysicsServer3D::body_get_applied_force(RID p_body) const { +void GodotPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) { GodotBody3D *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Vector3()); - return body->get_applied_force(); -}; + ERR_FAIL_COND(!body); + + _update_shapes(); + + body->apply_impulse(p_impulse, p_position); + body->wakeup(); +} -void GodotPhysicsServer3D::body_set_applied_torque(RID p_body, const Vector3 &p_torque) { +void GodotPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->set_applied_torque(p_torque); + _update_shapes(); + + body->apply_torque_impulse(p_impulse); body->wakeup(); -}; +} -Vector3 GodotPhysicsServer3D::body_get_applied_torque(RID p_body) const { +void GodotPhysicsServer3D::body_apply_central_force(RID p_body, const Vector3 &p_force) { GodotBody3D *body = body_owner.get_or_null(p_body); - ERR_FAIL_COND_V(!body, Vector3()); + ERR_FAIL_COND(!body); - return body->get_applied_torque(); -}; + body->apply_central_force(p_force); + body->wakeup(); +} -void GodotPhysicsServer3D::body_add_central_force(RID p_body, const Vector3 &p_force) { +void GodotPhysicsServer3D::body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->add_central_force(p_force); + body->apply_force(p_force, p_position); body->wakeup(); } -void GodotPhysicsServer3D::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { +void GodotPhysicsServer3D::body_apply_torque(RID p_body, const Vector3 &p_torque) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->add_force(p_force, p_position); + body->apply_torque(p_torque); body->wakeup(); -}; +} -void GodotPhysicsServer3D::body_add_torque(RID p_body, const Vector3 &p_torque) { +void GodotPhysicsServer3D::body_add_constant_central_force(RID p_body, const Vector3 &p_force) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - body->add_torque(p_torque); + body->add_constant_central_force(p_force); body->wakeup(); -}; +} -void GodotPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { +void GodotPhysicsServer3D::body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - _update_shapes(); + body->add_constant_force(p_force, p_position); + body->wakeup(); +} - body->apply_central_impulse(p_impulse); +void GodotPhysicsServer3D::body_add_constant_torque(RID p_body, const Vector3 &p_torque) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND(!body); + + body->add_constant_torque(p_torque); body->wakeup(); } -void GodotPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) { +void GodotPhysicsServer3D::body_set_constant_force(RID p_body, const Vector3 &p_force) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - _update_shapes(); + body->set_constant_force(p_force); + if (!p_force.is_equal_approx(Vector3())) { + body->wakeup(); + } +} - body->apply_impulse(p_impulse, p_position); - body->wakeup(); -}; +Vector3 GodotPhysicsServer3D::body_get_constant_force(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Vector3()); + return body->get_constant_force(); +} -void GodotPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { +void GodotPhysicsServer3D::body_set_constant_torque(RID p_body, const Vector3 &p_torque) { GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_COND(!body); - _update_shapes(); + body->set_constant_torque(p_torque); + if (!p_torque.is_equal_approx(Vector3())) { + body->wakeup(); + } +} - body->apply_torque_impulse(p_impulse); - body->wakeup(); -}; +Vector3 GodotPhysicsServer3D::body_get_constant_torque(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_COND_V(!body, Vector3()); + + return body->get_constant_torque(); +} void GodotPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { GodotBody3D *body = body_owner.get_or_null(p_body); @@ -772,7 +800,7 @@ void GodotPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_a v += p_axis_velocity; body->set_linear_velocity(v); body->wakeup(); -}; +} void GodotPhysicsServer3D::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { GodotBody3D *body = body_owner.get_or_null(p_body); @@ -881,10 +909,17 @@ bool GodotPhysicsServer3D::body_test_motion(RID p_body, const MotionParameters & PhysicsDirectBodyState3D *GodotPhysicsServer3D::body_get_direct_state(RID p_body) { ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + if (!body_owner.owns(p_body)) { + return nullptr; + } + GodotBody3D *body = body_owner.get_or_null(p_body); ERR_FAIL_NULL_V(body, nullptr); - ERR_FAIL_NULL_V(body->get_space(), nullptr); + if (!body->get_space()) { + return nullptr; + } + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); return body->get_direct_state(); @@ -899,7 +934,7 @@ RID GodotPhysicsServer3D::soft_body_create() { return rid; } -void GodotPhysicsServer3D::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) { +void GodotPhysicsServer3D::soft_body_update_rendering_server(RID p_body, PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) { GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); ERR_FAIL_COND(!soft_body); @@ -1334,7 +1369,7 @@ int GodotPhysicsServer3D::joint_get_solver_priority(RID p_joint) const { return joint->get_priority(); } -void GodotPhysicsServer3D::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { +void GodotPhysicsServer3D::joint_disable_collisions_between_bodies(RID p_joint, bool p_disable) { GodotJoint3D *joint = joint_owner.get_or_null(p_joint); ERR_FAIL_COND(!joint); @@ -1510,7 +1545,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); } @@ -1519,14 +1554,6 @@ void GodotPhysicsServer3D::free(RID p_rid) { } else if (body_owner.owns(p_rid)) { GodotBody3D *body = body_owner.get_or_null(p_rid); - /* - if (body->get_state_query()) - _clear_query(body->get_state_query()); - - if (body->get_direct_state_query()) - _clear_query(body->get_direct_state_query()); - */ - body->set_space(nullptr); while (body->get_shape_count()) { @@ -1545,11 +1572,6 @@ void GodotPhysicsServer3D::free(RID p_rid) { } else if (area_owner.owns(p_rid)) { GodotArea3D *area = area_owner.get_or_null(p_rid); - /* - if (area->get_monitor_query()) - _clear_query(area->get_monitor_query()); - */ - area->set_space(nullptr); while (area->get_shape_count()) { @@ -1562,7 +1584,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); } @@ -1581,20 +1603,15 @@ void GodotPhysicsServer3D::free(RID p_rid) { } else { ERR_FAIL_MSG("Invalid ID."); } -}; +} void GodotPhysicsServer3D::set_active(bool p_active) { active = p_active; -}; - -void GodotPhysicsServer3D::set_collision_iterations(int p_iterations) { - iterations = p_iterations; -}; +} void GodotPhysicsServer3D::init() { - iterations = 8; // 8? stepper = memnew(GodotStep3D); -}; +} void GodotPhysicsServer3D::step(real_t p_step) { #ifndef _3D_DISABLED @@ -1608,18 +1625,18 @@ 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, iterations); - 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 } void GodotPhysicsServer3D::sync() { doing_sync = true; -}; +} void GodotPhysicsServer3D::flush_queries() { #ifndef _3D_DISABLED @@ -1632,8 +1649,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(); } @@ -1653,9 +1670,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)); } } @@ -1668,19 +1685,19 @@ 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 -}; +} void GodotPhysicsServer3D::end_sync() { doing_sync = false; -}; +} void GodotPhysicsServer3D::finish() { memdelete(stepper); -}; +} int GodotPhysicsServer3D::get_process_info(ProcessInfo p_info) { switch (p_info) { @@ -1706,7 +1723,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 3ed9e320dc..b429f23a0c 100644 --- a/servers/physics_3d/godot_physics_server_3d.h +++ b/servers/physics_3d/godot_physics_server_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -44,7 +44,6 @@ class GodotPhysicsServer3D : public PhysicsServer3D { friend class GodotPhysicsDirectSpaceState3D; bool active = true; - int iterations = 0; int island_count = 0; int active_objects = 0; @@ -55,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; @@ -75,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); @@ -122,9 +121,6 @@ public: virtual RID area_create() override; - virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) override; - virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const override; - virtual void area_set_space(RID p_area, RID p_space) override; virtual RID area_get_space(RID p_area) const override; @@ -157,8 +153,8 @@ public: virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; - virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; - virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override; + virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override; + virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override; /* BODY API */ @@ -196,6 +192,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; @@ -207,19 +206,24 @@ public: virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; virtual Variant body_get_state(RID p_body, BodyState p_state) const override; - virtual void body_set_applied_force(RID p_body, const Vector3 &p_force) override; - virtual Vector3 body_get_applied_force(RID p_body) const override; - - virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque) override; - virtual Vector3 body_get_applied_torque(RID p_body) const override; - - virtual void body_add_central_force(RID p_body, const Vector3 &p_force) override; - virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; - virtual void body_add_torque(RID p_body, const Vector3 &p_torque) override; - virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override; virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override; + + virtual void body_apply_central_force(RID p_body, const Vector3 &p_force) override; + virtual void body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void body_apply_torque(RID p_body, const Vector3 &p_torque) override; + + virtual void body_add_constant_central_force(RID p_body, const Vector3 &p_force) override; + virtual void body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void body_add_constant_torque(RID p_body, const Vector3 &p_torque) override; + + virtual void body_set_constant_force(RID p_body, const Vector3 &p_force) override; + virtual Vector3 body_get_constant_force(RID p_body) const override; + + virtual void body_set_constant_torque(RID p_body, const Vector3 &p_torque) override; + virtual Vector3 body_get_constant_torque(RID p_body) const override; + virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override; virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) override; @@ -252,7 +256,7 @@ public: virtual RID soft_body_create() override; - virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) override; + virtual void soft_body_update_rendering_server(RID p_body, PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) override; virtual void soft_body_set_space(RID p_body, RID p_space) override; virtual RID soft_body_get_space(RID p_body) const override; @@ -352,7 +356,7 @@ public: virtual void joint_set_solver_priority(RID p_joint, int p_priority) override; virtual int joint_get_solver_priority(RID p_joint) const override; - virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) override; + virtual void joint_disable_collisions_between_bodies(RID p_joint, bool p_disable) override; virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; /* MISC */ @@ -367,8 +371,6 @@ public: virtual void end_sync() override; virtual void finish() override; - virtual void set_collision_iterations(int p_iterations) override; - virtual bool is_flushing_queries() const override { return flushing_queries; } int get_process_info(ProcessInfo p_info) override; diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp index 4c12a5a948..5e310670a5 100644 --- a/servers/physics_3d/godot_shape_3d.cpp +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -52,17 +52,17 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 -#define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 +constexpr double edge_support_threshold = 0.0002; +constexpr double face_support_threshold = 0.9998; -#define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002 -#define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999 +constexpr double cylinder_edge_support_threshold = 0.002; +constexpr double cylinder_face_support_threshold = 0.999; 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; } @@ -119,7 +119,7 @@ Vector3 GodotWorldBoundaryShape3D::get_support(const Vector3 &p_normal) const { return p_normal * 1e15; } -bool GodotWorldBoundaryShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool GodotWorldBoundaryShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { bool inters = plane.intersects_segment(p_begin, p_end, &r_result); if (inters) { r_normal = plane.normal; @@ -184,7 +184,7 @@ Vector3 GodotSeparationRayShape3D::get_support(const Vector3 &p_normal) const { } void GodotSeparationRayShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + if (Math::abs(p_normal.z) < edge_support_threshold) { r_amount = 2; r_type = FEATURE_EDGE; r_supports[0] = Vector3(0, 0, 0); @@ -200,7 +200,7 @@ void GodotSeparationRayShape3D::get_supports(const Vector3 &p_normal, int p_max, } } -bool GodotSeparationRayShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool GodotSeparationRayShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { return false; //simply not possible } @@ -268,7 +268,7 @@ void GodotSphereShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector r_type = FEATURE_POINT; } -bool GodotSphereShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool GodotSphereShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); } @@ -335,7 +335,7 @@ void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 * Vector3 axis; axis[i] = 1.0; real_t dot = p_normal.dot(axis); - if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + if (Math::abs(dot) > face_support_threshold) { //Vector3 axis_b; bool neg = dot < 0; @@ -376,7 +376,7 @@ void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 * Vector3 axis; axis[i] = 1.0; - if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + if (Math::abs(p_normal.dot(axis)) < edge_support_threshold) { r_amount = 2; r_type = FEATURE_EDGE; @@ -410,7 +410,7 @@ void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 * r_supports[0] = point; } -bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +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); return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); @@ -430,7 +430,7 @@ Vector3 GodotBoxShape3D::get_closest_point_to(const Vector3 &p_point) const { if (outside == 1) { //use plane if only one side matches Vector3 n; - n[i] = SGN(p_point[i]); + n[i] = SIGN(p_point[i]); Plane p(n, half_extents[i]); min_point = p.project(p_point); @@ -522,7 +522,7 @@ void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vecto real_t d = n.y; - if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + if (Math::abs(d) < edge_support_threshold) { // make it flat n.y = 0.0; n.normalize(); @@ -546,7 +546,7 @@ void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vecto } } -bool GodotCapsuleShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool GodotCapsuleShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { Vector3 norm = (p_end - p_begin).normalized(); real_t min_d = 1e20; @@ -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); @@ -703,7 +703,7 @@ Vector3 GodotCylinderShape3D::get_support(const Vector3 &p_normal) const { void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { real_t d = p_normal.y; - if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) { + if (Math::abs(d) > cylinder_face_support_threshold) { real_t h = (d > 0) ? height : -height; Vector3 n = p_normal; @@ -718,7 +718,7 @@ void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vect r_supports[1].x += radius; r_supports[2] = n; r_supports[2].z += radius; - } else if (Math::abs(d) < _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD) { + } else if (Math::abs(d) < cylinder_edge_support_threshold) { // make it flat Vector3 n = p_normal; n.y = 0.0; @@ -761,7 +761,7 @@ void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vect } } -bool GodotCylinderShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool GodotCylinderShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1); } @@ -911,7 +911,7 @@ void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, } for (int i = 0; i < fc; i++) { - if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + if (faces[i].plane.normal.dot(p_normal) > face_support_threshold) { int ic = faces[i].indices.size(); const int *ind = faces[i].indices.ptr(); @@ -940,7 +940,7 @@ 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); dot = ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { + if (dot < edge_support_threshold && (edges[i].a == vtx || edges[i].b == vtx)) { r_amount = 2; r_type = FEATURE_EDGE; r_supports[0] = vertices[edges[i].a]; @@ -954,7 +954,7 @@ void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, r_type = FEATURE_POINT; } -bool GodotConvexPolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool GodotConvexPolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); @@ -1140,7 +1140,7 @@ void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 Vector3 n = p_normal; /** TEST FACE AS SUPPORT **/ - if (Math::abs(normal.dot(n)) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { + if (Math::abs(normal.dot(n)) > face_support_threshold) { r_amount = 3; r_type = FEATURE_FACE; for (int i = 0; i < 3; i++) { @@ -1174,7 +1174,7 @@ void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 // check if edge is valid as a support real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); dot = ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { + if (dot < edge_support_threshold) { r_amount = 2; r_type = FEATURE_EDGE; r_supports[0] = vertex[i]; @@ -1188,12 +1188,12 @@ void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 r_supports[0] = vertex[vert_support_idx]; } -bool GodotFaceShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool GodotFaceShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); if (c) { r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; if (r_normal.dot(p_end - p_begin) > 0) { - if (backface_collision) { + if (backface_collision && p_hit_back_faces) { r_normal = -r_normal; } else { c = false; @@ -1284,12 +1284,6 @@ 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]; - /* - if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d) - return; //test against whole AABB, which isn't very costly - */ - - //printf("addr: %p\n",bvh); if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) { return; } @@ -1304,7 +1298,7 @@ void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_ Vector3 res; Vector3 normal; - if (face->intersect_segment(p_params->from, p_params->to, res, normal)) { + if (face->intersect_segment(p_params->from, p_params->to, res, normal, true)) { real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); if ((d > 0) && (d < p_params->min_d)) { p_params->min_d = d; @@ -1323,7 +1317,7 @@ void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_ } } -bool GodotConcavePolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool GodotConcavePolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { if (faces.size() == 0) { return false; } @@ -1334,7 +1328,7 @@ bool GodotConcavePolygonShape3D::intersect_segment(const Vector3 &p_begin, const const BVH *br = bvh.ptr(); GodotFaceShape3D face; - face.backface_collision = backface_collision; + face.backface_collision = backface_collision && p_hit_back_faces; _SegmentCullParams params; params.from = p_begin; @@ -1401,7 +1395,7 @@ bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const { return false; } -void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { +void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const { // make matrix local to concave if (faces.size() == 0) { return; @@ -1416,6 +1410,7 @@ void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_ GodotFaceShape3D face; // use this to send in the callback face.backface_collision = backface_collision; + face.invert_backface_collision = p_invert_backface_collision; _CullParams params; params.aabb = local_aabb; @@ -1443,7 +1438,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 { @@ -1466,10 +1461,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) { @@ -1675,7 +1670,7 @@ struct _HeightmapGridCullState { _FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) { Vector3 res; Vector3 normal; - if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal)) { + if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal, true)) { p_params.result = res; p_params.normal = normal; return true; @@ -1881,7 +1876,7 @@ bool GodotHeightMapShape3D::_intersect_grid_segment(ProcessFunction &p_process, return false; } -bool GodotHeightMapShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const { +bool GodotHeightMapShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, bool p_hit_back_faces) const { if (heights.is_empty()) { return false; } @@ -1899,7 +1894,7 @@ bool GodotHeightMapShape3D::intersect_segment(const Vector3 &p_begin, const Vect // Simple case for rays that don't traverse the grid horizontally. // Just perform a test on the given cell. GodotFaceShape3D face; - face.backface_collision = false; + face.backface_collision = p_hit_back_faces; _HeightmapSegmentCullParams params; params.from = p_begin; @@ -1961,7 +1956,7 @@ void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5); } -void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { +void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const { if (heights.is_empty()) { return; } @@ -1988,7 +1983,8 @@ void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callb int end_z = MIN(depth - 1, aabb_max[2]); GodotFaceShape3D face; - face.backface_collision = true; + face.backface_collision = !p_invert_backface_collision; + face.invert_backface_collision = p_invert_backface_collision; for (int z = start_z; z < end_z; z++) { for (int x = start_x; x < end_x; x++) { diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h index 8822d9487b..1fc8f7c711 100644 --- a/servers/physics_3d/godot_shape_3d.h +++ b/servers/physics_3d/godot_shape_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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); @@ -64,7 +64,7 @@ public: FEATURE_CIRCLE, }; - virtual real_t get_area() const { return aabb.get_area(); } + virtual real_t get_volume() const { return aabb.get_volume(); } _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } _FORCE_INLINE_ RID get_self() const { return self; } @@ -80,7 +80,7 @@ public: virtual Vector3 get_support(const Vector3 &p_normal) const; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, bool p_hit_back_faces) const = 0; virtual bool intersect_point(const Vector3 &p_point) const = 0; virtual Vector3 get_moment_of_inertia(real_t p_mass) const = 0; @@ -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(); @@ -107,7 +107,7 @@ public: // Returns true to stop the query. typedef bool (*QueryCallback)(void *p_userdata, GodotShape3D *p_convex); - virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const = 0; GodotConcaveShape3D() {} }; @@ -120,13 +120,13 @@ class GodotWorldBoundaryShape3D : public GodotShape3D { public: Plane get_plane() const; - virtual real_t get_area() const override { return INFINITY; } + virtual real_t get_volume() const override { return INFINITY; } virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_WORLD_BOUNDARY; } virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; @@ -147,13 +147,13 @@ public: real_t get_length() const; bool get_slide_on_slope() const; - virtual real_t get_area() const override { return 0.0; } + virtual real_t get_volume() const override { return 0.0; } virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SEPARATION_RAY; } virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; @@ -173,14 +173,14 @@ class GodotSphereShape3D : public GodotShape3D { public: real_t get_radius() const; - virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } + virtual real_t get_volume() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SPHERE; } virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; @@ -198,14 +198,14 @@ class GodotBoxShape3D : public GodotShape3D { public: _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } - virtual real_t get_area() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; } + virtual real_t get_volume() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; } virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_BOX; } virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; @@ -227,14 +227,14 @@ public: _FORCE_INLINE_ real_t get_height() const { return height; } _FORCE_INLINE_ real_t get_radius() const { return radius; } - virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; } + virtual real_t get_volume() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; } virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CAPSULE; } virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; @@ -256,14 +256,14 @@ public: _FORCE_INLINE_ real_t get_height() const { return height; } _FORCE_INLINE_ real_t get_radius() const { return radius; } - virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + virtual real_t get_volume() const override { return height * Math_PI * radius * radius; } virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CYLINDER; } virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; @@ -288,7 +288,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; @@ -366,11 +366,11 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override; virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; @@ -429,11 +429,11 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override; virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; @@ -448,6 +448,7 @@ struct GodotFaceShape3D : public GodotShape3D { Vector3 normal; //cache Vector3 vertex[3]; bool backface_collision = false; + bool invert_backface_collision = false; virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } @@ -456,7 +457,7 @@ struct GodotFaceShape3D : public GodotShape3D { virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; virtual Vector3 get_support(const Vector3 &p_normal) const override; virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; @@ -495,7 +496,7 @@ struct GodotMotionShape3D : public GodotShape3D { } virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override { return false; } + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override { return false; } virtual bool intersect_point(const Vector3 &p_point) const override { return false; } virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override { return p_point; } diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp index f214e3603a..173843072a 100644 --- a/servers/physics_3d/godot_soft_body_3d.cpp +++ b/servers/physics_3d/godot_soft_body_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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. @@ -147,7 +147,7 @@ void GodotSoftBody3D::set_mesh(RID p_mesh) { } } -void GodotSoftBody3D::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) { +void GodotSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) { if (soft_mesh.is_null()) { return; } @@ -245,7 +245,7 @@ void GodotSoftBody3D::update_area() { const Vector3 a = x1 - x0; const Vector3 b = x2 - x0; const Vector3 cr = vec3_cross(a, b); - face.ra = cr.length(); + face.ra = cr.length() * 0.5; } // Node area. @@ -374,7 +374,7 @@ void GodotSoftBody3D::unpin_vertex(int p_index) { uint32_t pinned_count = pinned_vertices.size(); for (uint32_t i = 0; i < pinned_count; ++i) { if (p_index == pinned_vertices[i]) { - pinned_vertices.remove(i); + pinned_vertices.remove_at(i); if (!soft_mesh.is_null()) { ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); @@ -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++; @@ -710,9 +710,11 @@ void GodotSoftBody3D::generate_bending_constraints(int p_distance) { // A small structure to track lists of dependent link calculations. class LinkDeps { public: - int value; // A link calculation that is dependent on this one - // Positive values = "input A" while negative values = "input B" - LinkDeps *next; // Next dependence in the list + // A link calculation that is dependent on this one. + // Positive values = "input A" while negative values = "input B". + int value; + // Next dependence in the list. + LinkDeps *next; }; typedef LinkDeps *LinkDepsPtr; @@ -915,9 +917,7 @@ void GodotSoftBody3D::add_velocity(const Vector3 &p_velocity) { } } -void GodotSoftBody3D::apply_forces(bool p_has_wind_forces) { - int ac = areas.size(); - +void GodotSoftBody3D::apply_forces(const LocalVector<GodotArea3D *> &p_wind_areas) { if (nodes.is_empty()) { return; } @@ -930,7 +930,6 @@ void GodotSoftBody3D::apply_forces(bool p_has_wind_forces) { // Iterate over faces (try not to iterate elsewhere if possible). for (i = 0, ni = faces.size(); i < ni; ++i) { - bool stopped = false; const Face &face = faces[i]; Vector3 wind_force(0, 0, 0); @@ -939,24 +938,10 @@ void GodotSoftBody3D::apply_forces(bool p_has_wind_forces) { volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org)); // Compute nodal forces from area winds. - if (ac && p_has_wind_forces) { - const AreaCMP *aa = &areas[0]; - for (j = ac - 1; j >= 0 && !stopped; j--) { - PhysicsServer3D::AreaSpaceOverrideMode mode = aa[j].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - wind_force += _compute_area_windforce(aa[j].area, &face); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - wind_force = _compute_area_windforce(aa[j].area, &face); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { - } - } + int wind_area_count = p_wind_areas.size(); + if (wind_area_count > 0) { + for (j = 0; j < wind_area_count; j++) { + wind_force += _compute_area_windforce(p_wind_areas[j], &face); } for (j = 0; j < 3; j++) { @@ -979,12 +964,6 @@ void GodotSoftBody3D::apply_forces(bool p_has_wind_forces) { } } -void GodotSoftBody3D::_compute_area_gravity(const GodotArea3D *p_area) { - Vector3 area_gravity; - p_area->compute_gravity(get_transform().get_origin(), area_gravity); - gravity += area_gravity; -} - Vector3 GodotSoftBody3D::_compute_area_windforce(const GodotArea3D *p_area, const Face *p_face) { real_t wfm = p_area->get_wind_force_magnitude(); real_t waf = p_area->get_wind_attenuation_factor(); @@ -1002,44 +981,59 @@ void GodotSoftBody3D::predict_motion(real_t p_delta) { ERR_FAIL_COND(!get_space()); - GodotArea3D *def_area = get_space()->get_default_area(); - ERR_FAIL_COND(!def_area); - gravity = def_area->get_gravity_vector() * def_area->get_gravity(); + bool gravity_done = false; + Vector3 gravity; - int ac = areas.size(); - bool stopped = false; - bool has_wind_forces = false; + LocalVector<GodotArea3D *> wind_areas; + int ac = areas.size(); if (ac) { areas.sort(); const AreaCMP *aa = &areas[0]; - for (int i = ac - 1; i >= 0 && !stopped; i--) { - // Avoids unnecessary loop in apply_forces(). - has_wind_forces = has_wind_forces || aa[i].area->get_wind_force_magnitude() > CMP_EPSILON; - - PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode(); - switch (mode) { - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { - _compute_area_gravity(aa[i].area); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; - } break; - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: - case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { - gravity = Vector3(0, 0, 0); - _compute_area_gravity(aa[i].area); - stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; - } break; - default: { + for (int i = ac - 1; i >= 0; i--) { + if (!gravity_done) { + PhysicsServer3D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE); + if (area_gravity_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + Vector3 area_gravity; + aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity); + switch (area_gravity_mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + gravity += area_gravity; + gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = Vector3(0, 0, 0); + gravity = area_gravity; + gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } } } + + if (aa[i].area->get_wind_force_magnitude() > CMP_EPSILON) { + wind_areas.push_back(aa[i].area); + } } } + // Add default gravity and damping from space area. + if (!gravity_done) { + GodotArea3D *default_area = get_space()->get_default_area(); + ERR_FAIL_COND(!default_area); + + Vector3 default_gravity; + default_area->compute_gravity(get_transform().get_origin(), default_gravity); + gravity += default_gravity; + } + // Apply forces. add_velocity(gravity * p_delta); - if (pressure_coefficient > CMP_EPSILON || has_wind_forces) { - apply_forces(has_wind_forces); + if (pressure_coefficient > CMP_EPSILON || !wind_areas.is_empty()) { + apply_forces(wind_areas); } // Avoid soft body from 'exploding' so use some upper threshold of maximum motion @@ -1278,7 +1272,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]); @@ -1298,7 +1292,7 @@ struct _SoftBodyIntersectSegmentInfo { } }; -bool GodotSoftBodyShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { +bool GodotSoftBodyShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const { _SoftBodyIntersectSegmentInfo query_info; query_info.soft_body = soft_body; query_info.from = p_begin; diff --git a/servers/physics_3d/godot_soft_body_3d.h b/servers/physics_3d/godot_soft_body_3d.h index 008d5dddb8..86f73c366b 100644 --- a/servers/physics_3d/godot_soft_body_3d.h +++ b/servers/physics_3d/godot_soft_body_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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; @@ -101,11 +101,9 @@ class GodotSoftBody3D : public GodotCollisionObject3D { real_t drag_coefficient = 0.0; // [0,1] LocalVector<int> pinned_vertices; - Vector3 gravity; - SelfList<GodotSoftBody3D> active_list; - Set<GodotConstraint3D *> constraints; + HashSet<GodotConstraint3D *> constraints; Vector<AreaCMP> areas; @@ -113,7 +111,6 @@ class GodotSoftBody3D : public GodotCollisionObject3D { uint64_t island_step = 0; - _FORCE_INLINE_ void _compute_area_gravity(const GodotArea3D *p_area); _FORCE_INLINE_ Vector3 _compute_area_windforce(const GodotArea3D *p_area, const Face *p_face); public: @@ -126,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); } @@ -151,16 +148,16 @@ public: if (index > -1) { areas.write[index].refCount -= 1; if (areas[index].refCount < 1) { - areas.remove(index); + areas.remove_at(index); } } } - virtual void set_space(GodotSpace3D *p_space); + virtual void set_space(GodotSpace3D *p_space) override; void set_mesh(RID p_mesh); - void update_rendering_server(RenderingServerHandler *p_rendering_server_handler); + void update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler); Vector3 get_vertex_position(int p_index) const; void set_vertex_position(int p_index, const Vector3 &p_position); @@ -207,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. @@ -218,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(); @@ -232,7 +229,7 @@ private: void add_velocity(const Vector3 &p_velocity); - void apply_forces(bool p_has_wind_forces); + void apply_forces(const LocalVector<GodotArea3D *> &p_wind_areas); bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices); void generate_bending_constraints(int p_distance); @@ -257,18 +254,18 @@ class GodotSoftBodyShape3D : public GodotShape3D { public: GodotSoftBody3D *get_soft_body() const { return soft_body; } - virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SOFT_BODY; } - virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; } - virtual Vector3 get_support(const Vector3 &p_normal) const { return Vector3(); } - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SOFT_BODY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { r_min = r_max = 0.0; } + virtual Vector3 get_support(const Vector3 &p_normal) const override { return Vector3(); } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } - virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; - virtual bool intersect_point(const Vector3 &p_point) const; - virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; - virtual Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); } + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); } - virtual void set_data(const Variant &p_data) {} - virtual Variant get_data() const { return Variant(); } + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } void update_bounds(); diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp index 750bf3a16d..074232dd66 100644 --- a/servers/physics_3d/godot_space_3d.cpp +++ b/servers/physics_3d/godot_space_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -35,6 +35,7 @@ #include "core/config/project_settings.h" +#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001 #define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 _FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject3D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { @@ -57,9 +58,9 @@ _FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject3D *p_object, u return true; } -int GodotPhysicsDirectSpaceState3D::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +int GodotPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) { ERR_FAIL_COND_V(space->locked, false); - int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + int amount = space->broadphase->cull_point(p_parameters.position, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int cc = 0; //Transform3D ai = p_xform.affine_inverse(); @@ -69,13 +70,13 @@ int GodotPhysicsDirectSpaceState3D::intersect_point(const Vector3 &p_point, Shap break; } - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } //area can't be picked by ray (default) - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { continue; } @@ -85,7 +86,7 @@ int GodotPhysicsDirectSpaceState3D::intersect_point(const Vector3 &p_point, Shap Transform3D inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); inv_xform.affine_invert(); - if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) { + if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_parameters.position))) { continue; } @@ -104,13 +105,13 @@ int GodotPhysicsDirectSpaceState3D::intersect_point(const Vector3 &p_point, Shap return cc; } -bool GodotPhysicsDirectSpaceState3D::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) { +bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) { ERR_FAIL_COND_V(space->locked, false); Vector3 begin, end; Vector3 normal; - begin = p_from; - end = p_to; + begin = p_parameters.from; + end = p_parameters.to; normal = (end - begin).normalized(); int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -124,15 +125,15 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const Vector3 &p_from, const real_t min_d = 1e10; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } - if (p_pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) { + if (p_parameters.pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) { continue; } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { continue; } @@ -148,7 +149,23 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const Vector3 &p_from, const Vector3 shape_point, shape_normal; - if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { + if (shape->intersect_point(local_from)) { + if (p_parameters.hit_from_inside) { + // Hit shape at starting point. + min_d = 0; + res_point = begin; + res_normal = Vector3(); + res_shape = shape_idx; + res_obj = col_obj; + collided = true; + break; + } else { + // Ignore shape when starting inside. + continue; + } + } + + if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal, p_parameters.hit_back_faces)) { Transform3D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); shape_point = xform.xform(shape_point); @@ -183,15 +200,15 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const Vector3 &p_from, const return true; } -int GodotPhysicsDirectSpaceState3D::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +int GodotPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) { if (p_result_max <= 0) { return 0; } - GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape); + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); ERR_FAIL_COND_V(!shape, 0); - AABB aabb = p_xform.xform(shape->get_aabb()); + AABB aabb = p_parameters.transform.xform(shape->get_aabb()); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -204,20 +221,20 @@ int GodotPhysicsDirectSpaceState3D::intersect_shape(const RID &p_shape, const Tr break; } - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } //area can't be picked by ray (default) - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { continue; } const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; int shape_idx = space->intersection_query_subindex_results[i]; - if (!GodotCollisionSolver3D::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) { + if (!GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_parameters.margin, 0)) { continue; } @@ -238,36 +255,36 @@ int GodotPhysicsDirectSpaceState3D::intersect_shape(const RID &p_shape, const Tr return cc; } -bool GodotPhysicsDirectSpaceState3D::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { - GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape); +bool GodotPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe, ShapeRestInfo *r_info) { + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); ERR_FAIL_COND_V(!shape, false); - AABB aabb = p_xform.xform(shape->get_aabb()); - aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion - aabb = aabb.grow(p_margin); + AABB aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.merge(AABB(aabb.position + p_parameters.motion, aabb.size)); //motion + aabb = aabb.grow(p_parameters.margin); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); real_t best_safe = 1; real_t best_unsafe = 1; - Transform3D xform_inv = p_xform.affine_inverse(); + Transform3D xform_inv = p_parameters.transform.affine_inverse(); GodotMotionShape3D mshape; mshape.shape = shape; - mshape.motion = xform_inv.basis.xform(p_motion); + mshape.motion = xform_inv.basis.xform(p_parameters.motion); bool best_first = true; - Vector3 motion_normal = p_motion.normalized(); + Vector3 motion_normal = p_parameters.motion.normalized(); Vector3 closest_A, closest_B; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } - if (p_exclude.has(space->intersection_query_results[i]->get_self())) { + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { continue; //ignore excluded } @@ -279,14 +296,14 @@ bool GodotPhysicsDirectSpaceState3D::cast_motion(const RID &p_shape, const Trans Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? - if (GodotCollisionSolver3D::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { + if (GodotCollisionSolver3D::solve_distance(&mshape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { continue; } //test initial overlap, ignore objects it's inside of. sep_axis = motion_normal; - if (!GodotCollisionSolver3D::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { + if (!GodotCollisionSolver3D::solve_distance(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { continue; } @@ -297,11 +314,11 @@ bool GodotPhysicsDirectSpaceState3D::cast_motion(const RID &p_shape, const Trans for (int j = 0; j < 8; j++) { //steps should be customizable.. real_t fraction = low + (hi - low) * fraction_coeff; - mshape.motion = xform_inv.basis.xform(p_motion * fraction); + mshape.motion = xform_inv.basis.xform(p_parameters.motion * fraction); Vector3 lA, lB; Vector3 sep = motion_normal; //important optimization for this to work fast enough - bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); + bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); if (collided) { hi = fraction; @@ -357,16 +374,16 @@ bool GodotPhysicsDirectSpaceState3D::cast_motion(const RID &p_shape, const Trans return true; } -bool GodotPhysicsDirectSpaceState3D::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { +bool GodotPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) { if (p_result_max <= 0) { return false; } - GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape); + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); ERR_FAIL_COND_V(!shape, 0); - AABB aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.grow(p_margin); + AABB aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.grow(p_parameters.margin); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); @@ -382,19 +399,19 @@ bool GodotPhysicsDirectSpaceState3D::collide_shape(RID p_shape, const Transform3 GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk; for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { + if (p_parameters.exclude.has(col_obj->get_self())) { continue; } int shape_idx = space->intersection_query_subindex_results[i]; - if (GodotCollisionSolver3D::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { + if (GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) { collided = true; } } @@ -428,7 +445,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(); @@ -487,28 +504,32 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect rd->best_result.local_shape = rd->local_shape; } -bool GodotPhysicsDirectSpaceState3D::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape); +bool GodotPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) { + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); ERR_FAIL_COND_V(!shape, 0); - real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE); - AABB aabb = p_shape_xform.xform(shape->get_aabb()); - aabb = aabb.grow(p_margin); + AABB aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.grow(margin); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); _RestCallbackData rcd; - rcd.min_allowed_depth = min_contact_depth; + + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + real_t motion_length = p_parameters.motion.length(); + real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); for (int i = 0; i < amount; i++) { - if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { continue; } const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; - if (p_exclude.has(col_obj->get_self())) { + if (p_parameters.exclude.has(col_obj->get_self())) { continue; } @@ -516,7 +537,7 @@ bool GodotPhysicsDirectSpaceState3D::rest_info(RID p_shape, const Transform3D &p rcd.object = col_obj; rcd.shape = shape_idx; - bool sc = GodotCollisionSolver3D::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); + bool sc = GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, margin); if (!sc) { continue; } @@ -659,11 +680,13 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: return false; } + real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE); + // Undo the currently transform the physics server is aware of and apply the provided one body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); - body_aabb = body_aabb.grow(p_parameters.margin); + body_aabb = body_aabb.grow(margin); - real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; real_t motion_length = p_parameters.motion.length(); Vector3 motion_normal = p_parameters.motion / motion_length; @@ -678,6 +701,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; @@ -687,6 +711,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; @@ -711,9 +736,13 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: int shape_idx = intersection_query_subindex_results[i]; - 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, p_parameters.margin)) { + 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++; + } } } @@ -721,6 +750,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; @@ -736,7 +771,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; } } @@ -883,7 +918,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 } @@ -903,6 +938,9 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); + body_aabb.position += p_parameters.motion * unsafe; + int amount = _cull_aabb_for_body(p_body, body_aabb); + int from_shape = best_shape != -1 ? best_shape : 0; int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); @@ -914,10 +952,6 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j); GodotShape3D *body_shape = p_body->get_shape(j); - body_aabb.position += p_parameters.motion * unsafe; - - int amount = _cull_aabb_for_body(p_body, body_aabb); - for (int i = 0; i < amount; i++) { const GodotCollisionObject3D *col_obj = intersection_query_results[i]; if (p_parameters.exclude_bodies.has(col_obj->get_self())) { @@ -931,7 +965,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: rcd.object = col_obj; rcd.shape = shape_idx; - bool sc = 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), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); + bool sc = 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), _rest_cbk_result, &rcd, nullptr, margin); if (!sc) { continue; } @@ -967,6 +1001,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; @@ -980,16 +1015,14 @@ 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; } +// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree. void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self) { - if (!A->interacts_with(B)) { - return nullptr; - } - GodotCollisionObject3D::Type type_A = A->get_type(); GodotCollisionObject3D::Type type_B = B->get_type(); if (type_A > type_B) { @@ -998,7 +1031,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++; @@ -1019,10 +1052,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 { @@ -1037,9 +1070,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); } @@ -1077,7 +1110,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; } @@ -1155,9 +1188,12 @@ void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_v case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break; - case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + contact_bias = p_value; + break; case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: body_linear_velocity_sleep_threshold = p_value; break; @@ -1167,11 +1203,8 @@ void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_v case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break; - case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: - body_angular_velocity_damp_ratio = p_value; - break; - case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - constraint_bias = p_value; + case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: + solver_iterations = p_value; break; } } @@ -1182,18 +1215,18 @@ real_t GodotSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const { return contact_recycle_radius; case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation; - case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration; + case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + return contact_bias; case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: return body_linear_velocity_sleep_threshold; case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: return body_angular_velocity_sleep_threshold; case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep; - case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: - return body_angular_velocity_damp_ratio; - case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: - return constraint_bias; + case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: + return solver_iterations; } return 0; } @@ -1219,7 +1252,21 @@ GodotSpace3D::GodotSpace3D() { body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(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")); - body_angular_velocity_damp_ratio = 10; + + solver_iterations = GLOBAL_DEF("physics/3d/solver/solver_iterations", 16); + ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/solver_iterations", PropertyInfo(Variant::INT, "physics/3d/solver/solver_iterations", PROPERTY_HINT_RANGE, "1,32,1,or_greater")); + + contact_recycle_radius = GLOBAL_DEF("physics/3d/solver/contact_recycle_radius", 0.01); + ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/contact_recycle_radius", PropertyInfo(Variant::FLOAT, "physics/3d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,0.1,0.01,or_greater")); + + contact_max_separation = GLOBAL_DEF("physics/3d/solver/contact_max_separation", 0.05); + ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/contact_max_separation", PropertyInfo(Variant::FLOAT, "physics/3d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,0.1,0.01,or_greater")); + + contact_max_allowed_penetration = GLOBAL_DEF("physics/3d/solver/contact_max_allowed_penetration", 0.01); + ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/contact_max_allowed_penetration", PropertyInfo(Variant::FLOAT, "physics/3d/solver/contact_max_allowed_penetration", PROPERTY_HINT_RANGE, "0,0.1,0.01,or_greater")); + + contact_bias = GLOBAL_DEF("physics/3d/solver/default_contact_bias", 0.8); + ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/default_contact_bias", PropertyInfo(Variant::FLOAT, "physics/3d/solver/default_contact_bias", PROPERTY_HINT_RANGE, "0,1,0.01")); broadphase = GodotBroadPhase3D::create_func(); broadphase->set_pair_callback(_broadphase_pair, this); diff --git a/servers/physics_3d/godot_space_3d.h b/servers/physics_3d/godot_space_3d.h index 3b36dd346c..df7315e96d 100644 --- a/servers/physics_3d/godot_space_3d.h +++ b/servers/physics_3d/godot_space_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -47,14 +47,14 @@ class GodotPhysicsDirectSpaceState3D : public PhysicsDirectSpaceState3D { GDCLASS(GodotPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D); public: - GodotSpace3D *space; - - virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override; - virtual int intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override; - virtual bool collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; - virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; + 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; + virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; + virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe, ShapeRestInfo *r_info = nullptr) override; + virtual bool collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) override; + virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override; virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override; GodotPhysicsDirectSpaceState3D(); @@ -75,10 +75,10 @@ public: private: uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; - GodotPhysicsDirectSpaceState3D *direct_access; + GodotPhysicsDirectSpaceState3D *direct_access = nullptr; RID self; - GodotBroadPhase3D *broadphase; + GodotBroadPhase3D *broadphase = nullptr; SelfList<GodotBody3D>::List active_list; SelfList<GodotBody3D>::List mass_properties_update_list; SelfList<GodotBody3D>::List state_query_list; @@ -89,14 +89,16 @@ 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; - real_t contact_recycle_radius = 0.01; - real_t contact_max_separation = 0.05; - real_t contact_max_allowed_penetration = 0.01; - real_t constraint_bias = 0.01; + int solver_iterations = 0; + + real_t contact_recycle_radius = 0.0; + real_t contact_max_separation = 0.0; + real_t contact_max_allowed_penetration = 0.0; + real_t contact_bias = 0.0; enum { INTERSECTION_QUERY_MAX = 2048 @@ -105,10 +107,9 @@ private: GodotCollisionObject3D *intersection_query_results[INTERSECTION_QUERY_MAX]; int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; - real_t body_linear_velocity_sleep_threshold; - real_t body_angular_velocity_sleep_threshold; - real_t body_time_to_sleep; - real_t body_angular_velocity_damp_ratio; + real_t body_linear_velocity_sleep_threshold = 0.0; + real_t body_angular_velocity_sleep_threshold = 0.0; + real_t body_time_to_sleep = 0.0; bool locked = false; @@ -157,16 +158,16 @@ 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; } _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } - _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } + _FORCE_INLINE_ real_t get_contact_bias() const { return contact_bias; } _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } - _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; } void update(); void setup(); diff --git a/servers/physics_3d/godot_step_3d.cpp b/servers/physics_3d/godot_step_3d.cpp index a8654c617b..f384c829a4 100644 --- a/servers/physics_3d/godot_step_3d.cpp +++ b/servers/physics_3d/godot_step_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -49,7 +49,7 @@ void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D } 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. } @@ -181,14 +181,14 @@ void GodotStep3D::_check_suspend(const LocalVector<GodotBody3D *> &p_body_island } } -void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) { +void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { p_space->lock(); // can't access space during this p_space->setup(); //update inertias, etc p_space->set_last_step(p_delta); - iterations = p_iterations; + iterations = p_space->get_solver_iterations(); delta = p_delta; const SelfList<GodotBody3D>::List *body_list = &p_space->get_active_body_list(); @@ -220,6 +220,9 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) p_space->set_active_objects(active_count); + // Update the broadphase to register collision pairs. + p_space->update(); + { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); @@ -233,8 +236,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) 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; } @@ -340,7 +343,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) /* 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(); @@ -359,11 +363,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) // Warning: _solve_island modifies the constraint islands for optimization purpose, // their content is not reliable after these calls and shouldn't be used anymore. - if (island_count > 1) { - work_pool.do_work(island_count, this, &GodotStep3D::_solve_island, nullptr); - } else if (island_count > 0) { - _solve_island(0); - } + 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(); @@ -402,7 +403,6 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) all_constraints.clear(); - p_space->update(); p_space->unlock(); _step++; } @@ -411,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 23ede4feff..189487757f 100644 --- a/servers/physics_3d/godot_step_3d.h +++ b/servers/physics_3d/godot_step_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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; @@ -56,7 +54,7 @@ class GodotStep3D { void _check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const; public: - void step(GodotSpace3D *p_space, real_t p_delta, int p_iterations); + void step(GodotSpace3D *p_space, real_t p_delta); GodotStep3D(); ~GodotStep3D(); }; 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 31a87fc595..28cc064a1e 100644 --- a/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -129,24 +129,26 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { plane_space(normal[0], normal[1], normal[2]); for (int i = 0; i < 3; i++) { - memnew_placement(&m_jac[i], GodotJacobianEntry3D( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normal[i], - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); + memnew_placement( + &m_jac[i], + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); } } 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.); @@ -156,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); @@ -167,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); @@ -192,13 +194,12 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; - m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + - B->compute_angular_impulse_denominator(m_swingAxis)); + m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + B->compute_angular_impulse_denominator(m_swingAxis)); } // 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)); @@ -212,8 +213,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { m_twistAxis.normalize(); m_twistAxis *= -1.0f; - m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + - B->compute_angular_impulse_denominator(m_twistAxis)); + m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis)); } else if (twist > m_twistSpan * lockedFreeFactor) { m_twistCorrection = (twist - m_twistSpan); @@ -222,8 +222,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); - m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + - B->compute_angular_impulse_denominator(m_twistAxis)); + m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis)); } } 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 999d0f0692..fdcc2ceea3 100644 --- a/servers/physics_3d/joints/godot_cone_twist_joint_3d.h +++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ 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 b88e2d1190..e0fa940104 100644 --- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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); @@ -279,25 +279,30 @@ void GodotGeneric6DOFJoint3D::calculateTransforms() { void GodotGeneric6DOFJoint3D::buildLinearJacobian( GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld, const Vector3 &pivotAInW, const Vector3 &pivotBInW) { - memnew_placement(&jacLinear, GodotJacobianEntry3D( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normalWorld, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); + memnew_placement( + &jacLinear, + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); } void GodotGeneric6DOFJoint3D::buildAngularJacobian( GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW) { - memnew_placement(&jacAngular, GodotJacobianEntry3D(jointAxisW, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); + memnew_placement( + &jacAngular, + GodotJacobianEntry3D( + jointAxisW, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); } bool GodotGeneric6DOFJoint3D::testAngularLimitMotor(int axis_index) { @@ -340,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( @@ -383,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 729b3fa1f9..bcf2d18647 100644 --- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -119,11 +119,11 @@ public: //! Test limit /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ + * - free means upper < lower, + * - locked means upper == lower + * - limited means upper > lower + * - limitIndex: first 3 are linear, next 3 are angular + */ inline bool isLimited(int limitIndex) { return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); } @@ -291,11 +291,11 @@ public: //! Test limit /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ + * - free means upper < lower, + * - locked means upper == lower + * - limited means upper > lower + * - limitIndex: first 3 are linear, next 3 are angular + */ bool isLimited(int limitIndex) { if (limitIndex < 3) { return m_linearLimits.isLimited(limitIndex); diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp index 7b7ca1b3ac..01c59395c8 100644 --- a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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); @@ -149,16 +149,18 @@ bool GodotHingeJoint3D::setup(real_t p_step) { plane_space(normal[0], normal[1], normal[2]); for (int i = 0; i < 3; i++) { - memnew_placement(&m_jac[i], GodotJacobianEntry3D( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - pivotAInW - A->get_transform().origin - A->get_center_of_mass(), - pivotBInW - B->get_transform().origin - B->get_center_of_mass(), - normal[i], - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); + memnew_placement( + &m_jac[i], + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); } } @@ -169,29 +171,38 @@ 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)); - - memnew_placement(&m_jacAng[0], GodotJacobianEntry3D(jointAxis0, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - memnew_placement(&m_jacAng[1], GodotJacobianEntry3D(jointAxis1, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); - - memnew_placement(&m_jacAng[2], GodotJacobianEntry3D(hingeAxisWorld, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); + Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); + + memnew_placement( + &m_jacAng[0], + GodotJacobianEntry3D( + jointAxis0, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + memnew_placement( + &m_jacAng[1], + GodotJacobianEntry3D( + jointAxis1, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + memnew_placement( + &m_jacAng[2], + GodotJacobianEntry3D( + hingeAxisWorld, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); // Compute limit information real_t hingeAngle = get_hinge_angle(); @@ -202,16 +213,12 @@ bool GodotHingeJoint3D::setup(real_t p_step) { m_solveLimit = false; m_accLimitImpulse = real_t(0.); - //if (m_lowerLimit < m_upperLimit) if (m_useLimit && m_lowerLimit <= m_upperLimit) { - //if (hingeAngle <= m_lowerLimit*m_limitSoftness) if (hingeAngle <= m_lowerLimit) { m_correction = (m_lowerLimit - hingeAngle); m_limitSign = 1.0f; m_solveLimit = true; - } - //else if (hingeAngle >= m_upperLimit*m_limitSoftness) - else if (hingeAngle >= m_upperLimit) { + } else if (hingeAngle >= m_upperLimit) { m_correction = m_upperLimit - hingeAngle; m_limitSign = -1.0f; m_solveLimit = true; @@ -219,9 +226,8 @@ 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)); - m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + - B->compute_angular_impulse_denominator(axisA)); + 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; } @@ -265,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(); @@ -284,7 +290,7 @@ void GodotHingeJoint3D::solve(real_t p_step) { if (len > real_t(0.00001)) { Vector3 normal = velrelOrthog.normalized(); real_t denom = A->compute_angular_impulse_denominator(normal) + - B->compute_angular_impulse_denominator(normal); + B->compute_angular_impulse_denominator(normal); // scale for mass and relaxation velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor; } @@ -295,7 +301,7 @@ void GodotHingeJoint3D::solve(real_t p_step) { if (len2 > real_t(0.00001)) { Vector3 normal2 = angularError.normalized(); real_t denom2 = A->compute_angular_impulse_denominator(normal2) + - B->compute_angular_impulse_denominator(normal2); + B->compute_angular_impulse_denominator(normal2); angularError *= (real_t(1.) / denom2) * relaxation; } @@ -378,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 ff1fbe0f25..b934540e8d 100644 --- a/servers/physics_3d/joints/godot_hinge_joint_3d.h +++ b/servers/physics_3d/joints/godot_hinge_joint_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/joints/godot_jacobian_entry_3d.h b/servers/physics_3d/joints/godot_jacobian_entry_3d.h index 90a77a9b61..0fe15751d5 100644 --- a/servers/physics_3d/joints/godot_jacobian_entry_3d.h +++ b/servers/physics_3d/joints/godot_jacobian_entry_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/joints/godot_pin_joint_3d.cpp b/servers/physics_3d/joints/godot_pin_joint_3d.cpp index 10d52ad5e9..c52ea2665c 100644 --- a/servers/physics_3d/joints/godot_pin_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_pin_joint_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -63,16 +63,18 @@ bool GodotPinJoint3D::setup(real_t p_step) { for (int i = 0; i < 3; i++) { normal[i] = 1; - memnew_placement(&m_jac[i], GodotJacobianEntry3D( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), - B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), - normal, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); + memnew_placement( + &m_jac[i], + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), + B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), + normal, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); normal[i] = 0; } diff --git a/servers/physics_3d/joints/godot_pin_joint_3d.h b/servers/physics_3d/joints/godot_pin_joint_3d.h index 17e2e6e973..eeeaa650bd 100644 --- a/servers/physics_3d/joints/godot_pin_joint_3d.h +++ b/servers/physics_3d/joints/godot_pin_joint_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/servers/physics_3d/joints/godot_slider_joint_3d.cpp b/servers/physics_3d/joints/godot_slider_joint_3d.cpp index 3be111ac92..4539be21e3 100644 --- a/servers/physics_3d/joints/godot_slider_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_slider_joint_3d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -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,33 +111,37 @@ bool GodotSliderJoint3D::setup(real_t p_step) { int i; //linear part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - memnew_placement(&m_jacLin[i], GodotJacobianEntry3D( - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - m_relPosA - A->get_center_of_mass(), - m_relPosB - B->get_center_of_mass(), - normalWorld, - A->get_inv_inertia(), - A->get_inv_mass(), - B->get_inv_inertia(), - B->get_inv_mass())); + normalWorld = m_calculatedTransformA.basis.get_column(i); + memnew_placement( + &m_jacLin[i], + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + m_relPosA - A->get_center_of_mass(), + m_relPosB - B->get_center_of_mass(), + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal(); m_depth[i] = m_delta.dot(normalWorld); } testLinLimits(); // angular part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); - memnew_placement(&m_jacAng[i], GodotJacobianEntry3D( - normalWorld, - A->get_principal_inertia_axes().transposed(), - B->get_principal_inertia_axes().transposed(), - A->get_inv_inertia(), - B->get_inv_inertia())); + normalWorld = m_calculatedTransformA.basis.get_column(i); + memnew_placement( + &m_jacAng[i], + GodotJacobianEntry3D( + normalWorld, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + 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); @@ -202,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(); @@ -293,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); } } @@ -331,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 9baaf1fa40..f596c9ff75 100644 --- a/servers/physics_3d/joints/godot_slider_joint_3d.h +++ b/servers/physics_3d/joints/godot_slider_joint_3d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ |