diff options
Diffstat (limited to 'servers/physics_3d')
24 files changed, 299 insertions, 260 deletions
diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index 23c8079538..ba362740b2 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -113,7 +113,7 @@ struct MinkowskiDiff { real_t margin_A = 0.0; real_t margin_B = 0.0; - Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t); + Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t) = nullptr; void Initialize(const GodotShape3D* shape0, const Transform3D& wtrs0, const real_t margin0, const GodotShape3D* shape1, const Transform3D& wtrs1, const real_t margin1) { @@ -191,13 +191,13 @@ struct GJK /* Fields */ tShape m_shape; Vector3 m_ray; - real_t m_distance; + real_t m_distance = 0.0f; sSimplex m_simplices[2]; sSV m_store[4]; sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; + U m_nfree = 0; + U m_current = 0; + sSimplex* m_simplex = nullptr; eStatus::_ m_status; /* Methods */ GJK() @@ -548,12 +548,12 @@ struct GJK struct sFace { Vector3 n; - real_t d; + real_t d = 0.0f; sSV* c[3]; sFace* f[3]; sFace* l[2]; U1 e[3]; - U1 pass; + U1 pass = 0; }; struct sList { @@ -583,10 +583,10 @@ struct GJK eStatus::_ m_status; GJK::sSimplex m_result; Vector3 m_normal; - real_t m_depth; + real_t m_depth = 0.0f; sSV m_sv_store[EPA_MAX_VERTICES]; sFace m_fc_store[EPA_MAX_FACES]; - U m_nextsv; + U m_nextsv = 0; sList m_hull; sList m_stock; /* Methods */ diff --git a/servers/physics_3d/godot_area_3d.cpp b/servers/physics_3d/godot_area_3d.cpp index e7df23d0d8..e2ad765d62 100644 --- a/servers/physics_3d/godot_area_3d.cpp +++ b/servers/physics_3d/godot_area_3d.cpp @@ -254,22 +254,24 @@ void GodotArea3D::call_queries() { resptr[i] = &res[i]; } - for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_bodies.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); E = next; continue; } - res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; + res[0] = E->value.state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_bodies.erase(E); + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); E = next; Callable::CallError ce; @@ -290,22 +292,24 @@ void GodotArea3D::call_queries() { resptr[i] = &res[i]; } - for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) { - if (E->get().state == 0) { // Nothing happened - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_areas.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); E = next; continue; } - res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; - res[1] = E->key().rid; - res[2] = E->key().instance_id; - res[3] = E->key().body_shape; - res[4] = E->key().area_shape; + res[0] = E->value.state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; - Map<BodyKey, BodyState>::Element *next = E->next(); - monitored_areas.erase(E); + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); E = next; Callable::CallError ce; diff --git a/servers/physics_3d/godot_area_3d.h b/servers/physics_3d/godot_area_3d.h index ce64fc802a..a00451f602 100644 --- a/servers/physics_3d/godot_area_3d.h +++ b/servers/physics_3d/godot_area_3d.h @@ -72,16 +72,15 @@ class GodotArea3D : public GodotCollisionObject3D { uint32_t body_shape = 0; uint32_t area_shape = 0; - _FORCE_INLINE_ bool operator<(const BodyKey &p_key) const { - if (rid == p_key.rid) { - if (body_shape == p_key.body_shape) { - return area_shape < p_key.area_shape; - } else { - return body_shape < p_key.body_shape; - } - } else { - return rid < p_key.rid; - } + static uint32_t hash(const BodyKey &p_key) { + uint32_t h = hash_one_uint64(p_key.rid.get_id()); + h = hash_djb2_one_64(p_key.instance_id, h); + h = hash_djb2_one_32(p_key.area_shape, h); + return hash_djb2_one_32(p_key.body_shape, h); + } + + _FORCE_INLINE_ bool operator==(const BodyKey &p_key) const { + return rid == p_key.rid && instance_id == p_key.instance_id && body_shape == p_key.body_shape && area_shape == p_key.area_shape; } _FORCE_INLINE_ BodyKey() {} @@ -96,13 +95,13 @@ class GodotArea3D : public GodotCollisionObject3D { _FORCE_INLINE_ void dec() { state--; } }; - Map<BodyKey, BodyState> monitored_soft_bodies; - Map<BodyKey, BodyState> monitored_bodies; - Map<BodyKey, BodyState> monitored_areas; + HashMap<BodyKey, BodyState, BodyKey> monitored_soft_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_areas; - Set<GodotConstraint3D *> constraints; + HashSet<GodotConstraint3D *> constraints; - virtual void _shapes_changed(); + virtual void _shapes_changed() override; void _queue_monitor_update(); void _set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::AreaSpaceOverrideMode p_new_mode); @@ -164,7 +163,7 @@ public: _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } void set_monitorable(bool p_monitorable); @@ -172,7 +171,7 @@ public: void set_transform(const Transform3D &p_transform); - void set_space(GodotSpace3D *p_space); + void set_space(GodotSpace3D *p_space) override; void call_queries(); diff --git a/servers/physics_3d/godot_area_pair_3d.h b/servers/physics_3d/godot_area_pair_3d.h index c416477204..64b43a3b51 100644 --- a/servers/physics_3d/godot_area_pair_3d.h +++ b/servers/physics_3d/godot_area_pair_3d.h @@ -37,8 +37,8 @@ #include "godot_soft_body_3d.h" class GodotAreaPair3D : public GodotConstraint3D { - GodotBody3D *body; - GodotArea3D *area; + GodotBody3D *body = nullptr; + GodotArea3D *area = nullptr; int body_shape; int area_shape; bool colliding = false; @@ -55,8 +55,8 @@ public: }; class GodotArea2Pair3D : public GodotConstraint3D { - GodotArea3D *area_a; - GodotArea3D *area_b; + GodotArea3D *area_a = nullptr; + GodotArea3D *area_b = nullptr; int shape_a; int shape_b; bool colliding_a = false; @@ -76,8 +76,8 @@ public: }; class GodotAreaSoftBodyPair3D : public GodotConstraint3D { - GodotSoftBody3D *soft_body; - GodotArea3D *area; + GodotSoftBody3D *soft_body = nullptr; + GodotArea3D *area = nullptr; int soft_body_shape; int area_shape; bool colliding = false; diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h index 1906e8aab1..93bd5a0071 100644 --- a/servers/physics_3d/godot_body_3d.h +++ b/servers/physics_3d/godot_body_3d.h @@ -109,10 +109,10 @@ class GodotBody3D : public GodotCollisionObject3D { bool first_time_kinematic = false; void _mass_properties_changed(); - virtual void _shapes_changed(); + virtual void _shapes_changed() override; Transform3D new_transform; - Map<GodotConstraint3D *, int> constraint_map; + HashMap<GodotConstraint3D *, int> constraint_map; Vector<AreaCMP> areas; @@ -196,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; } @@ -301,7 +301,7 @@ public: _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; } _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; } - void set_space(GodotSpace3D *p_space); + void set_space(GodotSpace3D *p_space) override; void update_mass_properties(); void reset_mass_properties(); diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index 89d5d59161..eebbe0196d 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -39,7 +39,7 @@ #define MAX_BIAS_ROTATION (Math_PI / 8) void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - GodotBodyPair3D *pair = (GodotBodyPair3D *)p_userdata; + GodotBodyPair3D *pair = static_cast<GodotBodyPair3D *>(p_userdata); pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); } @@ -562,7 +562,7 @@ GodotBodyPair3D::~GodotBodyPair3D() { } void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { - GodotBodySoftBodyPair3D *pair = (GodotBodySoftBodyPair3D *)p_userdata; + GodotBodySoftBodyPair3D *pair = static_cast<GodotBodySoftBodyPair3D *>(p_userdata); pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); } diff --git a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp index 9a6b96c411..435c1e8aec 100644 --- a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp +++ b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp @@ -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 7138019a9c..ae5a18d955 100644 --- a/servers/physics_3d/godot_broad_phase_3d_bvh.h +++ b/servers/physics_3d/godot_broad_phase_3d_bvh.h @@ -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.h b/servers/physics_3d/godot_collision_object_3d.h index 0178838a25..0f09f21962 100644 --- a/servers/physics_3d/godot_collision_object_3d.h +++ b/servers/physics_3d/godot_collision_object_3d.h @@ -112,7 +112,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); @@ -169,11 +169,11 @@ public: 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 81e1a88366..b2d3e4d876 100644 --- a/servers/physics_3d/godot_collision_solver_3d.cpp +++ b/servers/physics_3d/godot_collision_solver_3d.cpp @@ -29,6 +29,7 @@ /*************************************************************************/ #include "godot_collision_solver_3d.h" + #include "godot_collision_solver_3d_sat.h" #include "godot_soft_body_3d.h" @@ -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(); @@ -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]; @@ -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; @@ -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; diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp index ca429040f5..20e9300778 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp +++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp @@ -68,7 +68,7 @@ *************************************************************************/ struct _CollectorCallback { - GodotCollisionSolver3D::CallbackResult callback; + GodotCollisionSolver3D::CallbackResult callback = nullptr; void *userdata = nullptr; bool swap = false; bool collided = false; @@ -792,7 +792,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_ // test faces for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_b.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -819,7 +819,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_ // test edges for (int i = 0; i < 3; i++) { - Vector3 axis = point_axis.cross(p_transform_b.basis.get_axis(i)).cross(p_transform_b.basis.get_axis(i)).normalized(); + Vector3 axis = point_axis.cross(p_transform_b.basis.get_column(i)).cross(p_transform_b.basis.get_column(i)).normalized(); if (!separator.test_axis(axis)) { return; @@ -842,7 +842,7 @@ static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D //capsule sphere 1, sphere - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; @@ -883,7 +883,7 @@ static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3 } // Cylinder B end caps. - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1).normalized(); if (!separator.test_axis(cylinder_B_axis)) { return; } @@ -897,8 +897,8 @@ static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3 // Closest point to cylinder caps. const Vector3 &sphere_center = p_transform_a.origin; - Vector3 cyl_axis = p_transform_b.basis.get_axis(1); - Vector3 cap_axis = p_transform_b.basis.get_axis(0); + Vector3 cyl_axis = p_transform_b.basis.get_column(1); + Vector3 cap_axis = p_transform_b.basis.get_column(0); real_t height_scale = cyl_axis.length(); real_t cap_dist = cylinder_B->get_height() * 0.5 * height_scale; cyl_axis /= height_scale; @@ -1063,7 +1063,7 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra // test faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -1073,7 +1073,7 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra // test faces of B for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_b.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -1083,7 +1083,7 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra // test combined edges for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - Vector3 axis = p_transform_a.basis.get_axis(i).cross(p_transform_b.basis.get_axis(j)); + Vector3 axis = p_transform_a.basis.get_column(i).cross(p_transform_b.basis.get_column(j)); if (Math::is_zero_approx(axis.length_squared())) { continue; @@ -1129,14 +1129,14 @@ static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_tra for (int i = 0; i < 3; i++) { //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); + Vector3 axis_a = p_transform_a.basis.get_column(i); if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { return; } //b ->a - Vector3 axis_b = p_transform_b.basis.get_axis(i); + Vector3 axis_b = p_transform_b.basis.get_column(i); if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) { return; @@ -1160,20 +1160,20 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; } } - Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized(); // edges of A, capsule cylinder for (int i = 0; i < 3; i++) { // cylinder - Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 box_axis = p_transform_a.basis.get_column(i); Vector3 axis = box_axis.cross(cyl_axis); if (Math::is_zero_approx(axis.length_squared())) { continue; @@ -1196,7 +1196,7 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; + point += p_transform_a.basis.get_column(l) * he[l]; } //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); @@ -1212,7 +1212,7 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p // capsule balls, edges of A for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -1234,7 +1234,7 @@ static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p // test edges of A for (int j = 0; j < 3; j++) { - Vector3 axis = point_axis.cross(p_transform_a.basis.get_axis(j)).cross(p_transform_a.basis.get_axis(j)).normalized(); + Vector3 axis = point_axis.cross(p_transform_a.basis.get_column(j)).cross(p_transform_a.basis.get_column(j)).normalized(); if (!separator.test_axis(axis)) { return; @@ -1258,14 +1258,14 @@ static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D & // Faces of A. for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; } } - Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized(); // Cylinder end caps. { @@ -1276,7 +1276,7 @@ static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D & // Edges of A, cylinder lateral surface. for (int i = 0; i < 3; i++) { - Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 box_axis = p_transform_a.basis.get_column(i); Vector3 axis = box_axis.cross(cyl_axis); if (Math::is_zero_approx(axis.length_squared())) { continue; @@ -1300,7 +1300,7 @@ static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D & Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k]; point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * extent[l]; + point += p_transform_a.basis.get_column(l) * extent[l]; } } } @@ -1380,7 +1380,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -1401,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]); @@ -1438,7 +1438,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo for (int i = 0; i < 3; i++) { //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); + Vector3 axis_a = p_transform_a.basis.get_column(i); if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { return; @@ -1456,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++) { @@ -1497,7 +1497,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); if (axis.dot(normal) < 0.0) { axis *= -1.0; } @@ -1513,7 +1513,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr Vector3 e = vertex[i] - vertex[(i + 1) % 3]; for (int j = 0; j < 3; j++) { - Vector3 axis = e.cross(p_transform_a.basis.get_axis(j)).normalized(); + Vector3 axis = e.cross(p_transform_a.basis.get_column(j)).normalized(); if (axis.dot(normal) < 0.0) { axis *= -1.0; } @@ -1550,7 +1550,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr for (int i = 0; i < 3; i++) { //a ->b - Vector3 axis_a = p_transform_a.basis.get_axis(i); + Vector3 axis_a = p_transform_a.basis.get_column(i); Vector3 axis = axis_ab.cross(axis_a).cross(axis_a).normalized(); if (axis.dot(normal) < 0.0) { @@ -1573,7 +1573,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr he.z *= (k * 2 - 1); Vector3 point = p_transform_a.origin; for (int l = 0; l < 3; l++) { - point += p_transform_a.basis.get_axis(l) * he[l]; + point += p_transform_a.basis.get_column(l) * he[l]; } for (int e = 0; e < 3; e++) { @@ -1623,8 +1623,8 @@ static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3 // some values - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); - Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; @@ -1686,14 +1686,14 @@ static void _collision_capsule_cylinder(const GodotShape3D *p_a, const Transform } // Cylinder B end caps. - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1).normalized(); if (!separator.test_axis(cylinder_B_axis)) { return; } // Cylinder edge against capsule balls. - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1); + Vector3 capsule_A_axis = p_transform_a.basis.get_column(1); Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); @@ -1772,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; @@ -1784,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); @@ -1824,7 +1824,7 @@ static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D & // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); for (int i = 0; i < 3; i++) { // edge-cylinder @@ -1895,8 +1895,8 @@ static void _collision_cylinder_cylinder(const GodotShape3D *p_a, const Transfor SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); - Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1); - Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1); + Vector3 cylinder_A_axis = p_transform_a.basis.get_column(1); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1); if (!separator.test_previous_axis()) { return; @@ -1983,7 +1983,7 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D return; } - Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized(); + Vector3 cyl_axis = p_transform_a.basis.get_column(1).normalized(); if (cyl_axis.dot(normal) < 0.0) { cyl_axis *= -1.0; } diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp index a1912dc660..b735283ebe 100644 --- a/servers/physics_3d/godot_physics_server_3d.cpp +++ b/servers/physics_3d/godot_physics_server_3d.cpp @@ -920,7 +920,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); @@ -1355,7 +1355,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); @@ -1531,7 +1531,7 @@ void GodotPhysicsServer3D::free(RID p_rid) { GodotShape3D *shape = shape_owner.get_or_null(p_rid); while (shape->get_owners().size()) { - GodotShapeOwner3D *so = shape->get_owners().front()->key(); + GodotShapeOwner3D *so = shape->get_owners().begin()->key; so->remove_shape(shape); } @@ -1570,7 +1570,7 @@ void GodotPhysicsServer3D::free(RID p_rid) { GodotSpace3D *space = space_owner.get_or_null(p_rid); while (space->get_objects().size()) { - GodotCollisionObject3D *co = (GodotCollisionObject3D *)space->get_objects().front()->get(); + GodotCollisionObject3D *co = static_cast<GodotCollisionObject3D *>(*space->get_objects().begin()); co->set_space(nullptr); } @@ -1611,11 +1611,11 @@ void GodotPhysicsServer3D::step(real_t p_step) { island_count = 0; active_objects = 0; collision_pairs = 0; - for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { - stepper->step((GodotSpace3D *)E->get(), p_step); - island_count += E->get()->get_island_count(); - active_objects += E->get()->get_active_objects(); - collision_pairs += E->get()->get_collision_pairs(); + for (const GodotSpace3D *E : active_spaces) { + stepper->step(const_cast<GodotSpace3D *>(E), p_step); + island_count += E->get_island_count(); + active_objects += E->get_active_objects(); + collision_pairs += E->get_collision_pairs(); } #endif } @@ -1635,8 +1635,8 @@ void GodotPhysicsServer3D::flush_queries() { uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); - for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { - GodotSpace3D *space = (GodotSpace3D *)E->get(); + for (const GodotSpace3D *E : active_spaces) { + GodotSpace3D *space = const_cast<GodotSpace3D *>(E); space->call_queries(); } @@ -1656,9 +1656,9 @@ void GodotPhysicsServer3D::flush_queries() { total_time[i] = 0; } - for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) { + for (const GodotSpace3D *E : active_spaces) { for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) { - total_time[i] += E->get()->get_elapsed_time(GodotSpace3D::ElapsedTime(i)); + total_time[i] += E->get_elapsed_time(GodotSpace3D::ElapsedTime(i)); } } @@ -1671,7 +1671,7 @@ void GodotPhysicsServer3D::flush_queries() { values.push_back("flush_queries"); values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); - values.push_front("physics"); + values.push_front("physics_3d"); EngineDebugger::profiler_add_frame_data("servers", values); } #endif @@ -1709,7 +1709,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 b903f4808c..1d57451925 100644 --- a/servers/physics_3d/godot_physics_server_3d.h +++ b/servers/physics_3d/godot_physics_server_3d.h @@ -54,7 +54,7 @@ class GodotPhysicsServer3D : public PhysicsServer3D { bool flushing_queries = false; GodotStep3D *stepper = nullptr; - Set<const GodotSpace3D *> active_spaces; + HashSet<const GodotSpace3D *> active_spaces; mutable RID_PtrOwner<GodotShape3D, true> shape_owner; mutable RID_PtrOwner<GodotSpace3D, true> space_owner; @@ -74,7 +74,7 @@ public: struct CollCbkData { int max; int amount; - Vector3 *ptr; + Vector3 *ptr = nullptr; }; static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); @@ -253,7 +253,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; @@ -353,7 +353,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 */ diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp index 7762c4829e..5e310670a5 100644 --- a/servers/physics_3d/godot_shape_3d.cpp +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -62,7 +62,7 @@ void GodotShape3D::configure(const AABB &p_aabb) { aabb = p_aabb; configured = true; for (const KeyValue<GodotShapeOwner3D *, int> &E : owners) { - GodotShapeOwner3D *co = (GodotShapeOwner3D *)E.key; + GodotShapeOwner3D *co = const_cast<GodotShapeOwner3D *>(E.key); co->_shape_changed(); } } @@ -76,20 +76,20 @@ Vector3 GodotShape3D::get_support(const Vector3 &p_normal) const { } void GodotShape3D::add_owner(GodotShapeOwner3D *p_owner) { - Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner); + HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner); if (E) { - E->get()++; + E->value++; } else { owners[p_owner] = 1; } } void GodotShape3D::remove_owner(GodotShapeOwner3D *p_owner) { - Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner); + HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner); ERR_FAIL_COND(!E); - E->get()--; - if (E->get() == 0) { - owners.erase(E); + E->value--; + if (E->value == 0) { + owners.remove(E); } } @@ -97,7 +97,7 @@ bool GodotShape3D::is_owner(GodotShapeOwner3D *p_owner) const { return owners.has(p_owner); } -const Map<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const { +const HashMap<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const { return owners; } @@ -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); @@ -1438,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 { @@ -1461,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) { diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h index 9d171c3928..1fc8f7c711 100644 --- a/servers/physics_3d/godot_shape_3d.h +++ b/servers/physics_3d/godot_shape_3d.h @@ -51,7 +51,7 @@ class GodotShape3D { bool configured = false; real_t custom_bias = 0.0; - Map<GodotShapeOwner3D *, int> owners; + HashMap<GodotShapeOwner3D *, int> owners; protected: void configure(const AABB &p_aabb); @@ -93,7 +93,7 @@ public: void add_owner(GodotShapeOwner3D *p_owner); void remove_owner(GodotShapeOwner3D *p_owner); bool is_owner(GodotShapeOwner3D *p_owner) const; - const Map<GodotShapeOwner3D *, int> &get_owners() const; + const HashMap<GodotShapeOwner3D *, int> &get_owners() const; GodotShape3D() {} virtual ~GodotShape3D(); diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp index 095050b7f3..173843072a 100644 --- a/servers/physics_3d/godot_soft_body_3d.cpp +++ b/servers/physics_3d/godot_soft_body_3d.cpp @@ -33,7 +33,7 @@ #include "godot_space_3d.h" #include "core/math/geometry_3d.h" -#include "core/templates/map.h" +#include "core/templates/rb_map.h" #include "servers/rendering_server.h" // Based on Bullet soft body. @@ -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; } @@ -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++; @@ -1272,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]); diff --git a/servers/physics_3d/godot_soft_body_3d.h b/servers/physics_3d/godot_soft_body_3d.h index 5028e81dd8..86f73c366b 100644 --- a/servers/physics_3d/godot_soft_body_3d.h +++ b/servers/physics_3d/godot_soft_body_3d.h @@ -37,8 +37,8 @@ #include "core/math/aabb.h" #include "core/math/dynamic_bvh.h" #include "core/math/vector3.h" +#include "core/templates/hash_set.h" #include "core/templates/local_vector.h" -#include "core/templates/set.h" #include "core/templates/vset.h" class GodotConstraint3D; @@ -103,7 +103,7 @@ class GodotSoftBody3D : public GodotCollisionObject3D { SelfList<GodotSoftBody3D> active_list; - Set<GodotConstraint3D *> constraints; + HashSet<GodotConstraint3D *> constraints; Vector<AreaCMP> areas; @@ -123,7 +123,7 @@ public: _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } @@ -153,11 +153,11 @@ public: } } - 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); @@ -204,8 +204,8 @@ public: void predict_motion(real_t p_delta); void solve_constraints(real_t p_delta); - _FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return ((Node *)p_node)->index; } - _FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return ((Face *)p_face)->index; } + _FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return static_cast<Node *>(p_node)->index; } + _FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return static_cast<Face *>(p_face)->index; } // Return true to stop the query. // p_index is the node index for AABB query, face index for Ray query. @@ -215,7 +215,7 @@ public: void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata); protected: - virtual void _shapes_changed(); + virtual void _shapes_changed() override; private: void update_normals_and_centroids(); diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp index 2490a2f506..533d7605ce 100644 --- a/servers/physics_3d/godot_space_3d.cpp +++ b/servers/physics_3d/godot_space_3d.cpp @@ -153,7 +153,7 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parame if (p_parameters.hit_from_inside) { // Hit shape at starting point. min_d = 0; - res_point = local_from; + res_point = begin; res_normal = Vector3(); res_shape = shape_idx; res_obj = col_obj; @@ -445,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(); @@ -906,7 +906,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 } @@ -1007,11 +1007,8 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D:: 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) { @@ -1020,7 +1017,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++; @@ -1041,10 +1038,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 { @@ -1059,9 +1056,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); } @@ -1099,7 +1096,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; } diff --git a/servers/physics_3d/godot_space_3d.h b/servers/physics_3d/godot_space_3d.h index ac54c8bf69..df7315e96d 100644 --- a/servers/physics_3d/godot_space_3d.h +++ b/servers/physics_3d/godot_space_3d.h @@ -47,7 +47,7 @@ class GodotPhysicsDirectSpaceState3D : public PhysicsDirectSpaceState3D { GDCLASS(GodotPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D); public: - GodotSpace3D *space; + GodotSpace3D *space = nullptr; virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override; @@ -89,7 +89,7 @@ private: static void *_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self); static void _broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self); - Set<GodotCollisionObject3D *> objects; + HashSet<GodotCollisionObject3D *> objects; GodotArea3D *area = nullptr; @@ -158,7 +158,7 @@ public: void add_object(GodotCollisionObject3D *p_object); void remove_object(GodotCollisionObject3D *p_object); - const Set<GodotCollisionObject3D *> &get_objects() const; + const HashSet<GodotCollisionObject3D *> &get_objects() const; _FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; } _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } diff --git a/servers/physics_3d/godot_step_3d.cpp b/servers/physics_3d/godot_step_3d.cpp index 63635b224b..99656d01a0 100644 --- a/servers/physics_3d/godot_step_3d.cpp +++ b/servers/physics_3d/godot_step_3d.cpp @@ -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. } @@ -236,8 +236,8 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { const SelfList<GodotArea3D>::List &aml = p_space->get_moved_area_list(); while (aml.first()) { - for (const Set<GodotConstraint3D *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - GodotConstraint3D *constraint = E->get(); + for (GodotConstraint3D *E : aml.first()->self()->get_constraints()) { + GodotConstraint3D *constraint = E; if (constraint->get_island_step() == _step) { continue; } diff --git a/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp index dc4858ff9a..28cc064a1e 100644 --- a/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp @@ -147,8 +147,8 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { Vector3 b1Axis1, b1Axis2, b1Axis3; Vector3 b2Axis1, b2Axis2; - b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0)); - b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0)); + b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(0)); + b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_column(0)); real_t swing1 = real_t(0.), swing2 = real_t(0.); @@ -158,7 +158,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { // Get Frame into world space if (m_swingSpan1 >= real_t(0.05f)) { - b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1)); + b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(1)); //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); @@ -169,7 +169,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { } if (m_swingSpan2 >= real_t(0.05f)) { - b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2)); + b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_column(2)); //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); @@ -199,7 +199,7 @@ bool GodotConeTwistJoint3D::setup(real_t p_timestep) { // Twist limits if (m_twistSpan >= real_t(0.)) { - Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1)); + Vector3 b2Axis22 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_column(1)); Quaternion rotationArc = Quaternion(b2Axis1, b1Axis1); Vector3 TwistRef = rotationArc.xform(b2Axis22); real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp index 41e9bf06bb..e0fa940104 100644 --- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp @@ -249,8 +249,8 @@ void GodotGeneric6DOFJoint3D::calculateAngleInfo() { // easier to take the euler rate expression for d(angle[2])/dt with respect // to the components of w and set that to 0. - Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0); - Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2); + Vector3 axis0 = m_calculatedTransformB.basis.get_column(0); + Vector3 axis2 = m_calculatedTransformA.basis.get_column(2); m_calculatedAxis[1] = axis2.cross(axis0); m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); @@ -345,9 +345,9 @@ bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) { for (i = 0; i < 3; i++) { if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { if (m_useLinearReferenceFrameA) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); } else { - normalWorld = m_calculatedTransformB.basis.get_axis(i); + normalWorld = m_calculatedTransformB.basis.get_column(i); } buildLinearJacobian( @@ -388,9 +388,9 @@ void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) { jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); if (m_useLinearReferenceFrameA) { - linear_axis = m_calculatedTransformA.basis.get_axis(i); + linear_axis = m_calculatedTransformA.basis.get_column(i); } else { - linear_axis = m_calculatedTransformB.basis.get_axis(i); + linear_axis = m_calculatedTransformB.basis.get_column(i); } m_linearLimits.solveLinearAxis( diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp index 1c4d5dec23..01c59395c8 100644 --- a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp @@ -92,16 +92,16 @@ GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const V m_rbAFrame.origin = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis - Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0); + Vector3 rbAxisA1 = rbA->get_transform().basis.get_column(0); Vector3 rbAxisA2; real_t projection = axisInA.dot(rbAxisA1); if (projection >= 1.0f - CMP_EPSILON) { - rbAxisA1 = -rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); + rbAxisA1 = -rbA->get_transform().basis.get_column(2); + rbAxisA2 = rbA->get_transform().basis.get_column(1); } else if (projection <= -1.0f + CMP_EPSILON) { - rbAxisA1 = rbA->get_transform().basis.get_axis(2); - rbAxisA2 = rbA->get_transform().basis.get_axis(1); + rbAxisA1 = rbA->get_transform().basis.get_column(2); + rbAxisA2 = rbA->get_transform().basis.get_column(1); } else { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); @@ -171,11 +171,11 @@ bool GodotHingeJoint3D::setup(real_t p_step) { Vector3 jointAxis0local; Vector3 jointAxis1local; - plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local); + plane_space(m_rbAFrame.basis.get_column(2), jointAxis0local, jointAxis1local); Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local); Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); - Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); memnew_placement( &m_jacAng[0], @@ -226,7 +226,7 @@ bool GodotHingeJoint3D::setup(real_t p_step) { } //Compute K = J*W*J' for hinge axis - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); return true; @@ -271,8 +271,8 @@ void GodotHingeJoint3D::solve(real_t p_step) { ///solve angular part // get axes in world space - Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); - Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2)); + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); + Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(2)); const Vector3 &angVelA = A->get_angular_velocity(); const Vector3 &angVelB = B->get_angular_velocity(); @@ -384,9 +384,9 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { } real_t GodotHingeJoint3D::get_hinge_angle() { - const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0)); - const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1)); - const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1)); + const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0)); + const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(1)); + const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(1)); return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); } diff --git a/servers/physics_3d/joints/godot_slider_joint_3d.cpp b/servers/physics_3d/joints/godot_slider_joint_3d.cpp index f175421304..4539be21e3 100644 --- a/servers/physics_3d/joints/godot_slider_joint_3d.cpp +++ b/servers/physics_3d/joints/godot_slider_joint_3d.cpp @@ -102,7 +102,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; m_relPosA = m_projPivotInW - A->get_transform().origin; @@ -111,7 +111,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { int i; //linear part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); memnew_placement( &m_jacLin[i], GodotJacobianEntry3D( @@ -130,7 +130,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { testLinLimits(); // angular part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); memnew_placement( &m_jacAng[i], GodotJacobianEntry3D( @@ -141,7 +141,7 @@ bool GodotSliderJoint3D::setup(real_t p_step) { B->get_inv_inertia())); } testAngLimits(); - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); + Vector3 axisA = m_calculatedTransformA.basis.get_column(0); m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); // clear accumulator for motors m_accumulatedLinMotorImpulse = real_t(0.0); @@ -206,8 +206,8 @@ void GodotSliderJoint3D::solve(real_t p_step) { } // angular // get axes in world space - Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); - Vector3 axisB = m_calculatedTransformB.basis.get_axis(0); + Vector3 axisA = m_calculatedTransformA.basis.get_column(0); + Vector3 axisB = m_calculatedTransformB.basis.get_column(0); const Vector3 &angVelA = A->get_angular_velocity(); const Vector3 &angVelB = B->get_angular_velocity(); @@ -297,14 +297,14 @@ void GodotSliderJoint3D::calculateTransforms() { m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; - m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X + m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; Vector3 normalWorld; int i; //linear part for (i = 0; i < 3; i++) { - normalWorld = m_calculatedTransformA.basis.get_axis(i); + normalWorld = m_calculatedTransformA.basis.get_column(i); m_depth[i] = m_delta.dot(normalWorld); } } @@ -335,9 +335,9 @@ void GodotSliderJoint3D::testAngLimits() { m_angDepth = real_t(0.); m_solveAngLim = false; if (m_lowerAngLimit <= m_upperAngLimit) { - const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1); - const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2); - const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1); + const Vector3 axisA0 = m_calculatedTransformA.basis.get_column(1); + const Vector3 axisA1 = m_calculatedTransformA.basis.get_column(2); + const Vector3 axisB0 = m_calculatedTransformB.basis.get_column(1); real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); if (rot < m_lowerAngLimit) { m_angDepth = rot - m_lowerAngLimit; |