Merge topic 'CollisionContextAndSolvers'

4e92d9c1 ENH: Improve support for data reading
9e97b77f ENH: Integration of Assembler.
e5c39a98 ENH: Merge collision context branch.
d2f84191 ENH: Cherry-picked from Ricardo's commit
fe72dc5a ENH: Clean up the code and update examples.
6d66149e ENH: Refactor the FEMSimulator example to use
af2c34df ENH: UpdateFEMSimulator example to use the new
14ce80ff

 ENH: Clean and refactor the penalty method.
...
Acked-by: Kitware Robot <kwrobot@kitware.com>
Merge-request: !71

Assembler/Assembler.cpp

+// This file is part of the SimMedTK project.
+// Copyright (c) Center for Modeling, Simulation, and Imaging in Medicine,
+//                        Rensselaer Polytechnic Institute
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+//---------------------------------------------------------------------------
+//
+// Authors:
+//
+// Contact:
+//---------------------------------------------------------------------------
+
+#include "Assembler/Assembler.h"
+#include "Core/ContactHandling.h"
+#include "Core/Model.h"
+#include "TimeIntegrators/OdeSystem.h"
+
+Assembler::Assembler(std::shared_ptr<CollisionContext> collContext)
+{
+    this->collisionContext = collContext;
+    this->initSystem();
+}
+
+//---------------------------------------------------------------------------
+void Assembler::type1Interactions()
+{
+    if(!this->collisionContext)
+    {
+        // TODO: Log this
+        return;
+    }
+
+    auto ch = this->collisionContext->getContactHandlers();
+
+    for(auto &ch : this->collisionContext->getContactHandlers())
+    {
+        const auto &forces = ch->getContactForces();
+        auto sceneModel = ch->getSecondSceneObject();
+        sceneModel->updateExternalForces(forces);
+    }
+}
+
+//---------------------------------------------------------------------------
+void Assembler::initSystem()
+{
+    for(auto &rows : this->collisionContext->getIslands())
+    {
+        size_t dofSize = 0;
+        size_t nnz = 0;
+        std::vector<const core::SparseMatrixd*> systemMatrices;
+        std::vector<const core::Vectord*> rhsVector;
+        for(auto &col : rows)
+        {
+            auto sceneModel = this->collisionContext->getSceneModel(col);
+            auto odeSystem = std::dynamic_pointer_cast<OdeSystem>(
+                    this->collisionContext->getSceneModel(col));
+            if(sceneModel && odeSystem)
+            {
+                dofSize += sceneModel->getNumOfDOF();
+                systemMatrices.push_back(&odeSystem->getSystemMatrix());
+                rhsVector.push_back(&odeSystem->getRHS());
+                nnz += systemMatrices.back()->nonZeros();
+            }
+        }
+
+        if(dofSize > 0)
+        {
+            this->A.emplace_back(dofSize,dofSize);
+            this->A.back().reserve(nnz);
+            this->b.emplace_back(dofSize);
+            auto system = std::make_shared<LinearSystemType>(this->A.back(),this->b.back());
+
+            this->systemOfEquationsList.push_back(system);
+
+            size_t size = 0;
+            for(size_t i = 0, end = systemMatrices.size(); i < end; ++i)
+            {
+                auto M = systemMatrices[i];
+                auto rhs = rhsVector[i];
+
+                this->concatenateMatrix(*M,this->A.back(),size,size);
+                this->b.back().segment(size,rhs->size());
+
+                size += rhs->size();
+            }
+        }
+    }
+}
+
+//---------------------------------------------------------------------------
+void Assembler::concatenateMatrix(const core::SparseMatrixd &Q, core::SparseMatrixd &R, std::size_t i, std::size_t j)
+{
+    for(size_t k = i; k < i+Q.outerSize(); ++k)
+    {
+        for(core::SparseMatrixd::InnerIterator it(Q,k); it; ++it)
+        {
+            R.insert(k,it.col()+j) = it.value();
+        }
+    }
+}

