From 6b54ac98d78924ae8fa323001f5ea9083c0da0eb Mon Sep 17 00:00:00 2001 From: Marcel Admiraal Date: Sat, 10 Oct 2020 16:35:40 +0100 Subject: Add shape data to area overlap data. --- modules/bullet/area_bullet.cpp | 150 +++++++++++++++++------------ modules/bullet/area_bullet.h | 51 +++++----- modules/bullet/collision_object_bullet.cpp | 10 +- modules/bullet/space_bullet.cpp | 148 ++++++++++------------------ 4 files changed, 167 insertions(+), 192 deletions(-) diff --git a/modules/bullet/area_bullet.cpp b/modules/bullet/area_bullet.cpp index 0d4982baba..1c49029570 100644 --- a/modules/bullet/area_bullet.cpp +++ b/modules/bullet/area_bullet.cpp @@ -59,8 +59,8 @@ AreaBullet::AreaBullet() : AreaBullet::~AreaBullet() { // signal are handled by godot, so just clear without notify - for (int i = overlappingObjects.size() - 1; 0 <= i; --i) { - overlappingObjects[i].object->on_exit_area(this); + for (int i = 0; i < overlapping_shapes.size(); i++) { + overlapping_shapes[i].other_object->on_exit_area(this); } } @@ -70,20 +70,26 @@ void AreaBullet::dispatch_callbacks() { } isScratched = false; - // Reverse order because I've to remove EXIT objects - for (int i = overlappingObjects.size() - 1; 0 <= i; --i) { - OverlappingObjectData &otherObj = overlappingObjects.write[i]; + // Reverse order so items can be removed. + for (int i = overlapping_shapes.size() - 1; i >= 0; i--) { + OverlappingShapeData &overlapping_shape = overlapping_shapes.write[i]; - switch (otherObj.state) { + switch (overlapping_shape.state) { case OVERLAP_STATE_ENTER: - otherObj.state = OVERLAP_STATE_INSIDE; - call_event(otherObj.object, PhysicsServer3D::AREA_BODY_ADDED); - otherObj.object->on_enter_area(this); + overlapping_shape.state = OVERLAP_STATE_INSIDE; + call_event(overlapping_shape, PhysicsServer3D::AREA_BODY_ADDED); + if (_overlapping_shape_count(overlapping_shape.other_object) == 1) { + // This object's first shape being added. + overlapping_shape.other_object->on_enter_area(this); + } break; case OVERLAP_STATE_EXIT: - call_event(otherObj.object, PhysicsServer3D::AREA_BODY_REMOVED); - otherObj.object->on_exit_area(this); - overlappingObjects.remove(i); // Remove after callback + call_event(overlapping_shape, PhysicsServer3D::AREA_BODY_REMOVED); + if (_overlapping_shape_count(overlapping_shape.other_object) == 1) { + // This object's last shape being removed. + overlapping_shape.other_object->on_exit_area(this); + } + overlapping_shapes.remove(i); // Remove after callback break; case OVERLAP_STATE_INSIDE: { if (otherObj.object->getType() == TYPE_RIGID_BODY) { @@ -98,8 +104,8 @@ void AreaBullet::dispatch_callbacks() { } } -void AreaBullet::call_event(CollisionObjectBullet *p_otherObject, PhysicsServer3D::AreaBodyStatus p_status) { - InOutEventCallback &event = eventsCallbacks[static_cast(p_otherObject->getType())]; +void AreaBullet::call_event(const OverlappingShapeData &p_overlapping_shape, PhysicsServer3D::AreaBodyStatus p_status) { + InOutEventCallback &event = eventsCallbacks[static_cast(p_overlapping_shape.other_object->getType())]; if (!event.event_callback.is_valid()) { event.event_callback = Callable(); @@ -107,54 +113,92 @@ void AreaBullet::call_event(CollisionObjectBullet *p_otherObject, PhysicsServer3 } call_event_res[0] = p_status; - call_event_res[1] = p_otherObject->get_self(); // Other body - call_event_res[2] = p_otherObject->get_instance_id(); // instance ID - call_event_res[3] = 0; // other_body_shape ID - call_event_res[4] = 0; // self_shape ID + call_event_res[1] = p_overlapping_shape.other_object->get_self(); // RID + call_event_res[2] = p_overlapping_shape.other_object->get_instance_id(); // Object ID + call_event_res[3] = p_overlapping_shape.other_shape_id; // Other object's shape ID + call_event_res[4] = p_overlapping_shape.our_shape_id; // This area's shape ID Callable::CallError outResp; Variant ret; event.event_callback.call((const Variant **)call_event_res, 5, ret, outResp); } -void AreaBullet::scratch() { - if (isScratched) { - return; +int AreaBullet::_overlapping_shape_count(CollisionObjectBullet *p_other_object) { + int count = 0; + for (int i = 0; i < overlapping_shapes.size(); i++) { + if (overlapping_shapes[i].other_object == p_other_object) { + count++; + } } - isScratched = true; + return count; } -void AreaBullet::clear_overlaps(bool p_notify) { - for (int i = overlappingObjects.size() - 1; 0 <= i; --i) { - if (p_notify) { - call_event(overlappingObjects[i].object, PhysicsServer3D::AREA_BODY_REMOVED); +int AreaBullet::_find_overlapping_shape(CollisionObjectBullet *p_other_object, uint32_t p_other_shape_id, uint32_t p_our_shape_id) { + for (int i = 0; i < overlapping_shapes.size(); i++) { + const OverlappingShapeData &overlapping_shape = overlapping_shapes[i]; + if (overlapping_shape.other_object == p_other_object && overlapping_shape.other_shape_id == p_other_shape_id && overlapping_shape.our_shape_id == p_our_shape_id) { + return i; } - overlappingObjects[i].object->on_exit_area(this); } - overlappingObjects.clear(); + return -1; } -void AreaBullet::remove_overlap(CollisionObjectBullet *p_object, bool p_notify) { - for (int i = overlappingObjects.size() - 1; 0 <= i; --i) { - if (overlappingObjects[i].object == p_object) { - if (p_notify) { - call_event(overlappingObjects[i].object, PhysicsServer3D::AREA_BODY_REMOVED); - } - overlappingObjects[i].object->on_exit_area(this); - overlappingObjects.remove(i); - break; +void AreaBullet::mark_all_overlaps_dirty() { + OverlappingShapeData *overlapping_shapes_w = overlapping_shapes.ptrw(); + for (int i = 0; i < overlapping_shapes.size(); i++) { + // Don't overwrite OVERLAP_STATE_ENTER state. + if (overlapping_shapes_w[i].state != OVERLAP_STATE_ENTER) { + overlapping_shapes_w[i].state = OVERLAP_STATE_DIRTY; } } } -int AreaBullet::find_overlapping_object(CollisionObjectBullet *p_colObj) { - const int size = overlappingObjects.size(); - for (int i = 0; i < size; ++i) { - if (overlappingObjects[i].object == p_colObj) { - return i; +void AreaBullet::mark_object_overlaps_inside(CollisionObjectBullet *p_other_object) { + OverlappingShapeData *overlapping_shapes_w = overlapping_shapes.ptrw(); + for (int i = 0; i < overlapping_shapes.size(); i++) { + if (overlapping_shapes_w[i].other_object == p_other_object && overlapping_shapes_w[i].state == OVERLAP_STATE_DIRTY) { + overlapping_shapes_w[i].state = OVERLAP_STATE_INSIDE; + } + } +} + +void AreaBullet::set_overlap(CollisionObjectBullet *p_other_object, uint32_t p_other_shape_id, uint32_t p_our_shape_id) { + int i = _find_overlapping_shape(p_other_object, p_other_shape_id, p_our_shape_id); + if (i == -1) { // Not found, create new one. + OverlappingShapeData overlapping_shape(p_other_object, OVERLAP_STATE_ENTER, p_other_shape_id, p_our_shape_id); + overlapping_shapes.push_back(overlapping_shape); + p_other_object->notify_new_overlap(this); + isScratched = true; + } else { + overlapping_shapes.ptrw()[i].state = OVERLAP_STATE_INSIDE; + } +} + +void AreaBullet::mark_all_dirty_overlaps_as_exit() { + OverlappingShapeData *overlapping_shapes_w = overlapping_shapes.ptrw(); + for (int i = 0; i < overlapping_shapes.size(); i++) { + if (overlapping_shapes[i].state == OVERLAP_STATE_DIRTY) { + overlapping_shapes_w[i].state = OVERLAP_STATE_EXIT; + isScratched = true; + } + } +} + +void AreaBullet::remove_object_overlaps(CollisionObjectBullet *p_object) { + // Reverse order so items can be removed. + for (int i = overlapping_shapes.size() - 1; i >= 0; i--) { + if (overlapping_shapes[i].other_object == p_object) { + overlapping_shapes.remove(i); } } - return -1; +} + +void AreaBullet::clear_overlaps() { + for (int i = 0; i < overlapping_shapes.size(); i++) { + call_event(overlapping_shapes[i], PhysicsServer3D::AREA_BODY_REMOVED); + overlapping_shapes[i].other_object->on_exit_area(this); + } + overlapping_shapes.clear(); } void AreaBullet::set_monitorable(bool p_monitorable) { @@ -182,7 +226,7 @@ void AreaBullet::reload_body() { void AreaBullet::set_space(SpaceBullet *p_space) { // Clear the old space if there is one if (space) { - clear_overlaps(false); + clear_overlaps(); isScratched = false; // Remove this object form the physics world @@ -203,24 +247,6 @@ void AreaBullet::on_collision_filters_change() { updated = true; } -void AreaBullet::add_overlap(CollisionObjectBullet *p_otherObject) { - scratch(); - overlappingObjects.push_back(OverlappingObjectData(p_otherObject, OVERLAP_STATE_ENTER)); - p_otherObject->notify_new_overlap(this); -} - -void AreaBullet::put_overlap_as_exit(int p_index) { - scratch(); - overlappingObjects.write[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; - } -} - void AreaBullet::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) { switch (p_param) { case PhysicsServer3D::AREA_PARAM_GRAVITY: diff --git a/modules/bullet/area_bullet.h b/modules/bullet/area_bullet.h index 4d9227c31a..3bff364cc1 100644 --- a/modules/bullet/area_bullet.h +++ b/modules/bullet/area_bullet.h @@ -43,8 +43,6 @@ class btGhostObject; class AreaBullet : public RigidCollisionObjectBullet { - friend void SpaceBullet::check_ghost_overlaps(); - public: struct InOutEventCallback { Callable event_callback; @@ -59,21 +57,19 @@ public: OVERLAP_STATE_EXIT // Mark ended overlaps }; - struct OverlappingObjectData { - CollisionObjectBullet *object = nullptr; - OverlapState state = OVERLAP_STATE_ENTER; - - OverlappingObjectData() {} - OverlappingObjectData(CollisionObjectBullet *p_object, OverlapState p_state) : - object(p_object), - state(p_state) {} - OverlappingObjectData(const OverlappingObjectData &other) { - operator=(other); - } - void operator=(const OverlappingObjectData &other) { - object = other.object; - state = other.state; - } + struct OverlappingShapeData { + CollisionObjectBullet *other_object = nullptr; + OverlapState state = OVERLAP_STATE_DIRTY; + uint32_t other_shape_id = 0; + uint32_t our_shape_id = 0; + + OverlappingShapeData() {} + + OverlappingShapeData(CollisionObjectBullet *p_other_object, OverlapState p_state, uint32_t p_other_shape_id, uint32_t p_our_shape_id) : + other_object(p_other_object), + state(p_state), + other_shape_id(p_other_shape_id), + our_shape_id(p_our_shape_id) {} }; private: @@ -82,7 +78,9 @@ private: Variant *call_event_res_ptr[5] = {}; btGhostObject *btGhost = nullptr; - Vector overlappingObjects; + Vector overlapping_shapes; + int _overlapping_shape_count(CollisionObjectBullet *p_other_object); + int _find_overlapping_shape(CollisionObjectBullet *p_other_object, uint32_t p_other_shape_id, uint32_t p_our_shape_id); bool monitorable = true; PhysicsServer3D::AreaSpaceOverrideMode spOv_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; @@ -104,7 +102,6 @@ public: ~AreaBullet(); _FORCE_INLINE_ btGhostObject *get_bt_ghost() const { return btGhost; } - int find_overlapping_object(CollisionObjectBullet *p_colObj); void set_monitorable(bool p_monitorable); _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } @@ -143,20 +140,18 @@ public: virtual void set_space(SpaceBullet *p_space); virtual void dispatch_callbacks(); - void call_event(CollisionObjectBullet *p_otherObject, PhysicsServer3D::AreaBodyStatus p_status); - void scratch(); - - void clear_overlaps(bool p_notify); - // Dispatch the callbacks and removes from overlapping list - void remove_overlap(CollisionObjectBullet *p_object, bool p_notify); + void call_event(const OverlappingShapeData &p_overlapping_shape, PhysicsServer3D::AreaBodyStatus p_status); virtual void on_collision_filters_change(); virtual void on_collision_checker_start() {} virtual void on_collision_checker_end() { updated = false; } - void add_overlap(CollisionObjectBullet *p_otherObject); - void put_overlap_as_exit(int p_index); - void put_overlap_as_inside(int p_index); + void mark_all_overlaps_dirty(); + void mark_object_overlaps_inside(CollisionObjectBullet *p_other_object); + void set_overlap(CollisionObjectBullet *p_other_object, uint32_t p_other_shape_id, uint32_t p_our_shape_id); + void mark_all_dirty_overlaps_as_exit(); + void remove_object_overlaps(CollisionObjectBullet *p_object); + void clear_overlaps(); void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value); Variant get_param(PhysicsServer3D::AreaParameter p_param) const; diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp index afd8cf4bf4..987a45ad5f 100644 --- a/modules/bullet/collision_object_bullet.cpp +++ b/modules/bullet/collision_object_bullet.cpp @@ -93,11 +93,9 @@ CollisionObjectBullet::CollisionObjectBullet(Type p_type) : type(p_type) {} CollisionObjectBullet::~CollisionObjectBullet() { - // Remove all overlapping, notify is not required since godot take care of it - for (int i = areasOverlapped.size() - 1; 0 <= i; --i) { - areasOverlapped[i]->remove_overlap(this, /*Notify*/ false); + for (int i = 0; i < areasOverlapped.size(); i++) { + areasOverlapped[i]->remove_object_overlaps(this); } - destroyBulletCollisionObject(); } @@ -178,7 +176,9 @@ bool CollisionObjectBullet::is_collisions_response_enabled() { } void CollisionObjectBullet::notify_new_overlap(AreaBullet *p_area) { - areasOverlapped.push_back(p_area); + if (areasOverlapped.find(p_area) == -1) { + areasOverlapped.push_back(p_area); + } } void CollisionObjectBullet::on_exit_area(AreaBullet *p_area) { diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index bf47127877..7aa3815c94 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -662,101 +662,77 @@ void SpaceBullet::destroy_world() { } void SpaceBullet::check_ghost_overlaps() { - /// Algorithm support variables - btCollisionShape *other_body_shape; - btConvexShape *area_shape; - btGjkPairDetector::ClosestPointInput gjk_input; - AreaBullet *area; - int x(-1), i(-1), y(-1), z(-1), indexOverlap(-1); - - /// For each areas - for (x = areas.size() - 1; 0 <= x; --x) { - area = areas[x]; - - btVector3 area_scale(area->get_bt_body_scale()); - + // For each area + for (int area_idx = 0; area_idx < areas.size(); area_idx++) { + AreaBullet *area = areas[area_idx]; if (!area->is_monitoring()) { continue; } - /// 1. Reset all states - for (i = area->overlappingObjects.size() - 1; 0 <= i; --i) { - AreaBullet::OverlappingObjectData &otherObj = area->overlappingObjects.write[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) { - otherObj.state = AreaBullet::OVERLAP_STATE_DIRTY; - } - } + btGhostObject *bt_ghost = area->get_bt_ghost(); + const btTransform &area_transform = area->get_transform__bullet(); + const btVector3 &area_scale(area->get_bt_body_scale()); - /// 2. Check all overlapping objects using GJK + // Mark all current overlapping shapes dirty. + area->mark_all_overlaps_dirty(); - const btAlignedObjectArray ghostOverlaps = area->get_bt_ghost()->getOverlappingPairs(); + // Broadphase + const btAlignedObjectArray overlapping_pairs = bt_ghost->getOverlappingPairs(); + // Narrowphase + for (int pair_idx = 0; pair_idx < overlapping_pairs.size(); pair_idx++) { + btCollisionObject *other_bt_collision_object = overlapping_pairs[pair_idx]; + RigidCollisionObjectBullet *other_object = static_cast(other_bt_collision_object->getUserPointer()); + const btTransform &other_transform = other_object->get_transform__bullet(); + const btVector3 &other_scale(other_object->get_bt_body_scale()); - // For each overlapping - for (i = ghostOverlaps.size() - 1; 0 <= i; --i) { - bool hasOverlap = false; - btCollisionObject *overlapped_bt_co = ghostOverlaps[i]; - RigidCollisionObjectBullet *otherObject = static_cast(overlapped_bt_co->getUserPointer()); - btVector3 other_body_scale(otherObject->get_bt_body_scale()); - - if (!area->is_updated() && !otherObject->is_updated()) { - hasOverlap = -1 != area->find_overlapping_object(otherObject); - goto collision_found; + if (!area->is_updated() && !other_object->is_updated()) { + area->mark_object_overlaps_inside(other_object); + continue; } - if (overlapped_bt_co->getUserIndex() == CollisionObjectBullet::TYPE_AREA) { - if (!static_cast(overlapped_bt_co->getUserPointer())->is_monitorable()) { + if (other_bt_collision_object->getUserIndex() == CollisionObjectBullet::TYPE_AREA) { + if (!static_cast(other_bt_collision_object->getUserPointer())->is_monitorable()) { continue; } - } else if (overlapped_bt_co->getUserIndex() != CollisionObjectBullet::TYPE_RIGID_BODY) { + } else if (other_bt_collision_object->getUserIndex() != CollisionObjectBullet::TYPE_RIGID_BODY) { continue; } // For each area shape - for (y = area->get_shape_count() - 1; 0 <= y; --y) { - if (!area->get_bt_shape(y)->isConvex()) { + for (int our_shape_id = 0; our_shape_id < area->get_shape_count(); our_shape_id++) { + btCollisionShape *area_shape = area->get_bt_shape(our_shape_id); + if (!area_shape->isConvex()) { continue; } + btConvexShape *area_convex_shape = static_cast(area_shape); - btTransform area_shape_treansform(area->get_bt_shape_transform(y)); - area_shape_treansform.getOrigin() *= area_scale; - - gjk_input.m_transformA = - area->get_transform__bullet() * - area_shape_treansform; - - area_shape = static_cast(area->get_bt_shape(y)); + btTransform area_shape_transform(area->get_bt_shape_transform(our_shape_id)); + area_shape_transform.getOrigin() *= area_scale; + btGjkPairDetector::ClosestPointInput gjk_input; + gjk_input.m_transformA = area_transform * area_shape_transform; // For each other object shape - for (z = otherObject->get_shape_count() - 1; 0 <= z; --z) { - other_body_shape = static_cast(otherObject->get_bt_shape(z)); - - btTransform other_shape_transform(otherObject->get_bt_shape_transform(z)); - other_shape_transform.getOrigin() *= other_body_scale; - - gjk_input.m_transformB = - otherObject->get_transform__bullet() * - other_shape_transform; + for (int other_shape_id = 0; other_shape_id < other_object->get_shape_count(); other_shape_id++) { + btCollisionShape *other_shape = other_object->get_bt_shape(other_shape_id); + btTransform other_shape_transform(other_object->get_bt_shape_transform(other_shape_id)); + other_shape_transform.getOrigin() *= other_scale; + gjk_input.m_transformB = other_transform * other_shape_transform; - if (other_body_shape->isConvex()) { + if (other_shape->isConvex()) { btPointCollector result; btGjkPairDetector gjk_pair_detector( - area_shape, - static_cast(other_body_shape), + area_convex_shape, + static_cast(other_shape), gjk_simplex_solver, gjk_epa_pen_solver); - gjk_pair_detector.getClosestPoints(gjk_input, result, nullptr); - if (0 >= result.m_distance) { - hasOverlap = true; - goto collision_found; + gjk_pair_detector.getClosestPoints(gjk_input, result, nullptr); + if (result.m_distance <= 0) { + area->set_overlap(other_object, other_shape_id, our_shape_id); } - - } else { - btCollisionObjectWrapper obA(nullptr, area_shape, area->get_bt_ghost(), gjk_input.m_transformA, -1, y); - btCollisionObjectWrapper obB(nullptr, other_body_shape, otherObject->get_bt_collision_object(), gjk_input.m_transformB, -1, z); - + } else { // Other shape is not convex. + btCollisionObjectWrapper obA(nullptr, area_convex_shape, bt_ghost, gjk_input.m_transformA, -1, our_shape_id); + btCollisionObjectWrapper obB(nullptr, other_shape, other_bt_collision_object, gjk_input.m_transformB, -1, other_shape_id); btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, nullptr, BT_CONTACT_POINT_ALGORITHMS); if (!algorithm) { @@ -765,42 +741,20 @@ void SpaceBullet::check_ghost_overlaps() { GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB); algorithm->processCollision(&obA, &obB, dynamicsWorld->getDispatchInfo(), &contactPointResult); - algorithm->~btCollisionAlgorithm(); dispatcher->freeCollisionAlgorithm(algorithm); if (contactPointResult.hasHit()) { - hasOverlap = true; - goto collision_found; + area->set_overlap(other_object, our_shape_id, other_shape_id); } } + } // End for each other object shape + } // End for each area shape + } // End for each overlapping pair - } // ~For each other object shape - } // ~For each area shape - - collision_found: - if (!hasOverlap) { - continue; - } - - indexOverlap = area->find_overlapping_object(otherObject); - if (-1 == indexOverlap) { - // Not found - area->add_overlap(otherObject); - } else { - // Found - area->put_overlap_as_inside(indexOverlap); - } - } - - /// 3. Remove not overlapping - for (i = area->overlappingObjects.size() - 1; 0 <= i; --i) { - // If the overlap has DIRTY state it means that it's no more overlapping - if (area->overlappingObjects[i].state == AreaBullet::OVERLAP_STATE_DIRTY) { - area->put_overlap_as_exit(i); - } - } - } + // All overlapping shapes still marked dirty must have exited. + area->mark_all_dirty_overlaps_as_exit(); + } // End for each area } void SpaceBullet::check_body_collision() { -- cgit v1.2.3