From 9cc2a732cbc6656d727d2bc0bea2ba8bcbd04162 Mon Sep 17 00:00:00 2001 From: Andrea Catania Date: Sun, 21 Jun 2020 12:48:40 +0200 Subject: [PATCH] Optimized physics object spawn time and some other parts of the bullet server. - Optimized physics object spawn time, by integrating lazy body reload. - Optimized shape usage when the shape is not scaled. - Make sure to correctly fetch gravity when the integrate_forces function is used - Added override keyword - Using LocalVector --- modules/bullet/area_bullet.cpp | 27 ++-- modules/bullet/area_bullet.h | 22 ++-- modules/bullet/bullet_physics_server.h | 2 +- modules/bullet/collision_object_bullet.cpp | 104 +++++++++++---- modules/bullet/collision_object_bullet.h | 62 +++++---- modules/bullet/rigid_body_bullet.cpp | 121 +++++++++-------- modules/bullet/rigid_body_bullet.h | 53 ++++---- modules/bullet/shape_bullet.cpp | 58 +++++++-- modules/bullet/shape_bullet.h | 28 ++-- modules/bullet/soft_body_bullet.cpp | 24 ++-- modules/bullet/soft_body_bullet.h | 20 ++- modules/bullet/space_bullet.cpp | 145 ++++++++++++++++++--- modules/bullet/space_bullet.h | 23 +++- 13 files changed, 470 insertions(+), 219 deletions(-) diff --git a/modules/bullet/area_bullet.cpp b/modules/bullet/area_bullet.cpp index f8f7d79a1146..b35019bea3df 100644 --- a/modules/bullet/area_bullet.cpp +++ b/modules/bullet/area_bullet.cpp @@ -65,14 +65,11 @@ AreaBullet::~AreaBullet() { } void AreaBullet::dispatch_callbacks() { - if (!isScratched) { - return; - } - isScratched = false; + RigidCollisionObjectBullet::dispatch_callbacks(); // Reverse order because I've to remove EXIT objects for (int i = overlappingObjects.size() - 1; 0 <= i; --i) { - OverlappingObjectData &otherObj = overlappingObjects.write[i]; + OverlappingObjectData &otherObj = overlappingObjects[i]; switch (otherObj.state) { case OVERLAP_STATE_ENTER: @@ -112,10 +109,9 @@ void AreaBullet::call_event(CollisionObjectBullet *p_otherObject, PhysicsServer3 } void AreaBullet::scratch() { - if (isScratched) { - return; + if (space != nullptr) { + space->add_to_pre_flush_queue(this); } - isScratched = true; } void AreaBullet::clear_overlaps(bool p_notify) { @@ -164,7 +160,7 @@ void AreaBullet::main_shape_changed() { btGhost->setCollisionShape(get_main_shape()); } -void AreaBullet::reload_body() { +void AreaBullet::do_reload_body() { if (space) { space->remove_area(this); space->add_area(this); @@ -173,22 +169,25 @@ void AreaBullet::reload_body() { void AreaBullet::set_space(SpaceBullet *p_space) { // Clear the old space if there is one + if (space) { clear_overlaps(false); - isScratched = false; // Remove this object form the physics world + space->unregister_collision_object(this); space->remove_area(this); } space = p_space; if (space) { - space->add_area(this); + space->register_collision_object(this); + reload_body(); + scratch(); } } -void AreaBullet::on_collision_filters_change() { +void AreaBullet::do_reload_collision_filters() { if (space) { space->reload_collision_filters(this); } @@ -202,13 +201,13 @@ void AreaBullet::add_overlap(CollisionObjectBullet *p_otherObject) { void AreaBullet::put_overlap_as_exit(int p_index) { scratch(); - overlappingObjects.write[p_index].state = OVERLAP_STATE_EXIT; + overlappingObjects[p_index].state = OVERLAP_STATE_EXIT; } void AreaBullet::put_overlap_as_inside(int p_index) { // This check is required to be sure this body was inside if (OVERLAP_STATE_DIRTY == overlappingObjects[p_index].state) { - overlappingObjects.write[p_index].state = OVERLAP_STATE_INSIDE; + overlappingObjects[p_index].state = OVERLAP_STATE_INSIDE; } } diff --git a/modules/bullet/area_bullet.h b/modules/bullet/area_bullet.h index 152fd785c032..a95becc7ea51 100644 --- a/modules/bullet/area_bullet.h +++ b/modules/bullet/area_bullet.h @@ -83,7 +83,7 @@ class AreaBullet : public RigidCollisionObjectBullet { Variant *call_event_res_ptr[5]; btGhostObject *btGhost; - Vector overlappingObjects; + LocalVector overlappingObjects; bool monitorable = true; PhysicsServer3D::AreaSpaceOverrideMode spOv_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; @@ -96,8 +96,6 @@ class AreaBullet : public RigidCollisionObjectBullet { real_t spOv_angularDump = 0.1; int spOv_priority = 0; - bool isScratched = false; - InOutEventCallback eventsCallbacks[2]; public: @@ -139,11 +137,11 @@ class AreaBullet : public RigidCollisionObjectBullet { _FORCE_INLINE_ void set_spOv_priority(int p_priority) { spOv_priority = p_priority; } _FORCE_INLINE_ int get_spOv_priority() { return spOv_priority; } - virtual void main_shape_changed(); - virtual void reload_body(); - virtual void set_space(SpaceBullet *p_space); + virtual void main_shape_changed() override; + virtual void do_reload_body() override; + virtual void set_space(SpaceBullet *p_space) override; - virtual void dispatch_callbacks(); + virtual void dispatch_callbacks() override; void call_event(CollisionObjectBullet *p_otherObject, PhysicsServer3D::AreaBodyStatus p_status); void set_on_state_change(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); void scratch(); @@ -152,9 +150,9 @@ class AreaBullet : public RigidCollisionObjectBullet { // Dispatch the callbacks and removes from overlapping list void remove_overlap(CollisionObjectBullet *p_object, bool p_notify); - virtual void on_collision_filters_change(); - virtual void on_collision_checker_start() {} - virtual void on_collision_checker_end() { isTransformChanged = false; } + virtual void do_reload_collision_filters() override; + virtual void on_collision_checker_start() override {} + virtual void on_collision_checker_end() override { isTransformChanged = false; } void add_overlap(CollisionObjectBullet *p_otherObject); void put_overlap_as_exit(int p_index); @@ -166,8 +164,8 @@ class AreaBullet : public RigidCollisionObjectBullet { void set_event_callback(Type p_callbackObjectType, ObjectID p_id, const StringName &p_method); bool has_event_callback(Type p_callbackObjectType); - virtual void on_enter_area(AreaBullet *p_area); - virtual void on_exit_area(AreaBullet *p_area); + virtual void on_enter_area(AreaBullet *p_area) override; + virtual void on_exit_area(AreaBullet *p_area) override; }; #endif diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h index dca9339c448c..da246903c4c8 100644 --- a/modules/bullet/bullet_physics_server.h +++ b/modules/bullet/bullet_physics_server.h @@ -52,7 +52,7 @@ class BulletPhysicsServer3D : public PhysicsServer3D { bool active = true; char active_spaces_count = 0; - Vector active_spaces; + LocalVector active_spaces; mutable RID_PtrOwner space_owner; mutable RID_PtrOwner shape_owner; diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp index a3158a15e51e..3f5192376e49 100644 --- a/modules/bullet/collision_object_bullet.cpp +++ b/modules/bullet/collision_object_bullet.cpp @@ -79,7 +79,7 @@ btTransform CollisionObjectBullet::ShapeWrapper::get_adjusted_transform() const } void CollisionObjectBullet::ShapeWrapper::claim_bt_shape(const btVector3 &body_scale) { - if (!bt_shape) { + if (bt_shape == nullptr) { if (active) { bt_shape = shape->create_bt_shape(scale * body_scale); } else { @@ -88,6 +88,13 @@ void CollisionObjectBullet::ShapeWrapper::claim_bt_shape(const btVector3 &body_s } } +void CollisionObjectBullet::ShapeWrapper::release_bt_shape() { + if (bt_shape != nullptr) { + shape->destroy_bt_shape(bt_shape); + bt_shape = nullptr; + } +} + CollisionObjectBullet::CollisionObjectBullet(Type p_type) : RIDBullet(), type(p_type) {} @@ -158,6 +165,47 @@ bool CollisionObjectBullet::has_collision_exception(const CollisionObjectBullet return !bt_collision_object->checkCollideWith(p_otherCollisionObject->bt_collision_object); } +void CollisionObjectBullet::set_collision_layer(uint32_t p_layer) { + if (collisionLayer != p_layer) { + collisionLayer = p_layer; + needs_collision_filters_reload = true; + if (likely(space)) { + space->add_to_dirty_queue(this); + } + } +} + +void CollisionObjectBullet::set_collision_mask(uint32_t p_mask) { + if (collisionMask != p_mask) { + collisionMask = p_mask; + needs_collision_filters_reload = true; + if (likely(space)) { + space->add_to_dirty_queue(this); + } + } +} + +void CollisionObjectBullet::reload_body() { + needs_body_reload = true; + if (likely(space)) { + space->add_to_dirty_queue(this); + } +} + +void CollisionObjectBullet::dispatch_callbacks() {} + +void CollisionObjectBullet::flush_dirty() { + if (needs_body_reload) { + do_reload_body(); + } else if (needs_collision_filters_reload) { + do_reload_collision_filters(); + } + needs_body_reload = false; + needs_collision_filters_reload = false; +} + +void CollisionObjectBullet::pre_process() {} + void CollisionObjectBullet::set_collision_enabled(bool p_enabled) { collisionsEnabled = p_enabled; if (collisionsEnabled) { @@ -231,7 +279,7 @@ void RigidCollisionObjectBullet::add_shape(ShapeBullet *p_shape, const Transform } void RigidCollisionObjectBullet::set_shape(int p_index, ShapeBullet *p_shape) { - ShapeWrapper &shp = shapes.write[p_index]; + ShapeWrapper &shp = shapes[p_index]; shp.shape->remove_owner(this); p_shape->add_owner(this); shp.shape = p_shape; @@ -293,7 +341,7 @@ void RigidCollisionObjectBullet::remove_all_shapes(bool p_permanentlyFromThisBod void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform &p_transform) { ERR_FAIL_INDEX(p_index, get_shape_count()); - shapes.write[p_index].set_transform(p_transform); + shapes[p_index].set_transform(p_transform); shape_changed(p_index); } @@ -311,7 +359,7 @@ void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled if (shapes[p_index].active != p_disabled) { return; } - shapes.write[p_index].active = !p_disabled; + shapes[p_index].active = !p_disabled; shape_changed(p_index); } @@ -319,16 +367,31 @@ bool RigidCollisionObjectBullet::is_shape_disabled(int p_index) { return !shapes[p_index].active; } +void RigidCollisionObjectBullet::flush_dirty() { + if (need_shape_reload) { + do_reload_shapes(); + need_shape_reload = false; + } + CollisionObjectBullet::flush_dirty(); +} + void RigidCollisionObjectBullet::shape_changed(int p_shape_index) { - ShapeWrapper &shp = shapes.write[p_shape_index]; + ShapeWrapper &shp = shapes[p_shape_index]; if (shp.bt_shape == mainShape) { mainShape = nullptr; } - bulletdelete(shp.bt_shape); + shp.release_bt_shape(); reload_shapes(); } void RigidCollisionObjectBullet::reload_shapes() { + need_shape_reload = true; + if (likely(space)) { + space->add_to_dirty_queue(this); + } +} + +void RigidCollisionObjectBullet::do_reload_shapes() { if (mainShape && mainShape->isCompound()) { // Destroy compound bulletdelete(mainShape); @@ -336,41 +399,38 @@ void RigidCollisionObjectBullet::reload_shapes() { mainShape = nullptr; - ShapeWrapper *shpWrapper; const int shape_count = shapes.size(); - // Reset shape if required + // Reset all shapes if required if (force_shape_reset) { for (int i(0); i < shape_count; ++i) { - shpWrapper = &shapes.write[i]; - bulletdelete(shpWrapper->bt_shape); + shapes[i].release_bt_shape(); } force_shape_reset = false; } const btVector3 body_scale(get_bt_body_scale()); - // Try to optimize by not using compound if (1 == shape_count) { - shpWrapper = &shapes.write[0]; - btTransform transform = shpWrapper->get_adjusted_transform(); + // Is it possible to optimize by not using compound? + btTransform transform = shapes[0].get_adjusted_transform(); if (transform.getOrigin().isZero() && transform.getBasis() == transform.getBasis().getIdentity()) { - shpWrapper->claim_bt_shape(body_scale); - mainShape = shpWrapper->bt_shape; + shapes[0].claim_bt_shape(body_scale); + mainShape = shapes[0].bt_shape; main_shape_changed(); + // Nothing more to do return; } } - // Optimization not possible use a compound shape + // Optimization not possible use a compound shape. btCompoundShape *compoundShape = bulletnew(btCompoundShape(enableDynamicAabbTree, shape_count)); for (int i(0); i < shape_count; ++i) { - shpWrapper = &shapes.write[i]; - shpWrapper->claim_bt_shape(body_scale); - btTransform scaled_shape_transform(shpWrapper->get_adjusted_transform()); + shapes[i].claim_bt_shape(body_scale); + btTransform scaled_shape_transform(shapes[i].get_adjusted_transform()); scaled_shape_transform.getOrigin() *= body_scale; - compoundShape->addChildShape(scaled_shape_transform, shpWrapper->bt_shape); + compoundShape->addChildShape(scaled_shape_transform, shapes[i].bt_shape); } compoundShape->recalculateLocalAabb(); @@ -384,10 +444,10 @@ void RigidCollisionObjectBullet::body_scale_changed() { } void RigidCollisionObjectBullet::internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody) { - ShapeWrapper &shp = shapes.write[p_index]; + ShapeWrapper &shp = shapes[p_index]; shp.shape->remove_owner(this, p_permanentlyFromThisBody); if (shp.bt_shape == mainShape) { mainShape = nullptr; } - bulletdelete(shp.bt_shape); + shp.release_bt_shape(); } diff --git a/modules/bullet/collision_object_bullet.h b/modules/bullet/collision_object_bullet.h index e2d05f2c38dc..f6f50dcccd22 100644 --- a/modules/bullet/collision_object_bullet.h +++ b/modules/bullet/collision_object_bullet.h @@ -34,6 +34,7 @@ #include "core/math/transform.h" #include "core/math/vector3.h" #include "core/object/class_db.h" +#include "core/templates/local_vector.h" #include "core/templates/vset.h" #include "shape_owner_bullet.h" @@ -70,11 +71,12 @@ class CollisionObjectBullet : public RIDBullet { struct ShapeWrapper { ShapeBullet *shape = nullptr; - btCollisionShape *bt_shape = nullptr; btTransform transform; btVector3 scale; bool active = true; + btCollisionShape *bt_shape = nullptr; + public: ShapeWrapper() {} ShapeWrapper(ShapeBullet *p_shape, const btTransform &p_transform, bool p_active) : @@ -107,6 +109,7 @@ class CollisionObjectBullet : public RIDBullet { btTransform get_adjusted_transform() const; void claim_bt_shape(const btVector3 &body_scale); + void release_bt_shape(); }; protected: @@ -124,12 +127,20 @@ class CollisionObjectBullet : public RIDBullet { VSet exceptions; + bool needs_body_reload = false; + bool needs_collision_filters_reload = false; + /// This array is used to know all areas where this Object is overlapped in /// New area is added when overlap with new area (AreaBullet::addOverlap), then is removed when it exit (CollisionObjectBullet::onExitArea) /// This array is used mainly to know which area hold the pointer of this object - Vector areasOverlapped; + LocalVector areasOverlapped; bool isTransformChanged = false; +public: + bool is_in_world = false; + bool is_in_flush_queue = false; + bool is_in_dirty_queue = false; + public: CollisionObjectBullet(Type p_type); virtual ~CollisionObjectBullet(); @@ -161,36 +172,34 @@ class CollisionObjectBullet : public RIDBullet { bool has_collision_exception(const CollisionObjectBullet *p_otherCollisionObject) const; _FORCE_INLINE_ const VSet &get_exceptions() const { return exceptions; } - _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { - if (collisionLayer != p_layer) { - collisionLayer = p_layer; - on_collision_filters_change(); - } - } + void set_collision_layer(uint32_t p_layer); _FORCE_INLINE_ uint32_t get_collision_layer() const { return collisionLayer; } - _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { - if (collisionMask != p_mask) { - collisionMask = p_mask; - on_collision_filters_change(); - } - } + void set_collision_mask(uint32_t p_mask); _FORCE_INLINE_ uint32_t get_collision_mask() const { return collisionMask; } - virtual void on_collision_filters_change() = 0; + virtual void do_reload_collision_filters() = 0; _FORCE_INLINE_ bool test_collision_mask(CollisionObjectBullet *p_other) const { return collisionLayer & p_other->collisionMask || p_other->collisionLayer & collisionMask; } - virtual void reload_body() = 0; + bool need_reload_body() const { + return needs_body_reload; + } + + void reload_body(); + + virtual void do_reload_body() = 0; virtual void set_space(SpaceBullet *p_space) = 0; _FORCE_INLINE_ SpaceBullet *get_space() const { return space; } virtual void on_collision_checker_start() = 0; virtual void on_collision_checker_end() = 0; - virtual void dispatch_callbacks() = 0; + virtual void dispatch_callbacks(); + virtual void flush_dirty(); + virtual void pre_process(); void set_collision_enabled(bool p_enabled); bool is_collisions_response_enabled(); @@ -214,14 +223,15 @@ class CollisionObjectBullet : public RIDBullet { class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwnerBullet { protected: btCollisionShape *mainShape = nullptr; - Vector shapes; + LocalVector shapes; + bool need_shape_reload = false; public: RigidCollisionObjectBullet(Type p_type) : CollisionObjectBullet(p_type) {} ~RigidCollisionObjectBullet(); - _FORCE_INLINE_ const Vector &get_shapes_wrappers() const { return shapes; } + _FORCE_INLINE_ const LocalVector &get_shapes_wrappers() const { return shapes; } _FORCE_INLINE_ btCollisionShape *get_main_shape() const { return mainShape; } @@ -232,9 +242,9 @@ class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwn ShapeBullet *get_shape(int p_index) const; btCollisionShape *get_bt_shape(int p_index) const; - int find_shape(ShapeBullet *p_shape) const; + virtual int find_shape(ShapeBullet *p_shape) const override; - virtual void remove_shape_full(ShapeBullet *p_shape); + virtual void remove_shape_full(ShapeBullet *p_shape) override; void remove_shape_full(int p_index); void remove_all_shapes(bool p_permanentlyFromThisBody = false, bool p_force_not_reload = false); @@ -246,11 +256,15 @@ class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwn void set_shape_disabled(int p_index, bool p_disabled); bool is_shape_disabled(int p_index); - virtual void shape_changed(int p_shape_index); - virtual void reload_shapes(); + virtual void flush_dirty() override; + + virtual void shape_changed(int p_shape_index) override; + virtual void reload_shapes() override; + bool need_reload_shapes() const { return need_shape_reload; } + virtual void do_reload_shapes(); virtual void main_shape_changed() = 0; - virtual void body_scale_changed(); + virtual void body_scale_changed() override; private: void internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody = false); diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp index 0c64c3640fd7..a37d29bf9221 100644 --- a/modules/bullet/rigid_body_bullet.cpp +++ b/modules/bullet/rigid_body_bullet.cpp @@ -51,9 +51,7 @@ BulletPhysicsDirectBodyState3D *BulletPhysicsDirectBodyState3D::singleton = nullptr; Vector3 BulletPhysicsDirectBodyState3D::get_total_gravity() const { - Vector3 gVec; - B_TO_G(body->btBody->getGravity(), gVec); - return gVec; + return body->total_gravity; } float BulletPhysicsDirectBodyState3D::get_total_angular_damp() const { @@ -183,7 +181,7 @@ int BulletPhysicsDirectBodyState3D::get_contact_collider_shape(int p_contact_idx } Vector3 BulletPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const { - RigidBodyBullet::CollisionData &colDat = body->collisions.write[p_contact_idx]; + RigidBodyBullet::CollisionData &colDat = body->collisions[p_contact_idx]; btVector3 hitLocation; G_TO_B(colDat.hitLocalLocation, hitLocation); @@ -213,7 +211,7 @@ void RigidBodyBullet::KinematicUtilities::setSafeMargin(btScalar p_margin) { } void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() { - const Vector &shapes_wrappers(owner->get_shapes_wrappers()); + const LocalVector &shapes_wrappers(owner->get_shapes_wrappers()); const int shapes_count = shapes_wrappers.size(); just_delete_shapes(shapes_count); @@ -228,8 +226,8 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() { continue; } - shapes.write[i].transform = shape_wrapper->transform; - shapes.write[i].transform.getOrigin() *= owner_scale; + shapes[i].transform = shape_wrapper->transform; + shapes[i].transform.getOrigin() *= owner_scale; switch (shape_wrapper->shape->get_type()) { case PhysicsServer3D::SHAPE_SPHERE: case PhysicsServer3D::SHAPE_BOX: @@ -237,11 +235,11 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() { case PhysicsServer3D::SHAPE_CYLINDER: case PhysicsServer3D::SHAPE_CONVEX_POLYGON: case PhysicsServer3D::SHAPE_RAY: { - shapes.write[i].shape = static_cast(shape_wrapper->shape->create_bt_shape(owner_scale * shape_wrapper->scale, safe_margin)); + shapes[i].shape = static_cast(shape_wrapper->shape->internal_create_bt_shape(owner_scale * shape_wrapper->scale, safe_margin)); } break; default: WARN_PRINT("This shape is not supported for kinematic collision."); - shapes.write[i].shape = nullptr; + shapes[i].shape = nullptr; } } } @@ -249,7 +247,7 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() { void RigidBodyBullet::KinematicUtilities::just_delete_shapes(int new_size) { for (int i = shapes.size() - 1; 0 <= i; --i) { if (shapes[i].shape) { - bulletdelete(shapes.write[i].shape); + bulletdelete(shapes[i].shape); } } shapes.resize(new_size); @@ -272,8 +270,8 @@ RigidBodyBullet::RigidBodyBullet() : reload_axis_lock(); areasWhereIam.resize(maxAreasWhereIam); - for (int i = areasWhereIam.size() - 1; 0 <= i; --i) { - areasWhereIam.write[i] = nullptr; + for (uint32_t i = 0; i < areasWhereIam.size(); i += 1) { + areasWhereIam[i] = nullptr; } btBody->setSleepingThresholds(0.2, 0.2); @@ -308,7 +306,7 @@ void RigidBodyBullet::main_shape_changed() { set_continuous_collision_detection(is_continuous_collision_detection_enabled()); // Reset } -void RigidBodyBullet::reload_body() { +void RigidBodyBullet::do_reload_body() { if (space) { space->remove_rigid_body(this); if (get_main_shape()) { @@ -327,23 +325,25 @@ void RigidBodyBullet::set_space(SpaceBullet *p_space) { assert_no_constraints(); // Remove this object form the physics world + space->unregister_collision_object(this); space->remove_rigid_body(this); } space = p_space; if (space) { - space->add_rigid_body(this); + space->register_collision_object(this); + reload_body(); + space->add_to_flush_queue(this); + space->add_to_dirty_queue(this); } } void RigidBodyBullet::dispatch_callbacks() { + RigidCollisionObjectBullet::dispatch_callbacks(); + /// The check isFirstTransformChanged is necessary in order to call integrated forces only when the first transform is sent if ((btBody->isKinematicObject() || btBody->isActive() || previousActiveState != btBody->isActive()) && force_integration_callback && can_integrate_forces) { - if (omit_forces_integration) { - btBody->clearForces(); - } - BulletPhysicsDirectBodyState3D *bodyDirect = BulletPhysicsDirectBodyState3D::get_singleton(this); Variant variantBodyDirect = bodyDirect; @@ -361,16 +361,24 @@ void RigidBodyBullet::dispatch_callbacks() { } } + previousActiveState = btBody->isActive(); +} + +void RigidBodyBullet::flush_dirty() { + RigidCollisionObjectBullet::flush_dirty(); + if (isScratchedSpaceOverrideModificator || 0 < countGravityPointSpaces) { isScratchedSpaceOverrideModificator = false; reload_space_override_modificator(); } +} - /// Lock axis - btBody->setLinearVelocity(btBody->getLinearVelocity() * btBody->getLinearFactor()); - btBody->setAngularVelocity(btBody->getAngularVelocity() * btBody->getAngularFactor()); - - previousActiveState = btBody->isActive(); +void RigidBodyBullet::pre_process() { + if (is_active()) { + /// Lock axis + btBody->setLinearVelocity(btBody->getLinearVelocity() * btBody->getLinearFactor()); + btBody->setAngularVelocity(btBody->getAngularVelocity() * btBody->getAngularFactor()); + } } void RigidBodyBullet::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) { @@ -389,9 +397,12 @@ void RigidBodyBullet::set_force_integration_callback(ObjectID p_id, const String void RigidBodyBullet::scratch_space_override_modificator() { isScratchedSpaceOverrideModificator = true; + if (likely(space)) { + space->add_to_dirty_queue(this); + } } -void RigidBodyBullet::on_collision_filters_change() { +void RigidBodyBullet::do_reload_collision_filters() { if (space) { space->reload_collision_filters(this); } @@ -404,14 +415,15 @@ void RigidBodyBullet::on_collision_checker_start() { collisionsCount = 0; // Swap array - Vector *s = prev_collision_traces; - prev_collision_traces = curr_collision_traces; - curr_collision_traces = s; + SWAP(prev_collision_traces, curr_collision_traces); } void RigidBodyBullet::on_collision_checker_end() { // Always true if active and not a static or kinematic body isTransformChanged = btBody->isActive() && !btBody->isStaticOrKinematicObject(); + if (isTransformChanged && space != nullptr) { + space->add_to_flush_queue(this); + } } bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, const float &p_appliedImpulse, int p_other_shape_index, int p_local_shape_index) { @@ -419,7 +431,7 @@ bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const return false; } - CollisionData &cd = collisions.write[collisionsCount]; + CollisionData &cd = collisions[collisionsCount]; cd.hitLocalLocation = p_hitLocalLocation; cd.otherObject = p_otherObject; cd.hitWorldLocation = p_hitWorldLocation; @@ -428,7 +440,7 @@ bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const cd.other_object_shape = p_other_shape_index; cd.local_shape = p_local_shape_index; - curr_collision_traces->write[collisionsCount] = p_otherObject; + (*curr_collision_traces)[collisionsCount] = p_otherObject; ++collisionsCount; return true; @@ -463,6 +475,7 @@ bool RigidBodyBullet::is_active() const { void RigidBodyBullet::set_omit_forces_integration(bool p_omit) { omit_forces_integration = p_omit; + scratch_space_override_modificator(); } void RigidBodyBullet::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) { @@ -805,8 +818,8 @@ const btTransform &RigidBodyBullet::get_transform__bullet() const { } } -void RigidBodyBullet::reload_shapes() { - RigidCollisionObjectBullet::reload_shapes(); +void RigidBodyBullet::do_reload_shapes() { + RigidCollisionObjectBullet::do_reload_shapes(); const btScalar invMass = btBody->getInvMass(); const btScalar mass = invMass == 0 ? 0 : 1 / invMass; @@ -838,15 +851,15 @@ void RigidBodyBullet::on_enter_area(AreaBullet *p_area) { for (int i = 0; i < areaWhereIamCount; ++i) { if (nullptr == areasWhereIam[i]) { // This area has the highest priority - areasWhereIam.write[i] = p_area; + areasWhereIam[i] = p_area; break; } else { if (areasWhereIam[i]->get_spOv_priority() > p_area->get_spOv_priority()) { // The position was found, just shift all elements for (int j = areaWhereIamCount; j > i; j--) { - areasWhereIam.write[j] = areasWhereIam[j - 1]; + areasWhereIam[j] = areasWhereIam[j - 1]; } - areasWhereIam.write[i] = p_area; + areasWhereIam[i] = p_area; break; } } @@ -870,7 +883,7 @@ void RigidBodyBullet::on_exit_area(AreaBullet *p_area) { if (p_area == areasWhereIam[i]) { // The area was found, just shift down all elements for (int j = i; j < areaWhereIamCount; ++j) { - areasWhereIam.write[j] = areasWhereIam[j + 1]; + areasWhereIam[j] = areasWhereIam[j + 1]; } wasTheAreaFound = true; break; @@ -883,7 +896,7 @@ void RigidBodyBullet::on_exit_area(AreaBullet *p_area) { } --areaWhereIamCount; - areasWhereIam.write[areaWhereIamCount] = nullptr; // Even if this is not required, I clear the last element to be safe + areasWhereIam[areaWhereIamCount] = nullptr; // Even if this is not required, I clear the last element to be safe if (PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED != p_area->get_spOv_mode()) { scratch_space_override_modificator(); } @@ -895,36 +908,31 @@ void RigidBodyBullet::reload_space_override_modificator() { return; } - Vector3 newGravity(0.0, 0.0, 0.0); + Vector3 newGravity; real_t newLinearDamp = MAX(0.0, linearDamp); real_t newAngularDamp = MAX(0.0, angularDamp); - AreaBullet *currentArea; - // Variable used to calculate new gravity for gravity point areas, it is pointed by currentGravity pointer - Vector3 support_gravity(0, 0, 0); - bool stopped = false; - for (int i = areaWhereIamCount - 1; (0 <= i) && !stopped; --i) { - currentArea = areasWhereIam[i]; + for (int i = 0; i < areaWhereIamCount && !stopped; i += 1) { + AreaBullet *currentArea = areasWhereIam[i]; if (!currentArea || PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED == currentArea->get_spOv_mode()) { continue; } + Vector3 support_gravity; + /// Here is calculated the gravity if (currentArea->is_spOv_gravityPoint()) { /// It calculates the direction of new gravity support_gravity = currentArea->get_transform().xform(currentArea->get_spOv_gravityVec()) - get_transform().get_origin(); - real_t distanceMag = support_gravity.length(); + + const real_t distanceMag = support_gravity.length(); // Normalized in this way to avoid the double call of function "length()" if (distanceMag == 0) { - support_gravity.x = 0; - support_gravity.y = 0; - support_gravity.z = 0; + support_gravity = Vector3(); } else { - support_gravity.x /= distanceMag; - support_gravity.y /= distanceMag; - support_gravity.z /= distanceMag; + support_gravity /= distanceMag; } /// Here is calculated the final gravity @@ -986,10 +994,17 @@ void RigidBodyBullet::reload_space_override_modificator() { newAngularDamp += space->get_angular_damp(); } - btVector3 newBtGravity; - G_TO_B(newGravity * gravity_scale, newBtGravity); + total_gravity = newGravity; + + if (omit_forces_integration) { + // Custom behaviour. + btBody->setGravity(btVector3(0, 0, 0)); + } else { + btVector3 newBtGravity; + G_TO_B(newGravity * gravity_scale, newBtGravity); + btBody->setGravity(newBtGravity); + } - btBody->setGravity(newBtGravity); btBody->setDamping(newLinearDamp, newAngularDamp); } diff --git a/modules/bullet/rigid_body_bullet.h b/modules/bullet/rigid_body_bullet.h index c64361139720..762e5003201b 100644 --- a/modules/bullet/rigid_body_bullet.h +++ b/modules/bullet/rigid_body_bullet.h @@ -171,7 +171,7 @@ class RigidBodyBullet : public RigidCollisionObjectBullet { struct KinematicUtilities { RigidBodyBullet *owner; btScalar safe_margin; - Vector shapes; + LocalVector shapes; KinematicUtilities(RigidBodyBullet *p_owner); ~KinematicUtilities(); @@ -193,6 +193,7 @@ class RigidBodyBullet : public RigidCollisionObjectBullet { PhysicsServer3D::BodyMode mode; GodotMotionState *godotMotionState; btRigidBody *btBody; + Vector3 total_gravity; uint16_t locked_axis = 0; real_t mass = 1; real_t gravity_scale = 1; @@ -202,18 +203,18 @@ class RigidBodyBullet : public RigidCollisionObjectBullet { bool omit_forces_integration = false; bool can_integrate_forces = false; - Vector collisions; - Vector collision_traces_1; - Vector collision_traces_2; - Vector *prev_collision_traces; - Vector *curr_collision_traces; + LocalVector collisions; + LocalVector collision_traces_1; + LocalVector collision_traces_2; + LocalVector *prev_collision_traces; + LocalVector *curr_collision_traces; // these parameters are used to avoid vector resize - int maxCollisionsDetection = 0; - int collisionsCount = 0; - int prev_collision_count = 0; + uint32_t maxCollisionsDetection = 0; + uint32_t collisionsCount = 0; + uint32_t prev_collision_count = 0; - Vector areasWhereIam; + LocalVector areasWhereIam; // these parameters are used to avoid vector resize int maxAreasWhereIam = 10; int areaWhereIamCount = 0; @@ -235,21 +236,21 @@ class RigidBodyBullet : public RigidCollisionObjectBullet { _FORCE_INLINE_ btRigidBody *get_bt_rigid_body() { return btBody; } - virtual void main_shape_changed(); - virtual void reload_body(); - virtual void set_space(SpaceBullet *p_space); + virtual void main_shape_changed() override; + virtual void do_reload_body() override; + virtual void set_space(SpaceBullet *p_space) override; - virtual void dispatch_callbacks(); + virtual void dispatch_callbacks() override; + virtual void flush_dirty() override; + virtual void pre_process() override; void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); void scratch_space_override_modificator(); - virtual void on_collision_filters_change(); - virtual void on_collision_checker_start(); - virtual void on_collision_checker_end(); - - void set_max_collisions_detection(int p_maxCollisionsDetection) { - ERR_FAIL_COND(0 > p_maxCollisionsDetection); + virtual void do_reload_collision_filters() override; + virtual void on_collision_checker_start() override; + virtual void on_collision_checker_end() override; + void set_max_collisions_detection(uint32_t p_maxCollisionsDetection) { maxCollisionsDetection = p_maxCollisionsDetection; collisions.resize(p_maxCollisionsDetection); @@ -312,19 +313,19 @@ class RigidBodyBullet : public RigidCollisionObjectBullet { void set_angular_velocity(const Vector3 &p_velocity); Vector3 get_angular_velocity() const; - virtual void set_transform__bullet(const btTransform &p_global_transform); - virtual const btTransform &get_transform__bullet() const; + virtual void set_transform__bullet(const btTransform &p_global_transform) override; + virtual const btTransform &get_transform__bullet() const override; - virtual void reload_shapes(); + virtual void do_reload_shapes() override; - virtual void on_enter_area(AreaBullet *p_area); - virtual void on_exit_area(AreaBullet *p_area); + virtual void on_enter_area(AreaBullet *p_area) override; + virtual void on_exit_area(AreaBullet *p_area) override; void reload_space_override_modificator(); /// Kinematic void reload_kinematic_shapes(); - virtual void notify_transform_changed(); + virtual void notify_transform_changed() override; private: void _internal_set_mass(real_t p_mass); diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp index c7b761e92ad5..29572396f134 100644 --- a/modules/bullet/shape_bullet.cpp +++ b/modules/bullet/shape_bullet.cpp @@ -46,9 +46,15 @@ @author AndreaCatania */ -ShapeBullet::ShapeBullet() {} +ShapeBullet::ShapeBullet() { +} -ShapeBullet::~ShapeBullet() {} +ShapeBullet::~ShapeBullet() { + if (default_shape != nullptr) { + bulletdelete(default_shape); + default_shape = nullptr; + } +} btCollisionShape *ShapeBullet::create_bt_shape(const Vector3 &p_implicit_scale, real_t p_extra_edge) { btVector3 s; @@ -56,6 +62,25 @@ btCollisionShape *ShapeBullet::create_bt_shape(const Vector3 &p_implicit_scale, return create_bt_shape(s, p_extra_edge); } +btCollisionShape *ShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { + if (p_extra_edge == 0.0 && (p_implicit_scale - btVector3(1, 1, 1)).length2() <= CMP_EPSILON) { + if (unlikely(default_shape == nullptr)) { + default_shape = internal_create_bt_shape(btVector3(1, 1, 1)); + } + return default_shape; + } + + return internal_create_bt_shape(p_implicit_scale, p_extra_edge); +} + +void ShapeBullet::destroy_bt_shape(btCollisionShape *p_shape) const { + if (p_shape != default_shape && p_shape != old_default_shape) { + if (likely(p_shape != nullptr)) { + bulletdelete(p_shape); + } + } +} + btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const { p_btShape->setUserPointer(const_cast(this)); p_btShape->setMargin(margin); @@ -63,10 +88,21 @@ btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const { } void ShapeBullet::notifyShapeChanged() { + // Store the old shape ptr so to not lose the reference pointer. + old_default_shape = default_shape; + // Reset the default shape, so we can create the new one if needed. + default_shape = nullptr; + for (Map::Element *E = owners.front(); E; E = E->next()) { ShapeOwnerBullet *owner = static_cast(E->key()); owner->shape_changed(owner->find_shape(this)); } + + if (old_default_shape) { + // At this point now one has the old default shape; just delete it. + bulletdelete(old_default_shape); + old_default_shape = nullptr; + } } void ShapeBullet::add_owner(ShapeOwnerBullet *p_owner) { @@ -186,7 +222,7 @@ void PlaneShapeBullet::setup(const Plane &p_plane) { notifyShapeChanged(); } -btCollisionShape *PlaneShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { +btCollisionShape *PlaneShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { btVector3 btPlaneNormal; G_TO_B(plane.normal, btPlaneNormal); return prepare(PlaneShapeBullet::create_shape_plane(btPlaneNormal, plane.d)); @@ -214,7 +250,7 @@ void SphereShapeBullet::setup(real_t p_radius) { notifyShapeChanged(); } -btCollisionShape *SphereShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { +btCollisionShape *SphereShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { return prepare(ShapeBullet::create_shape_sphere(radius * p_implicit_scale[0] + p_extra_edge)); } @@ -241,7 +277,7 @@ void BoxShapeBullet::setup(const Vector3 &p_half_extents) { notifyShapeChanged(); } -btCollisionShape *BoxShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { +btCollisionShape *BoxShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { return prepare(ShapeBullet::create_shape_box((half_extents * p_implicit_scale) + btVector3(p_extra_edge, p_extra_edge, p_extra_edge))); } @@ -274,7 +310,7 @@ void CapsuleShapeBullet::setup(real_t p_height, real_t p_radius) { notifyShapeChanged(); } -btCollisionShape *CapsuleShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { +btCollisionShape *CapsuleShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { return prepare(ShapeBullet::create_shape_capsule(radius * p_implicit_scale[0] + p_extra_edge, height * p_implicit_scale[1])); } @@ -307,7 +343,7 @@ void CylinderShapeBullet::setup(real_t p_height, real_t p_radius) { notifyShapeChanged(); } -btCollisionShape *CylinderShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) { +btCollisionShape *CylinderShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) { return prepare(ShapeBullet::create_shape_cylinder(radius * p_implicit_scale[0] + p_margin, height * p_implicit_scale[1] + p_margin)); } @@ -349,7 +385,7 @@ void ConvexPolygonShapeBullet::setup(const Vector &p_vertices) { notifyShapeChanged(); } -btCollisionShape *ConvexPolygonShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { +btCollisionShape *ConvexPolygonShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { if (!vertices.size()) { // This is necessary since 0 vertices return prepare(ShapeBullet::create_shape_empty()); @@ -431,7 +467,7 @@ void ConcavePolygonShapeBullet::setup(Vector p_faces) { notifyShapeChanged(); } -btCollisionShape *ConcavePolygonShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { +btCollisionShape *ConcavePolygonShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { btCollisionShape *cs = ShapeBullet::create_shape_concave(meshShape); if (!cs) { // This is necessary since if 0 faces the creation of concave return null @@ -558,7 +594,7 @@ void HeightMapShapeBullet::setup(Vector &p_heights, int p_width, int p_d notifyShapeChanged(); } -btCollisionShape *HeightMapShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { +btCollisionShape *HeightMapShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { btCollisionShape *cs(ShapeBullet::create_shape_height_field(heights, width, depth, min_height, max_height)); cs->setLocalScaling(p_implicit_scale); prepare(cs); @@ -591,6 +627,6 @@ void RayShapeBullet::setup(real_t p_length, bool p_slips_on_slope) { notifyShapeChanged(); } -btCollisionShape *RayShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { +btCollisionShape *RayShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) { return prepare(ShapeBullet::create_shape_ray(length * p_implicit_scale[1] + p_extra_edge, slips_on_slope)); } diff --git a/modules/bullet/shape_bullet.h b/modules/bullet/shape_bullet.h index 1c29dc1b1fe2..520da660eee4 100644 --- a/modules/bullet/shape_bullet.h +++ b/modules/bullet/shape_bullet.h @@ -53,6 +53,10 @@ class ShapeBullet : public RIDBullet { Map owners; real_t margin = 0.04; + // Contains the default shape. + btCollisionShape *default_shape = nullptr; + btCollisionShape *old_default_shape = nullptr; + protected: /// return self btCollisionShape *prepare(btCollisionShape *p_btShape) const; @@ -63,7 +67,11 @@ class ShapeBullet : public RIDBullet { virtual ~ShapeBullet(); btCollisionShape *create_bt_shape(const Vector3 &p_implicit_scale, real_t p_extra_edge = 0); - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0) = 0; + btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); + + void destroy_bt_shape(btCollisionShape *p_shape) const; + + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0) = 0; void add_owner(ShapeOwnerBullet *p_owner); void remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody = false); @@ -102,7 +110,7 @@ class PlaneShapeBullet : public ShapeBullet { virtual void set_data(const Variant &p_data); virtual Variant get_data() const; virtual PhysicsServer3D::ShapeType get_type() const; - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); private: void setup(const Plane &p_plane); @@ -118,7 +126,7 @@ class SphereShapeBullet : public ShapeBullet { virtual void set_data(const Variant &p_data); virtual Variant get_data() const; virtual PhysicsServer3D::ShapeType get_type() const; - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); private: void setup(real_t p_radius); @@ -134,7 +142,7 @@ class BoxShapeBullet : public ShapeBullet { virtual void set_data(const Variant &p_data); virtual Variant get_data() const; virtual PhysicsServer3D::ShapeType get_type() const; - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); private: void setup(const Vector3 &p_half_extents); @@ -152,7 +160,7 @@ class CapsuleShapeBullet : public ShapeBullet { virtual void set_data(const Variant &p_data); virtual Variant get_data() const; virtual PhysicsServer3D::ShapeType get_type() const; - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); private: void setup(real_t p_height, real_t p_radius); @@ -170,7 +178,7 @@ class CylinderShapeBullet : public ShapeBullet { virtual void set_data(const Variant &p_data); virtual Variant get_data() const; virtual PhysicsServer3D::ShapeType get_type() const; - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin = 0); + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin = 0); private: void setup(real_t p_height, real_t p_radius); @@ -186,7 +194,7 @@ class ConvexPolygonShapeBullet : public ShapeBullet { void get_vertices(Vector &out_vertices); virtual Variant get_data() const; virtual PhysicsServer3D::ShapeType get_type() const; - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); private: void setup(const Vector &p_vertices); @@ -204,7 +212,7 @@ class ConcavePolygonShapeBullet : public ShapeBullet { virtual void set_data(const Variant &p_data); virtual Variant get_data() const; virtual PhysicsServer3D::ShapeType get_type() const; - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); private: void setup(Vector p_faces); @@ -223,7 +231,7 @@ class HeightMapShapeBullet : public ShapeBullet { virtual void set_data(const Variant &p_data); virtual Variant get_data() const; virtual PhysicsServer3D::ShapeType get_type() const; - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); private: void setup(Vector &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); @@ -239,7 +247,7 @@ class RayShapeBullet : public ShapeBullet { virtual void set_data(const Variant &p_data); virtual Variant get_data() const; virtual PhysicsServer3D::ShapeType get_type() const; - virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); + virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0); private: void setup(real_t p_length, bool p_slips_on_slope); diff --git a/modules/bullet/soft_body_bullet.cpp b/modules/bullet/soft_body_bullet.cpp index 6794d6c313c6..ee48b3c5f0e4 100644 --- a/modules/bullet/soft_body_bullet.cpp +++ b/modules/bullet/soft_body_bullet.cpp @@ -41,7 +41,7 @@ SoftBodyBullet::SoftBodyBullet() : SoftBodyBullet::~SoftBodyBullet() { } -void SoftBodyBullet::reload_body() { +void SoftBodyBullet::do_reload_body() { if (space) { space->remove_soft_body(this); space->add_soft_body(this); @@ -51,13 +51,15 @@ void SoftBodyBullet::reload_body() { void SoftBodyBullet::set_space(SpaceBullet *p_space) { if (space) { isScratched = false; + space->unregister_collision_object(this); space->remove_soft_body(this); } space = p_space; if (space) { - space->add_soft_body(this); + space->register_collision_object(this); + reload_body(); } } @@ -344,14 +346,14 @@ void SoftBodyBullet::set_trimesh_body_shape(Vector p_indices, Vector()); } - indices_table.write[vertex_id].push_back(vs_vertex_index); + indices_table[vertex_id].push_back(vs_vertex_index); vs_indices_to_physics_table.push_back(vertex_id); } } const int indices_map_size(indices_table.size()); - Vector bt_vertices; + LocalVector bt_vertices; { // Parse vertices to bullet @@ -359,13 +361,13 @@ void SoftBodyBullet::set_trimesh_body_shape(Vector p_indices, Vector bt_triangles; + LocalVector bt_triangles; const int triangles_size(p_indices.size() / 3); { // Parse indices @@ -375,9 +377,9 @@ void SoftBodyBullet::set_trimesh_body_shape(Vector p_indices, Vector> indices_table; + LocalVector> indices_table; btSoftBody::Material *mat0; // This is just a copy of pointer managed by btSoftBody bool isScratched = false; @@ -73,7 +72,7 @@ class SoftBodyBullet : public CollisionObjectBullet { real_t pose_matching_coefficient = 0.; // [0,1] real_t damping_coefficient = 0.01; // [0,1] real_t drag_coefficient = 0.; // [0,1] - Vector pinned_nodes; + LocalVector pinned_nodes; // Other property to add //btScalar kVC; // Volume conversation coefficient [0,+inf] @@ -87,15 +86,14 @@ class SoftBodyBullet : public CollisionObjectBullet { SoftBodyBullet(); ~SoftBodyBullet(); - virtual void reload_body(); - virtual void set_space(SpaceBullet *p_space); + virtual void do_reload_body() override; + virtual void set_space(SpaceBullet *p_space) override; - virtual void dispatch_callbacks() {} - virtual void on_collision_filters_change() {} - virtual void on_collision_checker_start() {} - virtual void on_collision_checker_end() {} - virtual void on_enter_area(AreaBullet *p_area); - virtual void on_exit_area(AreaBullet *p_area); + virtual void do_reload_collision_filters() override {} + virtual void on_collision_checker_start() override {} + virtual void on_collision_checker_end() override {} + virtual void on_enter_area(AreaBullet *p_area) override; + virtual void on_exit_area(AreaBullet *p_area) override; _FORCE_INLINE_ btSoftBody *get_bt_soft_body() const { return bt_soft_body; } diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index abad1beacbbe..197b24f58a89 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -79,6 +79,7 @@ int BulletPhysicsDirectSpaceState::intersect_point(const Vector3 &p_point, Shape GodotAllContactResultCallback btResult(&collision_object_point, r_results, p_result_max, &p_exclude, p_collide_with_bodies, p_collide_with_areas); btResult.m_collisionFilterGroup = 0; btResult.m_collisionFilterMask = p_collision_mask; + space->flush_dirty(); space->dynamicsWorld->contactTest(&collision_object_point, btResult); // The results is already populated by GodotAllConvexResultCallback @@ -98,6 +99,7 @@ bool BulletPhysicsDirectSpaceState::intersect_ray(const Vector3 &p_from, const V btResult.m_collisionFilterMask = p_collision_mask; btResult.m_pickRay = p_pick_ray; + space->flush_dirty(); space->dynamicsWorld->rayTest(btVec_from, btVec_to, btResult); if (btResult.hasHit()) { B_TO_G(btResult.m_hitPointWorld, r_result.position); @@ -127,7 +129,7 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale_abs(), p_margin); if (!btShape->isConvex()) { - bulletdelete(btShape); + shape->destroy_bt_shape(btShape); ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); return 0; } @@ -145,9 +147,10 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra btQuery.m_collisionFilterGroup = 0; btQuery.m_collisionFilterMask = p_collision_mask; btQuery.m_closestDistanceThreshold = 0; + space->flush_dirty(); space->dynamicsWorld->contactTest(&collision_object, btQuery); - bulletdelete(btConvex); + shape->destroy_bt_shape(btShape); return btQuery.m_count; } @@ -163,7 +166,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin); if (!btShape->isConvex()) { - bulletdelete(btShape); + shape->destroy_bt_shape(btShape); ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); return false; } @@ -177,7 +180,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf bt_xform_to.getOrigin() += bt_motion; if ((bt_xform_to.getOrigin() - bt_xform_from.getOrigin()).fuzzyZero()) { - bulletdelete(btShape); + shape->destroy_bt_shape(btShape); return false; } @@ -185,6 +188,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf btResult.m_collisionFilterGroup = 0; btResult.m_collisionFilterMask = p_collision_mask; + space->flush_dirty(); space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult, space->dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration); if (btResult.hasHit()) { @@ -207,7 +211,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf r_closest_unsafe = 1.0f; } - bulletdelete(bt_convex_shape); + shape->destroy_bt_shape(btShape); return true; // Mean success } @@ -222,7 +226,7 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform & btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin); if (!btShape->isConvex()) { - bulletdelete(btShape); + shape->destroy_bt_shape(btShape); ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); return false; } @@ -240,10 +244,12 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform & btQuery.m_collisionFilterGroup = 0; btQuery.m_collisionFilterMask = p_collision_mask; btQuery.m_closestDistanceThreshold = 0; + + space->flush_dirty(); space->dynamicsWorld->contactTest(&collision_object, btQuery); r_result_count = btQuery.m_count; - bulletdelete(btConvex); + shape->destroy_bt_shape(btShape); return btQuery.m_count; } @@ -254,7 +260,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin); if (!btShape->isConvex()) { - bulletdelete(btShape); + shape->destroy_bt_shape(btShape); ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type())); return false; } @@ -272,9 +278,11 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh btQuery.m_collisionFilterGroup = 0; btQuery.m_collisionFilterMask = p_collision_mask; btQuery.m_closestDistanceThreshold = 0; + + space->flush_dirty(); space->dynamicsWorld->contactTest(&collision_object, btQuery); - bulletdelete(btConvex); + shape->destroy_bt_shape(btShape); if (btQuery.m_collided) { if (btCollisionObject::CO_RIGID_BODY == btQuery.m_rest_info_collision_object->getInternalType()) { @@ -317,6 +325,7 @@ Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_ input.m_transformB = body_transform * child_transform; + space->flush_dirty(); btPointCollector result; btGjkPairDetector gjk_pair_detector(&point_shape, convex_shape, space->gjk_simplex_solver, space->gjk_epa_pen_solver); gjk_pair_detector.getClosestPoints(input, result, nullptr); @@ -349,14 +358,72 @@ SpaceBullet::~SpaceBullet() { destroy_world(); } +void SpaceBullet::add_to_pre_flush_queue(CollisionObjectBullet *p_co) { + if (p_co->is_in_flush_queue == false) { + p_co->is_in_flush_queue = true; + queue_pre_flush.push_back(p_co); + } +} + +void SpaceBullet::add_to_flush_queue(CollisionObjectBullet *p_co) { + if (p_co->is_in_flush_queue == false) { + p_co->is_in_flush_queue = true; + queue_flush.push_back(p_co); + } +} + +void SpaceBullet::add_to_dirty_queue(CollisionObjectBullet *p_co) { + if (p_co->is_in_dirty_queue == false) { + p_co->is_in_dirty_queue = true; + queue_dirty.push_back(p_co); + } +} + +void SpaceBullet::remove_from_any_queue(CollisionObjectBullet *p_co) { + if (p_co->is_in_flush_queue) { + p_co->is_in_flush_queue = false; + queue_pre_flush.erase(p_co); + queue_flush.erase(p_co); + } + if (p_co->is_in_dirty_queue) { + p_co->is_in_dirty_queue = false; + queue_dirty.erase(p_co); + } +} + void SpaceBullet::flush_queries() { - const btCollisionObjectArray &colObjArray = dynamicsWorld->getCollisionObjectArray(); - for (int i = colObjArray.size() - 1; 0 <= i; --i) { - static_cast(colObjArray[i]->getUserPointer())->dispatch_callbacks(); + // The actions are delayed till the next frame, so to avoid executes multiple + // times the same code. + // This function is called before the frame start. + // Two queues are used so the Areas can dispatch their callbacks before the + // bodies, that can the properly receive the notification. + for (uint32_t i = 0; i < queue_pre_flush.size(); i += 1) { + queue_pre_flush[i]->dispatch_callbacks(); + queue_pre_flush[i]->is_in_flush_queue = false; + } + for (uint32_t i = 0; i < queue_flush.size(); i += 1) { + queue_flush[i]->dispatch_callbacks(); + queue_flush[i]->is_in_flush_queue = false; } + queue_pre_flush.clear(); + queue_flush.clear(); +} + +void SpaceBullet::flush_dirty() { + for (uint32_t i = 0; i < queue_dirty.size(); i += 1) { + queue_dirty[i]->flush_dirty(); + queue_dirty[i]->is_in_dirty_queue = false; + } + queue_dirty.clear(); } void SpaceBullet::step(real_t p_delta_time) { + flush_dirty(); + + for (uint32_t i = 0; i < collision_objects.size(); i += 1) { + collision_objects[i]->pre_process(); + } + delta_time = p_delta_time; dynamicsWorld->stepSimulation(p_delta_time, 0, 0); } @@ -449,16 +516,30 @@ real_t SpaceBullet::get_param(PhysicsServer3D::SpaceParameter p_param) { } void SpaceBullet::add_area(AreaBullet *p_area) { +#ifdef TOOLS_ENABLED + // This never happen, and there is no way for the user to trigger it. + // If in future a bug is introduced into this bullet integration and this + // function is called twice, the crash will notify the developer that will + // fix it even before do the eventual PR. + CRASH_COND(p_area->is_in_world); +#endif areas.push_back(p_area); dynamicsWorld->addCollisionObject(p_area->get_bt_ghost(), p_area->get_collision_layer(), p_area->get_collision_mask()); + p_area->is_in_world = true; } void SpaceBullet::remove_area(AreaBullet *p_area) { - areas.erase(p_area); - dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost()); + if (p_area->is_in_world) { + areas.erase(p_area); + dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost()); + p_area->is_in_world = false; + } } void SpaceBullet::reload_collision_filters(AreaBullet *p_area) { + if (p_area->is_in_world == false) { + return; + } btGhostObject *ghost_object = p_area->get_bt_ghost(); btBroadphaseProxy *ghost_proxy = ghost_object->getBroadphaseHandle(); @@ -468,24 +549,47 @@ void SpaceBullet::reload_collision_filters(AreaBullet *p_area) { dynamicsWorld->refreshBroadphaseProxy(ghost_object); } +void SpaceBullet::register_collision_object(CollisionObjectBullet *p_object) { + collision_objects.push_back(p_object); +} + +void SpaceBullet::unregister_collision_object(CollisionObjectBullet *p_object) { + remove_from_any_queue(p_object); + collision_objects.erase(p_object); +} + void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) { +#ifdef TOOLS_ENABLED + // This never happen, and there is no way for the user to trigger it. + // If in future a bug is introduced into this bullet integration and this + // function is called twice, the crash will notify the developer that will + // fix it even before do the eventual PR. + CRASH_COND(p_body->is_in_world); +#endif if (p_body->is_static()) { dynamicsWorld->addCollisionObject(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask()); } else { dynamicsWorld->addRigidBody(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask()); p_body->scratch_space_override_modificator(); } + p_body->is_in_world = true; } void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) { - if (p_body->is_static()) { - dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body()); - } else { - dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body()); + if (p_body->is_in_world) { + if (p_body->is_static()) { + dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body()); + } else { + dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body()); + } + p_body->is_in_world = false; } } void SpaceBullet::reload_collision_filters(RigidBodyBullet *p_body) { + if (p_body->is_in_world == false) { + return; + } btRigidBody *rigid_body = p_body->get_bt_rigid_body(); btBroadphaseProxy *body_proxy = rigid_body->getBroadphaseProxy(); @@ -665,7 +769,7 @@ void SpaceBullet::check_ghost_overlaps() { /// 1. Reset all states for (i = area->overlappingObjects.size() - 1; 0 <= i; --i) { - AreaBullet::OverlappingObjectData &otherObj = area->overlappingObjects.write[i]; + AreaBullet::OverlappingObjectData &otherObj = area->overlappingObjects[i]; // This check prevent the overwrite of ENTER state // if this function is called more times before dispatchCallbacks if (otherObj.state != AreaBullet::OVERLAP_STATE_ENTER) { @@ -913,6 +1017,9 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f } #endif + ERR_FAIL_COND_V_MSG(p_body->get_space() == nullptr, false, "The body is not part of any space."); + p_body->get_space()->flush_dirty(); + btTransform body_transform; G_TO_B(p_from, body_transform); UNSCALE_BT_BASIS(body_transform); diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h index e362f27d39d1..53aed09cc9d0 100644 --- a/modules/bullet/space_bullet.h +++ b/modules/bullet/space_bullet.h @@ -31,7 +31,7 @@ #ifndef SPACE_BULLET_H #define SPACE_BULLET_H -#include "core/templates/vector.h" +#include "core/templates/local_vector.h" #include "core/variant/variant.h" #include "godot_result_callbacks.h" #include "rid_bullet.h" @@ -110,17 +110,27 @@ class SpaceBullet : public RIDBullet { real_t linear_damp = 0.0; real_t angular_damp = 0.0; - Vector areas; + LocalVector queue_pre_flush; + LocalVector queue_flush; + LocalVector queue_dirty; + LocalVector collision_objects; + LocalVector areas; - Vector contactDebug; - int contactDebugCount = 0; + LocalVector contactDebug; + uint32_t contactDebugCount = 0; real_t delta_time = 0.; public: SpaceBullet(); virtual ~SpaceBullet(); + void add_to_flush_queue(CollisionObjectBullet *p_co); + void add_to_pre_flush_queue(CollisionObjectBullet *p_co); + void add_to_dirty_queue(CollisionObjectBullet *p_co); + void remove_from_any_queue(CollisionObjectBullet *p_co); + void flush_queries(); + void flush_dirty(); real_t get_delta_time() { return delta_time; } void step(real_t p_delta_time); @@ -150,6 +160,9 @@ class SpaceBullet : public RIDBullet { void remove_area(AreaBullet *p_area); void reload_collision_filters(AreaBullet *p_area); + void register_collision_object(CollisionObjectBullet *p_object); + void unregister_collision_object(CollisionObjectBullet *p_object); + void add_rigid_body(RigidBodyBullet *p_body); void remove_rigid_body(RigidBodyBullet *p_body); void reload_collision_filters(RigidBodyBullet *p_body); @@ -173,7 +186,7 @@ class SpaceBullet : public RIDBullet { } _FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) { if (contactDebugCount < contactDebug.size()) { - contactDebug.write[contactDebugCount++] = p_contact; + contactDebug[contactDebugCount++] = p_contact; } } _FORCE_INLINE_ Vector get_debug_contacts() { return contactDebug; }