Assembler/Assembler.h

+// This file is part of the SimMedTK project.
+// Copyright (c) Center for Modeling, Simulation, and Imaging in Medicine,
+//                        Rensselaer Polytechnic Institute
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+//---------------------------------------------------------------------------
+//
+// Authors:
+//
+// Contact:
+//---------------------------------------------------------------------------
+#ifndef SM_ASSEMBLY
+#define SM_ASSEMBLY
+
+#include <memory>
+
+// SimMedTK includes
+#include "Core/CoreClass.h"
+
+//#include "Core/SDK.h"
+#include "Core/Config.h"
+#include "Core/ContactHandling.h"
+#include "CollisionContext/CollisionContext.h"
+#include "Solvers/SystemOfEquations.h"
+//#include "Core/SceneObject.h"
+#include "Core/Matrix.h"
+
+///
+/// \class Assembler
+///
+/// \brief This class is responsible for using the
+/// information in the collision context and the
+/// internal, external forces from scene objects
+/// to output systems of equations to be solved by
+/// solver module.
+///
+class Assembler : public CoreClass
+{
+public:
+    using LinearSystemType = LinearSystem<core::SparseMatrixd>;
+
+public:
+    ///
+    /// \brief Default constructor
+    ///
+    Assembler() = default;
+
+    ///
+    /// \brief Destructor
+    ///
+    ~Assembler() = default;
+
+    ///
+    /// \brief Constructor. Takes a collision context.
+    ///
+    Assembler(std::shared_ptr<CollisionContext> collisionContext);
+
+	///
+	/// \brief consolidate the forces/projectors from type 1 interactions such as
+    /// forces from penalty based contact handling
+	///
+    void type1Interactions();
+
+	///
+	/// \brief
+	///
+    void initSystem();
+
+    ///
+    /// \brief Helper to concatenate the matrix Q into an block of R.
+    ///
+    /// \param Q Submatrix
+    /// \param R Supermatrix
+    /// \param i row offset
+    /// \param j column offset
+    ///
+    void concatenateMatrix(const core::SparseMatrixd &Q, core::SparseMatrixd &R, size_t i, size_t j);
+
+private:
+    // inputs
+    std::shared_ptr<CollisionContext> collisionContext;
+
+    // output
+    std::vector<std::shared_ptr<LinearSystem<core::SparseMatrixd>>> systemOfEquationsList;
+
+    std::vector<core::SparseMatrixd> A;
+    std::vector<core::Vectord> b;
+};
+
+#endif // SM_ASSEMBLY

Assembler/CMakeLists.txt

+
+set(MODULE_NAME Assembler)
+simmedtk_add_library(${MODULE_NAME}
+  SOURCES
+    Assembler.cpp
+  PUBLIC_HEADERS
+    Assembler.h
+)
+
+target_link_libraries(Assembler
+  PUBLIC
+    Core
+  PRIVATE
+    Event
+	ContactHandling
+	Simulators
+	CollisionContext
+	TimeIntegrators
+	SceneModels
+)

CMake/SimMedTKMacros.cmake

@@ -120,3 +120,23 @@ function(DOWNLOAD_DATA url filename)
         message("${STATUS1}")
     endif()
 endfunction(DOWNLOAD_DATA)
+
+
+function(MIDAS_DOWNLOAD MIDAS_URL FILE_SHA1 LOCAL_FILE)
+    if(NOT EXISTS ${LOCAL_FILE})
+        file(
+            DOWNLOAD "${MIDAS_URL}"
+            "${LOCAL_FILE}"
+            SHOW_PROGRESS
+            STATUS DOWNLOAD_STATUS
+            TIMEOUT 15
+            TLS_VERIFY ON
+            EXPECTED_HASH SHA1=${FILE_SHA1}
+        )
+        list(GET DOWNLOAD_STATUS 0 STATUS0)
+        if(NOT STATUS0 STREQUAL "0")
+            list(GET DOWNLOAD_STATUS 1 STATUS1)
+            message("${STATUS1}")
+        endif()
+    endif()
+endfunction()

