[All] Clean collision detection in FreeMotionAnimationLoop (#5042)

* Divided constraint visitors and removed timers in pipeline (need to add them in the animation loops * Chaged to SCOPED_TIMER * Fix for SOFA_DUMP_VISITOR_INFO * Update Sofa/framework/Simulation/Core/src/sofa/simulation/CollisionVisitor.h Co-authored-by: Hugo <[email protected]> * Update Sofa/framework/Simulation/Core/src/sofa/simulation/CollisionVisitor.h Co-authored-by: Hugo <[email protected]> * Update Sofa/framework/Simulation/Core/src/sofa/simulation/CollisionVisitor.h Co-authored-by: Hugo <[email protected]> --------- Co-authored-by: Hugo <[email protected]>
sofa-framework · Nov 13, 2024 · ba947da · ba947da
1 parent f9afdf6
commit ba947da
Show file tree

Hide file tree

Showing 4 changed files with 112 additions and 46 deletions.
diff --git a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp
@@ -352,7 +352,14 @@ void FreeMotionAnimationLoop::computeFreeMotionAndCollisionDetection(const sofa:
         preCollisionComputation(params);
 
         {
-            ScopedAdvancedTimer collisionResetTimer("CollisionReset");
+            SCOPED_TIMER("ProcessGeometricalData");
+            ProcessGeometricalDataVisitor act(params);
+            act.setTags(this->getTags());
+            act.execute(node);
+        }
+
+        {
+            SCOPED_TIMER("CollisionReset");
             CollisionResetVisitor act(params);
             act.setTags(this->getTags());
             act.execute(node);
@@ -362,19 +369,19 @@ void FreeMotionAnimationLoop::computeFreeMotionAndCollisionDetection(const sofa:
         taskScheduler->addTask(freeMotionTaskStatus, [&]() { computeFreeMotion(params, cparams, dt, pos, freePos, freeVel, mop); });
 
         {
-            ScopedAdvancedTimer collisionDetectionTimer("CollisionDetection");
+            SCOPED_TIMER("CollisionDetection");
             CollisionDetectionVisitor act(params);
             act.setTags(this->getTags());
             act.execute(node);
         }
 
         {
-            ScopedAdvancedTimer waitFreeMotionTimer("WaitFreeMotion");
+            SCOPED_TIMER("WaitFreeMotion");
             taskScheduler->workUntilDone(&freeMotionTaskStatus);
         }
 
         {
-            ScopedAdvancedTimer collisionResponseTimer("CollisionResponse");
+            SCOPED_TIMER("CollisionResponse");
             CollisionResponseVisitor act(params);
             act.setTags(this->getTags());
             act.execute(node);

diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/CollisionVisitor.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/CollisionVisitor.cpp
@@ -30,34 +30,12 @@ namespace sofa::simulation
 {
 
 
-Visitor::Result CollisionVisitor::processNodeTopDown(simulation::Node* node)
-{
-    for_each(this, node, node->constraintSet, &CollisionVisitor::fwdConstraintSet);
-    for_each(this, node, node->collisionPipeline, &CollisionVisitor::processCollisionPipeline);
-    return RESULT_CONTINUE;
-}
 
-void CollisionVisitor::fwdConstraintSet(simulation::Node*
+void BaseCollisionVisitor::processCollisionPipeline(simulation::Node*
 #ifdef SOFA_DUMP_VISITOR_INFO
-        node
+                                                    node
 #endif
-        , core::behavior::BaseConstraintSet* c)
-{
-#ifdef SOFA_DUMP_VISITOR_INFO
-    printComment("computeCollisionDetectionInConstraints");
-    ctime_t t0=begin(node, c);
-#endif
-    c->processGeometricalData();
-#ifdef SOFA_DUMP_VISITOR_INFO
-    end(node, c,t0);
-#endif
-}
-
-void CollisionVisitor::processCollisionPipeline(simulation::Node*
-#ifdef SOFA_DUMP_VISITOR_INFO
-        node
-#endif
-        , core::collision::Pipeline* obj)
+                                                , core::collision::Pipeline* obj)
 {
     //msg_info()<<"CollisionVisitor::processCollisionPipeline"<<std::endl;
 #ifdef SOFA_DUMP_VISITOR_INFO
@@ -91,6 +69,62 @@ void CollisionVisitor::processCollisionPipeline(simulation::Node*
 #endif
 }
 
+
+
+Visitor::Result BaseCollisionVisitor::processNodeTopDown(simulation::Node* node)
+{
+    for_each(this, node, node->collisionPipeline, &BaseCollisionVisitor::processCollisionPipeline);
+    return RESULT_CONTINUE;
+}
+
+
+void CollisionVisitor::fwdConstraintSet(simulation::Node*
+#ifdef SOFA_DUMP_VISITOR_INFO
+                                            node
+#endif
+                                        , core::behavior::BaseConstraintSet* c)
+{
+#ifdef SOFA_DUMP_VISITOR_INFO
+    printComment("computeCollisionDetectionInConstraints");
+    ctime_t t0=begin(node, c);
+#endif
+    c->processGeometricalData();
+#ifdef SOFA_DUMP_VISITOR_INFO
+    end(node, c,t0);
+#endif
+}
+
+void ProcessGeometricalDataVisitor::fwdConstraintSet(simulation::Node*
+#ifdef SOFA_DUMP_VISITOR_INFO
+                                            node
+#endif
+                                        , core::behavior::BaseConstraintSet* c)
+{
+#ifdef SOFA_DUMP_VISITOR_INFO
+    printComment("computeCollisionDetectionInConstraints");
+    ctime_t t0=begin(node, c);
+#endif
+    c->processGeometricalData();
+#ifdef SOFA_DUMP_VISITOR_INFO
+    end(node, c,t0);
+#endif
+}
+
+Visitor::Result ProcessGeometricalDataVisitor::processNodeTopDown(simulation::Node* node)
+{
+    for_each<ProcessGeometricalDataVisitor,simulation::Node, NodeSequence<sofa::core::behavior::BaseConstraintSet>,core::behavior::BaseConstraintSet>(this, node, node->constraintSet, &ProcessGeometricalDataVisitor::fwdConstraintSet);
+    return RESULT_CONTINUE;
+}
+
+Visitor::Result CollisionVisitor::processNodeTopDown(simulation::Node* node)
+{
+    for_each<CollisionVisitor,simulation::Node, NodeSequence<sofa::core::behavior::BaseConstraintSet>,core::behavior::BaseConstraintSet>(this, node, node->constraintSet, &CollisionVisitor::fwdConstraintSet);
+    for_each<CollisionVisitor,simulation::Node, NodeSingle<sofa::core::collision::Pipeline>,sofa::core::collision::Pipeline>(this, node, node->collisionPipeline, &CollisionVisitor::processCollisionPipeline);
+    return RESULT_CONTINUE;
+}
+
+
+
 void CollisionResetVisitor::processCollisionPipeline(simulation::Node*
 #ifdef SOFA_DUMP_VISITOR_INFO
         node

diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/CollisionVisitor.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/CollisionVisitor.h
@@ -30,13 +30,10 @@ namespace sofa::simulation
 {
 
 
-/// Compute collision reset, detection and response in one step
-class SOFA_SIMULATION_CORE_API CollisionVisitor : public Visitor
+class SOFA_SIMULATION_CORE_API BaseCollisionVisitor : public Visitor
 {
-public:
-    CollisionVisitor(const core::ExecParams* params) :Visitor(params) , m_primitiveTestCount(0) {}
-
-    virtual void fwdConstraintSet(simulation::Node* node, core::behavior::BaseConstraintSet* cSet);
+   public:
+    BaseCollisionVisitor(const core::ExecParams* params) :Visitor(params) , m_primitiveTestCount(0) {}
 
     virtual void processCollisionPipeline(simulation::Node* node, core::collision::Pipeline* obj);
 
@@ -45,37 +42,68 @@ class SOFA_SIMULATION_CORE_API CollisionVisitor : public Visitor
     /// Return a category name for this action.
     /// Only used for debugging / profiling purposes
     const char* getCategoryName() const override { return "collision"; }
-    const char* getClassName() const override { return "CollisionVisitor"; }
+    const char* getClassName() const override { return "BaseCollisionVisitor"; }
 
     size_t getPrimitiveTestCount() const {return m_primitiveTestCount;}
-private:
+   private:
     size_t m_primitiveTestCount;
 };
 
+class SOFA_SIMULATION_CORE_API ProcessGeometricalDataVisitor : public Visitor
+{
+   public:
+    ProcessGeometricalDataVisitor(const core::ExecParams* params) :Visitor(params) {}
+
+    virtual void fwdConstraintSet(simulation::Node* node, core::behavior::BaseConstraintSet* cSet);
+    Result processNodeTopDown(simulation::Node* node) override;
+
+    /// Return a category name for this action.
+    /// Only used for debugging / profiling purposes
+    const char* getCategoryName() const override { return "collision"; }
+    const char* getClassName() const override { return "ProcessGeometricalDataVisitor"; }
+
+};
+
+/// Compute collision reset, detection and response in one step
+class SOFA_SIMULATION_CORE_API CollisionVisitor :  public BaseCollisionVisitor
+{
+public:
+    CollisionVisitor(const core::ExecParams* params) : BaseCollisionVisitor(params) {}
+
+    virtual void fwdConstraintSet(simulation::Node* node, core::behavior::BaseConstraintSet* cSet);
+    Result processNodeTopDown(simulation::Node* node) override;
+
+    /// Return a category name for this action.
+    /// Only used for debugging / profiling purposes
+    const char* getCategoryName() const override { return "collision"; }
+    const char* getClassName() const override { return "CollisionVisitor"; }
+
+};
+
 /// Remove collision response from last step
-class SOFA_SIMULATION_CORE_API CollisionResetVisitor : public CollisionVisitor
+class SOFA_SIMULATION_CORE_API CollisionResetVisitor : public BaseCollisionVisitor
 {
 
 public:
-    CollisionResetVisitor(const core::ExecParams* params) :CollisionVisitor(params) {}
+    CollisionResetVisitor(const core::ExecParams* params) : BaseCollisionVisitor(params) {}
     void processCollisionPipeline(simulation::Node* node, core::collision::Pipeline* obj) override;
     const char* getClassName() const override { return "CollisionResetVisitor"; }
 };
 
 /// Compute collision detection
-class SOFA_SIMULATION_CORE_API CollisionDetectionVisitor : public CollisionVisitor
+class SOFA_SIMULATION_CORE_API CollisionDetectionVisitor : public BaseCollisionVisitor
 {
 public:
-    CollisionDetectionVisitor(const core::ExecParams* params) :CollisionVisitor(params) {}
+    CollisionDetectionVisitor(const core::ExecParams* params) : BaseCollisionVisitor(params) {}
     void processCollisionPipeline(simulation::Node* node, core::collision::Pipeline* obj) override;
     const char* getClassName() const override { return "CollisionDetectionVisitor"; }
 };
 
 /// Compute collision response
-class SOFA_SIMULATION_CORE_API CollisionResponseVisitor : public CollisionVisitor
+class SOFA_SIMULATION_CORE_API CollisionResponseVisitor : public BaseCollisionVisitor
 {
 public:
-    CollisionResponseVisitor(const core::ExecParams* params) :CollisionVisitor(params) {}
+    CollisionResponseVisitor(const core::ExecParams* params) : BaseCollisionVisitor(params) {}
     void processCollisionPipeline(simulation::Node* node, core::collision::Pipeline* obj) override;
     const char* getClassName() const override { return "CollisionResponseVisitor"; }
 };

diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/PipelineImpl.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/PipelineImpl.cpp
@@ -106,15 +106,13 @@ void PipelineImpl::computeCollisionReset()
     if (contactManager!=nullptr && contactManager->getIntersectionMethod()!=intersectionMethod)
         contactManager->setIntersectionMethod(intersectionMethod);
 
-    SCOPED_TIMER("CollisionReset");
     doCollisionReset();
 }
 
 void PipelineImpl::computeCollisionDetection()
 {
     simulation::Node* root = dynamic_cast<simulation::Node*>(getContext());
     if(root == nullptr) return;
-    SCOPED_TIMER("CollisionDetection");
     std::vector<CollisionModel*> collisionModels;
     root->getTreeObjects<CollisionModel>(&collisionModels);
     doCollisionDetection(collisionModels);
@@ -125,7 +123,6 @@ void PipelineImpl::computeCollisionResponse()
     const simulation::Node* root = dynamic_cast<simulation::Node*>(getContext());
     if(root == nullptr) return;
 
-    SCOPED_TIMER("CollisionResponse");
     doCollisionResponse();
 }