diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h index 3bf603b30d09..0681ae3b19c7 100644 --- a/servers/physics_2d/area_2d_sw.h +++ b/servers/physics_2d/area_2d_sw.h @@ -34,11 +34,9 @@ #include "collision_object_2d_sw.h" #include "core/templates/self_list.h" #include "servers/physics_server_2d.h" -//#include "servers/physics_3d/query_sw.h" class Space2DSW; class Body2DSW; -class Constraint2DSW; class Area2DSW : public CollisionObject2DSW { PhysicsServer2D::AreaSpaceOverrideMode space_override_mode; @@ -94,17 +92,10 @@ class Area2DSW : public CollisionObject2DSW { Map monitored_bodies; Map monitored_areas; - //virtual void shape_changed_notify(Shape2DSW *p_shape); - //virtual void shape_deleted_notify(Shape2DSW *p_shape); - Set constraints; - virtual void _shapes_changed(); void _queue_monitor_update(); public: - //_FORCE_INLINE_ const Matrix32& get_inverse_transform() const { return inverse_transform; } - //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } - void set_monitor_callback(ObjectID p_id, const StringName &p_method); _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } @@ -147,11 +138,6 @@ class Area2DSW : public CollisionObject2DSW { _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ int get_priority() const { return priority; } - _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } - _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } - void set_monitorable(bool p_monitorable); _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index eb9fc0e80053..e452ae23d282 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -89,7 +89,7 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, area = p_area; body_shape = p_body_shape; area_shape = p_area_shape; - body->add_constraint(this, 0); + body->add_constraint(this); area->add_constraint(this); if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair p_body->set_active(true); @@ -105,7 +105,7 @@ AreaPair2DSW::~AreaPair2DSW() { area->remove_body_from_query(body, body_shape, area_shape); } } - body->remove_constraint(this, 0); + body->remove_constraint(this); area->remove_constraint(this); } diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index f78a487e27f3..e59a420e3a87 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -541,22 +541,24 @@ void Body2DSW::integrate_velocities(real_t p_step) { } void Body2DSW::wakeup_neighbours() { - for (List>::Element *E = constraint_list.front(); E; E = E->next()) { - const Constraint2DSW *c = E->get().first; - Body2DSW **n = c->get_body_ptr(); - int bc = c->get_body_count(); + for (const CollisionObject2DSW::T_ConstraintMap::Element *E = get_constraint_map().front(); E; E = E->next()) { + const Constraint2DSW *constraint = E->get(); - for (int i = 0; i < bc; i++) { - if (i == E->get().second) { + Body2DSW **bodies = constraint->get_body_ptr(); + int body_count = constraint->get_body_count(); + for (int i = 0; i < body_count; i++) { + Body2DSW *body = bodies[i]; + + if (body == this) { continue; } - Body2DSW *b = n[i]; - if (b->mode != PhysicsServer2D::BODY_MODE_RIGID) { + + if (body->mode != PhysicsServer2D::BODY_MODE_RIGID) { continue; } - if (!b->is_active()) { - b->set_active(true); + if (!body->is_active()) { + body->set_active(true); } } } diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index b4a95651cb78..13dd93105e06 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -33,12 +33,8 @@ #include "area_2d_sw.h" #include "collision_object_2d_sw.h" -#include "core/templates/list.h" -#include "core/templates/pair.h" #include "core/templates/vset.h" -class Constraint2DSW; - class Body2DSW : public CollisionObject2DSW { PhysicsServer2D::BodyMode mode; @@ -85,8 +81,6 @@ class Body2DSW : public CollisionObject2DSW { virtual void _shapes_changed(); Transform2D new_transform; - List> constraint_list; - struct AreaCMP { Area2DSW *area; int refCount; @@ -172,11 +166,6 @@ class Body2DSW : public CollisionObject2DSW { _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); } - _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); } - const List> &get_constraint_list() const { return constraint_list; } - _FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); } - _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index ff31825b83ec..f642b30bf763 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -534,11 +534,11 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh shape_A = p_shape_A; shape_B = p_shape_B; space = A->get_space(); - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); } BodyPair2DSW::~BodyPair2DSW() { - A->remove_constraint(this, 0); - B->remove_constraint(this, 1); + A->remove_constraint(this); + B->remove_constraint(this); } diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index c395e59694e3..895a99db764c 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -32,6 +32,8 @@ #define COLLISION_OBJECT_2D_SW_H #include "broad_phase_2d_sw.h" +#include "constraint_2d_sw.h" +#include "core/templates/map.h" #include "core/templates/self_list.h" #include "servers/physics_server_2d.h" #include "shape_2d_sw.h" @@ -45,6 +47,8 @@ class CollisionObject2DSW : public ShapeOwner2DSW { TYPE_BODY }; + typedef Map T_ConstraintMap; + private: Type type; RID self; @@ -77,6 +81,8 @@ class CollisionObject2DSW : public ShapeOwner2DSW { uint32_t collision_layer; bool _static; + T_ConstraintMap constraint_map; + SelfList pending_shape_update_list; void _update_shapes(); @@ -183,6 +189,11 @@ class CollisionObject2DSW : public ShapeOwner2DSW { void remove_shape(Shape2DSW *p_shape); void remove_shape(int p_index); + _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint) { constraint_map.insert(p_constraint->get_constraint_id(), p_constraint); } + _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraint_map.erase(p_constraint->get_constraint_id()); } + const T_ConstraintMap &get_constraint_map() const { return constraint_map; } + _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } + virtual void set_space(Space2DSW *p_space) = 0; _FORCE_INLINE_ bool is_static() const { return _static; } diff --git a/servers/physics_2d/constraint_2d_sw.cpp b/servers/physics_2d/constraint_2d_sw.cpp new file mode 100644 index 000000000000..42161e981a4e --- /dev/null +++ b/servers/physics_2d/constraint_2d_sw.cpp @@ -0,0 +1,33 @@ +/*************************************************************************/ +/* constraint_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "constraint_2d_sw.h" + +SafeNumeric Constraint2DSW::constraint_id_counter; diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index 5e8dfbe570f2..8935779c95ee 100644 --- a/servers/physics_2d/constraint_2d_sw.h +++ b/servers/physics_2d/constraint_2d_sw.h @@ -31,7 +31,11 @@ #ifndef CONSTRAINT_2D_SW_H #define CONSTRAINT_2D_SW_H -#include "body_2d_sw.h" +#include "core/math/math_defs.h" +#include "core/templates/rid.h" +#include "core/templates/safe_refcount.h" + +class Body2DSW; class Constraint2DSW { Body2DSW **_body_ptr; @@ -41,18 +45,24 @@ class Constraint2DSW { RID self; + static SafeNumeric constraint_id_counter; + uint32_t constraint_id = 0; + protected: Constraint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) { _body_ptr = p_body_ptr; _body_count = p_body_count; island_step = 0; disabled_collisions_between_bodies = true; + constraint_id = constraint_id_counter.increment(); } public: _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } _FORCE_INLINE_ RID get_self() const { return self; } + _FORCE_INLINE_ uint32_t get_constraint_id() const { return constraint_id; } + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index eaec582f9ba3..536afb0cb31f 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -215,9 +215,9 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p softness = 0; - p_body_a->add_constraint(this, 0); + p_body_a->add_constraint(this); if (p_body_b) { - p_body_b->add_constraint(this, 1); + p_body_b->add_constraint(this); } } @@ -370,8 +370,8 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_ B_anchor = B->get_inv_transform().xform(p_b_anchor); A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal(); - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); } ////////////////////////////////////////////// @@ -482,6 +482,6 @@ DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Ve stiffness = 20; damping = 1.5; - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); } diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h index ccc5c585a012..3cd701649391 100644 --- a/servers/physics_2d/joints_2d_sw.h +++ b/servers/physics_2d/joints_2d_sw.h @@ -70,7 +70,7 @@ class Joint2DSW : public Constraint2DSW { for (int i = 0; i < get_body_count(); i++) { Body2DSW *body = get_body_ptr()[i]; if (body) { - body->remove_constraint(this, i); + body->remove_constraint(this); } } }; diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp index 6d64f4126c21..53bf2ac43be3 100644 --- a/servers/physics_2d/physics_server_2d_sw.cpp +++ b/servers/physics_2d/physics_server_2d_sw.cpp @@ -304,7 +304,7 @@ void PhysicsServer2DSW::area_set_space(RID p_area, RID p_space) { return; //pointless } - area->clear_constraints(); + area->clear_constraint_map(); area->set_space(space); }; @@ -548,7 +548,7 @@ void PhysicsServer2DSW::body_set_space(RID p_body, RID p_space) { return; //pointless } - body->clear_constraint_list(); + body->clear_constraint_map(); body->set_space(space); }; diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index b8cb4cddc55f..f31d984d7e77 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -46,26 +46,30 @@ void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector &p_bod p_body_island.push_back(p_body); } - for (const List>::Element *E = p_body->get_constraint_list().front(); E; E = E->next()) { - Constraint2DSW *constraint = (Constraint2DSW *)E->get().first; + for (const CollisionObject2DSW::T_ConstraintMap::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { + Constraint2DSW *constraint = E->get(); if (constraint->get_island_step() == _step) { continue; // Already processed. } constraint->set_island_step(_step); p_constraint_island.push_back(constraint); + all_constraints.push_back(constraint); - for (int i = 0; i < constraint->get_body_count(); i++) { - if (i == E->get().second) { - continue; - } - Body2DSW *other_body = constraint->get_body_ptr()[i]; + // Find connected rigid bodies. + Body2DSW **bodies = constraint->get_body_ptr(); + int body_count = constraint->get_body_count(); + for (int i = 0; i < body_count; i++) { + Body2DSW *other_body = bodies[i]; + if (other_body->get_island_step() == _step) { - continue; // Already processed. + continue; // Already processed (can be this body). } + if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) { continue; // Static bodies don't connect islands. } + _populate_island(other_body, p_body_island, p_constraint_island); } } @@ -163,7 +167,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { const SelfList::List &aml = p_space->get_moved_area_list(); while (aml.first()) { - for (const Set::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + for (const CollisionObject2DSW::T_ConstraintMap::Element *E = aml.first()->self()->get_constraint_map().front(); E; E = E->next()) { Constraint2DSW *constraint = E->get(); if (constraint->get_island_step() == _step) { continue; diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h index 8a0a1e963bfe..780d4aafe7d2 100644 --- a/servers/physics_3d/area_3d_sw.h +++ b/servers/physics_3d/area_3d_sw.h @@ -34,11 +34,9 @@ #include "collision_object_3d_sw.h" #include "core/templates/self_list.h" #include "servers/physics_server_3d.h" -//#include "servers/physics_3d/query_sw.h" class Space3DSW; class Body3DSW; -class Constraint3DSW; class Area3DSW : public CollisionObject3DSW { PhysicsServer3D::AreaSpaceOverrideMode space_override_mode; @@ -94,18 +92,10 @@ class Area3DSW : public CollisionObject3DSW { Map monitored_bodies; Map monitored_areas; - //virtual void shape_changed_notify(ShapeSW *p_shape); - //virtual void shape_deleted_notify(ShapeSW *p_shape); - - Set constraints; - virtual void _shapes_changed(); void _queue_monitor_update(); public: - //_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; } - //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } - void set_monitor_callback(ObjectID p_id, const StringName &p_method); _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); } @@ -148,11 +138,6 @@ class Area3DSW : public CollisionObject3DSW { _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ int get_priority() const { return priority; } - _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } - _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } - void set_monitorable(bool p_monitorable); _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index 2073df7dee4c..0343989ac827 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -89,7 +89,7 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, area = p_area; body_shape = p_body_shape; area_shape = p_area_shape; - body->add_constraint(this, 0); + body->add_constraint(this); area->add_constraint(this); if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { p_body->set_active(true); diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index 4357c474e468..5562e9765c7d 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -644,22 +644,23 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) { */ void Body3DSW::wakeup_neighbours() { - for (Map::Element *E = constraint_map.front(); E; E = E->next()) { - const Constraint3DSW *c = E->key(); - Body3DSW **n = c->get_body_ptr(); - int bc = c->get_body_count(); - - for (int i = 0; i < bc; i++) { - if (i == E->get()) { + for (const CollisionObject3DSW::T_ConstraintMap::Element *E = get_constraint_map().front(); E; E = E->next()) { + const Constraint3DSW *constraint = E->get(); + + Body3DSW **bodies = constraint->get_body_ptr(); + int body_count = constraint->get_body_count(); + for (int i = 0; i < body_count; i++) { + Body3DSW *body = bodies[i]; + if (body == this) { continue; } - Body3DSW *b = n[i]; - if (b->mode != PhysicsServer3D::BODY_MODE_RIGID) { + + if (body->mode != PhysicsServer3D::BODY_MODE_RIGID) { continue; } - if (!b->is_active()) { - b->set_active(true); + if (!body->is_active()) { + body->set_active(true); } } } diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index 9afb8cd56f95..d152cd16c8f0 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -35,8 +35,6 @@ #include "collision_object_3d_sw.h" #include "core/templates/vset.h" -class Constraint3DSW; - class Body3DSW : public CollisionObject3DSW { PhysicsServer3D::BodyMode mode; @@ -95,8 +93,6 @@ class Body3DSW : public CollisionObject3DSW { virtual void _shapes_changed(); Transform new_transform; - Map constraint_map; - struct AreaCMP { Area3DSW *area; int refCount; @@ -186,11 +182,6 @@ class Body3DSW : public CollisionObject3DSW { _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } - _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint); } - const Map &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; } _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index cdb3da665e22..db2ca7e50564 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -517,8 +517,8 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh shape_A = p_shape_A; shape_B = p_shape_B; space = A->get_space(); - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); } BodyPair3DSW::~BodyPair3DSW() { @@ -849,7 +849,7 @@ BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBod soft_body = p_B; body_shape = p_shape_A; space = p_A->get_space(); - body->add_constraint(this, 0); + body->add_constraint(this); soft_body->add_constraint(this); } diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h index 85221b774680..67c15d38193b 100644 --- a/servers/physics_3d/collision_object_3d_sw.h +++ b/servers/physics_3d/collision_object_3d_sw.h @@ -32,6 +32,8 @@ #define COLLISION_OBJECT_SW_H #include "broad_phase_3d_sw.h" +#include "constraint_3d_sw.h" +#include "core/templates/map.h" #include "core/templates/self_list.h" #include "servers/physics_server_3d.h" #include "shape_3d_sw.h" @@ -52,6 +54,8 @@ class CollisionObject3DSW : public ShapeOwner3DSW { TYPE_SOFT_BODY, }; + typedef Map T_ConstraintMap; + private: Type type; RID self; @@ -77,6 +81,8 @@ class CollisionObject3DSW : public ShapeOwner3DSW { Transform inv_transform; bool _static; + T_ConstraintMap constraint_map; + SelfList pending_shape_update_list; void _update_shapes(); @@ -162,6 +168,11 @@ class CollisionObject3DSW : public ShapeOwner3DSW { void remove_shape(Shape3DSW *p_shape); void remove_shape(int p_index); + _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraint_map.insert(p_constraint->get_constraint_id(), p_constraint); } + _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint->get_constraint_id()); } + const T_ConstraintMap &get_constraint_map() const { return constraint_map; } + _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } + virtual void set_space(Space3DSW *p_space) = 0; _FORCE_INLINE_ bool is_static() const { return _static; } diff --git a/servers/physics_3d/constraint_3d_sw.cpp b/servers/physics_3d/constraint_3d_sw.cpp new file mode 100644 index 000000000000..0681b4901033 --- /dev/null +++ b/servers/physics_3d/constraint_3d_sw.cpp @@ -0,0 +1,33 @@ +/*************************************************************************/ +/* constraint_3d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "constraint_3d_sw.h" + +SafeNumeric Constraint3DSW::constraint_id_counter; diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h index 7b44726ef595..dc0355d46a03 100644 --- a/servers/physics_3d/constraint_3d_sw.h +++ b/servers/physics_3d/constraint_3d_sw.h @@ -31,6 +31,10 @@ #ifndef CONSTRAINT_SW_H #define CONSTRAINT_SW_H +#include "core/math/math_defs.h" +#include "core/templates/rid.h" +#include "core/templates/safe_refcount.h" + class Body3DSW; class SoftBody3DSW; @@ -43,6 +47,9 @@ class Constraint3DSW { RID self; + static SafeNumeric constraint_id_counter; + uint32_t constraint_id = 0; + protected: Constraint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) { _body_ptr = p_body_ptr; @@ -50,12 +57,15 @@ class Constraint3DSW { island_step = 0; priority = 1; disabled_collisions_between_bodies = true; + constraint_id = constraint_id_counter.increment(); } public: _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } _FORCE_INLINE_ RID get_self() const { return self; } + _FORCE_INLINE_ uint32_t get_constraint_id() const { return constraint_id; } + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp index e9efddf1654a..6cfc5e48a8fa 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -102,8 +102,8 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans m_solveTwistLimit = false; m_solveSwingLimit = false; - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); m_appliedImpulse = 0; } diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp index 7c504764a747..714441ed8329 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -226,8 +226,8 @@ Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const T m_useLinearReferenceFrameA(useLinearReferenceFrameA) { A = rbA; B = rbB; - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); } void Generic6DOFJoint3DSW::calculateAngleInfo() { diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index bb8858c28ada..64fca67389bc 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -94,8 +94,8 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &fr m_angularOnly = false; m_enableAngularMotor = false; - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); } HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, @@ -150,8 +150,8 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo m_angularOnly = false; m_enableAngularMotor = false; - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); } bool HingeJoint3DSW::setup(real_t p_step) { diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index 8eb84d1c2fd1..b5911d935c85 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -176,8 +176,8 @@ PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW m_impulseClamp = 0; m_appliedImpulse = 0; - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); } PinJoint3DSW::~PinJoint3DSW() { diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 8bd195131103..6d82d899710d 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -118,8 +118,8 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform & A = rbA; B = rbB; - A->add_constraint(this, 0); - B->add_constraint(this, 1); + A->add_constraint(this); + B->add_constraint(this); initParams(); } // SliderJointSW::SliderJointSW() diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index c08e2b579468..3d92b8955c84 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -234,7 +234,7 @@ void PhysicsServer3DSW::area_set_space(RID p_area, RID p_space) { return; //pointless } - area->clear_constraints(); + area->clear_constraint_map(); area->set_space(space); }; diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h index 98e554218b85..ff21b517e7a6 100644 --- a/servers/physics_3d/soft_body_3d_sw.h +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -37,12 +37,9 @@ #include "core/math/dynamic_bvh.h" #include "core/math/vector3.h" #include "core/templates/local_vector.h" -#include "core/templates/set.h" #include "core/templates/vset.h" #include "scene/resources/mesh.h" -class Constraint3DSW; - class SoftBody3DSW : public CollisionObject3DSW { Ref soft_mesh; @@ -102,8 +99,6 @@ class SoftBody3DSW : public CollisionObject3DSW { SelfList active_list; - Set constraints; - VSet exceptions; uint64_t island_step = 0; @@ -116,11 +111,6 @@ class SoftBody3DSW : public CollisionObject3DSW { void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); Variant get_state(PhysicsServer3D::BodyState p_state) const; - _FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } - _FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } - _FORCE_INLINE_ const Set &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); } _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index ba18bac61123..da6b30bd0df1 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -47,8 +47,8 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector &p_bod p_body_island.push_back(p_body); } - for (Map::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - Constraint3DSW *constraint = (Constraint3DSW *)E->key(); + for (const CollisionObject3DSW::T_ConstraintMap::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { + Constraint3DSW *constraint = E->get(); if (constraint->get_island_step() == _step) { continue; // Already processed. } @@ -58,26 +58,31 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector &p_bod all_constraints.push_back(constraint); // Find connected rigid bodies. - for (int i = 0; i < constraint->get_body_count(); i++) { - if (i == E->get()) { - continue; - } - Body3DSW *other_body = constraint->get_body_ptr()[i]; + Body3DSW **bodies = constraint->get_body_ptr(); + int body_count = constraint->get_body_count(); + for (int i = 0; i < body_count; i++) { + Body3DSW *other_body = bodies[i]; + if (other_body->get_island_step() == _step) { - continue; // Already processed. + continue; // Already processed (can be this body). } + if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { continue; // Static bodies don't connect islands. } + _populate_island(other_body, p_body_island, p_constraint_island); } // Find connected soft bodies. - for (int i = 0; i < constraint->get_soft_body_count(); i++) { + int soft_body_count = constraint->get_soft_body_count(); + for (int i = 0; i < soft_body_count; i++) { SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i); + if (soft_body->get_island_step() == _step) { continue; // Already processed. } + _populate_island_soft_body(soft_body, p_body_island, p_constraint_island); } } @@ -86,8 +91,8 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector &p_bod void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector &p_body_island, LocalVector &p_constraint_island) { p_soft_body->set_island_step(_step); - for (Set::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) { - Constraint3DSW *constraint = (Constraint3DSW *)E->get(); + for (const CollisionObject3DSW::T_ConstraintMap::Element *E = p_soft_body->get_constraint_map().front(); E; E = E->next()) { + Constraint3DSW *constraint = E->get(); if (constraint->get_island_step() == _step) { continue; // Already processed. } @@ -97,14 +102,19 @@ void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector all_constraints.push_back(constraint); // Find connected rigid bodies. - for (int i = 0; i < constraint->get_body_count(); i++) { - Body3DSW *body = constraint->get_body_ptr()[i]; + Body3DSW **bodies = constraint->get_body_ptr(); + int body_count = constraint->get_body_count(); + for (int i = 0; i < body_count; i++) { + Body3DSW *body = bodies[i]; + if (body->get_island_step() == _step) { continue; // Already processed. } + if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { continue; // Static bodies don't connect islands. } + _populate_island(body, p_body_island, p_constraint_island); } } @@ -230,7 +240,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { const SelfList::List &aml = p_space->get_moved_area_list(); while (aml.first()) { - for (const Set::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + for (const CollisionObject3DSW::T_ConstraintMap::Element *E = aml.first()->self()->get_constraint_map().front(); E; E = E->next()) { Constraint3DSW *constraint = E->get(); if (constraint->get_island_step() == _step) { continue;