CMakeLists.txt

@@ -286,13 +286,17 @@ set(SimMedTK_exports
   TimeIntegrators
   VTKRendering
   VirtualTools
+  SceneModels
+  SimulationManager
   )
 
 if(SimMedTK_USE_OCULUS)
   list(APPEND SimMedTK_exports RenderingOculus)
 endif()
 
+add_subdirectory(Assembler)
 add_subdirectory(Collision)
+add_subdirectory(CollisionContext)
 add_subdirectory(ContactHandling)
 add_subdirectory(Core)
 add_subdirectory(Devices)
@@ -304,7 +308,9 @@ add_subdirectory(IO)
 add_subdirectory(Mesh)
 add_subdirectory(Rendering)
 add_subdirectory(Simulators)
+add_subdirectory(SimulationManager)
 add_subdirectory(Solvers)
+add_subdirectory(SceneModels)
 add_subdirectory(TimeIntegrators)
 add_subdirectory(VTKRendering)
 add_subdirectory(VirtualTools)

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.cpp

@@ -31,7 +31,6 @@
 #include "Collision/OctreeCell.h"
 #include "Collision/CollisionMoller.h"
 #include "Collision/MeshCollisionModel.h"
-#include "Core/SDK.h"
 #include "Mesh/SurfaceMesh.h"
 
 

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
+)
+
+if(BUILD_TESTING)
+  add_subdirectory(UnitTests)
+endif()

CollisionContext/CollisionContext.cpp

+// This file is part of the SimMedTK project.
+// Copyright (c) Center for Modeling, Simulation, and Imaging in Medicine,
+//                        Rensselaer Polytechnic Institute
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+//---------------------------------------------------------------------------
+//
+// Authors:
+//
+// Contact:
+//---------------------------------------------------------------------------
+
+#include "CollisionContext/CollisionContext.h"
+
+//---------------------------------------------------------------------------
+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< CollisionManager > contactType, bool active)
+{
+    if (!sceneObjectA || !sceneObjectB ||
+        !collisionDetection || !contactHandlingA ||
+        !contactHandlingB || !contactType)
+    {
+        // TODO: log this
+        return;
+    }
+
+    if(sceneObjectA != sceneObjectB)
+    {
+        this->totalNumberOfSceneModels++;
+    }
+
+    auto scenePair = std::make_tuple(sceneObjectA, sceneObjectB);
+    auto scenePairData = std::make_tuple(collisionDetection,
+        contactHandlingA,
+        contactHandlingB,
+        contactType,
+        active);
+
+    this->interactionMap.emplace(scenePair, scenePairData);
+}
+
+//---------------------------------------------------------------------------
+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;
+}
+
+//---------------------------------------------------------------------------
+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;
+    std::get<Enabled>(this->interactionMap[pair]) = true;
+}
+
+//---------------------------------------------------------------------------
+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)
+{
+    auto it = this->interactionMap.find(std::make_tuple(sceneObject1, sceneObject2));
+
+    if (it == std::end(this->interactionMap))
+    {
+        std::cout
+            << "Warning: The interaction to be disabled was not found!"
+            << std::endl;
+    }
+    else
+    {
+        std::get<Enabled>(it->second) = false;
+    }
+}
+
+//---------------------------------------------------------------------------
+void CollisionContext::removeInteraction(
+    std::shared_ptr< SceneObject > sceneObjectA,
+    std::shared_ptr< SceneObject > sceneObjectB)
+{
+    if (!sceneObjectA || !sceneObjectB)
+    {
+        // TODO: log this
+        return;
+    }
+
+    auto it = this->interactionMap.find(std::make_tuple(sceneObjectA, sceneObjectB));
+
+    if (it == std::end(this->interactionMap))
+    {
+        // TODO: Log this