[MultiThreading] Parallel BVH narrow phase

SofaKernel/modules/SofaBaseCollision/src/SofaBaseCollision/BVHNarrowPhase.cpp

@@ -58,9 +58,6 @@ void BVHNarrowPhase::addCollisionPair(const std::pair<core::CollisionModel*, cor
 
     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)

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)

applications/plugins/MultiThreading/examples/ParallelBruteForceBroadPhase.scn

@@ -28,8 +28,13 @@ On a CPU with 6 cores, each core tests 3906 pairs of collision models.
     <!-- Basic Components to perform the collision detection -->
     <FreeMotionAnimationLoop name="FreeMotionAnimationLoop" parallel="true" />
     <DefaultPipeline name="CollisionPipeline" />
+
     <ParallelBruteForceBroadPhase/>
-    <BruteForceDetection name="BruteForceDetection" />
+    <ParallelBVHNarrowPhase/>
+
+<!--    <BruteForceBroadPhase/>-->
+<!--    <BVHNarrowPhase/>-->
+
     <LocalMinDistance name="Proximity" alarmDistance="0.2" contactDistance="0.09" angleCone="0.0" />
     <DefaultContactManager name="Response" response="FrictionContact" />
     <LCPConstraintSolver maxIt="1000" tolerance="0.001"/>

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 <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: contact@sofa-framework.org                             *
+******************************************************************************/
+#include <MultiThreading/ParallelBVHNarrowPhase.h>
+
+#include <sofa/core/ObjectFactory.h>
+#include <sofa/simulation/TaskScheduler.h>
+#include <sofa/helper/ScopedAdvancedTimer.h>
+#include <sofa/core/CollisionModel.h>
+#include <sofa/core/collision/Intersection.h>
+#include <SofaBaseTopology/MeshTopology.h>
+
+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<core::CollisionModel*, core::CollisionModel*> >& 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<unsigned int>(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 <std::pair<core::CollisionModel *, core::CollisionModel *>> &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<core::CollisionModel*, core::CollisionModel*> 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;
+}
+
+}

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 <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: contact@sofa-framework.org                             *
+******************************************************************************/
+#pragma once
+
+#include <MultiThreading/config.h>
+
+#include <SofaBaseCollision/BVHNarrowPhase.h>
+#include <sofa/simulation/CpuTask.h>
+#include <unordered_set>
+
+namespace sofa::component::collision
+{
+
+class ParallelBVHNarrowPhasePairTask;
+
+class SOFA_MULTITHREADING_PLUGIN_API ParallelBVHNarrowPhase : public BVHNarrowPhase
+{
+public:
+    SOFA_CLASS(ParallelBVHNarrowPhase, BVHNarrowPhase);
+
+protected:
+    ParallelBVHNarrowPhase();
+
+    std::vector<ParallelBVHNarrowPhasePairTask> m_tasks;
+
+    std::unordered_set< sofa::core::topology::BaseMeshTopology* > m_initializedTopology;
+    std::set< std::pair<core::CollisionModel*, core::CollisionModel*> > m_initializedPairs;
+
+public:
+
+    void init() override;
+    void addCollisionPairs(const sofa::helper::vector< std::pair<core::CollisionModel*, core::CollisionModel*> >& 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 <std::pair<core::CollisionModel *, core::CollisionModel *>> &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<core::CollisionModel*, core::CollisionModel*> pair);
+    ~ParallelBVHNarrowPhasePairTask() override = default;
+    sofa::simulation::Task::MemoryAlloc run() final;
+
+private:
+
+    ParallelBVHNarrowPhase* m_bvhNarrowPhase { nullptr };
+    std::pair<core::CollisionModel*, core::CollisionModel*> m_pair;
+};
+
+} //namespace sofa::component::collision