ENH: Merge collision context branch.

Revert some changes and make it build, run FEMSimulator example

CollisionContext/CMakeLists.txt was contributed by Sreekanth.

CMakeLists.txt

@@ -295,6 +295,7 @@ if(SimMedTK_USE_OCULUS)
 endif()
 
 add_subdirectory(Collision)
+add_subdirectory(CollisionContext)
 add_subdirectory(ContactHandling)
 add_subdirectory(Core)
 add_subdirectory(Devices)

Collision/MeshToMeshCollision.cpp

@@ -24,7 +24,7 @@
 #include "MeshToMeshCollision.h"
 
 // SimMedTK includes
-#include "Core/CollisionPair.h"
+#include "Core/CollisionManager.h"
 #include "Collision/CollisionMoller.h"
 #include "Collision/MeshCollisionModel.h"
 #include "Collision/SurfaceTree.h"
@@ -32,7 +32,7 @@
 // STL includes
 #include <vector>
 
-void MeshToMeshCollision::doComputeCollision(std::shared_ptr<CollisionPair> pairs)
+void MeshToMeshCollision::doComputeCollision(std::shared_ptr<CollisionManager> pairs)
 {
     auto meshA = std::static_pointer_cast<MeshCollisionModel>(pairs->getFirst());
     auto meshB = std::static_pointer_cast<MeshCollisionModel>(pairs->getSecond());

Collision/MeshToMeshCollision.h

@@ -28,7 +28,7 @@
 // SimMedTK includes
 #include "Core/CollisionDetection.h"
 
-class CollisionPair;
+class CollisionManager;
 
 ///
 /// @brief COllision detection based on BVH queries and
@@ -43,7 +43,7 @@ public:
     MeshToMeshCollision(const MeshToMeshCollision &) = delete;
 
 private:
-    virtual void doComputeCollision(std::shared_ptr<CollisionPair> pairs) override;
+    virtual void doComputeCollision(std::shared_ptr<CollisionManager> pairs) override;
 
 };
 

Collision/PlaneToMeshCollision.cpp

@@ -25,14 +25,14 @@
 
 // SimMedTK includes
 #include "Collision/CollisionMoller.h"
-#include "Core/CollisionPair.h"
+#include "Core/CollisionManager.h"
 #include "Collision/MeshCollisionModel.h"
 #include "Collision/PlaneCollisionModel.h"
 
 // STL includes
 #include <limits>
 
-void PlaneToMeshCollision::doComputeCollision(std::shared_ptr<CollisionPair> pair)
+void PlaneToMeshCollision::doComputeCollision(std::shared_ptr<CollisionManager> pair)
 {
     auto meshModel = std::static_pointer_cast<MeshCollisionModel>(pair->getFirst());
     auto planeModel = std::static_pointer_cast<PlaneCollisionModel>(pair->getSecond());

Collision/PlaneToMeshCollision.h

@@ -28,7 +28,7 @@
 // SimMedTK includes
 #include "Core/CollisionDetection.h"
 
-class CollisionPair;
+class CollisionManager;
 
 ///
 /// @brief Brute force collision detection between a mesh and a static plane
@@ -42,7 +42,7 @@ public:
     PlaneToMeshCollision(const PlaneToMeshCollision &) = delete;
 
 private:
-    virtual void doComputeCollision(std::shared_ptr<CollisionPair> pairs) override;
+    virtual void doComputeCollision(std::shared_ptr<CollisionManager> pairs) override;
 
 };
 

Collision/SpatialHashCollision.h

@@ -113,7 +113,7 @@ protected:
     void updateBVH();
 
 private:
