From cabbb77ba84793a158c3a2e6ca2550f1970630c7 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Thu, 15 Apr 2021 09:03:10 +0200 Subject: [PATCH 01/13] [SofaUserInteraction] Minor cleaning of RayTraceDetection --- .../SofaUserInteraction/RayTraceDetection.cpp | 15 ++++----- .../SofaUserInteraction/RayTraceDetection.h | 31 ++++++++++++------- 2 files changed, 27 insertions(+), 19 deletions(-) diff --git a/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.cpp b/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.cpp index 9580a062dd3..aa448ec98de 100644 --- a/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.cpp +++ b/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.cpp @@ -62,6 +62,11 @@ RayTraceDetection ():bDraw (initData { } +void RayTraceDetection::beginBroadPhase() +{ + core::collision::BroadPhaseDetection::beginBroadPhase(); + collisionModels.clear(); +} void RayTraceDetection::findPairsVolume (CubeCollisionModel * cm1, CubeCollisionModel * cm2) { @@ -240,10 +245,8 @@ void RayTraceDetection::addCollisionModel (core::CollisionModel * cm) { if (cm->empty ()) return; - for (sofa::helper::vector < core::CollisionModel * >::iterator it = - collisionModels.begin (); it != collisionModels.end (); ++it) + for (auto* cm2 : collisionModels) { - core::CollisionModel * cm2 = *it; if (!cm->isSimulated() && !cm2->isSimulated()) continue; if (!cm->canCollideWith (cm2)) @@ -257,11 +260,9 @@ void RayTraceDetection::addCollisionModel (core::CollisionModel * cm) core::CollisionModel* cm1 = (swapModels?cm2:cm); cm2 = (swapModels?cm:cm2); - // Here we assume a single root element is present in both models if (intersector->canIntersect (cm1->begin (), cm2->begin ())) { - cmPairs.push_back (std::make_pair (cm1, cm2)); } } @@ -314,8 +315,8 @@ void RayTraceDetection::draw (const core::visual::VisualParams* vparams) TriangleOctreeModel >::iterator it2 = (outputs)->begin (); it2 != outputs->end (); ++it2) { - vertices.push_back(sofa::defaulttype::Vector3(it2->point[0][0], it2->point[0][1],it2->point[0][2])); - vertices.push_back(sofa::defaulttype::Vector3(it2->point[1][0], it2->point[1][1],it2->point[1][2])); + vertices.push_back(it2->point[0]); + vertices.push_back(it2->point[1]); msg_error() << it2->point[0] << " " << it2->point[0]; diff --git a/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.h b/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.h index 077d729d1a6..171a169167f 100644 --- a/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.h +++ b/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.h @@ -51,28 +51,35 @@ class SOFA_SOFAUSERINTERACTION_API RayTraceDetection :public core::collision::Br public: typedef sofa::helper::vector OutputVector; + protected: RayTraceDetection (); + public: - void setDraw (bool val) - { - bDraw.setValue (val); - } - void selfCollision (TriangleOctreeModel * cm1); + + ///////////////////////////// + /// BROAD PHASE interface /// + ///////////////////////////// + + void beginBroadPhase() override; + void addCollisionModel (core::CollisionModel * cm) override; + + ////////////////////////////// + /// NARROW PHASE interface /// + ////////////////////////////// + void addCollisionPair (const std::pair < core::CollisionModel *, core::CollisionModel * >&cmPair) override; - void findPairsVolume (CubeCollisionModel * cm1, - CubeCollisionModel * cm2); +public: + void findPairsVolume (CubeCollisionModel * cm1, CubeCollisionModel * cm2); - void beginBroadPhase() override + void draw (const core::visual::VisualParams* vparams) override; + void setDraw (bool val) { - core::collision::BroadPhaseDetection::beginBroadPhase(); - collisionModels.clear(); + bDraw.setValue (val); } - - void draw (const core::visual::VisualParams* vparams) override; }; } // namespace sofa::component::collision From 7b8edebe77bf4793fea8743dfb868325a0d3835a Mon Sep 17 00:00:00 2001 From: alxbilger Date: Thu, 15 Apr 2021 10:16:54 +0200 Subject: [PATCH 02/13] [SofaBaseCollision] Introduce a brute force broad phase component --- .../modules/SofaBaseCollision/CMakeLists.txt | 2 + .../BruteForceBroadPhase.cpp | 160 ++++++++++++++++++ .../SofaBaseCollision/BruteForceBroadPhase.h | 89 ++++++++++ 3 files changed, 251 insertions(+) create mode 100644 SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp create mode 100644 SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h diff --git a/SofaKernel/modules/SofaBaseCollision/CMakeLists.txt b/SofaKernel/modules/SofaBaseCollision/CMakeLists.txt index 551c5d563c5..0faaa434ad2 100644 --- a/SofaKernel/modules/SofaBaseCollision/CMakeLists.txt +++ b/SofaKernel/modules/SofaBaseCollision/CMakeLists.txt @@ -10,6 +10,7 @@ set(HEADER_FILES ${SOFABASECOLLISION_SRC}/BaseContactMapper.h ${SOFABASECOLLISION_SRC}/BaseIntTool.h ${SOFABASECOLLISION_SRC}/BaseProximityIntersection.h + ${SOFABASECOLLISION_SRC}/BruteForceBroadPhase.h ${SOFABASECOLLISION_SRC}/BruteForceDetection.h ${SOFABASECOLLISION_SRC}/CapsuleIntTool.h ${SOFABASECOLLISION_SRC}/CapsuleIntTool.inl @@ -51,6 +52,7 @@ set(SOURCE_FILES ${SOFABASECOLLISION_SRC}/BaseContactMapper.cpp ${SOFABASECOLLISION_SRC}/BaseIntTool.cpp ${SOFABASECOLLISION_SRC}/BaseProximityIntersection.cpp + ${SOFABASECOLLISION_SRC}/BruteForceBroadPhase.cpp ${SOFABASECOLLISION_SRC}/BruteForceDetection.cpp ${SOFABASECOLLISION_SRC}/CapsuleIntTool.cpp ${SOFABASECOLLISION_SRC}/CapsuleModel.cpp diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp new file mode 100644 index 00000000000..a3b3d10d1de --- /dev/null +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp @@ -0,0 +1,160 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include + +#include +#include + +namespace sofa::component::collision +{ + +int BruteForceBroadPhaseClass = core::RegisterObject("Collision detection using extensive pair-wise tests") + .add< BruteForceBroadPhase >() +; + +BruteForceBroadPhase::BruteForceBroadPhase() + : box(initData(&box, "box", "if not empty, objects that do not intersect this bounding-box will be ignored")) +{} + +void BruteForceBroadPhase::init() +{ + reinit(); +} + +void BruteForceBroadPhase::reinit() +{ + if (box.getValue()[0][0] >= box.getValue()[1][0]) + { + boxModel.reset(); + } + else + { + if (!boxModel) boxModel = sofa::core::objectmodel::New(); + boxModel->resize(1); + boxModel->setParentOf(0, box.getValue()[0], box.getValue()[1]); + } +} + +void BruteForceBroadPhase::beginBroadPhase() +{ + core::collision::BroadPhaseDetection::beginBroadPhase(); + m_collisionModels.clear(); +} + +void BruteForceBroadPhase::addCollisionModel (core::CollisionModel *cm) +{ + if (cm == nullptr || cm->empty()) + return; + assert(intersectionMethod != nullptr); + + dmsg_info() << "CollisionModel " << cm->getName() << "(" << cm << ") of class " << cm->getClassName() + << " is added in broad phase (" << m_collisionModels.size() << " collision models)"; + + // If a box is defined, check that the collision model intersects the box + // If the collision model does not intersect the box, it is ignored from the collision detection + if (boxModel && !intersectWithBoxModel(cm)) + { + return; + } + + if (doesSelfCollide(cm)) + { + // add the collision model to be tested against itself + cmPairs.emplace_back(cm, cm); + } + + core::CollisionModel* finalCollisionModel = cm->getLast(); + + // Browse all other collision models to check if there is a potential collision (conservative check) + for (auto* cm2 : m_collisionModels) + { + // ignore this pair if both are NOT simulated (inactive) + if (!cm->isSimulated() && !cm2->isSimulated()) + { + continue; + } + + if (!keepCollisionBetween(finalCollisionModel, cm2->getLast())) + continue; + + bool swapModels = false; + core::collision::ElementIntersector* intersector = intersectionMethod->findIntersector(cm, cm2, swapModels); + if (intersector == nullptr) + continue; + + core::CollisionModel* cm1 = cm; + if (swapModels) + { + std::swap(cm1, cm2); + } + + // Here we assume a single root element is present in both models + if (intersector->canIntersect(cm1->begin(), cm2->begin())) + { + //both collision models will be further examined in the narrow phase + cmPairs.emplace_back(cm1, cm2); + } + } + + //accumulate CollisionModel's in a vector so the next CollisionModel can be tested against all previous ones + m_collisionModels.push_back(cm); +} + +bool BruteForceBroadPhase::keepCollisionBetween(core::CollisionModel *cm1, core::CollisionModel *cm2) +{ + return cm1->canCollideWith(cm2) && cm2->canCollideWith(cm1); +} + +bool BruteForceBroadPhase::doesSelfCollide(core::CollisionModel *cm) const +{ + if (cm->isSimulated() && cm->getLast()->canCollideWith(cm->getLast())) + { + // self collision + bool swapModels = false; + core::collision::ElementIntersector* intersector = intersectionMethod->findIntersector(cm, cm, swapModels); + if (intersector != nullptr) + { + return intersector->canIntersect(cm->begin(), cm->begin()); + } + } + + return false; +} + +bool BruteForceBroadPhase::intersectWithBoxModel(core::CollisionModel *cm) const +{ + bool swapModels = false; + core::collision::ElementIntersector* intersector = intersectionMethod->findIntersector(cm, boxModel.get(), swapModels); + if (intersector) + { + core::CollisionModel* cm1 = (swapModels?boxModel.get():cm); + core::CollisionModel* cm2 = (swapModels?cm:boxModel.get()); + + // Here we assume a single root element is present in both models + return intersector->canIntersect(cm1->begin(), cm2->begin()); + } + + return true; +} + +} \ No newline at end of file diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h new file mode 100644 index 00000000000..2b8ec4d0cad --- /dev/null +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h @@ -0,0 +1,89 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include + +namespace sofa::component::collision +{ + +/** + * @brief Perform an extensive pair-wise collision test based on the bounding volume of collision models + * + * This component is a broad phase algorithm used during collision detection to limit the number of pairs of objects + * that need to be checked for intersection. The algorithm output is a list of pairs of objects that can potentially + * be in intersection. This list is then used as an input for a narrow phase algorithm. + * In this algorithm, all possible pairs of objects are tested (brute force test). If there are n objects, there will be + * n^2 tests. The tests are based on the bounding volume of the objects, usually an axis-aligned bounding box. + */ +class SOFA_SOFABASECOLLISION_API BruteForceBroadPhase : public core::collision::BroadPhaseDetection +{ +public: + SOFA_CLASS(BruteForceBroadPhase, core::collision::BroadPhaseDetection); + +protected: + BruteForceBroadPhase(); + ~BruteForceBroadPhase() override = default; + +private: + + /// vector of accumulated CollisionModel's when the collision pipeline asks + /// to add a CollisionModel in BruteForceBroadPhase::addCollisionModel + /// This vector is emptied at each time step in BruteForceBroadPhase::beginBroadPhase + sofa::helper::vector m_collisionModels; + + ///< if not empty, objects that do not intersect this bounding-box will be ignored + Data< helper::fixed_array > box; + + CubeCollisionModel::SPtr boxModel; + +public: + void init() override; + void reinit() override; + + void beginBroadPhase() override; + + /** \brief In the broad phase, ignores collision with the provided collision model if possible and add pairs of + * collision models if in intersection. + * + * Ignore the collision with the provided collision model if it does not intersect with the box defined in + * the Data box when it is defined. + * Add the provided collision model to be investigated in the narrow phase in case of self collision. + * Check intersection with already added collision models. If it can intersect another collision model, the pair + * is added to be further investigated in the narrow phase. + */ + void addCollisionModel (core::CollisionModel *cm) override; + +protected: + + virtual bool keepCollisionBetween(core::CollisionModel *cm1, core::CollisionModel *cm2); + + /// Return true if the provided CollisionModel can collide with itself + bool doesSelfCollide(core::CollisionModel *cm) const; + + /// Return true if the provided CollisionModel intersect boxModel, false otherwise + bool intersectWithBoxModel(core::CollisionModel *cm) const; +}; + +} From e31f3916ca06348df251c0da2a4ce31519812cc3 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Thu, 15 Apr 2021 10:27:16 +0200 Subject: [PATCH 03/13] [SofaUserInteraction] Use the component BruteForceBroadPhase in RayTraceDetection RayTraceDetection inherits from BruteForceBroadPhase. It makes more sense because the broad phase of RayTraceDetection was actually a brute force broad phase. --- .../SofaUserInteraction/RayTraceDetection.cpp | 34 ------------------- .../SofaUserInteraction/RayTraceDetection.h | 16 ++------- 2 files changed, 3 insertions(+), 47 deletions(-) diff --git a/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.cpp b/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.cpp index aa448ec98de..68682746dfa 100644 --- a/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.cpp +++ b/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.cpp @@ -62,12 +62,6 @@ RayTraceDetection ():bDraw (initData { } -void RayTraceDetection::beginBroadPhase() -{ - core::collision::BroadPhaseDetection::beginBroadPhase(); - collisionModels.clear(); -} - void RayTraceDetection::findPairsVolume (CubeCollisionModel * cm1, CubeCollisionModel * cm2) { /*Obtain the CollisionModel at the lowest level, in this case it must be a TriangleOctreeModel */ @@ -241,34 +235,6 @@ void RayTraceDetection::findPairsVolume (CubeCollisionModel * cm1, CubeCollision } -void RayTraceDetection::addCollisionModel (core::CollisionModel * cm) -{ - if (cm->empty ()) - return; - for (auto* cm2 : collisionModels) - { - if (!cm->isSimulated() && !cm2->isSimulated()) - continue; - if (!cm->canCollideWith (cm2)) - continue; - - bool swapModels = false; - core::collision::ElementIntersector* intersector = intersectionMethod->findIntersector(cm, cm2, swapModels); - if (intersector == nullptr) - continue; - - core::CollisionModel* cm1 = (swapModels?cm2:cm); - cm2 = (swapModels?cm:cm2); - - // Here we assume a single root element is present in both models - if (intersector->canIntersect (cm1->begin (), cm2->begin ())) - { - cmPairs.push_back (std::make_pair (cm1, cm2)); - } - } - collisionModels.push_back (cm); -} - void RayTraceDetection::addCollisionPair (const std::pair < core::CollisionModel *, core::CollisionModel * >&cmPair) diff --git a/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.h b/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.h index 171a169167f..5c68bece1b5 100644 --- a/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.h +++ b/modules/SofaUserInteraction/src/SofaUserInteraction/RayTraceDetection.h @@ -22,7 +22,7 @@ #pragma once #include -#include +#include #include #include #include @@ -39,14 +39,13 @@ namespace sofa::component::collision * up to find a triangle in the other object. Both triangles are tested to evaluate if they are in * colliding state. It must be used with a TriangleOctreeModel,as an octree is used to traverse the object. */ -class SOFA_SOFAUSERINTERACTION_API RayTraceDetection :public core::collision::BroadPhaseDetection, +class SOFA_SOFAUSERINTERACTION_API RayTraceDetection :public BruteForceBroadPhase, public core::collision::NarrowPhaseDetection { public: - SOFA_CLASS2(RayTraceDetection, core::collision::BroadPhaseDetection, core::collision::NarrowPhaseDetection); + SOFA_CLASS2(RayTraceDetection, BruteForceBroadPhase, core::collision::NarrowPhaseDetection); private: - sofa::helper::vector < core::CollisionModel * >collisionModels; Data < bool > bDraw; public: @@ -56,15 +55,6 @@ class SOFA_SOFAUSERINTERACTION_API RayTraceDetection :public core::collision::Br RayTraceDetection (); public: - - ///////////////////////////// - /// BROAD PHASE interface /// - ///////////////////////////// - - void beginBroadPhase() override; - - void addCollisionModel (core::CollisionModel * cm) override; - ////////////////////////////// /// NARROW PHASE interface /// ////////////////////////////// From 198be246d25eb2e6c448f67fe0a0e1f490ccfe0a Mon Sep 17 00:00:00 2001 From: alxbilger Date: Wed, 21 Apr 2021 11:13:12 +0200 Subject: [PATCH 04/13] [MultiThreading] Introduce ParallelBruteForceBroadPhase The component is a parallel implementation of BruteForceBroadPhase --- .../BruteForceBroadPhase.cpp | 9 +- .../SofaBaseCollision/BruteForceBroadPhase.h | 25 ++- .../plugins/MultiThreading/CMakeLists.txt | 2 + .../ParallelBruteForceBroadPhase.cpp | 186 ++++++++++++++++++ .../ParallelBruteForceBroadPhase.h | 86 ++++++++ 5 files changed, 296 insertions(+), 12 deletions(-) create mode 100644 applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp create mode 100644 applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp index a3b3d10d1de..317bb30e1a3 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp @@ -86,15 +86,18 @@ void BruteForceBroadPhase::addCollisionModel (core::CollisionModel *cm) core::CollisionModel* finalCollisionModel = cm->getLast(); // Browse all other collision models to check if there is a potential collision (conservative check) - for (auto* cm2 : m_collisionModels) + for (const auto& model : m_collisionModels) { + auto* cm2 = model.firstCollisionModel; + auto* finalCm2 = model.lastCollisionModel; + // ignore this pair if both are NOT simulated (inactive) if (!cm->isSimulated() && !cm2->isSimulated()) { continue; } - if (!keepCollisionBetween(finalCollisionModel, cm2->getLast())) + if (!keepCollisionBetween(finalCollisionModel, finalCm2)) continue; bool swapModels = false; @@ -117,7 +120,7 @@ void BruteForceBroadPhase::addCollisionModel (core::CollisionModel *cm) } //accumulate CollisionModel's in a vector so the next CollisionModel can be tested against all previous ones - m_collisionModels.push_back(cm); + m_collisionModels.emplace_back(cm, finalCollisionModel); } bool BruteForceBroadPhase::keepCollisionBetween(core::CollisionModel *cm1, core::CollisionModel *cm2) diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h index 2b8ec4d0cad..f77a45dae2e 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h @@ -48,16 +48,9 @@ class SOFA_SOFABASECOLLISION_API BruteForceBroadPhase : public core::collision:: private: - /// vector of accumulated CollisionModel's when the collision pipeline asks - /// to add a CollisionModel in BruteForceBroadPhase::addCollisionModel - /// This vector is emptied at each time step in BruteForceBroadPhase::beginBroadPhase - sofa::helper::vector m_collisionModels; - ///< if not empty, objects that do not intersect this bounding-box will be ignored Data< helper::fixed_array > box; - CubeCollisionModel::SPtr boxModel; - public: void init() override; void reinit() override; @@ -75,15 +68,29 @@ class SOFA_SOFABASECOLLISION_API BruteForceBroadPhase : public core::collision:: */ void addCollisionModel (core::CollisionModel *cm) override; -protected: + static bool keepCollisionBetween(core::CollisionModel *cm1, core::CollisionModel *cm2); - virtual bool keepCollisionBetween(core::CollisionModel *cm1, core::CollisionModel *cm2); +protected: /// Return true if the provided CollisionModel can collide with itself bool doesSelfCollide(core::CollisionModel *cm) const; /// Return true if the provided CollisionModel intersect boxModel, false otherwise bool intersectWithBoxModel(core::CollisionModel *cm) const; + + CubeCollisionModel::SPtr boxModel; + + struct FirstLastCollisionModel + { + core::CollisionModel* firstCollisionModel { nullptr }; + core::CollisionModel* lastCollisionModel { nullptr }; + FirstLastCollisionModel(core::CollisionModel* a, core::CollisionModel* b) : firstCollisionModel(a), lastCollisionModel(b) {} + }; + + /// vector of accumulated CollisionModel's when the collision pipeline asks + /// to add a CollisionModel in BruteForceBroadPhase::addCollisionModel + /// This vector is emptied at each time step in BruteForceBroadPhase::beginBroadPhase + sofa::helper::vector m_collisionModels; }; } diff --git a/applications/plugins/MultiThreading/CMakeLists.txt b/applications/plugins/MultiThreading/CMakeLists.txt index 821ec1d4d39..231f1111c5e 100644 --- a/applications/plugins/MultiThreading/CMakeLists.txt +++ b/applications/plugins/MultiThreading/CMakeLists.txt @@ -12,6 +12,7 @@ set(HEADER_FILES src/MultiThreading/DataExchange.inl src/MultiThreading/MeanComputation.h src/MultiThreading/MeanComputation.inl + src/MultiThreading/ParallelBruteForceBroadPhase.h ) set(SOURCE_FILES @@ -21,6 +22,7 @@ set(SOURCE_FILES src/MultiThreading/BeamLinearMapping_mt.cpp src/MultiThreading/DataExchange.cpp src/MultiThreading/MeanComputation.cpp + src/MultiThreading/ParallelBruteForceBroadPhase.cpp ) find_package(SofaMiscMapping REQUIRED) diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp new file mode 100644 index 00000000000..a49755b40e9 --- /dev/null +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp @@ -0,0 +1,186 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include + +#include +#include +#include +#include + +namespace sofa::component::collision +{ + +using sofa::helper::ScopedAdvancedTimer; + +int ParallelBruteForceBroadPhaseClass = core::RegisterObject("Collision detection using extensive pair-wise tests performed in parallel") + .add< ParallelBruteForceBroadPhase >() +; + +ParallelBruteForceBroadPhase::ParallelBruteForceBroadPhase() + : BruteForceBroadPhase() +{} + +void ParallelBruteForceBroadPhase::init() +{ + BruteForceBroadPhase::init(); + + auto* taskScheduler = sofa::simulation::TaskScheduler::getInstance(); + assert(taskScheduler != nullptr); + if (taskScheduler->getThreadCount() < 1) + { + taskScheduler->init(0); + msg_info() << "Task scheduler initialized on " << taskScheduler->getThreadCount() << " threads"; + } + else + { + msg_info() << "Task scheduler already initialized on " << taskScheduler->getThreadCount() << " threads"; + } +} + +void ParallelBruteForceBroadPhase::addCollisionModel(core::CollisionModel *cm) +{ + if (cm == nullptr || cm->empty()) + return; + + assert(intersectionMethod != nullptr); + + if (boxModel && !intersectWithBoxModel(cm)) + { + return; + } + + if (doesSelfCollide(cm)) + { + // add the collision model to be tested against itself + cmPairs.emplace_back(cm, cm); + } + + core::CollisionModel* finalCollisionModel = cm->getLast(); + for (const auto& model : m_collisionModels) + { + m_pairs.emplace_back(FirstLastCollisionModel{cm, finalCollisionModel}, model); + } + + m_collisionModels.emplace_back(cm, finalCollisionModel); +} + +void ParallelBruteForceBroadPhase::addCollisionModels(const sofa::helper::vector& v) +{ + ScopedAdvancedTimer timer("ParallelBruteForceBroadPhase::addCollisionModels"); + + m_pairs.clear(); + BroadPhaseDetection::addCollisionModels(v); + + auto *taskScheduler = sofa::simulation::TaskScheduler::getInstance(); + assert(taskScheduler != nullptr); + + { + ScopedAdvancedTimer createTasksTimer("TasksCreation"); + + m_tasks.clear(); + + const auto nbThreads = taskScheduler->getThreadCount(); + m_tasks.reserve(nbThreads); + + const auto nbElements = m_pairs.size() / nbThreads; + auto first = m_pairs.begin(); + auto last = first + nbElements; + + for (unsigned int i = 0; i < nbThreads; ++i) + { + if (i == nbThreads - 1) + { + last = m_pairs.end(); + } + m_tasks.emplace_back(&m_status, first, last, intersectionMethod); + taskScheduler->addTask(&m_tasks.back()); + first += nbElements; + last += nbElements; + } + } + + { + ScopedAdvancedTimer waitTimer("ParallelTasks"); + taskScheduler->workUntilDone(&m_status); + } + + for (const auto& task : m_tasks) + { + cmPairs.insert(cmPairs.end(), task.m_intersectingPairs.begin(), task.m_intersectingPairs.end()); + } +} + +BruteForcePairTest::BruteForcePairTest(sofa::simulation::CpuTask::Status *status, + PairIterator first, PairIterator last, + core::collision::Intersection* intersectionMethod) + : sofa::simulation::CpuTask(status) + , m_intersectingPairs() + , m_first(first) + , m_last(last) + , m_intersectionMethod(intersectionMethod) +{} + +sofa::simulation::Task::MemoryAlloc BruteForcePairTest::run() +{ + auto it = m_first; + while(it != m_last) + { + auto* cm_1 = it->first.firstCollisionModel; + auto* lastCm_1 = it->first.lastCollisionModel; + auto* cm_2 = it->second.firstCollisionModel; + auto* lastCm_2 = it->second.lastCollisionModel; + ++it; + + // ignore this pair if both are NOT simulated (inactive) + if (!cm_1->isSimulated() && !cm_2->isSimulated()) + { + continue; + } + + if (!BruteForceBroadPhase::keepCollisionBetween(lastCm_1, lastCm_2)) + { + continue; + } + + bool swapModels = false; + core::collision::ElementIntersector *intersector = m_intersectionMethod->findIntersector(cm_1, cm_2,swapModels); + if (intersector == nullptr) + { + continue; + } + + if (swapModels) + { + std::swap(cm_1, cm_2); + } + + // Here we assume a single root element is present in both models + if (intersector->canIntersect(cm_1->begin(), cm_2->begin())) + { + m_intersectingPairs.emplace_back(cm_1, cm_2); + } + } + + return simulation::Task::Stack; +} + +} diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h new file mode 100644 index 00000000000..4df970a986f --- /dev/null +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h @@ -0,0 +1,86 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include + +#include +#include + +namespace sofa::core::collision +{ + class Intersection; + class ElementIntersector; +} + +namespace sofa::component::collision +{ + +class BruteForcePairTest; + +class SOFA_MULTITHREADING_PLUGIN_API ParallelBruteForceBroadPhase : public BruteForceBroadPhase +{ +public: + SOFA_CLASS(ParallelBruteForceBroadPhase, BruteForceBroadPhase); + + void init() override; + + void addCollisionModel(core::CollisionModel *cm) override; + void addCollisionModels(const sofa::helper::vector& v) override; + +protected: + ParallelBruteForceBroadPhase(); + ~ParallelBruteForceBroadPhase() override = default; + + std::vector m_tasks; + sofa::simulation::CpuTask::Status m_status; + +public: + using Pair = std::pair; + +protected: + std::vector m_pairs; +}; + +class SOFA_SOFABASECOLLISION_API BruteForcePairTest : public sofa::simulation::CpuTask +{ + using PairIterator = std::vector::const_iterator; + +public: + BruteForcePairTest(sofa::simulation::CpuTask::Status* status, + PairIterator first, PairIterator last, + core::collision::Intersection* intersectionMethod); + ~BruteForcePairTest() override = default; + sofa::simulation::Task::MemoryAlloc run() final; + + std::vector > m_intersectingPairs; + +private: + + PairIterator m_first; + PairIterator m_last; + + core::collision::Intersection* m_intersectionMethod { nullptr }; + +}; + +} \ No newline at end of file From af0e343883e4a7938df2a59966269f1c1bd1ff67 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Wed, 21 Apr 2021 16:49:13 +0200 Subject: [PATCH 05/13] [MultiThreading] Fix compilation after merge --- .../src/MultiThreading/ParallelBruteForceBroadPhase.cpp | 6 ++++-- .../src/MultiThreading/ParallelBruteForceBroadPhase.h | 3 +-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp index a49755b40e9..d2a555f475a 100644 --- a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp @@ -93,6 +93,8 @@ void ParallelBruteForceBroadPhase::addCollisionModels(const sofa::helper::vector auto *taskScheduler = sofa::simulation::TaskScheduler::getInstance(); assert(taskScheduler != nullptr); + sofa::simulation::CpuTask::Status status; + { ScopedAdvancedTimer createTasksTimer("TasksCreation"); @@ -111,7 +113,7 @@ void ParallelBruteForceBroadPhase::addCollisionModels(const sofa::helper::vector { last = m_pairs.end(); } - m_tasks.emplace_back(&m_status, first, last, intersectionMethod); + m_tasks.emplace_back(&status, first, last, intersectionMethod); taskScheduler->addTask(&m_tasks.back()); first += nbElements; last += nbElements; @@ -120,7 +122,7 @@ void ParallelBruteForceBroadPhase::addCollisionModels(const sofa::helper::vector { ScopedAdvancedTimer waitTimer("ParallelTasks"); - taskScheduler->workUntilDone(&m_status); + taskScheduler->workUntilDone(&status); } for (const auto& task : m_tasks) diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h index 4df970a986f..52fc17b4673 100644 --- a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h @@ -24,7 +24,7 @@ #include #include -#include +#include namespace sofa::core::collision { @@ -52,7 +52,6 @@ class SOFA_MULTITHREADING_PLUGIN_API ParallelBruteForceBroadPhase : public Brute ~ParallelBruteForceBroadPhase() override = default; std::vector m_tasks; - sofa::simulation::CpuTask::Status m_status; public: using Pair = std::pair; From 04c2d00314faa32ea75f9afe3758973d5f62db75 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Thu, 22 Apr 2021 07:51:31 +0200 Subject: [PATCH 06/13] [MultiThreading] Cleanup --- .../SofaBaseCollision/BruteForceBroadPhase.h | 8 ++++++- .../ParallelBruteForceBroadPhase.cpp | 5 +++++ .../ParallelBruteForceBroadPhase.h | 21 ++++++++++++++++--- 3 files changed, 30 insertions(+), 4 deletions(-) diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h index f77a45dae2e..dc620a31036 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.h @@ -35,7 +35,7 @@ namespace sofa::component::collision * that need to be checked for intersection. The algorithm output is a list of pairs of objects that can potentially * be in intersection. This list is then used as an input for a narrow phase algorithm. * In this algorithm, all possible pairs of objects are tested (brute force test). If there are n objects, there will be - * n^2 tests. The tests are based on the bounding volume of the objects, usually an axis-aligned bounding box. + * n^2/2 tests. The tests are based on the bounding volume of the objects, usually an axis-aligned bounding box. */ class SOFA_SOFABASECOLLISION_API BruteForceBroadPhase : public core::collision::BroadPhaseDetection { @@ -80,10 +80,16 @@ class SOFA_SOFABASECOLLISION_API BruteForceBroadPhase : public core::collision:: CubeCollisionModel::SPtr boxModel; + /// A data structure to store a pair of collision models + /// They both describe the same object struct FirstLastCollisionModel { + /// First collision model in the hierarchy of collision models of an object. Usually a bounding box core::CollisionModel* firstCollisionModel { nullptr }; + + // Last collision model in the hierarchy of collision models of an object. Holding more details than a bounding box core::CollisionModel* lastCollisionModel { nullptr }; + FirstLastCollisionModel(core::CollisionModel* a, core::CollisionModel* b) : firstCollisionModel(a), lastCollisionModel(b) {} }; diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp index d2a555f475a..630fe5bc5a2 100644 --- a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp @@ -43,6 +43,8 @@ void ParallelBruteForceBroadPhase::init() { BruteForceBroadPhase::init(); + // initialize the thread pool + auto* taskScheduler = sofa::simulation::TaskScheduler::getInstance(); assert(taskScheduler != nullptr); if (taskScheduler->getThreadCount() < 1) @@ -125,6 +127,7 @@ void ParallelBruteForceBroadPhase::addCollisionModels(const sofa::helper::vector taskScheduler->workUntilDone(&status); } + // Merge the output of the tasks for (const auto& task : m_tasks) { cmPairs.insert(cmPairs.end(), task.m_intersectingPairs.begin(), task.m_intersectingPairs.end()); @@ -143,6 +146,8 @@ BruteForcePairTest::BruteForcePairTest(sofa::simulation::CpuTask::Status *status sofa::simulation::Task::MemoryAlloc BruteForcePairTest::run() { + assert(m_intersectionMethod != nullptr); + auto it = m_first; while(it != m_last) { diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h index 52fc17b4673..2613c65eb05 100644 --- a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h @@ -37,6 +37,12 @@ namespace sofa::component::collision class BruteForcePairTest; +/** + * @brief A parallel implementation of the component BruteForceBroadPhase + * + * The work is divided into n tasks executed in parallel. n is the number of threads available in + * the global thread pool. + */ class SOFA_MULTITHREADING_PLUGIN_API ParallelBruteForceBroadPhase : public BruteForceBroadPhase { public: @@ -51,18 +57,23 @@ class SOFA_MULTITHREADING_PLUGIN_API ParallelBruteForceBroadPhase : public Brute ParallelBruteForceBroadPhase(); ~ParallelBruteForceBroadPhase() override = default; + /// List of tasks executed in parallel. + /// They are created at each time step, but the memory is not freed std::vector m_tasks; public: - using Pair = std::pair; + using FirstLastCollisionModelPair = std::pair; protected: - std::vector m_pairs; + std::vector m_pairs; }; +/** + * @brief Task meant to be executed in parallel, and performing pair-wise collision tests + */ class SOFA_SOFABASECOLLISION_API BruteForcePairTest : public sofa::simulation::CpuTask { - using PairIterator = std::vector::const_iterator; + using PairIterator = std::vector::const_iterator; public: BruteForcePairTest(sofa::simulation::CpuTask::Status* status, @@ -71,13 +82,17 @@ class SOFA_SOFABASECOLLISION_API BruteForcePairTest : public sofa::simulation::C ~BruteForcePairTest() override = default; sofa::simulation::Task::MemoryAlloc run() final; + /// After this task is executed, this list contains pairs of collision models which are intersecting std::vector > m_intersectingPairs; private: + /// Begining of a range of pairs of collision models to tests in this task PairIterator m_first; + /// End of a range of pairs of collision models to tests in this task PairIterator m_last; + /// The intersection method used to perform the collision tests core::collision::Intersection* m_intersectionMethod { nullptr }; }; From 67b5592be0280c93d70063f165942a79fa294704 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Thu, 22 Apr 2021 08:11:25 +0200 Subject: [PATCH 07/13] [MultiThreading] Add a meaningful scene using ParallelBruteForceBroadPhase --- .../examples/ParallelBruteForceBroadPhase.scn | 1564 +++++++++++++++++ 1 file changed, 1564 insertions(+) create mode 100644 applications/plugins/MultiThreading/examples/ParallelBruteForceBroadPhase.scn diff --git a/applications/plugins/MultiThreading/examples/ParallelBruteForceBroadPhase.scn b/applications/plugins/MultiThreading/examples/ParallelBruteForceBroadPhase.scn new file mode 100644 index 00000000000..cdcd13924fe --- /dev/null +++ b/applications/plugins/MultiThreading/examples/ParallelBruteForceBroadPhase.scn @@ -0,0 +1,1564 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 187c37de0e8620a52b43b3047ec55bc5fa5d72ff Mon Sep 17 00:00:00 2001 From: alxbilger Date: Fri, 23 Apr 2021 07:46:07 +0200 Subject: [PATCH 08/13] [MultiThreading] Fix wrong export --- .../src/MultiThreading/ParallelBruteForceBroadPhase.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h index 2613c65eb05..e420f688269 100644 --- a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.h @@ -71,7 +71,7 @@ class SOFA_MULTITHREADING_PLUGIN_API ParallelBruteForceBroadPhase : public Brute /** * @brief Task meant to be executed in parallel, and performing pair-wise collision tests */ -class SOFA_SOFABASECOLLISION_API BruteForcePairTest : public sofa::simulation::CpuTask +class SOFA_MULTITHREADING_PLUGIN_API BruteForcePairTest : public sofa::simulation::CpuTask { using PairIterator = std::vector::const_iterator; From 91b72d198f7b27579bafeecbb672e19c4d68a172 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Fri, 23 Apr 2021 09:29:00 +0200 Subject: [PATCH 09/13] [MultiThreading] Add more security --- .../ParallelBruteForceBroadPhase.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp index 630fe5bc5a2..717b4cb2933 100644 --- a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBruteForceBroadPhase.cpp @@ -92,20 +92,31 @@ void ParallelBruteForceBroadPhase::addCollisionModels(const sofa::helper::vector m_pairs.clear(); BroadPhaseDetection::addCollisionModels(v); + if (m_pairs.empty()) + { + return; + } + auto *taskScheduler = sofa::simulation::TaskScheduler::getInstance(); assert(taskScheduler != nullptr); + if (taskScheduler->getThreadCount() == 0) + { + msg_error() << "Task scheduler not correctly initialized"; + return; + } + sofa::simulation::CpuTask::Status status; { ScopedAdvancedTimer createTasksTimer("TasksCreation"); - m_tasks.clear(); + const auto nbPairs = static_cast(m_pairs.size()); - const auto nbThreads = taskScheduler->getThreadCount(); + const auto nbThreads = std::min(taskScheduler->getThreadCount(), nbPairs); m_tasks.reserve(nbThreads); - const auto nbElements = m_pairs.size() / nbThreads; + const auto nbElements = nbPairs / nbThreads; auto first = m_pairs.begin(); auto last = first + nbElements; @@ -132,6 +143,8 @@ void ParallelBruteForceBroadPhase::addCollisionModels(const sofa::helper::vector { cmPairs.insert(cmPairs.end(), task.m_intersectingPairs.begin(), task.m_intersectingPairs.end()); } + + m_tasks.clear(); } BruteForcePairTest::BruteForcePairTest(sofa::simulation::CpuTask::Status *status, From 88769a27a78589a93e17deca08a318e4ba2a70b9 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Fri, 23 Apr 2021 10:53:01 +0200 Subject: [PATCH 10/13] [SofaBaseCollision] BruteForceDetection inherits from BruteForceBroadPhase --- .../SofaBaseCollision/BruteForceDetection.cpp | 132 +----------------- .../SofaBaseCollision/BruteForceDetection.h | 47 +------ 2 files changed, 6 insertions(+), 173 deletions(-) diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp index 52466d117f8..3390367d07f 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp @@ -40,141 +40,15 @@ int BruteForceDetectionClass = core::RegisterObject("Collision detection using e using namespace core::objectmodel; BruteForceDetection::BruteForceDetection() - : box(initData(&box, "box", "if not empty, objects that do not intersect this bounding-box will be ignored")) -{ -} - -void BruteForceDetection::init() -{ - reinit(); -} - -void BruteForceDetection::reinit() -{ - if (box.getValue()[0][0] >= box.getValue()[1][0]) - { - boxModel.reset(); - } - else - { - if (!boxModel) boxModel = sofa::core::objectmodel::New(); - boxModel->resize(1); - boxModel->setParentOf(0, box.getValue()[0], box.getValue()[1]); - } -} - -void BruteForceDetection::addCollisionModel(core::CollisionModel *cm) -{ - if (cm == nullptr || cm->empty()) - return; - assert(intersectionMethod != nullptr); - - dmsg_info() << "CollisionModel " << cm->getName() << "(" << cm << ") of class " << cm->getClassName() << " is added in broad phase (" << collisionModels.size() << " collision models)"; - - // If a box is defined, check that the collision model intersects the box - // If the collision model does not intersect the box, it is ignored from the collision detection - if (boxModel && !intersectWithBoxModel(cm)) - { - return; - } - - if (doesSelfCollide(cm)) - { - // add the collision model to be tested against itself - cmPairs.emplace_back(cm, cm); - } - - core::CollisionModel* finalCollisionModel = cm->getLast(); - - // Browse all other collision models to check if there is a potential collision (conservative check) - for (auto* cm2 : collisionModels) - { - // ignore this pair if both are NOT simulated (inactive) - if (!cm->isSimulated() && !cm2->isSimulated()) - { - continue; - } - - if (!keepCollisionBetween(finalCollisionModel, cm2->getLast())) - continue; - - bool swapModels = false; - core::collision::ElementIntersector* intersector = intersectionMethod->findIntersector(cm, cm2, swapModels); - if (intersector == nullptr) - continue; - - core::CollisionModel* cm1 = cm; - if (swapModels) - { - std::swap(cm1, cm2); - } - - // Here we assume a single root element is present in both models - if (intersector->canIntersect(cm1->begin(), cm2->begin())) - { - //both collision models will be further examined in the narrow phase - cmPairs.emplace_back(cm1, cm2); - } - } - - //accumulate CollisionModel's in a vector so the next CollisionModel can be tested against all previous ones - collisionModels.push_back(cm); -} - -bool BruteForceDetection::intersectWithBoxModel(core::CollisionModel *cm) const -{ - bool swapModels = false; - core::collision::ElementIntersector* intersector = intersectionMethod->findIntersector(cm, boxModel.get(), swapModels); - if (intersector) - { - core::CollisionModel* cm1 = (swapModels?boxModel.get():cm); - core::CollisionModel* cm2 = (swapModels?cm:boxModel.get()); - - // Here we assume a single root element is present in both models - return intersector->canIntersect(cm1->begin(), cm2->begin()); - } - - return true; -} - -bool BruteForceDetection::doesSelfCollide(core::CollisionModel *cm) const -{ - if (cm->isSimulated() && cm->getLast()->canCollideWith(cm->getLast())) - { - // self collision - bool swapModels = false; - core::collision::ElementIntersector* intersector = intersectionMethod->findIntersector(cm, cm, swapModels); - if (intersector != nullptr) - { - return intersector->canIntersect(cm->begin(), cm->begin()); - } - } - - return false; -} + : BruteForceBroadPhase() + , core::collision::NarrowPhaseDetection() +{} bool BruteForceDetection::isSelfCollision(core::CollisionModel* cm1, core::CollisionModel* cm2) { return (cm1->getContext() == cm2->getContext()); } -void BruteForceDetection::beginBroadPhase() -{ - core::collision::BroadPhaseDetection::beginBroadPhase(); - collisionModels.clear(); -} - -void BruteForceDetection::endBroadPhase() -{ - core::collision::BroadPhaseDetection::endBroadPhase(); - dmsg_info() << cmPairs.size() << " pairs to investigate in narrow phase"; -} - -bool BruteForceDetection::keepCollisionBetween(core::CollisionModel *cm1, core::CollisionModel *cm2) -{ - return cm1->canCollideWith(cm2) && cm2->canCollideWith(cm1); -} - void BruteForceDetection::addCollisionPair(const std::pair& cmPair) { core::CollisionModel *cm1 = cmPair.first; //->getNext(); diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h index 9c9e5cda95e..4cc908a4e98 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h @@ -22,7 +22,7 @@ #pragma once #include -#include +#include #include #include #include @@ -41,7 +41,7 @@ namespace sofa::component::collision class MirrorIntersector; class SOFA_SOFABASECOLLISION_API BruteForceDetection : - public core::collision::BroadPhaseDetection, + public BruteForceBroadPhase, public core::collision::NarrowPhaseDetection { /// Range defined by two iterators in a container of CollisionElement @@ -54,50 +54,15 @@ class SOFA_SOFABASECOLLISION_API BruteForceDetection : using TestPair = std::pair< CollisionIteratorRange, CollisionIteratorRange >; public: - SOFA_CLASS2(BruteForceDetection, core::collision::BroadPhaseDetection, core::collision::NarrowPhaseDetection); - -private: - - /// vector of accumulated CollisionModel's when the collision pipeline asks - /// to add a CollisionModel in BruteForceDetection::addCollisionModel - /// This vector is emptied at each time step in BruteForceDetection::beginBroadPhase - sofa::helper::vector collisionModels; - - Data< helper::fixed_array > box; ///< if not empty, objects that do not intersect this bounding-box will be ignored - - CubeCollisionModel::SPtr boxModel; - + SOFA_CLASS2(BruteForceDetection, BruteForceBroadPhase, core::collision::NarrowPhaseDetection); protected: BruteForceDetection(); ~BruteForceDetection() override = default; - virtual bool keepCollisionBetween(core::CollisionModel *cm1, core::CollisionModel *cm2); - public: - void init() override; - void reinit() override; - - ////////////////////////////// - /// BROAD PHASE interface /// - ////////////////////////////// - - /** \brief In the broad phase, ignores collision with the provided collision model if possible and add pairs of - * collision models if in intersection. - * - * Ignore the collision with the provided collision model if it does not intersect with the box defined in - * the Data box when it is defined. - * Add the provided collision model to be investigated in the narrow phase in case of self collision. - * Check intersection with already added collision models. If it can intersect another collision model, the pair - * is added to be further investigated in the narrow phase. - */ - void addCollisionModel(core::CollisionModel *cm) override; - - void beginBroadPhase() override; - void endBroadPhase() override; - ////////////////////////////// /// NARROW PHASE interface /// ////////////////////////////// @@ -118,12 +83,6 @@ class SOFA_SOFABASECOLLISION_API BruteForceDetection : protected: - /// Return true if the provided CollisionModel intersect boxModel, false otherwise - bool intersectWithBoxModel(core::CollisionModel *cm) const; - - /// Return true if the provided CollisionModel can collide with itself - bool doesSelfCollide(core::CollisionModel *cm) const; - /// Return true if both collision models belong to the same object, false otherwise static bool isSelfCollision(core::CollisionModel* cm1, core::CollisionModel* cm2); From 2f1b2993db6aa65556a1baaf9ed514dae7e40487 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Fri, 23 Apr 2021 17:18:21 +0200 Subject: [PATCH 11/13] [SofaBaseCollision] Extract the narrow phase code from BruteForceDetection into a new component --- .../modules/SofaBaseCollision/CMakeLists.txt | 2 + .../src/SofaBaseCollision/BVHNarrowPhase.cpp | 341 ++++++++++++++++++ .../src/SofaBaseCollision/BVHNarrowPhase.h | 150 ++++++++ .../BruteForceBroadPhase.cpp | 2 +- .../SofaBaseCollision/BruteForceDetection.cpp | 308 +--------------- .../SofaBaseCollision/BruteForceDetection.h | 113 +----- 6 files changed, 499 insertions(+), 417 deletions(-) create mode 100644 SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.cpp create mode 100644 SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.h diff --git a/SofaKernel/modules/SofaBaseCollision/CMakeLists.txt b/SofaKernel/modules/SofaBaseCollision/CMakeLists.txt index 0faaa434ad2..79cb6891b8c 100644 --- a/SofaKernel/modules/SofaBaseCollision/CMakeLists.txt +++ b/SofaKernel/modules/SofaBaseCollision/CMakeLists.txt @@ -12,6 +12,7 @@ set(HEADER_FILES ${SOFABASECOLLISION_SRC}/BaseProximityIntersection.h ${SOFABASECOLLISION_SRC}/BruteForceBroadPhase.h ${SOFABASECOLLISION_SRC}/BruteForceDetection.h + ${SOFABASECOLLISION_SRC}/BVHNarrowPhase.h ${SOFABASECOLLISION_SRC}/CapsuleIntTool.h ${SOFABASECOLLISION_SRC}/CapsuleIntTool.inl ${SOFABASECOLLISION_SRC}/CapsuleModel.h @@ -54,6 +55,7 @@ set(SOURCE_FILES ${SOFABASECOLLISION_SRC}/BaseProximityIntersection.cpp ${SOFABASECOLLISION_SRC}/BruteForceBroadPhase.cpp ${SOFABASECOLLISION_SRC}/BruteForceDetection.cpp + ${SOFABASECOLLISION_SRC}/BVHNarrowPhase.cpp ${SOFABASECOLLISION_SRC}/CapsuleIntTool.cpp ${SOFABASECOLLISION_SRC}/CapsuleModel.cpp ${SOFABASECOLLISION_SRC}/ContactListener.cpp diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.cpp b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.cpp new file mode 100644 index 00000000000..51a988f6e72 --- /dev/null +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.cpp @@ -0,0 +1,341 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include + +#include +#include +#include + +namespace sofa::component::collision +{ + +int BVHNarrowPhaseClass = core::RegisterObject("Narrow phase collision detection based on boundary volume hierarchy") + .add< BVHNarrowPhase >() +; + +BVHNarrowPhase::BVHNarrowPhase() : core::collision::NarrowPhaseDetection() +{} + + +bool BVHNarrowPhase::isSelfCollision(core::CollisionModel* cm1, core::CollisionModel* cm2) +{ + return (cm1->getContext() == cm2->getContext()); +} + +void BVHNarrowPhase::addCollisionPair(const std::pair& cmPair) +{ + core::CollisionModel *cm1 = cmPair.first; //->getNext(); + core::CollisionModel *cm2 = cmPair.second; //->getNext(); + + if (!cm1->isSimulated() && !cm2->isSimulated()) + return; + + if (cm1->empty() || cm2->empty()) + return; + + core::CollisionModel *finestCollisionModel1 = cm1->getLast();//get the finest CollisionModel which is not a CubeModel + core::CollisionModel *finestCollisionModel2 = cm2->getLast(); + + const bool selfCollision = isSelfCollision(finestCollisionModel1, finestCollisionModel2); + + const std::string timerName = "BVHNarrowPhase addCollisionPair: " + finestCollisionModel1->getName() + " - " + finestCollisionModel2->getName(); + sofa::helper::ScopedAdvancedTimer bfTimer(timerName); + + bool swapModels = false; + core::collision::ElementIntersector* finestIntersector = intersectionMethod->findIntersector(finestCollisionModel1, finestCollisionModel2, swapModels);//find the method for the finest CollisionModels + if (finestIntersector == nullptr) + return; + if (swapModels) + { + std::swap(cm1, cm2); + std::swap(finestCollisionModel1, finestCollisionModel2); + } + + sofa::core::collision::DetectionOutputVector*& outputs = this->getDetectionOutputs(finestCollisionModel1, finestCollisionModel2); + + finestIntersector->beginIntersect(finestCollisionModel1, finestCollisionModel2, outputs);//creates outputs if null + + if (finestCollisionModel1 == cm1 || finestCollisionModel2 == cm2) + { + // The last model also contains the root element -> it does not only contains the final level of the tree + finestCollisionModel1 = nullptr; + finestCollisionModel2 = nullptr; + finestIntersector = nullptr; + } + + // Queue used for the iterative form of a tree traversal, avoiding the recursive form + std::queue< TestPair > externalCells; + initializeExternalCells(cm1, cm2, externalCells); + + core::collision::ElementIntersector* intersector = nullptr; + MirrorIntersector mirror; + cm1 = nullptr; // force later init of intersector + cm2 = nullptr; + + while (!externalCells.empty()) + { + TestPair root = externalCells.front(); + externalCells.pop(); + + processExternalCell(root, + cm1, cm2, + intersector, + {finestCollisionModel1, finestCollisionModel2, finestIntersector, selfCollision}, + &mirror, externalCells, outputs); + } +} + +void BVHNarrowPhase::initializeExternalCells( + core::CollisionModel *cm1, + core::CollisionModel *cm2, + std::queue& externalCells) +{ + //See CollisionModel::getInternalChildren(Index), CollisionModel::getExternalChildren(Index) and definition of CollisionModel class + const CollisionIteratorRange internalChildren1 = cm1->begin().getInternalChildren(); + const CollisionIteratorRange internalChildren2 = cm2->begin().getInternalChildren(); + const CollisionIteratorRange externalChildren1 = cm1->begin().getExternalChildren(); + const CollisionIteratorRange externalChildren2 = cm2->begin().getExternalChildren(); + + const auto addToExternalCells = [&externalCells]( + const CollisionIteratorRange& children1, + const CollisionIteratorRange& children2) + { + if (!isRangeEmpty(children1) && !isRangeEmpty(children2)) + { + externalCells.emplace(children1,children2); + } + }; + + addToExternalCells(internalChildren1, internalChildren2); + addToExternalCells(internalChildren1, externalChildren2); + addToExternalCells(externalChildren1, internalChildren2); + addToExternalCells(externalChildren1, externalChildren2); +} + +void BVHNarrowPhase::processExternalCell(const TestPair &externalCell, + core::CollisionModel *&cm1, + core::CollisionModel *&cm2, + core::collision::ElementIntersector *coarseIntersector, + const FinestCollision &finest, + MirrorIntersector *mirror, + std::queue &externalCells, + sofa::core::collision::DetectionOutputVector *&outputs) const +{ + const auto [collisionModel1, collisionModel2] = getCollisionModelsFromTestPair(externalCell); + + if (cm1 != collisionModel1 || cm2 != collisionModel2)//if the CollisionElements do not belong to cm1 and cm2, update cm1 and cm2 + { + cm1 = collisionModel1; + cm2 = collisionModel2; + if (!cm1 || !cm2) return; + + bool swapModels = false; + coarseIntersector = intersectionMethod->findIntersector(cm1, cm2, swapModels); + + if (coarseIntersector == nullptr) + { + msg_error() << "Error finding coarseIntersector " << intersectionMethod->getName() << " for "<getClassName()<<" - "<getClassName()<intersector = coarseIntersector; + coarseIntersector = mirror; + } + } + + if (coarseIntersector == nullptr) + return; + + // Stack used for the iterative form of a tree traversal, avoiding the recursive form + std::stack< TestPair > internalCells; + internalCells.push(externalCell); + + while (!internalCells.empty()) + { + TestPair current = internalCells.top(); + internalCells.pop(); + + processInternalCell(current, coarseIntersector, finest, externalCells, internalCells, outputs); + } +} + +void BVHNarrowPhase::processInternalCell(const TestPair &internalCell, + core::collision::ElementIntersector *coarseIntersector, + const FinestCollision &finest, + std::queue &externalCells, + std::stack &internalCells, + sofa::core::collision::DetectionOutputVector *&outputs) +{ + const auto [collisionModel1, collisionModel2] = getCollisionModelsFromTestPair(internalCell); + + if (collisionModel1 == finest.cm1 && collisionModel2 == finest.cm2) //the collision models are the finest ones + { + // Final collision pairs + finalCollisionPairs(internalCell, finest.selfCollision, coarseIntersector, outputs); + } + else + { + visitCollisionElements(internalCell, coarseIntersector, finest, externalCells, internalCells, outputs); + } +} + +void BVHNarrowPhase::visitCollisionElements(const TestPair &root, + core::collision::ElementIntersector *coarseIntersector, + const FinestCollision &finest, + std::queue &externalCells, + std::stack &internalCells, + sofa::core::collision::DetectionOutputVector *&outputs) +{ + const core::CollisionElementIterator begin1 = root.first.first; + const core::CollisionElementIterator end1 = root.first.second; + const core::CollisionElementIterator begin2 = root.second.first; + const core::CollisionElementIterator end2 = root.second.second; + + for (auto it1 = begin1; it1 != end1; ++it1) + { + for (auto it2 = begin2; it2 != end2; ++it2) + { + if (coarseIntersector->canIntersect(it1, it2)) + { + // Need to test recursively + // Note that an element cannot have both internal and external children + + TestPair newInternalTests(it1.getInternalChildren(), it2.getInternalChildren()); + + if (!isRangeEmpty(newInternalTests.first)) + { + if (!isRangeEmpty(newInternalTests.second)) + { + //both collision elements have internal children. They are added to the list + internalCells.push(std::move(newInternalTests)); + } + else + { + //only the first collision element has internal children. The second collision element + //is kept as it is + newInternalTests.second = {it2, it2 + 1}; + internalCells.push(std::move(newInternalTests)); + } + } + else + { + if (!isRangeEmpty(newInternalTests.second)) + { + //only the second collision element has internal children. The first collision element + //is kept as it is + newInternalTests.first = {it1, it1 + 1}; + internalCells.push(std::move(newInternalTests)); + } + else + { + // end of both internal tree of elements. + // need to test external children + visitExternalChildren(it1, it2, coarseIntersector, finest, externalCells, outputs); + } + } + } + } + } +} + +void BVHNarrowPhase::visitExternalChildren(const core::CollisionElementIterator &it1, + const core::CollisionElementIterator &it2, + core::collision::ElementIntersector *coarseIntersector, + const FinestCollision &finest, + std::queue &externalCells, + sofa::core::collision::DetectionOutputVector *&outputs) +{ + const TestPair externalChildren(it1.getExternalChildren(), it2.getExternalChildren()); + + const bool isExtChildrenRangeEmpty1 = isRangeEmpty(externalChildren.first); + const bool isExtChildrenRangeEmpty2 = isRangeEmpty(externalChildren.second); + + if (!isExtChildrenRangeEmpty1) + { + if (!isExtChildrenRangeEmpty2) + { + const auto [collisionModel1, collisionModel2] = getCollisionModelsFromTestPair(externalChildren); + if (collisionModel1 == finest.cm1 && collisionModel2 == finest.cm2) //the collision models are the finest ones + { + finalCollisionPairs(externalChildren, finest.selfCollision, finest.intersector, outputs); + } + else + { + externalCells.push(std::move(externalChildren)); + } + } + else + { + // only first element has external children + // test them against the second element + externalCells.emplace(externalChildren.first, std::make_pair(it2, it2 + 1)); + } + } + else if (!isExtChildrenRangeEmpty2) + { + // only second element has external children + // test them against the first element + externalCells.emplace(std::make_pair(it1, it1 + 1), externalChildren.second); + } + else + { + // No child -> final collision pair + if (!finest.selfCollision || it1.canCollideWith(it2)) + coarseIntersector->intersect(it1, it2, outputs); + } +} + +void BVHNarrowPhase::finalCollisionPairs(const TestPair& pair, + bool selfCollision, + core::collision::ElementIntersector* intersector, + sofa::core::collision::DetectionOutputVector*& outputs) +{ + const core::CollisionElementIterator begin1 = pair.first.first; + const core::CollisionElementIterator end1 = pair.first.second; + const core::CollisionElementIterator begin2 = pair.second.first; + const core::CollisionElementIterator end2 = pair.second.second; + + for (auto it1 = begin1; it1 != end1; ++it1) + { + for (auto it2 = begin2; it2 != end2; ++it2) + { + // Final collision pair + if (!selfCollision || it1.canCollideWith(it2)) + intersector->intersect(it1, it2, outputs); + } + } +} + +std::pair BVHNarrowPhase::getCollisionModelsFromTestPair(const TestPair& pair) +{ + auto* collisionModel1 = pair.first.first.getCollisionModel(); //get the first collision model + auto* collisionModel2 = pair.second.first.getCollisionModel(); //get the second collision model + return {collisionModel1, collisionModel2}; +} + +bool BVHNarrowPhase::isRangeEmpty(const CollisionIteratorRange& range) +{ + return range.first == range.second; +} +} //namespace sofa::component::collision diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.h b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.h new file mode 100644 index 00000000000..cc7591cf2e6 --- /dev/null +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.h @@ -0,0 +1,150 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include + +namespace sofa::core::collision +{ + class ElementIntersector; +} + +namespace sofa::component::collision +{ + +class MirrorIntersector; + +/** + * @brief Narrow phase collision detection based on bounding volume hierarchy + * + * The algorithm uses the result of a broad phase collision detection. For a pair of + * collision models, it traverses the hierarchy of bounding volumes in order to rapidly + * eliminate pairs of elements which are not in intersection. Finally, the intersection + * method is called on the remaining pairs of elements. + */ +class SOFA_SOFABASECOLLISION_API BVHNarrowPhase : public core::collision::NarrowPhaseDetection +{ +public: + SOFA_CLASS(BVHNarrowPhase, core::collision::NarrowPhaseDetection); + +protected: + BVHNarrowPhase(); + ~BVHNarrowPhase() override = default; + +private: + /// Range defined by two iterators in a container of CollisionElement + using CollisionIteratorRange = std::pair; + + /// A pair of CollisionIteratorRange where the first range contains elements from a first collision model + /// and the second range contains collision elements from a second collision model + /// This type is used when testing collision between two collision models + /// Note that the second collision model can be the same than the first in case of self collision + using TestPair = std::pair< CollisionIteratorRange, CollisionIteratorRange >; + +public: + + /** \brief In the narrow phase, examine a potential collision between a pair of collision models, which has + * been detected in the broad phase. + * + * The function traverses the hierarchy of CollisionElement's contained in each CollisionModel to avoid + * unnecessary pair intersections. + * An iterative form of the hierarchy traversal is adopted instead of a recursive form. + */ + void addCollisionPair(const std::pair& cmPair) override; + + void draw(const core::visual::VisualParams* /* vparams */) override { } + +protected: + + /// Return true if both collision models belong to the same object, false otherwise + static bool isSelfCollision(core::CollisionModel* cm1, core::CollisionModel* cm2); + + /// Build a list of TestPair's from internal and external children of two CollisionModel's + static void initializeExternalCells( + core::CollisionModel *cm1, + core::CollisionModel *cm2, + std::queue& externalCells); + + /// Store data related to two finest CollisionModel's + struct FinestCollision + { + core::CollisionModel* cm1 { nullptr }; + core::CollisionModel* cm2 { nullptr }; + + /// ElementIntersector corresponding to cm1 and cm2 + core::collision::ElementIntersector* intersector { nullptr }; + + // True in case cm1 and cm2 belong to the same object, false otherwise + bool selfCollision { false }; + }; + + void processExternalCell(const TestPair &externalCell, + core::CollisionModel *&cm1, + core::CollisionModel *&cm2, + core::collision::ElementIntersector *coarseIntersector, + const FinestCollision &finest, + MirrorIntersector *mirror, + std::queue &externalCells, + sofa::core::collision::DetectionOutputVector *&outputs) const; + + static void + processInternalCell(const TestPair &internalCell, + core::collision::ElementIntersector *coarseIntersector, + const FinestCollision &finest, + std::queue &externalCells, + std::stack &internalCells, + sofa::core::collision::DetectionOutputVector *&outputs); + + static void visitCollisionElements(const TestPair &root, + core::collision::ElementIntersector *coarseIntersector, + const FinestCollision &finest, + std::queue &externalCells, + std::stack &internalCells, + sofa::core::collision::DetectionOutputVector *&outputs); + + static void + visitExternalChildren(const core::CollisionElementIterator &it1, const core::CollisionElementIterator &it2, + core::collision::ElementIntersector *coarseIntersector, + const FinestCollision &finest, + std::queue &externalCells, + sofa::core::collision::DetectionOutputVector *&outputs); + + /// Test intersection between two ranges of CollisionElement's + /// The provided TestPair contains ranges of external CollisionElement's, which means that + /// they can be tested against each other for intersection + static void finalCollisionPairs(const TestPair& pair, + bool selfCollision, + core::collision::ElementIntersector* intersector, + sofa::core::collision::DetectionOutputVector*& outputs); + +private: + + /// Get both collision models corresponding to the provided TestPair + static std::pair getCollisionModelsFromTestPair(const TestPair& pair); + + static bool isRangeEmpty(const CollisionIteratorRange& range); +}; + +} //namespace sofa::component::collision \ No newline at end of file diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp index a3b3d10d1de..d32f3fe9a60 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceBroadPhase.cpp @@ -28,7 +28,7 @@ namespace sofa::component::collision { -int BruteForceBroadPhaseClass = core::RegisterObject("Collision detection using extensive pair-wise tests") +int BruteForceBroadPhaseClass = core::RegisterObject("Broad phase collision detection using extensive pair-wise tests") .add< BruteForceBroadPhase >() ; diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp index 3390367d07f..c1c5b6f7f22 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp @@ -24,7 +24,6 @@ #include #include #include -#include namespace sofa::component::collision { @@ -33,317 +32,14 @@ using namespace sofa::defaulttype; using namespace sofa::helper; using namespace collision; -int BruteForceDetectionClass = core::RegisterObject("Collision detection using extensive pair-wise tests") +int BruteForceDetectionClass = core::RegisterObject("Combination of brute force broad phase and BVH narrow phase collision detection") .add< BruteForceDetection >() ; -using namespace core::objectmodel; - BruteForceDetection::BruteForceDetection() : BruteForceBroadPhase() - , core::collision::NarrowPhaseDetection() + , BVHNarrowPhase() {} -bool BruteForceDetection::isSelfCollision(core::CollisionModel* cm1, core::CollisionModel* cm2) -{ - return (cm1->getContext() == cm2->getContext()); -} - -void BruteForceDetection::addCollisionPair(const std::pair& cmPair) -{ - core::CollisionModel *cm1 = cmPair.first; //->getNext(); - core::CollisionModel *cm2 = cmPair.second; //->getNext(); - - if (!cm1->isSimulated() && !cm2->isSimulated()) - return; - - if (cm1->empty() || cm2->empty()) - return; - - core::CollisionModel *finestCollisionModel1 = cm1->getLast();//get the finest CollisionModel which is not a CubeModel - core::CollisionModel *finestCollisionModel2 = cm2->getLast(); - - const bool selfCollision = isSelfCollision(finestCollisionModel1, finestCollisionModel2); - - const std::string timerName = "BruteForceDetection addCollisionPair: " + finestCollisionModel1->getName() + " - " + finestCollisionModel2->getName(); - sofa::helper::ScopedAdvancedTimer bfTimer(timerName); - - bool swapModels = false; - core::collision::ElementIntersector* finestIntersector = intersectionMethod->findIntersector(finestCollisionModel1, finestCollisionModel2, swapModels);//find the method for the finest CollisionModels - if (finestIntersector == nullptr) - return; - if (swapModels) - { - std::swap(cm1, cm2); - std::swap(finestCollisionModel1, finestCollisionModel2); - } - - sofa::core::collision::DetectionOutputVector*& outputs = this->getDetectionOutputs(finestCollisionModel1, finestCollisionModel2); - - finestIntersector->beginIntersect(finestCollisionModel1, finestCollisionModel2, outputs);//creates outputs if null - - if (finestCollisionModel1 == cm1 || finestCollisionModel2 == cm2) - { - // The last model also contains the root element -> it does not only contains the final level of the tree - finestCollisionModel1 = nullptr; - finestCollisionModel2 = nullptr; - finestIntersector = nullptr; - } - - // Queue used for the iterative form of a tree traversal, avoiding the recursive form - std::queue< TestPair > externalCells; - initializeExternalCells(cm1, cm2, externalCells); - - core::collision::ElementIntersector* intersector = nullptr; - MirrorIntersector mirror; - cm1 = nullptr; // force later init of intersector - cm2 = nullptr; - - while (!externalCells.empty()) - { - TestPair root = externalCells.front(); - externalCells.pop(); - - processExternalCell(root, - cm1, cm2, - intersector, - {finestCollisionModel1, finestCollisionModel2, finestIntersector, selfCollision}, - &mirror, externalCells, outputs); - } -} - -void BruteForceDetection::initializeExternalCells( - core::CollisionModel *cm1, - core::CollisionModel *cm2, - std::queue& externalCells) -{ - //See CollisionModel::getInternalChildren(Index), CollisionModel::getExternalChildren(Index) and definition of CollisionModel class - const CollisionIteratorRange internalChildren1 = cm1->begin().getInternalChildren(); - const CollisionIteratorRange internalChildren2 = cm2->begin().getInternalChildren(); - const CollisionIteratorRange externalChildren1 = cm1->begin().getExternalChildren(); - const CollisionIteratorRange externalChildren2 = cm2->begin().getExternalChildren(); - - const auto addToExternalCells = [&externalCells]( - const CollisionIteratorRange& children1, - const CollisionIteratorRange& children2) - { - if (!isRangeEmpty(children1) && !isRangeEmpty(children2)) - { - externalCells.emplace(children1,children2); - } - }; - - addToExternalCells(internalChildren1, internalChildren2); - addToExternalCells(internalChildren1, externalChildren2); - addToExternalCells(externalChildren1, internalChildren2); - addToExternalCells(externalChildren1, externalChildren2); -} - -void BruteForceDetection::processExternalCell(const TestPair &externalCell, - core::CollisionModel *&cm1, - core::CollisionModel *&cm2, - core::collision::ElementIntersector *coarseIntersector, - const FinestCollision &finest, - MirrorIntersector *mirror, - std::queue &externalCells, - sofa::core::collision::DetectionOutputVector *&outputs) const -{ - const auto [collisionModel1, collisionModel2] = getCollisionModelsFromTestPair(externalCell); - - if (cm1 != collisionModel1 || cm2 != collisionModel2)//if the CollisionElements do not belong to cm1 and cm2, update cm1 and cm2 - { - cm1 = collisionModel1; - cm2 = collisionModel2; - if (!cm1 || !cm2) return; - - bool swapModels = false; - coarseIntersector = intersectionMethod->findIntersector(cm1, cm2, swapModels); - - if (coarseIntersector == nullptr) - { - msg_error() << "Error finding coarseIntersector " << intersectionMethod->getName() << " for "<getClassName()<<" - "<getClassName()<intersector = coarseIntersector; - coarseIntersector = mirror; - } - } - - if (coarseIntersector == nullptr) - return; - - // Stack used for the iterative form of a tree traversal, avoiding the recursive form - std::stack< TestPair > internalCells; - internalCells.push(externalCell); - - while (!internalCells.empty()) - { - TestPair current = internalCells.top(); - internalCells.pop(); - - processInternalCell(current, coarseIntersector, finest, externalCells, internalCells, outputs); - } -} - -void BruteForceDetection::processInternalCell(const TestPair &internalCell, - core::collision::ElementIntersector *coarseIntersector, - const FinestCollision &finest, - std::queue &externalCells, - std::stack &internalCells, - sofa::core::collision::DetectionOutputVector *&outputs) -{ - const auto [collisionModel1, collisionModel2] = getCollisionModelsFromTestPair(internalCell); - - if (collisionModel1 == finest.cm1 && collisionModel2 == finest.cm2) //the collision models are the finest ones - { - // Final collision pairs - finalCollisionPairs(internalCell, finest.selfCollision, coarseIntersector, outputs); - } - else - { - visitCollisionElements(internalCell, coarseIntersector, finest, externalCells, internalCells, outputs); - } -} - -void BruteForceDetection::visitCollisionElements(const TestPair &root, - core::collision::ElementIntersector *coarseIntersector, - const FinestCollision &finest, - std::queue &externalCells, - std::stack &internalCells, - sofa::core::collision::DetectionOutputVector *&outputs) -{ - const core::CollisionElementIterator begin1 = root.first.first; - const core::CollisionElementIterator end1 = root.first.second; - const core::CollisionElementIterator begin2 = root.second.first; - const core::CollisionElementIterator end2 = root.second.second; - - for (auto it1 = begin1; it1 != end1; ++it1) - { - for (auto it2 = begin2; it2 != end2; ++it2) - { - if (coarseIntersector->canIntersect(it1, it2)) - { - // Need to test recursively - // Note that an element cannot have both internal and external children - - TestPair newInternalTests(it1.getInternalChildren(), it2.getInternalChildren()); - - if (!isRangeEmpty(newInternalTests.first)) - { - if (!isRangeEmpty(newInternalTests.second)) - { - //both collision elements have internal children. They are added to the list - internalCells.push(std::move(newInternalTests)); - } - else - { - //only the first collision element has internal children. The second collision element - //is kept as it is - newInternalTests.second = {it2, it2 + 1}; - internalCells.push(std::move(newInternalTests)); - } - } - else - { - if (!isRangeEmpty(newInternalTests.second)) - { - //only the second collision element has internal children. The first collision element - //is kept as it is - newInternalTests.first = {it1, it1 + 1}; - internalCells.push(std::move(newInternalTests)); - } - else - { - // end of both internal tree of elements. - // need to test external children - visitExternalChildren(it1, it2, coarseIntersector, finest, externalCells, outputs); - } - } - } - } - } -} - -void BruteForceDetection::visitExternalChildren(const core::CollisionElementIterator &it1, - const core::CollisionElementIterator &it2, - core::collision::ElementIntersector *coarseIntersector, - const FinestCollision &finest, - std::queue &externalCells, - sofa::core::collision::DetectionOutputVector *&outputs) -{ - const TestPair externalChildren(it1.getExternalChildren(), it2.getExternalChildren()); - - const bool isExtChildrenRangeEmpty1 = isRangeEmpty(externalChildren.first); - const bool isExtChildrenRangeEmpty2 = isRangeEmpty(externalChildren.second); - - if (!isExtChildrenRangeEmpty1) - { - if (!isExtChildrenRangeEmpty2) - { - const auto [collisionModel1, collisionModel2] = getCollisionModelsFromTestPair(externalChildren); - if (collisionModel1 == finest.cm1 && collisionModel2 == finest.cm2) //the collision models are the finest ones - { - finalCollisionPairs(externalChildren, finest.selfCollision, finest.intersector, outputs); - } - else - { - externalCells.push(std::move(externalChildren)); - } - } - else - { - // only first element has external children - // test them against the second element - externalCells.emplace(externalChildren.first, std::make_pair(it2, it2 + 1)); - } - } - else if (!isExtChildrenRangeEmpty2) - { - // only second element has external children - // test them against the first element - externalCells.emplace(std::make_pair(it1, it1 + 1), externalChildren.second); - } - else - { - // No child -> final collision pair - if (!finest.selfCollision || it1.canCollideWith(it2)) - coarseIntersector->intersect(it1, it2, outputs); - } -} - -void BruteForceDetection::finalCollisionPairs(const TestPair& pair, - bool selfCollision, - core::collision::ElementIntersector* intersector, - sofa::core::collision::DetectionOutputVector*& outputs) -{ - const core::CollisionElementIterator begin1 = pair.first.first; - const core::CollisionElementIterator end1 = pair.first.second; - const core::CollisionElementIterator begin2 = pair.second.first; - const core::CollisionElementIterator end2 = pair.second.second; - - for (auto it1 = begin1; it1 != end1; ++it1) - { - for (auto it2 = begin2; it2 != end2; ++it2) - { - // Final collision pair - if (!selfCollision || it1.canCollideWith(it2)) - intersector->intersect(it1, it2, outputs); - } - } -} - -std::pair BruteForceDetection::getCollisionModelsFromTestPair(const TestPair& pair) -{ - auto* collisionModel1 = pair.first.first.getCollisionModel(); //get the first collision model - auto* collisionModel2 = pair.second.first.getCollisionModel(); //get the second collision model - return {collisionModel1, collisionModel2}; -} - -bool BruteForceDetection::isRangeEmpty(const CollisionIteratorRange& range) -{ - return range.first == range.second; -} } // namespace sofa::component::collision diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h index 4cc908a4e98..4c5ed92159a 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h @@ -23,133 +23,26 @@ #include #include -#include +#include #include #include -#include -#include - -namespace sofa::core::collision -{ - class ElementIntersector; -} - namespace sofa::component::collision { -class MirrorIntersector; - class SOFA_SOFABASECOLLISION_API BruteForceDetection : public BruteForceBroadPhase, - public core::collision::NarrowPhaseDetection + public BVHNarrowPhase { - /// Range defined by two iterators in a container of CollisionElement - using CollisionIteratorRange = std::pair; - - /// A pair of CollisionIteratorRange where the first range contains elements from a first collision model - /// and the second range contains collision elements from a second collision model - /// This type is used when testing collision between two collision models - /// Note that the second collision model can be the same than the first in case of self collision - using TestPair = std::pair< CollisionIteratorRange, CollisionIteratorRange >; public: - SOFA_CLASS2(BruteForceDetection, BruteForceBroadPhase, core::collision::NarrowPhaseDetection); + SOFA_CLASS2(BruteForceDetection, BruteForceBroadPhase, BVHNarrowPhase); protected: BruteForceDetection(); ~BruteForceDetection() override = default; -public: - - ////////////////////////////// - /// NARROW PHASE interface /// - ////////////////////////////// - - /** \brief In the narrow phase, examine a potential collision between a pair of collision models, which has - * been detected in the broad phase. - * - * The function traverses the hierarchy of CollisionElement's contained in each CollisionModel to avoid - * unnecessary pair intersections. - * An iterative form of the hierarchy traversal is adopted instead of a recursive form. - */ - void addCollisionPair(const std::pair& cmPair) override; - - - void draw(const core::visual::VisualParams* /* vparams */) override { } - - inline bool needsDeepBoundingTree() const override { return true; } - -protected: - - /// Return true if both collision models belong to the same object, false otherwise - static bool isSelfCollision(core::CollisionModel* cm1, core::CollisionModel* cm2); - - /// Build a list of TestPair's from internal and external children of two CollisionModel's - static void initializeExternalCells( - core::CollisionModel *cm1, - core::CollisionModel *cm2, - std::queue& externalCells); - - /// Store data related to two finest CollisionModel's - struct FinestCollision - { - core::CollisionModel* cm1 { nullptr }; - core::CollisionModel* cm2 { nullptr }; - - /// ElementIntersector corresponding to cm1 and cm2 - core::collision::ElementIntersector* intersector { nullptr }; - - // True in case cm1 and cm2 belong to the same object, false otherwise - bool selfCollision { false }; - }; - - void processExternalCell(const TestPair &externalCell, - core::CollisionModel *&cm1, - core::CollisionModel *&cm2, - core::collision::ElementIntersector *coarseIntersector, - const FinestCollision &finest, - MirrorIntersector *mirror, - std::queue &externalCells, - sofa::core::collision::DetectionOutputVector *&outputs) const; - - static void - processInternalCell(const TestPair &internalCell, - core::collision::ElementIntersector *coarseIntersector, - const FinestCollision &finest, - std::queue &externalCells, - std::stack &internalCells, - sofa::core::collision::DetectionOutputVector *&outputs); - - static void visitCollisionElements(const TestPair &root, - core::collision::ElementIntersector *coarseIntersector, - const FinestCollision &finest, - std::queue &externalCells, - std::stack &internalCells, - sofa::core::collision::DetectionOutputVector *&outputs); - - static void - visitExternalChildren(const core::CollisionElementIterator &it1, const core::CollisionElementIterator &it2, - core::collision::ElementIntersector *coarseIntersector, - const FinestCollision &finest, - std::queue &externalCells, - sofa::core::collision::DetectionOutputVector *&outputs); - - /// Test intersection between two ranges of CollisionElement's - /// The provided TestPair contains ranges of external CollisionElement's, which means that - /// they can be tested against each other for intersection - static void finalCollisionPairs(const TestPair& pair, - bool selfCollision, - core::collision::ElementIntersector* intersector, - sofa::core::collision::DetectionOutputVector*& outputs); - -private: - - /// Get both collision models corresponding to the provided TestPair - static std::pair getCollisionModelsFromTestPair(const TestPair& pair); - - static bool isRangeEmpty(const CollisionIteratorRange& range); }; } // namespace sofa::component::collision From 9fe0db5f8301cc57227ff7818008012e98a501e4 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Fri, 23 Apr 2021 17:26:12 +0200 Subject: [PATCH 12/13] [SofaBaseCollision] Clean up --- .../src/SofaBaseCollision/BruteForceDetection.cpp | 7 ------- .../src/SofaBaseCollision/BruteForceDetection.h | 2 -- 2 files changed, 9 deletions(-) diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp index c1c5b6f7f22..60b3d6bd9c8 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.cpp @@ -21,17 +21,11 @@ ******************************************************************************/ #include -#include #include -#include namespace sofa::component::collision { -using namespace sofa::defaulttype; -using namespace sofa::helper; -using namespace collision; - int BruteForceDetectionClass = core::RegisterObject("Combination of brute force broad phase and BVH narrow phase collision detection") .add< BruteForceDetection >() ; @@ -41,5 +35,4 @@ BruteForceDetection::BruteForceDetection() , BVHNarrowPhase() {} - } // namespace sofa::component::collision diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h index 4c5ed92159a..a8a04cf6996 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BruteForceDetection.h @@ -24,8 +24,6 @@ #include #include -#include -#include namespace sofa::component::collision { From a9bde4991df9ac2f7cf3412b381e1159b882cd50 Mon Sep 17 00:00:00 2001 From: alxbilger Date: Tue, 27 Apr 2021 16:07:02 +0200 Subject: [PATCH 13/13] [MultiThreading] Introduce ParallelBVHNarrowPhase --- .../src/SofaBaseCollision/BVHNarrowPhase.cpp | 3 - .../plugins/MultiThreading/CMakeLists.txt | 2 + .../examples/ParallelBruteForceBroadPhase.scn | 7 +- .../MultiThreading/ParallelBVHNarrowPhase.cpp | 173 ++++++++++++++++++ .../MultiThreading/ParallelBVHNarrowPhase.h | 79 ++++++++ 5 files changed, 260 insertions(+), 4 deletions(-) create mode 100644 applications/plugins/MultiThreading/src/MultiThreading/ParallelBVHNarrowPhase.cpp create mode 100644 applications/plugins/MultiThreading/src/MultiThreading/ParallelBVHNarrowPhase.h diff --git a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.cpp b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.cpp index 51a988f6e72..ac3c852b06f 100644 --- a/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.cpp +++ b/SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.cpp @@ -58,9 +58,6 @@ void BVHNarrowPhase::addCollisionPair(const std::pairgetName() + " - " + finestCollisionModel2->getName(); - sofa::helper::ScopedAdvancedTimer bfTimer(timerName); - bool swapModels = false; core::collision::ElementIntersector* finestIntersector = intersectionMethod->findIntersector(finestCollisionModel1, finestCollisionModel2, swapModels);//find the method for the finest CollisionModels if (finestIntersector == nullptr) diff --git a/applications/plugins/MultiThreading/CMakeLists.txt b/applications/plugins/MultiThreading/CMakeLists.txt index 231f1111c5e..d636e84101e 100644 --- a/applications/plugins/MultiThreading/CMakeLists.txt +++ b/applications/plugins/MultiThreading/CMakeLists.txt @@ -13,6 +13,7 @@ set(HEADER_FILES src/MultiThreading/MeanComputation.h src/MultiThreading/MeanComputation.inl src/MultiThreading/ParallelBruteForceBroadPhase.h + src/MultiThreading/ParallelBVHNarrowPhase.h ) set(SOURCE_FILES @@ -23,6 +24,7 @@ set(SOURCE_FILES src/MultiThreading/DataExchange.cpp src/MultiThreading/MeanComputation.cpp src/MultiThreading/ParallelBruteForceBroadPhase.cpp + src/MultiThreading/ParallelBVHNarrowPhase.cpp ) find_package(SofaMiscMapping REQUIRED) diff --git a/applications/plugins/MultiThreading/examples/ParallelBruteForceBroadPhase.scn b/applications/plugins/MultiThreading/examples/ParallelBruteForceBroadPhase.scn index cdcd13924fe..3370b73faf6 100644 --- a/applications/plugins/MultiThreading/examples/ParallelBruteForceBroadPhase.scn +++ b/applications/plugins/MultiThreading/examples/ParallelBruteForceBroadPhase.scn @@ -28,8 +28,13 @@ On a CPU with 6 cores, each core tests 3906 pairs of collision models. + - + + + + + diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBVHNarrowPhase.cpp b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBVHNarrowPhase.cpp new file mode 100644 index 00000000000..dbb61a2a9be --- /dev/null +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBVHNarrowPhase.cpp @@ -0,0 +1,173 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include + +#include +#include +#include +#include +#include +#include + +namespace sofa::component::collision +{ + +using sofa::helper::ScopedAdvancedTimer; + +int ParallelBVHNarrowPhaseClass = core::RegisterObject("Narrow phase collision detection based on boundary volume hierarchy") + .add< ParallelBVHNarrowPhase >() +; + +ParallelBVHNarrowPhase::ParallelBVHNarrowPhase() +{} + +void ParallelBVHNarrowPhase::init() +{ + NarrowPhaseDetection::init(); + + // initialize the thread pool + + auto* taskScheduler = sofa::simulation::TaskScheduler::getInstance(); + assert(taskScheduler != nullptr); + if (taskScheduler->getThreadCount() < 1) + { + taskScheduler->init(0); + msg_info() << "Task scheduler initialized on " << taskScheduler->getThreadCount() << " threads"; + } + else + { + msg_info() << "Task scheduler already initialized on " << taskScheduler->getThreadCount() << " threads"; + } +} + +void ParallelBVHNarrowPhase::addCollisionPairs(const sofa::helper::vector< std::pair >& v) +{ + ScopedAdvancedTimer createTasksTimer("addCollisionPairs"); + + if (v.empty()) + { + return; + } + + auto *taskScheduler = sofa::simulation::TaskScheduler::getInstance(); + assert(taskScheduler != nullptr); + + if (taskScheduler->getThreadCount() == 0) + { + msg_error() << "Task scheduler not correctly initialized"; + return; + } + + // initialize output + createOutput(v); + + sofa::simulation::CpuTask::Status status; + const auto nbPairs = static_cast(v.size()); + m_tasks.reserve(nbPairs); + + { + ScopedAdvancedTimer createTasksTimer("TasksCreation"); + for (const auto &pair : v) + { + m_tasks.emplace_back(&status, this, pair); + taskScheduler->addTask(&m_tasks.back()); + } + } + + { + ScopedAdvancedTimer waitTimer("ParallelTasks"); + taskScheduler->workUntilDone(&status); + } + + m_tasks.clear(); + + // m_outputsMap should just be filled in addCollisionPair function + m_primitiveTestCount = m_outputsMap.size(); +} + +void ParallelBVHNarrowPhase::createOutput( + const helper::vector > &v) +{ + ScopedAdvancedTimer createTasksTimer("OutputCreation"); + + for (const auto &pair : v) + { + core::CollisionModel *cm1 = pair.first; + core::CollisionModel *cm2 = pair.second; + + core::CollisionModel *finestCollisionModel1 = cm1->getLast();//get the finest CollisionModel which is not a CubeModel + core::CollisionModel *finestCollisionModel2 = cm2->getLast(); + + initializeTopology(finestCollisionModel1->getCollisionTopology()); + initializeTopology(finestCollisionModel2->getCollisionTopology()); + + bool swapModels = false; + core::collision::ElementIntersector *finestIntersector = intersectionMethod->findIntersector( + finestCollisionModel1, finestCollisionModel2, + swapModels);//find the method for the finest CollisionModels + if (finestIntersector == nullptr) + continue; + if (swapModels) + { + std::swap(cm1, cm2); + std::swap(finestCollisionModel1, finestCollisionModel2); + } + + //force the creation of all Detection Output before the parallel computation + getDetectionOutputs(finestCollisionModel1, finestCollisionModel2); + }; +} + +void ParallelBVHNarrowPhase::initializeTopology(sofa::core::topology::BaseMeshTopology* topology) +{ + auto insertionIt = m_initializedTopology.insert(topology); + if (insertionIt.second) + { + // The following calls force the creation of some topology arrays before the concurrent computing. + // Those arrays cannot be created on the fly, in a concurrent environment, + // due to possible race conditions. + // Depending on the scene graph, it is possible that those calls are not enough. + topology->getTrianglesAroundVertex(0); + topology->getEdgesInTriangle(0); + topology->getTrianglesAroundEdge(0); + } +} + +ParallelBVHNarrowPhasePairTask::ParallelBVHNarrowPhasePairTask( + sofa::simulation::CpuTask::Status* status, + ParallelBVHNarrowPhase* bvhNarrowPhase, + std::pair pair) + : sofa::simulation::CpuTask(status) + , m_bvhNarrowPhase(bvhNarrowPhase) + , m_pair(pair) +{} + +sofa::simulation::Task::MemoryAlloc ParallelBVHNarrowPhasePairTask::run() +{ + assert(m_bvhNarrowPhase != nullptr); + + m_bvhNarrowPhase->addCollisionPair(m_pair); + + return simulation::Task::Stack; +} + +} \ No newline at end of file diff --git a/applications/plugins/MultiThreading/src/MultiThreading/ParallelBVHNarrowPhase.h b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBVHNarrowPhase.h new file mode 100644 index 00000000000..d2bdabde9f2 --- /dev/null +++ b/applications/plugins/MultiThreading/src/MultiThreading/ParallelBVHNarrowPhase.h @@ -0,0 +1,79 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include + +#include +#include +#include + +namespace sofa::component::collision +{ + +class ParallelBVHNarrowPhasePairTask; + +class SOFA_MULTITHREADING_PLUGIN_API ParallelBVHNarrowPhase : public BVHNarrowPhase +{ +public: + SOFA_CLASS(ParallelBVHNarrowPhase, BVHNarrowPhase); + +protected: + ParallelBVHNarrowPhase(); + + std::vector m_tasks; + + std::unordered_set< sofa::core::topology::BaseMeshTopology* > m_initializedTopology; + std::set< std::pair > m_initializedPairs; + +public: + + void init() override; + void addCollisionPairs(const sofa::helper::vector< std::pair >& v) override; + +private: + + /// Unlike the sequential algorithm which creates the output on the fly, the parallel implementation + /// requires to create the outputs before the computation, in order to avoid iterators invalidation + void createOutput(const helper::vector > &v); + + /// This function makes sure some topology arrays are initialized. They cannot be initialized concurrently + void initializeTopology(sofa::core::topology::BaseMeshTopology*); +}; + +class SOFA_MULTITHREADING_PLUGIN_API ParallelBVHNarrowPhasePairTask : public sofa::simulation::CpuTask +{ +public: + ParallelBVHNarrowPhasePairTask( + sofa::simulation::CpuTask::Status* status, + ParallelBVHNarrowPhase* bvhNarrowPhase, + std::pair pair); + ~ParallelBVHNarrowPhasePairTask() override = default; + sofa::simulation::Task::MemoryAlloc run() final; + +private: + + ParallelBVHNarrowPhase* m_bvhNarrowPhase { nullptr }; + std::pair m_pair; +}; + +} //namespace sofa::component::collision \ No newline at end of file