-    void doComputeCollision(std::shared_ptr<CollisionPair> /*pairs*/)
+    void doComputeCollision(std::shared_ptr<CollisionManager> /*pairs*/)
     {
         reset();
         updateBVH();

Collision/UnitTests/CollisionPairSpec.cpp

@@ -23,7 +23,7 @@
 
 #include <bandit/bandit.h>
 
-#include "Core/CollisionPair.h"
+#include "Core/CollisionManager.h"
 #include "Collision/MeshCollisionModel.h"
 #include "Core/Model.h"
 #include "Core/MakeUnique.h"
@@ -33,10 +33,10 @@ using namespace bandit;
 go_bandit([](){
     describe("Collision pair", []() {
         it("constructs ", []() {
-            std::unique_ptr<CollisionPair> collisionPair(Core::make_unique<CollisionPair>());
+            std::unique_ptr<CollisionManager> collisionPair(Core::make_unique<CollisionManager>());
         });
         it("attaches models ", []() {
-            std::unique_ptr<CollisionPair> collisionPair(Core::make_unique<CollisionPair>());
+            std::unique_ptr<CollisionManager> collisionPair(Core::make_unique<CollisionManager>());
 
             std::shared_ptr<Model> modelA = std::make_shared<MeshCollisionModel>();
             std::shared_ptr<Model> modelB = std::make_shared<MeshCollisionModel>();
@@ -48,14 +48,14 @@ go_bandit([](){
 
         });
         it("attaches contacts ", []() {
-            std::unique_ptr<CollisionPair> collisionPair(Core::make_unique<CollisionPair>());
+            std::unique_ptr<CollisionManager> collisionPair(Core::make_unique<CollisionManager>());
 
             float depth = 1.0;
             core::Vec3d contactPoint(0,0,1);
             core::Vec3d normal(1,0,0);
             collisionPair->addContact(depth,contactPoint,1,normal);
 
-            std::shared_ptr<Contact> contact = collisionPair->getContacts().back();
+            std::shared_ptr<PenetrationDepthCollisionData> contact = collisionPair->getContacts().back();
 
             AssertThat(collisionPair->hasContacts(), IsTrue());
             AssertThat(contact->normal, Equals(normal));

Collision/UnitTests/MeshToMeshCollisionSpec.cpp

@@ -25,7 +25,7 @@
 
 #include "Collision/MeshToMeshCollision.h"
 #include "Collision/MeshCollisionModel.h"
-#include "Core/CollisionPair.h"
+#include "Core/CollisionManager.h"
 #include "Core/MakeUnique.h"
 #include "Mesh/SurfaceMesh.h"
 
@@ -73,7 +73,7 @@ go_bandit([](){
 
             std::shared_ptr<Model> modelB = getModel(verticesB);
 
-            std::shared_ptr<CollisionPair> collisionPair = std::make_shared<CollisionPair>();
+            std::shared_ptr<CollisionManager> collisionPair = std::make_shared<CollisionManager>();
 
             collisionPair->setModels(modelA,modelB);
 

CollisionContext/CMakeLists.txt

+
+simmedtk_add_library(CollisionContext
+  SOURCES
+    CollisionContext.cpp
+  PUBLIC_HEADERS
+    CollisionContext.h
+)
+
+target_link_libraries(CollisionContext
+  PUBLIC
+    Core
+  PRIVATE
+    Event
+    Collision
+    Simulators
+	ContactHandling
+)

CollisionContext/CollisionContext.cpp

@@ -23,39 +23,26 @@
 
 #include "CollisionContext/CollisionContext.h"
 
-InteractionDataElement::InteractionDataElement(
-    int so1,
-    int so2,
-    core::CollisionDetectionType cd,
-    core::ContactHandlingType ch1,
-    core::ContactHandlingType ch2,
-    core::CollisionPairType cp)
-{
-    sceneObj1Id = so1;
-    sceneObj2Id = so2;
-    collDetectionType = cd;
-    contHandlingType1 = ch1;
-    contHandlingType2 = ch2;
-    collPairType = cp;
-}
-
 //---------------------------------------------------------------------------
-bool CollisionContext::addInteraction(
+void CollisionContext::addInteraction(
     std::shared_ptr< SceneObject > sceneObjectA,
     std::shared_ptr< SceneObject > sceneObjectB,
     std::shared_ptr< CollisionDetection > collisionDetection,
     std::shared_ptr< ContactHandling > contactHandlingA,
     std::shared_ptr< ContactHandling > contactHandlingB,
-    std::shared_ptr< CollisionDataBase > contactType, bool active)
+    std::shared_ptr< CollisionManager > contactType, bool active)
 {
-    if (!sceneObjectA ||
-        !sceneObjectB ||
-        !collisionDetection ||
-        !contactHandlingA ||
-        !contactHandlingB ||
-        !contactType)
+    if (!sceneObjectA || !sceneObjectB ||
+        !collisionDetection || !contactHandlingA ||
+        !contactHandlingB || !contactType)
+    {
+        // TODO: log this
+        return;
+    }
+
+    if(sceneObjectA != sceneObjectB)
     {
-        return false;
+        this->totalNumberOfSceneModels++;
     }
 
     auto scenePair = std::make_tuple(sceneObjectA, sceneObjectB);
@@ -64,26 +51,49 @@ bool CollisionContext::addInteraction(
         contactHandlingB,
         contactType,
         active);
+
     this->interactionMap.emplace(scenePair, scenePairData);
 }
 
 //---------------------------------------------------------------------------
-bool CollisionContext::addInteraction(
+void CollisionContext::addInteraction(
     std::shared_ptr< SceneObject > sceneObjectA,
     std::shared_ptr< SceneObject > sceneObjectB,
     std::shared_ptr< CollisionDetection > collisionDetection)
 {
+    if (!sceneObjectA || !sceneObjectB || !collisionDetection)
+    {
+        // TODO: log this
+        return;
+    }
+
+    if(sceneObjectA != sceneObjectB)
+    {
+        this->totalNumberOfSceneModels++;
+    }
+
     auto pair = std::make_tuple(sceneObjectA, sceneObjectB);
     std::get<Detection>(this->interactionMap[pair]) = collisionDetection;
     std::get<Enabled>(this->interactionMap[pair]) = true;
 }
 
 //---------------------------------------------------------------------------
-bool CollisionContext::addInteraction(
+void CollisionContext::addInteraction(
     std::shared_ptr< SceneObject > sceneObjectA,
     std::shared_ptr< SceneObject > sceneObjectB,
     std::shared_ptr< ContactHandling > contactHandler)
 {
+    if (!sceneObjectA || !sceneObjectB)
+    {
+        // TODO: log this
+        return;
+    }
+
+    if(sceneObjectA != sceneObjectB)
+    {
+        this->totalNumberOfSceneModels++;
+    }
+
     auto pair = std::make_tuple(sceneObjectA, sceneObjectB);
     std::get<HandlingA>(this->interactionMap[pair]) = contactHandler;
     std::get<HandlingB>(this->interactionMap[pair]) = contactHandler;
@@ -91,18 +101,28 @@ bool CollisionContext::addInteraction(
 }
 
 //---------------------------------------------------------------------------
-bool CollisionContext::addInteraction(
+void CollisionContext::addInteraction(
     std::shared_ptr< SceneObject > sceneObjectA,
     std::shared_ptr< SceneObject > sceneObjectB)
 {
+    if (!sceneObjectA || !sceneObjectB)
+    {
+        // TODO: log this
+        return;
+    }
+
+    if(sceneObjectA != sceneObjectB)
+    {
+        this->totalNumberOfSceneModels++;
+    }
+
     auto pair = std::make_tuple(sceneObjectA, sceneObjectB);
     this->interactionMap[pair] = std::make_tuple(nullptr, nullptr, nullptr, nullptr, false);
 }
 
 //---------------------------------------------------------------------------
-void CollisionContext::disableInteraction(
-    std::shared_ptr< SceneObject > sceneObject1,
-    std::shared_ptr< SceneObject > sceneObject2)
+void CollisionContext::disableInteraction(std::shared_ptr< SceneObject > sceneObject1,
+                                          std::shared_ptr< SceneObject > sceneObject2)
 {
     auto it = this->interactionMap.find(std::make_tuple(sceneObject1, sceneObject2));
 
@@ -120,46 +140,58 @@ void CollisionContext::disableInteraction(
 
 //---------------------------------------------------------------------------
 void CollisionContext::removeInteraction(
-    std::shared_ptr< SceneObject > sceneObject1,
-    std::shared_ptr< SceneObject > sceneObject2)
+    std::shared_ptr< SceneObject > sceneObjectA,
+    std::shared_ptr< SceneObject > sceneObjectB)
 {
-    auto it = this->interactionMap.find(std::make_tuple(sceneObject1, sceneObject2));
-
-    if (it == std::end(this->interactionMap))
+    if (!sceneObjectA || !sceneObjectB)
     {
-        std::cout
-            << "Warning: The interaction to be disabled is not found!"
-            << std::endl;
+        // TODO: log this
+        return;
     }
 
-    else
+    auto it = this->interactionMap.find(std::make_tuple(sceneObjectA, sceneObjectB));
+
+    if (it == std::end(this->interactionMap))
     {
-        this->interactionMap.erase(it);
+        // TODO: Log this
+        return;
     }
+
+    this->interactionMap.erase(it);
+    this->totalNumberOfSceneModels--;
 }
 
 //---------------------------------------------------------------------------
-bool CollisionContext::setCollisionDetection(
-    std::shared_ptr< SceneObject > sceneObjectA,
-    std::shared_ptr< SceneObject > sceneObjectB,
-    std::shared_ptr< CollisionDetection > collisionDetection)
+void CollisionContext::
+setCollisionDetection(std::shared_ptr< SceneObject > sceneObjectA,
+                      std::shared_ptr< SceneObject > sceneObjectB,
+                      std::shared_ptr< CollisionDetection > collisionDetection)
 {
+    if (!sceneObjectA || !sceneObjectB || !collisionDetection)
+    {
+        // TODO: log this
+        return;
+    }
     this->addInteraction(sceneObjectA, sceneObjectB, collisionDetection);
 }
 
 //---------------------------------------------------------------------------
-bool CollisionContext::setContactHandling(
-    std::shared_ptr< SceneObject > sceneObjectA,
-    std::shared_ptr< SceneObject > sceneObjectB,
-    std::shared_ptr< ContactHandling > contactHandler)
+void CollisionContext::
+setContactHandling(std::shared_ptr< SceneObject > sceneObjectA,
+                   std::shared_ptr< SceneObject > sceneObjectB,
+                   std::shared_ptr< ContactHandling > contactHandler)
 {
+    if (!sceneObjectA || !sceneObjectB || !contactHandler)
+    {
+        // TODO: log this
+        return;
+    }
     this->addInteraction(sceneObjectA, sceneObjectB, contactHandler);
 }
 
 //---------------------------------------------------------------------------
-bool CollisionContext::exist(
-    std::shared_ptr< SceneObject > sceneObject1,
-    std::shared_ptr< SceneObject > sceneObject2)
+bool CollisionContext::exist(std::shared_ptr< SceneObject > sceneObject1,
+                             std::shared_ptr< SceneObject > sceneObject2)
 {
     auto it = this->interactionMap.find(std::make_tuple(sceneObject1, sceneObject2));
     return it != std::end(this->interactionMap);
@@ -194,16 +226,13 @@ void CollisionContext::createAssemblerAdjacencyMatrix()
 
     // set the size of the adjacency matrix
     this->interactionMatrix.resize(numSceneObjects);
-    for (int i = 0; i < numSceneObjects; ++i)
+    for (size_t i = 0; i < numSceneObjects; ++i)
     {
         this->interactionMatrix[i].resize(numSceneObjects);
     }
 
     std::vector<Eigen::Triplet<int>> triplets;
 
-    // Populate the assembly adjacency matrix
-    int i = 0, j = 0;
-
     for (auto & interaction : this->interactionMap)
     {
         if (!std::get<Enabled>(interaction.second))
@@ -224,31 +253,31 @@ void CollisionContext::createAssemblerAdjacencyMatrix()
         }
         else
         {
-            this->interactionMatrix[i.second][j.second] = 1;
+            this->interactionMatrix[i->second][j->second] = 1;
         }
     }
 
-    for (auto & st : this->solveTogether)//
+    for (auto & st : this->modelPairs)//
     {
-        auto objA = this->objectIndexMap.find(st.first);
-        auto objB = this->objectIndexMap.find(st.second);
+        auto i = this->objectIndexMap.find(st.first);
+        auto j = this->objectIndexMap.find(st.second);
 
-        if (objA == std::end(this->objectIndexMap) || objB == std::end(this->objectIndexMap))
+        if (i == std::end(this->objectIndexMap) || j == std::end(this->objectIndexMap))
         {
             std::cout << "Warning: scene object does not have an index" << std::endl;
             continue;
         }
         else
         {
-            this->interactionMatrix[objA.second][objB.second] = 1;
+            this->interactionMatrix[i->second][j->second] = 1;
         }
 
     }
 }
+
 //---------------------------------------------------------------------------
-void CollisionContext::solveTogether(
-    std::shared_ptr< SceneObject > sceneObjectA,
-    std::shared_ptr< SceneObject > sceneObjectB)
+void CollisionContext::solveSimultaneously(std::shared_ptr< SceneObject > sceneObjectA,
+                                           std::shared_ptr< SceneObject > sceneObjectB)
 {
     auto i = this->objectIndexMap.find(sceneObjectA);
     auto j = this->objectIndexMap.find(sceneObjectB);
@@ -259,8 +288,7 @@ void CollisionContext::solveTogether(
     }
     else
     {
-        auto objPair = std::make_pair(sceneObjectA, sceneObjectB);
-        this->solveTogether.push_back(objPair);
+        this->modelPairs.emplace_back(sceneObjectA, sceneObjectB);
     }
 }
 
@@ -272,19 +300,19 @@ bool CollisionContext::configure()
 }
 
 //---------------------------------------------------------------------------
-std::size_t CollisionContext::getNumberOfInterations() const
+std::size_t CollisionContext::getNumberOfInteractions() const
 {
     return this->interactionMap.size();
 }
 
 //---------------------------------------------------------------------------
-CollisionContext::InteractionMapType &CollisionContext::getInteractions() const
+const CollisionContext::InteractionMapType &CollisionContext::getInteractions() const
 {
     return this->interactionMap;
 }
 
 //---------------------------------------------------------------------------
-std::vector< std::shared_ptr< ContactHandling > > CollisionContext::getContactHandlers()
+std::vector<std::shared_ptr<ContactHandling>> CollisionContext::getContactHandlers()
 {
     std::vector<std::shared_ptr<ContactHandling>> handlerList;
 
@@ -318,62 +346,39 @@ void CollisionContext::findIslands()
 
 }
 
-int CollisionContext::getNumOfIslands()
+//---------------------------------------------------------------------------
+size_t CollisionContext::getNumOfIslands()
 {
-    return this->islands->size();
+    return this->islands.size();
 }
 
+//---------------------------------------------------------------------------
 std::vector<std::vector<int>>& CollisionContext::getIslands()
 {
     return this->islands;
 }
 
-std::shared_ptr<SceneObject> CollisionContext::getObjectWithIndex(const int objIndex)
-{
-    for (int i = 0; i < this->objectIndexMap.size(); ++i)
-    {
-        if (this->objectIndexMap.at(i).second == objIndex)
-        {
-            return this->objectIndexMap.at(i).second;
-        }
-    }
-}
-
 //---------------------------------------------------------------------------
-template<typename T>
-int CollisionContext::numberOfInteractions() const
+void CollisionContext::formIslands()
 {
-    int count = 0;
-    auto counter = [&](const InteractionPairDataType &data)
+    if(this->totalNumberOfSceneModels == 0)
     {
-        if (std::dynamic_pointer_cast<T>(std::get<HandlingA>(data)) ||
-            std::dynamic_pointer_cast<T>(std::get<HandlingB>(data)))
-        {
-            ++count;
-        }
-    };
-    std::for_each(this->interactionMap.begin(), interactionMap.end(), counter);
-    return count;
-}
+        // TODO: Log this
+        return;
+    }
 
-//---------------------------------------------------------------------------
-// \todo test
-void CollisionContext::formIslands()
-{
-    int nSceneObj, nIslands;
-    std::vector<bool> visited;
-    visited.resize(nSceneObj);
+    std::vector<bool> visited(this->totalNumberOfSceneModels,false);
 
-    nIslands = 0;
-    for (int i = 0; i < nSceneObj; i++)
+    int nIslands = 0;
+    for (int i = 0; i < this->totalNumberOfSceneModels; i++)
     {
         if (!visited[i])
         {
-            nIslands++;
             std::vector<int> memberList;
-            appendNeighbors(visited, memberList, i);
-
-            visited[i] = 1;
+            memberList.push_back(i);
+            visited[i] = true;
+            nIslands++;
+            this->appendNeighbors(visited, memberList, i);
 
             this->islands.push_back(memberList);
         }
@@ -382,19 +387,22 @@ void CollisionContext::formIslands()
 
 //---------------------------------------------------------------------------
 // \todo test
-void CollisionContext::appendNeighbors(
-    std::vector<bool>& visited, std::vector<int>& memberList, int row)
+void CollisionContext::appendNeighbors(std::vector<bool>& visited,
+                                       std::vector<int>& memberList,
+                                       int row)
 {
-    for (int i = row; i < interactionMatrix.size(); ++i)// only check the upper triangular matrix for now
+    // NOTE: only check the upper triangular matrix for now
+    for (size_t col = row, end = interactionMatrix.size(); col < end; ++col)
     {
-        if (interactionMatrix[row][i] != 0
-            && std::find(memberList.begin(), memberList.end(), interactionMatrix[row][i]) != memberList.end())
+        const auto &entry = interactionMatrix[row][col];
+        if (entry != 0 &&
+            std::find(std::begin(memberList), std::end(memberList), entry) != memberList.end())
         {
-            memberList.push_back(interactionMatrix[row][i]);
-            if (!visited[interactionMatrix[row][i]])
+            memberList.push_back(interactionMatrix[row][col]);
+            if (!visited[interactionMatrix[row][col]])
             {
-                appendNeighbors(visited, memberList, interactionMatrix[row][i]);
+                this->appendNeighbors(visited, memberList, interactionMatrix[row][col]);
             }
         }
     }
-}
\ No newline at end of file
+}

CollisionContext/CollisionContext.h

@@ -27,14 +27,13 @@
 
 // SimMedTK includes
 #include "Core/CoreClass.h"
-#include "Core/SDK.h"
 #include "Core/Matrix.h"
 #include "Core/Config.h"
-#include "Core/SceneObject.h"
+#include "SceneModels/SceneObject.h"
 
 // collision detection includes
 #include "Core/CollisionDetection.h"
-#include "Core/CollisionDataManager.h"
+#include "Core/CollisionManager.h"
 #include "Collision/PlaneToMeshCollision.h"
 #include "Collision/SpatialHashCollision.h"
 
@@ -42,27 +41,6 @@
 #include "Core/ContactHandling.h"
 #include "ContactHandling/PenaltyContactFemToStatic.h"
 
-///
-/// \brief Contains the information required to define an interaction
-///