diff --git a/CMake/External/CMakeLists.txt b/CMake/External/CMakeLists.txt
index f668d532d918a6a8596ae18fc1b7fd30e744de4d..f72aa4fb7bd0e60cb92104c6c4d18513c6a495b9 100644
--- a/CMake/External/CMakeLists.txt
+++ b/CMake/External/CMakeLists.txt
@@ -95,6 +95,7 @@ ExternalProject_Add( ${PROJECT_NAME}
-D${PROJECT_NAME}_BUILD_EXAMPLES:BOOL=${${PROJECT_NAME}_BUILD_EXAMPLES}
-D${PROJECT_NAME}_BUILD_TESTING:BOOL=${${PROJECT_NAME}_BUILD_TESTING}
-D${PROJECT_NAME}_USE_OpenHaptics:BOOL=${${PROJECT_NAME}_USE_OpenHaptics}
+ -D${PROJECT_NAME}_USE_MODEL_REDUCTION:BOOL={${PROJECT_NAME}_USE_MODEL_REDUCTION}
-D${PROJECT_NAME}_USE_Vulkan:BOOL=${${PROJECT_NAME}_USE_Vulkan}
-D${PROJECT_NAME}_ENABLE_AUDIO:BOOL=${${PROJECT_NAME}_ENABLE_AUDIO}
-D${PROJECT_NAME}_ENABLE_VR:BOOL=${${PROJECT_NAME}_ENABLE_VR}
diff --git a/CMake/External/External_VTK.cmake b/CMake/External/External_VTK.cmake
index f410c3f6d994dba3c2c10a8eb5489238c625125f..3fcbe0425a8b83590c1d5b9e694d7b636ed4ee5e 100644
--- a/CMake/External/External_VTK.cmake
+++ b/CMake/External/External_VTK.cmake
@@ -16,6 +16,7 @@ set_property(CACHE ${PROJECT_NAME}_VTK_REPO_SOURCE PROPERTY STRINGS ${VTK_SOURCE
if(${PROJECT_NAME}_VTK_REPO_SOURCE EQUAL "8.2")
set(VTK_MODULE_SETTINGS
+ -DModule_vtkChartsCore:BOOL=ON
-DModule_vtkRenderingOpenGL2:BOOL=ON
-DModule_vtkIOXML:BOOL=ON
-DModule_vtkIOLegacy:BOOL=ON
@@ -28,6 +29,7 @@ if(${PROJECT_NAME}_VTK_REPO_SOURCE EQUAL "8.2")
-DModule_vtkglew:BOOL=ON
-DModule_vtkRenderingContext2D:BOOL=ON
-DModule_vtkRenderingVolumeOpenGL2:BOOL=ON
+ -DModule_vtkViewsContext2D:BOOL=ON
-DBUILD_EXAMPLES:BOOL=OFF
-DBUILD_TESTING:BOOL=OFF
-DVTK_Group_StandAlone:BOOL=OFF
@@ -42,6 +44,7 @@ else()
set(VTK_OPENVR "DONT_WANT")
endif()
set(VTK_MODULE_SETTINGS
+ -DVTK_MODULE_ENABLE_VTK_ChartsCore:STRING=YES
-DVTK_MODULE_ENABLE_VTK_RenderingOpenGL2:STRING=YES
-DVTK_MODULE_ENABLE_VTK_IOExport:STRING=YES
-DVTK_MODULE_ENABLE_VTK_IOImport:STRING=YES
@@ -58,6 +61,7 @@ else()
-DVTK_MODULE_ENABLE_VTK_glew:STRING=YES
-DVTK_MODULE_ENABLE_VTK_RenderingContext2D:STRING=YES
-DVTK_MODULE_ENABLE_VTK_RenderingVolumeOpenGL2:STRING=YES
+ -DVTK_MODULE_ENABLE_VTK_ViewsContext2D:STRING=YES
-DVTK_BUILD_EXAMPLES:STRING=DONT_WANT
-DVTK_BUILD_TESTING:STRING=OFF
-DVTK_GROUP_ENABLE_StandAlone:STRING=DONT_WANT
diff --git a/CMake/External/External_VegaFEM.cmake b/CMake/External/External_VegaFEM.cmake
index a5e30bedc047eab7d110ad97d3de17a54b841e58..27b616ce183dc977365dd5a6b0349e0b80a5c3aa 100644
--- a/CMake/External/External_VegaFEM.cmake
+++ b/CMake/External/External_VegaFEM.cmake
@@ -8,12 +8,12 @@ endif(WIN32)
include(imstkAddExternalProject)
imstk_add_external_project( VegaFEM
- GIT_REPOSITORY https://gitlab.kitware.com/iMSTK/vegafemv4.0
- GIT_TAG f9c96c4128437a559e5fb9a93830ef3c9a627d5e
+ URL https://gitlab.kitware.com/iMSTK/vegafemv4.0/-/archive/build_model_reduction/vegafemv4.0-build_model_reduction.zip
+ URL_MD5 3f04bb7c2ba080785bcadf44d1a462a3
CMAKE_CACHE_ARGS
-DVegaFEM_ENABLE_PTHREADS_SUPPORT:BOOL=ON
-DVegaFEM_ENABLE_OpenGL_SUPPORT:BOOL=OFF
- -DVegaFEM_BUILD_MODEL_REDUCTION:BOOL=OFF
+ -DVegaFEM_BUILD_MODEL_REDUCTION:BOOL=${${PROJECT_NAME}_USE_MODEL_REDUCTION}
-DVegaFEM_BUILD_UTILITIES:BOOL=OFF
DEPENDENCIES ${VegaFEM_DEPENDENCIES}
RELATIVE_INCLUDE_PATH ""
diff --git a/CMake/Utilities/iMSTKInnerbuildConfig.cmake.in b/CMake/Utilities/iMSTKInnerbuildConfig.cmake.in
deleted file mode 100644
index 1158fcca3b166ab8756c9198e43c651ad6849004..0000000000000000000000000000000000000000
--- a/CMake/Utilities/iMSTKInnerbuildConfig.cmake.in
+++ /dev/null
@@ -1,97 +0,0 @@
-# Include and library paths
-set(CMAKE_INCLUDE_PATH "@CMAKE_INCLUDE_PATH@")
-set(CMAKE_LIBRARY_PATH "@CMAKE_LIBRARY_PATH@")
-
-# Path to iMSTK source directory
-set(iMSTK_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@")
-set(iMSTK_BINARY_DIR "@CMAKE_BINARY_DIR@")
-set(iMSTK_INSTALL_ROOT "@iMSTK_INSTALL_ROOT@")
-
-# Update CMake module path
-list(INSERT CMAKE_MODULE_PATH 0 "${iMSTK_SOURCE_DIR}/CMake")
-list(INSERT CMAKE_MODULE_PATH 1 "${iMSTK_SOURCE_DIR}/CMake/Utilities")
-
-# The Find scripts use cmake variables so overwrite them
-set(CMAKE_INSTALL_PREFIX_CACHE ${CMAKE_INSTALL_PREFIX})
-set(CMAKE_BINARY_DIR_CACHE ${CMAKE_BINARY_DIR})
-set(CMAKE_DEBUG_POSTFIX_CACHE ${CMAKE_DEBUG_POSTFIX})
-
-set(CMAKE_INSTALL_PREFIX ${iMSTK_INSTALL_ROOT} CACHE STRING "" FORCE)
-mark_as_advanced(CMAKE_INSTALL_PREFIX)
-set(CMAKE_BINARY_DIR ${iMSTK_BINARY_DIR} CACHE STRING "" FORCE)
-mark_as_advanced(CMAKE_BINARY_DIR)
-set(CMAKE_DEBUG_POSTFIX "d" CACHE STRING "" FORCE)
-mark_as_advanced(CMAKE_DEBUG_POSTFIX)
-
-# iMSTK settings
-set(iMSTK_USE_OpenHaptics @iMSTK_USE_OpenHaptics@)
-set(iMSTK_USE_Vulkan @iMSTK_USE_Vulkan@)
-set(VegaFEM_DIR "@VegaFEM_DIR@")
-set(VTK_DIR "@VTK_DIR@")
-
-# Assimp
-find_package( Assimp REQUIRED )
-
-# g3log
-find_package( g3log REQUIRED )
-include_directories( ${g3log_INCLUDE_DIR} )
-
-# glm
-find_package( glm REQUIRED )
-
-if(iMSTK_USE_Vulkan)
- # glfw
- find_package( glfw REQUIRED )
-
- # gli
- find_package( gli REQUIRED )
-endif()
-
-# Eigen
-find_package( Eigen3 3.3 REQUIRED )
-include_directories( ${Eigen_INCLUDE_DIR} )
-
-# imgui
-find_package( imgui REQUIRED )
-include_directories( ${imgui_INCLUDE_DIR} )
-
-# PhysX
-find_package(PhysX REQUIRED)
-include_directories(${PHYSX_INCLUDE_DIRS})
-
-# SCCD
-find_package( SCCD REQUIRED )
-include_directories( ${SCCD_INCLUDE_DIR} )
-
-# tbb
-find_package(tbb REQUIRED)
-include_directories(${TBB_INCLUDE_DIR})
-if (MSVC)
- add_definitions(-D__TBB_NO_IMPLICIT_LINKAGE=1)
-endif()
-
-# VegaFEM
-find_package( VegaFEM REQUIRED CONFIG )
-
-# VTK
-find_package( VTK REQUIRED CONFIG )
-include( ${VTK_USE_FILE} )
-
-# VRPN
-find_package( VRPN REQUIRED )
-include_directories( ${VRPN_INCLUDE_DIRS} )
-add_definitions( -DVRPN_USE_LIBNIFALCON )
-if(iMSTK_USE_OpenHaptics)
- find_package(OpenHapticsSDK REQUIRED)
- add_definitions( -DiMSTK_USE_OPENHAPTICS )
-else()
- remove_definitions( -DiMSTK_USE_OPENHAPTICS )
-endif()
-
-# iMSTK
-#link_directories(@CMAKE_LIBRARY_PATH@)
-include("@CMAKE_CURRENT_BINARY_DIR@/iMSTKTargets.cmake")
-
-set(CMAKE_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX_CACHE} CACHE STRING "" FORCE)
-set(CMAKE_BINARY_DIR ${CMAKE_BINARY_DIR_CACHE} CACHE STRING "" FORCE)
-set(CMAKE_DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX_CACHE} CACHE STRING "" FORCE)
\ No newline at end of file
diff --git a/CMake/Utilities/imstkAddLibrary.cmake b/CMake/Utilities/imstkAddLibrary.cmake
index a56adf11e47974face309b7515050d29513af559..f124ed858ef0cdad7c997f7ee9997fd1e6b5ddfb 100644
--- a/CMake/Utilities/imstkAddLibrary.cmake
+++ b/CMake/Utilities/imstkAddLibrary.cmake
@@ -14,7 +14,7 @@ function(imstk_add_library target)
set(options VERBOSE)
set(oneValueArgs)
- set(multiValueArgs H_FILES CPP_FILES SUBDIR_LIST DEPENDS)
+ set(multiValueArgs H_FILES CPP_FILES SUBDIR_LIST EXCLUDE_FILES DEPENDS)
include(CMakeParseArguments)
cmake_parse_arguments(target "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN} )
@@ -42,6 +42,13 @@ function(imstk_add_library target)
endif()
endif()
+ # Exclude files
+ foreach(file ${target_EXCLUDE_FILES})
+ list(REMOVE_ITEM target_H_FILES "${CMAKE_CURRENT_SOURCE_DIR}/${file}")
+ list(REMOVE_ITEM target_CPP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/${file}")
+ list(REMOVE_ITEM testing_FILES "${CMAKE_CURRENT_SOURCE_DIR}/${file}")
+ endforeach()
+
if( NOT target_SUBDIR_LIST )
imstk_subdir_list(target_SUBDIR_LIST ${CMAKE_CURRENT_SOURCE_DIR})
endif()
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b7b817a9b8bad83a156f90c46f69f759272cf6e0..bac23b70716265dff58228959d5d4d392461969a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.9)
-project(iMSTK VERSION 2.0.0 LANGUAGES C CXX)
+project(iMSTK VERSION 3.0.0 LANGUAGES C CXX)
if(UNIX AND NOT APPLE)
set(LINUX TRUE)
@@ -88,6 +88,7 @@ set(BUILD_TESTING OFF)
if (${PROJECT_NAME}_BUILD_TESTING)
set(BUILD_TESTING ON)
endif ()
+option(${PROJECT_NAME}_USE_MODEL_REDUCTION "Build with model reduction, requires a VegaFEM built with Intel MKL and arpack" OFF)
#-----------------------------------------------------------------------------
# CTest/Dashboards
@@ -282,6 +283,7 @@ else()
# modules are linked via `VTK::CommonCore`
# vtk_module_autoinit is needed
find_package(VTK COMPONENTS
+ ChartsCore
CommonCore
CommonDataModel
FiltersGeneral
@@ -300,6 +302,7 @@ else()
RenderingOpenGL2
RenderingVolume
RenderingVolumeOpenGL2
+ ViewsContext2D
)
endif()
diff --git a/Data/asianDragon/asianDragon.URendering.float.sha512 b/Data/asianDragon/asianDragon.URendering.float.sha512
new file mode 100644
index 0000000000000000000000000000000000000000..c1f4df24c51ae47d0fcb8fadeb2f89cf0f8b4c27
--- /dev/null
+++ b/Data/asianDragon/asianDragon.URendering.float.sha512
@@ -0,0 +1 @@
+b598cc7b55f34c79eb2fc87505cec2e6069d7f3a8113710f6013eae88bd16ab0cf1fbbd699c83986b0ef6a7a4959dcc82afa68c4a9aff5ad4bb40a9a911e74fd
\ No newline at end of file
diff --git a/Data/asianDragon/asianDragon.cub.sha512 b/Data/asianDragon/asianDragon.cub.sha512
new file mode 100644
index 0000000000000000000000000000000000000000..e158f1fe59e6a12d061ed7036f1e893cd3a8eb00
--- /dev/null
+++ b/Data/asianDragon/asianDragon.cub.sha512
@@ -0,0 +1 @@
+b904fa19e85c7c59e3007ff4c6f80ac9876e0fb26fcc4db93ff13d7b723e2ea67b80ccd7d1171271e8166fa40f1fb730147b91c2bb6a1e04193542d13af4c608
\ No newline at end of file
diff --git a/Docs/source/documentation.rst b/Docs/source/documentation.rst
index 162bc1edd489604c482e204de245f85eed79f262..42beb8e981e5a7def5724b4357c688a19070a1a4 100644
--- a/Docs/source/documentation.rst
+++ b/Docs/source/documentation.rst
@@ -36,7 +36,7 @@ Setup for Development
=====================
iMSTK and its external dependencies can be configured and built from
-scratch Cmake to create a super-build on UNIX (MAC, Linux) and Windows
+scratch with Cmake to create a super-build on UNIX (MAC, Linux) and Windows
platforms. The instructions below describe this process in detail.
Configuration and Build
@@ -1011,7 +1011,7 @@ An example code on how to instantiate a haptic device is shown below
sdk->addModule(server);
-Paralle Support
+Parallel Support
===============
iMSTK allows CPU based shared memory parallelization using Intel TBB library.
@@ -1041,6 +1041,99 @@ options to parallelize over a dimension of choice.
Miscellaneous Topics
====================
+Task Graphs
+------------------
+iMSTK provides Task Graphs. These are used internally but may also be used by the user.
+A TaskGraph contains a set of TaskNode's. Each node accomplishes some computation.
+The TaskGraph also contains edges establishing successor/predeccessor relationships between
+the tasks. For example, one could specify that a task may only happen after two others have
+completed. Additionally a TaskGraph will always contain a source and sink node.
+
+This TaskGraph may be then executed by the user using one of the TaskGraphControllers.
+Currently, there are two provided TaskGraphControllers. SequentialTaskGraphController and
+TbbTaskGraphController. Sequential executes the tasks one by one in topological order while
+Tbb executes them in parallel.
+
+To use a TaskGraph, create one and begin adding functions/nodes to it. One can either
+create a TaskNode or use the overloaded function in TaskGraph to directly supply a function
+pointer. Then we can establish edges between the nodes and execute.
+
+::
+
+ // Inputs
+ const int x = 0;
+ const int y = 2;
+ const int z = 3;
+ const int w = 5;
+
+ // Results
+ int a = 0;
+ int b = 0;
+ int c = 0;
+
+ std::shared_ptr<TaskGraph> graph = std::make_shared<TaskGraph>();
+
+ // Setup Nodes
+ std::shared_ptr<TaskNode> addXYNode = graph->addFunction([&]() { a = x + y; });
+ std::shared_ptr<TaskNode> multZWNode = graph->addFunction([&]() { b = z * w; });
+ std::shared_ptr<TaskNode> addABNode = graph->addFunction([&]() { c = a + b; });
+
+ // Setup Edges
+ graph->addEdge(graph->getSource(), addXYNode);
+ graph->addEdge(graph->getSource(), multZWNode);
+ graph->addEdge(addXYNode, addABNode);
+ graph->addEdge(multZWNode, addABNode);
+ graph->addEdge(addABNode, graph->getSink());
+
+ // Finally, execute
+ std::shared_ptr<TbbTaskGraphController> controller = std::make_shared<TbbTaskGraphController>();
+ controller->setInput(graph);
+ controller->init(); // Possible preprocessing steps
+ controller->execute();
+
+TaskGraphs are also used internally in iMSTK's physics loop. This allows one to reconfigure
+iMSTK's sequence of events. The Scene, SceneObject, and AbstractDynamicalModel classes all contain
+member TaskGraphs which are composited into each other. To modify the main physics loop one should
+modify the top level TaskGraph, that would be the Scene TaskGraph. Keep in mind that the Scene's TaskGraph
+is rebuilt on initialize. One can have their own step within initialization like so:
+
+::
+
+ scene->setComputeGraphConfigureCallback([&](Scene* scene)
+ {
+ // Insert nodes or edges
+ });
+
+Additionally many of the classes in iMSTK expose getters for their nodes. This is useful, for example,
+if one wanted to insert a step after PbdModel does its position integration.
+
+::
+
+ std::shared_ptr<PbdModel> pbdModel = clothObj->getPbdModel();
+ ...
+ scene->setComputeGraphConfigureCallback([&](Scene* scene)
+ {
+ std::shared_ptr<ComputeGraph> graph = scene->getComputeGraph();
+
+ // A TaskNode to print all velocities
+ std::shared_ptr<TaskNode> printVelocities = std::make_shared<TaskNode>([&]()
+ {
+ const StdVectorOfVec3d& velocities = *pbdModel->getCurrentState()->getVelocities();
+ for (size_t i = 0; i < velocities.size(); i++)
+ {
+ printf("Velocity: %f, %f, %f\n", velocities[i].x(), velocities[i].y(), velocities[i].z());
+ }
+ });
+
+ // Add to the graph, after positions are integrated
+ graph->insertAfter(pbdModel->getIntegratePositionNode(), printVelocities);
+ });
+
+Lastly, the TaskGraph comes with a number of graph functions. Static functions such as sum, nest, topological sort,
+transitive reduction, cyclic check, etc. Member functions such as addNode, addFunction, addEdge, insertAfter,
+insertBefore, ... The graph sum and nest are especially useful when maintaining multiple decoupled graphs with identical
+nodes between them that you would then like to combine.
+
Object Controllers
------------------
diff --git a/Examples/BoneShaving/BoneShavingExample.cpp b/Examples/BoneShaving/BoneShavingExample.cpp
index ce8e3912fed516af4b1dd408759ab7c95a281626..1c1e80fd08caeb6a862d810a8b744fc9bd98ea59 100644
--- a/Examples/BoneShaving/BoneShavingExample.cpp
+++ b/Examples/BoneShaving/BoneShavingExample.cpp
@@ -105,13 +105,9 @@ main()
auto objController = std::make_shared<SceneObjectController>(drill, deviceTracker);
scene->addObjectController(objController);
- // Create a collision graph
- auto graph = scene->getCollisionGraph();
- auto pair = graph->addInteractionPair(bone,
- drill,
- CollisionDetection::Type::PointSetToSphere,
- CollisionHandling::Type::BoneDrilling,
- CollisionHandling::Type::None);
+ // Add interaction
+ scene->getCollisionGraph()->addInteraction(makeObjectInteractionPair(bone, drill,
+ InteractionType::FemObjToCollidingObjBoneDrilling, CollisionDetection::Type::PointSetToSphere));
#endif
diff --git a/Examples/CollisionDetection/ManualCDWithOctree/ManualCDWithOctreeExample.cpp b/Examples/CollisionDetection/ManualCDWithOctree/ManualCDWithOctreeExample.cpp
index 1e27dfc2ebb6bd7d121e98654ef881a67b4a8b28..73d2c198927d1b7d58102ab6136b6c92b019aaf7 100644
--- a/Examples/CollisionDetection/ManualCDWithOctree/ManualCDWithOctreeExample.cpp
+++ b/Examples/CollisionDetection/ManualCDWithOctree/ManualCDWithOctreeExample.cpp
@@ -33,7 +33,6 @@
#include "imstkCollidingObject.h"
#include "imstkSceneManager.h"
#include "imstkSurfaceMesh.h"
-#include "imstkCollisionGraph.h"
#include "imstkScene.h"
#include "imstkVTKRenderer.h"
diff --git a/Examples/CreateEnclosingMesh/CreateEnclosingMesh.cpp b/Examples/CreateEnclosingMesh/CreateEnclosingMesh.cpp
index 8130adb1417910bf97584269466a21bfca79cfcc..75206d18c752a6c81fc4e2b022875bc21f3cae35 100644
--- a/Examples/CreateEnclosingMesh/CreateEnclosingMesh.cpp
+++ b/Examples/CreateEnclosingMesh/CreateEnclosingMesh.cpp
@@ -22,7 +22,6 @@
#include "imstkSimulationManager.h"
#include "imstkLight.h"
#include "imstkCamera.h"
-#include "imstkCollisionGraph.h"
#include "imstkAPIUtilities.h"
#include "imstkSurfaceMesh.h"
#include "imstkTetrahedralMesh.h"
diff --git a/Examples/DebugRendering/DebugRenderingExample.cpp b/Examples/DebugRendering/DebugRenderingExample.cpp
index db5a9b8531f971f55b4dbda584bda7e06fde3d7f..f5d03ce135977416bb154fbe5b1becb151905987 100644
--- a/Examples/DebugRendering/DebugRenderingExample.cpp
+++ b/Examples/DebugRendering/DebugRenderingExample.cpp
@@ -24,7 +24,6 @@
#include "imstkLight.h"
#include "imstkCamera.h"
#include "imstkSceneManager.h"
-#include "imstkCollisionGraph.h"
#include "imstkDebugRenderGeometry.h"
#include "imstkAPIUtilities.h"
#include "imstkVTKViewer.h"
diff --git a/Examples/DeformableBody/DeformableBodyExample.cpp b/Examples/DeformableBody/DeformableBodyExample.cpp
index 3093eee8e20e670afea6d73b4e375470f0fe595c..6b2f5092dd4e4f2db818d03573e877f639f8b451 100644
--- a/Examples/DeformableBody/DeformableBodyExample.cpp
+++ b/Examples/DeformableBody/DeformableBodyExample.cpp
@@ -35,7 +35,6 @@
#include "imstkLight.h"
#include "imstkCamera.h"
#include "imstkFEMDeformableBodyModel.h"
-#include "imstkCollisionGraph.h"
#include "imstkSurfaceMesh.h"
#include "imstkScene.h"
@@ -68,25 +67,20 @@ main()
volTetMesh->extractSurfaceMesh(surfMesh, true);
- // Construct a map
-
// Construct one to one nodal map based on the above meshes
auto oneToOneNodalMap = std::make_shared<OneToOneMap>(tetMesh, surfMesh);
- // Scene object 1: Dragon
-
// Configure dynamic model
auto dynaModel = std::make_shared<FEMDeformableBodyModel>();
- //dynaModel->configure(iMSTK_DATA_ROOT "/asianDragon/asianDragon.config");
-
auto config = std::make_shared<FEMModelConfig>();
- config->m_fixedNodeIds = { 51, 127, 178 };
+ config->m_fixedNodeIds = { 50, 126, 177 };
dynaModel->configure(config);
+ //dynaModel->configure(iMSTK_DATA_ROOT "/asianDragon/asianDragon.config");
dynaModel->setTimeStepSizeType(TimeSteppingType::Fixed);
dynaModel->setModelGeometry(volTetMesh);
- auto timeIntegrator = std::make_shared<BackwardEuler>(0.05);// Create and add Backward Euler time integrator
+ auto timeIntegrator = std::make_shared<BackwardEuler>(0.01);// Create and add Backward Euler time integrator
dynaModel->setTimeIntegrator(timeIntegrator);
auto material = std::make_shared<RenderMaterial>();
@@ -97,7 +91,7 @@ main()
auto surfMeshModel = std::make_shared<VisualModel>(surfMesh);
surfMeshModel->setRenderMaterial(material);
- // Scene Object
+ // Scene object 1: Dragon
auto deformableObj = std::make_shared<FeDeformableObject>("Dragon");
deformableObj->addVisualModel(surfMeshModel);
deformableObj->setPhysicsGeometry(volTetMesh);
@@ -114,30 +108,6 @@ main()
planeObj->setCollidingGeometry(planeGeom);
scene->addSceneObject(planeObj);
- // create a nonlinear system
- auto nlSystem = std::make_shared<NonLinearSystem<SparseMatrixd>>(
- dynaModel->getFunction(),
- dynaModel->getFunctionGradient());
-
- nlSystem->setUnknownVector(dynaModel->getUnknownVec());
- nlSystem->setUpdateFunction(dynaModel->getUpdateFunction());
- nlSystem->setUpdatePreviousStatesFunction(dynaModel->getUpdatePrevStateFunction());
-
- // create a linear solver
- auto linSolver = std::make_shared<ConjugateGradient>();
-
- if (linSolver->getType() == imstk::LinearSolver<imstk::SparseMatrixd>::Type::GaussSeidel
- && dynaModel->isFixedBCImplemented())
- {
- LOG(WARNING) << "The GS solver may not be viable!";
- }
-
- // create a non-linear solver and add to the scene
- auto nlSolver = std::make_shared<NewtonSolver<SparseMatrixd>>();
- nlSolver->setLinearSolver(linSolver);
- nlSolver->setSystem(nlSystem);
- scene->addNonlinearSolver(nlSolver);
-
// Light
auto light = std::make_shared<DirectionalLight>("light");
light->setFocalPoint(Vec3d(5, -8, -5));
diff --git a/Examples/GeometryProcessing/GeometryProcessingExample.cpp b/Examples/GeometryProcessing/GeometryProcessingExample.cpp
index 49cab445e4764141a987c9b0486134061de4af6f..5a050fb534c2aed0e2fb9882bb9154c8ffe4b1ab 100644
--- a/Examples/GeometryProcessing/GeometryProcessingExample.cpp
+++ b/Examples/GeometryProcessing/GeometryProcessingExample.cpp
@@ -31,7 +31,6 @@
#include "imstkMeshIO.h"
#include "imstkSurfaceMesh.h"
#include "imstkTetrahedralMesh.h"
-#include "imstkCollisionGraph.h"
#include "imstkScene.h"
using namespace imstk;
diff --git a/Examples/GeometryTransforms/GeometryTransformsExample.cpp b/Examples/GeometryTransforms/GeometryTransformsExample.cpp
index 4417b0123ae67f7249580812307473983cb10bce..f1f69870900cd42b47d1e540c0edecb5c2aca512 100644
--- a/Examples/GeometryTransforms/GeometryTransformsExample.cpp
+++ b/Examples/GeometryTransforms/GeometryTransformsExample.cpp
@@ -21,7 +21,6 @@
#include "imstkSimulationManager.h"
#include "imstkSceneManager.h"
-#include "imstkCollisionGraph.h"
#include "imstkSceneObject.h"
#include "imstkCamera.h"
#include "imstkLight.h"
diff --git a/Examples/LineMesh/LineMeshExample.cpp b/Examples/LineMesh/LineMeshExample.cpp
index 7a8c6cf3de7ea5d97a7e721bf8a68c2bdf78b33b..89b8fa8f91ca5cee6b7c50fb17e8253b4d97d3f5 100644
--- a/Examples/LineMesh/LineMeshExample.cpp
+++ b/Examples/LineMesh/LineMeshExample.cpp
@@ -20,7 +20,6 @@
=========================================================================*/
#include "imstkSimulationManager.h"
-#include "imstkCollisionGraph.h"
#include "imstkAPIUtilities.h"
#include "imstkCamera.h"
#include "imstkLineMesh.h"
diff --git a/Examples/MultipleScenes/multipleScenes.cpp b/Examples/MultipleScenes/multipleScenes.cpp
index ca7f47f6260aa6a56494be66b41aedbb6ace7fbd..8b5cd383bc0ab66fc08555a8da941cc08f20ffaf 100644
--- a/Examples/MultipleScenes/multipleScenes.cpp
+++ b/Examples/MultipleScenes/multipleScenes.cpp
@@ -28,7 +28,6 @@
#include "imstkSurfaceMesh.h"
#include "imstkMeshIO.h"
#include "imstkTetrahedralMesh.h"
-#include "imstkCollisionGraph.h"
#include "imstkPlane.h"
#include "imstkScene.h"
@@ -66,15 +65,15 @@ createSoftBodyScene(std::shared_ptr<SimulationManager> simManager, const char* s
auto pbdParams = std::make_shared<PBDModelConfig>();
// FEM constraint
- pbdParams->m_YoungModulus = 100.0;
- pbdParams->m_PoissonRatio = 0.3;
+ pbdParams->m_femParams->m_YoungModulus = 100.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
pbdParams->m_fixedNodeIds = { 51, 127, 178 };
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet, PbdFEMConstraint::MaterialType::StVK);
// Other parameters
pbdParams->m_uniformMassValue = 1.0;
- pbdParams->m_gravity = Vec3d(0, -9.8, 0);
- pbdParams->m_maxIter = 45;
+ pbdParams->m_gravity = Vec3d(0, -9.8, 0);
+ pbdParams->m_iterations = 45;
// Set the parameters
pbdModel->configure(pbdParams);
@@ -164,9 +163,9 @@ createClothScene(std::shared_ptr<SimulationManager> simManager, const char* scen
// Other parameters
pbdParams->m_uniformMassValue = 1.0;
- pbdParams->m_gravity = Vec3d(0, -9.8, 0);
- pbdParams->m_DefaultDt = 0.03;
- pbdParams->m_maxIter = 5;
+ pbdParams->m_gravity = Vec3d(0, -9.8, 0);
+ pbdParams->m_defaultDt = 0.03;
+ pbdParams->m_iterations = 5;
// Set the parameters
pbdModel->configure(pbdParams);
diff --git a/Examples/NonlinearSolver/nonlinearSolver.cpp b/Examples/NonlinearSolver/nonlinearSolver.cpp
index 1725f74c8351bfb07f27fed3fbb7c273434782d9..1e50fa5a3f7ba643b2ef9eb0466e52ce2ab20dd6 100644
--- a/Examples/NonlinearSolver/nonlinearSolver.cpp
+++ b/Examples/NonlinearSolver/nonlinearSolver.cpp
@@ -34,40 +34,40 @@ using namespace imstk;
int
main(int argc, char** argv)
{
- const int N = 2;
- auto x = Vectord(N);
+ const int N = 2;
+ auto x = Vectord(N);
auto xe = Vectord(N);
- auto y = Vectord(N);
- auto A = Matrixd(N, N);
+ auto y = Vectord(N);
+ auto A = Matrixd(N, N);
- x[0] = 100.0;
- x[1] = 100.0;
+ x[0] = 100.0;
+ x[1] = 100.0;
xe[0] = 1.0;
xe[1] = 10.0;
auto func = [&y](const Vectord& x, const bool isSemiImplicit) -> const Vectord& {
- // auto y = Vectord(x.size());
- y[0] = x[0] * x[0] - 1.0;
- y[1] = x[1] * x[1] - 100.0;
+ // auto y = Vectord(x.size());
+ y[0] = x[0] * x[0] - 1.0;
+ y[1] = x[1] * x[1] - 100.0;
- return y;
- };
+ return y;
+ };
auto jac = [&A](const Vectord& x) -> const Matrixd& {
- // auto A = Matrixd(x.size(), x.size());
- A(0, 0) = 2 * x[0];
- A(0, 1) = 0.0;
- A(1, 0) = 0.0;
- A(1, 1) = 2 * x[1];
+ // auto A = Matrixd(x.size(), x.size());
+ A(0, 0) = 2 * x[0];
+ A(0, 1) = 0.0;
+ A(1, 0) = 0.0;
+ A(1, 1) = 2 * x[1];
- return A;
- };
+ return A;
+ };
auto updateX = [&x](const Vectord& du, const bool isSemiImplicit)
- {
- x -= du;
- return;
- };
+ {
+ x -= du;
+ return;
+ };
auto updateXold = [](void) {};
@@ -84,8 +84,8 @@ main(int argc, char** argv)
nlSolver->setSystem(nlSystem);
nlSolver->setLinearSolver(linSolver);
- std::cout << "init_error = " << std::setprecision(12) << std::scientific << (x-xe).norm() << std::endl;
+ std::cout << "init_error = " << std::setprecision(12) << std::scientific << (x - xe).norm() << std::endl;
nlSolver->solve();
- std::cout << "final_error = " << std::setprecision(12) << std::scientific << (x-xe).norm() << std::endl;
+ std::cout << "final_error = " << std::setprecision(12) << std::scientific << (x - xe).norm() << std::endl;
}
diff --git a/Examples/ObjectControllerDummyClient/ObjectCtrlDummyClientExample.cpp b/Examples/ObjectControllerDummyClient/ObjectCtrlDummyClientExample.cpp
index 87b0344db7294d62036c282e0f9aa48bbfc39362..632019b2aaa2cf9b8f9506b6c88b0ca63c1102f5 100644
--- a/Examples/ObjectControllerDummyClient/ObjectCtrlDummyClientExample.cpp
+++ b/Examples/ObjectControllerDummyClient/ObjectCtrlDummyClientExample.cpp
@@ -21,7 +21,6 @@
#include "imstkSimulationManager.h"
#include "imstkCollidingObject.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
#include "imstkLight.h"
#include "imstkSceneManager.h"
diff --git a/Examples/PBD/PBDCloth/pbdClothExample.cpp b/Examples/PBD/PBDCloth/pbdClothExample.cpp
index b1f99326c073190d53920319e6c88bbed074af63..30289cab718e0246749e2ef0f2df24ed285233da 100644
--- a/Examples/PBD/PBDCloth/pbdClothExample.cpp
+++ b/Examples/PBD/PBDCloth/pbdClothExample.cpp
@@ -24,7 +24,6 @@
#include "imstkPbdObject.h"
#include "imstkAPIUtilities.h"
#include "imstkSurfaceMesh.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
#include "imstkLight.h"
#include "imstkScene.h"
@@ -100,20 +99,19 @@ main()
auto pbdParams = std::make_shared<PBDModelConfig>();
// Constraints
- pbdParams->enableConstraint(PbdConstraint::Type::Distance, 0.1);
- pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 0.001);
- std::vector<size_t> fixedNodes(nCols);
- for (size_t i = 0; i < fixedNodes.size(); i++)
- {
- fixedNodes[i] = i;
- }
+ // pbdParams->enableConstraint(PbdConstraint::Type::Distance, 0.1);
+ // pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 1e2);
+ // pbdParams->m_solverType = PbdConstraint::SolverType::PBD;
+ pbdParams->enableConstraint(PbdConstraint::Type::Distance, 1e2);
+ pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 1e1);
+ std::vector<size_t> fixedNodes = { 0, nCols - 1 };
pbdParams->m_fixedNodeIds = fixedNodes;
// Other parameters
- pbdParams->m_uniformMassValue = 1.0;
- pbdParams->m_gravity = Vec3d(0, -9.8, 0);
- pbdParams->m_DefaultDt = 0.005;
- pbdParams->m_maxIter = 5;
+ pbdParams->m_uniformMassValue = width * height / (nRows * nCols);
+ pbdParams->m_gravity = Vec3d(0, -9.8, 0);
+ pbdParams->m_defaultDt = 0.005;
+ pbdParams->m_iterations = 5;
// Set the parameters
pbdModel->configure(pbdParams);
diff --git a/Examples/PBD/PBDCollisionMultipleObjects/PBDCollisionMultipleObjectsExample.cpp b/Examples/PBD/PBDCollisionMultipleObjects/PBDCollisionMultipleObjectsExample.cpp
index e620279b58b856420c1cfdea800110c2b56ec541..ed1e6083bdab7216fe910523c411c34c906dbf94 100644
--- a/Examples/PBD/PBDCollisionMultipleObjects/PBDCollisionMultipleObjectsExample.cpp
+++ b/Examples/PBD/PBDCollisionMultipleObjects/PBDCollisionMultipleObjectsExample.cpp
@@ -19,22 +19,18 @@
=========================================================================*/
-#include "imstkSimulationManager.h"
-#include "imstkLight.h"
#include "imstkCamera.h"
#include "imstkCollisionGraph.h"
-#include "imstkSceneManager.h"
-#include "imstkPbdModel.h"
-#include "imstkPbdObject.h"
-#include "imstkTetrahedralMesh.h"
+#include "imstkLight.h"
#include "imstkMeshIO.h"
+#include "imstkObjectInteractionFactory.h"
#include "imstkOneToOneMap.h"
-#include "imstkSurfaceMeshToSurfaceMeshCD.h"
-#include "imstkPBDCollisionHandling.h"
-#include "imstkVTKTextStatusManager.h"
-#include "imstkCollisionData.h"
-#include "imstkSurfaceMesh.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
#include "imstkScene.h"
+#include "imstkSimulationManager.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkTetrahedralMesh.h"
// Enable this macro to generate many dragons
#define BIG_SCENE
@@ -124,17 +120,16 @@ generateDragon(const std::shared_ptr<imstk::Scene>& scene,
auto pbdParams = std::make_shared<PBDModelConfig>();
// FEM constraint
- pbdParams->m_YoungModulus = 1000.0;
- pbdParams->m_PoissonRatio = 0.3;
+ pbdParams->m_femParams->m_YoungModulus = 1000.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet, PbdFEMConstraint::MaterialType::StVK);
// Other parameters
pbdParams->m_uniformMassValue = 5.0;
- pbdParams->m_gravity = Vec3d(0, -1.0, 0);
- pbdParams->m_DefaultDt = 0.01;
- pbdParams->m_maxIter = 20;
- pbdParams->m_proximity = 0.5;
- pbdParams->m_contactStiffness = 0.1;
+ pbdParams->m_gravity = Vec3d(0, -1.0, 0);
+ pbdParams->m_defaultDt = 0.01;
+ pbdParams->m_iterations = 20;
+ pbdParams->collisionParams->m_proximity = 0.5;
pbdModel->configure(pbdParams);
deformableObj->setDynamicalModel(pbdModel);
@@ -156,10 +151,6 @@ main()
auto viewer = std::dynamic_pointer_cast<VTKViewer>(simManager->getViewer());
viewer->getVtkRenderWindow()->SetSize(1920, 1080);
- auto statusManager = viewer->getTextStatusManager();
- statusManager->setStatusFontSize(VTKTextStatusManager::Custom, 25);
- statusManager->setStatusFontColor(VTKTextStatusManager::Custom, Color::Orange);
-
// Build floor geometry
const double width = 100.0;
const double height = 100.0;
@@ -194,48 +185,45 @@ main()
}
}
- auto floorMesh = std::make_shared<SurfaceMesh>();
- floorMesh->initialize(vertList, triangles);
-
- auto materialFloor = std::make_shared<RenderMaterial>();
- materialFloor->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
- auto floorMeshModel = std::make_shared<VisualModel>(floorMesh);
- floorMeshModel->setRenderMaterial(materialFloor);
-
- auto floor = std::make_shared<PbdObject>("Floor");
- floor->setCollidingGeometry(floorMesh);
- floor->setVisualGeometry(floorMesh);
- floor->setPhysicsGeometry(floorMesh);
-
- auto pbdModel2 = std::make_shared<PbdModel>();
- pbdModel2->setModelGeometry(floorMesh);
-
- // configure model
- auto pbdParams2 = std::make_shared<PBDModelConfig>();
- pbdParams2->m_uniformMassValue = 0.0;
- pbdParams2->m_proximity = 0.1;
- pbdParams2->m_contactStiffness = 0.1;
- pbdParams2->m_maxIter = 0;
-
- // Set the parameters
- pbdModel2->configure(pbdParams2);
- floor->setDynamicalModel(pbdModel2);
- scene->addSceneObject(floor);
+ auto floorObj = std::make_shared<PbdObject>("Floor");
+ {
+ auto floorMesh = std::make_shared<SurfaceMesh>();
+ floorMesh->initialize(vertList, triangles);
+
+ auto materialFloor = std::make_shared<RenderMaterial>();
+ materialFloor->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+ auto floorMeshModel = std::make_shared<VisualModel>(floorMesh);
+ floorMeshModel->setRenderMaterial(materialFloor);
+
+ floorObj->setCollidingGeometry(floorMesh);
+ floorObj->setVisualGeometry(floorMesh);
+ floorObj->setPhysicsGeometry(floorMesh);
+
+ auto pbdFloorModel = std::make_shared<PbdModel>();
+ pbdFloorModel->setModelGeometry(floorMesh);
+
+ // Configure floor
+ auto pbdFloorConfig = std::make_shared<PBDModelConfig>();
+ pbdFloorConfig->m_uniformMassValue = 0.0;
+ pbdFloorConfig->collisionParams->m_proximity = -0.1;
+ pbdFloorConfig->m_iterations = 0;
+
+ // Set the parameters
+ pbdFloorModel->configure(pbdFloorConfig);
+ floorObj->setDynamicalModel(pbdFloorModel);
+ scene->addSceneObject(floorObj);
+ }
#ifdef BIG_SCENE
const int expandsXZ = 1;
- const int expandsY = 4;
+ const int expandsY = 2;
#endif
const double shiftX = 5.0;
const double distanceXZ = 10.0;
const double distanceY = 5.0;
const double minHeight = -5.0;
- std::vector<std::shared_ptr<SurfaceMesh>> surfaceMeshes;
- std::vector<std::shared_ptr<PbdObject>> pbdObjs;
- //std::vector<std::shared_ptr<PbdSolver>> pbdSolvers;
- std::vector<std::shared_ptr<CollisionData>> allCollisionData;
- size_t numTriangles = 0;
+ std::vector<std::shared_ptr<PbdObject>> pbdObjs;
#ifdef BIG_SCENE
for (int i = -expandsXZ; i < expandsXZ; ++i)
@@ -260,59 +248,23 @@ main()
std::shared_ptr<PbdSolver> solver;
Vec3d translation(shiftX + i * distanceXZ, minHeight + j * distanceY, k * distanceXZ);
generateDragon(scene, translation, mesh, pbdObj, solver);
- surfaceMeshes.push_back(mesh);
pbdObjs.push_back(pbdObj);
- //pbdSolvers.push_back(solver);
- numTriangles += mesh->getNumTriangles();
-
- // Collision between dragon with floor
- const auto colData = std::make_shared<CollisionData>();
- const auto CD = std::make_shared<SurfaceMeshToSurfaceMeshCD>(mesh, floorMesh, colData);
- const auto CH = std::make_shared<PBDCollisionHandling>(CollisionHandling::Side::A,
- colData, pbdObj, floor);
- scene->getCollisionGraph()->addInteractionPair(pbdObj, floor, CD, CH, nullptr);
- allCollisionData.push_back(colData);
+
+ scene->getCollisionGraph()->addInteraction(makeObjectInteractionPair(pbdObj, floorObj,
+ InteractionType::PbdObjToPbdObjCollision, CollisionDetection::Type::SurfaceMeshToSurfaceMesh));
}
}
}
- for (size_t i = 0; i < surfaceMeshes.size(); ++i)
+ for (size_t i = 0; i < pbdObjs.size(); ++i)
{
- for (size_t j = 0; j < surfaceMeshes.size(); ++j)
+ for (size_t j = i + 1; j < pbdObjs.size(); ++j)
{
- if (i == j)
- {
- continue;
- }
-
- // Collision between two dragons
- const auto colData = std::make_shared<CollisionData>();
- const auto CD = std::make_shared<SurfaceMeshToSurfaceMeshCD>(surfaceMeshes[i], surfaceMeshes[j], colData);
- const auto CH = std::make_shared<PBDCollisionHandling>(CollisionHandling::Side::A,
- colData, pbdObjs[i], pbdObjs[j]);
- scene->getCollisionGraph()->addInteractionPair(pbdObjs[i], pbdObjs[j], CD, CH, nullptr);
+ scene->getCollisionGraph()->addInteraction(makeObjectInteractionPair(pbdObjs[i], pbdObjs[j],
+ InteractionType::PbdObjToPbdObjCollision, CollisionDetection::Type::SurfaceMeshToSurfaceMesh));
}
}
- auto updateFunc =
- [&](Module*) {
- size_t numVTCollisions = 0;
- size_t numEECollisions = 0;
- for (const auto& colData: allCollisionData)
- {
- numVTCollisions += colData->VTColData.getSize();
- numEECollisions += colData->EEColData.getSize();
- }
-
- std::stringstream ss;
- ss << "Num. dragons: " << surfaceMeshes.size()
- << "\nNum. surface triangles: " << numTriangles
- << "\nNum. collision: " << numVTCollisions << " (VT) | "
- << numEECollisions << " (EE)";
- statusManager->setCustomStatus(ss.str());
- };
- simManager->getSceneManager(scene)->setPostUpdateCallback(updateFunc);
-
// Light
auto light = std::make_shared<DirectionalLight>("light");
light->setFocalPoint(Vec3d(5, -8, -5));
diff --git a/Examples/PBD/PBDCollisionOneObject/PBDCollisionOneObjectExample.cpp b/Examples/PBD/PBDCollisionOneObject/PBDCollisionOneObjectExample.cpp
index bff74bf450b6e20285e5fd7ca717a812695faff7..6e07581e8058f2fc50066c8f65682da82af48f28 100644
--- a/Examples/PBD/PBDCollisionOneObject/PBDCollisionOneObjectExample.cpp
+++ b/Examples/PBD/PBDCollisionOneObject/PBDCollisionOneObjectExample.cpp
@@ -19,20 +19,19 @@
=========================================================================*/
-#include "imstkSimulationManager.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
+#include "imstkCollisionGraph.h"
#include "imstkLight.h"
-#include "imstkPbdModel.h"
-#include "imstkPbdObject.h"
-#include "imstkTetrahedralMesh.h"
#include "imstkMeshIO.h"
+#include "imstkObjectInteractionFactory.h"
#include "imstkOneToOneMap.h"
-#include "imstkMeshToMeshBruteForceCD.h"
-#include "imstkPBDCollisionHandling.h"
-#include "imstkTetraTriangleMap.h"
-#include "imstkSurfaceMesh.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
#include "imstkScene.h"
+#include "imstkSimulationManager.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkTaskGraphVizWriter.h"
+#include "imstkTetraTriangleMap.h"
using namespace imstk;
@@ -44,8 +43,7 @@ int
main()
{
auto simManager = std::make_shared<SimulationManager>();
- auto scene = simManager->createNewScene("PbdCollision");
-
+ auto scene = simManager->createNewScene("PbdCollisionOneDragon");
scene->getCamera()->setPosition(0, 3.0, 20.0);
scene->getCamera()->setFocalPoint(0.0, -10.0, 0.0);
@@ -84,18 +82,18 @@ main()
auto pbdParams = std::make_shared<PBDModelConfig>();
// FEM constraint
- pbdParams->m_YoungModulus = 1000.0;
- pbdParams->m_PoissonRatio = 0.3;
+ pbdParams->m_femParams->m_YoungModulus = 1000.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet,
PbdFEMConstraint::MaterialType::Corotation);
// Other parameters
pbdParams->m_uniformMassValue = 1.0;
- pbdParams->m_gravity = Vec3d(0, -10.0, 0);
- pbdParams->m_DefaultDt = 0.01;
- pbdParams->m_maxIter = 5;
- pbdParams->m_proximity = 0.3;
- pbdParams->m_contactStiffness = 0.1;
+ pbdParams->m_gravity = Vec3d(0, -10.0, 0);
+ pbdParams->m_defaultDt = 0.01;
+ pbdParams->m_iterations = 5;
+ pbdParams->collisionParams->m_proximity = 0.3;
+ pbdParams->collisionParams->m_stiffness = 0.1;
pbdModel->configure(pbdParams);
deformableObj->setDynamicalModel(pbdModel);
@@ -155,9 +153,8 @@ main()
// configure model
auto pbdParams2 = std::make_shared<PBDModelConfig>();
pbdParams2->m_uniformMassValue = 0.0;
- pbdParams2->m_proximity = 0.1;
- pbdParams2->m_contactStiffness = 1.0;
- pbdParams2->m_maxIter = 0;
+ pbdParams2->m_iterations = 0;
+ pbdParams2->collisionParams->m_proximity = -0.1;
// Set the parameters
pbdModel2->configure(pbdParams2);
@@ -166,10 +163,8 @@ main()
scene->addSceneObject(floor);
// Collision
- scene->getCollisionGraph()->addInteractionPair(deformableObj, floor,
- CollisionDetection::Type::MeshToMeshBruteForce,
- CollisionHandling::Type::PBD,
- CollisionHandling::Type::None);
+ scene->getCollisionGraph()->addInteraction(makeObjectInteractionPair(deformableObj, floor,
+ InteractionType::PbdObjToPbdObjCollision, CollisionDetection::Type::MeshToMeshBruteForce));
// Light
auto light = std::make_shared<DirectionalLight>("light");
diff --git a/Examples/PBD/PBDCollisionStairs/PBDCollisionStairsExample.cpp b/Examples/PBD/PBDCollisionStairs/PBDCollisionStairsExample.cpp
index c6b99cbd37d7d093f9704d354dcb22f08be9fca4..f14c7df8d337bc6187ba9ac0a515d049c55637bc 100644
--- a/Examples/PBD/PBDCollisionStairs/PBDCollisionStairsExample.cpp
+++ b/Examples/PBD/PBDCollisionStairs/PBDCollisionStairsExample.cpp
@@ -20,6 +20,8 @@
//=========================================================================*/
#include "imstkCamera.h"
+#include "imstkCollisionGraph.h"
+#include "imstkLight.h"
#include "imstkMeshIO.h"
#include "imstkMeshToMeshBruteForceCD.h"
#include "imstkOneToOneMap.h"
@@ -27,13 +29,12 @@
#include "imstkPbdModel.h"
#include "imstkPbdObject.h"
#include "imstkPbdSolver.h"
+#include "imstkScene.h"
#include "imstkSimulationManager.h"
+#include "imstkSurfaceMesh.h"
#include "imstkTetrahedralMesh.h"
#include "imstkTetraTriangleMap.h"
-#include "imstkCollisionGraph.h"
-#include "imstkSurfaceMesh.h"
-#include "imstkLight.h"
-#include "imstkScene.h"
+#include "imstkObjectInteractionFactory.h"
using namespace imstk;
@@ -92,6 +93,87 @@ buildStairs(int nSteps, double width, double height, double depth)
return stairMesh;
}
+static std::shared_ptr<PbdObject>
+makeDragonPbdObject(const std::string& name)
+{
+ auto pbdObj = std::make_shared<PbdObject>(name);
+
+ // Read in the dragon mesh
+ auto highResSurfMesh = std::dynamic_pointer_cast<SurfaceMesh>(MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.obj"));
+ auto coarseTetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.veg"));
+ highResSurfMesh->translate(Vec3d(0.0f, 10.0f, 0.0f), Geometry::TransformType::ApplyToData);
+ coarseTetMesh->translate(Vec3d(0.0f, 10.0f, 0.0f), Geometry::TransformType::ApplyToData);
+ auto coarseSurfMesh = std::make_shared<SurfaceMesh>();
+ coarseTetMesh->extractSurfaceMesh(coarseSurfMesh, true);
+
+ // Setup the Parameters
+ auto pbdParams = std::make_shared<PBDModelConfig>();
+ pbdParams->m_femParams->m_YoungModulus = 1000.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
+ pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet,
+ PbdFEMConstraint::MaterialType::StVK);
+ pbdParams->m_uniformMassValue = 1.0;
+ pbdParams->m_gravity = Vec3d(0, -10.0, 0);
+ pbdParams->m_defaultDt = 0.01;
+ pbdParams->m_iterations = 10;
+ pbdParams->collisionParams->m_proximity = 0.3;
+ pbdParams->collisionParams->m_stiffness = 0.1;
+
+ // Setup the Model
+ auto pbdModel = std::make_shared<PbdModel>();
+ pbdModel->setModelGeometry(coarseTetMesh);
+ pbdModel->configure(pbdParams);
+
+ // Setup the VisualModel
+ auto material = std::make_shared<RenderMaterial>();
+ material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+ auto surfMeshModel = std::make_shared<VisualModel>(highResSurfMesh);
+ surfMeshModel->setRenderMaterial(material);
+
+ // Setup the Object
+ pbdObj->addVisualModel(surfMeshModel);
+ pbdObj->setCollidingGeometry(coarseSurfMesh);
+ pbdObj->setPhysicsGeometry(coarseTetMesh);
+ pbdObj->setPhysicsToCollidingMap(std::make_shared<OneToOneMap>(coarseTetMesh, coarseSurfMesh));
+ pbdObj->setPhysicsToVisualMap(std::make_shared<TetraTriangleMap>(coarseTetMesh, highResSurfMesh));
+ pbdObj->setDynamicalModel(pbdModel);
+
+ return pbdObj;
+}
+
+static std::shared_ptr<PbdObject>
+makeStairsPbdObject(const std::string& name, int numSteps, double width, double height, double depth)
+{
+ auto stairObj = std::make_shared<PbdObject>(name);
+
+ std::shared_ptr<SurfaceMesh> stairMesh(std::move(buildStairs(numSteps, width, height, depth)));
+
+ // Setup the parameters
+ auto pbdParams = std::make_shared<PBDModelConfig>();
+ pbdParams->m_uniformMassValue = 0.0;
+ pbdParams->collisionParams->m_proximity = -0.1;
+ pbdParams->m_iterations = 0;
+
+ // Setup the model
+ auto pbdModel = std::make_shared<PbdModel>();
+ pbdModel->setModelGeometry(stairMesh);
+ pbdModel->configure(pbdParams);
+
+ // Setup the VisualModel
+ auto stairMaterial = std::make_shared<RenderMaterial>();
+ stairMaterial->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+ auto stairMeshModel = std::make_shared<VisualModel>(stairMesh);
+ stairMeshModel->setRenderMaterial(stairMaterial);
+
+ stairObj->addVisualModel(stairMeshModel);
+ stairObj->setDynamicalModel(pbdModel);
+ stairObj->setCollidingGeometry(stairMesh);
+ stairObj->setVisualGeometry(stairMesh);
+ stairObj->setPhysicsGeometry(stairMesh);
+
+ return stairObj;
+}
+
///
/// \brief This example demonstrates the collision interaction
/// using Position based dynamic on a more complex mesh
@@ -100,97 +182,25 @@ int
main()
{
auto simManager = std::make_shared<SimulationManager>();
- auto scene = simManager->createNewScene("PbdCollision");
+ auto scene = simManager->createNewScene("PbdStairsCollision");
scene->getCamera()->setPosition(0.0, 0.0, -30.0);
scene->getCamera()->setFocalPoint(0.0, 0.0, 0.0);
- auto deformableObj = std::make_shared<PbdObject>("DeformableObj");
- {
- // Read in the dragon mesh
- auto highResSurfMesh = std::dynamic_pointer_cast<SurfaceMesh>(MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.obj"));
- auto coarseTetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.veg"));
- highResSurfMesh->translate(Vec3d(0.0f, 10.0f, 0.0f), Geometry::TransformType::ApplyToData);
- coarseTetMesh->translate(Vec3d(0.0f, 10.0f, 0.0f), Geometry::TransformType::ApplyToData);
- auto coarseSurfMesh = std::make_shared<SurfaceMesh>();
- coarseTetMesh->extractSurfaceMesh(coarseSurfMesh, true);
-
- // Setup parameters
- auto pbdParams = std::make_shared<PBDModelConfig>();
- pbdParams->m_YoungModulus = 1000.0;
- pbdParams->m_PoissonRatio = 0.3;
- pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet, PbdFEMConstraint::MaterialType::StVK);
- pbdParams->m_uniformMassValue = 1.0;
- pbdParams->m_gravity = Vec3d(0, -10.0, 0);
- pbdParams->m_DefaultDt = 0.01;
- pbdParams->m_maxIter = 5;
- pbdParams->m_proximity = 0.3;
- pbdParams->m_contactStiffness = 0.08;
-
- // Setup Model
- auto pbdModel = std::make_shared<PbdModel>();
- pbdModel->setModelGeometry(coarseTetMesh);
- pbdModel->configure(pbdParams);
-
- // Setup VisualModel
- auto material = std::make_shared<RenderMaterial>();
- material->setDisplayMode(RenderMaterial::DisplayMode::Surface);
- auto surfMeshModel = std::make_shared<VisualModel>(highResSurfMesh);
- surfMeshModel->setRenderMaterial(material);
-
- // Setup Object
- deformableObj->addVisualModel(surfMeshModel);
- deformableObj->setCollidingGeometry(coarseSurfMesh);
- deformableObj->setPhysicsGeometry(coarseTetMesh);
- deformableObj->setPhysicsToCollidingMap(std::make_shared<OneToOneMap>(coarseTetMesh, coarseSurfMesh));
- deformableObj->setPhysicsToVisualMap(std::make_shared<TetraTriangleMap>(coarseTetMesh, highResSurfMesh));
- deformableObj->setDynamicalModel(pbdModel);
-
- // Add to scene
- scene->addSceneObject(deformableObj);
- }
+ // Create and add the dragon to the scene
+ auto pbdDragon1 = makeDragonPbdObject("PbdDragon1");
+ scene->addSceneObject(pbdDragon1);
- auto stairObj = std::make_shared<PbdObject>("Floor");
- {
- std::shared_ptr<SurfaceMesh> stairMesh(std::move(buildStairs(15, 20.0, 10.0, 20.0)));
-
- // Setup parameters
- auto pbdParams2 = std::make_shared<PBDModelConfig>();
- pbdParams2->m_uniformMassValue = 0.0;
- pbdParams2->m_proximity = 0.1;
- pbdParams2->m_contactStiffness = 0.08;
- pbdParams2->m_maxIter = 0;
-
- // Setup model
- auto pbdModel2 = std::make_shared<PbdModel>();
- pbdModel2->setModelGeometry(stairMesh);
- pbdModel2->configure(pbdParams2);
-
- // Setup VisualModel
- auto stairMaterial = std::make_shared<RenderMaterial>();
- stairMaterial->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
- stairMaterial->setEdgeColor(Color::Black);
- auto stairMeshModel = std::make_shared<VisualModel>(stairMesh);
- stairMeshModel->setRenderMaterial(stairMaterial);
-
- stairObj->addVisualModel(stairMeshModel);
- stairObj->setDynamicalModel(pbdModel2);
- stairObj->setCollidingGeometry(stairMesh);
- stairObj->setVisualGeometry(stairMesh);
- stairObj->setPhysicsGeometry(stairMesh);
-
- scene->addSceneObject(stairObj);
- }
+ auto stairObj = makeStairsPbdObject("PbdStairs", 12, 20.0, 10.0, 20.0);
+ scene->addSceneObject(stairObj);
// Collision
- scene->getCollisionGraph()->addInteractionPair(deformableObj, stairObj,
- CollisionDetection::Type::SurfaceMeshToSurfaceMesh,
- CollisionHandling::Type::PBD,
- CollisionHandling::Type::None);
+ scene->getCollisionGraph()->addInteraction(makeObjectInteractionPair(pbdDragon1, stairObj,
+ InteractionType::PbdObjToPbdObjCollision, CollisionDetection::Type::MeshToMeshBruteForce));
// Light
- auto light = std::make_shared<DirectionalLight>("light");
- light->setFocalPoint(Vec3d(5, -8, 5));
- light->setIntensity(1.2);
+ auto light = std::make_shared<DirectionalLight>("Light");
+ light->setFocalPoint(Vec3d(5.0, -8.0, 5.0));
+ light->setIntensity(1.0);
scene->addLight(light);
auto light2 = std::make_shared<DirectionalLight>("light 2");
diff --git a/Examples/PBD/PBDDeformableObject/PBD3DDeformableObject.cpp b/Examples/PBD/PBDDeformableObject/PBD3DDeformableObject.cpp
index aedde8a7dcc6b6a878977a39b19dfbaefc9fb28e..925ae79cce40699377c0c54e960e9ecf21e727f9 100644
--- a/Examples/PBD/PBDDeformableObject/PBD3DDeformableObject.cpp
+++ b/Examples/PBD/PBDDeformableObject/PBD3DDeformableObject.cpp
@@ -27,7 +27,6 @@
#include "imstkOneToOneMap.h"
#include "imstkAPIUtilities.h"
#include "imstkTetraTriangleMap.h"
-#include "imstkCollisionGraph.h"
#include "imstkSurfaceMesh.h"
#include "imstkCamera.h"
#include "imstkPlane.h"
@@ -67,15 +66,16 @@ main()
auto pbdParams = std::make_shared<PBDModelConfig>();
// FEM constraint
- pbdParams->m_YoungModulus = 100.0;
- pbdParams->m_PoissonRatio = 0.3;
- pbdParams->m_fixedNodeIds = { 51, 127, 178 };
+ pbdParams->m_femParams->m_YoungModulus = 1000.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
+ pbdParams->m_fixedNodeIds = { 50, 126, 177 };
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet, PbdFEMConstraint::MaterialType::StVK);
// Other parameters
pbdParams->m_uniformMassValue = 1.0;
- pbdParams->m_gravity = Vec3d(0, -9.8, 0);
- pbdParams->m_maxIter = 45;
+ pbdParams->m_gravity = Vec3d(0, -9.8, 0);
+ pbdParams->m_iterations = 15;
+ // pbdParams->m_solverType = PbdConstraint::SolverType::PBD;
// Set the parameters
pbdModel->configure(pbdParams);
@@ -87,8 +87,6 @@ main()
deformableObj->setPhysicsGeometry(tetMesh);
deformableObj->setPhysicsToVisualMap(map); //assign the computed map
- deformableObj->setDynamicalModel(pbdModel);
-
scene->addSceneObject(deformableObj);
// Setup plane
diff --git a/Examples/PBD/PBDFluids/PBDFluidsExample.cpp b/Examples/PBD/PBDFluids/PBDFluidsExample.cpp
index d7eaf3870a28f85c1ca3abb92daf121883e6fbde..4af5e4ce16d5db25090b6df7732351326983afaa 100644
--- a/Examples/PBD/PBDFluids/PBDFluidsExample.cpp
+++ b/Examples/PBD/PBDFluids/PBDFluidsExample.cpp
@@ -19,18 +19,19 @@
=========================================================================*/
-#include "imstkSimulationManager.h"
-#include "imstkCollisionGraph.h"
+#include "imstkAPIUtilities.h"
#include "imstkCamera.h"
+#include "imstkCollisionGraph.h"
#include "imstkLight.h"
#include "imstkMeshIO.h"
-#include "imstkPbdModel.h"
-#include "imstkPbdObject.h"
-#include "imstkAPIUtilities.h"
#include "imstkMeshToMeshBruteForceCD.h"
+#include "imstkObjectInteractionFactory.h"
#include "imstkPBDCollisionHandling.h"
-#include "imstkSurfaceMesh.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
#include "imstkScene.h"
+#include "imstkSimulationManager.h"
+#include "imstkSurfaceMesh.h"
using namespace imstk;
@@ -76,11 +77,10 @@ main()
// Other parameters
pbdParams->m_uniformMassValue = 1.0;
- pbdParams->m_gravity = Vec3d(0, -9.8, 0);
- pbdParams->m_DefaultDt = 0.01;
- pbdParams->m_maxIter = 2;
- pbdParams->m_proximity = 0.01;
- pbdParams->m_contactStiffness = 0.2;
+ pbdParams->m_gravity = Vec3d(0, -9.8, 0);
+ pbdParams->m_defaultDt = 0.01;
+ pbdParams->m_iterations = 2;
+ pbdParams->collisionParams->m_proximity = 0.01;
// Set the parameters
pbdModel->configure(pbdParams);
@@ -212,8 +212,7 @@ main()
// Configure model
auto pbdParams2 = std::make_shared<PBDModelConfig>();
pbdParams2->m_uniformMassValue = 0.0;
- pbdParams2->m_proximity = 0.1;
- pbdParams2->m_contactStiffness = 1.0;
+ pbdParams2->collisionParams->m_proximity = 0.1;
pbdModel2->configure(pbdParams2);
floor->setDynamicalModel(pbdModel2);
@@ -221,11 +220,8 @@ main()
scene->addSceneObject(floor);
// Collisions
- auto colGraph = scene->getCollisionGraph();
- auto CD = std::make_shared<MeshToMeshBruteForceCD>(fluidMesh, floorMeshColliding, nullptr);
- auto CH = std::make_shared<PBDCollisionHandling>(CollisionHandling::Side::A,
- CD->getCollisionData(), deformableObj, floor);
- colGraph->addInteractionPair(deformableObj, floor, CD, CH, nullptr);
+ scene->getCollisionGraph()->addInteraction(makeObjectInteractionPair(deformableObj, floor,
+ InteractionType::PbdObjToPbdObjCollision, CollisionDetection::Type::MeshToMeshBruteForce));
// Light (white)
auto whiteLight = std::make_shared<DirectionalLight>("whiteLight");
diff --git a/Examples/PBD/PBDString/pbdStringExample.cpp b/Examples/PBD/PBDString/pbdStringExample.cpp
index 563eb90797418dbdd834fe2d53af45000ef926c2..6295471f383bbb1f5a123767a8b6cb9a39a93a6f 100644
--- a/Examples/PBD/PBDString/pbdStringExample.cpp
+++ b/Examples/PBD/PBDString/pbdStringExample.cpp
@@ -25,7 +25,6 @@
#include "imstkPbdObject.h"
#include "imstkSimulationManager.h"
#include "imstkSceneManager.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
#include "imstkScene.h"
@@ -92,13 +91,13 @@ main()
// Configure the parameters with bend stiffnesses varying from 0.001 to ~0.1
sims[i].params = std::make_shared<PBDModelConfig>();
- sims[i].params->enableConstraint(PbdConstraint::Type::Distance, 0.001);
- sims[i].params->enableConstraint(PbdConstraint::Type::Bend, static_cast<double>(i) * 0.1 / numStrings + 0.001);
+ sims[i].params->enableConstraint(PbdConstraint::Type::Distance, 1e7);
+ sims[i].params->enableConstraint(PbdConstraint::Type::Bend, (static_cast<double>(i) * 0.1 / numStrings + 0.001) * 1e6);
sims[i].params->m_fixedNodeIds = { 0 };
sims[i].params->m_uniformMassValue = 5.0;
- sims[i].params->m_gravity = Vec3d(0, -9.8, 0);
- sims[i].params->m_DefaultDt = 0.0005;
- sims[i].params->m_maxIter = 5;
+ sims[i].params->m_gravity = Vec3d(0, -9.8, 0);
+ sims[i].params->m_defaultDt = 0.0005;
+ sims[i].params->m_iterations = 5;
// Set the parameters
sims[i].model->configure(sims[i].params);
diff --git a/Examples/ReducedFEM/CMakeLists.txt b/Examples/ReducedFEM/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f7cb1579910ced2759b1fb92c82b46f9a7169bf4
--- /dev/null
+++ b/Examples/ReducedFEM/CMakeLists.txt
@@ -0,0 +1,36 @@
+###########################################################################
+#
+# Copyright (c) Kitware, Inc.
+#
+# 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.txt
+#
+# 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.
+#
+###########################################################################
+
+project(Example-ReducedFEM)
+
+if (${iMSTK_USE_MODEL_REDUCTION})
+ #-----------------------------------------------------------------------------
+ # Create executable
+ #-----------------------------------------------------------------------------
+ imstk_add_executable(${PROJECT_NAME} ReducedFEM.cpp)
+
+ #-----------------------------------------------------------------------------
+ # Add the target to Examples folder
+ #-----------------------------------------------------------------------------
+ SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples)
+
+ #-----------------------------------------------------------------------------
+ # Link libraries to executable
+ #-----------------------------------------------------------------------------
+ target_link_libraries(${PROJECT_NAME} SimulationManager apiUtilities)
+endif()
\ No newline at end of file
diff --git a/Examples/ReducedFEM/ReducedFEM.cpp b/Examples/ReducedFEM/ReducedFEM.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f8c9bfea57234fb6bf4274634e4df150266ec9e
--- /dev/null
+++ b/Examples/ReducedFEM/ReducedFEM.cpp
@@ -0,0 +1,129 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkTimer.h"
+#include "imstkSimulationManager.h"
+#include "imstkDeformableObject.h"
+#include "imstkBackwardEuler.h"
+#include "imstkNonLinearSystem.h"
+#include "imstkNewtonSolver.h"
+#include "imstkGaussSeidel.h"
+#include "imstkPlane.h"
+#include "imstkTetrahedralMesh.h"
+#include "imstkMeshIO.h"
+#include "imstkOneToOneMap.h"
+#include "imstkAPIUtilities.h"
+#include "imstkConjugateGradient.h"
+#include "imstkLight.h"
+#include "imstkCamera.h"
+#include "imstkReducedStVKBodyModel.h"
+#include "imstkReducedFeDeformableObject.h"
+#include "imstkCollisionGraph.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkScene.h"
+
+using namespace imstk;
+
+///
+/// \brief This example demonstrates the soft body simulation
+/// using Finite elements
+///
+int
+main()
+{
+ // simManager and Scene
+ auto simConfig = std::make_shared<SimManagerConfig>();
+ simConfig->simulationMode = SimulationMode::Rendering;
+ auto simManager = std::make_shared<SimulationManager>(simConfig);
+ auto scene = simManager->createNewScene("DeformableBodyFEM");
+ scene->getCamera()->setPosition(0, 2.0, 15.0);
+
+ // Load a tetrahedral mesh
+ // auto tetMesh = MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.veg");
+ auto tetMesh = MeshIO::read(iMSTK_DATA_ROOT "/asianDragon/asianDragon.veg");
+
+ CHECK(tetMesh != nullptr) << "Could not read mesh from file.";
+
+ // Extract the surface mesh
+ auto surfMesh = std::make_shared<SurfaceMesh>();
+ auto volTetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(tetMesh);
+
+ CHECK(volTetMesh != nullptr) << "Dynamic pointer cast from PointSet to TetrahedralMesh failed!";
+
+ volTetMesh->extractSurfaceMesh(surfMesh, true);
+
+ // Construct a map
+
+ // Construct one to one nodal map based on the above meshes
+ auto oneToOneNodalMap = std::make_shared<OneToOneMap>(tetMesh, surfMesh);
+
+ // Scene object 1: Dragon
+
+ // Configure dynamic model
+ auto dynaModel = std::make_shared<ReducedStVK>();
+
+ //dynaModel->configure(iMSTK_DATA_ROOT "/asianDragon/asianDragon.config");
+
+ auto config = std::make_shared<ReducedStVKConfig>();
+ // config->m_fixedNodeIds = { 51, 127, 178 };
+ config->m_cubicPolynomialFilename = iMSTK_DATA_ROOT "/asianDragon/asianDragon.cub";
+ config->m_modesFileName = iMSTK_DATA_ROOT "/asianDragon/asianDragon.URendering.float";
+ dynaModel->configure(config);
+
+ dynaModel->setTimeStepSizeType(TimeSteppingType::Fixed);
+ dynaModel->setModelGeometry(volTetMesh);
+ auto timeIntegrator = std::make_shared<BackwardEuler>(0.01);// Create and add Backward Euler time integrator
+ dynaModel->setTimeIntegrator(timeIntegrator);
+
+ auto material = std::make_shared<RenderMaterial>();
+ material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+ auto surfMeshModel = std::make_shared<VisualModel>(surfMesh);
+ surfMeshModel->setRenderMaterial(material);
+
+ // Scene Object
+ auto deformableObj = std::make_shared<ReducedFeDeformableObject>("Dragon");
+ deformableObj->addVisualModel(surfMeshModel);
+ deformableObj->setPhysicsGeometry(volTetMesh);
+ deformableObj->setPhysicsToVisualMap(oneToOneNodalMap); //assign the computed map
+ deformableObj->setDynamicalModel(dynaModel);
+ scene->addSceneObject(deformableObj);
+
+ // Scene object 2: Plane
+ auto planeGeom = std::make_shared<Plane>();
+ planeGeom->setWidth(40);
+ planeGeom->setPosition(0, -6, 0);
+ auto planeObj = std::make_shared<CollidingObject>("Plane");
+ planeObj->setVisualGeometry(planeGeom);
+ planeObj->setCollidingGeometry(planeGeom);
+ scene->addSceneObject(planeObj);
+
+ // Light
+ auto light = std::make_shared<DirectionalLight>("light");
+ light->setFocalPoint(Vec3d(5, -8, -5));
+ light->setIntensity(1);
+ scene->addLight(light);
+
+ // Run the simulation
+ simManager->setActiveScene(scene);
+ simManager->start(SimulationStatus::Paused);
+
+ return 0;
+}
diff --git a/Examples/Rendering/RenderingExample.cpp b/Examples/Rendering/RenderingExample.cpp
index f2e9d4d41e3b250fe442dd69265a21d8f79988db..736aa9553ac80d89a71faa5c20398929b0ece2e0 100644
--- a/Examples/Rendering/RenderingExample.cpp
+++ b/Examples/Rendering/RenderingExample.cpp
@@ -22,7 +22,6 @@
#include "imstkSimulationManager.h"
#include "imstkAPIUtilities.h"
#include "imstkVisualObjectImporter.h"
-#include "imstkCollisionGraph.h"
#include "imstkIBLProbe.h"
#include "imstkCamera.h"
#include "imstkMeshIO.h"
diff --git a/Examples/SPHFluid/Fluid.hpp b/Examples/SPHFluid/Fluid.hpp
index 7f3c47183426bfd323e4800c4fce09a926f8510e..04a4fc3410b14ddec024020139585343e157281c 100644
--- a/Examples/SPHFluid/Fluid.hpp
+++ b/Examples/SPHFluid/Fluid.hpp
@@ -21,7 +21,6 @@
#include "imstkSimulationManager.h"
#include "imstkSPHObject.h"
-#include "imstkSPHSolver.h"
#include "imstkSPHModel.h"
#include "imstkPointSet.h"
#include "imstkScene.h"
@@ -176,10 +175,5 @@ generateFluid(const std::shared_ptr<Scene>& scene, const double particleRadius)
fluidObj->setPhysicsGeometry(fluidGeometry); // TODO: Look into API duplication and resulting conflicts
scene->addSceneObject(fluidObj);
- // Configure the solver
- auto sphSolver = std::make_shared<SPHSolver>();
- sphSolver->setSPHObject(fluidObj);
- scene->addNonlinearSolver(sphSolver);
-
return fluidObj;
}
diff --git a/Examples/SPHFluid/SPHFluidExample.hpp b/Examples/SPHFluid/SPHFluidExample.hpp
index 8ca080ff2b16c484e1b62982cf2d662ff00cec62..b69c04ea0674eb321ccc70c0225e9fdb8ce97ebf 100644
--- a/Examples/SPHFluid/SPHFluidExample.hpp
+++ b/Examples/SPHFluid/SPHFluidExample.hpp
@@ -31,6 +31,8 @@
#include "imstkCollisionGraph.h"
#include "imstkSceneManager.h"
#include "imstkCamera.h"
+#include "imstkObjectInteractionFactory.h"
+#include "imstkCollisionDetection.h"
#include "Fluid.hpp"
#include "Solid.hpp"
@@ -101,23 +103,18 @@ main(int argc, char* argv[])
});
// Collision between fluid and solid objects
- auto colGraph = scene->getCollisionGraph();
-
+ std::shared_ptr<CollisionGraph> collisionGraph = scene->getCollisionGraph();
for (auto& solid: solids)
{
if (std::dynamic_pointer_cast<Plane>(solid->getCollidingGeometry()))
{
- colGraph->addInteractionPair(fluidObj, solid,
- CollisionDetection::Type::PointSetToPlane,
- CollisionHandling::Type::SPH,
- CollisionHandling::Type::None);
+ collisionGraph->addInteraction(makeObjectInteractionPair(fluidObj, solid,
+ InteractionType::SphObjToCollidingObjCollision, CollisionDetection::Type::PointSetToPlane));
}
else if (std::dynamic_pointer_cast<Sphere>(solid->getCollidingGeometry()))
{
- colGraph->addInteractionPair(fluidObj, solid,
- CollisionDetection::Type::PointSetToSphere,
- CollisionHandling::Type::SPH,
- CollisionHandling::Type::None);
+ collisionGraph->addInteraction(makeObjectInteractionPair(fluidObj, solid,
+ InteractionType::SphObjToCollidingObjCollision, CollisionDetection::Type::PointSetToSphere));
}
else
{
diff --git a/Examples/SceneManagement/SceneManagementExample.cpp b/Examples/SceneManagement/SceneManagementExample.cpp
index 45418a5f9d8cd4ec668ac39e7da2267528f3ed41..f0c4668a32b388d3994f06cae739ec753519ccd0 100644
--- a/Examples/SceneManagement/SceneManagementExample.cpp
+++ b/Examples/SceneManagement/SceneManagementExample.cpp
@@ -19,10 +19,10 @@
=========================================================================*/
-#include "imstkSimulationManager.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
+#include "imstkCollisionGraph.h"
#include "imstkScene.h"
+#include "imstkSimulationManager.h"
using namespace imstk;
diff --git a/Examples/Screenshot/ScreenshotExample.cpp b/Examples/Screenshot/ScreenshotExample.cpp
index b34ce731ddc63cf47514588b4766097ef0c6e3cb..9b88ed1ac828d23e7b2d9f4cca453a3d41cb9f6b 100644
--- a/Examples/Screenshot/ScreenshotExample.cpp
+++ b/Examples/Screenshot/ScreenshotExample.cpp
@@ -21,7 +21,6 @@
#include "imstkSimulationManager.h"
#include "imstkSceneObject.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
#include "imstkLight.h"
#include "imstkCube.h"
diff --git a/Examples/TaskGraph/CMakeLists.txt b/Examples/TaskGraph/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..964a9fe46555e89800ccc1ca30875fc8ac896dab
--- /dev/null
+++ b/Examples/TaskGraph/CMakeLists.txt
@@ -0,0 +1,21 @@
+#-----------------------------------------------------------------------------
+# Add Example subdirectories
+#-----------------------------------------------------------------------------
+macro(listOfSubDir result curdir)
+ file(GLOB children RELATIVE ${curdir} ${curdir}/*)
+ set(dirlist "")
+ foreach(child ${children})
+ if(IS_DIRECTORY ${curdir}/${child})
+ list(APPEND dirlist ${child})
+ endif()
+ endforeach()
+ set(${result} ${dirlist})
+endmacro()
+
+listOfSubDir(subDirs ${CMAKE_CURRENT_SOURCE_DIR})
+
+foreach(subdir ${subDirs})
+ add_subdirectory(${subdir})
+endforeach()
+
+
diff --git a/Examples/TaskGraph/Configuration/CMakeLists.txt b/Examples/TaskGraph/Configuration/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..09512ba38eb3a3f85dd128ed4793b9d215eafa96
--- /dev/null
+++ b/Examples/TaskGraph/Configuration/CMakeLists.txt
@@ -0,0 +1,34 @@
+###########################################################################
+#
+# Copyright (c) Kitware, Inc.
+#
+# 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.txt
+#
+# 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.
+#
+###########################################################################
+
+project(Example-TaskGraphConfigure)
+
+#-----------------------------------------------------------------------------
+# Create executable
+#-----------------------------------------------------------------------------
+imstk_add_executable(${PROJECT_NAME} taskGraphConfigureExample.cpp)
+
+#-----------------------------------------------------------------------------
+# Add the target to Examples folder
+#-----------------------------------------------------------------------------
+SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/TaskGraph)
+
+#-----------------------------------------------------------------------------
+# Link libraries to executable
+#-----------------------------------------------------------------------------
+target_link_libraries(${PROJECT_NAME} SimulationManager apiUtilities)
diff --git a/Examples/TaskGraph/Configuration/taskGraphConfigureExample.cpp b/Examples/TaskGraph/Configuration/taskGraphConfigureExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6cbfc8178d9da8e1d46a25d73a70e60df1bc9230
--- /dev/null
+++ b/Examples/TaskGraph/Configuration/taskGraphConfigureExample.cpp
@@ -0,0 +1,206 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkAPIUtilities.h"
+#include "imstkCamera.h"
+#include "imstkLight.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+#include "imstkScene.h"
+#include "imstkSimulationManager.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkTaskGraph.h"
+#include "imstkTaskGraphVizWriter.h"
+
+using namespace imstk;
+
+static std::unique_ptr<SurfaceMesh>
+makeCloth(
+ const double width, const double height, const int nRows, const int nCols)
+{
+ // Create surface mesh
+ std::unique_ptr<SurfaceMesh> clothMesh = std::make_unique<SurfaceMesh>();
+ StdVectorOfVec3d vertList;
+
+ vertList.resize(nRows * nCols);
+ const double dy = width / (double)(nCols - 1);
+ const double dx = height / (double)(nRows - 1);
+ for (int i = 0; i < nRows; ++i)
+ {
+ for (int j = 0; j < nCols; j++)
+ {
+ vertList[i * nCols + j] = Vec3d((double)dx * i, 1.0, (double)dy * j);
+ }
+ }
+ clothMesh->setInitialVertexPositions(vertList);
+ clothMesh->setVertexPositions(vertList);
+
+ // Add connectivity data
+ std::vector<SurfaceMesh::TriangleArray> triangles;
+ for (std::size_t i = 0; i < nRows - 1; ++i)
+ {
+ for (std::size_t j = 0; j < nCols - 1; j++)
+ {
+ SurfaceMesh::TriangleArray tri[2];
+ const size_t index1 = i * nCols + j;
+ const size_t index2 = index1 + nCols;
+ const size_t index3 = index1 + 1;
+ const size_t index4 = index2 + 1;
+
+ // Interleave [/][\]
+ if (i % 2 ^ j % 2)
+ {
+ tri[0] = { { index1, index2, index3 } };
+ tri[1] = { { index4, index3, index2 } };
+ }
+ else
+ {
+ tri[0] = { { index2, index4, index1 } };
+ tri[1] = { { index4, index3, index1 } };
+ }
+ triangles.push_back(tri[0]);
+ triangles.push_back(tri[1]);
+ }
+ }
+
+ clothMesh->setTrianglesVertices(triangles);
+
+ return clothMesh;
+}
+
+static std::shared_ptr<PbdObject>
+makeClothObj(const std::string& name, double width, double height, int nRows, int nCols)
+{
+ auto clothObj = std::make_shared<PbdObject>(name);
+
+ std::shared_ptr<SurfaceMesh> clothMesh(std::move(makeCloth(width, height, nRows, nCols)));
+
+ // Setup the Parameters
+ auto pbdParams = std::make_shared<PBDModelConfig>();
+ pbdParams->m_uniformMassValue = 1.0;
+ pbdParams->m_gravity = Vec3d(0, -9.8, 0);
+ pbdParams->m_defaultDt = 0.005;
+ pbdParams->m_iterations = 5;
+ pbdParams->m_viscousDampingCoeff = 0.05;
+ pbdParams->enableConstraint(PbdConstraint::Type::Distance, 0.1);
+ pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 0.001);
+ std::vector<size_t> fixedNodes(nCols);
+ for (size_t i = 0; i < fixedNodes.size(); i++)
+ {
+ fixedNodes[i] = i;
+ }
+ pbdParams->m_fixedNodeIds = fixedNodes;
+
+ // Setup the Model
+ auto pbdModel = std::make_shared<PbdModel>();
+ pbdModel->setModelGeometry(clothMesh);
+ pbdModel->configure(pbdParams);
+
+ // Setup the VisualModel
+ auto material = std::make_shared<RenderMaterial>();
+ material->setBackFaceCulling(false);
+ material->setColor(Color::LightGray);
+ material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+
+ auto clothVisualModel = std::make_shared<VisualModel>(clothMesh);
+ clothVisualModel->setRenderMaterial(material);
+
+ // Setup the Object
+ clothObj->addVisualModel(clothVisualModel);
+ clothObj->setPhysicsGeometry(clothMesh);
+ clothObj->setDynamicalModel(pbdModel);
+
+ return clothObj;
+}
+
+///
+/// \brief This example demonstrates how to modify the task graph
+///
+int
+main()
+{
+ auto simManager = std::make_shared<SimulationManager>();
+ auto scene = simManager->createNewScene("PBDCloth");
+
+ const double width = 10.0;
+ const double height = 10.0;
+ const int nRows = 16;
+ const int nCols = 16;
+ std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", width, height, nRows, nCols);
+ scene->addSceneObject(clothObj);
+
+ // Light (white)
+ auto whiteLight = std::make_shared<DirectionalLight>("whiteLight");
+ whiteLight->setFocalPoint(Vec3d(5, -8, -5));
+ whiteLight->setIntensity(7);
+ scene->addLight(whiteLight);
+
+ // Light (red)
+ auto colorLight = std::make_shared<SpotLight>("colorLight");
+ colorLight->setPosition(Vec3d(-5, -3, 5));
+ colorLight->setFocalPoint(Vec3d(0, -5, 5));
+ colorLight->setIntensity(100);
+ colorLight->setColor(Color::Red);
+ colorLight->setSpotAngle(30);
+ scene->addLight(colorLight);
+
+ // Adjust camera
+ scene->getCamera()->setFocalPoint(0, -5, 5);
+ scene->getCamera()->setPosition(-15., -5.0, 15.0);
+
+ // Adds a custom physics step to print out intermediate velocities
+ {
+ std::shared_ptr<PbdModel> pbdModel = clothObj->getPbdModel();
+ scene->setTaskGraphConfigureCallback([&](Scene* scene)
+ {
+ // Get the graph
+ std::shared_ptr<TaskGraph> graph = scene->getTaskGraph();
+
+ // First write the graph before we make modifications, just to show the changes
+ TaskGraphVizWriter writer;
+ writer.setInput(graph);
+ writer.setFileName("taskGraphConfigureExampleOld.svg");
+ writer.write();
+
+ std::shared_ptr<TaskNode> printVelocities = std::make_shared<TaskNode>([&]()
+ {
+ const StdVectorOfVec3d& velocities = *pbdModel->getCurrentState()->getVelocities();
+ for (size_t i = 0; i < velocities.size(); i++)
+ {
+ printf("Velocity: %f, %f, %f\n", velocities[i].x(), velocities[i].y(), velocities[i].z());
+ }
+ }, "PrintVelocities");
+
+ // After IntegratePosition
+ graph->insertAfter(pbdModel->getIntegratePositionNode(), printVelocities);
+
+ // Write the modified graph
+ writer.setFileName("taskGraphConfigureExampleNew.svg");
+ writer.write();
+ });
+ }
+
+ // Start
+ simManager->setActiveScene(scene);
+ simManager->start(SimulationStatus::Paused);
+
+ return 0;
+}
diff --git a/Examples/TaskGraph/TaskGraph/CMakeLists.txt b/Examples/TaskGraph/TaskGraph/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e51e2e76989b6317c48160e5dfc264cd977697b0
--- /dev/null
+++ b/Examples/TaskGraph/TaskGraph/CMakeLists.txt
@@ -0,0 +1,34 @@
+###########################################################################
+#
+# Copyright (c) Kitware, Inc.
+#
+# 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.txt
+#
+# 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.
+#
+###########################################################################
+
+project(Example-TaskGraph)
+
+#-----------------------------------------------------------------------------
+# Create executable
+#-----------------------------------------------------------------------------
+imstk_add_executable(${PROJECT_NAME} taskGraphExample.cpp)
+
+#-----------------------------------------------------------------------------
+# Add the target to Examples folder
+#-----------------------------------------------------------------------------
+SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/TaskGraph)
+
+#-----------------------------------------------------------------------------
+# Link libraries to executable
+#-----------------------------------------------------------------------------
+target_link_libraries(${PROJECT_NAME} SimulationManager apiUtilities)
diff --git a/Examples/TaskGraph/TaskGraph/taskGraphExample.cpp b/Examples/TaskGraph/TaskGraph/taskGraphExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c05cf60888a790e16c442768eef771f22410ca16
--- /dev/null
+++ b/Examples/TaskGraph/TaskGraph/taskGraphExample.cpp
@@ -0,0 +1,107 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkParallelFor.h"
+#include "imstkSequentialTaskGraphController.h"
+#include "imstkTaskGraph.h"
+#include "imstkTbbTaskGraphController.h"
+#include <iostream>
+
+using namespace imstk;
+
+///
+/// \brief This example how to use a TaskGraph standalone
+///
+int
+main()
+{
+ const bool runExampleInParallel = true;
+
+ // Initialize some data arrays
+ const int countA = 100;
+ const int countB = 255;
+ int x[countA];
+ int y[countA];
+ int w[countB];
+ int z[countB];
+
+ for (int i = 0; i < 100; i++)
+ {
+ x[i] = i + 5;
+ y[i] = i * 6 + 1;
+ }
+ for (int i = 0; i < 255; i++)
+ {
+ w[i] = i % 10;
+ z[i] = i;
+ }
+
+ int sumA = 0;
+ int sumB = 0;
+
+ // Now create a graph that computes results = sum_i(x_i + y_i) * sum_i(w_i * z_i)
+ std::shared_ptr<TaskGraph> graph = std::make_shared<TaskGraph>();
+
+ // Create and add the nodes
+ std::shared_ptr<TaskNode> addNode = graph->addFunction("Add Step", [&]()
+ {
+ for (int i = 0; i < countA; i++)
+ {
+ sumA += (x[i] + y[i]);
+ }
+ });
+ std::shared_ptr<TaskNode> multNode = graph->addFunction("Mult Step", [&]()
+ {
+ for (int i = 0; i < countB; i++)
+ {
+ sumB += (w[i] * z[i]);
+ }
+ });
+
+ // Define the edges, add and mult steps will be done in parallel
+ graph->addEdge(graph->getSource(), addNode);
+ graph->addEdge(graph->getSource(), multNode);
+ graph->addEdge(addNode, graph->getSink());
+ graph->addEdge(multNode, graph->getSink());
+
+ // Set which controller to use
+ std::shared_ptr<TaskGraphController> controller = nullptr;
+ if (runExampleInParallel)
+ {
+ controller = std::make_shared<TbbTaskGraphController>();
+ }
+ else
+ {
+ controller = std::make_shared<SequentialTaskGraphController>();
+ }
+
+ // Compute
+ controller->setTaskGraph(graph);
+ if (!controller->initialize())
+ {
+ std::cout << "TaskGraph failed to initialize" << std::endl;
+ return 1;
+ }
+ controller->execute();
+ std::cout << "Results: " << sumA + sumB << std::endl;
+
+ return 0;
+}
diff --git a/Examples/TaskGraph/Timing/CMakeLists.txt b/Examples/TaskGraph/Timing/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ea4991855fcfe14cb594f62e1370b62de19b2eff
--- /dev/null
+++ b/Examples/TaskGraph/Timing/CMakeLists.txt
@@ -0,0 +1,34 @@
+###########################################################################
+#
+# Copyright (c) Kitware, Inc.
+#
+# 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.txt
+#
+# 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.
+#
+###########################################################################
+
+project(Example-TaskGraphTiming)
+
+#-----------------------------------------------------------------------------
+# Create executable
+#-----------------------------------------------------------------------------
+imstk_add_executable(${PROJECT_NAME} taskGraphTimingExample.cpp)
+
+#-----------------------------------------------------------------------------
+# Add the target to Examples folder
+#-----------------------------------------------------------------------------
+SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/TaskGraph)
+
+#-----------------------------------------------------------------------------
+# Link libraries to executable
+#-----------------------------------------------------------------------------
+target_link_libraries(${PROJECT_NAME} SimulationManager apiUtilities)
diff --git a/Examples/TaskGraph/Timing/taskGraphTimingExample.cpp b/Examples/TaskGraph/Timing/taskGraphTimingExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b9f7862660cb6417b2f594442b9f75020a27b88a
--- /dev/null
+++ b/Examples/TaskGraph/Timing/taskGraphTimingExample.cpp
@@ -0,0 +1,160 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#include "imstkAPIUtilities.h"
+#include "imstkCamera.h"
+#include "imstkLineMesh.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+#include "imstkScene.h"
+#include "imstkSceneManager.h"
+#include "imstkSimulationManager.h"
+#include "imstkTaskGraphVizWriter.h"
+
+using namespace imstk;
+
+///
+/// \brief This examples uses the timing features of the task graph This allows one
+/// to see the elapsed time of every step of iMSTK as well as export the computational
+/// graph and show information such as the critical path
+///
+int
+main()
+{
+ auto simManager = std::make_shared<SimulationManager>();
+ auto scene = simManager->createNewScene("PBDString");
+ scene->getConfig()->taskTimingEnabled = true;
+
+ // Setup N separate string simulations with varying bend stiffnesses
+ const unsigned int numStrings = 8;
+ const unsigned int numVerts = 30;
+ const double stringSpacing = 2.0; // How far each string is apart
+ const double stringLength = 10.0; // Total length of string
+ const Color startColor = Color::Red; // Color of first string
+ const Color endColor = Color::Green; // Color of last string
+
+ struct PbdSim
+ {
+ std::shared_ptr<LineMesh> geometry;
+ std::shared_ptr<PbdObject> object;
+ std::shared_ptr<PbdModel> model;
+ std::shared_ptr<PBDModelConfig> params;
+ std::shared_ptr<VisualModel> visualModel;
+ };
+ std::vector<PbdSim> sims(numStrings);
+
+ const double size = stringSpacing * (numStrings - 1);
+ const double vertexSpacing = stringLength / numVerts;
+ for (unsigned int i = 0; i < numStrings; i++)
+ {
+ // Setup the line mesh
+ sims[i].geometry = std::make_shared<LineMesh>();
+ StdVectorOfVec3d vertList;
+ vertList.resize(numVerts);
+ for (size_t j = 0; j < numVerts; j++)
+ {
+ vertList[j] = Vec3d(
+ static_cast<double>(i) * stringSpacing - size * 0.5,
+ stringLength * 0.5 - static_cast<double>(j) * vertexSpacing, 0.0);
+ }
+ sims[i].geometry->setInitialVertexPositions(vertList);
+ sims[i].geometry->setVertexPositions(vertList);
+
+ // Add connectivity data
+ std::vector<LineMesh::LineArray> segments;
+ for (size_t j = 0; j < numVerts - 1; j++)
+ {
+ LineMesh::LineArray seg = { j, j + 1 };
+ segments.push_back(seg);
+ }
+
+ sims[i].geometry->setLinesVertices(segments);
+
+ sims[i].object = std::make_shared<PbdObject>("String " + std::to_string(i));
+ sims[i].model = std::make_shared<PbdModel>();
+ sims[i].model->setModelGeometry(sims[i].geometry);
+
+ // Configure the parameters with bend stiffnesses varying from 0.001 to ~0.1
+ sims[i].params = std::make_shared<PBDModelConfig>();
+ sims[i].params->enableConstraint(PbdConstraint::Type::Distance, 0.001);
+ sims[i].params->enableConstraint(PbdConstraint::Type::Bend, static_cast<double>(i) * 0.1 / numStrings + 0.001);
+ sims[i].params->m_fixedNodeIds = { 0 };
+ sims[i].params->m_uniformMassValue = 5.0;
+ sims[i].params->m_gravity = Vec3d(0, -9.8, 0);
+ sims[i].params->m_defaultDt = 0.0005;
+ sims[i].params->m_iterations = 5;
+
+ // Set the parameters
+ sims[i].model->configure(sims[i].params);
+ sims[i].object->setDynamicalModel(sims[i].model);
+ sims[i].object->setPhysicsGeometry(sims[i].geometry);
+
+ sims[i].visualModel = std::make_shared<VisualModel>(sims[i].geometry);
+ std::shared_ptr<RenderMaterial> material = std::make_shared<RenderMaterial>();
+ material->setDisplayMode(RenderMaterial::DisplayMode::Wireframe);
+ material->setDebugColor(Color::lerpRgb(startColor, endColor, static_cast<double>(i) / (numStrings - 1)));
+ material->setLineWidth(2.0f);
+ sims[i].visualModel->setRenderMaterial(material);
+ sims[i].object->addVisualModel(sims[i].visualModel);
+
+ // Add in scene
+ scene->addSceneObject(sims[i].object);
+ }
+
+ // Adjust the camera
+ scene->getCamera()->setFocalPoint(0.0, 0.0, 0.0);
+ scene->getCamera()->setPosition(0.0, 0.0, 15.0);
+
+ // Move the points every frame
+ double t = 0.0;
+ const double dt = 0.0005;
+ const double radius = 1.5;
+ auto movePoints =
+ [&sims, &t, dt, radius](Module* module)
+ {
+ for (unsigned int i = 0; i < sims.size(); i++)
+ {
+ Vec3d pos = sims[i].model->getCurrentState()->getVertexPosition(0);
+ // Move in circle, derivatives of parametric eq of circle
+ sims[i].model->getCurrentState()->setVertexPosition(0, imstk::Vec3d(
+ pos.x() + -std::sin(t) * radius * dt,
+ pos.y(),
+ pos.z() + std::cos(t) * radius * dt));
+ }
+ t += dt;
+ };
+ simManager->getSceneManager(scene)->setPostUpdateCallback(movePoints);
+
+ // Start
+ simManager->setActiveScene(scene);
+ simManager->start();
+
+ // Write the graph, highlighting the critical path and putting the completion time in the name
+ TaskGraphVizWriter writer;
+ writer.setInput(scene->getTaskGraph());
+ writer.setFileName("taskGraphBenchmarkExample.svg");
+ writer.setHighlightCriticalPath(true);
+ writer.setWriteNodeComputeTimesColor(true);
+ writer.setWriteNodeComputeTimesText(true);
+ writer.write();
+
+ return 0;
+}
diff --git a/Examples/VirtualCoupling/VirtualCouplingExample.cpp b/Examples/VirtualCoupling/VirtualCouplingExample.cpp
index cce2bcbab236876fa738c15987234067b53a59e6..a59c63db9860f6db1cfe48c6c89c9416a80b61ee 100644
--- a/Examples/VirtualCoupling/VirtualCouplingExample.cpp
+++ b/Examples/VirtualCoupling/VirtualCouplingExample.cpp
@@ -94,17 +94,20 @@ main()
auto objController = std::make_shared<SceneObjectController>(obj, deviceTracker);
scene->addObjectController(objController);
- // Create a collision graph
- auto graph = scene->getCollisionGraph();
- auto pair = graph->addInteractionPair(planeObj, obj,
- CollisionDetection::Type::UnidirectionalPlaneToSphere,
- CollisionHandling::Type::None,
- CollisionHandling::Type::VirtualCoupling);
-
- // Customize collision handling algorithm
- auto colHandlingAlgo = std::dynamic_pointer_cast<VirtualCouplingCH>(pair->getCollisionHandlingB());
- colHandlingAlgo->setStiffness(5e-01);
- colHandlingAlgo->setDamping(0.005);
+ {
+ // Setup CD, and collision data
+ auto colData = std::make_shared<CollisionData>();
+ std::shared_ptr<CollisionDetection> colDetect = makeCollisionDetectionObject(CollisionDetection::Type::UnidirectionalPlaneToSphere,
+ planeObj, obj, colData);
+
+ // Setup the handler
+ auto colHandler = std::make_shared<VirtualCouplingCH>(CollisionHandling::Side::B, colData, planeObj, obj);
+ colHandler->setStiffness(5e-01);
+ colHandler->setDamping(0.005);
+
+ auto pair = std::make_shared<CollisionPair>(planeObj, obj, colDetect, nullptr, colHandler);
+ scene->getCollisionGraph()->addInteractionPair(pair);
+ }
// Camera
auto cam = scene->getCamera();
diff --git a/Examples/VolumeRendering/VolumeRenderingExample.cpp b/Examples/VolumeRendering/VolumeRenderingExample.cpp
index f5d5dff507d36730c65873e7fdc5b5ec83dd89a8..97e96ed9d5527c3da31378a4784be155fa2cfc97 100644
--- a/Examples/VolumeRendering/VolumeRenderingExample.cpp
+++ b/Examples/VolumeRendering/VolumeRenderingExample.cpp
@@ -28,7 +28,6 @@
#include "imstkVolumeRenderMaterialPresets.h"
#include "imstkVTKTextStatusManager.h"
#include "imstkSceneManager.h"
-#include "imstkCollisionGraph.h"
#include "imstkScene.h"
#include "imstkVTKRenderer.h"
diff --git a/Source/CollisionDetection/CollisionDetection/imstkBidirectionalPlaneToSphereCD.h b/Source/CollisionDetection/CollisionDetection/imstkBidirectionalPlaneToSphereCD.h
index 3c081937650d6a0e26c37970575d7112e5c40b11..7a8460f22a83b7bf7fe71617de3db8f68d09b39c 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkBidirectionalPlaneToSphereCD.h
+++ b/Source/CollisionDetection/CollisionDetection/imstkBidirectionalPlaneToSphereCD.h
@@ -21,7 +21,6 @@
#pragma once
-#include <memory>
#include "imstkCollisionDetection.h"
namespace imstk
diff --git a/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.cpp b/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.cpp
index 231430dddb4aaf9ce99a7f82c03c134254bc4a39..f6999298191a1c413775e78ab8eb9f8504a16493 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.cpp
+++ b/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.cpp
@@ -21,15 +21,16 @@
#include "imstkCollisionDetection.h"
#include "imstkCollisionData.h"
+#include "imstkTaskNode.h"
+#include "imstkLogger.h"
#include "imstkOctreeBasedCD.h"
#include "imstkSurfaceMesh.h"
-#include <g3log/g3log.hpp>
-
namespace imstk
{
CollisionDetection::CollisionDetection(const CollisionDetection::Type& type, std::shared_ptr<CollisionData> colData) : m_type(type),
- m_colData((colData == nullptr) ? std::make_shared<CollisionData>() : colData)
+ m_colData((colData == nullptr) ? std::make_shared<CollisionData>() : colData),
+ m_taskNode(std::make_shared<TaskNode>(std::bind(&CollisionDetection::computeCollisionData, this), "CollisionDetection"))
{
}
diff --git a/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.h b/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.h
index ed8c7bcde19590d53231893748fcfa321e057731..a1be7c2f1761c7a040545f7c88f63c60ec538426 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.h
+++ b/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.h
@@ -25,10 +25,12 @@
namespace imstk
{
+struct CollisionData;
+
class CollidingObject;
-class OctreeBasedCD;
class Geometry;
-struct CollisionData;
+class OctreeBasedCD;
+class TaskNode;
///
/// \class CollisionDetection
@@ -63,6 +65,9 @@ public:
SphereToCylinder,
SphereToSphere,
+ // Image based CD
+ SignedDistanceField,
+
Custom
};
@@ -92,6 +97,11 @@ public:
///
const std::shared_ptr<CollisionData> getCollisionData() const { return m_colData; }
+ ///
+ /// \brief Returns computational node
+ ///
+ std::shared_ptr<TaskNode> getTaskNode() const { return m_taskNode; }
+
///
/// \brief Update the intrernal octree, preparing for collision detection
///
@@ -112,8 +122,9 @@ public:
const std::shared_ptr<CollisionData>& collisionData);
protected:
- Type m_type = Type::Custom; ///< Collision detection algorithm type
- std::shared_ptr<CollisionData> m_colData; ///< Collision data
+ Type m_type = Type::Custom; ///> Collision detection algorithm type
+ std::shared_ptr<CollisionData> m_colData = nullptr; ///> Collision data
+ std::shared_ptr<TaskNode> m_taskNode = nullptr; ///> Computational node to execute the detection
/// Static octree for collision detection
/// This octree is valid throughout the lifetime of the program
diff --git a/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.cpp b/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.cpp
index aa5ab7477f14c03434be2256290f414f4294630e..49ed12f5936fbb85171110419d46c7802de0d221 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.cpp
+++ b/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.cpp
@@ -35,9 +35,6 @@ SurfaceMeshToSurfaceMeshCCD::SurfaceMeshToSurfaceMeshCCD(std::shared_ptr<Surface
m_modelA(std::make_shared<DeformModel>(meshA->getVertexPositions(), meshA->getTrianglesVertices())),
m_modelB(std::make_shared<DeformModel>(meshB->getVertexPositions(), meshB->getTrianglesVertices()))
{
- //m_modelA = std::make_shared<DeformModel>(meshA->getVertexPositions(), meshA->getTrianglesVertices());
- //m_modelB = std::make_shared<DeformModel>(meshB->getVertexPositions(), meshB->getTrianglesVertices());
-
// Setup Callbacks
m_modelA->SetEECallBack(SurfaceMeshToSurfaceMeshCCD::EECallback, this);
m_modelA->SetVFCallBack(SurfaceMeshToSurfaceMeshCCD::VFCallbackA, this);
diff --git a/Source/Scene/imstkCDObjectFactory.h b/Source/CollisionDetection/imstkCDObjectFactory.cpp
similarity index 53%
rename from Source/Scene/imstkCDObjectFactory.h
rename to Source/CollisionDetection/imstkCDObjectFactory.cpp
index 58cd39ec7e5ca13dfea5570a5e78cd16b556ad97..668990d220005b1e0974f328ba32820441ed2c1e 100644
--- a/Source/Scene/imstkCDObjectFactory.h
+++ b/Source/CollisionDetection/imstkCDObjectFactory.cpp
@@ -1,54 +1,53 @@
/*=========================================================================
- Library: iMSTK
+Library: iMSTK
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & Imaging in Medicine, Rensselaer Polytechnic Institute.
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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
+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.txt
+http://www.apache.org/licenses/LICENSE-2.0.txt
- 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.
+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.
=========================================================================*/
-#pragma once
-
-#include "imstkCollisionDetection.h"
+#include "imstkCDObjectFactory.h"
#include "imstkCollisionData.h"
#include "imstkOctreeBasedCD.h"
-#include "imstkCollidingObject.h"
// Points to objects
#include "imstkPointSetToCapsuleCD.h"
-#include "imstkPointSetToSphereCD.h"
#include "imstkPointSetToPlaneCD.h"
-#include "imstkPointSetToSurfaceMeshCD.h"
+#include "imstkPointSetToSphereCD.h"
#include "imstkPointSetToSpherePickingCD.h"
+#include "imstkPointSetToSurfaceMeshCD.h"
// Mesh to mesh
-#include "imstkSurfaceMeshToSurfaceMeshCD.h"
-#include "imstkSurfaceMeshToSurfaceMeshCCD.h"
#include "imstkMeshToMeshBruteForceCD.h"
+#include "imstkSurfaceMeshToSurfaceMeshCCD.h"
+#include "imstkSurfaceMeshToSurfaceMeshCD.h"
#include "imstkTetraToTetraCD.h"
// Analytical object to analytical object
-#include "imstkUnidirectionalPlaneToSphereCD.h"
#include "imstkBidirectionalPlaneToSphereCD.h"
#include "imstkSphereToCylinderCD.h"
#include "imstkSphereToSphereCD.h"
+#include "imstkUnidirectionalPlaneToSphereCD.h"
+// Geometry
+#include "imstkCapsule.h"
+#include "imstkCylinder.h"
+#include "imstkImageData.h"
#include "imstkPlane.h"
#include "imstkSphere.h"
-#include "imstkCylinder.h"
-#include "imstkCapsule.h"
#include "imstkSurfaceMesh.h"
#include "imstkTetrahedralMesh.h"
@@ -60,40 +59,33 @@
namespace imstk
{
-///
-/// \brief Static factory for collision detection sub classes
-/// If the collision pair is PointSet to SurfaceMesh, or SurfaceMesh to SurfaceMesh,
-/// it will be added to an internal static octree for detecting collision
-/// \todo Other collision pair may be considered to use octree too
-///
-static
std::shared_ptr<CollisionDetection>
-makeCollisionDetectionObject(const CollisionDetection::Type type,
- std::shared_ptr<CollidingObject> objA,
- std::shared_ptr<CollidingObject> objB,
- std::shared_ptr<CollisionData> colData)
+makeCollisionDetectionObject(const CollisionDetection::Type type,
+ std::shared_ptr<Geometry> collidingGeometryA,
+ std::shared_ptr<Geometry> collidingGeometryB,
+ std::shared_ptr<CollisionData> colData)
{
switch (type)
{
// Points to objects
case CollisionDetection::Type::PointSetToSphere:
{
- auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
- auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
+ auto pointset = std::dynamic_pointer_cast<PointSet>(collidingGeometryA);
+ auto sphere = std::dynamic_pointer_cast<Sphere>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, sphere)
return std::make_shared<PointSetToSphereCD>(pointset, sphere, colData);
}
case CollisionDetection::Type::PointSetToPlane:
{
- auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
- auto plane = std::dynamic_pointer_cast<Plane>(objB->getCollidingGeometry());
+ auto pointset = std::dynamic_pointer_cast<PointSet>(collidingGeometryA);
+ auto plane = std::dynamic_pointer_cast<Plane>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, plane)
return std::make_shared<PointSetToPlaneCD>(pointset, plane, colData);
}
case CollisionDetection::Type::PointSetToCapsule:
{
- auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
- auto capsule = std::dynamic_pointer_cast<Capsule>(objB->getCollidingGeometry());
+ auto pointset = std::dynamic_pointer_cast<PointSet>(collidingGeometryA);
+ auto capsule = std::dynamic_pointer_cast<Capsule>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, capsule)
return std::make_shared<PointSetToCapsuleCD>(pointset, capsule, colData);
}
@@ -108,37 +100,33 @@ makeCollisionDetectionObject(const CollisionDetection::Type type,
#endif
case CollisionDetection::Type::PointSetToSurfaceMesh:
{
- const auto& geomA = objA->getCollidingGeometry();
- const auto& geomB = objB->getCollidingGeometry();
- auto pointset = std::dynamic_pointer_cast<PointSet>(geomA);
- auto triMesh = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
+ auto pointset = std::dynamic_pointer_cast<PointSet>(collidingGeometryA);
+ auto triMesh = std::dynamic_pointer_cast<SurfaceMesh>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, triMesh)
- CollisionDetection::addCollisionPairToOctree(geomA, geomB, type, colData);
+ CollisionDetection::addCollisionPairToOctree(collidingGeometryA, collidingGeometryB, type, colData);
return std::make_shared<PointSetToSurfaceMeshCD>(pointset, triMesh, colData);
}
// Mesh to mesh
case CollisionDetection::Type::SurfaceMeshToSurfaceMesh:
{
- const auto& geomA = objA->getCollidingGeometry();
- const auto& geomB = objB->getCollidingGeometry();
- auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(geomA);
- auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
+ auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(collidingGeometryA);
+ auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(meshA, meshB)
- CollisionDetection::addCollisionPairToOctree(geomA, geomB, type, colData);
+ CollisionDetection::addCollisionPairToOctree(collidingGeometryA, collidingGeometryB, type, colData);
return std::make_shared<SurfaceMeshToSurfaceMeshCD>(meshA, meshB, colData);
}
case CollisionDetection::Type::SurfaceMeshToSurfaceMeshCCD:
{
- auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
- auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
+ auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(collidingGeometryA);
+ auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(meshA, meshB)
return std::make_shared<SurfaceMeshToSurfaceMeshCCD>(meshA, meshB, colData);
}
case CollisionDetection::Type::VolumeMeshToVolumeMesh:
{
- auto tet1 = std::dynamic_pointer_cast<TetrahedralMesh>(objA->getCollidingGeometry());
- auto tet2 = std::dynamic_pointer_cast<TetrahedralMesh>(objB->getCollidingGeometry());
+ auto tet1 = std::dynamic_pointer_cast<TetrahedralMesh>(collidingGeometryA);
+ auto tet2 = std::dynamic_pointer_cast<TetrahedralMesh>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(tet1, tet2)
return std::make_shared<TetraToTetraCD>(tet1, tet2, colData);
}
@@ -146,38 +134,37 @@ makeCollisionDetectionObject(const CollisionDetection::Type type,
// Analytical object to analytical object
case CollisionDetection::Type::UnidirectionalPlaneToSphere:
{
- auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
- auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
+ auto plane = std::dynamic_pointer_cast<Plane>(collidingGeometryA);
+ auto sphere = std::dynamic_pointer_cast<Sphere>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(plane, sphere)
return std::make_shared<UnidirectionalPlaneToSphereCD>(plane, sphere, colData);
}
case CollisionDetection::Type::BidirectionalPlaneToSphere:
{
- auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
- auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
+ auto plane = std::dynamic_pointer_cast<Plane>(collidingGeometryA);
+ auto sphere = std::dynamic_pointer_cast<Sphere>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(plane, sphere)
return std::make_shared<BidirectionalPlaneToSphereCD>(plane, sphere, colData);
}
case CollisionDetection::Type::SphereToSphere:
{
- auto sphereA = std::dynamic_pointer_cast<Sphere>(objA->getCollidingGeometry());
- auto sphereB = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
+ auto sphereA = std::dynamic_pointer_cast<Sphere>(collidingGeometryA);
+ auto sphereB = std::dynamic_pointer_cast<Sphere>(collidingGeometryB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(sphereA, sphereB)
return std::make_shared<SphereToSphereCD>(sphereA, sphereB, colData);
}
case CollisionDetection::Type::SphereToCylinder:
{
- auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
- auto cylinder = std::dynamic_pointer_cast<Cylinder>(objA->getCollidingGeometry());
+ auto sphere = std::dynamic_pointer_cast<Sphere>(collidingGeometryB);
+ auto cylinder = std::dynamic_pointer_cast<Cylinder>(collidingGeometryA);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(sphere, cylinder)
return std::make_shared<SphereToCylinderCD>(sphere, cylinder, colData);
}
case CollisionDetection::Type::MeshToMeshBruteForce:
{
- auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
- auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
- IMSTK_CHECK_FOR_VALID_GEOMETRIES(meshA, meshB)
- return std::make_shared<MeshToMeshBruteForceCD>(meshA, meshB, colData);
+ auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(collidingGeometryB);
+ IMSTK_CHECK_FOR_VALID_GEOMETRIES(collidingGeometryA, meshB)
+ return std::make_shared<MeshToMeshBruteForceCD>(collidingGeometryA, meshB, colData);
}
default:
{
@@ -186,4 +173,4 @@ makeCollisionDetectionObject(const CollisionDetection::Type type,
}
}
}
-}
+}
\ No newline at end of file
diff --git a/Source/CollisionDetection/imstkCDObjectFactory.h b/Source/CollisionDetection/imstkCDObjectFactory.h
new file mode 100644
index 0000000000000000000000000000000000000000..1f27972361706d8a9771a9a721057d8c7cabc54c
--- /dev/null
+++ b/Source/CollisionDetection/imstkCDObjectFactory.h
@@ -0,0 +1,40 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkCollisionDetection.h"
+
+namespace imstk
+{
+class Geometry;
+
+///
+/// \brief Static factory for collision detection sub classes
+/// If the collision pair is PointSet to SurfaceMesh, or SurfaceMesh to SurfaceMesh,
+/// it will be added to an internal static octree for detecting collision
+/// \todo Other collision pair may be considered to use octree too
+///
+extern std::shared_ptr<CollisionDetection> makeCollisionDetectionObject(const CollisionDetection::Type type,
+ std::shared_ptr<Geometry> collidingGeometryA,
+ std::shared_ptr<Geometry> collidingGeometryB,
+ std::shared_ptr<CollisionData> colData);
+}
diff --git a/Source/CollisionHandling/imstkBoneDrillingCH.cpp b/Source/CollisionHandling/imstkBoneDrillingCH.cpp
index 23578cc8e044cba3503441cedd47f2ea1b279762..04877d74a5ea42d9ad926e97c0b71391ec0281ab 100644
--- a/Source/CollisionHandling/imstkBoneDrillingCH.cpp
+++ b/Source/CollisionHandling/imstkBoneDrillingCH.cpp
@@ -21,13 +21,11 @@
#include "imstkBoneDrillingCH.h"
#include "imstkCollidingObject.h"
-#include "imstkTetrahedralMesh.h"
#include "imstkCollisionData.h"
#include "imstkDeviceTracker.h"
-#include "imstkMath.h"
+#include "imstkLogger.h"
#include "imstkParallelUtils.h"
-
-#include <g3log/g3log.hpp>
+#include "imstkTetrahedralMesh.h"
namespace imstk
{
diff --git a/Source/CollisionHandling/imstkBoneDrillingCH.h b/Source/CollisionHandling/imstkBoneDrillingCH.h
index 9f1719cd985beff64eae2f50ce9986738abffeb8..25bfe35077f74ee10a35943d019d78d16560c8fe 100644
--- a/Source/CollisionHandling/imstkBoneDrillingCH.h
+++ b/Source/CollisionHandling/imstkBoneDrillingCH.h
@@ -23,6 +23,7 @@
// imstk
#include "imstkCollisionHandling.h"
+#include "imstkMath.h"
namespace imstk
{
diff --git a/Source/CollisionHandling/imstkCollisionHandling.cpp b/Source/CollisionHandling/imstkCollisionHandling.cpp
index 1fd29c45812fbc950261ce2f82485721d68127e0..2cb17bd165141f8e23e9208a8fbde67d925ece60 100644
--- a/Source/CollisionHandling/imstkCollisionHandling.cpp
+++ b/Source/CollisionHandling/imstkCollisionHandling.cpp
@@ -1,82 +1,33 @@
/*=========================================================================
- Library: iMSTK
+Library: iMSTK
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & Imaging in Medicine, Rensselaer Polytechnic Institute.
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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
+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.txt
+http://www.apache.org/licenses/LICENSE-2.0.txt
- 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.
+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.
=========================================================================*/
#include "imstkCollisionHandling.h"
-
-#include "imstkPenaltyCH.h"
-#include "imstkVirtualCouplingCH.h"
-#include "imstkPickingCH.h"
-#include "imstkDeformableObject.h"
-#include "imstkBoneDrillingCH.h"
-#include "imstkSPHCollisionHandling.h"
-#include "imstkPBDCollisionHandling.h"
-#include "imstkCollisionData.h"
-
-#include <g3log/g3log.hpp>
+#include "imstkTaskNode.h"
namespace imstk
{
-std::shared_ptr<CollisionHandling>
-CollisionHandling::make_collision_handling(const Type& type,
- const Side& side,
- const std::shared_ptr<CollisionData> colData,
- std::shared_ptr<CollidingObject> objA,
- std::shared_ptr<CollidingObject> objB)
+CollisionHandling::CollisionHandling(const Type& type, const Side& side,
+ const std::shared_ptr<CollisionData> colData) :
+ m_type(type), m_side(side), m_colData(colData),
+ m_taskNode(std::make_shared<TaskNode>(std::bind(&CollisionHandling::processCollisionData, this), "CollisionHandling", true))
{
- if (type != Type::None
- && objA->getType() == SceneObject::Type::Visual)
- {
- LOG(FATAL) << "CollisionHandling::make_collision_handling error: "
- << "collision handling only implemented for colliding objects.";
- return nullptr;
- }
-
- switch (type)
- {
- case Type::None:
- return nullptr;
-
- case Type::Penalty:
-
- return std::make_shared<PenaltyCH>(side, colData, objA);
-
- case Type::VirtualCoupling:
-
- return std::make_shared<VirtualCouplingCH>(side, colData, objA);
-
- case Type::NodalPicking:
- return std::make_shared<PickingCH>(side, colData, objA);
-
- case Type::BoneDrilling:
- return std::make_shared<BoneDrillingCH>(side, colData, objA, objB);
-
- case Type::PBD:
- return std::make_shared<PBDCollisionHandling>(side, colData, objA, objB);
-
- case Type::SPH:
- return std::make_shared<SPHCollisionHandling>(side, colData, objA);
-
- default:
- LOG(FATAL) << "CollisionHandling::make_collision_handling error: type not implemented.";
- return nullptr;
- }
-}
}
+}
\ No newline at end of file
diff --git a/Source/CollisionHandling/imstkCollisionHandling.h b/Source/CollisionHandling/imstkCollisionHandling.h
index 15aba621f4ef2df47931d51a5e9d82988e719cc2..ea9e05a2d77e4617a022d7ab855cb7018585c0e0 100644
--- a/Source/CollisionHandling/imstkCollisionHandling.h
+++ b/Source/CollisionHandling/imstkCollisionHandling.h
@@ -21,14 +21,15 @@
#pragma once
-// imstk
-#include "imstkCollidingObject.h"
+#include <memory>
namespace imstk
{
-class InteractionPair;
struct CollisionData;
-class PbdObject;
+
+class CollidingObject;
+class InteractionPair;
+class TaskNode;
///
/// \class CollisionHandling
@@ -62,32 +63,13 @@ public:
AB
};
- ///
- /// \brief Static factory for collision handling sub classes
- ///
- static std::shared_ptr<CollisionHandling> make_collision_handling(
- const Type& type,
- const Side& side,
- const std::shared_ptr<CollisionData> colData,
- std::shared_ptr<CollidingObject> objA,
- std::shared_ptr<CollidingObject> objB = nullptr);
-
- ///
- /// \brief Constructor
- ///
- CollisionHandling(const Type& type,
- const Side& side,
- const std::shared_ptr<CollisionData> colData) :
- m_type(type),
- m_side(side),
- m_colData(colData) {}
+public:
+ CollisionHandling(const Type& type, const Side& side, const std::shared_ptr<CollisionData> colData);
CollisionHandling() = delete;
- ///
- /// \brief Destructor
- ///
virtual ~CollisionHandling() = default;
+public:
///
/// \brief Compute forces based on collision data (pure virtual)
///
@@ -96,12 +78,15 @@ public:
///
/// \brief Returns collision handling type
///
- const Type& getType() const { return m_type; };
+ const Type& getType() const { return m_type; }
+ const Side& getSide() const { return m_side; }
-protected:
+ std::shared_ptr<TaskNode> getTaskNode() const { return m_taskNode; }
- Type m_type; ///< Collision handling algorithm type
- Side m_side; ///< Direction of the collisionData
- const std::shared_ptr<CollisionData> m_colData; ///< Collision data
+protected:
+ Type m_type; ///< Collision handling algorithm type
+ Side m_side; ///< Direction of the collisionData
+ const std::shared_ptr<CollisionData> m_colData = nullptr; ///< Collision data
+ std::shared_ptr<TaskNode> m_taskNode = nullptr;
};
}
diff --git a/Source/CollisionHandling/imstkPBDCollisionHandling.cpp b/Source/CollisionHandling/imstkPBDCollisionHandling.cpp
index 42bec2550ad03d92b7549bc822661ea4efe46d98..31a2f57a37f9ffdf83f10da556ed648caf9bcfa6 100644
--- a/Source/CollisionHandling/imstkPBDCollisionHandling.cpp
+++ b/Source/CollisionHandling/imstkPBDCollisionHandling.cpp
@@ -21,28 +21,24 @@
#include "imstkPBDCollisionHandling.h"
#include "imstkCollisionData.h"
-#include "imstkDeformableObject.h"
+#include "imstkGeometryMap.h"
+#include "imstkPbdEdgeEdgeCollisionConstraint.h"
#include "imstkPbdModel.h"
#include "imstkPbdObject.h"
-#include "imstkPbdEdgeEdgeCollisionConstraint.h"
#include "imstkPbdPointTriCollisionConstraint.h"
#include "imstkPbdSolver.h"
-#include "imstkParallelUtils.h"
-#include "imstkGeometryMap.h"
#include "imstkSurfaceMesh.h"
-#include <memory>
-#include <g3log/g3log.hpp>
-
namespace imstk
{
PBDCollisionHandling::PBDCollisionHandling(const Side& side,
const std::shared_ptr<CollisionData> colData,
- std::shared_ptr<CollidingObject> obj1,
- std::shared_ptr<CollidingObject> obj2) :
+ std::shared_ptr<PbdObject> pbdObject1,
+ std::shared_ptr<PbdObject> pbdObject2) :
CollisionHandling(Type::PBD, side, colData),
- m_pbdObject1(std::dynamic_pointer_cast<PbdObject>(obj1)),
- m_pbdObject2(std::dynamic_pointer_cast<PbdObject>(obj2))
+ m_PbdObject1(pbdObject1),
+ m_PbdObject2(pbdObject2),
+ m_pbdCollisionSolver(std::make_shared<PbdCollisionSolver>())
{
}
@@ -61,36 +57,30 @@ PBDCollisionHandling::~PBDCollisionHandling()
void
PBDCollisionHandling::processCollisionData()
{
- /*if (auto deformableObj = std::dynamic_pointer_cast<DeformableObject>(m_object))
- {
- this->computeContactForcesDiscreteDeformable(deformableObj);
- }
- else if (auto analyticObj = std::dynamic_pointer_cast<CollidingObject>(m_object))
- {
- this->computeContactForcesAnalyticRigid(analyticObj);
- }
- else
- {
- LOG(WARNING) << "PenaltyRigidCH::computeContactForces error: "
- << "no penalty collision handling available for " << m_object->getName()
- << " (rigid mesh not yet supported).";
- }*/
this->generatePBDConstraints();
- CHECK(m_PBDSolver != nullptr) << "No PbdSolver found to handle the Collision constraints!";
+ if (m_PBDConstraints.size() == 0)
+ {
+ return;
+ }
- m_PBDSolver->addCollisionConstraints(&m_PBDConstraints);
+ m_pbdCollisionSolver->addCollisionConstraints(&m_PBDConstraints,
+ m_PbdObject1->getPbdModel()->getCurrentState()->getPositions(),
+ m_PbdObject1->getPbdModel()->getInvMasses(),
+ m_PbdObject2->getPbdModel()->getCurrentState()->getPositions(),
+ m_PbdObject2->getPbdModel()->getInvMasses());
}
void
PBDCollisionHandling::generatePBDConstraints()
{
- const auto dynaModel1 = std::static_pointer_cast<PbdModel>(m_pbdObject1->getDynamicalModel());
- const auto dynaModel2 = std::static_pointer_cast<PbdModel>(m_pbdObject2->getDynamicalModel());
- const auto colGeo2 = std::static_pointer_cast<SurfaceMesh>(m_pbdObject2->getCollidingGeometry());
+ std::shared_ptr<PbdCollisionConstraintConfig> config1 = m_PbdObject1->getPbdModel()->getParameters()->collisionParams;
+ std::shared_ptr<PbdCollisionConstraintConfig> config2 = m_PbdObject2->getPbdModel()->getParameters()->collisionParams;
+
+ const auto colGeo2 = std::static_pointer_cast<SurfaceMesh>(m_PbdObject2->getCollidingGeometry());
- const auto map1 = m_pbdObject1->getPhysicsToCollidingMap();
- const auto map2 = m_pbdObject2->getPhysicsToCollidingMap();
+ const auto map1 = m_PbdObject1->getPhysicsToCollidingMap();
+ const auto map2 = m_PbdObject2->getPhysicsToCollidingMap();
// std::cout << "EE: " << m_colData->EEColData.getSize() << "TV: " << m_colData->VTColData.getSize() << std::endl;
@@ -133,7 +123,7 @@ PBDCollisionHandling::generatePBDConstraints()
}
const auto constraint = m_EEConstraintPool[idx];
- constraint->initConstraint(dynaModel1, edgeA1, edgeA2, dynaModel2, edgeB1, edgeB2);
+ constraint->initConstraint(edgeA1, edgeA2, edgeB1, edgeB2, config1, config2);
});
// Generate vertex-triangle pbd constraints
@@ -166,7 +156,10 @@ PBDCollisionHandling::generatePBDConstraints()
}
const auto constraint = m_VTConstraintPool[idx];
- constraint->initConstraint(dynaModel1, colData.vertexIdx, dynaModel2, v1, v2, v3);
+ constraint->initConstraint(
+ colData.vertexIdx,
+ v1, v2, v3,
+ config1, config2);
});
// Copy constraints
diff --git a/Source/CollisionHandling/imstkPBDCollisionHandling.h b/Source/CollisionHandling/imstkPBDCollisionHandling.h
index 39b81c847f0bb2d1dc04bf3fc74bdfec3dbd07be..3935e0db3328f3f4c93a77482f7b6fbae6a81cbe 100644
--- a/Source/CollisionHandling/imstkPBDCollisionHandling.h
+++ b/Source/CollisionHandling/imstkPBDCollisionHandling.h
@@ -24,14 +24,15 @@
// imstk
#include "imstkCollisionHandling.h"
+#include <vector>
+
namespace imstk
{
-class CollidingObject;
class PbdObject;
class PbdCollisionConstraint;
class PbdEdgeEdgeConstraint;
class PbdPointTriangleConstraint;
-class PbdSolver;
+class PbdCollisionSolver;
struct CollisionData;
///
@@ -41,7 +42,6 @@ struct CollisionData;
///
class PBDCollisionHandling : public CollisionHandling
{
-typedef std::vector<PbdCollisionConstraint*> PBDConstraintVector;
public:
///
@@ -49,8 +49,8 @@ public:
///
PBDCollisionHandling(const Side& side,
const std::shared_ptr<CollisionData> colData,
- std::shared_ptr<CollidingObject> obj1,
- std::shared_ptr<CollidingObject> obj2);
+ std::shared_ptr<PbdObject> pbdObject1,
+ std::shared_ptr<PbdObject> pbdObject2);
PBDCollisionHandling() = delete;
@@ -69,18 +69,14 @@ public:
///
void generatePBDConstraints();
- ///
- /// \brief Get/Set pbd solver
- ///
- void setSolver(std::shared_ptr<PbdSolver> solver) { m_PBDSolver = solver; };
- std::shared_ptr<PbdSolver> getSolver() { return m_PBDSolver; };
+ std::shared_ptr<PbdCollisionSolver> getCollisionSolver() const { return m_pbdCollisionSolver; }
private:
+ std::shared_ptr<PbdObject> m_PbdObject1 = nullptr; ///> PBD object
+ std::shared_ptr<PbdObject> m_PbdObject2 = nullptr; ///> PBD object
+ std::shared_ptr<PbdCollisionSolver> m_pbdCollisionSolver = nullptr;
- std::shared_ptr<PbdObject> m_pbdObject1; ///> PBD object
- std::shared_ptr<PbdObject> m_pbdObject2; ///> PBD object
- PBDConstraintVector m_PBDConstraints; ///> List of PBD constraints
- std::shared_ptr<PbdSolver> m_PBDSolver; /// The Solver for the collision constraints
+ std::vector<PbdCollisionConstraint*> m_PBDConstraints; ///> List of PBD constraints
std::vector<PbdEdgeEdgeConstraint*> m_EEConstraintPool;
std::vector<PbdPointTriangleConstraint*> m_VTConstraintPool;
diff --git a/Source/CollisionHandling/imstkPenaltyCH.cpp b/Source/CollisionHandling/imstkPenaltyCH.cpp
index d19f90068678929ad63a1b40ceff9905b3824d32..b3a482040a49f1323be2fd7876eb9ac0e05a8b03 100644
--- a/Source/CollisionHandling/imstkPenaltyCH.cpp
+++ b/Source/CollisionHandling/imstkPenaltyCH.cpp
@@ -20,14 +20,13 @@
=========================================================================*/
#include "imstkPenaltyCH.h"
-
#include "imstkCollidingObject.h"
#include "imstkCollisionData.h"
#include "imstkDeformableObject.h"
+#include "imstkFEMDeformableBodyModel.h"
+#include "imstkLogger.h"
#include "imstkParallelUtils.h"
-#include <g3log/g3log.hpp>
-
namespace imstk
{
PenaltyCH::PenaltyCH(const Side& side,
@@ -98,8 +97,9 @@ PenaltyCH::computeContactForcesDiscreteDeformable(const std::shared_ptr<FeDeform
<< m_object->getName() << " is not a deformable object.";
// Get current force vector
- auto& force = deformableObj->getContactForce();
- const auto& velVector = deformableObj->getVelocities();
+ std::shared_ptr<FEMDeformableBodyModel> model = deformableObj->getFEMModel();
+ auto& force = model->getContactForce();
+ const auto& velVector = model->getCurrentState()->getQDot();
// If collision data, append forces
ParallelUtils::SpinLock lock;
diff --git a/Source/CollisionHandling/imstkPenaltyCH.h b/Source/CollisionHandling/imstkPenaltyCH.h
index a75938f13e91902a49b3b5e47800afa99e835062..35b3724034bc90c0cb26bd141eb20640b372f121 100644
--- a/Source/CollisionHandling/imstkPenaltyCH.h
+++ b/Source/CollisionHandling/imstkPenaltyCH.h
@@ -38,7 +38,6 @@ struct CollisionData;
class PenaltyCH : public CollisionHandling
{
public:
-
///
/// \brief Constructor
///
@@ -78,10 +77,9 @@ public:
}
private:
+ std::shared_ptr<CollidingObject> m_object = nullptr; ///>
- std::shared_ptr<CollidingObject> m_object; ///>
-
- double m_stiffness = 5.0e5; ///> Stiffness of contact
- double m_damping = 0.5; ///> Damping of the contact
+ double m_stiffness = 5.0e5; ///> Stiffness of contact
+ double m_damping = 0.5; ///> Damping of the contact
};
}
diff --git a/Source/CollisionHandling/imstkPickingCH.cpp b/Source/CollisionHandling/imstkPickingCH.cpp
index eca40104ca43190738e656c0f3f28af5c934b56a..ac70bb54389fc2dd923b6be4e8be87c827db9739 100644
--- a/Source/CollisionHandling/imstkPickingCH.cpp
+++ b/Source/CollisionHandling/imstkPickingCH.cpp
@@ -32,9 +32,9 @@ namespace imstk
{
PickingCH::PickingCH(const CollisionHandling::Side& side,
const std::shared_ptr<CollisionData> colData,
- std::shared_ptr<CollidingObject> obj) :
+ std::shared_ptr<FeDeformableObject> obj) :
CollisionHandling(Type::NodalPicking, side, colData),
- m_object(std::dynamic_pointer_cast<FeDeformableObject>(obj))
+ m_object(obj)
{
}
@@ -60,8 +60,9 @@ PickingCH::addPickConstraints(std::shared_ptr<FeDeformableObject> deformableObj)
CHECK(deformableObj != nullptr) << "PenaltyRigidCH::addPickConstraints error: "
<< " not a deformable object.";
- const auto& Uprev = deformableObj->getDisplacements();
- const auto& Vprev = deformableObj->getVelocities();
+ std::shared_ptr<FEMDeformableBodyModel> model = deformableObj->getFEMModel();
+ const Vectord& Uprev = model->getCurrentState()->getQ();
+ const Vectord& Vprev = model->getCurrentState()->getQDot();
auto PhysTetMesh = std::dynamic_pointer_cast<PointSet>(deformableObj->getPhysicsGeometry());
auto dT = std::dynamic_pointer_cast<FEMDeformableBodyModel>(m_object->getDynamicalModel())->getTimeIntegrator()->getTimestepSize();
diff --git a/Source/CollisionHandling/imstkPickingCH.h b/Source/CollisionHandling/imstkPickingCH.h
index 49934c063d2c4b795a440b509fd1e27dace84eae..2614dde37f9ad5adc8ec3e0feb8bce1919fdbf8e 100644
--- a/Source/CollisionHandling/imstkPickingCH.h
+++ b/Source/CollisionHandling/imstkPickingCH.h
@@ -24,6 +24,8 @@
// imstk
#include "imstkCollisionHandling.h"
+#include <vector>
+
namespace imstk
{
class CollidingObject;
@@ -45,7 +47,7 @@ public:
///
PickingCH(const Side& side,
const std::shared_ptr<CollisionData> colData,
- std::shared_ptr<CollidingObject> obj);
+ std::shared_ptr<FeDeformableObject> obj);
PickingCH() = delete;
diff --git a/Source/CollisionHandling/imstkSPHCollisionHandling.cpp b/Source/CollisionHandling/imstkSPHCollisionHandling.cpp
index 5c34f080fa9af109b70347a297025abcc12f34a4..99670ce38a06b5cb74183a108335ebe09ccb8034 100644
--- a/Source/CollisionHandling/imstkSPHCollisionHandling.cpp
+++ b/Source/CollisionHandling/imstkSPHCollisionHandling.cpp
@@ -20,19 +20,18 @@
=========================================================================*/
#include "imstkSPHCollisionHandling.h"
-#include "imstkParallelUtils.h"
#include "imstkCollisionData.h"
-#include "imstkSPHObject.h"
+#include "imstkLogger.h"
+#include "imstkParallelUtils.h"
#include "imstkSPHModel.h"
-
-#include <g3log/g3log.hpp>
+#include "imstkSPHObject.h"
namespace imstk
{
-SPHCollisionHandling::SPHCollisionHandling(const CollisionHandling::Side& side,
- const std::shared_ptr<CollisionData>& colData,
- const std::shared_ptr<CollidingObject>& obj) :
- CollisionHandling(Type::SPH, side, colData), m_SPHObject(std::dynamic_pointer_cast<SPHObject>(obj))
+SPHCollisionHandling::SPHCollisionHandling(const CollisionHandling::Side& side,
+ std::shared_ptr<CollisionData> colData,
+ std::shared_ptr<SPHObject> obj) :
+ CollisionHandling(Type::SPH, side, colData), m_SPHObject(obj)
{
LOG_IF(FATAL, (!m_SPHObject)) << "Invalid SPH object";
}
@@ -55,9 +54,9 @@ SPHCollisionHandling::processCollisionData()
ParallelUtils::parallelFor(m_colData->MAColData.getSize(),
[&](const size_t idx)
{
- const auto& cd = m_colData->MAColData[idx];
- const auto pidx = cd.nodeIdx; // Fluid particle index
- auto n = cd.penetrationVector; // This vector should point into solid object
+ const MeshToAnalyticalCollisionDataElement& cd = m_colData->MAColData[idx];
+ const uint32_t pidx = cd.nodeIdx; // Fluid particle index
+ Vec3d n = cd.penetrationVector; // This vector should point into solid object
// Correct particle position
state.getPositions()[pidx] -= n;
diff --git a/Source/CollisionHandling/imstkSPHCollisionHandling.h b/Source/CollisionHandling/imstkSPHCollisionHandling.h
index 27a819f09668e4609b1c97dc48768c64b3060f0d..e024c18d70375230690c0267cccf1c05a0c8e120 100644
--- a/Source/CollisionHandling/imstkSPHCollisionHandling.h
+++ b/Source/CollisionHandling/imstkSPHCollisionHandling.h
@@ -30,9 +30,9 @@ class SPHObject;
class SPHCollisionHandling : public CollisionHandling
{
public:
- SPHCollisionHandling(const Side& side,
- const std::shared_ptr<CollisionData>& colData,
- const std::shared_ptr<CollidingObject>& obj);
+ SPHCollisionHandling(const Side& side,
+ std::shared_ptr<CollisionData> colData,
+ std::shared_ptr<SPHObject> obj);
SPHCollisionHandling() = delete;
@@ -42,6 +42,6 @@ public:
virtual void processCollisionData() override;
private:
- std::shared_ptr<SPHObject> m_SPHObject;
+ std::shared_ptr<SPHObject> m_SPHObject = nullptr;
};
} // end namespace imstk
diff --git a/Source/CollisionHandling/imstkVirtualCouplingCH.h b/Source/CollisionHandling/imstkVirtualCouplingCH.h
index 7fdbcd391a7a243875ed352003c7468ab48439c8..3ad40c87f577256666822a6c9362931296c99bad 100644
--- a/Source/CollisionHandling/imstkVirtualCouplingCH.h
+++ b/Source/CollisionHandling/imstkVirtualCouplingCH.h
@@ -23,6 +23,7 @@
// imstk
#include "imstkCollisionHandling.h"
+#include "imstkMath.h"
namespace imstk
{
diff --git a/Source/SceneEntities/Objects/deprecated/imstkPbdRigidObject.cpp b/Source/Common/TaskGraph/imstkSequentialTaskGraphController.cpp
similarity index 69%
rename from Source/SceneEntities/Objects/deprecated/imstkPbdRigidObject.cpp
rename to Source/Common/TaskGraph/imstkSequentialTaskGraphController.cpp
index a70731e37ffd09107a38272930b3f615c45515e4..f9cccef2ad552d2db107bf901b2b2783084f7fd5 100644
--- a/Source/SceneEntities/Objects/deprecated/imstkPbdRigidObject.cpp
+++ b/Source/Common/TaskGraph/imstkSequentialTaskGraphController.cpp
@@ -19,20 +19,25 @@
=========================================================================*/
-#include <memory>
-
-#include "imstkPbdRigidObject.h"
-#include "imstkGeometry.h"
-#include "imstkGeometryMap.h"
-#include "imstkPbdModel.h"
-
-#include <g3log/g3log.hpp>
+#include "imstkSequentialTaskGraphController.h"
+#include "imstkTaskGraph.h"
+#include "imstkTaskNode.h"
namespace imstk
{
void
-PbdRigidObject::updatePbdStates()
+SequentialTaskGraphController::init()
+{
+ m_executionOrderedNodes = TaskGraph::topologicalSort(m_graph);
+}
+
+void
+SequentialTaskGraphController::execute()
{
- m_pbdModel->updatePbdStateFromPhysicsGeometry();
+ // Sequential
+ for (std::shared_ptr<TaskNode> node : *m_executionOrderedNodes)
+ {
+ node->execute();
+ }
}
-} //imstk
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkSequentialTaskGraphController.h b/Source/Common/TaskGraph/imstkSequentialTaskGraphController.h
new file mode 100644
index 0000000000000000000000000000000000000000..b645df75204427d9bd2b86d5473c19565c9d2923
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkSequentialTaskGraphController.h
@@ -0,0 +1,51 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkTaskGraphController.h"
+#include <list>
+
+namespace imstk
+{
+class TaskNode;
+
+///
+/// \class SequentialTaskGraphController
+///
+/// \brief This class executes a TaskGraph by first topologically sorting them (Kahn's algorithm)
+/// then sequentially running them
+///
+class SequentialTaskGraphController : public TaskGraphController
+{
+public:
+ ///
+ /// \brief Sorts the computational nodes
+ ///
+ void init() override;
+
+ void execute() override;
+
+private:
+ // The current nodes to execute, ordered
+ std::shared_ptr<std::list<std::shared_ptr<TaskNode>>> m_executionOrderedNodes;
+};
+};
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTaskGraph.cpp b/Source/Common/TaskGraph/imstkTaskGraph.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..04a9ce5f09b5c85fbdd68e1cfe27746bf2042aef
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTaskGraph.cpp
@@ -0,0 +1,843 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#include "imstkTaskGraph.h"
+#include "imstkLogger.h"
+#include "imstkTaskNode.h"
+#include <queue>
+#include <set>
+#include <stack>
+
+namespace imstk
+{
+TaskGraph::TaskGraph(std::string sourceName, std::string sinkName) :
+ m_source(std::make_shared<TaskNode>()),
+ m_sink(std::make_shared<TaskNode>())
+{
+ m_source->m_name = sourceName;
+ m_sink->m_name = sinkName;
+ addNode(m_source);
+ addNode(m_sink);
+}
+
+TaskNodeVector::iterator
+TaskGraph::findNode(std::string name)
+{
+ return std::find_if(m_nodes.begin(), m_nodes.end(),
+ [&name](const std::shared_ptr<TaskNode>& x) { return x->m_name == name; });
+}
+
+TaskNodeVector::const_iterator
+TaskGraph::findNode(std::string name) const
+{
+ return std::find_if(m_nodes.begin(), m_nodes.end(),
+ [&name](const std::shared_ptr<TaskNode>& x) { return x->m_name == name; });
+}
+
+TaskNodeVector::iterator
+TaskGraph::findNode(std::shared_ptr<TaskNode> node)
+{
+ return std::find(m_nodes.begin(), m_nodes.end(), node);
+}
+
+TaskNodeVector::const_iterator
+TaskGraph::findNode(std::shared_ptr<TaskNode> node) const
+{
+ return std::find(m_nodes.begin(), m_nodes.end(), node);
+}
+
+bool
+TaskGraph::containsNode(std::shared_ptr<TaskNode> node) const
+{
+ return findNode(node) != endNode();
+}
+
+bool
+TaskGraph::containsEdge(std::shared_ptr<TaskNode> srcNode, std::shared_ptr<TaskNode> destNode)
+{
+ return (m_adjList.count(srcNode) != 0 && m_adjList[srcNode].count(destNode) != 0);
+}
+
+void
+TaskGraph::addEdge(std::shared_ptr<TaskNode> srcNode, std::shared_ptr<TaskNode> destNode)
+{
+ m_adjList[srcNode].insert(destNode);
+ m_invAdjList[destNode].insert(srcNode);
+}
+
+void
+TaskGraph::nestGraph(std::shared_ptr<TaskGraph> subgraph, std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink)
+{
+ // Ensure source and sink are in the graph
+ if (findNode(source) == endNode())
+ {
+ LOG(WARNING) << "Tried to nest a graph using source, but source does not exist in this";
+ return;
+ }
+ if (findNode(sink) == endNode())
+ {
+ LOG(WARNING) << "Tried to nest a graph using sink, but sink does not exist in this";
+ return;
+ }
+
+ // Copy all of the nodes into this graph (check duplicates)
+ for (TaskNodeVector::iterator it = subgraph->getNodes().begin(); it != subgraph->getNodes().end(); it++)
+ {
+ addNode(*it);
+ }
+ // Copy all the edges into the graph (no need to check for duplicates)
+ const TaskNodeAdjList& adjList = subgraph->getAdjList();
+ for (TaskNodeAdjList::const_iterator it = adjList.begin(); it != adjList.end(); it++)
+ {
+ const TaskNodeSet& outputNodes = it->second;
+ for (TaskNodeSet::const_iterator jt = outputNodes.begin(); jt != outputNodes.end(); jt++)
+ {
+ addEdge(it->first, *jt);
+ }
+ }
+ // Add source and sink edges
+ addEdge(source, subgraph->getSource());
+ addEdge(subgraph->getSink(), sink);
+}
+
+void
+TaskGraph::removeEdge(std::shared_ptr<TaskNode> srcNode, std::shared_ptr<TaskNode> destNode)
+{
+ if (m_adjList.count(srcNode) == 0)
+ {
+ return;
+ }
+ if (m_adjList[srcNode].count(destNode) == 0)
+ {
+ return;
+ }
+ m_adjList[srcNode].erase(destNode);
+ m_invAdjList[destNode].erase(srcNode);
+ if (m_adjList[srcNode].size() == 0)
+ {
+ m_adjList.erase(srcNode);
+ }
+ if (m_invAdjList[destNode].size() == 0)
+ {
+ m_invAdjList.erase(destNode);
+ }
+}
+
+bool
+TaskGraph::addNode(std::shared_ptr<TaskNode> node)
+{
+ // If the node already exists return false
+ if (!containsNode(node))
+ {
+ // Put it in this graph
+ m_nodes.push_back(node);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+std::shared_ptr<TaskNode>
+TaskGraph::addFunction(std::string name, std::function<void()> func)
+{
+ std::shared_ptr<TaskNode> node = std::make_shared<TaskNode>(func, name);
+ m_nodes.push_back(node);
+ return node;
+}
+
+bool
+TaskGraph::removeNode(std::shared_ptr<TaskNode> node)
+{
+ if (!containsNode(node))
+ {
+ LOG(INFO) << "Tried to remove node " + node->m_name + " from graph but it doesn't contain the node.";
+ return false;
+ }
+
+ // Erase edges
+ TaskNodeSet inputs = m_invAdjList[node];
+ TaskNodeSet outputs = m_adjList[node];
+ for (TaskNodeSet::iterator i = inputs.begin(); i != inputs.end(); i++)
+ {
+ removeEdge(*i, node);
+ }
+ for (TaskNodeSet::iterator i = outputs.begin(); i != outputs.end(); i++)
+ {
+ removeEdge(node, *i);
+ }
+
+ // Finally remove the node from the graph
+ TaskNodeVector::iterator it = findNode(node);
+ if (it != endNode())
+ {
+ m_nodes.erase(it);
+ }
+ return true;
+}
+
+bool
+TaskGraph::removeNodeAndRedirect(std::shared_ptr<TaskNode> node)
+{
+ if (!containsNode(node))
+ {
+ LOG(INFO) << "Tried to remove node " + node->m_name + " from graph but it doesn't contain the node.";
+ return false;
+ }
+
+ // Erase edges
+ TaskNodeSet inputs = m_invAdjList[node];
+ TaskNodeSet outputs = m_adjList[node];
+ for (TaskNodeSet::iterator i = inputs.begin(); i != inputs.end(); i++)
+ {
+ removeEdge(*i, node);
+ }
+ for (TaskNodeSet::iterator i = outputs.begin(); i != outputs.end(); i++)
+ {
+ removeEdge(node, *i);
+ }
+
+ // Fix the edges
+ for (TaskNodeSet::iterator i = inputs.begin(); i != inputs.end(); i++)
+ {
+ for (TaskNodeSet::iterator j = outputs.begin(); j != outputs.end(); j++)
+ {
+ addEdge(*i, *j);
+ }
+ }
+
+ // Finally remove the node from the graph
+ TaskNodeVector::iterator it = findNode(node);
+ if (it != endNode())
+ {
+ m_nodes.erase(it);
+ }
+
+ return true;
+}
+
+void
+TaskGraph::insertAfter(std::shared_ptr<TaskNode> refNode, std::shared_ptr<TaskNode> newNode)
+{
+ // Try to add to graph, if already exists, exit
+ if (!addNode(newNode))
+ {
+ return;
+ }
+
+ // Remove output edges
+ TaskNodeSet outputs = m_adjList[refNode]; // Copy (since we are modifying)
+ for (TaskNodeSet::iterator i = outputs.begin(); i != outputs.end(); i++)
+ {
+ removeEdge(refNode, *i);
+ }
+
+ // Add refNode->newNode
+ addEdge(refNode, newNode);
+
+ // Add newNode->outputs[i]
+ for (TaskNodeSet::iterator i = outputs.begin(); i != outputs.end(); i++)
+ {
+ addEdge(newNode, *i);
+ }
+}
+
+void
+TaskGraph::insertBefore(std::shared_ptr<TaskNode> refNode, std::shared_ptr<TaskNode> newNode)
+{
+ // Try to add to graph, if already exists, exit
+ if (!addNode(newNode))
+ {
+ return;
+ }
+
+ // Remove input edges
+ TaskNodeSet inputs = m_invAdjList[refNode]; // Copy (since we are modifying)
+ for (TaskNodeSet::iterator i = inputs.begin(); i != inputs.end(); i++)
+ {
+ removeEdge(*i, refNode);
+ }
+
+ // inputs[i]->newNode
+ for (TaskNodeSet::iterator i = inputs.begin(); i != inputs.end(); i++)
+ {
+ addEdge(*i, newNode);
+ }
+
+ // refNode->newNode
+ addEdge(newNode, refNode);
+}
+
+bool
+TaskGraph::isReachable(std::shared_ptr<TaskNode> srcNode, std::shared_ptr<TaskNode> destNode)
+{
+ const TaskNodeAdjList& adjList = getAdjList();
+
+ // Simple BFS
+ std::unordered_set<std::shared_ptr<TaskNode>> visitedNodes;
+
+ // It inserts itself as well
+ std::queue<std::shared_ptr<TaskNode>> nodeStack;
+ nodeStack.push(srcNode);
+ while (!nodeStack.empty())
+ {
+ std::shared_ptr<TaskNode> currNode = nodeStack.front();
+ nodeStack.pop();
+
+ // If we've arrived at the desired node
+ if (destNode == currNode)
+ {
+ return true;
+ }
+
+ // Add children to stack if not yet visited
+ if (adjList.count(currNode) != 0)
+ {
+ const TaskNodeSet& outputNodes = adjList.at(currNode);
+ for (TaskNodeSet::const_iterator j = outputNodes.begin(); j != outputNodes.end(); j++)
+ {
+ std::shared_ptr<TaskNode> childNode = *j;
+ if (visitedNodes.count(childNode) == 0)
+ {
+ visitedNodes.insert(childNode);
+ nodeStack.push(childNode);
+ }
+ }
+ }
+ }
+ return false;
+}
+
+void
+TaskGraph::clear()
+{
+ m_nodes.clear();
+ clearEdges();
+ addNode(m_source);
+ addNode(m_sink);
+}
+
+//std::shared_ptr<TaskGraph>
+//TaskGraph::sum(std::shared_ptr<TaskGraph> graphA, std::shared_ptr<TaskGraph> graphB)
+//{
+// std::shared_ptr<TaskGraph> results = std::make_shared<TaskGraph>();
+// // Get rid of the results source/sink
+// results->m_source = nullptr;
+// results->m_sink = nullptr;
+// results->m_nodes.clear();
+//
+// // Sum the nodes
+// TaskNodeVector& nodesA = graphA->getNodes();
+// for (size_t i = 0; i < nodesA.size(); i++)
+// {
+// results->addNode(nodesA[i]);
+// }
+// TaskNodeVector& nodesB = graphB->getNodes();
+// for (size_t i = 0; i < nodesB.size(); i++)
+// {
+// results->addNode(nodesB[i]);
+// }
+//
+// // Now sum the edges
+// const TaskNodeAdjList& adjListA = graphA->getAdjList();
+// for (TaskNodeAdjList::const_iterator it = adjListA.begin(); it != adjListA.end(); it++)
+// {
+// const TaskNodeSet& outputNodes = it->second;
+// for (TaskNodeSet::const_iterator jt = outputNodes.begin(); jt != outputNodes.end(); jt++)
+// {
+// results->addEdge(it->first, *jt);
+// }
+// }
+// const TaskNodeAdjList& adjListB = graphB->getAdjList();
+// for (TaskNodeAdjList::const_iterator it = adjListB.begin(); it != adjListB.end(); it++)
+// {
+// const TaskNodeSet& outputNodes = it->second;
+// for (TaskNodeSet::const_iterator jt = outputNodes.begin(); jt != outputNodes.end(); jt++)
+// {
+// results->addEdge(it->first, *jt);
+// }
+// }
+//
+// return results;
+//}
+
+std::shared_ptr<TaskNodeList>
+TaskGraph::topologicalSort(std::shared_ptr<TaskGraph> graph)
+{
+ // Compute the number of inputs to each node (we will remove these as we go)
+ std::unordered_map<std::shared_ptr<TaskNode>, size_t> numInputs;
+
+ const TaskNodeAdjList& adjList = graph->getAdjList();
+ const TaskNodeAdjList& invAdjList = graph->getInvAdjList();
+
+ for (TaskNodeAdjList::const_iterator i = invAdjList.begin(); i != invAdjList.end(); i++)
+ {
+ numInputs[i->first] = invAdjList.size();
+ }
+
+ // Create an edge blacklist for edge removal during algorithm
+ std::unordered_map<std::shared_ptr<TaskNode>, std::shared_ptr<TaskNode>> removedEdges;
+
+ auto edgeHasBeenRemoved = [&](const std::shared_ptr<TaskNode>& node1, const std::shared_ptr<TaskNode>& node2)
+ {
+ return (removedEdges.count(node1) != 0) && (removedEdges[node1] == node2);
+ };
+
+ // Kahns algorithm (BFS/queue)
+ // iterate through all nodes (BFS or DFS) removing edges
+ // nodes are accepted when all input edges have been removed
+ std::queue<std::shared_ptr<TaskNode>> sources;
+ sources.push(graph->m_source);
+ std::shared_ptr<TaskNodeList> results = std::make_shared<TaskNodeList>();
+
+ while (!sources.empty())
+ {
+ // Remove a node
+ std::shared_ptr<TaskNode> node = sources.front();
+ sources.pop();
+
+ results->push_back(node);
+
+ // As long as there are children
+ if (adjList.count(node) != 0)
+ {
+ const TaskNodeSet& outputNodes = adjList.at(node);
+ for (TaskNodeSet::const_iterator it = outputNodes.begin(); it != outputNodes.end(); it++)
+ {
+ std::shared_ptr<TaskNode> childNode = *it;
+ // If edge is present
+ if (!edgeHasBeenRemoved(node, childNode))
+ {
+ // Mark edge as removed
+ removedEdges[node] = childNode;
+
+ // Decrement amount of inputs
+ numInputs[childNode]--;
+ if (numInputs[childNode] <= 0)
+ {
+ sources.push(childNode);
+ }
+ }
+ }
+ }
+ }
+ return results;
+}
+
+std::shared_ptr<TaskGraph>
+TaskGraph::resolveCriticalNodes(std::shared_ptr<TaskGraph> graph)
+{
+ std::shared_ptr<TaskGraph> results = std::make_shared<TaskGraph>(*graph);
+
+ const TaskNodeAdjList& adjList = graph->getAdjList();
+ const TaskNodeVector& nodes = graph->getNodes();
+
+ // Compute the levels of each node via DFS
+ std::unordered_map<std::shared_ptr<TaskNode>, int> depths;
+ {
+ std::unordered_set<std::shared_ptr<TaskNode>> visitedNodes;
+
+ // DFS for the dependencies
+ std::stack<std::shared_ptr<TaskNode>> nodeStack;
+ depths[graph->getSource()] = 0;
+ nodeStack.push(graph->getSource());
+ while (!nodeStack.empty())
+ {
+ std::shared_ptr<TaskNode> currNode = nodeStack.top();
+ int currLevel = depths[currNode];
+ nodeStack.pop();
+
+ // Add children to stack if not yet visited
+ if (adjList.count(currNode) != 0)
+ {
+ const TaskNodeSet& outputNodes = adjList.at(currNode);
+ for (TaskNodeSet::const_iterator i = outputNodes.begin(); i != outputNodes.end(); i++)
+ {
+ std::shared_ptr<TaskNode> childNode = *i;
+ if (visitedNodes.count(childNode) == 0)
+ {
+ visitedNodes.insert(childNode);
+ depths[childNode] = currLevel + 1;
+ nodeStack.push(childNode);
+ }
+ }
+ }
+ }
+ }
+
+ // Identify the set of critical nodes
+ TaskNodeVector critNodes;
+ for (size_t i = 0; i < nodes.size(); i++)
+ {
+ if (nodes[i]->m_isCritical)
+ {
+ critNodes.push_back(nodes[i]);
+ }
+ }
+
+ // Compute the critical adjacency list
+ TaskNodeAdjList critAdjList;
+ for (size_t i = 0; i < critNodes.size(); i++)
+ {
+ std::unordered_set<std::shared_ptr<TaskNode>> visitedNodes;
+
+ // DFS for the dependencies
+ std::stack<std::shared_ptr<TaskNode>> nodeStack;
+ nodeStack.push(critNodes[i]);
+ while (!nodeStack.empty())
+ {
+ std::shared_ptr<TaskNode> currNode = nodeStack.top();
+ nodeStack.pop();
+
+ // If you can reach one critical node from the other then they are adjacent
+ if (currNode->m_isCritical)
+ {
+ critAdjList[critNodes[i]].insert(currNode);
+ }
+
+ // Add children to stack if not yet visited
+ if (adjList.count(currNode) != 0)
+ {
+ const TaskNodeSet& outputNodes = adjList.at(currNode);
+ for (TaskNodeSet::const_iterator j = outputNodes.begin(); j != outputNodes.end(); j++)
+ {
+ std::shared_ptr<TaskNode> childNode = *j;
+ if (visitedNodes.count(childNode) == 0)
+ {
+ visitedNodes.insert(childNode);
+ nodeStack.push(childNode);
+ }
+ }
+ }
+ }
+ }
+
+ // Now we know which critical nodes depend on each other (we are interested in those that aren't)
+ // For every critical pair
+ for (size_t i = 0; i < critNodes.size(); i++)
+ {
+ std::shared_ptr<TaskNode> srcNode = critNodes[i];
+ for (size_t j = i + 1; j < critNodes.size(); j++)
+ {
+ std::shared_ptr<TaskNode> destNode = critNodes[j];
+ // If the edge doesn't exist, either way
+ if ((critAdjList.count(srcNode) == 0 || critAdjList.at(srcNode).count(destNode) == 0)
+ && (critAdjList.count(destNode) == 0 || critAdjList.at(destNode).count(srcNode) == 0))
+ {
+ // Add an edge between the critical nodes in the direction of levels
+ int leveli = depths[srcNode];
+ int levelj = depths[destNode];
+ // If i is below j, then j->i
+ if (leveli > levelj)
+ {
+ results->addEdge(destNode, srcNode);
+ }
+ else
+ {
+ results->addEdge(srcNode, destNode);
+ }
+ }
+ }
+ }
+
+ return results;
+}
+
+std::shared_ptr<TaskGraph>
+TaskGraph::transitiveReduce(std::shared_ptr<TaskGraph> graph)
+{
+ // It's a bad idea to do this method if the graph is cyclic
+ if (isCyclic(graph))
+ {
+ return nullptr;
+ }
+
+ std::shared_ptr<TaskGraph> results = std::make_shared<TaskGraph>(*graph);
+
+ // Copy the adjList
+ const TaskNodeAdjList adjList = results->getAdjList();
+
+ // For every edge in the graph
+ for (TaskNodeAdjList::const_iterator i = adjList.begin(); i != adjList.end(); i++)
+ {
+ std::shared_ptr<TaskNode> inputNode = i->first;
+ const TaskNodeSet& outputNodes = i->second;
+ for (TaskNodeSet::const_iterator j = outputNodes.begin(); j != outputNodes.end(); j++)
+ {
+ std::shared_ptr<TaskNode> outputNode = *j;
+ // Edge inputNode->outputNode
+
+ // Remove the edge and see if it still reaches
+ results->removeEdge(inputNode, outputNode);
+
+ // If there still exists a path inputNode->outputNode, leave it removed
+ if (!results->isReachable(inputNode, outputNode))
+ {
+ results->addEdge(inputNode, outputNode);
+ }
+ }
+ }
+
+ return results;
+}
+
+std::shared_ptr<TaskGraph>
+TaskGraph::removeRedundantNodes(std::shared_ptr<TaskGraph> graph)
+{
+ std::shared_ptr<TaskGraph> results = std::make_shared<TaskGraph>(*graph);
+
+ const TaskNodeAdjList& adjList = results->getAdjList();
+ const TaskNodeAdjList& invAdjList = results->getInvAdjList();
+ TaskNodeVector& nodes = results->getNodes();
+
+ for (size_t i = 0; i < nodes.size(); i++)
+ {
+ std::shared_ptr<TaskNode> node = nodes[i];
+
+ // These can't be removed (following code would break as they have no inputs/outputs)
+ if (node == graph->getSource() || node == graph->getSink())
+ {
+ continue;
+ }
+
+ // If node not functional and has single input/output it is removeable
+ if (!node->isFunctional())
+ {
+ // Get copies of the inputs and outputs of the node
+ TaskNodeSet inputs = invAdjList.at(node);
+ TaskNodeSet outputs = adjList.at(node);
+ if (inputs.size() == 1 && outputs.size() == 1)
+ {
+ // Remove inputs/outputs of node
+ for (TaskNodeSet::iterator j = inputs.begin(); j != inputs.end(); j++)
+ {
+ results->removeEdge(*j, node);
+ }
+ for (TaskNodeSet::iterator j = outputs.begin(); j != outputs.end(); j++)
+ {
+ results->removeEdge(node, *j);
+ }
+
+ // Fix the edges
+ for (TaskNodeSet::iterator j = inputs.begin(); j != inputs.end(); j++)
+ {
+ for (TaskNodeSet::iterator k = outputs.begin(); k != outputs.end(); k++)
+ {
+ results->addEdge(*j, *k);
+ }
+ }
+
+ // Finally remove the node from the graph
+ nodes.erase(nodes.begin() + i);
+
+ // If node was removed, don't advance
+ i--;
+ }
+ }
+ }
+ return results;
+}
+
+bool
+TaskGraph::isCyclic(std::shared_ptr<TaskGraph> graph)
+{
+ // Brute force, DFS every node to find it again
+ const TaskNodeAdjList& adjList = graph->getAdjList();
+ const TaskNodeVector& nodes = graph->getNodes();
+ for (size_t i = 0; i < nodes.size(); i++)
+ {
+ std::unordered_set<std::shared_ptr<TaskNode>> visitedNodes;
+
+ // DFS for the dependencies (start at children instead of itself)
+ std::stack<std::shared_ptr<TaskNode>> nodeStack;
+ // Add children to stack if not yet visited
+ if (adjList.count(nodes[i]) != 0)
+ {
+ const TaskNodeSet& outputNodes = adjList.at(nodes[i]);
+ for (TaskNodeSet::const_iterator j = outputNodes.begin(); j != outputNodes.end(); j++)
+ {
+ std::shared_ptr<TaskNode> childNode = *j;
+ if (visitedNodes.count(childNode) == 0)
+ {
+ visitedNodes.insert(childNode);
+ nodeStack.push(childNode);
+ }
+ }
+ }
+ while (!nodeStack.empty())
+ {
+ std::shared_ptr<TaskNode> currNode = nodeStack.top();
+ nodeStack.pop();
+
+ // If we revisit a node then it must be cyclic
+ if (nodes[i] == currNode)
+ {
+ return true;
+ }
+
+ // Add children to stack if not yet visited
+ if (adjList.count(currNode) != 0)
+ {
+ const TaskNodeSet& outputNodes = adjList.at(currNode);
+ for (TaskNodeSet::const_iterator j = outputNodes.begin(); j != outputNodes.end(); j++)
+ {
+ std::shared_ptr<TaskNode> childNode = *j;
+ if (visitedNodes.count(childNode) == 0)
+ {
+ visitedNodes.insert(childNode);
+ nodeStack.push(childNode);
+ }
+ }
+ }
+ }
+ }
+
+ return false;
+}
+
+std::unordered_map<std::shared_ptr<TaskNode>, std::string>
+TaskGraph::getUniqueNodeNames(std::shared_ptr<TaskGraph> graph, bool apply)
+{
+ // Produce non colliding names
+ std::unordered_map<std::shared_ptr<TaskNode>, std::string> nodeNames;
+ std::unordered_map<std::string, int> names;
+ const TaskNodeVector& nodes = graph->getNodes();
+ for (size_t i = 0; i < nodes.size(); i++)
+ {
+ nodeNames[nodes[i]] = nodes[i]->m_name;
+ names[nodes[i]->m_name]++;
+ }
+ // Adjust names
+ for (std::unordered_map<std::shared_ptr<TaskNode>, std::string>::iterator it = nodeNames.begin();
+ it != nodeNames.end(); it++)
+ {
+ int nameIter = 0;
+ std::string currName = it->second;
+ // If we can find a node with this name, increment name counter and try again
+ while (names[currName] > 1)
+ {
+ names[currName]--;
+ currName = it->second + std::to_string(nameIter);
+ names[currName]++;
+ nameIter++;
+ }
+ nodeNames[it->first] = currName;
+ }
+ if (apply)
+ {
+ for (std::shared_ptr<TaskNode> node : nodes)
+ {
+ node->m_name = nodeNames[node];
+ }
+ }
+ return nodeNames;
+}
+
+std::unordered_map<std::shared_ptr<TaskNode>, double>
+TaskGraph::getNodeStartTimes(std::shared_ptr<TaskGraph> graph)
+{
+ const TaskNodeAdjList& adjList = graph->getAdjList();
+
+ // Setup a map for total elapsed times at each node
+ std::unordered_map<std::shared_ptr<TaskNode>, double> startTimes;
+
+ {
+ std::unordered_set<std::shared_ptr<TaskNode>> visitedNodes;
+
+ // BFS down the tree computing total times
+ std::stack<std::shared_ptr<TaskNode>> nodeStack;
+ startTimes[graph->getSource()] = 0.0;
+ nodeStack.push(graph->getSource());
+ while (!nodeStack.empty())
+ {
+ std::shared_ptr<TaskNode> currNode = nodeStack.top();
+ nodeStack.pop();
+
+ // Add children to stack if not yet visited
+ if (adjList.count(currNode) != 0)
+ {
+ const TaskNodeSet& outputNodes = adjList.at(currNode);
+ for (TaskNodeSet::const_iterator i = outputNodes.begin(); i != outputNodes.end(); i++)
+ {
+ std::shared_ptr<TaskNode> childNode = *i;
+
+ // Determine the time it would take to start this child
+ const double childStartTime = startTimes[currNode] + currNode->m_computeTime;
+ if (childStartTime > startTimes[childNode])
+ {
+ // Accept the longest time as nodes can't continue until all child inputs complete
+ startTimes[childNode] = childStartTime;
+ }
+ // If not visited yet add child to stack
+ if (visitedNodes.count(childNode) == 0)
+ {
+ visitedNodes.insert(childNode);
+ //times[childNode] = times[currNode] + childNode->m_computeTime;
+ nodeStack.push(childNode);
+ }
+ }
+ }
+ }
+ }
+ return startTimes;
+}
+
+TaskNodeList
+TaskGraph::getCriticalPath(std::shared_ptr<TaskGraph> graph)
+{
+ std::unordered_map<std::shared_ptr<TaskNode>, double> nodeStartTimes = getNodeStartTimes(graph);
+
+ // Now backtrack to acquire the path of longest duration
+ const TaskNodeAdjList& invAdjList = graph->getInvAdjList();
+ std::list<std::shared_ptr<TaskNode>> results;
+ {
+ std::shared_ptr<TaskNode> currNode = graph->getSink();
+ // Starting from the sink, always choose the input with the longer start time
+ while (currNode != graph->getSource())
+ {
+ results.push_front(currNode);
+ std::shared_ptr<TaskNode> longestNode = nullptr;
+ double maxTime = 0.0;
+
+ // For every parent
+ if (invAdjList.count(currNode) != 0)
+ {
+ const TaskNodeSet& inputNodes = invAdjList.at(currNode);
+ for (TaskNodeSet::const_iterator i = inputNodes.begin(); i != inputNodes.end(); i++)
+ {
+ std::shared_ptr<TaskNode> parentNode = *i;
+ if (nodeStartTimes[parentNode] >= maxTime)
+ {
+ maxTime = nodeStartTimes[parentNode];
+ longestNode = parentNode;
+ }
+ }
+ }
+
+ // Move to the node with the longest time
+ currNode = longestNode;
+ }
+ }
+ results.push_front(graph->getSource());
+ return results;
+}
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTaskGraph.h b/Source/Common/TaskGraph/imstkTaskGraph.h
new file mode 100644
index 0000000000000000000000000000000000000000..79c2d99c5a0e8a4176c6a085217c003dba877b12
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTaskGraph.h
@@ -0,0 +1,239 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkTaskNode.h"
+#include <list>
+#include <memory>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+namespace imstk
+{
+using TaskNodeVector = std::vector<std::shared_ptr<TaskNode>>;
+using TaskNodeList = std::list<std::shared_ptr<TaskNode>>;
+using TaskNodeSet = std::unordered_set<std::shared_ptr<TaskNode>>;
+using TaskNodeAdjList = std::unordered_map<std::shared_ptr<TaskNode>, TaskNodeSet>;
+
+///
+/// \class TaskGraph
+///
+/// \brief Base class for TaskGraph, a collection of TaskNode's. Maintains nodes,
+/// adjacencyList, and invAdjacencyList.
+///
+class TaskGraph
+{
+public:
+ TaskGraph(std::string sourceName = "Source", std::string sinkName = "Sink");
+ virtual ~TaskGraph() = default;
+
+public:
+ std::shared_ptr<TaskNode> getSource() { return m_source; }
+ std::shared_ptr<TaskNode> getSink() { return m_sink; }
+
+ ///
+ /// \brief Get the nodes belonging to this graph
+ ///
+ TaskNodeVector& getNodes() { return m_nodes; }
+
+ ///
+ /// \brief Get the edges belonging to this graph
+ ///
+ TaskNodeAdjList& getAdjList() { return m_adjList; }
+
+ ///
+ /// \brief Get the inverse edges belonging to this graph
+ ///
+ TaskNodeAdjList& getInvAdjList() { return m_invAdjList; }
+
+// Node operations
+public:
+ ///
+ /// \brief Linear search for node by name within this graph
+ ///
+ TaskNodeVector::iterator findNode(std::string name);
+ TaskNodeVector::const_iterator findNode(std::string name) const;
+ ///
+ /// \brief Linear search for node within this graph
+ ///
+ TaskNodeVector::iterator findNode(std::shared_ptr<TaskNode> node);
+ TaskNodeVector::const_iterator findNode(std::shared_ptr<TaskNode> node) const;
+
+ ///
+ /// \brief Check if node exists in this graph
+ ///
+ bool containsNode(std::shared_ptr<TaskNode> node) const;
+
+ ///
+ /// \brief Returns the start of the node container
+ ///
+ TaskNodeVector::iterator endNode() { return m_nodes.end(); }
+ TaskNodeVector::const_iterator endNode() const { return m_nodes.end(); }
+ ///
+ /// \brief Returns the start of the node container
+ ///
+ TaskNodeVector::iterator beginNode() { return m_nodes.begin(); }
+ TaskNodeVector::const_iterator beginNode() const { return m_nodes.begin(); }
+
+ ///
+ /// \brief Adds a node to the graph, returns if successful.
+ /// Returns false and fails if node already exists in graph
+ ///
+ bool addNode(std::shared_ptr<TaskNode> node);
+
+ ///
+ /// \brief Creates a node for the function, adds it to the graph
+ ///
+ std::shared_ptr<TaskNode> addFunction(std::string name, std::function<void()> func);
+
+ ///
+ /// \brief Removes node from the graph and all relevant edges
+ /// Returns false and fails if node is not present in graph
+ ///
+ bool removeNode(std::shared_ptr<TaskNode> node);
+
+ ///
+ /// \brief Removes node from the graph and all relevant edges.
+ /// Any incoming edges to the node are redirected to all its outputs
+ ///
+ bool removeNodeAndRedirect(std::shared_ptr<TaskNode> node);
+
+ ///
+ /// \brief newNode gets placed after refNode & added to the graph. newNode takes on refNode's outputs
+ ///
+ void insertAfter(std::shared_ptr<TaskNode> refNode, std::shared_ptr<TaskNode> newNode);
+
+ ///
+ /// \brief newNode gets placed before refNode & added to the graph. newNode takes on refNode's inputs
+ ///
+ void insertBefore(std::shared_ptr<TaskNode> refNode, std::shared_ptr<TaskNode> newNode);
+
+// Edge operations
+public:
+ ///
+ /// \brief Returns whether or not this graph contains the given directed edge
+ ///
+ bool containsEdge(std::shared_ptr<TaskNode> srcNode, std::shared_ptr<TaskNode> destNode);
+
+ ///
+ /// \brief Adds a directed edge to the graph (doesn't check if nodes are in graph)
+ ///
+ void addEdge(std::shared_ptr<TaskNode> srcNode, std::shared_ptr<TaskNode> destNode);
+
+ ///
+ /// \brief Attachs another TaskGraph as a subgraph (copies nodes and edges, then connects source->subgraph::source, subgraph::sink->sink),
+ /// source and sink must exist in this graph. Also serves as a graph sum between this and subgraph
+ ///
+ void nestGraph(std::shared_ptr<TaskGraph> subgraph, std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink);
+
+ ///
+ /// \brief Removes an edge from the graph (removes from adjList and invAdjList, cleans)
+ ///
+ void removeEdge(std::shared_ptr<TaskNode> srcNode, std::shared_ptr<TaskNode> destNode);
+
+ ///
+ /// \brief Returns true if srcNode reaches destNode
+ ///
+ bool isReachable(std::shared_ptr<TaskNode> srcNode, std::shared_ptr<TaskNode> destNode);
+
+public:
+ ///
+ /// \brief Removes all nodes & edges from the graph. Source and sink nodes maintained.
+ ///
+ void clear();
+
+ ///
+ /// \brief Removes all edges from the graph
+ ///
+ void clearEdges()
+ {
+ m_adjList.clear();
+ m_invAdjList.clear();
+ }
+
+// Graph algorithms, todo: Move into filtering module
+public:
+ ///
+ /// \brief Graph sum, shared references are considered identical nodes, source/sink of results invalidated/nullptr
+ ///
+ //static std::shared_ptr<TaskGraph> sum(std::shared_ptr<TaskGraph> graphA, std::shared_ptr<TaskGraph> graphB);
+
+ ///
+ /// \brief Topological sort of all nodes within graph
+ ///
+ static std::shared_ptr<TaskNodeList> topologicalSort(std::shared_ptr<TaskGraph> graph);
+
+ ///
+ /// \brief Makes sure no two *critical nodes* run at the same time by establishing an edge between them.
+ ///
+ static std::shared_ptr<TaskGraph> resolveCriticalNodes(std::shared_ptr<TaskGraph> graph);
+
+ ///
+ /// \brief Remove redudant edges. Removal is such that all vertices are still reachable and graph goes from source->sink
+ /// returns nullptr if failed. Only fails if graph is cyclic.
+ ///
+ static std::shared_ptr<TaskGraph> transitiveReduce(std::shared_ptr<TaskGraph> graph);
+
+ ///
+ /// \brief Removes nullptr/nonfunctional TaskNode's that don't split/join
+ ///
+ static std::shared_ptr<TaskGraph> removeRedundantNodes(std::shared_ptr<TaskGraph> graph);
+
+ ///
+ /// \brief Simplifies the graph in a way that retains functionality
+ /// Performs transitive reduction then redundant node removal to remove redundant nodes and edges
+ ///
+ static std::shared_ptr<TaskGraph> reduce(std::shared_ptr<TaskGraph> graph)
+ {
+ return removeRedundantNodes(transitiveReduce(graph));
+ }
+
+ ///
+ /// \brief Returns if Graph is cyclic or not
+ ///
+ static bool isCyclic(std::shared_ptr<TaskGraph> graph);
+
+ ///
+ /// \brief Nodes may not have unique names. Iterates the names with numeric postfix to generate uniques
+ ///
+ static std::unordered_map<std::shared_ptr<TaskNode>, std::string> getUniqueNodeNames(std::shared_ptr<TaskGraph> graph, bool apply = false);
+
+ ///
+ /// \brief Gets the completion times of each node (source = ~0s)
+ ///
+ static std::unordered_map<std::shared_ptr<TaskNode>, double> getNodeStartTimes(std::shared_ptr<TaskGraph> graph);
+
+ ///
+ /// \brief Computes the critical path
+ ///
+ static TaskNodeList getCriticalPath(std::shared_ptr<TaskGraph> graph);
+
+protected:
+ TaskNodeVector m_nodes;
+ TaskNodeAdjList m_adjList; ///> This gives the outputs of every node
+ TaskNodeAdjList m_invAdjList; ///> This gives the inputs of every node
+
+ std::shared_ptr<TaskNode> m_source = nullptr;
+ std::shared_ptr<TaskNode> m_sink = nullptr;
+};
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTaskGraphController.cpp b/Source/Common/TaskGraph/imstkTaskGraphController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..faa7d9b5729d4eb2c90407542d4a6be16b027d26
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTaskGraphController.cpp
@@ -0,0 +1,48 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#include "imstkTaskGraphController.h"
+#include "imstkTaskGraph.h"
+#include "imstkLogger.h"
+
+namespace imstk
+{
+bool
+TaskGraphController::initialize()
+{
+ // Ensure the source is reachable from the sink and the graph is not cyclic
+ // todo: Safer check would be to ensure all nodes reach sink
+ if (!m_graph->isReachable(m_graph->getSource(), m_graph->getSink()))
+ {
+ LOG(WARNING) << "TaskGraph Sink not reachable from source. Graph initialization failed.";
+ return false;
+ }
+
+ if (TaskGraph::isCyclic(m_graph))
+ {
+ LOG(WARNING) << "TaskGraph is cyclic. Graph initialization failed.";
+ return false;
+ }
+
+ init();
+ return true;
+}
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTaskGraphController.h b/Source/Common/TaskGraph/imstkTaskGraphController.h
new file mode 100644
index 0000000000000000000000000000000000000000..29786544b84eece69a16306c54f574f543666445
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTaskGraphController.h
@@ -0,0 +1,67 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include <memory>
+
+namespace imstk
+{
+class TaskGraph;
+
+///
+/// \class TaskGraphController
+///
+/// \brief Base class for TaskGraph controllers which are responsible for executing the TaskGraph
+///
+class TaskGraphController
+{
+public:
+ TaskGraphController() = default;
+ virtual ~TaskGraphController() = default;
+
+public:
+ virtual void setTaskGraph(std::shared_ptr<TaskGraph> graph) { this->m_graph = graph; }
+
+ std::shared_ptr<TaskGraph> getTaskGraph() const { return m_graph; }
+
+ ///
+ /// \brief Initialization of the TaskGraphController, good for anything the controller may need
+ /// to do after it recieves input, before execution, but not do everytime execution is called.
+ /// Returns if successful
+ ///
+ bool initialize();
+
+ ///
+ /// \brief Executes the TaskGraph
+ ///
+ virtual void execute() = 0;
+
+protected:
+ ///
+ /// \brief Subclass initialization call
+ ///
+ virtual void init() { }
+
+protected:
+ std::shared_ptr<TaskGraph> m_graph = nullptr;
+};
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTaskGraphVizWriter.cpp b/Source/Common/TaskGraph/imstkTaskGraphVizWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2bd010b468cb5e2e5361d7661209c010c3fee0d8
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTaskGraphVizWriter.cpp
@@ -0,0 +1,151 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#include "imstkTaskGraphVizWriter.h"
+#include "imstkLogger.h"
+#include "imstkTaskGraph.h"
+#include "imstkColor.h"
+#include <fstream>
+
+namespace imstk
+{
+void
+TaskGraphVizWriter::write()
+{
+ if (m_inputGraph == nullptr)
+ {
+ LOG(WARNING) << "No input set, unable to write ComputeGraph";
+ return;
+ }
+
+ // Compute range of compute times for color function
+ double maxTime = std::numeric_limits<double>::min();
+ if (m_writeNodeComputeTimesColor)
+ {
+ for (auto node : m_inputGraph->getNodes())
+ {
+ if (node->m_computeTime > maxTime)
+ {
+ maxTime = node->m_computeTime;
+ }
+ }
+ }
+
+ // Hardcoded color function
+ std::vector<Color> colorFunc = std::vector<Color>(3);
+ colorFunc[0] = Color::Blue;
+ colorFunc[1] = Color::Green;
+ colorFunc[2] = Color::Red;
+ const int colorFuncExtent = static_cast<int>(colorFunc.size() - 1);
+
+ // Compute the critical path (ie: longest path in duration)
+ TaskNodeList critPath;
+ if (m_highlightCriticalPath)
+ {
+ critPath = TaskGraph::getCriticalPath(m_inputGraph);
+ }
+ // Test if edge exists in critical path by linear searching
+ auto edgeExists = [&](const std::shared_ptr<TaskNode>& a, const std::shared_ptr<TaskNode>& b)
+ {
+ TaskNodeList::iterator srcNode = std::find(critPath.begin(), critPath.end(), a);
+ // If srcNode was found and the next node is b
+ return (srcNode != critPath.end() && *std::next(srcNode) == b);
+ };
+
+ // Write the file
+ {
+ std::ofstream file;
+ file.open(m_fileName);
+
+ if (!file.is_open() || file.fail())
+ {
+ return;
+ }
+
+ file <<
+ "digraph imstkDependency\n"
+ "{\n"
+ "style=filled;\n"
+ "color=lightgrey;\n"
+ "edge[arrowhead=vee, arrowtail=inv, arrowsize=.7, color=grey20];\n";
+
+ // Write the node section
+ const TaskNodeVector& nodes = m_inputGraph->getNodes();
+ std::unordered_map<std::shared_ptr<TaskNode>, std::string> nodeIds;
+ for (size_t i = 0; i < nodes.size(); i++)
+ {
+ const std::string nodeUniqueName = "node" + std::to_string(i);
+ nodeIds[nodes[i]] = nodeUniqueName;
+
+ file << "\"" << nodeUniqueName << "\" [";
+
+ // Write label property
+ if (m_writeNodeComputeTimesText)
+ {
+ file << " label=\"" << nodes[i]->m_name << " (" << nodes[i]->m_computeTime << "ms)" << "\"";
+ }
+ else
+ {
+ file << " label=\"" << nodes[i]->m_name << '\"';
+ }
+
+ // Write style property
+ file << " style=filled";
+
+ // Write color property
+ if (m_writeNodeComputeTimesColor)
+ {
+ const double t = nodes[i]->m_computeTime / maxTime;
+ const int i1 = static_cast<int>(t * colorFuncExtent);
+ const int i2 = std::min(colorFuncExtent, i1 + 1);
+ Color color = Color::lerpRgb(colorFunc[i1], colorFunc[i2], t);
+ file << " color=\"#" << color.rgbHex() << "\"";
+ }
+ else
+ {
+ file << " color=cornflowerblue";
+ }
+ file << "];" << std::endl;
+ }
+
+ // Write out all the edges
+ const TaskNodeAdjList& adjList = m_inputGraph->getAdjList();
+ for (TaskNodeAdjList::const_iterator it = adjList.begin(); it != adjList.end(); it++)
+ {
+ std::shared_ptr<TaskNode> srcNode = it->first;
+ const TaskNodeSet& outputNodes = it->second;
+ for (TaskNodeSet::const_iterator jt = outputNodes.begin(); jt != outputNodes.end(); jt++)
+ {
+ std::shared_ptr<TaskNode> destNode = *jt;
+ file << '\"' << nodeIds[srcNode] << '\"' << " -> " << '\"' << nodeIds[destNode] << '\"';
+
+ if (m_highlightCriticalPath && edgeExists(srcNode, destNode))
+ {
+ file << "[color=red]";
+ }
+ file << std::endl;
+ }
+ }
+ file << "}\n";
+ file.close();
+ }
+}
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTaskGraphVizWriter.h b/Source/Common/TaskGraph/imstkTaskGraphVizWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1f913b7f597b9926e35b7276ff3dc7082a61c313
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTaskGraphVizWriter.h
@@ -0,0 +1,87 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+namespace imstk
+{
+class TaskGraph;
+
+///
+/// \class TaskGraphVizWriter
+///
+/// \brief Writes a TaskGraph to an svg file. Produces unique node names from duplicates with postfix.
+/// Can also color by node compute time and highlight the critical path
+///
+class TaskGraphVizWriter
+{
+public:
+ TaskGraphVizWriter() = default;
+ virtual ~TaskGraphVizWriter() = default;
+
+public:
+ ///
+ /// \brief The graph to write
+ ///
+ void setInput(std::shared_ptr<TaskGraph> graph) { this->m_inputGraph = graph; }
+
+ ///
+ /// \brief The filename and path to write too
+ ///
+ void setFileName(std::string fileName) { this->m_fileName = fileName; }
+
+ ///
+ /// \brief If on, will highlight the critical path in red
+ ///
+ void setHighlightCriticalPath(bool highlightCriticalPath) { this->m_highlightCriticalPath = highlightCriticalPath; }
+
+ ///
+ /// \brief If on, will write the time the node took to complete as a color
+ ///
+ void setWriteNodeComputeTimesColor(bool writeNodeComputeTimesColor) { this->m_writeNodeComputeTimesColor = writeNodeComputeTimesColor; }
+
+ ///
+ /// \brief If on, will write the time the node took to complete in name as text
+ ///
+ void setWriteNodeComputeTimesText(bool writeNodeComputeTimesText) { this->m_writeNodeComputeTimesText = writeNodeComputeTimesText; }
+
+ std::shared_ptr<TaskGraph> getInput() const { return m_inputGraph; }
+ const std::string& getFileName() const { return m_fileName; }
+ bool getHighlightCriticalPath() const { return m_highlightCriticalPath; }
+ bool getWriteNodeComputeTimesColor() const { return m_writeNodeComputeTimesColor; }
+ bool getWriteNodeComputeTimesText() const { return m_writeNodeComputeTimesText; }
+
+ ///
+ /// \brief Writes the graph to a file given the filename
+ ///
+ void write();
+
+private:
+ std::shared_ptr<TaskGraph> m_inputGraph = nullptr;
+ std::string m_fileName = "";
+ bool m_highlightCriticalPath = false;
+ bool m_writeNodeComputeTimesColor = false;
+ bool m_writeNodeComputeTimesText = false;
+};
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTaskNode.cpp b/Source/Common/TaskGraph/imstkTaskNode.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e401244ed3a842c91f8176c1b134d0e0476c54c5
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTaskNode.cpp
@@ -0,0 +1,50 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#include "imstkTaskNode.h"
+#include "imstkTimer.h"
+
+namespace imstk
+{
+void
+TaskNode::execute()
+{
+ if (m_enabled && m_func != nullptr)
+ {
+ if (!m_enableTiming)
+ {
+ m_func();
+ }
+ else
+ {
+ StopWatch timer;
+ timer.start();
+ m_func();
+ m_computeTime = timer.getTimeElapsed();
+ timer.stop();
+ }
+ }
+ else
+ {
+ m_computeTime = 0.0;
+ }
+}
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTaskNode.h b/Source/Common/TaskGraph/imstkTaskNode.h
new file mode 100644
index 0000000000000000000000000000000000000000..10bdccb27e74e95685e4c6f8fcab976febf36ecb
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTaskNode.h
@@ -0,0 +1,69 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include <functional>
+#include <string>
+
+namespace imstk
+{
+///
+/// \class TaskNode
+///
+/// \brief Base class for TaskGraph nodes
+///
+class TaskNode
+{
+public:
+ TaskNode() = default;
+ TaskNode(std::function<void()> func, std::string name = "none", bool isCritical = false) :
+ m_func(func), m_name(name), m_isCritical(isCritical), m_computeTime(0.0)
+ {
+ }
+
+ virtual ~TaskNode() = default;
+
+public:
+ void setFunction(std::function<void()> func) { this->m_func = func; }
+ void setEnabled(bool enabled) { this->m_enabled = enabled; }
+
+ ///
+ /// \brief Returns true if function is nullptr
+ ///
+ bool isFunctional() const { return m_func != nullptr; }
+
+ ///
+ /// \brief Calls the function pointer provided if node enabled
+ ///
+ virtual void execute();
+
+public:
+ std::string m_name = "none";
+ bool m_enabled = true;
+ bool m_isCritical = false;
+ double m_computeTime = 0.0;
+ bool m_enableTiming = false;
+
+protected:
+ std::function<void()> m_func = nullptr; ///> Don't allow user to call directly (must use execute)
+};
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTbbTaskGraphController.cpp b/Source/Common/TaskGraph/imstkTbbTaskGraphController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..15581278657cd1de25eca99c46b5d7accbf8fec6
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTbbTaskGraphController.cpp
@@ -0,0 +1,103 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkTbbTaskGraphController.h"
+#include "imstkTaskGraph.h"
+#include <tbb/tbb.h>
+
+namespace imstk
+{
+class NodeTbbTask : public tbb::task
+{
+public:
+ NodeTbbTask(std::shared_ptr<TaskNode> node) : m_node(node) { }
+
+public:
+ task* execute() override
+ {
+ __TBB_ASSERT(ref_count() == 0, NULL);
+
+ m_node->execute();
+
+ for (size_t i = 0; i < successors.size(); i++)
+ {
+ if (successors[i]->decrement_ref_count() == 0)
+ {
+ spawn(*successors[i]);
+ }
+ }
+ return NULL;
+ }
+
+public:
+ std::shared_ptr<TaskNode> m_node = nullptr;
+ std::vector<NodeTbbTask*> successors;
+};
+
+void
+TbbTaskGraphController::execute()
+{
+ // Create a Task for every node
+ const TaskNodeVector& nodes = m_graph->getNodes();
+ if (nodes.size() == 0)
+ {
+ return;
+ }
+
+ // Create a task for every node
+ std::unordered_map<std::shared_ptr<TaskNode>, NodeTbbTask*> tasks;
+ tasks.reserve(nodes.size());
+
+ for (size_t i = 0; i < nodes.size(); i++)
+ {
+ std::shared_ptr<TaskNode> node = nodes[i];
+ tasks[node] = new (tbb::task::allocate_root())NodeTbbTask(node);
+ }
+ // Increment successor reference counts
+ const TaskNodeAdjList& adjList = m_graph->getAdjList();
+ // For every node in graph
+ for (size_t i = 0; i < nodes.size(); i++)
+ {
+ // If it contains outputs
+ if (adjList.count(nodes[i]) != 0)
+ {
+ // For every output
+ const TaskNodeSet& outputNodes = adjList.at(nodes[i]);
+ for (TaskNodeSet::const_iterator it = outputNodes.begin(); it != outputNodes.end(); it++)
+ {
+ // Lookup the task of the node
+ NodeTbbTask* successor = tasks[*it];
+ tasks[nodes[i]]->successors.push_back(successor);
+ successor->increment_ref_count();
+ }
+ }
+ }
+
+ NodeTbbTask* startTask = tasks[m_graph->getSource()];
+ NodeTbbTask* finalTask = tasks[m_graph->getSink()];
+
+ // Extra ref count on the final task
+ finalTask->increment_ref_count();
+ finalTask->spawn_and_wait_for_all(*startTask);
+ finalTask->execute(); // Execute final task explicitly
+ tbb::task::destroy(*finalTask);
+}
+}
\ No newline at end of file
diff --git a/Source/Common/TaskGraph/imstkTbbTaskGraphController.h b/Source/Common/TaskGraph/imstkTbbTaskGraphController.h
new file mode 100644
index 0000000000000000000000000000000000000000..d17155958d75ef681ab7fcb371fbf1e183fb20ab
--- /dev/null
+++ b/Source/Common/TaskGraph/imstkTbbTaskGraphController.h
@@ -0,0 +1,38 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkTaskGraphController.h"
+
+namespace imstk
+{
+///
+/// \class TbbTaskGraphController
+///
+/// \brief This class runs an input TaskGraph in parallel using tbb tasks
+///
+class TbbTaskGraphController : public TaskGraphController
+{
+public:
+ void execute() override;
+};
+};
\ No newline at end of file
diff --git a/Source/Common/imstkColor.cpp b/Source/Common/imstkColor.cpp
index 8a8cc79284da498befb60c8693d2a000eee12497..1bd6848732073986ab915ea04c6c6084d97f9f81 100644
--- a/Source/Common/imstkColor.cpp
+++ b/Source/Common/imstkColor.cpp
@@ -20,8 +20,9 @@
=========================================================================*/
#include "imstkColor.h"
-
-#include <g3log/g3log.hpp>
+#include "imstkLogger.h"
+#include <iomanip>
+#include <sstream>
namespace imstk
{
@@ -166,6 +167,19 @@ Color::lerpRgb(const Color& start, const Color& end, const double t)
return Color(start + (end - start) * t, 1.0);
}
+std::string
+Color::rgbHex()
+{
+ std::stringstream ss;
+ const int red = static_cast<int>(r * 255.0);
+ const int green = static_cast<int>(g * 255.0);
+ const int blue = static_cast<int>(b * 255.0);
+ ss << std::setfill('0') << std::setw(2) << std::hex << red;
+ ss << std::setfill('0') << std::setw(2) << std::hex << green;
+ ss << std::setfill('0') << std::setw(2) << std::hex << blue;
+ return ss.str();
+}
+
Color
operator*(const Color& color_lhs, const Color& color_rhs)
{
diff --git a/Source/Common/imstkColor.h b/Source/Common/imstkColor.h
index 7b7ca429731a51690be6df8b15094f4eb1833933..052519e51e4f58ef5251204d641517b3fbd82100 100644
--- a/Source/Common/imstkColor.h
+++ b/Source/Common/imstkColor.h
@@ -101,6 +101,8 @@ struct Color
///
const double* getValue() const;
+ std::string rgbHex();
+
///
/// \brief interpolate between two colors by ratio t
///
diff --git a/Source/Constraint/CMakeLists.txt b/Source/Constraint/CMakeLists.txt
index aab16ab80c97ec608d61a539b678e2b7b4f9b759..df474dc4b0d6e0c0fc3f540bd351fe3b70751061 100644
--- a/Source/Constraint/CMakeLists.txt
+++ b/Source/Constraint/CMakeLists.txt
@@ -3,8 +3,11 @@
#-----------------------------------------------------------------------------
include(imstkAddLibrary)
imstk_add_library( Constraints
+ SUBDIR_LIST
+ PbdConstraints
DEPENDS
Common
+ DataStructures
Geometry
)
diff --git a/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6121b15e54634b7d15e52f26976341c327a21ed0
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.cpp
@@ -0,0 +1,81 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkPbdAreaConstraint.h"
+
+namespace imstk
+{
+void
+PbdAreaConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const size_t& pIdx2,
+ const double k)
+{
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
+
+ this->m_stiffness = k;
+ this->m_compliance = 1.0 / k;
+
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
+
+ m_restArea = 0.5 * (p1 - p0).cross(p2 - p0).norm();
+}
+
+bool
+PbdAreaConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
+{
+ const auto i1 = m_vertexIds[0];
+ const auto i2 = m_vertexIds[1];
+ const auto i3 = m_vertexIds[2];
+
+ const Vec3d& p0 = currVertexPositions[i1];
+ const Vec3d& p1 = currVertexPositions[i2];
+ const Vec3d& p2 = currVertexPositions[i3];
+
+ const Vec3d e0 = p0 - p1;
+ const Vec3d e1 = p1 - p2;
+ const Vec3d e2 = p2 - p0;
+
+ Vec3d n = e0.cross(e1);
+ c = 0.5 * n.norm();
+
+ if (c < m_epsilon)
+ {
+ return false;
+ }
+
+ n /= 2 * c;
+ c -= m_restArea;
+
+ dcdx[0] = e1.cross(n);
+ dcdx[1] = e2.cross(n);
+ dcdx[2] = e0.cross(n);
+
+ return true;
+}
+}
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdAreaConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h
similarity index 74%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdAreaConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h
index 02d5099dbc31eea0da2ac13e1c06d9d4a47c67bb..94c7d57b9f82ba56d02a4eeabf6c7cdcbd0ec595 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdAreaConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h
@@ -46,19 +46,16 @@ public:
///
/// \brief Initializes the area constraint
///
- void initConstraint(PbdModel& model,
- const size_t& pIdx1,
- const size_t& pIdx2,
- const size_t& pIdx3,
- const double k = 2.5);
+ void initConstraint(
+ const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx1, const size_t& pIdx2, const size_t& pIdx3,
+ const double k = 2.5);
- ///
- /// \brief Solves the area constraint
- ///
- bool solvePositionConstraint(PbdModel& model) override;
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
- double m_restArea = 0.; ///> Area at the rest position
- double m_stiffness = 0.; ///> Stiffness of the area constraint
+ double m_restArea = 0.; ///> Area at the rest position
};
} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9bb0315a17ef4617bd539746ca0feeda88b6947f
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.cpp
@@ -0,0 +1,82 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkPbdBendConstraint.h"
+
+namespace imstk
+{
+void
+PbdBendConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const size_t& pIdx2,
+ const double k)
+{
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
+
+ m_stiffness = k;
+ m_compliance = 1.0 / k;
+
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
+
+ // Instead of using the angle between the segments we can use the distance
+ // from the center of the triangle
+ const Vec3d& center = (p0 + p1 + p2) / 3.0;
+ m_restLength = (p1 - center).norm();
+}
+
+bool
+PbdBendConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
+{
+ const size_t i0 = m_vertexIds[0];
+ const size_t i1 = m_vertexIds[1];
+ const size_t i2 = m_vertexIds[2];
+
+ const Vec3d& p0 = currVertexPositions[i0];
+ const Vec3d& p1 = currVertexPositions[i1];
+ const Vec3d& p2 = currVertexPositions[i2];
+
+ // Move towards triangle center
+ const Vec3d& center = (p0 + p1 + p2) / 3.0;
+ const Vec3d& diff = p1 - center;
+ const double dist = diff.norm();
+
+ if (dist < m_epsilon)
+ {
+ return false;
+ }
+
+ c = dist - m_restLength;
+
+ dcdx.resize(3);
+ dcdx[0] = (-2.0 / dist) * diff;
+ dcdx[1] = -2.0 * dcdx[0];
+ dcdx[2] = dcdx[0];
+
+ return true;
+}
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdBendConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h
similarity index 77%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdBendConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h
index c51f9efc11d98f8a0385e49c627177ad11e0c6e0..c2e68dc2f8702512a4f14127834fe361994831f2 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdBendConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h
@@ -53,22 +53,23 @@ public:
/
p2
\param model
- \param pIdx1 index of p0
+ \param pIdx0 index of p0
\param pIdx2 index of p1
\param pIdx3 index of p2
\param k stiffness
*/
- void initConstraint(PbdModel& model,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const double k);
+ void initConstraint(
+ const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx1, const size_t& pIdx2,
+ const size_t& pIdx3, const double k);
///
- /// \brief Solves the bend constraint
+ /// \brief Compute the value and gradient of constraint
///
- bool solvePositionConstraint(PbdModel& model) override;
-
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPosition,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
double m_restLength = 0.; ///> Rest length
- double m_stiffness = 0.; ///> Bend stiffness
};
} //imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dda274ce9101a8d8b14404fd0ee0bd6062378fa5
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp
@@ -0,0 +1,83 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkPbdCollisionConstraint.h"
+#include "imstkLogger.h"
+
+namespace imstk
+{
+PbdCollisionConstraint::PbdCollisionConstraint(const unsigned int& n1, const unsigned int& n2)
+{
+ m_bodiesFirst.resize(n1);
+ m_bodiesSecond.resize(n2);
+}
+
+void
+PbdCollisionConstraint::projectConstraint(const StdVectorOfReal& invMassA,
+ const StdVectorOfReal& invMassB,
+ StdVectorOfVec3d& posA,
+ StdVectorOfVec3d& posB)
+{
+ double c;
+ StdVectorOfVec3d dcdxA;
+ StdVectorOfVec3d dcdxB;
+
+ bool update = this->computeValueAndGradient(posA, posB, c, dcdxA, dcdxB);
+ if (!update)
+ {
+ return;
+ }
+
+ double lambda = 0.0;
+
+ for (size_t i = 0; i < m_bodiesFirst.size(); ++i)
+ {
+ lambda += invMassA[m_bodiesFirst[i]] * dcdxA[i].squaredNorm();
+ }
+
+ for (size_t i = 0; i < m_bodiesSecond.size(); ++i)
+ {
+ lambda += invMassB[m_bodiesSecond[i]] * dcdxB[i].squaredNorm();
+ }
+
+ lambda = c / lambda;
+
+ for (size_t i = 0, vid = 0; i < m_bodiesFirst.size(); ++i)
+ {
+ vid = m_bodiesFirst[i];
+ if (invMassA[vid] > 0.0)
+ {
+ posA[vid] -= invMassA[vid] * lambda * dcdxA[i] * m_configA->m_stiffness;
+ }
+ }
+
+ for (size_t i = 0, vid = 0; i < m_bodiesSecond.size(); ++i)
+ {
+ vid = m_bodiesSecond[i];
+ if (invMassB[vid] > 0.0)
+ {
+ posB[vid] -= invMassB[vid] * lambda * dcdxB[i] * m_configB->m_stiffness;
+ }
+ }
+
+ return;
+}
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..bd5270f7224acfaa8362fa49001b2e5bac916a5a
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h
@@ -0,0 +1,103 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkMath.h"
+
+namespace imstk
+{
+///
+/// \struct PbdCollisionConstraintConfig
+/// \brief Parameters for PBD collision constraints
+///
+struct PbdCollisionConstraintConfig
+{
+ PbdCollisionConstraintConfig(double proximity, double stiffness) : m_proximity(proximity), m_stiffness(stiffness) { }
+
+ double m_proximity = 0.1; ///> Proximity for static collision
+ double m_stiffness = 1.0; ///> Stiffness for collision
+};
+
+///
+/// \class PbdCollisionConstraint
+///
+/// \brief
+///
+class PbdCollisionConstraint
+{
+public:
+ enum class Type
+ {
+ EdgeEdge,
+ PointTriangle
+ };
+
+ ///
+ /// \brief
+ ///
+ PbdCollisionConstraint(const unsigned int& nA, const unsigned int& nB);
+
+ ///
+ /// \brief Destructor
+ ///
+ virtual ~PbdCollisionConstraint() = default;
+
+ ///
+ /// \brief Get vertex indices of first object
+ ///
+ const std::vector<size_t>& getVertexIdsFirst() const { return m_bodiesFirst; }
+ const std::vector<size_t>& getVertexIdsSecond() const { return m_bodiesSecond; }
+
+ ///
+ /// \brief Get config
+ ///
+ const PbdCollisionConstraintConfig& getConfigFirst() const { return *m_configA; }
+ const PbdCollisionConstraintConfig& getConfigSecond() const { return *m_configB; }
+
+ ///
+ /// \brief compute value and gradient of constraint function
+ ///
+ /// \param[in] currVertexPositionsA current positions from object A
+ /// \param[in] currVertexPositionsA current positions from object B
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
+ ///
+ virtual bool computeValueAndGradient(const StdVectorOfVec3d& posA,
+ const StdVectorOfVec3d& posB,
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const = 0;
+
+ virtual void projectConstraint(const StdVectorOfReal& invMassA,
+ const StdVectorOfReal& invMassB,
+ StdVectorOfVec3d& posA,
+ StdVectorOfVec3d& posB);
+protected:
+ std::vector<size_t> m_bodiesFirst; ///> index of points for the first object
+ std::vector<size_t> m_bodiesSecond; ///> index of points for the second object
+
+ std::shared_ptr<PbdCollisionConstraintConfig> m_configA = nullptr; ///> parameters of the collision constraint
+ std::shared_ptr<PbdCollisionConstraintConfig> m_configB = nullptr; ///> parameters of the collision constraint
+};
+
+using PBDCollisionConstraintVector = std::vector<PbdCollisionConstraint*>;
+}
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdConstantDensityConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
similarity index 82%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
rename to Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
index 8875e63f5727cb082f25c449c579bec1bcbe9d99..03fe3fd841019825d024481ad38f58ffb441640d 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
@@ -20,16 +20,14 @@ limitations under the License.
=========================================================================*/
#include "imstkPbdConstantDensityConstraint.h"
-#include "imstkPbdModel.h"
#include "imstkParallelUtils.h"
namespace imstk
{
void
-PbdConstantDensityConstraint::initConstraint(PbdModel& model, const double)
+PbdConstantDensityConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions, const double)
{
- const auto& state = model.getCurrentState();
- const auto numParticles = state->getPositions().size();
+ const size_t numParticles = initVertexPositions.size();
// constraint parameters
// Refer: Miller, et al 2003 "Particle-Based Fluid Simulation for Interactive Applications."
@@ -46,32 +44,31 @@ PbdConstantDensityConstraint::initConstraint(PbdModel& model, const double)
m_NeighborSearcher = std::make_shared<NeighborSearch>(m_NeighborSearchMethod, m_maxDist);
}
-bool
-PbdConstantDensityConstraint::solvePositionConstraint(PbdModel& model)
+void
+PbdConstantDensityConstraint::projectConstraint(const StdVectorOfReal& currInvMasses,
+ const double dt,
+ const PbdConstraint::SolverType& type,
+ StdVectorOfVec3d& currVertexPositions)
{
- const auto& state = model.getCurrentState();
- auto& positions = state->getPositions();
- const auto numParticles = positions.size();
+ const size_t numParticles = currVertexPositions.size();
// Search neighbor for each particle
- m_NeighborSearcher->getNeighbors(m_neighborList, positions);
+ m_NeighborSearcher->getNeighbors(m_neighborList, currVertexPositions);
ParallelUtils::parallelFor(numParticles,
[&](const size_t idx) {
- computeDensity(positions[idx], idx, positions);
+ computeDensity(currVertexPositions[idx], idx, currVertexPositions);
});
ParallelUtils::parallelFor(numParticles,
[&](const size_t idx) {
- computeLambdaScalingFactor(positions[idx], idx, positions);
+ computeLambdaScalingFactor(currVertexPositions[idx], idx, currVertexPositions);
});
ParallelUtils::parallelFor(numParticles,
[&](const size_t idx) {
- updatePositions(positions[idx], idx, positions);
+ updatePositions(currVertexPositions[idx], idx, currVertexPositions);
});
-
- return true;
}
double
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdConstantDensityConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h
similarity index 86%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdConstantDensityConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h
index 9c5bccc7952b47b4b5859817d4aa025f53ad14ef..81f97b656f2a54f98b8c07ad9640af60e30f50e6 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdConstantDensityConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h
@@ -49,7 +49,7 @@ public:
///
/// \Constant Density Constraint Initialization
///
- void initConstraint(PbdModel& model, const double k);
+ void initConstraint(const StdVectorOfVec3d& initVertexPositions, const double k);
///
/// \brief Returns PBD constraint of type Type::ConstantDensity
@@ -59,7 +59,17 @@ public:
///
/// \brief Solves the constant density constraint
///
- bool solvePositionConstraint(PbdModel& model) override;
+ void projectConstraint(const StdVectorOfReal& currInvMasses,
+ const double dt,
+ const PbdConstraint::SolverType& type,
+ StdVectorOfVec3d& currVertexPositions) override;
+
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
+ {
+ return true;
+ }
private:
///
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..443c2a17ad140350c8ed738534e314f6a40b1d09
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp
@@ -0,0 +1,79 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+void
+PbdConstraint::projectConstraint(const StdVectorOfReal& invMasses, const double dt, const SolverType& solverType, StdVectorOfVec3d& pos)
+{
+ double c;
+ StdVectorOfVec3d dcdx;
+
+ bool update = this->computeValueAndGradient(pos, c, dcdx);
+ if (!update)
+ {
+ return;
+ }
+
+ double dcMidc = 0.0;
+ double lambda = 0.0;
+ double alpha;
+
+ for (size_t i = 0; i < m_vertexIds.size(); ++i)
+ {
+ dcMidc += invMasses[m_vertexIds[i]] * dcdx[i].squaredNorm();
+ }
+
+ if (dcMidc < VERY_SMALL_EPSILON)
+ {
+ return;
+ }
+
+ switch (solverType)
+ {
+ case (SolverType::xPBD):
+ alpha = m_compliance / (dt * dt);
+ lambda = -(c + alpha * m_lambda) / (dcMidc + alpha);
+ m_lambda += lambda;
+ break;
+ case (SolverType::PBD):
+ lambda = -c * m_stiffness / dcMidc;
+ break;
+ default:
+ alpha = m_compliance / (dt * dt);
+ lambda = -(c + alpha * m_lambda) / (dcMidc + alpha);
+ m_lambda += lambda;
+ }
+
+ for (size_t i = 0, vid = 0; i < m_vertexIds.size(); ++i)
+ {
+ vid = m_vertexIds[i];
+ if (invMasses[vid] > 0.0)
+ {
+ pos[vid] += invMasses[vid] * lambda * dcdx[i];
+ }
+ }
+
+ return;
+}
+}
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
similarity index 54%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdConstraint.h
index 37e96681c01879a3be04de0c4ab322c798622772..316ae0309a05d1808ab0022ab33ab88b65b23ad6 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
@@ -21,12 +21,10 @@
#pragma once
-#include <vector>
+#include "imstkMath.h"
namespace imstk
{
-class PbdModel;
-
///
/// \class PbdConstraint
///
@@ -51,6 +49,14 @@ public:
None
};
+ ///
+ /// \brief Type of solvers
+ enum class SolverType
+ {
+ xPBD = 0,
+ PBD,
+ GCD
+ };
///
/// \brief Constructor
///
@@ -68,11 +74,15 @@ public:
virtual Type getType() const = 0;
///
- /// \brief compute delta position from the constraint function
- /// \param model \class PbdModel
- /// \return true if succeeded
+ /// \brief Compute value and gradient of the constraint
+ ///
+ /// \param[in] currVertexPositions vector of current positions
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
///
- virtual bool solvePositionConstraint(PbdModel& model) = 0;
+ virtual bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const = 0;
///
/// \brief Get the vertex indices of the constraint
@@ -84,8 +94,41 @@ public:
///
void setTolerance(const double eps) { m_epsilon = eps; }
+ ///
+ /// \brief Get the tolerance used for pbd constraints
+ ///
+ double getTolerance() const { return m_epsilon; }
+
+ ///
+ /// \brief Set the stiffness
+ ///
+ void setStiffness(const double stiffness)
+ {
+ m_stiffness = stiffness;
+ m_compliance = 1.0 / stiffness;
+ }
+
+ ///
+ /// \brief Get the stiffness
+ ///
+ double getStiffness() const { return m_stiffness; }
+
+ ///
+ /// \brief Use PBD
+ ///
+ void zeroOutLambda() { m_lambda = 0.0; }
+
+ ///
+ /// \brief Update positions by projecting constraints.
+ ///
+ virtual void projectConstraint(const StdVectorOfReal& currInvMasses, const double dt, const SolverType& type, StdVectorOfVec3d& pos);
protected:
std::vector<size_t> m_vertexIds; ///> index of points for the constraint
- double m_epsilon = 1.0e-16; ///> Tolerance used for the costraints
+ double m_epsilon = 1.0e-16; ///> Tolerance used for the costraints
+ double m_stiffness = 1.0; ///> used in PBD, [0, 1]
+ double m_compliance = 1e-7; ///> used in xPBD, inverse of Young's Modulus
+ mutable double m_lambda = 0.0; ///> Lagrange multiplier
};
+
+using PBDConstraintVector = std::vector<std::shared_ptr<PbdConstraint>>;
}
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d45e2edd87341aa2e7d24fe589cef5e3411168b4
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp
@@ -0,0 +1,98 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkPbdDihedralConstraint.h"
+#include <iostream>
+
+namespace imstk
+{
+void
+PbdDihedralConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const size_t& pIdx2,
+ const size_t& pIdx3,
+ const double k)
+{
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
+ m_vertexIds[3] = pIdx3;
+
+ m_stiffness = k;
+ m_compliance = 1.0 / k;
+
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
+ const Vec3d& p3 = initVertexPositions[pIdx3];
+
+ const Vec3d n1 = (p2 - p0).cross(p3 - p0).normalized();
+ const Vec3d n2 = (p3 - p1).cross(p2 - p1).normalized();
+
+ m_restAngle = atan2(n1.cross(n2).dot(p3 - p2), (p3 - p2).norm() * n1.dot(n2));
+}
+
+bool
+PbdDihedralConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
+{
+ const auto i0 = m_vertexIds[0];
+ const auto i1 = m_vertexIds[1];
+ const auto i2 = m_vertexIds[2];
+ const auto i3 = m_vertexIds[3];
+
+ const Vec3d& p0 = currVertexPositions[i0];
+ const Vec3d& p1 = currVertexPositions[i1];
+ const Vec3d& p2 = currVertexPositions[i2];
+ const Vec3d& p3 = currVertexPositions[i3];
+
+ const auto e = p3 - p2;
+ const auto e1 = p3 - p0;
+ const auto e2 = p0 - p2;
+ const auto e3 = p3 - p1;
+ const auto e4 = p1 - p2;
+ // To accelerate, all normal (area) vectors and edge length should be precomputed in parallel
+ auto n1 = e1.cross(e);
+ auto n2 = e.cross(e3);
+ const auto A1 = n1.norm();
+ const auto A2 = n2.norm();
+ n1 /= A1;
+ n2 /= A2;
+
+ const double l = e.norm();
+ if (l < m_epsilon)
+ {
+ return false;
+ }
+
+ dcdx.resize(4);
+ dcdx[0] = -(l / A1) * n1;
+ dcdx[1] = -(l / A2) * n2;
+ dcdx[2] = (e.dot(e1) / (A1 * l)) * n1 + (e.dot(e3) / (A2 * l)) * n2;
+ dcdx[3] = (e.dot(e2) / (A1 * l)) * n1 + (e.dot(e4) / (A2 * l)) * n2;
+
+ c = atan2(n1.cross(n2).dot(e), l * n1.dot(n2)) - m_restAngle;
+
+ return true;
+}
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDihedralConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h
similarity index 71%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDihedralConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h
index 500b6f6f64e92f8369990f8d986699bf65345cc5..98ad174c203726cf03ff14263504481b79aa5ca4 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDihedralConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h
@@ -59,18 +59,24 @@ public:
\param pIdx4 index of p3
\param k stiffness
*/
- void initConstraint(PbdModel& model,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4,
- const double k);
+ void initConstraint(
+ const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0, const size_t& pIdx1,
+ const size_t& pIdx2, const size_t& pIdx3,
+ const double k);
///
- /// \brief Solves the dihedral angular constraint
+ /// \brief Compute value and gradient of the constraint
///
- bool solvePositionConstraint(PbdModel& model) override;
+ /// \param[in] currVertexPositions vector of current positions
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
+ ///
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
- double m_restAngle = 0.; ///> Rest angle
- double m_stiffness = 0.; ///> Angular stiffness
+ double m_restAngle = 0.0; ///> Rest angle
};
} //imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0815882793e062cb43330b643a06f8264a4bf77
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp
@@ -0,0 +1,60 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkPbdDistanceConstraint.h"
+
+namespace imstk
+{
+void
+PbdDistanceConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const double k)
+{
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_stiffness = k;
+ m_compliance = 1.0 / k;
+
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+
+ m_restLength = (p0 - p1).norm();
+}
+
+bool
+PbdDistanceConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
+{
+ const Vec3d& p0 = currVertexPositions[m_vertexIds[0]];
+ const Vec3d& p1 = currVertexPositions[m_vertexIds[1]];
+ dcdx.resize(m_vertexIds.size());
+
+ dcdx[0] = p0 - p1;
+ const double len = dcdx[0].norm();
+ dcdx[0] /= len;
+ dcdx[1] = -dcdx[0];
+ c = len - m_restLength;
+
+ return true;
+}
+}
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDistanceConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h
similarity index 72%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDistanceConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h
index 4d4c2f706abf4e17d12a902104afbeb25e12e2df..12a7516718723d43461778b97f7ef90b6c3be1b5 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDistanceConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h
@@ -46,16 +46,16 @@ public:
///
/// \brief Initializes the distance constraint
///
- void initConstraint(PbdModel& model, const size_t& pIdx1,
- const size_t& pIdx2, const double k = 1e-1);
+ void initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const double k = 1e5);
- ///
- /// \brief Solves the Distance constraint
- ///
- bool solvePositionConstraint(PbdModel& model) override;
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
- double m_restLength = 0.; ///> Rest length between the nodes
- double m_stiffness = 0.; ///> Stiffness of the constaint
+ double m_restLength = 0.0; ///> Rest length between the nodes
};
-} // imstk
\ No newline at end of file
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..85986e9963e3690f7977c2f6e1854dd387c8b30f
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
@@ -0,0 +1,111 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkPbdEdgeEdgeCollisionConstraint.h"
+#include "imstkLogger.h"
+
+namespace imstk
+{
+void
+PbdEdgeEdgeConstraint::initConstraint(const size_t& pIdxA1,
+ const size_t& pIdxA2,
+ const size_t& pIdxB1,
+ const size_t& pIdxB2,
+ std::shared_ptr<PbdCollisionConstraintConfig> configA,
+ std::shared_ptr<PbdCollisionConstraintConfig> configB)
+{
+ m_configA = configA;
+ m_configB = configB;
+ m_bodiesFirst[0] = pIdxA1;
+ m_bodiesFirst[1] = pIdxA2;
+ m_bodiesSecond[0] = pIdxB1;
+ m_bodiesSecond[1] = pIdxB2;
+}
+
+bool
+PbdEdgeEdgeConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
+ const StdVectorOfVec3d& currVertexPositionsB,
+ double& cc,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const
+{
+ const auto i0 = m_bodiesFirst[0];
+ const auto i1 = m_bodiesFirst[1];
+ const auto i2 = m_bodiesSecond[0];
+ const auto i3 = m_bodiesSecond[1];
+
+ const Vec3d& x0 = currVertexPositionsA[i0];
+ const Vec3d& x1 = currVertexPositionsA[i1];
+ const Vec3d& x2 = currVertexPositionsB[i2];
+ const Vec3d& x3 = currVertexPositionsB[i3];
+
+ auto a = (x3 - x2).dot(x1 - x0);
+ auto b = (x1 - x0).dot(x1 - x0);
+ auto c = (x0 - x2).dot(x1 - x0);
+ auto d = (x3 - x2).dot(x3 - x2);
+ auto e = a;
+ auto f = (x0 - x2).dot(x3 - x2);
+
+ auto det = a * e - d * b;
+ double s = 0.5;
+ double t = 0.5;
+ if (fabs(det) > 1e-12)
+ {
+ s = (c * e - b * f) / det;
+ t = (c * d - a * f) / det;
+ if (s < 0 || s > 1.0 || t < 0 || t > 1.0)
+ {
+ c = 0.0;
+ return false;
+ }
+ }
+ else
+ {
+ //LOG(WARNING) << "det is null";
+ }
+
+ Vec3d P = x0 + t * (x1 - x0);
+ Vec3d Q = x2 + s * (x3 - x2);
+
+ Vec3d n = Q - P;
+ auto l = n.norm();
+ n /= l;
+
+ const auto dist = m_configA->m_proximity + m_configB->m_proximity;
+
+ if (l > dist)
+ {
+ c = 0.0;
+ return false;
+ }
+
+ dcdxA.resize(2);
+ dcdxB.resize(2);
+ dcdxA[0] = -(1 - t) * n;
+ dcdxA[1] = -(t) * n;
+ dcdxB[0] = (1 - s) * n;
+ dcdxB[1] = (s) * n;
+
+ cc = l - dist;
+
+ return true;
+}
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
similarity index 63%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
index 967c31df18cf163c2094b77c338aaa41c2bf1c03..e07dc618c39aa32d3a2e6f091ec39fe0d0d291b4 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
@@ -48,14 +48,24 @@ public:
/// \param pIdx4 second point of the edge from object2
/// \return true if succeeded
///
- void initConstraint(std::shared_ptr<PbdModel> model1,
- const size_t& pIdx1, const size_t& pIdx2,
- std::shared_ptr<PbdModel> model2,
- const size_t& pIdx3, const size_t& pIdx4);
+ void initConstraint(
+ const size_t& pIdxA1, const size_t& pIdxA2,
+ const size_t& pIdxB1, const size_t& pIdxB2,
+ std::shared_ptr<PbdCollisionConstraintConfig> configA,
+ std::shared_ptr<PbdCollisionConstraintConfig> configB);
///
- /// \brief Solve edge-edge collision constraint
+ /// \brief compute value and gradient of constraint function
///
- bool solvePositionConstraint() override;
+ /// \param[in] currVertexPositionsA current positions from object A
+ /// \param[in] currVertexPositionsA current positions from object B
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
+ ///
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
+ const StdVectorOfVec3d& currVertexPositionsB,
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const override;
};
-}
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFEMConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.cpp
similarity index 99%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFEMConstraint.cpp
rename to Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.cpp
index f9acfad1d5e7291c29bba6812e08ec843c25ec59..6a9dfe926781091c7cccb3c0d78629180ceae9d9 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFEMConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.cpp
@@ -32,4 +32,4 @@ PbdFEMConstraint::PbdFEMConstraint(const unsigned int cardinality,
{
m_vertexIds.resize(cardinality);
}
-} // imstk
\ No newline at end of file
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFEMConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.h
similarity index 62%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFEMConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.h
index 404cbc9f8fbcd7ca9a81e53386b2608d65e7b29d..d06dea03362a70a519005fff7082e62c5613da91 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFEMConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.h
@@ -22,10 +22,28 @@
#pragma once
#include "imstkPbdConstraint.h"
-#include "imstkMath.h"
namespace imstk
{
+///
+/// \class PbdFEMConstraintConfig
+///
+/// \brief Either mu/lambda used, may be computed from youngs modulus and poissons ratio
+///
+struct PbdFEMConstraintConfig
+{
+ PbdFEMConstraintConfig(double mu, double lambda, double youngModulus, double poissonRatio) :
+ m_mu(mu), m_lambda(lambda), m_YoungModulus(youngModulus), m_PoissonRatio(poissonRatio)
+ {
+ }
+
+ double m_mu = 0.0; ///> Lame constant, if constraint type is FEM
+ double m_lambda = 0.0; ///> Lame constant, if constraint type is FEM
+
+ double m_YoungModulus = 1000; ///> FEM parameter, if constraint type is FEM
+ double m_PoissonRatio = 0.2; ///> FEM parameter, if constraint type is FEM
+};
+
///
/// \class PbdFEMConstraint
///
@@ -52,8 +70,10 @@ public:
explicit PbdFEMConstraint(const unsigned int cardinality, MaterialType mtype = MaterialType::StVK);
public:
- double m_elementVolume; ///> Volume of the element
- MaterialType m_material; ///> Material type
- Mat3d m_invRestMat; ///>
+ double m_elementVolume = 0.0; ///> Volume of the element
+ MaterialType m_material; ///> Material type
+ Mat3d m_invRestMat;
+
+ std::shared_ptr<PbdFEMConstraintConfig> m_config = nullptr;
};
-}
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFETetConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp
similarity index 68%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFETetConstraint.cpp
rename to Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp
index 6d1b0e87c8af19e6af062591a44e30cc7409bc14..4763b2f29c6d5d94db7561aec5eac5eb262a230d 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFETetConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp
@@ -20,30 +20,30 @@
=========================================================================*/
#include "imstkPbdFETetConstraint.h"
-#include "imstkPbdModel.h"
#include "imstkLogger.h"
-#include "imstkMath.h"
+#include <iostream>
namespace imstk
{
bool
-PbdFEMTetConstraint::initConstraint(PbdModel& model,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4)
+PbdFEMTetConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0, const size_t& pIdx1,
+ const size_t& pIdx2, const size_t& pIdx3,
+ std::shared_ptr<PbdFEMConstraintConfig> config)
{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
- m_vertexIds[3] = pIdx4;
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
+ m_vertexIds[3] = pIdx3;
- auto state = model.getInitialState();
-
- const Vec3d& p0 = state->getVertexPosition(pIdx1);
- const Vec3d& p1 = state->getVertexPosition(pIdx2);
- const Vec3d& p2 = state->getVertexPosition(pIdx3);
- const Vec3d& p3 = state->getVertexPosition(pIdx4);
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
+ const Vec3d& p3 = initVertexPositions[pIdx3];
m_elementVolume = (1.0 / 6.0) * (p3 - p0).dot((p1 - p0).cross(p2 - p0));
+ m_config = config;
+ m_compliance = 1.0 / (config->m_lambda + 2 * config->m_mu);
Mat3d m;
m.col(0) = p0 - p3;
@@ -61,21 +61,19 @@ PbdFEMTetConstraint::initConstraint(PbdModel& model,
}
bool
-PbdFEMTetConstraint::solvePositionConstraint(PbdModel& model)
+PbdFEMTetConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& cval,
+ StdVectorOfVec3d& dcdx) const
{
- const auto i1 = m_vertexIds[0];
- const auto i2 = m_vertexIds[1];
- const auto i3 = m_vertexIds[2];
- const auto i4 = m_vertexIds[3];
-
- auto state = model.getCurrentState();
-
- Vec3d& p0 = state->getVertexPosition(i1);
- Vec3d& p1 = state->getVertexPosition(i2);
- Vec3d& p2 = state->getVertexPosition(i3);
- Vec3d& p3 = state->getVertexPosition(i4);
+ const auto i0 = m_vertexIds[0];
+ const auto i1 = m_vertexIds[1];
+ const auto i2 = m_vertexIds[2];
+ const auto i3 = m_vertexIds[3];
- //double currentVolume = (1.0 / 6.0) * (p3 - p0).dot((p1 - p0).cross(p2 - p0));
+ const Vec3d& p0 = currVertexPositions[i0];
+ const Vec3d& p1 = currVertexPositions[i1];
+ const Vec3d& p2 = currVertexPositions[i2];
+ const Vec3d& p3 = currVertexPositions[i3];
Mat3d m;
m.col(0) = p0 - p3;
@@ -89,8 +87,8 @@ PbdFEMTetConstraint::solvePositionConstraint(PbdModel& model)
// energy constraint
double C = 0;
- const auto mu = model.getParameters()->m_mu;
- const auto lambda = model.getParameters()->m_lambda;
+ const auto mu = m_config->m_mu;
+ const auto lambda = m_config->m_lambda;
switch (m_material)
{
@@ -176,46 +174,14 @@ PbdFEMTetConstraint::solvePositionConstraint(PbdModel& model)
}
}
- const double im1 = model.getInvMass(i1);
- const double im2 = model.getInvMass(i2);
- const double im3 = model.getInvMass(i3);
- const double im4 = model.getInvMass(i4);
-
Mat3d gradC = m_elementVolume * P * m_invRestMat.transpose();
-
- double sum = im1 * gradC.col(0).squaredNorm()
- + im2 * gradC.col(1).squaredNorm()
- + im3 * gradC.col(2).squaredNorm()
- + im4 * (gradC.col(0) + gradC.col(1) + gradC.col(2)).squaredNorm();
-
- if (sum < m_epsilon)
- {
- return false;
- }
-
- C = C * m_elementVolume;
-
- const double s = C / sum;
-
- if (im1 > 0)
- {
- p0 += -s* im1* gradC.col(0);
- }
-
- if (im2 > 0)
- {
- p1 += -s* im2* gradC.col(1);
- }
-
- if (im3 > 0)
- {
- p2 += -s* im3* gradC.col(2);
- }
-
- if (im4 > 0)
- {
- p3 += s * im4 * (gradC.col(0) + gradC.col(1) + gradC.col(2));
- }
+ cval = C;
+ cval *= m_elementVolume;
+ dcdx.resize(m_vertexIds.size());
+ dcdx[0] = gradC.col(0);
+ dcdx[1] = gradC.col(1);
+ dcdx[2] = gradC.col(2);
+ dcdx[3] = -dcdx[0] - dcdx[1] - dcdx[2];
return true;
}
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFETetConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h
similarity index 80%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFETetConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h
index 1c1b41eca3fc43d073640f9d7609974bc5980e4f..e098bef1872fb467f9a2b1ffd64f76ab0e85604a 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFETetConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h
@@ -48,13 +48,16 @@ public:
///
/// \brief Initialize the tetrahedral FEM constraint
///
- bool initConstraint(PbdModel& model,
+ bool initConstraint(const StdVectorOfVec3d& initVertexPositions,
const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4);
+ const size_t& pIdx3, const size_t& pIdx4,
+ std::shared_ptr<PbdFEMConstraintConfig> config);
///
- /// \brief Solve the tetrahedral FEM constraint
+ /// \brief Compute the value and gradient of constraint
///
- bool solvePositionConstraint(PbdModel& model) override;
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPosition,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
};
} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..75b13a8d9e49a1a6ab776c8a3cd7b9e55464758a
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
@@ -0,0 +1,99 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkPbdPointTriCollisionConstraint.h"
+
+#include "imstkLogger.h"
+
+namespace imstk
+{
+void
+PbdPointTriangleConstraint::initConstraint(const size_t& pIdxA1,
+ const size_t& pIdxB1,
+ const size_t& pIdxB2,
+ const size_t& pIdxB3,
+ std::shared_ptr<PbdCollisionConstraintConfig> configA,
+ std::shared_ptr<PbdCollisionConstraintConfig> configB)
+{
+ m_bodiesFirst[0] = pIdxA1;
+ m_bodiesSecond[0] = pIdxB1;
+ m_bodiesSecond[1] = pIdxB2;
+ m_bodiesSecond[2] = pIdxB3;
+ m_configA = configA;
+ m_configB = configB;
+}
+
+bool
+PbdPointTriangleConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
+ const StdVectorOfVec3d& currVertexPositionsB,
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const
+{
+ const size_t i0 = m_bodiesFirst[0];
+ const size_t i1 = m_bodiesSecond[0];
+ const size_t i2 = m_bodiesSecond[1];
+ const size_t i3 = m_bodiesSecond[2];
+
+ const Vec3d& x0 = currVertexPositionsA[i0];
+ const Vec3d& x1 = currVertexPositionsB[i1];
+ const Vec3d& x2 = currVertexPositionsB[i2];
+ const Vec3d& x3 = currVertexPositionsB[i3];
+
+ const Vec3d x12 = x2 - x1;
+ const Vec3d x13 = x3 - x1;
+ Vec3d n = x12.cross(x13);
+ const Vec3d x01 = x0 - x1;
+
+ double alpha = n.dot(x12.cross(x01)) / (n.dot(n));
+ double beta = n.dot(x01.cross(x13)) / (n.dot(n));
+
+ if (alpha < 0 || beta < 0 || alpha + beta > 1)
+ {
+ //LOG(WARNING) << "Projection point not inside the triangle";
+ c = 0.0;
+ return false;
+ }
+
+ const double dist = m_configA->m_proximity + m_configB->m_proximity;
+
+ n.normalize();
+
+ double l = x01.dot(n);
+
+ if (l > dist)
+ {
+ c = 0.0;
+ return false;
+ }
+
+ double gamma = 1.0 - alpha - beta;
+ dcdxA.resize(1);
+ dcdxB.resize(3);
+ dcdxA[0] = n;
+ dcdxB[0] = -alpha * n;
+ dcdxB[1] = -beta * n;
+ dcdxB[2] = -gamma * n;
+
+ c = l - dist;
+ return true;
+}
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdPointTriCollisionConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
similarity index 54%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
index ba1335e4927542b9acbdc3c111f34e876932e49a..6351c1dae540ccf23b56b7e071589f67880f7b35 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
@@ -46,19 +46,29 @@ public:
///
/// \brief initialize constraint
- /// \param pIdx1 index of the point from object1
- /// \param pIdx2 first point of the triangle from object2
- /// \param pIdx3 second point of the triangle from object2
- /// \param pIdx4 third point of the triangle from object2
+ /// \param pIdxA1 index of the point from object1
+ /// \param pIdxB1 first point of the triangle from object2
+ /// \param pIdxB2 second point of the triangle from object2
+ /// \param pIdxB3 third point of the triangle from object2
/// \return
///
- void initConstraint(std::shared_ptr<PbdModel> model1, const size_t& pIdx1,
- std::shared_ptr<PbdModel> model2, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4);
+ void initConstraint(const size_t& pIdxA1,
+ const size_t& pIdxB1, const size_t& pIdxB2, const size_t& pIdxB3,
+ std::shared_ptr<PbdCollisionConstraintConfig> configA,
+ std::shared_ptr<PbdCollisionConstraintConfig> configB);
///
- /// \brief
+ /// \brief compute value and gradient of constraint function
///
- bool solvePositionConstraint() override;
+ /// \param[in] currVertexPositionsA current positions from object A
+ /// \param[in] currVertexPositionsA current positions from object B
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
+ ///
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
+ const StdVectorOfVec3d& currVertexPositionsB,
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const override;
};
-}
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef63215ebcfa6e58be138a90c170eb9f9012941f
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
@@ -0,0 +1,73 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkPbdVolumeConstraint.h"
+
+namespace imstk
+{
+void
+PbdVolumeConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0, const size_t& pIdx1,
+ const size_t& pIdx2, const size_t& pIdx3,
+ const double k)
+{
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
+ m_vertexIds[3] = pIdx3;
+
+ m_stiffness = k;
+ m_compliance = 1.0 / k;
+
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
+ const Vec3d& p3 = initVertexPositions[pIdx3];
+
+ m_restVolume = (1.0 / 6.0) * ((p1 - p0).cross(p2 - p0)).dot(p3 - p0);
+}
+
+bool
+PbdVolumeConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
+{
+ const auto i0 = m_vertexIds[0];
+ const auto i1 = m_vertexIds[1];
+ const auto i2 = m_vertexIds[2];
+ const auto i3 = m_vertexIds[3];
+
+ const Vec3d& x0 = currVertexPositions[i0];
+ const Vec3d& x1 = currVertexPositions[i1];
+ const Vec3d& x2 = currVertexPositions[i2];
+ const Vec3d& x3 = currVertexPositions[i3];
+
+ const double onesixth = 1.0 / 6.0;
+
+ dcdx[0] = onesixth * (x1 - x2).cross(x3 - x1);
+ dcdx[1] = onesixth * (x2 - x0).cross(x3 - x0);
+ dcdx[2] = onesixth * (x3 - x0).cross(x1 - x0);
+ dcdx[3] = onesixth * (x1 - x0).cross(x2 - x0);
+
+ c = dcdx[3].dot(x3 - x0);
+ return true;
+}
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdVolumeConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h
similarity index 67%
rename from Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdVolumeConstraint.h
rename to Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h
index 0fb646cb953facf8c3fac8ad09a5235ee0b60164..71a5e48ecda4a933cef788bc737be021efa73cfc 100644
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdVolumeConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h
@@ -36,7 +36,7 @@ public:
///
/// \brief constructor
///
- PbdVolumeConstraint() : PbdConstraint() { m_vertexIds.resize(4); }
+ PbdVolumeConstraint() { m_vertexIds.resize(4); }
///
/// \brief Returns PBD constraint of type Type::Volume
@@ -46,17 +46,19 @@ public:
///
/// \brief Initializes the volume constraint
///
- void initConstraint(PbdModel& model, const size_t& pIdx1,
- const size_t& pIdx2, const size_t& pIdx3,
- const size_t& pIdx4, const double k = 2.0);
+ void initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx1, const size_t& pIdx2,
+ const size_t& pIdx3, const size_t& pIdx4,
+ const double k = 2.0);
///
- /// \brief Solves the volume constraint
+ /// \brief Compute the value and gradient of constraint
///
- bool solvePositionConstraint(PbdModel& model) override;
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPosition,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
-public:
- double m_restVolume = 0.; ///> Rest volume
- double m_stiffness = 0.; ///> Stiffness of the volume constraint
+protected:
+ double m_restVolume = 0.0; ///> Rest volume
};
-}
+} // imstk
diff --git a/Source/Controllers/imstkSceneObjectController.cpp b/Source/Controllers/imstkSceneObjectController.cpp
index 7056dcb7f7f92d77da5286d338714f0715a88895..1b6a94a8e151a4a2d0c8d20ad62bbec0f0306c62 100644
--- a/Source/Controllers/imstkSceneObjectController.cpp
+++ b/Source/Controllers/imstkSceneObjectController.cpp
@@ -25,10 +25,7 @@
#include "imstkDeviceClient.h"
#include "imstkCollidingObject.h"
#include "imstkGeometry.h"
-
-#include <utility>
-
-#include <g3log/g3log.hpp>
+#include "imstkLogger.h"
namespace imstk
{
diff --git a/Source/Devices/CMakeLists.txt b/Source/Devices/CMakeLists.txt
index 74242f5af6beaecd5987721d569330b85b3b349a..83e5e995fa7283bcf8e360a999df99158e1adf68 100644
--- a/Source/Devices/CMakeLists.txt
+++ b/Source/Devices/CMakeLists.txt
@@ -1,17 +1,33 @@
#-----------------------------------------------------------------------------
# Create target
#-----------------------------------------------------------------------------
+
+list(APPEND Dependencies
+ Common
+ VRPN)
+
+list(APPEND ExclusionFiles "")
+if(${PROJECT_NAME}_USE_OpenHaptics)
+ list(APPEND Dependencies
+ ${HAPTIC_DEVICE_LIBS})
+else()
+ list(APPEND ExclusionFiles
+ imstkHDAPIDeviceClient.h
+ imstkHDAPIDeviceClient.cpp
+ imstkHDAPIDeviceServer.h
+ imstkHDAPIDeviceServer.cpp)
+endif()
+
include(imstkAddLibrary)
set(HAPTIC_DEVICE_LIBS)
if(${PROJECT_NAME}_USE_OpenHaptics)
set(HAPTIC_DEVICE_LIBS OpenHapticsSDK)
endif()
-imstk_add_library( Devices
+imstk_add_library(Devices
+ EXCLUDE_FILES
+ ${ExclusionFiles}
DEPENDS
- Common
- VRPN
- ${HAPTIC_DEVICE_LIBS}
- )
+ ${Dependencies})
#-----------------------------------------------------------------------------
# Testing
diff --git a/Source/Devices/imstkHDAPIDeviceClient.cpp b/Source/Devices/imstkHDAPIDeviceClient.cpp
index da70ba044d450770a84c2e76575ab628a7fe213f..654e22acd62afa36e2c73b704e284351705c4e5b 100644
--- a/Source/Devices/imstkHDAPIDeviceClient.cpp
+++ b/Source/Devices/imstkHDAPIDeviceClient.cpp
@@ -19,8 +19,6 @@
=========================================================================*/
-#ifdef iMSTK_USE_OPENHAPTICS
-
#include "imstkHDAPIDeviceClient.h"
#include <HDU/hduVector.h>
@@ -93,4 +91,3 @@ HDAPIDeviceClient::hapticCallback(void* pData)
return HD_CALLBACK_DONE;
}
} // imstk
-#endif // ifdef iMSTK_USE_OPENHAPTICS
diff --git a/Source/Devices/imstkHDAPIDeviceClient.h b/Source/Devices/imstkHDAPIDeviceClient.h
index c4d2ab3ae5a8dd8739356cccb864eb8fc33ffe56..525054168d34ed7d5a03656e4a8360dc0513b68c 100644
--- a/Source/Devices/imstkHDAPIDeviceClient.h
+++ b/Source/Devices/imstkHDAPIDeviceClient.h
@@ -19,10 +19,7 @@
=========================================================================*/
-#ifdef iMSTK_USE_OPENHAPTICS
-
-#ifndef imstkHDAPIDeviceClient_h
-#define imstkHDAPIDeviceClient_h
+#pragma once
#include "imstkDeviceClient.h"
@@ -81,6 +78,3 @@ private:
HD_state m_state; ///< device reading state
};
}
-
-#endif // ifndef imstkHDAPIDeviceClient_h
-#endif // ifdef iMSTK_USE_OpenHaptics
diff --git a/Source/Devices/imstkHDAPIDeviceServer.cpp b/Source/Devices/imstkHDAPIDeviceServer.cpp
index 380a3a2a3ffcc7ab42b649d672d84d9eff3e4390..302d076e64a89ee3a20f9a7ab78a7050831ac009 100644
--- a/Source/Devices/imstkHDAPIDeviceServer.cpp
+++ b/Source/Devices/imstkHDAPIDeviceServer.cpp
@@ -19,8 +19,6 @@
=========================================================================*/
-#ifdef iMSTK_USE_OPENHAPTICS
-
#include "imstkHDAPIDeviceServer.h"
#include "imstkHDAPIDeviceClient.h"
@@ -65,5 +63,3 @@ HDAPIDeviceServer::cleanUpModule()
}
}
} // imstk
-
-#endif // ifdef iMSTK_USE_OpenHaptics
diff --git a/Source/Devices/imstkHDAPIDeviceServer.h b/Source/Devices/imstkHDAPIDeviceServer.h
index 9d25aeece66471cb127818292ef24116952ee542..0638b75dbd28e17ffb7b8cbad52f868bb09b4202 100644
--- a/Source/Devices/imstkHDAPIDeviceServer.h
+++ b/Source/Devices/imstkHDAPIDeviceServer.h
@@ -19,10 +19,7 @@
=========================================================================*/
-#ifdef iMSTK_USE_OPENHAPTICS
-
-#ifndef imstkHDAPIDeviceServer_h
-#define imstkHDAPIDeviceServer_h
+#pragma once
#include <vector>
@@ -78,6 +75,3 @@ private:
std::vector<std::shared_ptr<HDAPIDeviceClient>> m_deviceClients; ///< list of OpenHaptics
};
} // imstk
-
-#endif // ifndef imstkHDAPIDeviceServer_h
-#endif // ifdef iMSTK_USE_OpenHaptics
diff --git a/Source/DynamicalModels/CMakeLists.txt b/Source/DynamicalModels/CMakeLists.txt
index b69970a4c2ad14e919c332a40d157f5059c74a5a..db13a0b3ff196d398aa8577a727a4c0544cab352 100644
--- a/Source/DynamicalModels/CMakeLists.txt
+++ b/Source/DynamicalModels/CMakeLists.txt
@@ -3,31 +3,48 @@
#-----------------------------------------------------------------------------
include(imstkAddLibrary)
-imstk_add_library( DynamicalModels
+list(APPEND Dependencies
+ Common
+ DataStructures
+ Constraints
+ Geometry
+ Solvers
+ MeshIO
+ VegaFEM::massSpringSystem
+ VegaFEM::corotationalLinearFEM
+ VegaFEM::isotropicHyperelasticFEM
+ VegaFEM::forceModel
+ VegaFEM::stvk
+ VegaFEM::graph
+ VegaFEM::volumetricMesh
+ PhysX)
+list(APPEND ExclusionFiles "")
+
+# If using model reduction link to these libs
+if (${${PROJECT_NAME}_USE_MODEL_REDUCTION})
+ list(APPEND Dependencies
+ VegaFEM::reducedStvk
+ VegaFEM::reducedElasticForceModel)
+else()
+ list(APPEND ExclusionFiles
+ ObjectModels/imstkReducedStVKBodyModel.h
+ ObjectModels/imstkReducedStVKBodyModel.cpp)
+endif()
+
+imstk_add_library(DynamicalModels
SUBDIR_LIST
ObjectStates
TimeIntegrators
ObjectModels
- ObjectModels/PbdConstraints
InternalForceModel
- DEPENDS
- DataStructures
- Constraints
- Geometry
- Solvers
- VegaFEM::massSpringSystem
- VegaFEM::corotationalLinearFEM
- VegaFEM::isotropicHyperelasticFEM
- VegaFEM::forceModel
- VegaFEM::stvk
- VegaFEM::graph
- VegaFEM::volumetricMesh
- PhysX
- )
+ EXCLUDE_FILES
+ ${ExclusionFiles}
+ DEPENDS
+ ${Dependencies})
#-----------------------------------------------------------------------------
# Testing
#-----------------------------------------------------------------------------
#if( ${PROJECT_NAME}_BUILD_TESTING )
# add_subdirectory( Testing )
-#endif()
\ No newline at end of file
+#endif()
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdAreaConstraint.cpp b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdAreaConstraint.cpp
deleted file mode 100644
index 18d71d89575468daa5b36f0423e852d8171f98ba..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdAreaConstraint.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkPbdAreaConstraint.h"
-#include "imstkPbdModel.h"
-
-namespace imstk
-{
-void
-PbdAreaConstraint::initConstraint(PbdModel& model, const size_t& pIdx1,
- const size_t& pIdx2, const size_t& pIdx3,
- const double k)
-{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
-
- m_stiffness = k;
-
- auto state = model.getInitialState();
-
- const Vec3d& p0 = state->getVertexPosition(pIdx1);
- const Vec3d& p1 = state->getVertexPosition(pIdx2);
- const Vec3d& p2 = state->getVertexPosition(pIdx3);
-
- m_restArea = 0.5 * (p1 - p0).cross(p2 - p0).norm();
-}
-
-bool
-PbdAreaConstraint::solvePositionConstraint(PbdModel& model)
-{
- const auto i1 = m_vertexIds[0];
- const auto i2 = m_vertexIds[1];
- const auto i3 = m_vertexIds[2];
-
- auto state = model.getCurrentState();
-
- Vec3d& p0 = state->getVertexPosition(i1);
- Vec3d& p1 = state->getVertexPosition(i2);
- Vec3d& p2 = state->getVertexPosition(i3);
-
- const auto im0 = model.getInvMass(i1);
- const auto im1 = model.getInvMass(i2);
- const auto im2 = model.getInvMass(i3);
-
- const auto e1 = p0 - p1;
- const auto e2 = p1 - p2;
- const auto e3 = p2 - p0;
-
- auto n = e1.cross(e2);
- const auto A = 0.5 * n.norm();
-
- if (A < m_epsilon)
- {
- return false;
- }
-
- n /= 2 * A;
-
- const Vec3d grad0 = e2.cross(n);
- const Vec3d grad1 = e3.cross(n);
- const Vec3d grad2 = e1.cross(n);
-
- auto lambda = im0 * grad0.squaredNorm() + im1 * grad1.squaredNorm() + im2 * grad2.squaredNorm();
-
- lambda = (A - m_restArea) / lambda * m_stiffness;
-
- if (im0 > 0)
- {
- p0 += -im0 * lambda * grad0;
- }
-
- if (im1 > 0)
- {
- p1 += -im1 * lambda * grad1;
- }
-
- if (im2 > 0)
- {
- p2 += -im2 * lambda * grad2;
- }
-
- return true;
-}
-} // imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdBendConstraint.cpp b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdBendConstraint.cpp
deleted file mode 100644
index cde7439e76c8b05a798b17a8b6af634404d6736f..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdBendConstraint.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkPbdBendConstraint.h"
-#include "imstkPbdModel.h"
-
-namespace imstk
-{
-void
-PbdBendConstraint::initConstraint(PbdModel& model,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const double k)
-{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
-
- m_stiffness = k;
- auto state = model.getInitialState();
-
- const Vec3d& p0 = state->getVertexPosition(pIdx1);
- const Vec3d& p1 = state->getVertexPosition(pIdx2);
- const Vec3d& p2 = state->getVertexPosition(pIdx3);
-
- // Instead of using the angle between the segments we can use the distance
- // from the center of the triangle
- const Vec3d& center = (p0 + p1 + p2) / 3.0;
- m_restLength = (p1 - center).norm();
-}
-
-bool
-PbdBendConstraint::solvePositionConstraint(PbdModel& model)
-{
- const auto i1 = m_vertexIds[0];
- const auto i2 = m_vertexIds[1];
- const auto i3 = m_vertexIds[2];
-
- auto state = model.getCurrentState();
-
- Vec3d& p0 = state->getVertexPosition(i1);
- Vec3d& p1 = state->getVertexPosition(i2);
- Vec3d& p2 = state->getVertexPosition(i3);
-
- const auto im0 = model.getInvMass(i1);
- const auto im1 = model.getInvMass(i2);
- const auto im2 = model.getInvMass(i3);
-
- const auto m0 = (im0 > 0.0) ? 1.0 / im0 : 0.0;
- const auto m1 = (im1 > 0.0) ? 1.0 / im1 : 0.0;
- const auto m2 = (im2 > 0.0) ? 1.0 / im2 : 0.0;
-
- // Move towards triangle center
- const Vec3d& center = (p0 + p1 + p2) / 3.0;
- const Vec3d& diff = p1 - center;
- const double dist = diff.norm();
- if (dist < m_epsilon)
- {
- return false;
- }
- const Vec3d& dir = diff / dist;
- const double c = (dist - m_restLength) * m_stiffness;
-
- // Now Apply movement weighted by masses
- if (im0 > 0.0)
- {
- p0 += c * dir * 2.0 * m0 / (m0 + m1 + m2);
- }
-
- if (im1 > 0.0)
- {
- p1 -= c * dir * 4.0 * m1 / (m0 + m1 + m2);
- }
-
- if (im2 > 0.0)
- {
- p2 += c * dir * 2.0 * m2 / (m0 + m1 + m2);
- }
-
- return true;
-}
-} // imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdCollisionConstraint.cpp b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdCollisionConstraint.cpp
deleted file mode 100644
index 529c8103cdb64f2193a53a45cd13be91f296449b..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdCollisionConstraint.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkPbdCollisionConstraint.h"
-#include "imstkLogger.h"
-
-namespace imstk
-{
-PbdCollisionConstraint::PbdCollisionConstraint(const unsigned int& n1, const unsigned int& n2)
-{
- m_bodiesFirst.resize(n1);
- m_bodiesSecond.resize(n2);
-}
-} // imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdCollisionConstraint.h b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdCollisionConstraint.h
deleted file mode 100644
index 854c1fe65c590f5dc015ea6ed0f2e099c454ebac..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdCollisionConstraint.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#pragma once
-
-#include <vector>
-#include <memory>
-
-namespace imstk
-{
-class PbdModel;
-
-///
-/// \class PbdCollisionConstraint
-///
-/// \brief
-///
-class PbdCollisionConstraint
-{
-public:
- enum class Type
- {
- EdgeEdge,
- PointTriangle
- };
-
- ///
- /// \brief
- ///
- PbdCollisionConstraint(const unsigned int& n1, const unsigned int& n2);
-
- ///
- /// \brief Destructor
- ///
- virtual ~PbdCollisionConstraint() = default;
-
- ///
- /// \brief
- ///
- virtual bool solvePositionConstraint() = 0;
-
-public:
- std::vector<size_t> m_bodiesFirst; ///> index of points for the first object
- std::vector<size_t> m_bodiesSecond; ///> index of points for the second object
-
- std::shared_ptr<PbdModel> m_model1;
- std::shared_ptr<PbdModel> m_model2;
-};
-}
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDihedralConstraint.cpp b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDihedralConstraint.cpp
deleted file mode 100644
index ed327ad64ed545897f96d7cef41d5aaef8daa535..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDihedralConstraint.cpp
+++ /dev/null
@@ -1,131 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkPbdDihedralConstraint.h"
-#include "imstkPbdModel.h"
-
-namespace imstk
-{
-void
-PbdDihedralConstraint::initConstraint(PbdModel& model,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4,
- const double k)
-{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
- m_vertexIds[3] = pIdx4;
-
- m_stiffness = k;
- auto state = model.getInitialState();
-
- const Vec3d& p0 = state->getVertexPosition(pIdx1);
- const Vec3d& p1 = state->getVertexPosition(pIdx2);
- const Vec3d& p2 = state->getVertexPosition(pIdx3);
- const Vec3d& p3 = state->getVertexPosition(pIdx4);
-
- const Vec3d n1 = (p2 - p0).cross(p3 - p0).normalized();
- const Vec3d n2 = (p3 - p1).cross(p2 - p1).normalized();
-
- m_restAngle = atan2(n1.cross(n2).dot(p3 - p2), (p3 - p2).norm() * n1.dot(n2));
-}
-
-bool
-PbdDihedralConstraint::solvePositionConstraint(PbdModel& model)
-{
- const auto i1 = m_vertexIds[0];
- const auto i2 = m_vertexIds[1];
- const auto i3 = m_vertexIds[2];
- const auto i4 = m_vertexIds[3];
-
- auto state = model.getCurrentState();
-
- Vec3d& p0 = state->getVertexPosition(i1);
- Vec3d& p1 = state->getVertexPosition(i2);
- Vec3d& p2 = state->getVertexPosition(i3);
- Vec3d& p3 = state->getVertexPosition(i4);
-
- const auto im0 = model.getInvMass(i1);
- const auto im1 = model.getInvMass(i2);
- const auto im2 = model.getInvMass(i3);
- const auto im3 = model.getInvMass(i4);
-
- if (im0 == 0.0 && im1 == 0.0)
- {
- return false;
- }
-
- const auto e = p3 - p2;
- const auto e1 = p3 - p0;
- const auto e2 = p0 - p2;
- const auto e3 = p3 - p1;
- const auto e4 = p1 - p2;
- // To accelerate, all normal (area) vectors and edge length should be precomputed in parallel
- auto n1 = e1.cross(e);
- auto n2 = e.cross(e3);
- const auto A1 = n1.norm();
- const auto A2 = n2.norm();
- n1 /= A1;
- n2 /= A2;
-
- const double l = e.norm();
- if (l < m_epsilon)
- {
- return false;
- }
-
- const Vec3d grad0 = -(l / A1) * n1;
- const Vec3d grad1 = -(l / A2) * n2;
- const Vec3d grad2 = (e.dot(e1) / (A1 * l)) * n1 + (e.dot(e3) / (A2 * l)) * n2;
- const Vec3d grad3 = (e.dot(e2) / (A1 * l)) * n1 + (e.dot(e4) / (A2 * l)) * n2;
-
- auto lambda = im0 * grad0.squaredNorm() +
- im1 * grad1.squaredNorm() +
- im2 * grad2.squaredNorm() +
- im3 * grad3.squaredNorm();
-
- // huge difference if use acos instead of atan2
- lambda = (atan2(n1.cross(n2).dot(e), l * n1.dot(n2)) - m_restAngle) / lambda * m_stiffness;
-
- if (im0 > 0)
- {
- p0 += -im0 * lambda * grad0;
- }
-
- if (im1 > 0)
- {
- p1 += -im1 * lambda * grad1;
- }
-
- if (im2 > 0)
- {
- p2 += -im2 * lambda * grad2;
- }
-
- if (im3 > 0)
- {
- p3 += -im3 * lambda * grad3;
- }
-
- return true;
-}
-} // imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDistanceConstraint.cpp b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDistanceConstraint.cpp
deleted file mode 100644
index a989446063f7cb62cbc4dfed88eb7aadc8aade66..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdDistanceConstraint.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkPbdDistanceConstraint.h"
-#include "imstkPbdModel.h"
-
-namespace imstk
-{
-void
-PbdDistanceConstraint::initConstraint(PbdModel& model, const size_t& pIdx1,
- const size_t& pIdx2, const double k)
-{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_stiffness = k;
-
- auto state = model.getInitialState();
- const Vec3d& p1 = state->getVertexPosition(pIdx1);
- const Vec3d& p2 = state->getVertexPosition(pIdx2);
-
- m_restLength = (p1 - p2).norm();
-}
-
-bool
-PbdDistanceConstraint::solvePositionConstraint(PbdModel& model)
-{
- const auto i1 = m_vertexIds[0];
- const auto i2 = m_vertexIds[1];
-
- auto state = model.getCurrentState();
-
- Vec3d& p0 = state->getVertexPosition(i1);
- Vec3d& p1 = state->getVertexPosition(i2);
-
- const auto im1 = model.getInvMass(i1);
- const auto im2 = model.getInvMass(i2);
-
- const auto wsum = im1 + im2;
-
- if (wsum == 0.0)
- {
- return false;
- }
-
- Vec3d n = p1 - p0;
- const auto len = n.norm();
- n /= len;
-
- auto gradC = n * m_stiffness * (len - m_restLength) / wsum;
-
- if (im1 > 0)
- {
- p0 += im1 * gradC;
- }
-
- if (im2 > 0)
- {
- p1 += -im2 * gradC;
- }
- return true;
-}
-} // imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
deleted file mode 100644
index 38401eec48d23588835940325f5e7acd831cde4f..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
+++ /dev/null
@@ -1,138 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkPbdEdgeEdgeCollisionConstraint.h"
-#include "imstkPbdModel.h"
-
-#include "imstkLogger.h"
-
-namespace imstk
-{
-void
-PbdEdgeEdgeConstraint::initConstraint(std::shared_ptr<PbdModel> model1,
- const size_t& pIdx1, const size_t& pIdx2,
- std::shared_ptr<PbdModel> model2,
- const size_t& pIdx3, const size_t& pIdx4)
-{
- m_model1 = model1;
- m_model2 = model2;
- m_bodiesFirst[0] = pIdx1;
- m_bodiesFirst[1] = pIdx2;
- m_bodiesSecond[0] = pIdx3;
- m_bodiesSecond[1] = pIdx4;
-}
-
-bool
-PbdEdgeEdgeConstraint::solvePositionConstraint()
-{
- const auto i0 = m_bodiesFirst[0];
- const auto i1 = m_bodiesFirst[1];
- const auto i2 = m_bodiesSecond[0];
- const auto i3 = m_bodiesSecond[1];
-
- auto state1 = m_model1->getCurrentState();
- auto state2 = m_model2->getCurrentState();
-
- Vec3d& x0 = state1->getVertexPosition(i0);
- Vec3d& x1 = state1->getVertexPosition(i1);
- Vec3d& x2 = state2->getVertexPosition(i2);
- Vec3d& x3 = state2->getVertexPosition(i3);
-
- auto a = (x3 - x2).dot(x1 - x0);
- auto b = (x1 - x0).dot(x1 - x0);
- auto c = (x0 - x2).dot(x1 - x0);
- auto d = (x3 - x2).dot(x3 - x2);
- auto e = a;
- auto f = (x0 - x2).dot(x3 - x2);
-
- auto det = a * e - d * b;
- double s = 0.5;
- double t = 0.5;
- if (fabs(det) > 1e-12)
- {
- s = (c * e - b * f) / det;
- t = (c * d - a * f) / det;
- if (s < 0 || s > 1.0 || t < 0 || t > 1.0)
- {
- return false;
- }
- }
- else
- {
- //LOG(WARNING) << "det is null";
- }
-
- Vec3d P = x0 + t * (x1 - x0);
- Vec3d Q = x2 + s * (x3 - x2);
-
- Vec3d n = Q - P;
- auto l = n.norm();
- n /= l;
-
- const auto dist = m_model1->getParameters()->m_proximity + m_model2->getParameters()->m_proximity;
-
- if (l > dist)
- {
- return false;
- }
-
- Vec3d grad0 = -(1 - t) * n;
- Vec3d grad1 = -(t) * n;
- Vec3d grad2 = (1 - s) * n;
- Vec3d grad3 = (s) * n;
-
- const auto im0 = m_model1->getInvMass(i0);
- const auto im1 = m_model1->getInvMass(i1);
- const auto im2 = m_model2->getInvMass(i2);
- const auto im3 = m_model2->getInvMass(i3);
-
- auto lambda = im0 * grad0.squaredNorm() +
- im1 * grad1.squaredNorm() +
- im2 * grad2.squaredNorm() +
- im3 * grad3.squaredNorm();
-
- lambda = (l - dist) / lambda;
-
-// LOG(INFO) << "Lambda:" << lambda <<" Normal:" << n[0] <<" " << n[1] <<" "<<n[2];
-
- if (im0 > 0)
- {
- x0 += -im0* lambda* grad0* m_model1->getParameters()->m_contactStiffness;
- }
-
- if (im1 > 0)
- {
- x1 += -im1* lambda* grad1* m_model1->getParameters()->m_contactStiffness;
- }
-
- if (im2 > 0)
- {
- x2 += -im2* lambda* grad2* m_model2->getParameters()->m_contactStiffness;
- }
-
- if (im3 > 0)
- {
- x3 += -im3* lambda* grad3* m_model2->getParameters()->m_contactStiffness;
- }
-
- return true;
-}
-} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFEHexConstraint.h b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFEHexConstraint.h
deleted file mode 100644
index 6d772d6675c75b8a65522063b3c7519fd59fda1a..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdFEHexConstraint.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#pragma once
-
-#include "imstkPbdFEMConstraint.h"
-
-namespace imstk
-{
-///
-/// \class PbdFEMHexConstraint
-///
-/// \brief The FEMHexConstraint class class for constraint as the elastic energy
-/// computed by linear shape functions with hexahedral mesh.
-///
-class PbdFEMHexConstraint : public PbdFEMConstraint
-{
-public:
- ///
- /// \brief Constructor
- ///
- explicit PbdFEMHexConstraint(MaterialType mtype = MaterialType::StVK) :
- PbdFEMConstraint(8, mtype) {}
-
- ///
- /// \brief Get the type of FEM constraint
- ///
- inline Type getType() const override { return Type::FEMHex; }
-
- ///
- /// \brief Initializes the FEM hexahedral element constraint
- ///
- bool initConstraint(PbdModel& model, const unsigned int& pIdx1,
- const unsigned int& pIdx2, const unsigned int& pIdx3,
- const unsigned int& pIdx4, const unsigned int& pIdx5,
- const unsigned int& pIdx6, const unsigned int& pIdx7,
- const unsigned int& pIdx8);
-
- ///
- /// \brief Solves the FEM hexahedral element constraint
- ///
- bool solvePositionConstraint(PbdModel& model) override;
-};
-} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
deleted file mode 100644
index f263d1f6a733c4ce5cc266bb77fd81474792f824..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
+++ /dev/null
@@ -1,125 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkPbdPointTriCollisionConstraint.h"
-#include "imstkPbdModel.h"
-
-#include "imstkLogger.h"
-
-namespace imstk
-{
-void
-PbdPointTriangleConstraint::initConstraint(std::shared_ptr<PbdModel> model1, const size_t& pIdx1,
- std::shared_ptr<PbdModel> model2, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4)
-{
- m_model1 = model1;
- m_model2 = model2;
- m_bodiesFirst[0] = pIdx1;
- m_bodiesSecond[0] = pIdx2;
- m_bodiesSecond[1] = pIdx3;
- m_bodiesSecond[2] = pIdx4;
-}
-
-bool
-PbdPointTriangleConstraint::solvePositionConstraint()
-{
- const auto i0 = m_bodiesFirst[0];
- const auto i1 = m_bodiesSecond[0];
- const auto i2 = m_bodiesSecond[1];
- const auto i3 = m_bodiesSecond[2];
-
- auto state1 = m_model1->getCurrentState();
- auto state2 = m_model2->getCurrentState();
-
- Vec3d& x0 = state1->getVertexPosition(i0);
- Vec3d& x1 = state2->getVertexPosition(i1);
- Vec3d& x2 = state2->getVertexPosition(i2);
- Vec3d& x3 = state2->getVertexPosition(i3);
-
- Vec3d x12 = x2 - x1;
- Vec3d x13 = x3 - x1;
- Vec3d n = x12.cross(x13);
- Vec3d x01 = x0 - x1;
-
- double alpha = n.dot(x12.cross(x01)) / (n.dot(n));
- double beta = n.dot(x01.cross(x13)) / (n.dot(n));
-
- if (alpha < 0 || beta < 0 || alpha + beta > 1)
- {
- //LOG(WARNING) << "Projection point not inside the triangle";
- return false;
- }
-
- const double dist = m_model1->getParameters()->m_proximity + m_model2->getParameters()->m_proximity;
-
- n.normalize();
-
- double l = x01.dot(n);
-
- if (l > dist)
- {
- return false;
- }
-
- double gamma = 1.0 - alpha - beta;
- Vec3d grad0 = n;
- Vec3d grad1 = -alpha * n;
- Vec3d grad2 = -beta * n;
- Vec3d grad3 = -gamma * n;
-
- const auto im0 = m_model1->getInvMass(i0);
- const auto im1 = m_model2->getInvMass(i1);
- const auto im2 = m_model2->getInvMass(i2);
- const auto im3 = m_model2->getInvMass(i3);
-
- double lambda = im0 * grad0.squaredNorm() +
- im1 * grad1.squaredNorm() +
- im2 * grad2.squaredNorm() +
- im3 * grad3.squaredNorm();
-
- lambda = (l - dist) / lambda;
-
- //LOG(INFO) << "Lambda:" << lambda <<" Normal:" << n[0] <<" " << n[1] <<" "<<n[2];
-
- if (im0 > 0)
- {
- x0 += -im0* lambda* grad0* m_model1->getParameters()->m_contactStiffness;
- }
-
- if (im1 > 0)
- {
- x1 += -im1* lambda* grad1* m_model2->getParameters()->m_contactStiffness;
- }
-
- if (im2 > 0)
- {
- x2 += -im2* lambda* grad2* m_model2->getParameters()->m_contactStiffness;
- }
-
- if (im3 > 0)
- {
- x3 += -im3* lambda* grad3* m_model2->getParameters()->m_contactStiffness;
- }
-
- return true;
-}
-} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdVolumeConstraint.cpp b/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdVolumeConstraint.cpp
deleted file mode 100644
index b6dce29fd6d1ffa45a05836416fb20f20dd04fcc..0000000000000000000000000000000000000000
--- a/Source/DynamicalModels/ObjectModels/PbdConstraints/imstkPbdVolumeConstraint.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkPbdVolumeConstraint.h"
-#include "imstkPbdModel.h"
-
-namespace imstk
-{
-void
-PbdVolumeConstraint::initConstraint(PbdModel& model, const size_t& pIdx1,
- const size_t& pIdx2, const size_t& pIdx3,
- const size_t& pIdx4, const double k)
-{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
- m_vertexIds[3] = pIdx4;
-
- m_stiffness = k;
-
- auto state = model.getInitialState();
-
- const Vec3d& p0 = state->getVertexPosition(pIdx1);
- const Vec3d& p1 = state->getVertexPosition(pIdx2);
- const Vec3d& p2 = state->getVertexPosition(pIdx3);
- const Vec3d& p3 = state->getVertexPosition(pIdx4);
-
- m_restVolume = (1.0 / 6.0) * ((p1 - p0).cross(p2 - p0)).dot(p3 - p0);
-}
-
-bool
-PbdVolumeConstraint::solvePositionConstraint(PbdModel& model)
-{
- const auto i1 = m_vertexIds[0];
- const auto i2 = m_vertexIds[1];
- const auto i3 = m_vertexIds[2];
- const auto i4 = m_vertexIds[3];
-
- auto state = model.getCurrentState();
-
- Vec3d& x1 = state->getVertexPosition(i1);
- Vec3d& x2 = state->getVertexPosition(i2);
- Vec3d& x3 = state->getVertexPosition(i3);
- Vec3d& x4 = state->getVertexPosition(i4);
-
- const auto im1 = model.getInvMass(i1);
- const auto im2 = model.getInvMass(i2);
- const auto im3 = model.getInvMass(i3);
- const auto im4 = model.getInvMass(i4);
-
- const double onesixth = 1.0 / 6.0;
-
- const Vec3d grad1 = onesixth * (x2 - x3).cross(x4 - x2);
- const Vec3d grad2 = onesixth * (x3 - x1).cross(x4 - x1);
- const Vec3d grad3 = onesixth * (x4 - x1).cross(x2 - x1);
- const Vec3d grad4 = onesixth * (x2 - x1).cross(x3 - x1);
-
- const auto V = grad4.dot(x4 - x1);
-
- double lambda = im1 * grad1.squaredNorm() +
- im2 * grad2.squaredNorm() +
- im3 * grad3.squaredNorm() +
- im4 * grad4.squaredNorm();
-
- lambda = (V - m_restVolume) / lambda * m_stiffness;
-
- if (im1 > 0)
- {
- x1 += -im1 * lambda * grad1;
- }
-
- if (im2 > 0)
- {
- x2 += -im2 * lambda * grad2;
- }
-
- if (im3 > 0)
- {
- x3 += -im3 * lambda * grad3;
- }
-
- if (im4 > 0)
- {
- x4 += -im4 * lambda * grad4;
- }
-
- return true;
-}
-} // imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/imstkAbstractDynamicalModel.cpp b/Source/DynamicalModels/ObjectModels/imstkAbstractDynamicalModel.cpp
index 436d8d5e5d1442b235e2dbe883637100b8efd326..7264f71fa8c33601aa358b3d8d99751b19bcd454 100644
--- a/Source/DynamicalModels/ObjectModels/imstkAbstractDynamicalModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkAbstractDynamicalModel.cpp
@@ -21,9 +21,15 @@
#include "imstkAbstractDynamicalModel.h"
#include "imstkLogger.h"
+#include "imstkTaskGraph.h"
namespace imstk
{
+AbstractDynamicalModel::AbstractDynamicalModel(DynamicalModelType type) :
+ m_type(type), m_numDOF(0), m_taskGraph(std::make_shared<TaskGraph>("AbstractDynamicalModel_Source", "AbstractDynamicalModel_Sink"))
+{
+}
+
bool
AbstractDynamicalModel::isGeometryValid(const std::shared_ptr<Geometry> geometry)
{
@@ -65,4 +71,17 @@ AbstractDynamicalModel::setModelGeometry(std::shared_ptr<Geometry> geometry)
LOG(WARNING) << "Invalid geometry for Model";
}
}
+
+void
+AbstractDynamicalModel::initGraphEdges()
+{
+ m_taskGraph->clearEdges();
+ initGraphEdges(m_taskGraph->getSource(), m_taskGraph->getSink());
+}
+
+void
+AbstractDynamicalModel::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink)
+{
+ m_taskGraph->addEdge(source, sink);
+}
} // imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/imstkAbstractDynamicalModel.h b/Source/DynamicalModels/ObjectModels/imstkAbstractDynamicalModel.h
index 6d7a547292831a79e2f71e79d414da4e3cd3b5cb..8201dde345b65c1dc7b37e7e1f2c629440afc5a8 100644
--- a/Source/DynamicalModels/ObjectModels/imstkAbstractDynamicalModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkAbstractDynamicalModel.h
@@ -26,9 +26,13 @@
#include <set>
#include <string>
+#include <unordered_map>
namespace imstk
{
+class TaskGraph;
+class TaskNode;
+
///
/// \brief Type of the time dependent mathematical model
///
@@ -58,7 +62,6 @@ enum class TimeSteppingType
class AbstractDynamicalModel
{
public:
-
///
/// \brief Type of the update of the state of the body
///
@@ -71,17 +74,13 @@ public:
None
};
-public:
- ///
- /// \brief Constructor
- ///
- AbstractDynamicalModel(DynamicalModelType type = DynamicalModelType::None) : m_type(type), m_numDOF(0) {}
+protected:
+ AbstractDynamicalModel(DynamicalModelType type = DynamicalModelType::None);
- ///
- /// \brief Destructor
- ///
+public:
virtual ~AbstractDynamicalModel() = default;
+public:
///
/// \brief Reset the current state to the initial state
///
@@ -93,6 +92,8 @@ public:
std::size_t getNumDegreeOfFreedom() const { return m_numDOF; }
void setNumDegreeOfFreedom(const size_t nDof) { m_numDOF = nDof; }
+ std::shared_ptr<TaskGraph> getTaskGraph() const { return m_taskGraph; }
+
///
/// \brief Get the type of the object
///
@@ -104,9 +105,9 @@ public:
virtual void updateBodyStates(const Vectord& q, const StateUpdateType updateType = StateUpdateType::Displacement) = 0;
///
- /// \brief
+ /// \brief Update the geometry of the model
///
- virtual void updatePhysicsGeometry() {}
+ virtual void updatePhysicsGeometry() { }
///
/// \brief Set the time step size
@@ -138,20 +139,33 @@ public:
///
virtual bool initialize() = 0;
+ ///
+ /// \brief Initializes the edges of the graph
+ ///
+ void initGraphEdges();
+
///
/// \brief Set the type of approach used to update the time step size after every frame
///
virtual void setTimeStepSizeType(const TimeSteppingType type) { m_timeStepSizeType = type; }
TimeSteppingType getTimeStepSizeType() { return m_timeStepSizeType; }
+protected:
+ ///
+ /// \brief Setup connectivity of the compute graph
+ ///
+ virtual void initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink);
+
protected:
DynamicalModelType m_type; ///> Mathematical model type
std::size_t m_numDOF; ///> Total number of degree of freedom
- std::shared_ptr<Geometry> m_geometry; ///> Physics geometry of the model
+ std::shared_ptr<Geometry> m_geometry = nullptr; ///> Physics geometry of the model
std::set<Geometry::Type> m_validGeometryTypes; ///> Valid geometry types of this model
TimeSteppingType m_timeStepSizeType = TimeSteppingType::Fixed;
+
+ std::shared_ptr<TaskGraph> m_taskGraph = nullptr;
};
} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/imstkDynamicalModel.h b/Source/DynamicalModels/ObjectModels/imstkDynamicalModel.h
index e6b8ee325a45672956f2c12b40c35981ece876c6..b1b79d3556403de0ca74a1e604586454ebea3100 100644
--- a/Source/DynamicalModels/ObjectModels/imstkDynamicalModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkDynamicalModel.h
@@ -62,7 +62,7 @@ public:
///
/// \brief Reset the current state to the initial state
///
- void resetToInitialState() override
+ virtual void resetToInitialState() override
{
m_currentState->setState(m_initialState);
m_previousState->setState(m_initialState);
diff --git a/Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.cpp b/Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.cpp
index 02d42bdfcfbfad82ad859c45fbcf1eb9547f8c2e..00c523e8bac8825ffcfe97c79632a2cdbe3bb178 100644
--- a/Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.cpp
@@ -24,17 +24,18 @@
//imstk
#include "imstkFEMDeformableBodyModel.h"
-#include "imstkStVKForceModel.h"
-#include "imstkLinearFEMForceModel.h"
+#include "imstkConjugateGradient.h"
#include "imstkCorotationalFEMForceModel.h"
+#include "imstkInternalForceModel.h"
#include "imstkIsotropicHyperelasticFEMForceModel.h"
-#include "imstkVegaMeshIO.h"
-#include "imstkVolumetricMesh.h"
+#include "imstkLinearFEMForceModel.h"
+#include "imstkMath.h"
+#include "imstkSolverBase.h"
+#include "imstkStVKForceModel.h"
+#include "imstkTaskGraph.h"
#include "imstkTimeIntegrator.h"
#include "imstkVegaMeshIO.h"
-#include "imstkNewtonSolver.h"
-#include "imstkInternalForceModel.h"
-#include "imstkMath.h"
+#include "imstkVolumetricMesh.h"
// vega
#include "generateMassMatrix.h"
@@ -56,6 +57,8 @@ FEMDeformableBodyModel::FEMDeformableBodyModel() :
Geometry::Type::TetrahedralMesh,
Geometry::Type::HexahedralMesh
};
+
+ m_solveNode = m_taskGraph->addFunction("FEMModel_Solve", [&]() { getSolver()->solve(); });
}
FEMDeformableBodyModel::~FEMDeformableBodyModel()
@@ -86,7 +89,6 @@ FEMDeformableBodyModel::configure(const std::string& configFileName)
vegaConfigFileOptions.addOptionOptional("deformationCompliance", &m_FEModelConfig->m_deformationCompliance, m_FEModelConfig->m_deformationCompliance);
vegaConfigFileOptions.addOptionOptional("compressionResistance", &m_FEModelConfig->m_compressionResistance, m_FEModelConfig->m_compressionResistance);
vegaConfigFileOptions.addOptionOptional("inversionThreshold", &m_FEModelConfig->m_inversionThreshold, m_FEModelConfig->m_inversionThreshold);
- vegaConfigFileOptions.addOptionOptional("numberOfThreads", &m_FEModelConfig->m_numberOfThreads, m_FEModelConfig->m_numberOfThreads);
vegaConfigFileOptions.addOptionOptional("gravity", &m_FEModelConfig->m_gravity, m_FEModelConfig->m_gravity);
// Parse the configuration file
@@ -153,47 +155,37 @@ FEMDeformableBodyModel::configure(std::shared_ptr<FEMModelConfig> config)
m_FEModelConfig = config;
}
-void
-FEMDeformableBodyModel::setForceModelConfiguration(std::shared_ptr<FEMModelConfig> fmConfig)
-{
- m_FEModelConfig = fmConfig;
-}
-
-std::shared_ptr<imstk::FEMModelConfig>
-FEMDeformableBodyModel::getForceModelConfiguration() const
+bool
+FEMDeformableBodyModel::initialize()
{
- return m_FEModelConfig;
-}
+ // prerequisite of for successfully initializing
+ CHECK(m_geometry != nullptr && m_FEModelConfig != nullptr) << "DeformableBodyModel::initialize: Physics mesh or force model configuration not set yet!";
-void
-FEMDeformableBodyModel::setInternalForceModel(std::shared_ptr<InternalForceModel> fm)
-{
- m_internalForceModel = fm;
-}
+ // Setup default solver if model doesn't yet have one
+ if (m_solver == nullptr)
+ {
+ // Create a nonlinear system
+ auto nlSystem = std::make_shared<NonLinearSystem<SparseMatrixd>>(getFunction(), getFunctionGradient());
-std::shared_ptr<imstk::InternalForceModel>
-FEMDeformableBodyModel::getInternalForceModel() const
-{
- return m_internalForceModel;
-}
+ nlSystem->setUnknownVector(getUnknownVec());
+ nlSystem->setUpdateFunction(getUpdateFunction());
+ nlSystem->setUpdatePreviousStatesFunction(getUpdatePrevStateFunction());
-void
-FEMDeformableBodyModel::setTimeIntegrator(std::shared_ptr<TimeIntegrator> timeIntegrator)
-{
- m_timeIntegrator = timeIntegrator;
-}
+ // Create a linear solver
+ auto linSolver = std::make_shared<ConjugateGradient>();
-std::shared_ptr<imstk::TimeIntegrator>
-FEMDeformableBodyModel::getTimeIntegrator() const
-{
- return m_timeIntegrator;
-}
+ if (linSolver->getType() == imstk::LinearSolver<imstk::SparseMatrixd>::Type::GaussSeidel
+ && isFixedBCImplemented())
+ {
+ LOG(WARNING) << "The GS solver may not be viable!";
+ }
-bool
-FEMDeformableBodyModel::initialize()
-{
- // prerequisite of for successfully initializing
- CHECK(m_geometry != nullptr && m_FEModelConfig != nullptr) << "DeformableBodyModel::initialize: Physics mesh or force model configuration not set yet!";
+ // Create a non-linear solver and add to the scene
+ auto nlSolver = std::make_shared<NewtonSolver<SparseMatrixd>>();
+ nlSolver->setLinearSolver(linSolver);
+ nlSolver->setSystem(nlSystem);
+ setSolver(nlSolver);
+ }
auto physicsMesh = std::dynamic_pointer_cast<imstk::VolumetricMesh>(this->getModelGeometry());
m_vegaPhysicsMesh = VegaMeshIO::convertVolumetricMeshToVegaMesh(physicsMesh);
@@ -293,7 +285,9 @@ bool
FEMDeformableBodyModel::initializeForceModel()
{
const double g = m_FEModelConfig->m_gravity;
- const bool isGravityPresent = (g > 0) ? true : false;
+ // Since vega 4.0 doesn't add gravity correcntly in all cases, we do it ourselves; see \ref initializeGravityForce
+ // const bool isGravityPresent = (g > 0) ? true : false;
+ const bool isGravityPresent = false;
m_numDOF = (size_t)m_vegaPhysicsMesh->getNumVertices() * 3;
@@ -775,12 +769,6 @@ FEMDeformableBodyModel::initializeEigenMatrixFromVegaMatrix(const vega::SparseMa
eigenMatrix.makeCompressed();
}
-Vectord&
-FEMDeformableBodyModel::getContactForce()
-{
- return m_Fcontact;
-}
-
void
FEMDeformableBodyModel::setFixedSizeTimeStepping()
{
@@ -799,4 +787,12 @@ FEMDeformableBodyModel::getTimeStep() const
{
return m_timeIntegrator->getTimestepSize();
};
+
+void
+FEMDeformableBodyModel::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink)
+{
+ // Setup graph connectivity
+ m_taskGraph->addEdge(source, m_solveNode);
+ m_taskGraph->addEdge(m_solveNode, sink);
+}
} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.h b/Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.h
index 9641e0cba7dfb8409c037f238f0619cd75070c82..01c1b9ea94fd57850b15fff607979ac2a1a487ae 100644
--- a/Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.h
@@ -25,6 +25,7 @@
#include "imstkDynamicalModel.h"
#include "imstkInternalForceModelTypes.h"
#include "imstkNonLinearSystem.h"
+#include "imstkNewtonSolver.h"
// vega
#include "sparseMatrix.h"
@@ -36,12 +37,10 @@ class VolumetricMesh;
namespace imstk
{
+class InternalForceModel;
class TimeIntegrator;
+class SolverBase;
class VegaMeshIO;
-// class NewtonSolver<SparseMatrixd>;
-// template <typename SystemMatrix>
-// class NewtonSolver;
-class InternalForceModel;
struct FEMModelConfig
{
@@ -59,9 +58,6 @@ struct FEMModelConfig
double m_compressionResistance = 500.0;
double m_inversionThreshold = -std::numeric_limits<double>::max();
double m_gravity = 9.81;
-
- // \todo remove from here ?
- int m_numberOfThreads = 4;
};
///
@@ -87,6 +83,7 @@ public:
///
~FEMDeformableBodyModel();
+public:
///
/// \brief Configure the force model from external file
///
@@ -101,20 +98,20 @@ public:
///
/// \brief Set/Get force model configuration
///
- void setForceModelConfiguration(std::shared_ptr<FEMModelConfig> fmConfig);
- std::shared_ptr<FEMModelConfig> getForceModelConfiguration() const;
+ void setForceModelConfiguration(std::shared_ptr<FEMModelConfig> fmConfig) { this->m_FEModelConfig = fmConfig; }
+ std::shared_ptr<FEMModelConfig> getForceModelConfiguration() const { return m_FEModelConfig; }
///
/// \brief Set/Get internal force model
///
- void setInternalForceModel(std::shared_ptr<InternalForceModel> fm);
- std::shared_ptr<InternalForceModel> getInternalForceModel() const;
+ void setInternalForceModel(std::shared_ptr<InternalForceModel> fm) { m_internalForceModel = fm; }
+ std::shared_ptr<InternalForceModel> getInternalForceModel() const { return m_internalForceModel; }
///
/// \brief Set/Get time integrator
///
- void setTimeIntegrator(std::shared_ptr<TimeIntegrator> timeIntegrator);
- std::shared_ptr<TimeIntegrator> getTimeIntegrator() const;
+ void setTimeIntegrator(std::shared_ptr<TimeIntegrator> timeIntegrator) { this->m_timeIntegrator = timeIntegrator; }
+ std::shared_ptr<TimeIntegrator> getTimeIntegrator() const { return m_timeIntegrator; }
///
/// \brief Load the initial conditions of the deformable object
@@ -230,7 +227,7 @@ public:
///
/// \brief Get the contact force vector
///
- Vectord& getContactForce();
+ Vectord& getContactForce() { return m_Fcontact; }
///
/// \brief Returns the unknown vectors
@@ -269,39 +266,54 @@ public:
void disableFixedBC() { m_implementFixedBC = false; };
bool isFixedBCImplemented() const { return m_implementFixedBC; };
+ std::shared_ptr<TaskNode> getSolveNode() const { return m_solveNode; }
+
+ std::shared_ptr<SolverBase> getSolver() const { return m_solver; }
+ void setSolver(std::shared_ptr<SolverBase> solver) { this->m_solver = solver; }
+
protected:
- std::shared_ptr<InternalForceModel> m_internalForceModel; ///> Mathematical model for intenal forces
- std::shared_ptr<TimeIntegrator> m_timeIntegrator; ///> Time integrator
- std::shared_ptr<System> m_nonLinearSystem; ///> Nonlinear system resulting from TI and force model
+ ///
+ /// \brief Setup the computational graph of FEM
+ ///
+ void initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink) override;
- std::shared_ptr<FEMModelConfig> m_FEModelConfig;
+protected:
+ std::shared_ptr<SolverBase> m_solver = nullptr;
+ std::shared_ptr<InternalForceModel> m_internalForceModel = nullptr; ///> Mathematical model for intenal forces
+ std::shared_ptr<TimeIntegrator> m_timeIntegrator = nullptr; ///> Time integrator
+ std::shared_ptr<NonLinearSystem<SparseMatrixd>> m_nonLinearSystem = nullptr; ///> Nonlinear system resulting from TI and force model
+
+ std::shared_ptr<FEMModelConfig> m_FEModelConfig = nullptr;
/// Matrices typical to a elastodynamics and 2nd order analogous systems
- SparseMatrixd m_M; ///> Mass matrix
- SparseMatrixd m_C; ///> Damping coefficient matrix
- SparseMatrixd m_K; ///> Tangent (derivative of internal force w.r.t displacements) stiffness matrix
- SparseMatrixd m_Keff; ///> Effective stiffness matrix (dependent on internal force model and time integrator)
+ SparseMatrixd m_M; ///> Mass matrix
+ SparseMatrixd m_C; ///> Damping coefficient matrix
+ SparseMatrixd m_K; ///> Tangent (derivative of internal force w.r.t displacements) stiffness matrix
+ SparseMatrixd m_Keff; ///> Effective stiffness matrix (dependent on internal force model and time integrator)
- Vectord m_Finternal; ///> Vector of internal forces
- Vectord m_Feff; ///> Vector of effective forces
- Vectord m_Fcontact; ///> Vector of contact forces
- Vectord m_Fgravity; ///> Vector of gravity forces
- Vectord m_FexplicitExternal; ///> Vector of explicitly defined external forces
- Vectord m_qSol; ///> Vector to maintain solution at each iteration of nonlinear solver
+ Vectord m_Finternal; ///> Vector of internal forces
+ Vectord m_Feff; ///> Vector of effective forces
+ Vectord m_Fcontact; ///> Vector of contact forces
+ Vectord m_Fgravity; ///> Vector of gravity forces
+ Vectord m_FexplicitExternal; ///> Vector of explicitly defined external forces
+ Vectord m_qSol; ///> Vector to maintain solution at each iteration of nonlinear solver
- std::shared_ptr<vega::VolumetricMesh> m_vegaPhysicsMesh; ///> Mesh used for Physics
- std::shared_ptr<vega::SparseMatrix> m_vegaMassMatrix; ///> Vega mass matrix
- std::shared_ptr<vega::SparseMatrix> m_vegaTangentStiffnessMatrix; ///> Vega Tangent stiffness matrix
- std::shared_ptr<vega::SparseMatrix> m_vegaDampingMatrix; ///> Vega Laplacian damping matrix
+ std::shared_ptr<vega::VolumetricMesh> m_vegaPhysicsMesh = nullptr; ///> Mesh used for Physics
+ std::shared_ptr<vega::SparseMatrix> m_vegaMassMatrix = nullptr; ///> Vega mass matrix
+ std::shared_ptr<vega::SparseMatrix> m_vegaTangentStiffnessMatrix = nullptr; ///> Vega Tangent stiffness matrix
+ std::shared_ptr<vega::SparseMatrix> m_vegaDampingMatrix = nullptr; ///> Vega Laplacian damping matrix
- std::vector<std::size_t> m_fixedNodeIds; ///> Nodal IDs of the nodes that are fixed
+ std::vector<std::size_t> m_fixedNodeIds; ///> Nodal IDs of the nodes that are fixed
- StateUpdateType m_updateType = StateUpdateType::DeltaVelocity; ///> Update type of the model
+ StateUpdateType m_updateType = StateUpdateType::DeltaVelocity; ///> Update type of the model
- bool m_damped = false; ///> Viscous or structurally damped system
+ bool m_damped = false; ///> Viscous or structurally damped system
// If this is true, the tangent stiffness and force vector will be modified to
// accommodate (the rows and columns will be nullified) the fixed boundary conditions
bool m_implementFixedBC = true;
+
+private:
+ std::shared_ptr<TaskNode> m_solveNode = nullptr;
};
} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
index 188949934a7743c086eb10660386b07ba27b1fda..a5a583654b712f1d1df16e669756a55adaa4e6d8 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
@@ -19,34 +19,54 @@
=========================================================================*/
-#include "imstkGraph.h"
#include "imstkPbdModel.h"
-#include "imstkTetrahedralMesh.h"
-#include "imstkSurfaceMesh.h"
+#include "imstkGeometryUtilities.h"
+#include "imstkGraph.h"
#include "imstkLineMesh.h"
-#include "imstkPbdVolumeConstraint.h"
-#include "imstkPbdDistanceConstraint.h"
-#include "imstkPbdDihedralConstraint.h"
#include "imstkPbdAreaConstraint.h"
#include "imstkPbdBendConstraint.h"
-#include "imstkPbdFETetConstraint.h"
-#include "imstkPbdFEHexConstraint.h"
#include "imstkPbdConstantDensityConstraint.h"
-#include "imstkParallelUtils.h"
-#include "imstkGeometryUtilities.h"
-#include "imstkMeshIO.h"
-//#include "imstkColor.h"
+#include "imstkPbdDihedralConstraint.h"
+#include "imstkPbdDistanceConstraint.h"
+#include "imstkPbdFETetConstraint.h"
+#include "imstkPbdSolver.h"
+#include "imstkPbdVolumeConstraint.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkTaskGraph.h"
+#include "imstkTetrahedralMesh.h"
#include <unordered_map>
namespace imstk
{
+PbdModel::PbdModel() : DynamicalModel(DynamicalModelType::PositionBasedDynamics),
+ m_constraints(std::make_shared<PBDConstraintVector>()),
+ m_partitionedConstraints(std::make_shared<std::vector<PBDConstraintVector>>()),
+ m_mass(std::make_shared<StdVectorOfReal>()),
+ m_invMass(std::make_shared<StdVectorOfReal>()),
+ m_parameters(std::make_shared<PBDModelConfig>())
+{
+ m_validGeometryTypes = {
+ Geometry::Type::PointSet,
+ Geometry::Type::LineMesh,
+ Geometry::Type::SurfaceMesh,
+ Geometry::Type::TetrahedralMesh,
+ Geometry::Type::HexahedralMesh
+ };
+
+ // Setup PBD compute nodes
+ m_integrationPositionNode = m_taskGraph->addFunction("PbdModel_IntegratePosition", std::bind(&PbdModel::integratePosition, this));
+ m_updateCollisionGeometryNode = m_taskGraph->addFunction("PbdModel_UpdateCollisionGeometry", std::bind(&PbdModel::updatePhysicsGeometry, this));
+ m_solveConstraintsNode = m_taskGraph->addFunction("PbdModel_SolveConstraints", [&]() { m_pbdSolver->solve(); }); // Avoids rebinding on solver swap
+ m_updateVelocityNode = m_taskGraph->addFunction("PbdModel_UpdateVelocity", std::bind(&PbdModel::updateVelocity, this));
+}
+
void
PBDModelConfig::enableConstraint(PbdConstraint::Type type, double stiffness)
{
LOG_IF(FATAL, (type == PbdConstraint::Type::FEMTet || type == PbdConstraint::Type::FEMHex))
<< "FEM constraint should be enabled by the enableFEMConstraint function";
- m_RegularConstraints.push_back({ type, stiffness });
+ m_regularConstraints.push_back({ type, stiffness });
}
void
@@ -58,11 +78,11 @@ PBDModelConfig::enableFEMConstraint(PbdConstraint::Type type, PbdFEMConstraint::
}
void
-PbdModel::configure(const std::shared_ptr<PBDModelConfig>& params)
+PbdModel::configure(std::shared_ptr<PBDModelConfig> params)
{
LOG_IF(FATAL, (!this->getModelGeometry())) << "PbdModel::configure - Set PBD Model geometry before configuration!";
- m_Parameters = params;
+ m_parameters = params;
this->setNumDegreeOfFreedom(std::dynamic_pointer_cast<PointSet>(m_geometry)->getNumVertices() * 3);
}
@@ -76,31 +96,38 @@ PbdModel::initialize()
m_previousState = std::make_shared<PbdState>();
m_currentState = std::make_shared<PbdState>();
- bool option[3] = { 1, 0, 0 };
- m_initialState->initialize(m_mesh, option);
- m_previousState->initialize(m_mesh, option);
-
- option[1] = option[2] = 1;
- m_currentState->initialize(m_mesh, option);
-
- m_initialState->setPositions(m_mesh->getVertexPositions());
- m_currentState->setPositions(m_mesh->getVertexPositions());
+ m_initialState->initialize(m_mesh->getVertexPositions());
+ m_previousState->initialize(m_mesh->getVertexPositions());
+ m_currentState->initialize(m_mesh->getVertexPositions());
auto numParticles = m_mesh->getNumVertices();
- m_mass.resize(numParticles, 0);
- m_invMass.resize(numParticles, 0);
- setUniformMass(m_Parameters->m_uniformMassValue);
+ m_mass->resize(numParticles, 0);
+ m_invMass->resize(numParticles, 0);
+ setUniformMass(m_parameters->m_uniformMassValue);
- for (auto i : m_Parameters->m_fixedNodeIds)
+ for (auto i : m_parameters->m_fixedNodeIds)
{
setFixedPoint(i);
}
bool bOK = true; // Return immediately if some constraint failed to initialize
+ // Setup the default pbd solver if none exists
+ if (m_pbdSolver == nullptr)
+ {
+ m_pbdSolver = std::make_shared<PbdSolver>();
+ m_pbdSolver->setIterations(m_parameters->m_iterations);
+ m_pbdSolver->setSolverType(m_parameters->m_solverType);
+ }
+ m_pbdSolver->setPositions(getCurrentState()->getPositions());
+ m_pbdSolver->setInvMasses(getInvMasses());
+ m_pbdSolver->setConstraints(getConstraints());
+ m_pbdSolver->setPartitionedConstraints(getPartitionedConstraints());
+ m_pbdSolver->setTimeStep(m_parameters->m_dt);
+
// Initialize FEM constraints
- for (auto& constraint: m_Parameters->m_FEMConstraints)
+ for (auto& constraint: m_parameters->m_FEMConstraints)
{
computeElasticConstants();
if (!initializeFEMConstraints(constraint.second))
@@ -110,8 +137,17 @@ PbdModel::initialize()
}
// Initialize other constraints
- for (auto& constraint: m_Parameters->m_RegularConstraints)
+ for (auto& constraint: m_parameters->m_regularConstraints)
{
+ if (m_parameters->m_solverType == PbdConstraint::SolverType::PBD && constraint.second > 1.0)
+ {
+ LOG(WARNING) << "for PBD, k should be between [0, 1]";
+ }
+ else if (m_parameters->m_solverType == PbdConstraint::SolverType::xPBD && constraint.second <= 1.0)
+ {
+ LOG(WARNING) << "for xPBD, k is Young's Modulu, and should be much larger than 1";
+ }
+
if (!bOK)
{
return false;
@@ -148,30 +184,45 @@ PbdModel::initialize()
}
// Partition constraints for parallel computation
- partitionConstraints();
+ if (!m_partitioned)
+ {
+ this->partitionConstraints();
+ m_partitioned = true;
+ }
this->setTimeStepSizeType(m_timeStepSizeType);
return bOK;
}
+void
+PbdModel::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink)
+{
+ // Setup graph connectivity
+ m_taskGraph->addEdge(source, m_integrationPositionNode);
+ m_taskGraph->addEdge(m_integrationPositionNode, m_updateCollisionGeometryNode);
+ m_taskGraph->addEdge(m_updateCollisionGeometryNode, m_solveConstraintsNode);
+ m_taskGraph->addEdge(m_solveConstraintsNode, m_updateVelocityNode);
+ m_taskGraph->addEdge(m_updateVelocityNode, sink);
+}
+
void
PbdModel::computeElasticConstants()
{
- if (std::abs(m_Parameters->m_mu) < MIN_REAL
- && std::abs(m_Parameters->m_lambda) < MIN_REAL)
+ if (std::abs(m_parameters->m_femParams->m_mu) < MIN_REAL
+ && std::abs(m_parameters->m_femParams->m_lambda) < MIN_REAL)
{
- const auto E = m_Parameters->m_YoungModulus;
- const auto nu = m_Parameters->m_PoissonRatio;
- m_Parameters->m_mu = E / Real(2.0) / (Real(1.0) + nu);
- m_Parameters->m_lambda = E * nu / ((Real(1.0) + nu) * (Real(1.0) - Real(2.0) * nu));
+ const auto E = m_parameters->m_femParams->m_YoungModulus;
+ const auto nu = m_parameters->m_femParams->m_PoissonRatio;
+ m_parameters->m_femParams->m_mu = E / Real(2.0) / (Real(1.0) + nu);
+ m_parameters->m_femParams->m_lambda = E * nu / ((Real(1.0) + nu) * (Real(1.0) - Real(2.0) * nu));
}
else
{
- const auto mu = m_Parameters->m_mu;
- const auto lambda = m_Parameters->m_lambda;
- m_Parameters->m_YoungModulus = mu * (Real(3.0) * lambda + Real(2.0) * mu) / (lambda + mu);
- m_Parameters->m_PoissonRatio = lambda / Real(2.0) / (lambda + mu);
+ const auto mu = m_parameters->m_femParams->m_mu;
+ const auto lambda = m_parameters->m_femParams->m_lambda;
+ m_parameters->m_femParams->m_YoungModulus = mu * (Real(3.0) * lambda + Real(2.0) * mu) / (lambda + mu);
+ m_parameters->m_femParams->m_PoissonRatio = lambda / Real(2.0) / (lambda + mu);
}
}
@@ -192,9 +243,10 @@ PbdModel::initializeFEMConstraints(PbdFEMConstraint::MaterialType type)
{
auto& tet = elements[k];
auto c = std::make_shared<PbdFEMTetConstraint>(type);
- c->initConstraint(*this, tet[0], tet[1], tet[2], tet[3]);
+ c->initConstraint(*m_initialState->getPositions(),
+ tet[0], tet[1], tet[2], tet[3], m_parameters->m_femParams);
lock.lock();
- m_constraints.push_back(std::move(c));
+ m_constraints->push_back(std::move(c));
lock.unlock();
});
return true;
@@ -216,9 +268,10 @@ PbdModel::initializeVolumeConstraints(const double stiffness)
{
auto& tet = elements[k];
auto c = std::make_shared<PbdVolumeConstraint>();
- c->initConstraint(*this, tet[0], tet[1], tet[2], tet[3], stiffness);
+ c->initConstraint(*m_initialState->getPositions(),
+ tet[0], tet[1], tet[2], tet[3], stiffness);
lock.lock();
- m_constraints.push_back(std::move(c));
+ m_constraints->push_back(std::move(c));
lock.unlock();
});
return true;
@@ -238,8 +291,8 @@ PbdModel::initializeDistanceConstraints(const double stiffness)
{
E[i1][i2] = 0;
auto c = std::make_shared<PbdDistanceConstraint>();
- c->initConstraint(*this, i1, i2, stiffness);
- m_constraints.push_back(std::move(c));
+ c->initConstraint(*m_initialState->getPositions(), i1, i2, stiffness);
+ m_constraints->push_back(std::move(c));
}
};
@@ -310,9 +363,9 @@ PbdModel::initializeAreaConstraints(const double stiffness)
{
auto& tri = elements[k];
auto c = std::make_shared<PbdAreaConstraint>();
- c->initConstraint(*this, tri[0], tri[1], tri[2], stiffness);
+ c->initConstraint(*m_initialState->getPositions(), tri[0], tri[1], tri[2], stiffness);
lock.lock();
- m_constraints.push_back(std::move(c));
+ m_constraints->push_back(std::move(c));
lock.unlock();
});
return true;
@@ -338,8 +391,8 @@ PbdModel::initializeBendConstraints(const double stiffness)
}
auto c = std::make_shared<PbdBendConstraint>();
- c->initConstraint(*this, i1, i2, i3, k);
- m_constraints.push_back(std::move(c));
+ c->initConstraint(*m_initialState->getPositions(), i1, i2, i3, k);
+ m_constraints->push_back(std::move(c));
};
// Create constraints
@@ -411,8 +464,8 @@ PbdModel::initializeDihedralConstraints(const double stiffness)
}
}
auto c = std::make_shared<PbdDihedralConstraint>();
- c->initConstraint(*this, tri[2], tri[idx], tri[0], tri[1], stiffness);
- m_constraints.push_back(std::move(c));
+ c->initConstraint(*m_initialState->getPositions(), tri[2], tri[idx], tri[0], tri[1], stiffness);
+ m_constraints->push_back(std::move(c));
}
}
};
@@ -448,8 +501,8 @@ PbdModel::initializeConstantDensityConstraint(const double stiffness)
<< "Constant constraint should come with a mesh!";
auto c = std::make_shared<PbdConstantDensityConstraint>();
- c->initConstraint(*this, stiffness);
- m_constraints.push_back(std::move(c));
+ c->initConstraint(*m_initialState->getPositions(), stiffness);
+ m_constraints->push_back(std::move(c));
return true;
}
@@ -458,10 +511,14 @@ void
PbdModel::partitionConstraints(const bool print)
{
// Form the map { vertex : list_of_constraints_involve_vertex }
+ PBDConstraintVector& allConstraints = *m_constraints;
+
+ //std::cout << "---------partitionConstraints: " << allConstraints.size() << std::endl;
+
std::unordered_map<size_t, std::vector<size_t>> vertexConstraints;
- for (size_t constrIdx = 0; constrIdx < m_constraints.size(); ++constrIdx)
+ for (size_t constrIdx = 0; constrIdx < allConstraints.size(); ++constrIdx)
{
- const auto& constr = m_constraints[constrIdx];
+ const auto& constr = allConstraints[constrIdx];
for (const auto& vIds : constr->getVertexIds())
{
vertexConstraints[vIds].push_back(constrIdx);
@@ -470,7 +527,7 @@ PbdModel::partitionConstraints(const bool print)
// Add edges to the constraint graph
// Each edge represent a shared vertex between two constraints
- Graph constraintGraph(m_constraints.size());
+ Graph constraintGraph(allConstraints.size());
for (const auto& kv : vertexConstraints)
{
const auto& constraints = kv.second; // the list of constraints for a vertex
@@ -485,89 +542,68 @@ PbdModel::partitionConstraints(const bool print)
vertexConstraints.clear();
// do graph coloring for the constraint graph
- const auto coloring = constraintGraph.doColoring();
+ const auto coloring = constraintGraph.doColoring(Graph::ColoringMethod::WelshPowell);
const auto& partitionIndices = coloring.first;
const auto numPartitions = coloring.second;
- assert(partitionIndices.size() == m_constraints.size());
+ assert(partitionIndices.size() == allConstraints.size());
- m_partitionedConstraints.resize(0);
- m_partitionedConstraints.resize(static_cast<size_t>(numPartitions));
+ std::vector<PBDConstraintVector>& partitionedConstraints = *m_partitionedConstraints;
+ partitionedConstraints.resize(0);
+ partitionedConstraints.resize(static_cast<size_t>(numPartitions));
for (size_t constrIdx = 0; constrIdx < partitionIndices.size(); ++constrIdx)
{
const auto partitionIdx = partitionIndices[constrIdx];
- m_partitionedConstraints[partitionIdx].push_back(std::move(m_constraints[constrIdx]));
+ partitionedConstraints[partitionIdx].push_back(std::move(allConstraints[constrIdx]));
}
// If a partition has size smaller than the partition threshold, then move its constraints back
// These constraints will be processed sequentially
// Because small size partitions yield bad performance upon running in parallel
- m_constraints.resize(0);
- for (const auto& constraints : m_partitionedConstraints)
+ allConstraints.resize(0);
+ for (const auto& constraints : partitionedConstraints)
{
if (constraints.size() < m_partitionThreshold)
{
for (size_t constrIdx = 0; constrIdx < constraints.size(); ++constrIdx)
{
- m_constraints.push_back(std::move(constraints[constrIdx]));
+ allConstraints.push_back(std::move(constraints[constrIdx]));
}
}
}
// Remove all empty partitions
size_t writeIdx = 0;
- for (size_t readIdx = 0; readIdx < m_partitionedConstraints.size(); ++readIdx)
+ for (size_t readIdx = 0; readIdx < partitionedConstraints.size(); ++readIdx)
{
- if (m_partitionedConstraints[readIdx].size() >= m_partitionThreshold)
+ if (partitionedConstraints[readIdx].size() >= m_partitionThreshold)
{
- m_partitionedConstraints[writeIdx++] = std::move(m_partitionedConstraints[readIdx]);
+ partitionedConstraints[writeIdx++] = std::move(partitionedConstraints[readIdx]);
}
}
- m_partitionedConstraints.resize(writeIdx);
+ partitionedConstraints.resize(writeIdx);
// Print
if (print)
{
size_t numConstraints = 0;
int idx = 0;
- for (const auto& constraints : m_partitionedConstraints)
+ for (const auto& constraints : partitionedConstraints)
{
std::cout << "Partition # " << idx++ << " | # nodes: " << constraints.size() << std::endl;
numConstraints += constraints.size();
}
- std::cout << "Sequential processing # nodes: " << m_constraints.size() << std::endl;
- numConstraints += m_constraints.size();
+ std::cout << "Sequential processing # nodes: " << allConstraints.size() << std::endl;
+ numConstraints += allConstraints.size();
std::cout << "Total constraints: " << numConstraints << " | Graph size: "
<< constraintGraph.size() << std::endl;
}
}
-void
-PbdModel::projectConstraints()
-{
- unsigned int i = 0;
- while (++i < m_Parameters->m_maxIter)
- {
- for (auto c: m_constraints)
- {
- c->solvePositionConstraint(*this);
- }
-
- for (auto& partitionConstraints : m_partitionedConstraints)
- {
- ParallelUtils::parallelFor(partitionConstraints.size(),
- [&](const size_t idx)
- {
- partitionConstraints[idx]->solvePositionConstraint(*this);
- });
- }
- }
-}
-
void
PbdModel::updatePhysicsGeometry()
{
- m_mesh->setVertexPositions(m_currentState->getPositions());
+ m_mesh->setVertexPositions(*m_currentState->getPositions());
}
void
@@ -582,7 +618,7 @@ PbdModel::setTimeStepSizeType(const TimeSteppingType type)
m_timeStepSizeType = type;
if (type == TimeSteppingType::Fixed)
{
- m_Parameters->m_dt = m_Parameters->m_DefaultDt;
+ m_parameters->m_dt = m_parameters->m_defaultDt;
}
}
@@ -591,57 +627,55 @@ PbdModel::setUniformMass(const double val)
{
if (val != 0.0)
{
- std::fill(m_mass.begin(), m_mass.end(), val);
- std::fill(m_invMass.begin(), m_invMass.end(), 1 / val);
+ std::fill(m_mass->begin(), m_mass->end(), val);
+ std::fill(m_invMass->begin(), m_invMass->end(), 1.0 / val);
}
else
{
- std::fill(m_invMass.begin(), m_invMass.end(), 0.0);
- std::fill(m_mass.begin(), m_mass.end(), 0.0);
+ std::fill(m_invMass->begin(), m_invMass->end(), 0.0);
+ std::fill(m_mass->begin(), m_mass->end(), 0.0);
}
}
void
PbdModel::setParticleMass(const double val, const size_t idx)
{
+ StdVectorOfReal& masses = *m_mass;
+ StdVectorOfReal& invMasses = *m_invMass;
if (idx < m_mesh->getNumVertices())
{
- m_mass[idx] = val;
- m_invMass[idx] = 1.0 / val;
+ masses[idx] = val;
+ invMasses[idx] = 1.0 / val;
}
}
void
PbdModel::setFixedPoint(const size_t idx)
{
+ StdVectorOfReal& invMasses = *m_invMass;
if (idx < m_mesh->getNumVertices())
{
- m_invMass[idx] = 0;
+ invMasses[idx] = 0.0;
}
}
-double
-PbdModel::getInvMass(const size_t idx) const
-{
- return m_invMass[idx];
-}
-
void
PbdModel::integratePosition()
{
- const auto& accn = m_currentState->getAccelerations();
- auto& prevPos = m_previousState->getPositions();
- auto& pos = m_currentState->getPositions();
- auto& vel = m_currentState->getVelocities();
+ StdVectorOfVec3d& prevPos = *m_previousState->getPositions();
+ StdVectorOfVec3d& pos = *m_currentState->getPositions();
+ StdVectorOfVec3d& vel = *m_currentState->getVelocities();
+ const StdVectorOfVec3d& accn = *m_currentState->getAccelerations();
+ const StdVectorOfReal& invMasses = *m_invMass;
ParallelUtils::parallelFor(m_mesh->getNumVertices(),
[&](const size_t i)
{
- if (std::abs(m_invMass[i]) > MIN_REAL)
+ if (std::abs(invMasses[i]) > MIN_REAL)
{
- vel[i] += (accn[i] + m_Parameters->m_gravity) * m_Parameters->m_dt;
+ vel[i] += (accn[i] + m_parameters->m_gravity) * m_parameters->m_dt;
prevPos[i] = pos[i];
- pos[i] += (1.0 - m_Parameters->m_viscousDampingCoeff) * vel[i] * m_Parameters->m_dt;
+ pos[i] += (1.0 - m_parameters->m_viscousDampingCoeff) * vel[i] * m_parameters->m_dt;
}
});
}
@@ -649,16 +683,17 @@ PbdModel::integratePosition()
void
PbdModel::updateVelocity()
{
- const auto& prevPos = m_previousState->getPositions();
- const auto& pos = m_currentState->getPositions();
- auto& vel = m_currentState->getVelocities();
+ const StdVectorOfVec3d& prevPos = *m_previousState->getPositions();
+ const StdVectorOfVec3d& pos = *m_currentState->getPositions();
+ StdVectorOfVec3d& vel = *m_currentState->getVelocities();
+ const StdVectorOfReal& invMasses = *m_invMass;
ParallelUtils::parallelFor(m_mesh->getNumVertices(),
[&](const size_t i)
{
- if (std::abs(m_invMass[i]) > MIN_REAL && m_Parameters->m_dt > 0.)
+ if (std::abs(invMasses[i]) > MIN_REAL && m_parameters->m_dt > 0.0)
{
- vel[i] = (pos[i] - prevPos[i]) / m_Parameters->m_dt;
+ vel[i] = (pos[i] - prevPos[i]) / m_parameters->m_dt;
}
});
}
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
index 6d882c25601e33e8e4191a6cd5183f997a66a73e..3cb8f5f587db0eb0be2026b6763c9edc72606856 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
@@ -21,18 +21,16 @@
#pragma once
-#include <vector>
-#include <Eigen/Dense>
-
-#include "imstkPbdConstraint.h"
-#include "imstkPbdFEMConstraint.h"
#include "imstkDynamicalModel.h"
+#include "imstkPbdCollisionConstraint.h"
+#include "imstkPbdFEMConstraint.h"
#include "imstkPbdState.h"
-#include "imstkMath.h"
+#include "imstkLogger.h"
namespace imstk
{
class PointSet;
+class PbdSolver;
///
/// \struct PBDModelConfig
@@ -40,28 +38,35 @@ class PointSet;
///
struct PBDModelConfig
{
- double m_uniformMassValue = 1.0; ///> Mass properties
- double m_viscousDampingCoeff = 0.01; ///> Viscous damping coefficient [0, 1]
+ double m_uniformMassValue = 1.0; ///> Mass properties
+ double m_viscousDampingCoeff = 0.01; ///> Viscous damping coefficient [0, 1]
- double m_contactStiffness = 1.0; ///> Contact stiffness for collisions
- double m_proximity = 0.1; ///> Proximity for collisions
+ std::shared_ptr<PbdCollisionConstraintConfig> collisionParams =
+ std::make_shared<PbdCollisionConstraintConfig>(PbdCollisionConstraintConfig
+ {
+ 0.1, // Proximity
+ 1.0 // Contact stiffness
+ }); ///> Info shared between the collision constraints
- unsigned int m_maxIter = 10; ///> Max. pbd iterations
- double m_dt = 0.0; ///> Time step size
- double m_DefaultDt = 0.01; ///> Default Time step size
+ unsigned int m_iterations = 10; ///> Pbd iterations
+ unsigned int m_collisionIterations = 5; ///> Pbd collision iterations
+ double m_dt = 0.0; ///> Time step size
+ double m_defaultDt = 0.01; ///> Default Time step size
- std::vector<std::size_t> m_fixedNodeIds; ///> Nodal IDs of the nodes that are fixed
- Vec3r m_gravity = Vec3r(0, -9.81, 0); ///> Gravity
+ std::vector<std::size_t> m_fixedNodeIds; ///> Nodal IDs of the nodes that are fixed
+ Vec3r m_gravity = Vec3r(0, -9.81, 0); ///> Gravity
- double m_mu = 0; ///> Lame constant, if constraint type is FEM
- double m_lambda = 0; ///> Lame constant, if constraint type is FEM
+ std::shared_ptr<PbdFEMConstraintConfig> m_femParams =
+ std::make_shared<PbdFEMConstraintConfig>(PbdFEMConstraintConfig
+ {
+ 0.0, // Lame constant, if constraint type is FEM
+ 0.0, // Lame constant, if constraint type is FEM
+ 1000.0, // FEM parameter, if constraint type is FEM
+ 0.2 // FEM parameter, if constraint type is FEM
+ }); ///> Info shared between the fem constraints
- double m_YoungModulus = 1000; ///> FEM parameter, if constraint type is FEM
- double m_PoissonRatio = 0.2; ///> FEM parameter, if constraint type is FEM
-
- std::vector<std::pair<PbdConstraint::Type, double>> m_RegularConstraints; ///> Constraints except FEM
- std::vector<std::pair<PbdConstraint::Type,
- PbdFEMConstraint::MaterialType>> m_FEMConstraints; ///> FEM constraints
+ std::vector<std::pair<PbdConstraint::Type, double>> m_regularConstraints; ///> Constraints except FEM
+ std::vector<std::pair<PbdConstraint::Type, PbdFEMConstraint::MaterialType>> m_FEMConstraints; ///> Constraints except FEM
///
/// \brief Enable a regular constraint (constraint that is not FEM constraint)
@@ -70,9 +75,26 @@ struct PBDModelConfig
void enableConstraint(PbdConstraint::Type type, double stiffness);
///
- /// \brief Enable a FEM constraint with a given FEM material
+ /// \brief Enable a FEM constraint with mu, lambda
///
void enableFEMConstraint(PbdConstraint::Type type, PbdFEMConstraint::MaterialType material);
+
+ ///
+ /// \brief Set the PBD solver type
+ ///
+ void setSolverType(const PbdConstraint::SolverType& type)
+ {
+ if (type == PbdConstraint::SolverType::GCD)
+ {
+ LOG(WARNING) << "GCD is NOT implemented yet, use xPBD instead";
+ m_solverType = PbdConstraint::SolverType::xPBD;
+ return;
+ }
+
+ m_solverType = type;
+ }
+
+ PbdConstraint::SolverType m_solverType = PbdConstraint::SolverType::xPBD;
};
///
@@ -86,16 +108,7 @@ public:
///
/// \brief Constructor
///
- PbdModel() : DynamicalModel(DynamicalModelType::PositionBasedDynamics)
- {
- m_validGeometryTypes = {
- Geometry::Type::PointSet,
- Geometry::Type::LineMesh,
- Geometry::Type::SurfaceMesh,
- Geometry::Type::TetrahedralMesh,
- Geometry::Type::HexahedralMesh
- };
- }
+ PbdModel();
///
/// \brief Destructor
@@ -105,12 +118,12 @@ public:
///
/// \brief Set simulation parameters
///
- void configure(const std::shared_ptr<PBDModelConfig>& params);
+ void configure(std::shared_ptr<PBDModelConfig> params);
///
/// \brief Get the simulation parameters
///
- const std::shared_ptr<PBDModelConfig>& getParameters() const { assert(m_Parameters); return m_Parameters; }
+ const std::shared_ptr<PBDModelConfig>& getParameters() const { assert(m_parameters); return m_parameters; }
///
/// \brief Compute elastic constants: Young Modulus, Poisson Ratio, first and second Lame
@@ -154,11 +167,6 @@ public:
///
bool initializeConstantDensityConstraint(const double stiffness);
- ///
- /// \brief compute delta x (position) and update position
- ///
- void projectConstraints();
-
///
/// \brief Update the model geometry from the newest PBD state
///
@@ -172,13 +180,13 @@ public:
///
/// \brief Returns true if there is at least one constraint
///
- bool hasConstraints() const { return !m_constraints.empty() || !m_partitionedConstraints.empty(); }
+ bool hasConstraints() const { return !m_constraints->empty() || !m_partitionedConstraints->empty(); }
///
/// \brief Set the time step size
///
- virtual void setTimeStep(const Real timeStep) override { m_Parameters->m_dt = timeStep; }
- void setDefaultTimeStep(const Real timeStep) { m_Parameters->m_DefaultDt = static_cast<Real>(timeStep); }
+ virtual void setTimeStep(const Real timeStep) override { m_parameters->m_dt = timeStep; }
+ void setDefaultTimeStep(const Real timeStep) { m_parameters->m_defaultDt = static_cast<Real>(timeStep); }
///
/// \brief Set the time step size to fixed size
@@ -188,8 +196,18 @@ public:
///
/// \brief Returns the time step size
///
- virtual double getTimeStep() const override { return m_Parameters->m_dt; }
- double getDefaultTimeStep() const { return m_Parameters->m_DefaultDt; }
+ virtual double getTimeStep() const override { return m_parameters->m_dt; }
+ double getDefaultTimeStep() const { return m_parameters->m_defaultDt; }
+
+ ///
+ /// \brief
+ ///
+ std::shared_ptr<PBDConstraintVector> getConstraints() { return m_constraints; }
+
+ ///
+ /// \brief
+ ///
+ std::shared_ptr<std::vector<PBDConstraintVector>> getPartitionedConstraints() { return m_partitionedConstraints; }
///
/// \brief Set uniform mass to all the nodes
@@ -209,7 +227,9 @@ public:
///
/// \brief Get the inverse of mass of a certain node
///
- double getInvMass(const size_t idx) const;
+ double getInvMass(const size_t idx) const { return (*m_invMass)[idx]; }
+
+ std::shared_ptr<StdVectorOfReal> getInvMasses() { return m_invMass; }
///
/// \brief Time integrate the position
@@ -237,22 +257,47 @@ public:
///
void setConstraintPartitionThreshold(size_t threshold) { m_partitionThreshold = threshold; }
+ std::shared_ptr<PbdSolver> getSolver() const { return m_pbdSolver; }
+
+ void setSolver(std::shared_ptr<PbdSolver> solver) { this->m_pbdSolver = solver; }
+
+ std::shared_ptr<TaskNode> getIntegratePositionNode() const { return m_integrationPositionNode; }
+
+ std::shared_ptr<TaskNode> getUpdateCollisionGeometryNode() const { return m_updateCollisionGeometryNode; }
+
+ std::shared_ptr<TaskNode> getSolveNode() const { return m_solveConstraintsNode; }
+
+ std::shared_ptr<TaskNode> getUpdateVelocityNode() const { return m_updateVelocityNode; }
+
protected:
///
/// \brief Partition constraints for parallelization
///
void partitionConstraints(const bool print = false);
- size_t m_partitionThreshold = 16; ///> Threshold for constraint partitioning
+ ///
+ /// \brief Setup the computational graph of PBD
+ ///
+ void initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink) override;
+
+protected:
+ bool m_partitioned = false; /// \todo this is used in initialize() as a temporary fix to problems on linux
+ size_t m_partitionThreshold = 16; ///> Threshold for constraint partitioning
- std::shared_ptr<PointSet> m_mesh; ///> PointSet on which the pbd model operates on
- std::vector<double> m_mass; ///> Mass of nodes
- std::vector<double> m_invMass; ///> Inverse of mass of nodes
+ std::shared_ptr<PbdSolver> m_pbdSolver = nullptr; ///> PBD solver
+ std::shared_ptr<PointSet> m_mesh = nullptr; ///> PointSet on which the pbd model operates on
+ std::shared_ptr<StdVectorOfReal> m_mass = nullptr; ///> Mass of nodes
+ std::shared_ptr<StdVectorOfReal> m_invMass = nullptr; ///> Inverse of mass of nodes
- using PBDConstraintVector = std::vector<std::shared_ptr<PbdConstraint>>;
+ std::shared_ptr<PBDConstraintVector> m_constraints = nullptr; ///> List of pbd constraints
+ std::shared_ptr<std::vector<PBDConstraintVector>> m_partitionedConstraints = nullptr; ///> List of pbd constraints
+ std::shared_ptr<PBDModelConfig> m_parameters = nullptr; ///> Model parameters, must be set before simulation
- PBDConstraintVector m_constraints; ///> List of pbd constraints
- std::vector<PBDConstraintVector> m_partitionedConstraints; ///> List of pbd constraints
- std::shared_ptr<PBDModelConfig> m_Parameters; ///> Model parameters, must be set before simulation
+protected:
+ // Computational Nodes
+ std::shared_ptr<TaskNode> m_integrationPositionNode = nullptr;
+ std::shared_ptr<TaskNode> m_updateCollisionGeometryNode = nullptr;
+ std::shared_ptr<TaskNode> m_solveConstraintsNode = nullptr;
+ std::shared_ptr<TaskNode> m_updateVelocityNode = nullptr;
};
} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.cpp b/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..26fa5f05d9752db528933007b12e8708ca33ffda
--- /dev/null
+++ b/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.cpp
@@ -0,0 +1,693 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+// std lib
+#include <fstream>
+#include <ios>
+#include <iomanip>
+
+// imstk
+#include "imstkMath.h"
+#include "imstkNewtonSolver.h"
+#include "imstkReducedStVKBodyModel.h"
+#include "imstkTimeIntegrator.h"
+#include "imstkVegaMeshIO.h"
+#include "imstkVolumetricMesh.h"
+#include "imstkTaskGraph.h"
+#include "imstkNewtonSolver.h"
+
+// vega
+#include "StVKReducedInternalForces.h"
+#include "matrixIO.h"
+#include "modalMatrix.h"
+#include "reducedStVKForceModel.h"
+
+#pragma warning(push)
+#pragma warning(disable : 4458)
+#include "configFile.h"
+#pragma warning(pop)
+
+namespace imstk
+{
+ReducedStVK::ReducedStVK() : DynamicalModel(DynamicalModelType::ElastoDynamics)
+{
+ // m_fixedNodeIds.reserve(1000);
+
+ m_validGeometryTypes = { Geometry::Type::TetrahedralMesh, Geometry::Type::HexahedralMesh };
+
+ m_solveNode = m_taskGraph->addFunction("FEMModel_Solve", [&]() { getSolver()->solve(); });
+}
+
+ReducedStVK::~ReducedStVK()
+{
+ // Get vega to destruct first (before the shared pointer to the vega mesh is cleaned up)
+ m_internalForceModel = nullptr;
+}
+
+void
+ReducedStVK::configure(const std::string& configFileName)
+{
+ std::cerr << "not implemented yet" << std::endl;
+ // Configure the ReducedStVKConfig
+ m_config = std::make_shared<ReducedStVKConfig>();
+
+ // char femMethod[256];
+ // char invertibleMaterial[256];
+ // char fixedDOFFilename[256];
+
+ vega::ConfigFile vegaConfigFileOptions;
+
+ // configure the options
+ vegaConfigFileOptions.addOptionOptional("dampingMassCoefficient",
+ &m_config->m_dampingMassCoefficient,
+ m_config->m_dampingMassCoefficient);
+ vegaConfigFileOptions.addOptionOptional("dampingStiffnessCoefficient",
+ &m_config->m_dampingStiffnessCoefficient,
+ m_config->m_dampingStiffnessCoefficient);
+ vegaConfigFileOptions.addOptionOptional("dampingLaplacianCoefficient",
+ &m_config->m_dampingLaplacianCoefficient,
+ m_config->m_dampingLaplacianCoefficient);
+ vegaConfigFileOptions.addOptionOptional("numberOfThreads",
+ &m_config->m_numberOfThreads,
+ m_config->m_numberOfThreads);
+ vegaConfigFileOptions.addOptionOptional("gravity", &m_config->m_gravity, m_config->m_gravity);
+
+ // Parse the configuration file
+ CHECK(vegaConfigFileOptions.parseOptions(configFileName.data()) == 0)
+ << "ForceModelConfig::parseConfig - Unable to load the configuration file";
+
+ // get the root directory of the boundary file name
+ // std::string rootDir;
+ // const size_t last_slash_idx = configFileName.rfind('/');
+ // if (std::string::npos != last_slash_idx)
+ // {
+ // rootDir = configFileName.substr(0, last_slash_idx);
+ // }
+
+ // file names (remove from here?)
+ // m_config->m_fixedDOFFilename = rootDir + std::string("/") + std::string(fixedDOFFilename);
+}
+
+void
+ReducedStVK::configure(std::shared_ptr<ReducedStVKConfig> config)
+{
+ m_config = config;
+}
+
+void
+ReducedStVK::setForceModelConfiguration(std::shared_ptr<ReducedStVKConfig> fmConfig)
+{
+ m_config = fmConfig;
+}
+
+std::shared_ptr<imstk::ReducedStVKConfig>
+ReducedStVK::getForceModelConfiguration() const
+{
+ return m_config;
+}
+
+void
+// ReducedStVK::setInternalForceModel(std::shared_ptr<InternalForceModel> fm)
+ReducedStVK::setInternalForceModel(std::shared_ptr<vega::StVKReducedInternalForces> fm)
+{
+ m_internalForceModel = fm;
+}
+
+std::shared_ptr<imstk::InternalForceModel>
+ReducedStVK::getInternalForceModel() const
+{
+ // return m_internalForceModel;
+ LOG(FATAL) << "current implementation can not return an imstk::InternalForceModel.";
+ // todo: to implement this function, we have to
+ // 1) template InternalForceModel with SystemMatrix
+ // 2) define ReducedStVKForceModel by inherit from InternalForceModel
+ // 3) replace vega::StVKReducedInternalForces with ReducedStVKForceModel
+ return nullptr;
+}
+
+void
+ReducedStVK::setTimeIntegrator(std::shared_ptr<TimeIntegrator> timeIntegrator)
+{
+ m_timeIntegrator = timeIntegrator;
+}
+
+std::shared_ptr<imstk::TimeIntegrator>
+ReducedStVK::getTimeIntegrator() const
+{
+ return m_timeIntegrator;
+}
+
+bool
+ReducedStVK::initialize()
+{
+ // prerequisite of for successfully initializing
+ CHECK(m_geometry != nullptr && m_config != nullptr)
+ << "DeformableBodyModel::initialize: Physics mesh or force model configuration not set "
+ "yet!";
+
+ // Setup default solver if model wasn't assigned one
+ if (m_solver == nullptr)
+ {
+ // create a nonlinear system
+ auto nlSystem = std::make_shared<NonLinearSystem<Matrixd>>(getFunction(), getFunctionGradient());
+
+ nlSystem->setUnknownVector(getUnknownVec());
+ nlSystem->setUpdateFunction(getUpdateFunction());
+ nlSystem->setUpdatePreviousStatesFunction(getUpdatePrevStateFunction());
+
+ // create a non-linear solver and add to the scene
+ auto nlSolver = std::make_shared<NewtonSolver<Matrixd>>();
+ // nlSolver->setLinearSolver(linSolver);
+ nlSolver->setMaxIterations(1);
+ nlSolver->setSystem(nlSystem);
+ setSolver(nlSolver);
+ }
+
+ // This will specify \p m_numDOF and \p m_numDOFReduced
+ this->readModalMatrix(m_config->m_modesFileName);
+ this->loadInitialStates();
+
+ auto physicsMesh = std::dynamic_pointer_cast<imstk::VolumetricMesh>(this->getModelGeometry());
+ m_vegaPhysicsMesh = VegaMeshIO::convertVolumetricMeshToVegaMesh(physicsMesh);
+ CHECK(m_numDOF == m_vegaPhysicsMesh->getNumVertices() * 3);
+
+ if (!this->initializeForceModel() || !this->initializeMassMatrix()
+ || !this->initializeTangentStiffness() || !this->initializeDampingMatrix()
+ || !this->initializeGravityForce() || !this->initializeExplicitExternalForces())
+ {
+ return false;
+ }
+
+ // reduced-space
+ m_Feff.resize(m_numDOFReduced);
+ m_Finternal.resize(m_numDOFReduced);
+ m_Finternal.setConstant(0.0);
+ m_qSolReduced.resize(m_numDOFReduced);
+ m_qSolReduced.setConstant(0.0);
+ m_FcontactReduced.resize(m_numDOFReduced);
+ m_FgravityReduced.resize(m_numDOFReduced);
+ m_FexplicitExternalReduced.resize(m_numDOFReduced);
+
+ // full-space variable
+ m_Fcontact.resize(m_numDOF);
+ m_Fcontact.setConstant(0.0);
+ m_qSol.resize(m_numDOF);
+ m_qSol.setConstant(0.0);
+
+ return true;
+}
+
+void
+ReducedStVK::readModalMatrix(const std::string& fname)
+{
+ std::vector<float> Ufloat;
+ int m, n;
+ vega::ReadMatrixFromDisk_(fname.c_str(), m, n, Ufloat);
+ m_numDOF = m;
+ m_numDOFReduced = n;
+ std::vector<double> Udouble(Ufloat.size());
+
+ for (size_t i = 0; i < Ufloat.size(); ++i)
+ {
+ Udouble[i] = Ufloat[i];
+ }
+
+ m_modalMatrix =
+ std::make_shared<vega::ModalMatrix>(m_numDOF / 3, m_numDOFReduced, Udouble.data());
+ return;
+}
+
+void
+ReducedStVK::loadInitialStates()
+{
+ if (m_numDOF == 0 || m_numDOFReduced == 0)
+ {
+ LOG(WARNING) << "ReducedStVK::loadInitialStates() - Num. of degree of freedom is zero!";
+ }
+
+ // For now the initial states are set to zero
+ m_initialState = std::make_shared<kinematicState>(m_numDOF);
+ m_previousState = std::make_shared<kinematicState>(m_numDOF);
+ m_currentState = std::make_shared<kinematicState>(m_numDOF);
+
+ m_initialStateReduced = std::make_shared<kinematicState>(m_numDOFReduced);
+ m_previousStateReduced = std::make_shared<kinematicState>(m_numDOFReduced);
+ m_currentStateReduced = std::make_shared<kinematicState>(m_numDOFReduced);
+}
+
+bool
+ReducedStVK::initializeForceModel()
+{
+ const double g = m_config->m_gravity;
+ const bool isGravityPresent = (g > 0) ? true : false;
+
+ // m_numDOFReduced = m_config->r;
+ m_internalForceModel = std::make_shared<vega::StVKReducedInternalForces>(
+ m_config->m_cubicPolynomialFilename.c_str(),
+ m_numDOFReduced);
+ m_forceModel = std::make_shared<vega::ReducedStVKForceModel>(m_internalForceModel.get());
+
+ return true;
+}
+
+bool
+ReducedStVK::initializeMassMatrix()
+{
+ CHECK(m_geometry != nullptr)
+ << "DeformableBodyModel::initializeMassMatrix Force model geometry not set!";
+ this->m_massMatrix.resize(m_numDOFReduced * m_numDOFReduced, 0.0);
+
+ // set M to identity
+ for (size_t i = 0; i < m_numDOFReduced; ++i)
+ {
+ this->m_massMatrix[i * m_numDOFReduced + i] = 1.0;
+ }
+ this->m_M.resize(m_numDOFReduced, m_numDOFReduced);
+ this->initializeEigenMatrixFromStdVector(m_massMatrix, m_M);
+
+ return true;
+}
+
+bool
+ReducedStVK::initializeDampingMatrix()
+{
+ // \todo: only mass dampings are considered here.
+ this->m_dampingMatrix.resize(m_numDOFReduced * m_numDOFReduced, 0.0);
+ auto cM = m_config->m_dampingMassCoefficient;
+ auto cK = m_config->m_dampingStiffnessCoefficient;
+ for (size_t i = 0; i < m_dampingMatrix.size(); ++i)
+ {
+ m_dampingMatrix[i] = m_massMatrix[i] * cM + m_stiffnessMatrix[i] * cK;
+ }
+ this->m_C.resize(m_numDOFReduced, m_numDOFReduced);
+ this->initializeEigenMatrixFromStdVector(m_massMatrix, m_C);
+ m_damped = true;
+
+ return true;
+}
+
+bool
+ReducedStVK::initializeTangentStiffness()
+{
+ CHECK(m_forceModel != nullptr)
+ << "DeformableBodyModel::initializeTangentStiffness: Tangent stiffness cannot be "
+ "initialized without force model";
+
+ this->m_stiffnessMatrix.resize(m_numDOFReduced * m_numDOFReduced, 0.0);
+ m_forceModel->GetTangentStiffnessMatrix(m_initialStateReduced->getQ().data(),
+ m_stiffnessMatrix.data());
+ this->m_K.resize(m_numDOFReduced, m_numDOFReduced);
+ this->initializeEigenMatrixFromStdVector(m_stiffnessMatrix, m_K);
+ return true;
+}
+
+bool
+ReducedStVK::initializeGravityForce()
+{
+ m_Fgravity.resize(m_numDOF);
+ const double gravity = m_config->m_gravity;
+
+ m_vegaPhysicsMesh->computeGravity(m_Fgravity.data(), gravity);
+ m_FgravityReduced.resize(m_numDOFReduced);
+ this->project(m_Fgravity, m_FgravityReduced);
+
+ return true;
+}
+
+void
+ReducedStVK::computeImplicitSystemRHS(kinematicState& stateAtT,
+ kinematicState& newState,
+ const StateUpdateType updateType)
+{
+ const auto& uPrev = stateAtT.getQ();
+ const auto& vPrev = stateAtT.getQDot();
+ auto& u = newState.getQ();
+ const auto& v = newState.getQDot();
+
+ // Do checks if there are uninitialized matrices
+ m_forceModel->GetTangentStiffnessMatrix(u.data(), m_stiffnessMatrix.data());
+ this->initializeEigenMatrixFromStdVector(m_stiffnessMatrix, m_K);
+
+ const double dT = m_timeIntegrator->getTimestepSize();
+
+ switch (updateType)
+ {
+ case StateUpdateType::DeltaVelocity:
+
+ m_Feff = m_K * -(uPrev - u + v * dT);
+
+ if (m_damped)
+ {
+ m_Feff -= m_C * v;
+ }
+
+ m_forceModel->GetInternalForce(u.data(), m_Finternal.data());
+ m_Feff -= m_Finternal;
+ this->project(m_FexplicitExternal, m_FexplicitExternalReduced);
+ m_Feff += m_FexplicitExternalReduced;
+ // the reduced gravity has alredy been initialized in initializeGravityForce
+ // this->project(m_Fgravity, m_FgravityReduced);
+ m_Feff += m_FgravityReduced;
+ this->project(m_Fcontact, m_FcontactReduced);
+ m_Feff += m_FcontactReduced;
+ m_Feff *= dT;
+ m_Feff += m_M * (vPrev - v);
+
+ break;
+ default:
+ LOG(WARNING) << "ReducedStVK::computeImplicitSystemRHS: Update type not supported";
+ }
+}
+
+void
+ReducedStVK::computeSemiImplicitSystemRHS(kinematicState& stateAtT,
+ kinematicState& newState,
+ const StateUpdateType updateType)
+{
+ // auto& uPrev = stateAtT.getQ();
+ const auto& vPrev = stateAtT.getQDot();
+ auto& u = newState.getQ();
+ // auto& v = newState.getQDot();
+
+ // Do checks if there are uninitialized matrices
+ m_forceModel->GetTangentStiffnessMatrix(u.data(), m_stiffnessMatrix.data());
+ this->initializeEigenMatrixFromStdVector(m_stiffnessMatrix, m_K);
+
+ const double dT = m_timeIntegrator->getTimestepSize();
+
+ switch (updateType)
+ {
+ case StateUpdateType::DeltaVelocity:
+
+ m_Feff = m_K * (vPrev * -dT);
+
+ if (m_damped)
+ {
+ m_Feff -= m_C * vPrev;
+ }
+
+ m_forceModel->GetInternalForce(u.data(), m_Finternal.data());
+ m_Feff -= m_Finternal;
+ this->project(m_FexplicitExternal, m_FexplicitExternalReduced);
+ m_Feff += m_FexplicitExternalReduced;
+ this->project(m_Fgravity, m_FgravityReduced);
+ m_Feff += m_FgravityReduced;
+ this->project(m_Fcontact, m_FcontactReduced);
+ m_Feff += m_FcontactReduced;
+ m_Feff *= dT;
+
+ break;
+
+ default:
+ LOG(FATAL) << "ReducedStVK::computeSemiImplicitSystemRHS: Update type not supported";
+ }
+}
+
+void
+ReducedStVK::computeImplicitSystemLHS(const kinematicState& stateAtT,
+ kinematicState& newState,
+ const StateUpdateType updateType)
+{
+ const double dT = m_timeIntegrator->getTimestepSize();
+
+ switch (updateType)
+ {
+ case StateUpdateType::DeltaVelocity:
+
+ stateAtT; // supress warning (state is not used in this update type hence can be
+ // ignored)
+
+ this->updateMassMatrix();
+ m_forceModel->GetTangentStiffnessMatrix(newState.getQ().data(),
+ m_stiffnessMatrix.data());
+ this->initializeEigenMatrixFromStdVector(m_stiffnessMatrix, m_K);
+ this->updateDampingMatrix();
+
+ m_Keff = m_M;
+ if (m_damped)
+ {
+ m_Keff += dT * m_C;
+ }
+ m_Keff += (dT * dT) * m_K;
+
+ break;
+
+ default:
+ LOG(FATAL) << "ReducedStVK::computeImplicitSystemLHS: Update type not supported";
+ }
+}
+
+bool
+ReducedStVK::initializeExplicitExternalForces()
+{
+ m_FexplicitExternal.resize(m_numDOF);
+ m_FexplicitExternal.setZero();
+ m_FexplicitExternalReduced.resize(m_numDOFReduced);
+ m_FexplicitExternalReduced.setZero();
+
+ return true;
+}
+
+void
+ReducedStVK::updateDampingMatrix()
+{
+ if (!m_damped)
+ {
+ return;
+ }
+
+ const auto& dampingStiffnessCoefficient = m_config->m_dampingStiffnessCoefficient;
+ const auto& dampingMassCoefficient = m_config->m_dampingMassCoefficient;
+
+ if (dampingMassCoefficient > 0)
+ {
+ m_C = dampingMassCoefficient * m_M;
+
+ if (dampingStiffnessCoefficient > 0)
+ {
+ m_C += m_K * dampingStiffnessCoefficient;
+ }
+ }
+ else if (dampingStiffnessCoefficient > 0)
+ {
+ m_C = m_K * dampingStiffnessCoefficient;
+ }
+}
+
+void
+ReducedStVK::applyBoundaryConditions(Matrixd& M, const bool withCompliance) const
+{
+ // I believe there is nothing to do here since the full space has already been constrained
+ // before the reduction.
+ return;
+}
+
+void
+ReducedStVK::applyBoundaryConditions(Vectord& x) const
+{
+ LOG(WARNING) << "Boundary conditions are not not allowed to change";
+}
+
+void
+ReducedStVK::updateMassMatrix()
+{
+ // Do nothing for now as topology changes are not supported yet!
+}
+
+void
+ReducedStVK::updatePhysicsGeometry()
+{
+ auto volMesh = std::static_pointer_cast<VolumetricMesh>(m_geometry);
+ auto& uReduced = m_currentStateReduced->getQ();
+ auto& uFull = m_currentState->getQ();
+ this->prolongate(uReduced, uFull);
+ volMesh->setVertexDisplacements(uFull);
+}
+
+void
+ReducedStVK::updateBodyPreviousStates()
+{
+ // full-space
+ m_previousStateReduced->setU(m_currentStateReduced->getQ());
+ m_previousStateReduced->setV(m_currentStateReduced->getQDot());
+ prolongate(*m_previousStateReduced, *m_previousState);
+}
+
+void
+ReducedStVK::updateBodyStates(const Vectord& solution, const StateUpdateType updateType)
+{
+ this->updateBodyPreviousStates();
+ this->updateBodyIntermediateStates(solution, updateType);
+}
+
+void
+ReducedStVK::updateBodyIntermediateStates(const Vectord& solution, const StateUpdateType updateType)
+{
+ const auto& uPrev = m_previousStateReduced->getQ();
+ // auto& u = m_currentState->getQ();
+ const auto& v = m_currentStateReduced->getQDot();
+ const double dT = m_timeIntegrator->getTimestepSize();
+
+ switch (updateType)
+ {
+ case StateUpdateType::DeltaVelocity:
+ m_currentStateReduced->setV(v + solution);
+ m_currentStateReduced->setU(uPrev + dT * v);
+
+ break;
+
+ case StateUpdateType::Velocity:
+ m_currentStateReduced->setV(solution);
+ m_currentStateReduced->setU(uPrev + dT * v);
+
+ break;
+
+ default:
+ LOG(FATAL) << "DeformableBodyModel::updateBodyIntermediateStates: Unknown state update "
+ "type";
+ }
+ prolongate(*m_currentStateReduced, *m_currentState);
+ m_qSolReduced = m_currentStateReduced->getQ();
+}
+
+NonLinearSystem<Matrixd>::VectorFunctionType
+ReducedStVK::getFunction()
+{
+#pragma warning(push)
+#pragma warning(disable : 4100)
+
+ // Function to evaluate the nonlinear objective function given the current state
+ return [&, this](const Vectord& q, const bool semiImplicit) -> const Vectord& {
+ (semiImplicit) ? this->computeSemiImplicitSystemRHS(*m_previousStateReduced,
+ *m_currentStateReduced,
+ m_updateType)
+ : this->computeImplicitSystemRHS(*m_previousStateReduced,
+ *m_currentStateReduced,
+ m_updateType);
+ return m_Feff;
+ };
+
+#pragma warning(pop)
+}
+
+NonLinearSystem<Matrixd>::MatrixFunctionType
+ReducedStVK::getFunctionGradient()
+{
+#pragma warning(push)
+#pragma warning(disable : 4100)
+ // Gradient of the nonlinear objective function given the current state
+ return [&, this](const Vectord& q) -> const Matrixd& {
+ this->computeImplicitSystemLHS(*m_previousStateReduced,
+ *m_currentStateReduced,
+ m_updateType);
+ return m_Keff;
+ };
+
+#pragma warning(pop)
+}
+
+NonLinearSystem<Matrixd>::UpdateFunctionType
+ReducedStVK::getUpdateFunction()
+{
+ // Function to evaluate the nonlinear objective function given the current state
+ return [&, this](const Vectord& q, const bool fullyImplicit) -> void {
+ (fullyImplicit) ? this->updateBodyIntermediateStates(q, m_updateType)
+ : this->updateBodyStates(q, m_updateType);
+ };
+}
+
+NonLinearSystem<Matrixd>::UpdatePrevStateFunctionType
+ReducedStVK::getUpdatePrevStateFunction()
+{
+ // Function to evaluate the nonlinear objective function given the current state
+ return [&, this]() -> void { this->updateBodyPreviousStates(); };
+}
+
+void
+ReducedStVK::initializeEigenMatrixFromStdVector(const std::vector<double>& A, Matrixd& eigenMatrix)
+{
+ CHECK(eigenMatrix.rows() == eigenMatrix.cols());
+ CHECK(A.size() == eigenMatrix.rows() * eigenMatrix.cols());
+
+ // A is column-major
+ for (size_t j = 0; j < eigenMatrix.cols(); ++j)
+ {
+ size_t offset = j * eigenMatrix.rows();
+ for (size_t i = 0; i < eigenMatrix.rows(); ++i)
+ {
+ eigenMatrix(i, j) = A[i + offset];
+ }
+ }
+}
+
+Vectord&
+ReducedStVK::getContactForce()
+{
+ return m_Fcontact;
+}
+
+void
+ReducedStVK::setFixedSizeTimeStepping()
+{
+ m_timeStepSizeType = TimeSteppingType::Fixed;
+ m_timeIntegrator->setTimestepSizeToDefault();
+}
+
+void
+ReducedStVK::setTimeStep(const double timeStep)
+{
+ m_timeIntegrator->setTimestepSize(timeStep);
+}
+
+double
+ReducedStVK::getTimeStep() const
+{
+ return m_timeIntegrator->getTimestepSize();
+};
+
+void
+ReducedStVK::prolongate(const Vectord& uReduced, Vectord& u) const
+{
+ //\todo: check sizes
+ m_modalMatrix->AssembleVector(const_cast<double*>(uReduced.data()), u.data());
+}
+
+void
+ReducedStVK::prolongate(kinematicState& uReduced, kinematicState& u) const
+{
+ this->prolongate(uReduced.getQ(), u.getQ());
+ this->prolongate(uReduced.getQDot(), u.getQDot());
+}
+
+void
+ReducedStVK::project(const Vectord& u, Vectord& uReduced) const
+{
+ m_modalMatrix->ProjectVector(const_cast<double*>(u.data()), uReduced.data());
+}
+
+void
+ReducedStVK::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink)
+{
+ // Setup graph connectivity
+ m_taskGraph->addEdge(source, m_solveNode);
+ m_taskGraph->addEdge(m_solveNode, sink);
+}
+} // namespace imstk
diff --git a/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.h b/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.h
new file mode 100644
index 0000000000000000000000000000000000000000..b8f55955b2676b86608c8d7bf861c99eedc82a9a
--- /dev/null
+++ b/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.h
@@ -0,0 +1,369 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#pragma once
+
+#include <memory>
+
+#include "imstkDynamicalModel.h"
+#include "imstkNonLinearSystem.h"
+#include "imstkLogger.h"
+
+namespace vega
+{
+class VolumetricMesh;
+class ReducedStVKForceModel;
+class StVKReducedInternalForces;
+class ModalMatrix;
+}
+
+namespace imstk
+{
+class InternalForceModel;
+class SolverBase;
+class TaskNode;
+class TimeIntegrator;
+class VegaMeshIO;
+
+struct ReducedStVKConfig
+{
+ std::string m_cubicPolynomialFilename;
+ std::string m_modesFileName;
+ // int r;
+
+ double m_dampingMassCoefficient = 0.1;
+ double m_dampingStiffnessCoefficient = 0.01;
+ double m_dampingLaplacianCoefficient = 0.0;
+ double m_deformationCompliance = 1.0;
+ double m_gravity = 9.81;
+
+ int m_numberOfThreads = 4;
+};
+
+class ReducedStVK : public DynamicalModel<FeDeformBodyState>
+{
+using kinematicState = FeDeformBodyState;
+// using Matrixd = Eigen::MatrixXd;
+public:
+ ///
+ /// \brief Constructor
+ ///
+ ReducedStVK();
+ ///
+ /// \brief Destructor
+ ///
+ ~ReducedStVK();
+
+ ///
+ /// \brief Configure the force model from external file
+ ///
+ void configure(const std::string& configFileName);
+ void configure(std::shared_ptr<ReducedStVKConfig> config = std::make_shared<ReducedStVKConfig>());
+
+ ///
+ /// \brief Initialize the deformable body model
+ ///
+ bool initialize() override;
+
+ ///
+ /// \brief Set/Get internal force model
+ ///
+ void setForceModelConfiguration(std::shared_ptr<ReducedStVKConfig> config);
+ std::shared_ptr<ReducedStVKConfig> getForceModelConfiguration() const;
+
+ ///
+ /// \brief Set/Get time integrator
+ ///
+ /// \note the return type is different from FEMDeformableBodyModel::setInternalForceModel
+ void setInternalForceModel(std::shared_ptr<vega::StVKReducedInternalForces> fm);
+ std::shared_ptr<imstk::InternalForceModel> getInternalForceModel() const;
+
+ ///
+ /// \brief Set/Get time integrator
+ ///
+ void setTimeIntegrator(std::shared_ptr<TimeIntegrator> timeIntegrator);
+ std::shared_ptr<TimeIntegrator> getTimeIntegrator() const;
+
+ ///
+ /// \brief Load the initial conditions of the deformable object
+ ///
+ void loadInitialStates();
+
+ ///
+ /// \brief Initialize the force model
+ ///
+ bool initializeForceModel();
+
+ ///
+ /// \brief Initialize the mass matrix from the mesh
+ ///
+ bool initializeMassMatrix();
+
+ ///
+ /// \brief Initialize the damping (combines structural and viscous damping) matrix
+ ///
+ bool initializeDampingMatrix();
+
+ ///
+ /// \brief Initialize the tangent stiffness matrix
+ ///
+ bool initializeTangentStiffness();
+
+ ///
+ /// \brief Initialize the gravity force
+ ///
+ bool initializeGravityForce();
+
+ ///
+ /// \brief Initialize explicit external forces
+ ///
+ bool initializeExplicitExternalForces();
+
+ ///
+ /// \brief Initialize the Eigen matrix with data inside vega sparse matrix
+ ///
+ static void initializeEigenMatrixFromStdVector(const std::vector<double>& v,
+ Matrixd& eigenMatrix);
+
+ ///
+ /// \brief Compute the RHS of the resulting linear system
+ ///
+ void computeImplicitSystemRHS(kinematicState& prevState,
+ kinematicState& newState,
+ const StateUpdateType updateType);
+
+ ///
+ /// \brief Compute the RHS of the resulting linear system using semi-implicit scheme
+ ///
+ void computeSemiImplicitSystemRHS(kinematicState& stateAtT,
+ kinematicState& newState,
+ const StateUpdateType updateType);
+
+ ///
+ /// \brief Compute the LHS of the resulting linear system
+ ///
+ void computeImplicitSystemLHS(const kinematicState& prevState,
+ kinematicState& newState,
+ const StateUpdateType updateType);
+
+ ///
+ /// \brief Update damping Matrix
+ ///
+ void updateDampingMatrix();
+
+ ///
+ /// \brief Update mass matrix
+ /// Note: Not supported yet!
+ ///
+ void updateMassMatrix();
+
+ ///
+ /// \brief Applies boundary conditions to matrix and a vector
+ ///
+ void applyBoundaryConditions(Matrixd& M, const bool withCompliance = false) const;
+ void applyBoundaryConditions(Vectord& x) const;
+
+ ///
+ /// \brief Updates the Physics geometry
+ ///
+ void updatePhysicsGeometry() override;
+
+ ///
+ /// \brief Update states
+ ///
+ void updateBodyStates(const Vectord& solution, const StateUpdateType updateType) override;
+ void updateBodyIntermediateStates(const Vectord& solution, const StateUpdateType updateType);
+
+ ///
+ /// \brief Update the previous states given the current state
+ ///
+ void updateBodyPreviousStates();
+
+ ///
+ /// \brief Returns the "function" that evaluates the nonlinear function given
+ /// the state vector
+ ///
+ NonLinearSystem<Matrixd>::VectorFunctionType getFunction();
+
+ ///
+ /// \brief Get the function that updates the model given the solution
+ ///
+ NonLinearSystem<Matrixd>::UpdateFunctionType getUpdateFunction();
+ NonLinearSystem<Matrixd>::UpdatePrevStateFunctionType getUpdatePrevStateFunction();
+
+ ///
+ /// \brief Returns the "function" that evaluates the gradient of the nonlinear
+ /// function given the state vector
+ ///
+ NonLinearSystem<Matrixd>::MatrixFunctionType getFunctionGradient();
+
+ ///
+ /// \brief Get the contact force vector
+ ///
+ Vectord& getContactForce();
+
+ ///
+ /// \brief Returns the unknown vectors
+ ///
+ Vectord& getUnknownVec()
+ {
+ return m_qSol;
+ }
+
+ ///
+ /// \brief Set/Get the update type
+ ///
+ void setUpdateType(const StateUpdateType& updateType)
+ {
+ m_updateType = updateType;
+ }
+
+ const StateUpdateType& getUpdateType() const
+ {
+ return m_updateType;
+ }
+
+ /// \brief Returns the unknown vectors
+ ///
+ std::vector<std::size_t>& getFixNodeIds()
+ {
+ return m_fixedNodeIds;
+ }
+
+ ///
+ /// \brief Set the time step size
+ ///
+ virtual void setTimeStep(const double timeStep);
+
+ ///
+ /// \brief Returns the time step size
+ ///
+ virtual double getTimeStep() const;
+
+ ///
+ /// \brief Set the time step size to fixed size
+ ///
+ void setFixedSizeTimeStepping();
+
+ ///
+ /// \brief Set the fixed BC implementation state
+ ///
+ void enableFixedBC()
+ {
+ LOG(WARNING) << "Boundary conditions are not allowed to change";
+ m_implementFixedBC = true;
+ };
+ void disableFixedBC()
+ {
+ // LOG(FATAL, "Boundary conditions are not allowed to change");
+ m_implementFixedBC = false;
+ };
+ bool isFixedBCImplemented() const
+ {
+ return m_implementFixedBC;
+ };
+
+ ///
+ /// \brief prolongate reduced vector into full space, ie, u = U * uReduced
+ ///
+ void prolongate(const Vectord& uReduced, Vectord& u) const;
+
+ ///
+ /// \brief prolongate reduced state into full space
+ ///
+ void prolongate(kinematicState& uReduced, kinematicState& u) const;
+
+ ///
+ /// \brief project full-space vector into reduced space, uReduced = U^T u
+ ///
+ void project(const Vectord& u, Vectord& uReduced) const;
+ ///
+ /// \brief Read in the basis file and create m_modalMatrix
+ /// \todo: make it private/protected?
+ ///
+ void readModalMatrix(const std::string& fname);
+
+ std::shared_ptr<TaskNode> getSolveNode() const { return m_solveNode; }
+
+ std::shared_ptr<SolverBase> getSolver() const { return m_solver; }
+ void setSolver(std::shared_ptr<SolverBase> solver) { this->m_solver = solver; }
+
+protected:
+ ///
+ /// \brief Setup the computational graph of FEM
+ ///
+ void initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink) override;
+
+protected:
+ std::shared_ptr<SolverBase> m_solver = nullptr;
+ std::shared_ptr<vega::StVKReducedInternalForces> m_internalForceModel; ///> Mathematical model for intenal forces
+ std::shared_ptr<vega::ReducedStVKForceModel> m_forceModel;
+ std::shared_ptr<TimeIntegrator> m_timeIntegrator; ///> Time integrator
+ std::shared_ptr<NonLinearSystem<Matrixd>> m_nonLinearSystem; ///> Nonlinear system resulting from TI and force model
+ std::shared_ptr<vega::ModalMatrix> m_modalMatrix;
+
+ std::shared_ptr<ReducedStVKConfig> m_config;
+
+ /// Matrices typical to a elastodynamics and 2nd order analogous systems
+ Matrixd m_M; ///> Mass matrix
+ Matrixd m_C; ///> Damping coefficient matrix
+ Matrixd m_K; ///> Tangent (derivative of internal force w.r.t displacements) stiffness matrix
+ Matrixd m_Keff; ///> Effective stiffness matrix (dependent on internal force model and time
+ /// integrator)
+
+ // full-space vectors
+ Vectord m_Fcontact; ///> Vector of contact forces
+ Vectord m_Fgravity; ///> Vector of gravity forces
+ Vectord m_FexplicitExternal; ///> Vector of explicitly defined external forces
+ Vectord m_qSol; ///> Vector to maintain solution at each iteration of nonlinear solver
+
+ // reduced-space vectors
+ Vectord m_Feff; ///> Vector of effective forces
+ Vectord m_Finternal; ///> Vector of internal forces
+ Vectord m_qSolReduced; ///> Vector to maintain solution at each iteration of nonlinear solver
+ Vectord m_FcontactReduced;
+ Vectord m_FgravityReduced; ///> Vector of gravity forces
+ Vectord m_FexplicitExternalReduced; ///> Vector of explicitly defined external forces
+ size_t m_numDOFReduced;
+
+ std::shared_ptr<vega::VolumetricMesh> m_vegaPhysicsMesh; ///> Mesh used for Physics
+ std::vector<double> m_massMatrix; ///> Vega mass matrix
+ std::vector<double> m_stiffnessMatrix; ///> Vega Tangent stiffness matrix
+ std::vector<double> m_dampingMatrix; ///> Vega Laplacian damping matrix
+
+ std::vector<std::size_t> m_fixedNodeIds; ///> Nodal IDs of the nodes that are fixed
+
+ StateUpdateType m_updateType = StateUpdateType::DeltaVelocity; ///> Update type of the model
+
+ bool m_damped = false; ///> Viscous or structurally damped system
+
+ // If this is true, the tangent stiffness and force vector will be modified to
+ // accommodate (the rows and columns will be nullified) the fixed boundary conditions
+ bool m_implementFixedBC = false;
+
+ std::shared_ptr<kinematicState> m_initialStateReduced;
+ std::shared_ptr<kinematicState> m_previousStateReduced;
+ std::shared_ptr<kinematicState> m_currentStateReduced;
+
+private:
+ std::shared_ptr<TaskNode> m_solveNode = nullptr;
+};
+} // namespace imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel.h b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel.h
index b9d377d7bd34a1ece03e01dfd423c71ada8bbdf4..7c9e020342cfe46e1bfbcbff817a80e89916bcd7 100644
--- a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel.h
@@ -19,8 +19,7 @@
=========================================================================*/
-#ifndef imstkRigidBodyModel_h
-#define imstkRigidBodyModel_h
+#pragma once
#include "imstkDynamicalModel.h"
#include "imstkRigidBodyState.h"
@@ -175,6 +174,4 @@ private:
void createCube();
void createMesh();
};
-} // imstk
-
-#endif
\ No newline at end of file
+} // imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/imstkSPHModel.cpp b/Source/DynamicalModels/ObjectModels/imstkSPHModel.cpp
index ca91fa161179157371ec49e46d519426f85c2c32..1ff32db59d7db2d415b1ab2395e33ae7095ba13c 100644
--- a/Source/DynamicalModels/ObjectModels/imstkSPHModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkSPHModel.cpp
@@ -1,27 +1,29 @@
/*=========================================================================
- Library: iMSTK
+Library: iMSTK
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & Imaging in Medicine, Rensselaer Polytechnic Institute.
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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
+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.txt
+http://www.apache.org/licenses/LICENSE-2.0.txt
- 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.
+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.
=========================================================================*/
#include "imstkSPHModel.h"
+#include "imstkLogger.h"
#include "imstkParallelUtils.h"
#include "imstkPointSet.h"
+#include "imstkTaskGraph.h"
namespace imstk
{
@@ -54,6 +56,34 @@ SPHModelConfig::initialize()
m_kernelRadiusSqr = m_kernelRadius * m_kernelRadius;
}
+SPHModel::SPHModel() : DynamicalModel<SPHKinematicState>(DynamicalModelType::SmoothedParticleHydrodynamics)
+{
+ m_validGeometryTypes = { Geometry::Type::PointSet };
+
+ m_findParticleNeighborsNode = m_taskGraph->addFunction("SPHModel_Partition", std::bind(&SPHModel::findParticleNeighbors, this));
+ m_computeDensityNode = m_taskGraph->addFunction("SPHModel_ComputeDensity", [&]()
+ {
+ computeNeighborRelativePositions();
+ computeDensity();
+ normalizeDensity();
+ collectNeighborDensity();
+ });
+ m_computePressureAccelNode =
+ m_taskGraph->addFunction("SPHModel_ComputePressureAccel", std::bind(&SPHModel::computePressureAcceleration, this));
+ m_computeSurfaceTensionNode =
+ m_taskGraph->addFunction("SPHModel_ComputeSurfaceTensionAccel", std::bind(&SPHModel::computeSurfaceTension, this));
+ m_computeTimeStepSizeNode =
+ m_taskGraph->addFunction("SPHModel_ComputeTimestep", std::bind(&SPHModel::computeTimeStepSize, this));
+ m_integrateNode =
+ m_taskGraph->addFunction("SPHModel_Integrate", [&]()
+ {
+ sumAccels();
+ updateVelocity(getTimeStep());
+ computeViscosity();
+ moveParticles(getTimeStep());
+ });
+}
+
bool
SPHModel::initialize()
{
@@ -80,7 +110,10 @@ SPHModel::initialize()
// Initialize neighbor searcher
m_neighborSearcher = std::make_shared<NeighborSearch>(m_modelParameters->m_NeighborSearchMethod,
- m_modelParameters->m_kernelRadius);
+ m_modelParameters->m_kernelRadius);
+
+ m_pressureAccels = std::make_shared<StdVectorOfVec3d>(getState().getNumParticles());
+ m_surfaceTensionAccels = std::make_shared<StdVectorOfVec3d>(getState().getNumParticles());
return true;
}
@@ -93,19 +126,22 @@ SPHModel::updatePhysicsGeometry()
}
void
-SPHModel::advanceTimeStep()
+SPHModel::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink)
{
- findParticleNeighbors();
- computeNeighborRelativePositions();
- computeDensity();
- normalizeDensity();
- collectNeighborDensity();
- computePressureAcceleration();
- computeSurfaceTension();
- computeTimeStepSize();
- updateVelocity(getTimeStep());
- computeViscosity();
- moveParticles(getTimeStep());
+ // Setup graph connectivity
+ m_taskGraph->addEdge(source, m_findParticleNeighborsNode);
+ m_taskGraph->addEdge(m_findParticleNeighborsNode, m_computeDensityNode);
+
+ // Pressure, Surface Tension, and time step size can be done in parallel
+ m_taskGraph->addEdge(m_computeDensityNode, m_computePressureAccelNode);
+ m_taskGraph->addEdge(m_computeDensityNode, m_computeSurfaceTensionNode);
+ m_taskGraph->addEdge(m_computeDensityNode, m_computeTimeStepSizeNode);
+
+ m_taskGraph->addEdge(m_computePressureAccelNode, m_integrateNode);
+ m_taskGraph->addEdge(m_computeSurfaceTensionNode, m_integrateNode);
+ m_taskGraph->addEdge(m_computeTimeStepSizeNode, m_integrateNode);
+
+ m_taskGraph->addEdge(m_integrateNode, sink);
}
void
@@ -282,6 +318,7 @@ SPHModel::computePressureAcceleration()
return error > Real(0) ? error : Real(0);
};
+ StdVectorOfVec3d& pressureAccels = *m_pressureAccels;
ParallelUtils::parallelFor(getState().getNumParticles(),
[&](const size_t p) {
Vec3r accel(0, 0, 0);
@@ -307,7 +344,19 @@ SPHModel::computePressureAcceleration()
}
accel *= m_modelParameters->m_pressureStiffness * m_modelParameters->m_particleMass;
- getState().getAccelerations()[p] = accel;
+ //getState().getAccelerations()[p] = accel;
+ pressureAccels[p] = accel;
+ });
+}
+
+void
+SPHModel::sumAccels()
+{
+ const StdVectorOfVec3d& pressureAccels = *m_pressureAccels;
+ const StdVectorOfVec3d& surfaceTensionAccels = *m_surfaceTensionAccels;
+ ParallelUtils::parallelFor(getState().getNumParticles(),
+ [&](const size_t p) {
+ getState().getAccelerations()[p] = pressureAccels[p] + surfaceTensionAccels[p];
});
}
@@ -366,7 +415,7 @@ SPHModel::computeViscosity()
ParallelUtils::parallelFor(getState().getNumParticles(),
[&](const size_t p) {
getState().getVelocities()[p] += getState().getDiffuseVelocities()[p];
- });
+ });
}
void
@@ -396,6 +445,7 @@ SPHModel::computeSurfaceTension()
});
// Second, compute surface tension acceleration
+ StdVectorOfVec3d& surfaceTensionAccels = *m_surfaceTensionAccels;
ParallelUtils::parallelFor(getState().getNumParticles(),
[&](const size_t p) {
const auto& fluidNeighborList = getState().getFluidNeighborLists()[p];
@@ -436,7 +486,8 @@ SPHModel::computeSurfaceTension()
}
accel *= m_modelParameters->m_surfaceTensionStiffness;
- getState().getAccelerations()[p] += accel;
+ //getState().getAccelerations()[p] += accel;
+ surfaceTensionAccels[p] = accel;
});
}
@@ -446,6 +497,6 @@ SPHModel::moveParticles(Real timestep)
ParallelUtils::parallelFor(getState().getNumParticles(),
[&](const size_t p) {
getState().getPositions()[p] += getState().getVelocities()[p] * timestep;
- });
+ });
}
} // end namespace imstk
diff --git a/Source/DynamicalModels/ObjectModels/imstkSPHModel.h b/Source/DynamicalModels/ObjectModels/imstkSPHModel.h
index d5cedcf23b8a2eabe9996d56bbaaed58cc5bae2d..56895525b9f7a9fdfe3601b48e816a6d47e3d669 100644
--- a/Source/DynamicalModels/ObjectModels/imstkSPHModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkSPHModel.h
@@ -28,6 +28,7 @@
namespace imstk
{
+class ComputeNode;
class PointSet;
///
@@ -92,10 +93,7 @@ public:
///
/// \brief Constructor
///
- SPHModel() : DynamicalModel<SPHKinematicState>(DynamicalModelType::SmoothedParticleHydrodynamics)
- {
- m_validGeometryTypes = { Geometry::Type::PointSet };
- }
+ SPHModel();
///
/// \brief Destructor
@@ -175,10 +173,19 @@ public:
virtual double getTimeStep() const override
{ return static_cast<double>(m_dt); }
+ std::shared_ptr<TaskNode> getFindParticleNeighborsNode() const { return m_findParticleNeighborsNode; }
+ std::shared_ptr<TaskNode> getComputeDensityNode() const { return m_computeDensityNode; }
+ std::shared_ptr<TaskNode> getComputePressureNode() const { return m_computePressureAccelNode; }
+ std::shared_ptr<TaskNode> getComputeSurfaceTensionNode() const { return m_computeSurfaceTensionNode; }
+ std::shared_ptr<TaskNode> getComputeTimeStepSizeNode() const { m_computeTimeStepSizeNode; }
+ std::shared_ptr<TaskNode> getSumAccelsNode() const { m_sumAccelsNode; }
+ std::shared_ptr<TaskNode> getIntegrateNode() const { m_integrateNode; }
+
+protected:
///
- /// \brief Do one time step simulation
+ /// \brief Setup SPH compute graph connectivity
///
- void advanceTimeStep();
+ virtual void initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink) override;
private:
///
@@ -222,6 +229,11 @@ private:
///
void computePressureAcceleration();
+ ///
+ /// \brief Sum the forces computed in parallel
+ ///
+ void sumAccels();
+
///
/// \brief Update particle velocities due to pressure
///
@@ -244,6 +256,16 @@ private:
///
void moveParticles(const Real timestep);
+protected:
+ std::shared_ptr<TaskNode> m_findParticleNeighborsNode = nullptr;
+ std::shared_ptr<TaskNode> m_computeDensityNode = nullptr;
+ std::shared_ptr<TaskNode> m_computePressureAccelNode = nullptr;
+ std::shared_ptr<TaskNode> m_computeSurfaceTensionNode = nullptr;
+ std::shared_ptr<TaskNode> m_computeTimeStepSizeNode = nullptr;
+ std::shared_ptr<TaskNode> m_sumAccelsNode = nullptr;
+ std::shared_ptr<TaskNode> m_integrateNode = nullptr;
+
+private:
std::shared_ptr<PointSet> m_pointSetGeometry;
SPHSimulationState m_simulationState;
@@ -253,5 +275,8 @@ private:
SPHSimulationKernels m_kernels; ///> SPH kernels (must be initialized during model initialization)
std::shared_ptr<SPHModelConfig> m_modelParameters; ///> SPH Model parameters (must be set before simulation)
std::shared_ptr<NeighborSearch> m_neighborSearcher; ///> Neighbor Search (must be initialized during model initialization)
+
+ std::shared_ptr<StdVectorOfVec3d> m_pressureAccels = nullptr;
+ std::shared_ptr<StdVectorOfVec3d> m_surfaceTensionAccels = nullptr;
};
} // end namespace imstk
diff --git a/Source/DynamicalModels/ObjectStates/imstkPbdState.cpp b/Source/DynamicalModels/ObjectStates/imstkPbdState.cpp
index 4c06dc9f5a2b0df9df9bf4474a7e259c10f30f5a..13c84c079c919daf9bc67d76a1d167534e89e937 100644
--- a/Source/DynamicalModels/ObjectStates/imstkPbdState.cpp
+++ b/Source/DynamicalModels/ObjectStates/imstkPbdState.cpp
@@ -25,36 +25,25 @@
namespace imstk
{
void
-PbdState::initialize(const size_t numNodes, const bool (& options)[3])
+PbdState::initialize(const size_t numNodes)
{
- // CHECKBACK : m_pos could actually not be another copy
- if (options[0])
- {
- m_pos.resize(numNodes, Vec3d(0, 0, 0));
- }
-
- if (options[1])
- {
- m_vel.resize(numNodes, Vec3d(0, 0, 0));
- }
-
- if (options[2])
- {
- m_acc.resize(numNodes, Vec3d(0, 0, 0));
- }
+ m_pos->resize(numNodes, Vec3d(0.0, 0.0, 0.0));
+ m_vel->resize(numNodes, Vec3d(0.0, 0.0, 0.0));
+ m_acc->resize(numNodes, Vec3d(0.0, 0.0, 0.0));
}
void
-PbdState::initialize(const std::shared_ptr<PointSet>& m, const bool (& options)[3])
+PbdState::initialize(const StdVectorOfVec3d& vertices)
{
- this->initialize(m->getNumVertices(), options);
+ this->initialize(vertices.size());
+ setPositions(vertices);
}
void
PbdState::setState(std::shared_ptr<PbdState> rhs)
{
- m_pos = rhs->getPositions();
- m_vel = rhs->getVelocities();
- m_acc = rhs->getAccelerations();
+ *m_pos = *rhs->getPositions();
+ *m_vel = *rhs->getVelocities();
+ *m_acc = *rhs->getAccelerations();
}
} // imstk
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectStates/imstkPbdState.h b/Source/DynamicalModels/ObjectStates/imstkPbdState.h
index bcc2884edf0417daf9cf185002813c922f46a4e4..65f5019e57a89f52c6b0097059741b7a5bdf3223 100644
--- a/Source/DynamicalModels/ObjectStates/imstkPbdState.h
+++ b/Source/DynamicalModels/ObjectStates/imstkPbdState.h
@@ -43,30 +43,40 @@ public:
///
/// \brief Initialize the pbd state
///
- void initialize(const size_t numNodes, const bool (& options)[3]);
- void initialize(const std::shared_ptr<PointSet>& m, const bool (& options)[3]);
+ void initialize(const size_t numNodes);
+ void initialize(const StdVectorOfVec3d& vertices);
///
/// \brief Get/Set nodal position given the index
///
- void setVertexPosition(const size_t& idx, const Vec3d& pos) { m_pos.at(idx) = pos; }
- Vec3d& getVertexPosition(const size_t& idx) { return m_pos.at(idx); }
+ void setVertexPosition(const size_t& idx, const Vec3d& pos) { m_pos->at(idx) = pos; }
+ Vec3d& getVertexPosition(const size_t& idx) { return m_pos->at(idx); }
///
/// \brief Returns the vector of current nodal positions
///
- StdVectorOfVec3d& getPositions() { return m_pos; }
- void setPositions(const StdVectorOfVec3d& p) { m_pos = p; }
+ std::shared_ptr<StdVectorOfVec3d> getPositions() { return m_pos; }
+ void setPositions(std::shared_ptr<StdVectorOfVec3d> p)
+ {
+ m_pos->resize(p->size());
+ std::copy(p->begin(), p->end(), m_pos->begin());
+ }
+
+ void setPositions(const StdVectorOfVec3d& p)
+ {
+ m_pos->resize(p.size());
+ std::copy(p.begin(), p.end(), m_pos->begin());
+ }
///
/// \brief Returns the vector of current nodal velocities
///
- StdVectorOfVec3d& getVelocities() { return m_vel; }
+ std::shared_ptr<StdVectorOfVec3d> getVelocities() { return m_vel; }
///
/// \brief Returns the vector of current nodal accelerations
///
- StdVectorOfVec3d& getAccelerations() { return m_acc; }
+ std::shared_ptr<StdVectorOfVec3d> getAccelerations() { return m_acc; }
///
/// \brief Set the state to a given one
@@ -74,8 +84,8 @@ public:
void setState(std::shared_ptr<PbdState> rhs);
private:
- StdVectorOfVec3d m_pos; ///> Nodal positions
- StdVectorOfVec3d m_vel; ///> Nodal velocities
- StdVectorOfVec3d m_acc; ///> Nodal acelerations
+ std::shared_ptr<StdVectorOfVec3d> m_pos = std::make_shared<StdVectorOfVec3d>(); ///> Nodal positions
+ std::shared_ptr<StdVectorOfVec3d> m_vel = std::make_shared<StdVectorOfVec3d>(); ///> Nodal velocities
+ std::shared_ptr<StdVectorOfVec3d> m_acc = std::make_shared<StdVectorOfVec3d>(); ///> Nodal acelerations
};
} // imstk
diff --git a/Source/DynamicalModels/ObjectStates/imstkRigidBodyState.h b/Source/DynamicalModels/ObjectStates/imstkRigidBodyState.h
index 2f0764a6d07e0a0016081a0b14f60138a38db3ea..b67ce30c98cce95d020b5a7b7080949d4f3efad8 100644
--- a/Source/DynamicalModels/ObjectStates/imstkRigidBodyState.h
+++ b/Source/DynamicalModels/ObjectStates/imstkRigidBodyState.h
@@ -19,8 +19,7 @@
=========================================================================*/
-#ifndef imstkRigidBodyState_h
-#define imstkRigidBodyState_h
+#pragma once
#include <Eigen/Dense>
#include <memory>
@@ -77,6 +76,4 @@ private:
//Can add linear velocity and angular velocity too.
};
-} // imstk
-
-#endif // imstkPbdState_h
\ No newline at end of file
+} // imstk
\ No newline at end of file
diff --git a/Source/Geometry/Mesh/imstkTetrahedralMesh.h b/Source/Geometry/Mesh/imstkTetrahedralMesh.h
index b4881a889acec38508f97390f464c720611e6ebd..0324193bddb08b788bcfb24b495d6abfc8c2df74 100644
--- a/Source/Geometry/Mesh/imstkTetrahedralMesh.h
+++ b/Source/Geometry/Mesh/imstkTetrahedralMesh.h
@@ -21,8 +21,8 @@
#pragma once
-// imstk
#include "imstkVolumetricMesh.h"
+#include <array>
namespace imstk
{
diff --git a/Source/MeshIO/imstkVegaMeshIO.cpp b/Source/MeshIO/imstkVegaMeshIO.cpp
index 77bdb4dff607042008450b584100e51ad31b085f..98a61e6f3e72dc0847740115163438cdfd751bc9 100644
--- a/Source/MeshIO/imstkVegaMeshIO.cpp
+++ b/Source/MeshIO/imstkVegaMeshIO.cpp
@@ -159,8 +159,8 @@ VegaMeshIO::convertVolumetricMeshToVegaMesh(const std::shared_ptr<imstk::Volumet
if (imstkVolMesh->getType() == Geometry::Type::TetrahedralMesh)
{
// Using default material properties to append to the .veg file
- const double E = 1E6;
- const double nu = 0.45;
+ const double E = 1E7;
+ const double nu = 0.4;
const double density = 1000.0;
auto imstkVolTetMesh = std::dynamic_pointer_cast<imstk::TetrahedralMesh>(imstkVolMesh);
diff --git a/Source/Rendering/VTKRenderer/imstkVTKRenderer.cpp b/Source/Rendering/VTKRenderer/imstkVTKRenderer.cpp
index 64ea662dde19fcdfd0e30cd51a201a7463e5721d..4b1fa596e0c3d8db22c34216cac857ab7080fc06 100644
--- a/Source/Rendering/VTKRenderer/imstkVTKRenderer.cpp
+++ b/Source/Rendering/VTKRenderer/imstkVTKRenderer.cpp
@@ -24,7 +24,6 @@
#include "imstkScene.h"
#include "imstkSceneObject.h"
#include "imstkCamera.h"
-#include "imstkCollisionGraph.h"
#include "imstkVTKRenderDelegate.h"
#include "imstkVTKSurfaceMeshRenderDelegate.h"
#include "imstkLight.h"
@@ -38,6 +37,18 @@
#include <vtkProp.h>
#include <vtkLight.h>
#include <vtkRenderer.h>
+#include <vtkRenderWindow.h>
+
+#include <vtkAxis.h>
+#include <vtkChartXY.h>
+#include <vtkContextActor.h>
+#include <vtkContextScene.h>
+#include <vtkDoubleArray.h>
+#include <vtkIntArray.h>
+#include <vtkPlotBar.h>
+#include <vtkStringArray.h>
+#include <vtkTable.h>
+#include <vtkTextProperty.h>
namespace imstk
{
@@ -171,7 +182,6 @@ VTKRenderer::VTKRenderer(std::shared_ptr<Scene> scene, const bool enableVR) : m_
m_defaultVtkCamera = vtkSmartPointer<vtkOpenVRCamera>::New();
m_vtkRenderer->SetActiveCamera(m_defaultVtkCamera);
}
-
#endif
///TODO : based on scene properties
// Customize background colors
@@ -231,6 +241,49 @@ VTKRenderer::VTKRenderer(std::shared_ptr<Scene> scene, const bool enableVR) : m_
m_camPos[1].Translation[2] = 0.0;
}
#endif
+
+ {
+ // Add the benchmarking chart
+ m_timeTableChart = vtkSmartPointer<vtkChartXY>::New();
+ vtkSmartPointer<vtkContextScene> m_benchmarkChartScene = vtkSmartPointer<vtkContextScene>::New();
+ m_timeTableChartActor = vtkSmartPointer<vtkContextActor>::New();
+ m_vtkRenderer->AddActor(m_timeTableChartActor);
+ m_benchmarkChartScene->SetRenderer(m_vtkRenderer);
+
+ m_timeTableChart->SetAutoSize(true);
+ m_timeTableChart->SetSize(vtkRectf(0.0, 0.0, 600.0, 600.0));
+
+ m_benchmarkChartScene->AddItem(m_timeTableChart);
+ m_timeTableChartActor->SetScene(m_benchmarkChartScene);
+ m_timeTableChartActor->SetVisibility(false);
+
+ m_timeTablePlot = vtkPlotBar::SafeDownCast(m_timeTableChart->AddPlot(vtkChart::BAR));
+ m_timeTablePlot->SetColor(0.6, 0.1, 0.1);
+ m_timeTablePlot->SetOrientation(vtkPlotBar::HORIZONTAL);
+ m_timeTableChart->GetAxis(vtkAxis::BOTTOM)->SetTitle("ms");
+ m_timeTableChart->GetAxis(vtkAxis::LEFT)->SetTitle("");
+ m_timeTableChart->GetAxis(vtkAxis::LEFT)->GetLabelProperties()->SetVerticalJustification(VTK_TEXT_CENTERED);
+ m_timeTableChart->GetAxis(vtkAxis::LEFT)->GetLabelProperties()->SetJustification(VTK_TEXT_RIGHT);
+
+ m_timeTable = vtkSmartPointer<vtkTable>::New();
+ vtkSmartPointer<vtkDoubleArray> xIndices = vtkSmartPointer<vtkDoubleArray>::New();
+ xIndices->SetName("Indices");
+ xIndices->SetNumberOfValues(0);
+ vtkSmartPointer<vtkDoubleArray> yElapsedTimes = vtkSmartPointer<vtkDoubleArray>::New();
+ yElapsedTimes->SetName("Elapsed Times");
+ yElapsedTimes->SetNumberOfValues(0);
+ vtkSmartPointer<vtkStringArray> labels = vtkSmartPointer<vtkStringArray>::New();
+ labels->SetName("Labels");
+ labels->SetNumberOfValues(0);
+ m_timeTable->AddColumn(xIndices);
+ m_timeTable->AddColumn(yElapsedTimes);
+ m_timeTable->AddColumn(labels);
+ m_timeTablePlot->SetInputData(m_timeTable, 0, 1);
+
+ vtkAxis* axisY = m_timeTableChart->GetAxis(vtkAxis::LEFT);
+ //axisY->SetRange(xIndices->GetRange());
+ axisY->SetCustomTickPositions(xIndices, labels);
+ }
}
vtkSmartPointer<vtkRenderer>
@@ -360,15 +413,83 @@ VTKRenderer::setAxesVisibility(const bool visible)
m_AxesActor->SetVisibility(visible);
}
-///
-/// \brief Returns whether the debug axes is visible or not
-///
bool
VTKRenderer::getAxesVisibility() const
{
return m_AxesActor->GetVisibility();
}
+void
+VTKRenderer::setTimeTable(const std::unordered_map<std::string, double>& timeTable)
+{
+ // Sort by elapsed times
+ std::vector<std::pair<std::string, double>> nameToTimesVec(timeTable.begin(), timeTable.end());
+ std::sort(nameToTimesVec.begin(), nameToTimesVec.end(),
+ [](const std::pair<std::string, double>& a, const std::pair<std::string, double>& b) { return a.second < b.second; });
+
+ // Construct vtkTable from provided data
+ vtkSmartPointer<vtkDoubleArray> xIndices = vtkDoubleArray::SafeDownCast(m_timeTable->GetColumn(0));
+ vtkSmartPointer<vtkDoubleArray> yElapsedTimes = vtkDoubleArray::SafeDownCast(m_timeTable->GetColumn(1));
+ vtkSmartPointer<vtkStringArray> labels = vtkStringArray::SafeDownCast(m_timeTable->GetColumn(2));
+
+ labels->SetNumberOfValues(nameToTimesVec.size());
+ xIndices->SetNumberOfValues(nameToTimesVec.size());
+ yElapsedTimes->SetNumberOfValues(nameToTimesVec.size());
+ for (size_t i = 0; i < nameToTimesVec.size(); i++)
+ {
+ labels->SetValue(i, nameToTimesVec[i].first.c_str());
+ xIndices->SetValue(i, i + 1);
+ yElapsedTimes->SetValue(i, nameToTimesVec[i].second);
+ }
+
+ // The range for the x axis is based on history of the elapsed times
+ vtkAxis* botAxis = m_timeTableChart->GetAxis(vtkAxis::BOTTOM);
+
+ // Get the previous and current range
+ double newMaxElapsed = yElapsedTimes->GetRange()[1];
+ yElapsedTimes->Modified();
+ double currMaxElapsed = botAxis->GetMaximum();
+
+ // Always respect the max as all information should be shown
+ if (newMaxElapsed > currMaxElapsed)
+ {
+ botAxis->SetRange(0.0, newMaxElapsed);
+ }
+ // But if current elapsed is less than the existing one we can lag
+ else
+ {
+ // Lag downscaling by 400 iterations
+ if (m_timeTableIter % 400 == 0)
+ {
+ botAxis->SetRange(0.0, newMaxElapsed);
+ }
+ else
+ {
+ botAxis->SetRange(0.0, currMaxElapsed);
+ }
+ m_timeTableIter++;
+ }
+ botAxis->Modified();
+
+ vtkAxis* leftAxis = m_timeTableChart->GetAxis(vtkAxis::LEFT);
+ leftAxis->SetRange(xIndices->GetRange());
+ leftAxis->SetCustomTickPositions(xIndices, labels);
+
+ m_timeTable->Modified();
+}
+
+void
+VTKRenderer::setTimeTableVisibility(const bool visible)
+{
+ m_timeTableChartActor->SetVisibility(visible);
+}
+
+bool
+VTKRenderer::getTimeTableVisibility() const
+{
+ return m_timeTableChartActor->GetVisibility();
+}
+
void
VTKRenderer::updateSceneCamera(std::shared_ptr<Camera> imstkCam)
{
diff --git a/Source/Rendering/VTKRenderer/imstkVTKRenderer.h b/Source/Rendering/VTKRenderer/imstkVTKRenderer.h
index 765e1be2f2f323a22839ba7f6533c523b62e8e36..516920cfa84f14eed6b631058620c198748b9c55 100644
--- a/Source/Rendering/VTKRenderer/imstkVTKRenderer.h
+++ b/Source/Rendering/VTKRenderer/imstkVTKRenderer.h
@@ -35,11 +35,17 @@
#include <vtkOpenVROverlayInternal.h>
#endif
+#include <unordered_map>
+
class vtkAxesActor;
class vtkCamera;
-class vtkProp;
+class vtkChartXY;
+class vtkContextActor;
class vtkLight;
+class vtkPlotBar;
+class vtkProp;
class vtkRenderer;
+class vtkTable;
namespace imstk
{
@@ -93,6 +99,21 @@ public:
///
bool getAxesVisibility() const;
+ ///
+ /// \brief Sets the benchmarking table using unordered_map
+ ///
+ void setTimeTable(const std::unordered_map<std::string, double>& timeTable);
+
+ ///
+ /// \brief Set the visibility of the benchmark graph
+ ///
+ void setTimeTableVisibility(const bool visible);
+
+ ///
+ /// \brief Get the visibility of the benchmark graph
+ ///
+ bool getTimeTableVisibility() const;
+
///
/// \brief Updates the scene camera's position and orientation
///
@@ -129,6 +150,7 @@ protected:
///
void addActors(const std::vector<vtkSmartPointer<vtkProp>>& actorList);
+protected:
vtkSmartPointer<vtkRenderer> m_vtkRenderer;
// cameras
@@ -154,5 +176,12 @@ protected:
#ifdef iMSTK_ENABLE_VR
std::vector<vtkOpenVRCameraPose> m_camPos;
#endif
+
+ vtkSmartPointer<vtkChartXY> m_timeTableChart = nullptr;
+ vtkSmartPointer<vtkContextActor> m_timeTableChartActor = nullptr;
+ vtkSmartPointer<vtkTable> m_timeTable = nullptr;
+
+ vtkPlotBar* m_timeTablePlot = nullptr;
+ int m_timeTableIter = 0;
};
}
diff --git a/Source/Scene/imstkCollisionGraph.cpp b/Source/Scene/imstkCollisionGraph.cpp
index afd290de55049a70ae28f8f4afcf7de79bf321e7..6805e2faa8b2f64aa9d9dab2681bdf7f4978a515 100644
--- a/Source/Scene/imstkCollisionGraph.cpp
+++ b/Source/Scene/imstkCollisionGraph.cpp
@@ -20,168 +20,160 @@
=========================================================================*/
#include "imstkCollisionGraph.h"
-
-#include <g3log/g3log.hpp>
+#include "imstkLogger.h"
+#include "imstkObjectInteractionPair.h"
+#include "imstkSceneObject.h"
namespace imstk
{
-std::shared_ptr<InteractionPair>
-CollisionGraph::addInteractionPair(CollidingObjectPtr A,
- CollidingObjectPtr B,
- CollisionDetection::Type CDType,
- CollisionHandling::Type CHAType,
- CollisionHandling::Type CHBType)
-{
- // Check that interaction pair does not exist
- if (this->getInteractionPair(A, B) != nullptr)
- {
- LOG(WARNING) << "CollisionGraph::addInteractionPair error: interaction already defined for "
- << A->getName() << " & " << B->getName() << ".";
- return nullptr;
- }
-
- // Create interaction pair
- auto intPair = std::make_shared<InteractionPair>(A, B, CDType, CHAType, CHBType);
-
- // Check validity
- if (!intPair->isValid())
- {
- LOG(WARNING) << "CollisionGraph::addInteractionPair error: could not create interaction for "
- << A->getName() << " & " << B->getName() << " with those parameters.";
- intPair.reset();
- return nullptr;
- }
-
- // Populate book-keeping
- m_interactionPairList.push_back(intPair);
- m_interactionPairMap[A].push_back(intPair);
- m_interactionPairMap[B].push_back(intPair);
-
- // Return interaction pair
- return intPair;
-}
+//CollisionGraph::ObjectInteractionPtr
+//CollisionGraph::addInteractionPair(SceneObjectPtr A,
+// SceneObjectPtr B,
+// CollisionDetection::Type CDType,
+// CollisionHandling::Type CHAType,
+// CollisionHandling::Type CHBType)
+//{
+// // Check that interaction pair does not exist
+// //if (this->getInteractionPair(A, B) != nullptr)
+// //{
+// // LOG(WARNING) << "CollisionGraph::addInteractionPair error: interaction already defined for "
+// // << A->getName() << " & " << B->getName() << ".";
+// // return nullptr;
+// //}
+//
+// //// Create interaction pair
+// //auto intPair = std::make_shared<CollisionPair>(A, B, CDType, CHAType, CHBType);
+//
+// //// Check validity
+// ///*if (!intPair->isValid())
+// //{
+// // LOG(WARNING) << "CollisionGraph::addInteractionPair error: could not create interaction for "
+// // << A->getName() << " & " << B->getName() << " with those parameters.";
+// // intPair.reset();
+// // return nullptr;
+// //}*/
+//
+// //// Populate book-keeping
+// //m_interactionPairs.push_back(intPair);
+// //m_interactionPairMap[A].push_back(intPair);
+// //m_interactionPairMap[B].push_back(intPair);
+//
+// //// Return interaction pair
+// //return intPair;
+// return nullptr;
+//}
+//
+//CollisionGraph::ObjectInteractionPtr
+//CollisionGraph::addInteractionPair(SceneObjectPtr A,
+// SceneObjectPtr B,
+// CollisionDetectionPtr CD,
+// CollisionHandlingPtr CHA,
+// CollisionHandlingPtr CHB)
+//{
+// //// Check that interaction pair does not exist
+// //if (this->getInteractionPair(A, B) != nullptr)
+// //{
+// // LOG(WARNING) << "CollisionGraph::addInteractionPair error: interaction already defined for "
+// // << A->getName() << " & " << B->getName() << ".";
+// // return nullptr;
+// //}
+//
+// //// Create interaction pair
+// //auto intPair = std::make_shared<CollisionPair>(A, B, CD, CHA, CHB);
+//
+// //// Check validity
+// ///*if (!intPair->isValid())
+// //{
+// // LOG(WARNING) << "CollisionGraph::addInteractionPair error: could not create interaction for "
+// // << A->getName() << " & " << B->getName() << " with those parameters.";
+// // intPair.reset();
+// // return nullptr;
+// //}*/
+//
+// //// Populate book-keeping
+// //m_interactionPairs.push_back(intPair);
+// //m_interactionPairMap[A].push_back(intPair);
+// //m_interactionPairMap[B].push_back(intPair);
+//
+// //// Return interaction pair
+// //return intPair;
+// return nullptr;
+//}
-//\todo Refactor -> PBD only
void
-CollisionGraph::addInteractionPair(std::shared_ptr<PbdInteractionPair> pair)
+CollisionGraph::addInteraction(ObjectInteractionPtr pair)
{
- m_interactionPbdPairList.push_back(pair);
-}
-
-std::shared_ptr<InteractionPair>
-CollisionGraph::addInteractionPair(CollidingObjectPtr A,
- CollidingObjectPtr B,
- CollisionDetectionPtr CD,
- CollisionHandlingPtr CHA,
- CollisionHandlingPtr CHB)
-{
- // Check that interaction pair does not exist
- if (this->getInteractionPair(A, B) != nullptr)
- {
- LOG(WARNING) << "CollisionGraph::addInteractionPair error: interaction already defined for "
- << A->getName() << " & " << B->getName() << ".";
- return nullptr;
- }
-
- // Create interaction pair
- auto intPair = std::make_shared<InteractionPair>(A, B, CD, CHA, CHB);
-
- // Check validity
- if (!intPair->isValid())
- {
- LOG(WARNING) << "CollisionGraph::addInteractionPair error: could not create interaction for "
- << A->getName() << " & " << B->getName() << " with those parameters.";
- intPair.reset();
- return nullptr;
- }
-
- // Populate book-keeping
- m_interactionPairList.push_back(intPair);
- m_interactionPairMap[A].push_back(intPair);
- m_interactionPairMap[B].push_back(intPair);
-
- // Return interaction pair
- return intPair;
+ m_interactionPairs.push_back(pair);
+ m_interactionPairMap[pair->getObjectsPair().first].push_back(pair);
+ m_interactionPairMap[pair->getObjectsPair().second].push_back(pair);
}
bool
-CollisionGraph::removeInteractionPair(CollidingObjectPtr A, CollidingObjectPtr B)
+CollisionGraph::removeInteractionPair(SceneObjectPtr A, SceneObjectPtr B)
{
- std::shared_ptr<InteractionPair> intPair = this->getInteractionPair(A, B);
-
- // Check that interaction pair exists
- if (intPair == nullptr)
- {
- LOG(WARNING) << "CollisionGraph::removeInteractionPair error: no such pair for objects "
- << A->getName() << " & " << B->getName() << ".";
- return false;
- }
-
- // Remove interaction pair from list
- auto it = std::remove(m_interactionPairList.begin(),
- m_interactionPairList.end(),
- intPair);
- m_interactionPairList.erase(it, m_interactionPairList.end());
-
- // Remove interaction pair from maps
- it = std::remove(m_interactionPairMap.at(A).begin(),
- m_interactionPairMap.at(A).end(),
- intPair);
- m_interactionPairMap.at(A).erase(it, m_interactionPairMap.at(A).end());
-
- it = std::remove(m_interactionPairMap.at(B).begin(),
- m_interactionPairMap.at(B).end(),
- intPair);
- m_interactionPairMap.at(B).erase(it, m_interactionPairMap.at(B).end());
-
- // If no more interactions for objects, remove objects from list
- if (m_interactionPairMap.at(A).empty())
- {
- m_interactionPairMap.erase(A);
- }
- if (m_interactionPairMap.at(B).empty())
- {
- m_interactionPairMap.erase(B);
- }
+ //std::shared_ptr<InteractionPair> intPair = this->getInteractionPair(A, B);
+
+ //// Check that interaction pair exists
+ //if (intPair == nullptr)
+ //{
+ // LOG(WARNING) << "CollisionGraph::removeInteractionPair error: no such pair for objects "
+ // << A->getName() << " & " << B->getName() << ".";
+ // return false;
+ //}
+
+ //// Remove interaction pair from list
+ //auto it = std::remove(m_interactionPairs.begin(),
+ // m_interactionPairs.end(),
+ // intPair);
+ //m_interactionPairs.erase(it, m_interactionPairs.end());
+
+ //// Remove interaction pair from maps
+ //it = std::remove(m_interactionPairMap.at(A).begin(),
+ // m_interactionPairMap.at(A).end(),
+ // intPair);
+ //m_interactionPairMap.at(A).erase(it, m_interactionPairMap.at(A).end());
+
+ //it = std::remove(m_interactionPairMap.at(B).begin(),
+ // m_interactionPairMap.at(B).end(),
+ // intPair);
+ //m_interactionPairMap.at(B).erase(it, m_interactionPairMap.at(B).end());
+
+ //// If no more interactions for objects, remove objects from list
+ //if (m_interactionPairMap.at(A).empty())
+ //{
+ // m_interactionPairMap.erase(A);
+ //}
+ //if (m_interactionPairMap.at(B).empty())
+ //{
+ // m_interactionPairMap.erase(B);
+ //}
return true;
}
bool
-CollisionGraph::removeInteractionPair(InteractionPairPtr intPair)
-{
- return this->removeInteractionPair(intPair->getObjectsPair().first,
- intPair->getObjectsPair().second);
-}
-
-const std::vector<std::shared_ptr<PbdInteractionPair>>&
-CollisionGraph::getPbdPairList() const
+CollisionGraph::removeInteractionPair(ObjectInteractionPtr intPair)
{
- return m_interactionPbdPairList;
+ /* std::shared_ptr<CollidingObject> obj1 = std::static_pointer_cast<CollidingObject>(intPair->getObjectsPair().first);
+ std::shared_ptr<CollidingObject> obj2 = std::static_pointer_cast<CollidingObject>(intPair->getObjectsPair().second);
+ return this->removeInteractionPair(obj1, obj2);*/
+ return true;
}
-std::shared_ptr<InteractionPair>
-CollisionGraph::getInteractionPair(CollidingObjectPtr A, CollidingObjectPtr B)
+CollisionGraph::ObjectInteractionPtr
+CollisionGraph::getInteractionPair(SceneObjectPtr A, SceneObjectPtr B)
{
- for (const auto& intPair : m_interactionPairList)
+ /*for (const auto& intPair : m_interactionPairs)
{
- if (intPair->getObjectsPair() == std::pair<CollidingObjectPtr, CollidingObjectPtr>(A, B)
- /*|| intPair->getObjectsPair() == std::pair<CollidingObjectPtr, CollidingObjectPtr>(B, A)*/
- )
+ if (intPair->getObjectsPair() == std::pair<std::shared_ptr<SceneObject>, std::shared_ptr<SceneObject>>(A, B))
{
return intPair;
}
- }
+ }*/
return nullptr;
}
-const std::vector<std::shared_ptr<InteractionPair>>&
-CollisionGraph::getInteractionPairList() const
-{
- return m_interactionPairList;
-}
-
-const std::unordered_map<std::shared_ptr<CollidingObject>, std::vector<std::shared_ptr<InteractionPair>>>&
+const std::unordered_map<CollisionGraph::SceneObjectPtr, std::vector<CollisionGraph::ObjectInteractionPtr>>&
CollisionGraph::getInteractionPairMap() const
{
return m_interactionPairMap;
diff --git a/Source/Scene/imstkCollisionGraph.h b/Source/Scene/imstkCollisionGraph.h
index 56668270a540f102407224b91f59fca06038a460..a547067b0cacd57181f4b5559789ed1560bc3354 100644
--- a/Source/Scene/imstkCollisionGraph.h
+++ b/Source/Scene/imstkCollisionGraph.h
@@ -21,31 +21,25 @@
#pragma once
-#include <vector>
+#include <memory>
#include <unordered_map>
-
-// imstk
-#include "imstkCollidingObject.h"
-#include "imstkInteractionPair.h"
-#include "imstkCollisionDetection.h"
-#include "imstkCollisionHandling.h"
-
-#include "imstkPbdInteractionPair.h"
+#include <vector>
namespace imstk
{
+class SceneObject;
+class ObjectInteractionPair;
+
///
/// \class CollisionGraph
///
-/// \brief
+/// \brief The CollisionGraph holds a set of interacting pairs of SceneObject's
///
class CollisionGraph
{
public:
- using CollidingObjectPtr = std::shared_ptr<CollidingObject>;
- using CollisionHandlingPtr = std::shared_ptr<CollisionHandling>;
- using CollisionDetectionPtr = std::shared_ptr<CollisionDetection>;
- using InteractionPairPtr = std::shared_ptr<InteractionPair>;
+ using SceneObjectPtr = std::shared_ptr<SceneObject>;
+ using ObjectInteractionPtr = std::shared_ptr<ObjectInteractionPair>;
///
/// \brief Default constructor
@@ -57,51 +51,35 @@ public:
///
~CollisionGraph() = default;
+public:
///
- /// \brief Add interaction pair in collision graph
+ /// \brief Add an interaction pair to the graph
///
- InteractionPairPtr addInteractionPair(CollidingObjectPtr A,
- CollidingObjectPtr B,
- CollisionDetection::Type CDType,
- CollisionHandling::Type CHAType,
- CollisionHandling::Type CHBType);
-
- //\todo Refactor -> PBD only
- InteractionPairPtr addInteractionPair(CollidingObjectPtr A,
- CollidingObjectPtr B,
- CollisionDetectionPtr CD,
- CollisionHandlingPtr CHA,
- CollisionHandlingPtr CHB);
-
- void addInteractionPair(std::shared_ptr<PbdInteractionPair> pair);
+ void addInteraction(ObjectInteractionPtr pair);
///
/// \brief Remove interaction pair in collision graph
///
- bool removeInteractionPair(CollidingObjectPtr A, CollidingObjectPtr B);
- bool removeInteractionPair(InteractionPairPtr intPair);
+ bool removeInteractionPair(SceneObjectPtr A, SceneObjectPtr B);
+ bool removeInteractionPair(ObjectInteractionPtr intPair);
///
/// \brief Returns the interaction pair if it exists
///
- InteractionPairPtr getInteractionPair(CollidingObjectPtr A, CollidingObjectPtr B);
+ ObjectInteractionPtr getInteractionPair(SceneObjectPtr A, SceneObjectPtr B);
///
- /// \brief Returns a vector of all interaction pairs in the collision graph
+ /// \brief Returns all interaction pairs
///
- const std::vector<InteractionPairPtr>& getInteractionPairList() const;
-
- const std::vector<std::shared_ptr<PbdInteractionPair>>& getPbdPairList() const;
+ const std::vector<ObjectInteractionPtr>& getInteractionPairs() const { return m_interactionPairs; }
///
/// \brief Returns a map of all interaction pairs per object
///
- const std::unordered_map<CollidingObjectPtr, std::vector<InteractionPairPtr>>& getInteractionPairMap() const;
+ const std::unordered_map<SceneObjectPtr, std::vector<ObjectInteractionPtr>>& getInteractionPairMap() const;
protected:
- std::vector<std::shared_ptr<PbdInteractionPair>> m_interactionPbdPairList; //\todo Refactor -> PBD only
-
- std::vector<InteractionPairPtr> m_interactionPairList; ///< All interaction pairs in the collision graph
- std::unordered_map<CollidingObjectPtr, std::vector<InteractionPairPtr>> m_interactionPairMap; ///< Map of interaction pairs per colliding object
+ std::vector<ObjectInteractionPtr> m_interactionPairs; ///< All interaction pairs in the collision graph
+ std::unordered_map<SceneObjectPtr, std::vector<ObjectInteractionPtr>> m_interactionPairMap; ///< Map of interaction pairs per colliding object
};
}
diff --git a/Source/Scene/imstkCollisionPair.cpp b/Source/Scene/imstkCollisionPair.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ae296795d07cf8366e02c4d56b8fea12adc381e9
--- /dev/null
+++ b/Source/Scene/imstkCollisionPair.cpp
@@ -0,0 +1,188 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#include "imstkCollisionPair.h"
+#include "imstkCollidingObject.h"
+#include "imstkCollisionData.h"
+#include "imstkDynamicObject.h"
+#include "imstkInteractionPair.h"
+#include "imstkLogger.h"
+#include "imstkTaskGraph.h"
+
+namespace imstk
+{
+CollisionPair::CollisionPair(std::shared_ptr<CollidingObject> objA,
+ std::shared_ptr<CollidingObject> objB) : ObjectInteractionPair(objA, objB)
+{
+}
+
+CollisionPair::CollisionPair(std::shared_ptr<CollidingObject> objA,
+ std::shared_ptr<CollidingObject> objB,
+ std::shared_ptr<CollisionDetection> cd,
+ std::shared_ptr<CollisionHandling> chA,
+ std::shared_ptr<CollisionHandling> chB) : ObjectInteractionPair(objA, objB)
+{
+ setCollisionDetection(cd);
+
+ if (chA != nullptr)
+ {
+ setCollisionHandlingA(chA);
+ }
+ if (chB != nullptr)
+ {
+ setCollisionHandlingB(chB);
+ }
+}
+
+CollisionPair::CollisionPair(std::shared_ptr<CollidingObject> objA, std::shared_ptr<CollidingObject> objB,
+ std::shared_ptr<CollisionDetection> cd,
+ std::shared_ptr<CollisionHandling> chAB) : ObjectInteractionPair(objA, objB)
+{
+ setCollisionDetection(cd);
+
+ if (chAB != nullptr)
+ {
+ setCollisionHandlingAB(chAB);
+ }
+}
+
+void
+CollisionPair::setCollisionDetection(std::shared_ptr<CollisionDetection> colDetect)
+{
+ m_colDetect = colDetect;
+ m_collisionDetectionNode = m_interactionFunction = m_colDetect->getTaskNode();
+ m_collisionDetectionNode->m_name = getObjectsPair().first->getName() + "_" + getObjectsPair().second->getName() + "_CollisionDetection";
+ m_colData = m_colDetect->getCollisionData();
+}
+
+void
+CollisionPair::setCollisionHandlingA(std::shared_ptr<CollisionHandling> colHandlingA)
+{
+ m_colHandlingA = colHandlingA;
+ m_collisionHandleANode = m_colHandlingA->getTaskNode();
+ m_collisionHandleANode->m_name = getObjectsPair().first->getName() + "_CollisionHandling";
+}
+
+void
+CollisionPair::setCollisionHandlingB(std::shared_ptr<CollisionHandling> colHandlingB)
+{
+ m_colHandlingB = colHandlingB;
+ m_collisionHandleBNode = m_colHandlingB->getTaskNode();
+ m_collisionHandleBNode->m_name = getObjectsPair().second->getName() + "_CollisionHandling";
+}
+
+void
+CollisionPair::setCollisionHandlingAB(std::shared_ptr<CollisionHandling> colHandlingAB)
+{
+ m_colHandlingA = m_colHandlingB = colHandlingAB;
+ m_collisionHandleANode = m_collisionHandleBNode = colHandlingAB->getTaskNode();
+ m_collisionHandleANode->m_name = getObjectsPair().first->getName() + '_' + getObjectsPair().second->getName() + "_CollisionHandling";
+}
+
+void
+CollisionPair::apply()
+{
+ std::shared_ptr<TaskGraph> computeGraphA = m_objects.first->getTaskGraph();
+ std::shared_ptr<TaskGraph> computeGraphB = m_objects.second->getTaskGraph();
+
+ // If nothing was added to the input/output list use default collision location
+ if ((m_taskNodeInputs.first.size() == 0) && (m_taskNodeInputs.second.size() == 0)
+ && (m_taskNodeOutputs.first.size() == 0) && (m_taskNodeOutputs.second.size() == 0))
+ {
+ m_taskNodeInputs.first.clear();
+ m_taskNodeInputs.second.clear();
+ m_taskNodeOutputs.first.clear();
+ m_taskNodeOutputs.second.clear();
+
+ // Default location is inbetween the source->updateNode
+ m_taskNodeInputs.first.push_back(computeGraphA->getSource());
+ m_taskNodeInputs.second.push_back(computeGraphB->getSource());
+
+ m_taskNodeOutputs.first.push_back(m_objects.first->getUpdateNode());
+ m_taskNodeOutputs.second.push_back(m_objects.second->getUpdateNode());
+ }
+
+ // Add all the nodes to the graph
+ computeGraphA->addNode(m_collisionDetectionNode);
+ computeGraphB->addNode(m_collisionDetectionNode);
+ if (m_collisionHandleANode != nullptr)
+ {
+ computeGraphA->addNode(m_collisionHandleANode);
+ }
+ if (m_collisionHandleBNode != nullptr)
+ {
+ computeGraphB->addNode(m_collisionHandleBNode);
+ }
+
+ // Add the edges
+ {
+ // Connect inputA's->CD
+ for (size_t i = 0; i < m_taskNodeInputs.first.size(); i++)
+ {
+ computeGraphA->addEdge(m_taskNodeInputs.first[i], m_collisionDetectionNode);
+ }
+
+ // Connect inputB's->CD
+ for (size_t i = 0; i < m_taskNodeInputs.second.size(); i++)
+ {
+ computeGraphA->addEdge(m_taskNodeInputs.second[i], m_collisionDetectionNode);
+ }
+ }
+
+ // Now connect CD to CHA/CHB/CHAB
+ // This also works for the case CHA = CHB = CHAB
+ {
+ if (m_collisionHandleANode != nullptr)
+ {
+ computeGraphA->addEdge(m_collisionDetectionNode, m_collisionHandleANode);
+ }
+ if (m_collisionHandleBNode != nullptr)
+ {
+ computeGraphB->addEdge(m_collisionDetectionNode, m_collisionHandleBNode);
+ }
+ }
+
+ // Connect either CD or CHA/CHAB to outputA's
+ for (size_t i = 0; i < m_taskNodeOutputs.first.size(); i++)
+ {
+ if (m_collisionHandleANode != nullptr)
+ {
+ computeGraphA->addEdge(m_collisionHandleANode, m_taskNodeOutputs.first[i]);
+ }
+ else
+ {
+ computeGraphA->addEdge(m_collisionDetectionNode, m_taskNodeOutputs.first[i]);
+ }
+ }
+ // Connect eitehr CD or CHB/CHAB to outputB's
+ for (size_t i = 0; i < m_taskNodeOutputs.second.size(); i++)
+ {
+ if (m_collisionHandleBNode != nullptr)
+ {
+ computeGraphB->addEdge(m_collisionHandleBNode, m_taskNodeOutputs.second[i]);
+ }
+ else
+ {
+ computeGraphB->addEdge(m_collisionDetectionNode, m_taskNodeOutputs.second[i]);
+ }
+ }
+}
+}
diff --git a/Source/Scene/imstkCollisionPair.h b/Source/Scene/imstkCollisionPair.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ed5e23782c86503e25aa0104c9e32286f27fab2
--- /dev/null
+++ b/Source/Scene/imstkCollisionPair.h
@@ -0,0 +1,89 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkCollisionDetection.h"
+#include "imstkCollisionHandling.h"
+#include "imstkObjectInteractionPair.h"
+
+namespace imstk
+{
+struct CollisionData;
+class CollidingObject;
+
+///
+/// \class CollisionPair
+///
+/// \brief CollisionPair is a specialization of InteractionPair that adds Handler functions.
+/// The handler functions precede the interaction node/step as their own computational node/step.
+/// The handler functions may be a singular handler node or two separate concurrent nodes.
+///
+class CollisionPair : public ObjectInteractionPair
+{
+public:
+ CollisionPair(std::shared_ptr<CollidingObject> objA, std::shared_ptr<CollidingObject> objB);
+
+ ///
+ /// \brief Specifies a CollisionPair with two handles (one or both can be nullptr)
+ ///
+ CollisionPair(std::shared_ptr<CollidingObject> objA, std::shared_ptr<CollidingObject> objB,
+ std::shared_ptr<CollisionDetection> cd,
+ std::shared_ptr<CollisionHandling> chA,
+ std::shared_ptr<CollisionHandling> chB);
+
+ ///
+ /// \brief Specifies CollisionPair with an AB handler
+ ///
+ CollisionPair(std::shared_ptr<CollidingObject> objA, std::shared_ptr<CollidingObject> objB,
+ std::shared_ptr<CollisionDetection> cd,
+ std::shared_ptr<CollisionHandling> chAB);
+
+ ~CollisionPair() = default;
+
+public:
+ void setCollisionDetection(std::shared_ptr<CollisionDetection> colDetect);
+ void setCollisionHandlingA(std::shared_ptr<CollisionHandling> colHandlingA);
+ void setCollisionHandlingB(std::shared_ptr<CollisionHandling> colHandlingB);
+ void setCollisionHandlingAB(std::shared_ptr<CollisionHandling> colHandlingAB);
+
+ std::shared_ptr<CollisionDetection> getCollisionDetection() const { return m_colDetect; }
+ std::shared_ptr<CollisionHandling> getCollisionHandlingA() const { return m_colHandlingA; }
+ std::shared_ptr<CollisionHandling> getCollisionHandlingB() const { return m_colHandlingB; }
+
+ std::shared_ptr<TaskNode> getCollisionDetectionNode() const { return m_collisionDetectionNode; }
+ std::shared_ptr<TaskNode> getCollisionHandlingANode() const { return m_collisionHandleANode; }
+ std::shared_ptr<TaskNode> getCollisionHandlingBNode() const { return m_collisionHandleBNode; }
+
+public:
+ virtual void apply() override;
+
+protected:
+ std::shared_ptr<CollisionDetection> m_colDetect = nullptr; ///< Collision detection algorithm
+ std::shared_ptr<CollisionData> m_colData = nullptr; ///< Common Collision Data
+ std::shared_ptr<CollisionHandling> m_colHandlingA = nullptr;
+ std::shared_ptr<CollisionHandling> m_colHandlingB = nullptr;
+
+ std::shared_ptr<TaskNode> m_collisionDetectionNode = nullptr;
+ std::shared_ptr<TaskNode> m_collisionHandleANode = nullptr;
+ std::shared_ptr<TaskNode> m_collisionHandleBNode = nullptr;
+};
+}
diff --git a/Source/Scene/imstkInteractionPair.cpp b/Source/Scene/imstkInteractionPair.cpp
deleted file mode 100644
index b82844816017604575d2bcd9395569084e136e67..0000000000000000000000000000000000000000
--- a/Source/Scene/imstkInteractionPair.cpp
+++ /dev/null
@@ -1,159 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkInteractionPair.h"
-#include "imstkCollisionData.h"
-#include "imstkCollidingObject.h"
-#include "imstkCDObjectFactory.h"
-
-#include <g3log/g3log.hpp>
-
-namespace imstk
-{
-InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
- std::shared_ptr<CollidingObject> B,
- CollisionDetection::Type CDType,
- CollisionHandling::Type CHAType,
- CollisionHandling::Type CHBType) :
- m_colData(std::make_shared<CollisionData>()),
- m_valid(false)
-{
- // Check that objects exist
- CHECK(A != nullptr && B != nullptr) << "InteractionPair error: invalid objects (nullptr).";
-
- // Check if objects are different
- /*if (A == B)
- {
- LOG(WARNING) << "InteractionPair error: object cannot interact with itself.";
- return;
- }*/
-
- // Collision Detection
- std::shared_ptr<CollisionDetection> CD = makeCollisionDetectionObject(CDType, A, B, m_colData);
-
- CHECK(CD != nullptr) << "InteractionPair error: can not instantiate collision detection algorithm.";
-
- // Collision Handling A
- std::shared_ptr<CollisionHandling> CHA;
- if (CHAType != CollisionHandling::Type::None)
- {
- CHA = CollisionHandling::make_collision_handling(CHAType, CollisionHandling::Side::A, m_colData, A, B);
-
- CHECK(CHA != nullptr) << "InteractionPair error: can not instantiate collision handling for '"
- << A->getName() << "' object.";
- }
-
- // Collision Handling B
- std::shared_ptr<CollisionHandling> CHB;
- if (CHBType != CollisionHandling::Type::None)
- {
- CHB = CollisionHandling::make_collision_handling(CHBType, CollisionHandling::Side::B, m_colData, B, A);
-
- CHECK(CHB != nullptr) << "InteractionPair error: can not instantiate collision handling for '"
- << B->getName() << "' object.";
- }
-
- // Init interactionPair
- m_objects = ObjectsPair(A, B);
- m_colDetect = CD;
- m_colHandlingA = CHA;
- m_colHandlingB = CHB;
- m_valid = true;
-}
-
-InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
- std::shared_ptr<CollidingObject> B,
- std::shared_ptr<CollisionDetection> CD,
- std::shared_ptr<CollisionHandling> CHA,
- std::shared_ptr<CollisionHandling> CHB)
-{
- m_valid = false;
-
- // Check that objects exist
- CHECK(A != nullptr && B != nullptr) << "InteractionPair error: invalid objects (nullptr).";
-
- // Check if objects are different
- /*if (A == B)
- {
- LOG(WARNING) << "InteractionPair error: object cannot interact with itself.";
- return;
- }*/
-
- m_objects = ObjectsPair(A, B);
- m_colDetect = CD;
- m_colHandlingA = CHA;
- m_colHandlingB = CHB;
- m_colData = CD->getCollisionData();
- m_valid = true;
-}
-
-void
-InteractionPair::computeCollisionData()
-{
- CHECK(m_valid) << "InteractionPair::computeCollisionData error: interaction not valid.";
- m_colDetect->computeCollisionData();
-}
-
-void
-InteractionPair::processCollisionData()
-{
- CHECK(m_valid) << "InteractionPair::computeContactForces error: interaction not valid.";
-
- if (m_colHandlingA)
- {
- m_colHandlingA->processCollisionData();
- }
- if (m_colHandlingB)
- {
- m_colHandlingB->processCollisionData();
- }
-}
-
-const bool&
-InteractionPair::isValid()
-{
- return m_valid;
-}
-
-const InteractionPair::ObjectsPair&
-InteractionPair::getObjectsPair() const
-{
- return m_objects;
-}
-
-std::shared_ptr<CollisionDetection>
-InteractionPair::getCollisionDetection() const
-{
- return m_colDetect;
-}
-
-std::shared_ptr<CollisionHandling>
-InteractionPair::getCollisionHandlingA() const
-{
- return m_colHandlingA;
-}
-
-std::shared_ptr<CollisionHandling>
-InteractionPair::getCollisionHandlingB() const
-{
- return m_colHandlingB;
-}
-}
diff --git a/Source/Scene/imstkInteractionPair.h b/Source/Scene/imstkInteractionPair.h
index 85f1d67d1c239cedd98916cf5b9a1c7347a821f5..f2b5337f1c306174a5f03b30d65648b7d74afad5 100644
--- a/Source/Scene/imstkInteractionPair.h
+++ b/Source/Scene/imstkInteractionPair.h
@@ -21,89 +21,35 @@
#pragma once
-// imstk
-#include "imstkCollisionDetection.h"
-#include "imstkCollisionHandling.h"
+#include <memory>
+#include <vector>
namespace imstk
{
-struct CollisionData;
-class CollidingObject;
+class TaskNode;
///
/// \class InteractionPair
///
-/// \brief This class implements collision interaction between two given scene objects
+/// \brief This class defines an interaction between nodes
///
class InteractionPair
{
-using ObjectsPair = std::pair<std::shared_ptr<CollidingObject>, std::shared_ptr<CollidingObject>>;
-
public:
+ using Inputs = std::pair<std::vector<std::shared_ptr<TaskNode>>, std::vector<std::shared_ptr<TaskNode>>>;
+ using Outputs = std::pair<std::vector<std::shared_ptr<TaskNode>>, std::vector<std::shared_ptr<TaskNode>>>;
- ///
- /// \brief Constructor
- ///
- InteractionPair(std::shared_ptr<CollidingObject> A,
- std::shared_ptr<CollidingObject> B,
- CollisionDetection::Type CDType,
- CollisionHandling::Type CHAType,
- CollisionHandling::Type CHBType);
-
- InteractionPair(std::shared_ptr<CollidingObject> A,
- std::shared_ptr<CollidingObject> B,
- std::shared_ptr<CollisionDetection> CD,
- std::shared_ptr<CollisionHandling> CHA,
- std::shared_ptr<CollisionHandling> CHB);
-
- ///
- /// \brief Destructor
- ///
- ~InteractionPair() = default;
-
- ///
- /// \brief Call collision detection algorithm to compute collision data
- ///
- void computeCollisionData();
-
- ///
- /// \brief Call collision handling algorithm to compute contact forces for an object
- ///
- void processCollisionData();
-
- ///
- /// \brief Call collision handling algorithm to compute contact forces for an object
- ///
- const bool& isValid();
-
- ///
- /// \brief Returns objects pair
- ///
- const ObjectsPair& getObjectsPair() const;
-
- ///
- /// \brief Returns collision detection algorithm for A-B
- ///
- std::shared_ptr<CollisionDetection> getCollisionDetection() const;
-
- ///
- /// \brief Returns collision handling algorithm for object B
- ///
- std::shared_ptr<CollisionHandling> getCollisionHandlingA() const;
+public:
+ InteractionPair() = default;
+ virtual ~InteractionPair() = default;
- ///
- /// \brief Returns collision handling algorithm for object A
- ///
- std::shared_ptr<CollisionHandling> getCollisionHandlingB() const;
+public:
+ const Inputs& getTaskNodeInputs() const { return m_taskNodeInputs; }
+ const Outputs& getTaskNodeOutputs() const { return m_taskNodeOutputs; }
protected:
-
- ObjectsPair m_objects; ///< Colliding objects
- std::shared_ptr<CollisionDetection> m_colDetect; ///< Collision detection algorithm
- std::shared_ptr<CollisionData> m_colData; ///< Common Collision Data
- std::shared_ptr<CollisionHandling> m_colHandlingA; ///< Collision handling algorithm for A
- std::shared_ptr<CollisionHandling> m_colHandlingB; ///< Collision handling algorithm for B
-
- bool m_valid;
+ Inputs m_taskNodeInputs; ///> The interacting nodes
+ Outputs m_taskNodeOutputs; ///> The interacting nodes
+ std::shared_ptr<TaskNode> m_interactionFunction; ///> Function to execute on interaction
};
}
diff --git a/Source/Scene/imstkObjectInteractionFactory.cpp b/Source/Scene/imstkObjectInteractionFactory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eaba788f7d8d03002a21f191d9e6d3d32c89133b
--- /dev/null
+++ b/Source/Scene/imstkObjectInteractionFactory.cpp
@@ -0,0 +1,115 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#include "imstkObjectInteractionFactory.h"
+#include "imstkBoneDrillingCH.h"
+#include "imstkCDObjectFactory.h"
+#include "imstkCollidingObject.h"
+#include "imstkCollisionData.h"
+#include "imstkCollisionDetection.h"
+#include "imstkDeformableObject.h"
+#include "imstkLogger.h"
+#include "imstkPbdObject.h"
+#include "imstkPbdObjectCollisionPair.h"
+#include "imstkPenaltyCH.h"
+#include "imstkPickingCH.h"
+#include "imstkSPHCollisionHandling.h"
+#include "imstkSPHObject.h"
+
+namespace imstk
+{
+// Cast type check
+template<typename ObjectType>
+static bool
+isType(std::shared_ptr<SceneObject> obj)
+{
+ return std::dynamic_pointer_cast<ObjectType>(obj) != nullptr;
+}
+
+std::shared_ptr<ObjectInteractionPair>
+makeObjectInteractionPair(std::shared_ptr<CollidingObject> obj1, std::shared_ptr<CollidingObject> obj2,
+ InteractionType intType, CollisionDetection::Type cdType)
+{
+ std::shared_ptr<ObjectInteractionPair> results = nullptr;
+ if (intType == InteractionType::PbdObjToPbdObjCollision && isType<PbdObject>(obj1) && isType<PbdObject>(obj2))
+ {
+ results = std::make_shared<PbdObjectCollisionPair>(std::dynamic_pointer_cast<PbdObject>(obj1), std::dynamic_pointer_cast<PbdObject>(obj2), cdType);
+ }
+ //else if (intType == InteractionType::PbdObjToCollidingObjCollision && isType<PbdObject>(obj1))
+ //{
+ // results = std::make_shared<PbdCollidingObjCollisionPair>(
+ // std::dynamic_pointer_cast<PbdObject>(obj1),
+ // std::dynamic_pointer_cast<PbdObject>(obj2), cdType);
+ //}
+ else if (intType == InteractionType::SphObjToCollidingObjCollision && isType<SPHObject>(obj1))
+ {
+ // Setup CD, and collision data
+ std::shared_ptr<CollisionData> colData = std::make_shared<CollisionData>();
+ std::shared_ptr<CollisionDetection> colDetect = makeCollisionDetectionObject(cdType, obj1->getCollidingGeometry(), obj2->getCollidingGeometry(), colData);
+
+ // Setup the handler
+ std::shared_ptr<SPHCollisionHandling> colHandler =
+ std::make_shared<SPHCollisionHandling>(CollisionHandling::Side::A, colData, std::dynamic_pointer_cast<SPHObject>(obj1));
+
+ results = std::make_shared<CollisionPair>(obj1, obj2, colDetect, colHandler, nullptr);
+ }
+ else if (intType == InteractionType::FemObjToCollidingObjNodalPicking && isType<FeDeformableObject>(obj1))
+ {
+ // Setup CD, and collision data
+ std::shared_ptr<CollisionData> colData = std::make_shared<CollisionData>();
+ std::shared_ptr<CollisionDetection> colDetect = makeCollisionDetectionObject(cdType, obj1->getCollidingGeometry(), obj2->getCollidingGeometry(), colData);
+
+ // Setup the handler
+ std::shared_ptr<PickingCH> colHandler =
+ std::make_shared<PickingCH>(CollisionHandling::Side::A, colData, std::dynamic_pointer_cast<FeDeformableObject>(obj1));
+
+ results = std::make_shared<CollisionPair>(obj1, obj2, colDetect, colHandler, nullptr);
+ }
+ else if (intType == InteractionType::FemObjToCollidingObjPenaltyForce && isType<FeDeformableObject>(obj1))
+ {
+ // Setup CD, and collision data
+ std::shared_ptr<CollisionData> colData = std::make_shared<CollisionData>();
+ std::shared_ptr<CollisionDetection> colDetect = makeCollisionDetectionObject(cdType, obj1->getCollidingGeometry(), obj2->getCollidingGeometry(), colData);
+
+ // Setup the handler
+ std::shared_ptr<PenaltyCH> colHandler =
+ std::make_shared<PenaltyCH>(CollisionHandling::Side::A, colData, obj1);
+ }
+ else if (intType == InteractionType::FemObjToCollidingObjBoneDrilling && isType<FeDeformableObject>(obj1))
+ {
+ // Setup CD, and collision data
+ std::shared_ptr<CollisionData> colData = std::make_shared<CollisionData>();
+ std::shared_ptr<CollisionDetection> colDetect = makeCollisionDetectionObject(cdType, obj1->getCollidingGeometry(), obj2->getCollidingGeometry(), colData);
+
+ // Setup the handler
+ std::shared_ptr<BoneDrillingCH> colHandler =
+ std::make_shared<BoneDrillingCH>(CollisionHandling::Side::A, colData, obj1, obj2);
+
+ results = std::make_shared<CollisionPair>(obj1, obj2, colDetect, colHandler, nullptr);
+ }
+
+ if (results == nullptr)
+ {
+ LOG(WARNING) << "Invalid SceneObjects for requested interaction.";
+ }
+ return results;
+}
+}
\ No newline at end of file
diff --git a/Source/Scene/imstkObjectInteractionFactory.h b/Source/Scene/imstkObjectInteractionFactory.h
new file mode 100644
index 0000000000000000000000000000000000000000..4f861d8090b638259a29e66623ffbd695e60d1e1
--- /dev/null
+++ b/Source/Scene/imstkObjectInteractionFactory.h
@@ -0,0 +1,53 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkCollisionDetection.h"
+#include "imstkObjectInteractionPair.h"
+
+namespace imstk
+{
+class CollidingObject;
+
+// Predefined standard/types of interaction from imstk
+enum class InteractionType
+{
+ PbdObjToPbdObjCollision,
+
+ PbdObjToCollidingObjCollision,
+ SphObjToCollidingObjCollision,
+ FemObjToCollidingObjCollision,
+ //RigidObjToCollidingObjCollision,
+
+ //RigidObjToRigidObjCollision,
+
+ FemObjToCollidingObjPenaltyForce,
+ FemObjToCollidingObjBoneDrilling,
+ FemObjToCollidingObjNodalPicking
+};
+
+///
+/// \brief Factory for InteractionPairs, returns nullptr and logs warning if failed
+///
+extern std::shared_ptr<ObjectInteractionPair> makeObjectInteractionPair(std::shared_ptr<CollidingObject> obj1, std::shared_ptr<CollidingObject> obj2,
+ InteractionType intType, CollisionDetection::Type cdType);
+}
\ No newline at end of file
diff --git a/Source/Scene/imstkObjectInteractionPair.cpp b/Source/Scene/imstkObjectInteractionPair.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..df5c701441e539b0139f9efcb52c5324b5e9a04a
--- /dev/null
+++ b/Source/Scene/imstkObjectInteractionPair.cpp
@@ -0,0 +1,35 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#include "imstkObjectInteractionPair.h"
+#include "imstkLogger.h"
+
+namespace imstk
+{
+ObjectInteractionPair::ObjectInteractionPair(std::shared_ptr<SceneObject> objA, std::shared_ptr<SceneObject> objB)
+{
+ // Check that objects exist
+ CHECK(objA != nullptr && objB != nullptr) << "InteractionPair error: invalid objects (nullptr).";
+
+ this->m_objects.first = objA;
+ this->m_objects.second = objB;
+}
+}
\ No newline at end of file
diff --git a/Source/Scene/imstkObjectInteractionPair.h b/Source/Scene/imstkObjectInteractionPair.h
new file mode 100644
index 0000000000000000000000000000000000000000..dc7e4d1a477ad70b396cf25839e9b29a0d9b748b
--- /dev/null
+++ b/Source/Scene/imstkObjectInteractionPair.h
@@ -0,0 +1,60 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkInteractionPair.h"
+
+namespace imstk
+{
+class SceneObject;
+
+///
+/// \class ObjectInteractionPair
+///
+/// \brief This class defines an interaction between two SceneObjects
+/// An interaction is a function occuring between two SceneObjects at some point
+///
+class ObjectInteractionPair : public InteractionPair
+{
+public:
+ using SceneObjectPair = std::pair<std::shared_ptr<SceneObject>, std::shared_ptr<SceneObject>>;
+
+// Cannot be constructed as synch points are externally undefinable
+protected:
+ ObjectInteractionPair() = default;
+ ObjectInteractionPair(std::shared_ptr<SceneObject> objA, std::shared_ptr<SceneObject> objB);
+
+ virtual ~ObjectInteractionPair() override = default;
+
+public:
+ const SceneObjectPair& getObjectsPair() const { return m_objects; }
+
+public:
+ ///
+ /// \brief Modifies the graph of the provide objects to apply the interaction
+ ///
+ virtual void apply() = 0;
+
+protected:
+ SceneObjectPair m_objects; ///> The two objects interacting
+};
+}
\ No newline at end of file
diff --git a/Source/Scene/imstkPbdInteractionPair.cpp b/Source/Scene/imstkPbdInteractionPair.cpp
deleted file mode 100644
index ddfa7243d7fb0dc25e1df9273c9ae4756d1981b5..0000000000000000000000000000000000000000
--- a/Source/Scene/imstkPbdInteractionPair.cpp
+++ /dev/null
@@ -1,444 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#include "imstkPbdInteractionPair.h"
-#include "imstkSurfaceMesh.h"
-#include "imstkLineMesh.h"
-#include "imstkGeometryMap.h"
-#include "imstkCollisionUtils.h"
-#include "imstkPbdModel.h"
-#include "imstkPbdObject.h"
-//#include "imstkColor.h"
-#include "imstkPbdEdgeEdgeCollisionConstraint.h"
-#include "imstkPbdPointTriCollisionConstraint.h"
-
-namespace imstk
-{
-PbdInteractionPair::PbdInteractionPair(const std::shared_ptr<PbdObject>& A,
- const std::shared_ptr<PbdObject>& B) :
- m_firstObj(A), m_secondObj(B)
-{
-}
-
-bool
-PbdInteractionPair::doBroadPhaseCollision()
-{
- Vec3d min1, max1;
- m_firstObj->getCollidingGeometry()->computeBoundingBox(min1, max1);
-
- Vec3d min2, max2;
- m_secondObj->getCollidingGeometry()->computeBoundingBox(min2, max2);
-
- auto prox1 = m_firstObjProximity;
- auto prox2 = m_secondObjProximity;
-
- return CollisionUtils::testAABBToAABB(min1[0] - prox1, max1[0] + prox1, min1[1] - prox1, max1[1] + prox1,
- min1[2] - prox1, max1[2] + prox1, min2[0] - prox2, max2[0] + prox2,
- min2[1] - prox2, max2[1] + prox2, min2[2] - prox2, max2[2] + prox2);
-}
-
-void
-PbdInteractionPair::doNarrowPhaseCollision()
-{
- auto g1 = m_firstObj->getCollidingGeometry();
- auto g2 = m_secondObj->getCollidingGeometry();
-
- auto map1 = m_firstObj->getPhysicsToCollidingMap();
- auto map2 = m_secondObj->getPhysicsToCollidingMap();
-
- auto dynaModel1 = std::static_pointer_cast<PbdModel>(m_firstObj->getDynamicalModel());
- auto dynaModel2 = std::static_pointer_cast<PbdModel>(m_secondObj->getDynamicalModel());
-
- auto prox1 = dynaModel1->getParameters()->m_proximity;
- auto prox2 = dynaModel2->getParameters()->m_proximity;
-
- auto mesh2 = std::static_pointer_cast<SurfaceMesh>(g2);
-
- if (g1->getType() == Geometry::Type::LineMesh)
- {
- auto mesh1 = std::static_pointer_cast<LineMesh>(g1);
-
- // brute force, use BVH or spatial grid would be much better
- // point
- for (size_t i = 0; i < mesh1->getNumVertices(); ++i)
- {
- Vec3d p = mesh1->getVertexPosition(i);
- std::vector<SurfaceMesh::TriangleArray> elements = mesh2->getTrianglesVertices();
-
- for (size_t j = 0; j < elements.size(); ++j)
- {
- SurfaceMesh::TriangleArray& e = elements[j];
- const Vec3d p0 = mesh2->getVertexPosition(e[0]);
- const Vec3d p1 = mesh2->getVertexPosition(e[1]);
- const Vec3d p2 = mesh2->getVertexPosition(e[2]);
-
- if (CollisionUtils::testPointToTriAABB(p[0], p[1], p[2],
- p0[0], p0[1], p0[2],
- p1[0], p1[1], p1[2],
- p2[0], p2[1], p2[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdPointTriangleConstraint>();
- c->initConstraint(dynaModel1, i,
- dynaModel2, map2->getMapIdx(e[0]), map2->getMapIdx(e[1]), map2->getMapIdx(e[2]));
- m_collisionConstraints.push_back(c);
- }
- }
- }
-
- const auto nL1 = mesh1->getNumLines();
- const auto nV2 = mesh2->getNumVertices();
- std::vector<std::vector<bool>> E2(nV2, std::vector<bool>(nV2, 1));
- std::vector<SurfaceMesh::TriangleArray> elements2 = mesh2->getTrianglesVertices();
-
- for (int k = 0; k < nL1; ++k)
- {
- auto nodes = mesh1->getLinesVertices()[k];
- size_t i1 = nodes[0];
- size_t i2 = nodes[1];
-
- const Vec3d P = mesh1->getVertexPosition(i1);
- const Vec3d Q = mesh1->getVertexPosition(i2);
-
- for (size_t j = 0; j < elements2.size(); ++j)
- {
- SurfaceMesh::TriangleArray& e = elements2[j];
- const Vec3d p0 = mesh2->getVertexPosition(e[0]);
- const Vec3d p1 = mesh2->getVertexPosition(e[1]);
- const Vec3d p2 = mesh2->getVertexPosition(e[2]);
-
- if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p0[0], p0[1], p0[2],
- p1[0], p1[1], p1[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[0]), map1->getMapIdx(e[1]));
- m_collisionConstraints.push_back(c);
- }
- E2[e[0]][e[1]] = 0;
- }
- if (E2[e[1]][e[2]] && E2[e[2]][e[1]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p1[0], p1[1], p1[2],
- p2[0], p2[1], p2[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[1]), map1->getMapIdx(e[2]));
- m_collisionConstraints.push_back(c);
- }
- E2[e[1]][e[2]] = 0;
- }
- if (E2[e[2]][e[0]] && E2[e[0]][e[2]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p2[0], p2[1], p2[2],
- p0[0], p0[1], p0[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[2]), map1->getMapIdx(e[0]));
- m_collisionConstraints.push_back(c);
- }
- E2[e[2]][e[0]] = 0;
- }
- }
- }
- }
- else if (g1->getType() == Geometry::Type::PointSet)
- {
- auto mesh1 = std::static_pointer_cast<PointSet>(g1);
-
- // brute force, use BVH or spatial grid would be much better
- // point
- for (size_t i = 0; i < mesh1->getNumVertices(); ++i)
- {
- const auto p = mesh1->getVertexPosition(i);
- auto elements = mesh2->getTrianglesVertices();
-
- for (size_t j = 0; j < elements.size(); ++j)
- {
- auto& e = elements[j];
- const Vec3d p0 = mesh2->getVertexPosition(e[0]);
- const Vec3d p1 = mesh2->getVertexPosition(e[1]);
- const Vec3d p2 = mesh2->getVertexPosition(e[2]);
-
- if (CollisionUtils::testPointToTriAABB(p[0], p[1], p[2],
- p0[0], p0[1], p0[2],
- p1[0], p1[1], p1[2],
- p2[0], p2[1], p2[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdPointTriangleConstraint>();
- auto mappedIndex1 = (map1) ? map1->getMapIdx(i) : i;
-
- c->initConstraint(dynaModel1, mappedIndex1, dynaModel2, map2->getMapIdx(e[0]), map2->getMapIdx(e[1]), map2->getMapIdx(e[2]));
- m_collisionConstraints.push_back(c);
- }
- }
- }
- }
- else
- {
- auto mesh1 = std::static_pointer_cast<SurfaceMesh>(g1);
-
- // brute force, use BVH or spatial grid would be much better
- // point
- for (size_t i = 0; i < mesh1->getNumVertices(); ++i)
- {
- const Vec3d p = mesh1->getVertexPosition(i);
- auto elements = mesh2->getTrianglesVertices();
-
- for (size_t j = 0; j < elements.size(); ++j)
- {
- SurfaceMesh::TriangleArray& e = elements[j];
- const Vec3d p0 = mesh2->getVertexPosition(e[0]);
- const Vec3d p1 = mesh2->getVertexPosition(e[1]);
- const Vec3d p2 = mesh2->getVertexPosition(e[2]);
-
- if (CollisionUtils::testPointToTriAABB(p[0], p[1], p[2],
- p0[0], p0[1], p0[2],
- p1[0], p1[1], p1[2],
- p2[0], p2[1], p2[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdPointTriangleConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i),
- dynaModel2, map2->getMapIdx(e[0]), map2->getMapIdx(e[1]), map2->getMapIdx(e[2]));
- m_collisionConstraints.push_back(c);
- }
- }
- }
-
- // edge
- // since we don't have edge structure, the following is not good
- const auto nV = mesh1->getNumVertices();
- std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
-
- const auto nV2 = mesh2->getNumVertices();
- std::vector<std::vector<bool>> E2(nV2, std::vector<bool>(nV2, 1));
-
- std::vector<SurfaceMesh::TriangleArray> elements = mesh1->getTrianglesVertices();
- for (size_t k = 0; k < elements.size(); ++k)
- {
- SurfaceMesh::TriangleArray& tri = elements[k];
- size_t i1 = tri[0];
- size_t i2 = tri[1];
-
- if (E[i1][i2] && E[i2][i1])
- {
- const Vec3d P = mesh1->getVertexPosition(i1);
- const Vec3d Q = mesh1->getVertexPosition(i2);
- std::vector<SurfaceMesh::TriangleArray> elements2 = mesh2->getTrianglesVertices();
- for (size_t j = 0; j < elements2.size(); ++j)
- {
- SurfaceMesh::TriangleArray& e = elements2[j];
- const Vec3d p0 = mesh2->getVertexPosition(e[0]);
- const Vec3d p1 = mesh2->getVertexPosition(e[1]);
- const Vec3d p2 = mesh2->getVertexPosition(e[2]);
- if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p0[0], p0[1], p0[2],
- p1[0], p1[1], p1[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[0]), map1->getMapIdx(e[1]));
- m_collisionConstraints.push_back(c);
- E2[e[0]][e[1]] = 0;
- }
- }
- if (E2[e[1]][e[2]] && E2[e[2]][e[1]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p1[0], p1[1], p1[2],
- p2[0], p2[1], p2[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[1]), map1->getMapIdx(e[2]));
- m_collisionConstraints.push_back(c);
- E2[e[1]][e[2]] = 0;
- }
- }
- if (E2[e[2]][e[0]] && E2[e[0]][e[2]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p2[0], p2[1], p2[2],
- p0[0], p0[1], p0[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[2]), map1->getMapIdx(e[0]));
- m_collisionConstraints.push_back(c);
- E2[e[2]][e[0]] = 0;
- }
- }
- }
- E[i1][i2] = 0;
- }
-
- i1 = tri[1];
- i2 = tri[2];
- if (E[i1][i2] && E[i2][i1])
- {
- const Vec3d P = mesh1->getVertexPosition(i1);
- const Vec3d Q = mesh1->getVertexPosition(i2);
- std::vector<SurfaceMesh::TriangleArray> elements2 = mesh2->getTrianglesVertices();
-
- for (size_t j = 0; j < elements2.size(); ++j)
- {
- SurfaceMesh::TriangleArray& e = elements2[j];
- const Vec3d p0 = mesh2->getVertexPosition(e[0]);
- const Vec3d p1 = mesh2->getVertexPosition(e[1]);
- const Vec3d p2 = mesh2->getVertexPosition(e[2]);
- if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p0[0], p0[1], p0[2],
- p1[0], p1[1], p1[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[0]), map1->getMapIdx(e[1]));
- m_collisionConstraints.push_back(c);
- E2[e[0]][e[1]] = 0;
- }
- }
- if (E2[e[1]][e[2]] && E2[e[2]][e[1]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p1[0], p1[1], p1[2],
- p2[0], p2[1], p2[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[1]), map1->getMapIdx(e[2]));
- m_collisionConstraints.push_back(c);
- E2[e[1]][e[2]] = 0;
- }
- }
- if (E2[e[2]][e[0]] && E2[e[0]][e[2]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p2[0], p2[1], p2[2],
- p0[0], p0[1], p0[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[2]), map1->getMapIdx(e[0]));
- m_collisionConstraints.push_back(c);
- E2[e[2]][e[0]] = 0;
- }
- }
- }
- E[i1][i2] = 0;
- }
-
- i1 = tri[2];
- i2 = tri[0];
- if (E[i1][i2] && E[i2][i1])
- {
- const Vec3d P = mesh1->getVertexPosition(i1);
- const Vec3d Q = mesh1->getVertexPosition(i2);
- std::vector<SurfaceMesh::TriangleArray> elements2 = mesh2->getTrianglesVertices();
- for (size_t j = 0; j < elements2.size(); ++j)
- {
- SurfaceMesh::TriangleArray& e = elements2[j];
- const Vec3d p0 = mesh2->getVertexPosition(e[0]);
- const Vec3d p1 = mesh2->getVertexPosition(e[1]);
- const Vec3d p2 = mesh2->getVertexPosition(e[2]);
- if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p0[0], p0[1], p0[2],
- p1[0], p1[1], p1[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[0]), map1->getMapIdx(e[1]));
- m_collisionConstraints.push_back(c);
- E2[e[0]][e[1]] = 0;
- }
- }
- if (E2[e[1]][e[2]] && E2[e[2]][e[1]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p1[0], p1[1], p1[2],
- p2[0], p2[1], p2[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[1]), map1->getMapIdx(e[2]));
- m_collisionConstraints.push_back(c);
- E2[e[1]][e[2]] = 0;
- }
- }
- if (E2[e[2]][e[0]] && E2[e[0]][e[2]])
- {
- if (CollisionUtils::testLineToLineAABB(P[0], P[1], P[2],
- Q[0], Q[1], Q[2],
- p2[0], p2[1], p2[2],
- p0[0], p0[1], p0[2], prox1, prox2))
- {
- auto c = std::make_shared<PbdEdgeEdgeConstraint>();
- c->initConstraint(dynaModel1, map1->getMapIdx(i1), map1->getMapIdx(i2),
- dynaModel2, map2->getMapIdx(e[2]), map1->getMapIdx(e[0]));
- m_collisionConstraints.push_back(c);
- E2[e[2]][e[0]] = 0;
- }
- }
- }
- E[i1][i2] = 0;
- }
- }
- }
-}
-
-void
-PbdInteractionPair::resolveCollision()
-{
- if (!m_collisionConstraints.empty())
- {
- unsigned int i = 0;
- while (++i < maxIter)
- {
- for (size_t k = 0; k < m_collisionConstraints.size(); ++k)
- {
- m_collisionConstraints[k]->solvePositionConstraint();
- }
- }
- }
-}
-} // imstk
diff --git a/Source/Scene/imstkPbdInteractionPair.h b/Source/Scene/imstkPbdInteractionPair.h
deleted file mode 100644
index 1aaa93553c4ef0f69830a9d361f99322edfa4170..0000000000000000000000000000000000000000
--- a/Source/Scene/imstkPbdInteractionPair.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#pragma once
-
-#include "imstkInteractionPair.h"
-
-namespace imstk
-{
-class PbdObject;
-class PbdCollisionConstraint;
-
-///
-/// \class PbdInteractionPair
-///
-/// \brief
-///
-class PbdInteractionPair
-{
-public:
- ///
- /// \brief Constructor
- ///
- PbdInteractionPair(const std::shared_ptr<PbdObject>& A, const std::shared_ptr<PbdObject>& B);
-
- ///
- /// \brief Clear the collisions from previous step
- ///
- inline void resetConstraints()
- {
- m_collisionConstraints.clear();
- }
-
- ///
- /// \brief
- ///
- inline void setNumberOfInterations(const unsigned int& n)
- {
- maxIter = n;
- }
-
- ///
- /// \brief Broad phase collision detection using AABB
- ///
- bool doBroadPhaseCollision();
-
- ///
- /// \brief Narrow phase collision detection
- ///
- void doNarrowPhaseCollision();
-
- ///
- /// \brief Resolves the collision by solving pbd collision constraints
- ///
- void resolveCollision();
-
-private:
- std::vector<std::shared_ptr<PbdCollisionConstraint>> m_collisionConstraints;
-
- std::shared_ptr<PbdObject> m_firstObj;
- std::shared_ptr<PbdObject> m_secondObj;
-
- double m_firstObjProximity = MAX_D;
- double m_secondObjProximity = MAX_D;
- unsigned int maxIter = 50;
-};
-}
diff --git a/Source/Scene/imstkPbdObjectCollisionPair.cpp b/Source/Scene/imstkPbdObjectCollisionPair.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..80d51aa78416f917ccde3caa4f111e876c204fcf
--- /dev/null
+++ b/Source/Scene/imstkPbdObjectCollisionPair.cpp
@@ -0,0 +1,96 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#include "imstkPbdObjectCollisionPair.h"
+#include "imstkCDObjectFactory.h"
+#include "imstkCollisionData.h"
+#include "imstkPBDCollisionHandling.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+#include "imstkPbdSolver.h"
+#include "imstkTaskGraph.h"
+
+namespace imstk
+{
+// Pbd Collision will be tested before any step of pbd, then resolved after the solve steps of the two objects
+PbdObjectCollisionPair::PbdObjectCollisionPair(std::shared_ptr<PbdObject> obj1, std::shared_ptr<PbdObject> obj2,
+ CollisionDetection::Type cdType) : CollisionPair(obj1, obj2)
+{
+ std::shared_ptr<PbdModel> pbdModel1 = obj1->getPbdModel();
+ std::shared_ptr<PbdModel> pbdModel2 = obj2->getPbdModel();
+
+ // Define where collision interaction happens
+ m_taskNodeInputs.first.push_back(pbdModel1->getUpdateCollisionGeometryNode());
+ m_taskNodeInputs.second.push_back(pbdModel2->getUpdateCollisionGeometryNode());
+
+ m_taskNodeOutputs.first.push_back(pbdModel1->getSolveNode());
+ m_taskNodeOutputs.second.push_back(pbdModel2->getSolveNode());
+
+ // Define where solver interaction happens
+ m_solveNodeInputs.first.push_back(pbdModel1->getSolveNode());
+ m_solveNodeInputs.second.push_back(pbdModel2->getSolveNode());
+
+ m_solveNodeOutputs.first.push_back(pbdModel1->getUpdateVelocityNode());
+ m_solveNodeOutputs.second.push_back(pbdModel2->getUpdateVelocityNode());
+
+ // Setup the CD
+ m_colData = std::make_shared<CollisionData>();
+ setCollisionDetection(makeCollisionDetectionObject(cdType, obj1->getCollidingGeometry(), obj2->getCollidingGeometry(), m_colData));
+
+ // Setup the handler
+ std::shared_ptr<PBDCollisionHandling> ch = std::make_shared<PBDCollisionHandling>(CollisionHandling::Side::AB, m_colData, obj1, obj2);
+ setCollisionHandlingAB(ch);
+
+ // Setup compute node for collision solver (true/critical node)
+ m_collisionSolveNode = std::make_shared<TaskNode>([ch]() { ch->getCollisionSolver()->solve(); },
+ obj1->getName() + "_vs_" + obj2->getName() + "_CollisionSolver", true);
+}
+
+void
+PbdObjectCollisionPair::apply()
+{
+ // Add the collision interaction
+ CollisionPair::apply();
+
+ // Add a secondary interaction
+ m_objects.first->getTaskGraph()->addNode(m_collisionSolveNode);
+ m_objects.second->getTaskGraph()->addNode(m_collisionSolveNode);
+
+ // Add the solver interaction
+ for (size_t i = 0; i < m_solveNodeInputs.first.size(); i++)
+ {
+ m_objects.first->getTaskGraph()->addEdge(m_solveNodeInputs.first[i], m_collisionSolveNode);
+ }
+ for (size_t i = 0; i < m_solveNodeInputs.second.size(); i++)
+ {
+ m_objects.second->getTaskGraph()->addEdge(m_solveNodeInputs.second[i], m_collisionSolveNode);
+ }
+
+ for (size_t i = 0; i < m_solveNodeOutputs.first.size(); i++)
+ {
+ m_objects.first->getTaskGraph()->addEdge(m_collisionSolveNode, m_solveNodeOutputs.first[i]);
+ }
+ for (size_t i = 0; i < m_solveNodeOutputs.second.size(); i++)
+ {
+ m_objects.second->getTaskGraph()->addEdge(m_collisionSolveNode, m_solveNodeOutputs.second[i]);
+ }
+}
+}
\ No newline at end of file
diff --git a/Source/Scene/imstkPbdObjectCollisionPair.h b/Source/Scene/imstkPbdObjectCollisionPair.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb476aeef6de4a26cfbd49af7a9b9378e649eff5
--- /dev/null
+++ b/Source/Scene/imstkPbdObjectCollisionPair.h
@@ -0,0 +1,50 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& 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.txt
+
+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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkCollisionPair.h"
+
+namespace imstk
+{
+class PbdObject;
+class PbdSolver;
+
+///
+/// \class PbdObjectCollisionPair
+///
+/// \brief This class defines a collision interaction between two PbdObjects
+///
+class PbdObjectCollisionPair : public CollisionPair
+{
+public:
+ PbdObjectCollisionPair(std::shared_ptr<PbdObject> obj1, std::shared_ptr<PbdObject> obj2,
+ CollisionDetection::Type cdType = CollisionDetection::Type::MeshToMeshBruteForce);
+
+ void apply() override;
+
+private:
+ // Pbd defines two interactions (one at CD and one at solver)
+ Inputs m_solveNodeInputs;
+ Outputs m_solveNodeOutputs;
+ std::shared_ptr<TaskNode> m_collisionSolveNode = nullptr;
+};
+}
\ No newline at end of file
diff --git a/Source/Scene/imstkScene.cpp b/Source/Scene/imstkScene.cpp
index 4409293f4133b3f07c2a55c074c8771ff5209b0e..dc6ee53090191c9257b1f81ad56cd1c14ecc3396 100644
--- a/Source/Scene/imstkScene.cpp
+++ b/Source/Scene/imstkScene.cpp
@@ -20,29 +20,40 @@
=========================================================================*/
#include "imstkScene.h"
-#include "imstkSceneObject.h"
-#include "imstkVisualModel.h"
-#include "imstkCameraController.h"
-#include "imstkSceneObjectControllerBase.h"
+#include "imstkCamera.h"
#include "imstkCameraController.h"
+#include "imstkCollidingObject.h"
+#include "imstkCollisionGraph.h"
+#include "imstkCollisionPair.h"
#include "imstkDebugRenderGeometry.h"
#include "imstkDeformableObject.h"
-#include "imstkTimer.h"
-#include "imstkRigidBodyWorld.h"
-#include "imstkPbdSolver.h"
-#include "imstkPbdObject.h"
-#include "imstkPBDCollisionHandling.h"
-#include "imstkSolverBase.h"
-#include "imstkCamera.h"
-#include "imstkIBLProbe.h"
-#include "imstkCollisionGraph.h"
+#include "imstkDynamicObject.h"
+#include "imstkFEMDeformableBodyModel.h"
+#include "imstkInteractionPair.h"
#include "imstkLight.h"
-#include "imstkLogger.h"
-
-#include <iterator>
+#include "imstkParallelUtils.h"
+#include "imstkPbdObject.h"
+#include "imstkRigidBodyWorld.h"
+#include "imstkSceneObject.h"
+#include "imstkSceneObjectControllerBase.h"
+#include "imstkSequentialTaskGraphController.h"
+#include "imstkTaskGraph.h"
+#include "imstkTaskGraphVizWriter.h"
+#include "imstkTbbTaskGraphController.h"
+#include "imstkTimer.h"
namespace imstk
{
+Scene::Scene(const std::string& name, std::shared_ptr<SceneConfig> config) :
+ m_name(name),
+ m_camera(std::make_shared<Camera>()),
+ m_collisionGraph(std::make_shared<CollisionGraph>()),
+ m_config(config),
+ m_taskGraph(std::make_shared<TaskGraph>("Scene_" + name + "_Source", "Scene_" + name + "_Sink")),
+ computeTimesLock(std::make_shared<ParallelUtils::SpinLock>())
+{
+}
+
Scene::~Scene()
{
// Join Camera Controllers
@@ -56,54 +67,133 @@ Scene::~Scene()
bool
Scene::initialize()
{
- std::unordered_map<std::shared_ptr<PbdObject>, std::shared_ptr<PbdSolver>> pbdObjSolver;
+ // Initialize all the SceneObjects
+ for (auto const& it : m_sceneObjectsMap)
+ {
+ auto sceneObject = it.second;
+ CHECK(sceneObject->initialize()) << "Error initializing scene object: " << sceneObject->getName();
+ }
+ // Build the compute graph
+ buildTaskGraph();
+
+ // Opportunity for external configuration
+ if (m_postTaskGraphConfigureCallback != nullptr)
+ {
+ m_postTaskGraphConfigureCallback(this);
+ }
+
+ // Then init
+ initTaskGraph();
+
+ m_isInitialized = true;
+ LOG(INFO) << "Scene '" << this->getName() << "' initialized!";
+ return true;
+}
+
+void
+Scene::buildTaskGraph()
+{
+ // Configures the edges/flow of the graph
+
+ // Clear the compute graph of all nodes/edges except source+sink
+ m_taskGraph->clear();
+
+ // Setup all SceneObject compute graphs (and segment the rigid bodies)
+ std::list<std::shared_ptr<SceneObject>> rigidBodies;
for (auto const& it : m_sceneObjectsMap)
{
- // Add pbd solver for all the pbd scene objects
- if (it.second->getType() == SceneObject::Type::Pbd)
+ auto sceneObject = it.second;
+ if (sceneObject->getType() == SceneObject::Type::Rigid)
{
- auto pbdSolver = std::make_shared<PbdSolver>();
- auto pbdObj = std::dynamic_pointer_cast<PbdObject>(it.second);
- pbdSolver->setPbdObject(pbdObj);
- this->addNonlinearSolver(pbdSolver);
-
- pbdObjSolver[pbdObj] = pbdSolver;
+ rigidBodies.push_back(sceneObject);
}
- auto sceneObject = it.second;
+ sceneObject->initGraphEdges();
+ }
- CHECK(sceneObject->initialize()) << "Error initializing scene object: " << sceneObject->getName();
+ // Apply all the interaction graph element operations to the SceneObject graphs
+ const std::vector<std::shared_ptr<ObjectInteractionPair>>& pairs = m_collisionGraph->getInteractionPairs();
+ for (size_t i = 0; i < pairs.size(); i++)
+ {
+ pairs[i]->apply();
}
- for (auto const& it : m_collisionGraph->getInteractionPairList())
+ // Nest all the SceneObject graphs within this Scene's ComputeGraph
+ for (auto const& it : m_sceneObjectsMap)
{
- auto chA = it->getCollisionHandlingA();
- if (chA)
+ std::shared_ptr<TaskGraph> objComputeGraph = it.second->getTaskGraph();
+ if (objComputeGraph != nullptr)
{
- if (chA->getType() == CollisionHandling::Type::PBD)
- {
- auto ch = std::dynamic_pointer_cast<PBDCollisionHandling>(chA);
- ch->setSolver(pbdObjSolver[std::dynamic_pointer_cast<PbdObject>(it->getObjectsPair().first)]);
- }
+ // Add edges between any nodes that are marked critical and running simulatenously
+ objComputeGraph = TaskGraph::resolveCriticalNodes(objComputeGraph);
+ // Sum and nest the graph
+ m_taskGraph->nestGraph(objComputeGraph, m_taskGraph->getSource(), m_taskGraph->getSink());
}
- else
- {
- auto chB = it->getCollisionHandlingB();
- if (chB)
+ }
+
+ // Edge Case: Rigid bodies all have a singular update point because of how PhysX works
+ // Think about generalizes these islands of interaction to Systems
+ if (rigidBodies.size() > 0)
+ {
+ // The node that updates the rigid body system
+ auto physXUpdate = m_taskGraph->addFunction("PhysXUpdate", [&]()
{
- if (chB->getType() == CollisionHandling::Type::PBD)
- {
- auto ch = std::dynamic_pointer_cast<PBDCollisionHandling>(chB);
- ch->setSolver(pbdObjSolver[std::dynamic_pointer_cast<PbdObject>(it->getObjectsPair().second)]);
- }
- }
+ auto physxScene = RigidBodyWorld::getInstance()->m_Scene;
+ // TODO: update the time step, split into two steps, collide and advance
+ physxScene->simulate(RigidBodyWorld::getInstance()->getTimeStep());
+ physxScene->fetchResults(true);
+ });
+
+ // Scene Source->physX Update->Rigid Body Update Geometry[i]
+ m_taskGraph->addEdge(m_taskGraph->getSource(), physXUpdate);
+ m_taskGraph->addEdge(physXUpdate, m_taskGraph->getSink());
+ for (std::list<std::shared_ptr<SceneObject>>::iterator i = rigidBodies.begin(); i != rigidBodies.end(); i++)
+ {
+ m_taskGraph->addEdge(physXUpdate, (*i)->getUpdateGeometryNode());
}
}
+}
- m_isInitialized = true;
- LOG(INFO) << "Scene '" << this->getName() << "' initialized!";
- return true;
+void
+Scene::initTaskGraph()
+{
+ // Pick a controller for the graph execution
+ if (m_config->taskParallelizationEnabled)
+ {
+ m_taskGraphController = std::make_shared<TbbTaskGraphController>();
+ }
+ else
+ {
+ m_taskGraphController = std::make_shared<SequentialTaskGraphController>();
+ }
+ m_taskGraphController->setTaskGraph(m_taskGraph);
+
+ // Reduce the graph, removing nonfunctional nodes, and redundant edges
+ if (m_config->graphReductionEnabled)
+ {
+ m_taskGraph = TaskGraph::reduce(m_taskGraph);
+ }
+
+ // If user wants to benchmark, tell all the nodes to time themselves
+ for (std::shared_ptr<TaskNode> node : m_taskGraph->getNodes())
+ {
+ node->m_enableTiming = m_config->taskTimingEnabled;
+ }
+
+ // Generate unique names among the nodes
+ TaskGraph::getUniqueNodeNames(m_taskGraph, true);
+ m_nodeComputeTimes.clear();
+
+ if (m_config->writeTaskGraph)
+ {
+ TaskGraphVizWriter writer;
+ writer.setInput(m_taskGraph);
+ writer.setFileName("sceneTaskGraph.svg");
+ writer.write();
+ }
+
+ m_taskGraphController->initialize();
}
void
@@ -304,18 +394,6 @@ Scene::getCollisionGraph() const
return m_collisionGraph;
}
-const std::vector<std::shared_ptr<SolverBase>>
-Scene::getSolvers()
-{
- return m_solvers;
-}
-
-void
-Scene::addNonlinearSolver(std::shared_ptr<SolverBase> solver)
-{
- m_solvers.push_back(solver);
-}
-
void
Scene::addObjectController(std::shared_ptr<SceneObjectControllerBase> controller)
{
@@ -340,20 +418,7 @@ Scene::resetSceneObjects()
// Apply the geometry and apply maps to all the objects
for (auto obj : this->getSceneObjects())
{
- const auto objType = obj->getType();
- if (objType == SceneObject::Type::Rigid
- || objType == SceneObject::Type::FEMDeformable
- || objType == SceneObject::Type::Pbd
- || objType == SceneObject::Type::SPH)
- {
- obj->reset();
- }
- }
-
- // Apply the geometry and apply maps to all the objects
- for (auto obj : this->getSceneObjects())
- {
- obj->updateGeometries();
+ obj->reset();
}
//\todo reset the timestep to the fixed default value when paused->run or reset
@@ -365,25 +430,20 @@ Scene::advance()
StopWatch wwt;
wwt.start();
- advance(elapsedTime);
+ advance(m_elapsedTime);
- elapsedTime = wwt.getTimeElapsed(StopWatch::TimeUnitType::seconds);
+ m_elapsedTime = wwt.getTimeElapsed(StopWatch::TimeUnitType::seconds);
}
void
Scene::advance(const double dt)
{
- // PhysX update; move this to solver
- auto physxScene = RigidBodyWorld::getInstance()->m_Scene;
- physxScene->simulate(RigidBodyWorld::getInstance()->getTimeStep()); // TODO: update the time step
- physxScene->fetchResults(true);
-
// Reset Contact forces to 0
for (auto obj : this->getSceneObjects())
{
if (auto defObj = std::dynamic_pointer_cast<FeDeformableObject>(obj))
{
- defObj->getContactForce().setConstant(0.0);
+ defObj->getFEMModel()->getContactForce().setConstant(0.0);
}
else if (auto collidingObj = std::dynamic_pointer_cast<CollidingObject>(obj))
{
@@ -397,26 +457,10 @@ Scene::advance(const double dt)
controller->updateControlledObjects();
}
- // Update the static octree and perform collision detection for some collision pairs
CollisionDetection::updateInternalOctreeAndDetectCollision();
- // Compute collision data per interaction pair
- for (auto intPair : this->getCollisionGraph()->getInteractionPairList())
- {
- intPair->computeCollisionData();
- }
-
- // Process collision data per interaction pair
- for (auto intPair : this->getCollisionGraph()->getInteractionPairList())
- {
- intPair->processCollisionData();
- }
-
- // Run the solvers
- for (auto solvers : this->getSolvers())
- {
- solvers->solve();
- }
+ // Execute the computational graph
+ m_taskGraphController->execute();
// Apply updated forces on device
for (auto controller : this->getSceneObjectControllers())
@@ -424,39 +468,19 @@ Scene::advance(const double dt)
controller->applyForces();
}
- // Apply the geometry and apply maps to all the objects
- for (auto obj : this->getSceneObjects())
- {
- obj->updateGeometries();
- }
-
// Set the trackers of the scene object controllers to out-of-date
for (auto controller : this->getSceneObjectControllers())
{
controller->setTrackerToOutOfDate();
}
- // Update time step size of the dynamic objects
for (auto obj : this->getSceneObjects())
{
- if (obj->getType() == SceneObject::Type::Pbd)
- {
- if (auto dynaObj = std::dynamic_pointer_cast<PbdObject>(obj))
- {
- if (dynaObj->getDynamicalModel()->getTimeStepSizeType() == TimeSteppingType::RealTime)
- {
- dynaObj->getDynamicalModel()->setTimeStep(dt);
- }
- }
- }
- else if (obj->getType() == SceneObject::Type::FEMDeformable)
+ if (auto dynaObj = std::dynamic_pointer_cast<DynamicObject>(obj))
{
- if (auto dynaObj = std::dynamic_pointer_cast<FeDeformableObject>(obj))
+ if (dynaObj->getDynamicalModel()->getTimeStepSizeType() == TimeSteppingType::RealTime)
{
- if (dynaObj->getDynamicalModel()->getTimeStepSizeType() == TimeSteppingType::RealTime)
- {
- dynaObj->getDynamicalModel()->setTimeStep(dt);
- }
+ dynaObj->getDynamicalModel()->setTimeStep(dt);
}
}
}
@@ -469,5 +493,42 @@ Scene::advance(const double dt)
}
this->setFPS(1.0 / dt);
+
+ // If benchmarking enabled, produce a time table for each step
+ if (m_config->taskTimingEnabled)
+ {
+ lockComputeTimes();
+ for (std::shared_ptr<TaskNode> node : m_taskGraph->getNodes())
+ {
+ m_nodeComputeTimes[node->m_name] = node->m_computeTime;
+ }
+ unlockComputeTimes();
+ }
+}
+
+double
+Scene::getElapsedTime(const std::string& stepName) const
+{
+ if (m_nodeComputeTimes.count(stepName) == 0)
+ {
+ LOG(WARNING) << "Tried to get elapsed time of nonexistent step. Is benchmarking enabled?";
+ return 0.0;
+ }
+ else
+ {
+ return m_nodeComputeTimes.at(stepName);
+ }
+}
+
+void
+Scene::lockComputeTimes()
+{
+ computeTimesLock->lock();
+}
+
+void
+Scene::unlockComputeTimes()
+{
+ computeTimesLock->unlock();
}
} // imstk
diff --git a/Source/Scene/imstkScene.h b/Source/Scene/imstkScene.h
index b367f41caf415d267c1a56447c4755a104060697..097cfb7db2c8f6b48bffe399a3b1b81b7fdf0558 100644
--- a/Source/Scene/imstkScene.h
+++ b/Source/Scene/imstkScene.h
@@ -21,23 +21,26 @@
#pragma once
-#include <unordered_map>
-#include <thread>
#include <atomic>
+#include <functional>
+#include <thread>
+#include <unordered_map>
#include <vector>
namespace imstk
{
-class SceneObjectControllerBase;
-class VisualModel;
-class CameraController;
-class DebugRenderGeometry;
-class SceneObject;
-class SolverBase;
class Camera;
-class IBLProbe;
+class CameraController;
class CollisionGraph;
+class IBLProbe;
class Light;
+class SceneObject;
+class SceneObjectControllerBase;
+class TaskGraph;
+class TaskGraphController;
+class VisualModel;
+
+namespace ParallelUtils { class SpinLock; }
enum class TimeSteppingPolicy
{
@@ -56,6 +59,18 @@ struct SceneConfig
// Keep track of the fps for the scene
bool trackFPS = false;
+
+ // If off, tasks will run sequentially
+ bool taskParallelizationEnabled = true;
+
+ // If on, elapsed times for computational steps will be reported in map
+ bool taskTimingEnabled = false;
+
+ // If on, the task graph will be written to a file
+ bool writeTaskGraph = false;
+
+ // If on, non functional nodes and redundant edges will be removed from final graph
+ bool graphReductionEnabled = true;
};
///
@@ -69,26 +84,32 @@ template<class T>
using NamedMap = std::unordered_map<std::string, std::shared_ptr<T>>;
public:
-
///
/// \brief Constructor
///
- explicit Scene(const std::string& name, std::shared_ptr<SceneConfig> config = std::make_shared<SceneConfig>()) :
- m_name(name),
- m_camera(std::make_shared<Camera>()),
- m_collisionGraph(std::make_shared<CollisionGraph>()),
- m_config(config) {}
+ explicit Scene(const std::string& name, std::shared_ptr<SceneConfig> config = std::make_shared<SceneConfig>());
///
/// \brief Destructor
///
~Scene();
+public:
///
/// \brief Initialize the scene
///
bool initialize();
+ ///
+ /// \brief Setup the task graph, this completely rebuilds the graph
+ ///
+ void buildTaskGraph();
+
+ ///
+ /// \brief Intializes the graph after its in a built state
+ ///
+ void initTaskGraph();
+
///
/// \brief Launch camera controller and other scene specific modules that need to run independently
///
@@ -180,6 +201,11 @@ public:
///
const std::string& getName() const;
+ ///
+ /// \brief Get the computational graph of the scene
+ ///
+ std::shared_ptr<TaskGraph> getTaskGraph() const { return m_taskGraph; }
+
///
/// \brief Get the camera for the scene
///
@@ -190,16 +216,6 @@ public:
///
std::shared_ptr<CollisionGraph> getCollisionGraph() const;
- ///
- /// \brief Get the vector of non-linear solvers
- ///
- const std::vector<std::shared_ptr<SolverBase>> getSolvers();
-
- ///
- /// \brief Add nonlinear solver to the scene
- ///
- void addNonlinearSolver(std::shared_ptr<SolverBase> solver);
-
///
/// \brief Add objects controllers
///
@@ -224,7 +240,32 @@ public:
///
/// \brief Get the elapsed time
///
- double getElapsedTime() { return elapsedTime; }
+ double getElapsedTime() { return m_elapsedTime; }
+
+ ///
+ /// \brief Get the elapsed time of a particular step
+ ///
+ double getElapsedTime(const std::string& stepName) const;
+
+ ///
+ /// \brief Get the map of elapsed times
+ ///
+ const std::unordered_map<std::string, double>& getTaskComputeTimes() const { return m_nodeComputeTimes; }
+
+ ///
+ /// \brief Lock the compute times resource
+ ///
+ void lockComputeTimes();
+
+ ///
+ /// \brief Unlock the compute times resource
+ ///
+ void unlockComputeTimes();
+
+ ///
+ /// \brief Called after compute graph is built, but before initialized
+ ///
+ void setTaskGraphConfigureCallback(std::function<void(Scene*)> callback) { this->m_postTaskGraphConfigureCallback = callback; }
///
/// \brief Get the configuration
@@ -236,19 +277,27 @@ protected:
std::shared_ptr<SceneConfig> m_config;
std::string m_name; ///> Name of the scene
- NamedMap<SceneObject> m_sceneObjectsMap;
- NamedMap<VisualModel> m_DebugRenderModelMap;
- NamedMap<Light> m_lightsMap;
- std::shared_ptr<IBLProbe> m_globalIBLProbe = nullptr;
- std::shared_ptr<Camera> m_camera = nullptr;
+ NamedMap<SceneObject> m_sceneObjectsMap;
+ NamedMap<VisualModel> m_DebugRenderModelMap;
+ NamedMap<Light> m_lightsMap;
+ std::shared_ptr<IBLProbe> m_globalIBLProbe = nullptr;
+
+ std::shared_ptr<Camera> m_camera = std::make_shared<Camera>();
+
std::shared_ptr<CollisionGraph> m_collisionGraph = nullptr;
- std::vector<std::shared_ptr<SolverBase>> m_solvers; ///> List of non-linear solvers
std::vector<std::shared_ptr<SceneObjectControllerBase>> m_objectControllers; ///> List of object controllers
std::vector<std::shared_ptr<CameraController>> m_cameraControllers; ///> List of camera controllers
std::unordered_map<std::string, std::thread> m_threadMap; ///>
- double m_fps = 0.0;
- double elapsedTime = 0.0;
+ std::shared_ptr<TaskGraph> m_taskGraph = nullptr; ///> Computational graph
+ std::shared_ptr<TaskGraphController> m_taskGraphController = nullptr; ///> Controller for the computational graph
+ std::function<void(Scene*)> m_postTaskGraphConfigureCallback = nullptr;
+
+ std::shared_ptr<ParallelUtils::SpinLock> computeTimesLock = nullptr;
+ std::unordered_map<std::string, double> m_nodeComputeTimes; ///> Map of ComputeNode names to elapsed times for benchmarking
+
+ double m_fps = 0.0;
+ double m_elapsedTime = 0.0;
bool m_isInitialized = false;
diff --git a/Source/SceneEntities/CMakeLists.txt b/Source/SceneEntities/CMakeLists.txt
index e17ebff50371bb55c64312c1a7dc53ea298eb6ea..b53941e54921a2c759e7711b3b179f3fd91ceb93 100644
--- a/Source/SceneEntities/CMakeLists.txt
+++ b/Source/SceneEntities/CMakeLists.txt
@@ -1,8 +1,17 @@
#-----------------------------------------------------------------------------
# Create target
#-----------------------------------------------------------------------------
+
+if (NOT ${${PROJECT_NAME}_USE_MODEL_REDUCTION})
+ list(APPEND ExclusionFiles
+ Objects/imstkReducedFeDeformableObject.h
+ Objects/imstkReducedFeDeformableObject.cpp)
+endif()
+
include(imstkAddLibrary)
-imstk_add_library( SceneEntities
+imstk_add_library(SceneEntities
+ EXCLUDE_FILES
+ ${ExclusionFiles}
DEPENDS
Common
Geometry
@@ -11,8 +20,7 @@ imstk_add_library( SceneEntities
MeshIO
Constraints
Devices
- DynamicalModels
- )
+ DynamicalModels)
#-----------------------------------------------------------------------------
# Testing
diff --git a/Source/SceneEntities/Objects/deprecated/imstkPbdRigidObject.h b/Source/SceneEntities/Objects/deprecated/imstkPbdRigidObject.h
deleted file mode 100644
index e9bfb4de1444facae73261e35b38e69f08dac32b..0000000000000000000000000000000000000000
--- a/Source/SceneEntities/Objects/deprecated/imstkPbdRigidObject.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#pragma once
-
-#include "imstkPbdObject.h"
-
-namespace imstk
-{
-class Geometry;
-class GeometryMap;
-
-///
-/// \class PbdRigidObject
-///
-/// \brief
-///
-class PbdRigidObject : public PbdObject
-{
-public:
- ///
- /// \brief Constructor
- ///
- explicit PbdRigidObject(const std::string& name) : PbdObject(name)
- { m_type = SceneObject::Type::Pbd; }
-
- ///
- /// \brief Destructor
- ///
- ~PbdRigidObject() = default;
-
- ///
- /// \brief
- ///
- virtual void integratePosition() { return; }
-
- ///
- /// \brief
- ///
- virtual void integrateVelocity() { return; }
-
- ///
- /// \brief
- ///
- virtual void updatePbdStates();
-
-protected:
-};
-} // imstk
diff --git a/Source/SceneEntities/Objects/imstkCollidingObject.cpp b/Source/SceneEntities/Objects/imstkCollidingObject.cpp
index cb07dc45a23d0f5e97954361081d3569e1aefc7a..39560d82a9fa651b6f45a80af711a34de92c39bd 100644
--- a/Source/SceneEntities/Objects/imstkCollidingObject.cpp
+++ b/Source/SceneEntities/Objects/imstkCollidingObject.cpp
@@ -27,19 +27,17 @@ namespace imstk
bool
CollidingObject::initialize()
{
- if (SceneObject::initialize())
+ if (!SceneObject::initialize())
{
- if (m_collidingToVisualMap)
- {
- m_collidingToVisualMap->initialize();
- }
-
- return true;
+ return false;
}
- else
+
+ if (m_collidingToVisualMap)
{
- return false;
+ m_collidingToVisualMap->initialize();
}
+
+ return true;
}
const std::shared_ptr<Geometry>&
diff --git a/Source/SceneEntities/Objects/imstkCollidingObject.h b/Source/SceneEntities/Objects/imstkCollidingObject.h
index dfe6e5e14c8c4fe4d11706088d08c6a8401a3417..512f1ab55de7aa72f44107a1909a17ba42b332d4 100644
--- a/Source/SceneEntities/Objects/imstkCollidingObject.h
+++ b/Source/SceneEntities/Objects/imstkCollidingObject.h
@@ -23,6 +23,7 @@
#include "imstkSceneObject.h"
#include "imstkMath.h"
+#include <unordered_map>
namespace imstk
{
@@ -81,7 +82,6 @@ public:
virtual bool initialize() override;
protected:
-
std::shared_ptr<Geometry> m_collidingGeometry; ///> Geometry for collisions
std::shared_ptr<GeometryMap> m_collidingToVisualMap; ///> Maps transformations to visual geometry
Vec3d m_force = Vec3d::Zero();
diff --git a/Source/SceneEntities/Objects/imstkDeformableObject.cpp b/Source/SceneEntities/Objects/imstkDeformableObject.cpp
index cd760dd8a30aaf2e6aac4662701484e53a028a3b..e4d1fb73f7da0a3df888484e3fa1bc0ce7d4ed8b 100644
--- a/Source/SceneEntities/Objects/imstkDeformableObject.cpp
+++ b/Source/SceneEntities/Objects/imstkDeformableObject.cpp
@@ -25,62 +25,19 @@
namespace imstk
{
-Vectord&
-FeDeformableObject::getContactForce()
-{
- CHECK(m_defModel != nullptr) << "deformation model pointer not valid DeformableObject::getContactForce()";
-
- return m_defModel->getContactForce();
-}
-
bool
FeDeformableObject::initialize()
{
- m_defModel = std::dynamic_pointer_cast<FEMDeformableBodyModel>(m_dynamicalModel);
- if (m_defModel)
- {
- return DynamicObject::initialize();
- }
- else
+ m_femModel = std::dynamic_pointer_cast<FEMDeformableBodyModel>(m_dynamicalModel);
+ if (m_femModel == nullptr)
{
LOG(FATAL) << "Dynamics pointer cast failure in DeformableObject::initialize()";
return false;
}
-}
-
-const Vectord&
-FeDeformableObject::getDisplacements() const
-{
- return m_defModel->getCurrentState()->getQ();
-}
-
-const Vectord&
-FeDeformableObject::getPrevDisplacements() const
-{
- return m_defModel->getPreviousState()->getQ();
-}
-const Vectord&
-FeDeformableObject::getVelocities() const
-{
- return m_defModel->getCurrentState()->getQDot();
-}
+ DynamicObject::initialize();
+ m_femModel->initialize();
-const Vectord&
-FeDeformableObject::getPrevVelocities() const
-{
- return m_defModel->getPreviousState()->getQDot();
-}
-
-const Vectord&
-FeDeformableObject::getAccelerations() const
-{
- return m_defModel->getCurrentState()->getQDotDot();
-}
-
-const Vectord&
-FeDeformableObject::getPrevAccelerations() const
-{
- return m_defModel->getPreviousState()->getQDotDot();
+ return true;
}
} // imstk
diff --git a/Source/SceneEntities/Objects/imstkDeformableObject.h b/Source/SceneEntities/Objects/imstkDeformableObject.h
index b2e9119e252280e4ea5a73dbe11bd08501fc29db..26c2fbc7475063dc9a7e12d04bb718d9732c2eea 100644
--- a/Source/SceneEntities/Objects/imstkDeformableObject.h
+++ b/Source/SceneEntities/Objects/imstkDeformableObject.h
@@ -30,8 +30,6 @@
namespace imstk
{
-class Geometry;
-class GeometryMap;
class FEMDeformableBodyModel;
///
@@ -42,70 +40,20 @@ class FEMDeformableBodyModel;
class FeDeformableObject : public DynamicObject
{
public:
-
- ///
- /// \brief Constructor
- ///
explicit FeDeformableObject(const std::string& name) : DynamicObject(name) { m_type = Type::FEMDeformable; }
FeDeformableObject() = delete;
- ///
- /// \brief Destructor
- ///
~FeDeformableObject() = default;
+public:
///
/// \brief Initialize the deformable object
///
bool initialize() override;
- ///
- /// \brief Initialize the kinematic state of the body
- ///
- void initializeState();
- void initializeState(const Vectord& p, const Vectord& v);
-
- ///
- /// \brief Set/Get dynamical model
- ///
- //void setDynamicalModel(std::shared_ptr<DynamicalModel<VectorizedState>> dynaDefModel) override;
-
- ///
- /// \brief Get the vector that holds the contact forces
- ///
- Vectord& getContactForce();
-
- ///
- /// \brief Get the vector of current displacements
- ///
- const Vectord& getDisplacements() const;
-
- ///
- /// \brief Get the vector of displacements from previous time step
- ///
- const Vectord& getPrevDisplacements() const;
-
- ///
- /// \brief Get the vector of current velocities
- ///
- const Vectord& getVelocities() const;
-
- ///
- /// \brief Get the vector of velocities from previous time step
- ///
- const Vectord& getPrevVelocities() const;
-
- ///
- /// \brief Get the vector of current accelerations
- ///
- const Vectord& getAccelerations() const;
-
- ///
- /// \brief Get the vector of accelerations from previous time step
- ///
- const Vectord& getPrevAccelerations() const;
+ std::shared_ptr<FEMDeformableBodyModel> getFEMModel() const { return m_femModel; }
protected:
- std::shared_ptr<FEMDeformableBodyModel> m_defModel;
+ std::shared_ptr<FEMDeformableBodyModel> m_femModel = nullptr;
};
} // imstk
diff --git a/Source/SceneEntities/Objects/imstkDynamicObject.cpp b/Source/SceneEntities/Objects/imstkDynamicObject.cpp
index 50d28d7b2ed4f744a3bcbd752cc506f5242af72a..c6bf8e64caa458d01b2e9b98fd575f00602f7335 100644
--- a/Source/SceneEntities/Objects/imstkDynamicObject.cpp
+++ b/Source/SceneEntities/Objects/imstkDynamicObject.cpp
@@ -20,9 +20,10 @@
=========================================================================*/
#include "imstkDynamicObject.h"
-#include "imstkGeometryMap.h"
#include "imstkAbstractDynamicalModel.h"
+#include "imstkGeometryMap.h"
#include "imstkLogger.h"
+#include "imstkTaskGraph.h"
namespace imstk
{
@@ -61,6 +62,17 @@ DynamicObject::updateGeometries()
}
}
+void
+DynamicObject::updatePhysicsGeometry()
+{
+ m_dynamicalModel->updatePhysicsGeometry();
+
+ if (m_physicsToCollidingGeomMap)
+ {
+ m_physicsToCollidingGeomMap->apply();
+ }
+}
+
bool
DynamicObject::initialize()
{
@@ -84,6 +96,16 @@ DynamicObject::initialize()
}
}
+void
+DynamicObject::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink)
+{
+ // Copy, sum, and connect the model graph to nest within this graph
+ m_taskGraph->addEdge(source, getUpdateNode());
+ m_dynamicalModel->initGraphEdges();
+ m_taskGraph->nestGraph(m_dynamicalModel->getTaskGraph(), getUpdateNode(), getUpdateGeometryNode());
+ m_taskGraph->addEdge(getUpdateGeometryNode(), sink);
+}
+
void
DynamicObject::reset()
{
diff --git a/Source/SceneEntities/Objects/imstkDynamicObject.h b/Source/SceneEntities/Objects/imstkDynamicObject.h
index 0535d059ab62f7389f3b3d3dda621778552d056b..58a6bb7930dc3030c91312c81c700b06106f8181 100644
--- a/Source/SceneEntities/Objects/imstkDynamicObject.h
+++ b/Source/SceneEntities/Objects/imstkDynamicObject.h
@@ -37,7 +37,6 @@ class AbstractDynamicalModel;
class DynamicObject : public CollidingObject
{
public:
-
///
/// \brief Destructor
///
@@ -82,16 +81,27 @@ public:
///
void updateGeometries() final;
+ ///
+ /// \brief Update only the physics geometry and apply collision map
+ ///
+ void updatePhysicsGeometry();
+
///
/// \brief Initialize the scene object
///
virtual bool initialize() override;
///
- /// \brief Reset the dynamic object to its initial state
+ /// \brief Reset the dynamic object by reseting the respective DynamicalModel and Geometry
///
virtual void reset() override;
+protected:
+ ///
+ /// \brief Setup connectivity of compute graph
+ ///
+ virtual void initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink) override;
+
protected:
///
@@ -99,12 +109,12 @@ protected:
///
explicit DynamicObject(const std::string& name) : CollidingObject(name) {}
- std::shared_ptr<AbstractDynamicalModel> m_dynamicalModel; ///> Dynamical model
- std::shared_ptr<Geometry> m_physicsGeometry; ///> Geometry used for Physics
+ std::shared_ptr<AbstractDynamicalModel> m_dynamicalModel = nullptr; ///> Dynamical model
+ std::shared_ptr<Geometry> m_physicsGeometry = nullptr; ///> Geometry used for Physics
- //Maps
- std::shared_ptr<GeometryMap> m_physicsToCollidingGeomMap; ///> Maps from Physics to collision geometry
- std::shared_ptr<GeometryMap> m_physicsToVisualGeomMap; ///> Maps from Physics to visual geometry
+ // Maps
+ std::shared_ptr<GeometryMap> m_physicsToCollidingGeomMap = nullptr; ///> Maps from Physics to collision geometry
+ std::shared_ptr<GeometryMap> m_physicsToVisualGeomMap = nullptr; ///> Maps from Physics to visual geometry
bool m_updateVisualFromPhysicsGeometry = true; ///> Defines if visual is updated from colliding mapping or physics mapping
};
} // imstk
diff --git a/Source/SceneEntities/Objects/imstkPbdObject.cpp b/Source/SceneEntities/Objects/imstkPbdObject.cpp
index ae380e2f71d42a60afbbf080900b0f7d70ad8782..731aa243662478c8d175300cbc08b51910aef344 100644
--- a/Source/SceneEntities/Objects/imstkPbdObject.cpp
+++ b/Source/SceneEntities/Objects/imstkPbdObject.cpp
@@ -20,53 +20,31 @@
=========================================================================*/
#include "imstkPbdObject.h"
-#include "imstkGeometryMap.h"
-#include "imstkPbdModel.h"
#include "imstkLogger.h"
+#include "imstkPbdModel.h"
namespace imstk
{
-bool
-PbdObject::initialize()
+std::shared_ptr<PbdModel>
+PbdObject::getPbdModel()
{
m_pbdModel = std::dynamic_pointer_cast<PbdModel>(m_dynamicalModel);
+ return m_pbdModel;
+};
- CHECK(m_pbdModel != nullptr) << "Dynamics pointer cast failure in PbdObject::initialize()";
-
- return DynamicObject::initialize();
-}
-
-void
-PbdObject::integratePosition()
-{
- if (m_pbdModel && m_pbdModel->hasConstraints())
- {
- m_pbdModel->integratePosition();
- }
-}
-
-void
-PbdObject::updateVelocity()
+bool
+PbdObject::initialize()
{
- if (m_pbdModel && m_pbdModel->hasConstraints())
+ m_pbdModel = std::dynamic_pointer_cast<PbdModel>(m_dynamicalModel);
+ if (m_pbdModel == nullptr)
{
- m_pbdModel->updateVelocity();
+ LOG(FATAL) << "Dynamics pointer cast failure in PbdObject::initialize()";
+ return false;
}
-}
-void
-PbdObject::solveConstraints()
-{
- if (m_pbdModel && m_pbdModel->hasConstraints())
- {
- m_pbdModel->projectConstraints();
- }
-}
+ DynamicObject::initialize();
+ m_pbdModel->initialize();
-void
-PbdObject::reset()
-{
- DynamicObject::reset();
- this->updateVelocity();
+ return true;
}
} //imstk
diff --git a/Source/SceneEntities/Objects/imstkPbdObject.h b/Source/SceneEntities/Objects/imstkPbdObject.h
index 17feda4e8ffffc9326316bba6045e9ca91b316bd..0ee18a27ce575a4c5a19b7b5e4d9139fa2964722 100644
--- a/Source/SceneEntities/Objects/imstkPbdObject.h
+++ b/Source/SceneEntities/Objects/imstkPbdObject.h
@@ -25,22 +25,17 @@
namespace imstk
{
-class Geometry;
-class GeometryMap;
class PbdModel;
///
/// \class PbdObject
///
/// \brief Base class for scene objects that move and/or deform under position
-/// based dynamics formulation
+/// based dynamics formulation, implements the PbdModel and PbdSolver
///
class PbdObject : public DynamicObject
{
public:
- ///
- /// \brief Constructor
- ///
explicit PbdObject(const std::string& name) : DynamicObject(name)
{
m_type = SceneObject::Type::Pbd;
@@ -48,37 +43,20 @@ public:
PbdObject() = delete;
- ///
- /// \brief Destructor
- ///
~PbdObject() override = default;
+public:
///
- /// \brief Initialize the pbd scene object
- ///
- bool initialize() override;
-
- ///
- /// \brief Update the position based on Verlet time stepping rule
- ///
- virtual void integratePosition();
-
- ///
- /// \brief Update the velocity
- ///
- virtual void updateVelocity();
-
- ///
- /// \brief Solve the pbd constraints by projection
+ /// \biref Get the Pbd model of the object
///
- virtual void solveConstraints();
+ std::shared_ptr<PbdModel> getPbdModel();
///
- /// \brief Reset the PBD object to its initial state
+ /// \brief Initialize the pbd scene object
///
- void reset() override;
+ bool initialize() override;
protected:
- std::shared_ptr<PbdModel> m_pbdModel; ///> PBD mathematical model
+ std::shared_ptr<PbdModel> m_pbdModel = nullptr; ///> PBD mathematical model
};
} // imstk
diff --git a/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.cpp b/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..93830c7de4616392e5f9418cdf44f89189c2bd96
--- /dev/null
+++ b/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.cpp
@@ -0,0 +1,87 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#include "imstkReducedFeDeformableObject.h"
+#include "imstkReducedStVKBodyModel.h"
+#include "imstkLogger.h"
+
+namespace imstk
+{
+Vectord&
+ReducedFeDeformableObject::getContactForce()
+{
+ CHECK(m_defModel != nullptr) << "deformation model pointer not valid DeformableObject::getContactForce()";
+
+ return m_defModel->getContactForce();
+}
+
+bool
+ReducedFeDeformableObject::initialize()
+{
+ m_defModel = std::dynamic_pointer_cast<ReducedStVK>(m_dynamicalModel);
+
+ if (m_defModel)
+ {
+ return DynamicObject::initialize();
+ }
+ else
+ {
+ LOG(FATAL) << "Dynamics pointer cast failure in DeformableObject::initialize()";
+ return false;
+ }
+}
+
+const Vectord&
+ReducedFeDeformableObject::getDisplacements() const
+{
+ return m_defModel->getCurrentState()->getQ();
+}
+
+const Vectord&
+ReducedFeDeformableObject::getPrevDisplacements() const
+{
+ return m_defModel->getPreviousState()->getQ();
+}
+
+const Vectord&
+ReducedFeDeformableObject::getVelocities() const
+{
+ return m_defModel->getCurrentState()->getQDot();
+}
+
+const Vectord&
+ReducedFeDeformableObject::getPrevVelocities() const
+{
+ return m_defModel->getPreviousState()->getQDot();
+}
+
+const Vectord&
+ReducedFeDeformableObject::getAccelerations() const
+{
+ return m_defModel->getCurrentState()->getQDotDot();
+}
+
+const Vectord&
+ReducedFeDeformableObject::getPrevAccelerations() const
+{
+ return m_defModel->getPreviousState()->getQDotDot();
+}
+} // imstk
diff --git a/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.h b/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.h
new file mode 100644
index 0000000000000000000000000000000000000000..5f577e2e31e7c0ac806082921422e22baa44a3be
--- /dev/null
+++ b/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.h
@@ -0,0 +1,111 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & 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.txt
+
+ 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.
+
+=========================================================================*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+// imstk
+#include "imstkDynamicObject.h"
+#include "imstkMath.h"
+
+namespace imstk
+{
+class Geometry;
+class GeometryMap;
+class ReducedStVK;
+
+///
+/// \class DeformableObject
+///
+/// \brief Scene objects that can deform
+///
+class ReducedFeDeformableObject : public DynamicObject
+{
+public:
+
+ ///
+ /// \brief Constructor
+ ///
+ explicit ReducedFeDeformableObject(const std::string& name) : DynamicObject(name) { m_type = Type::ReducedFEMDeformable; }
+ ReducedFeDeformableObject() = delete;
+
+ ///
+ /// \brief Destructor
+ ///
+ ~ReducedFeDeformableObject() = default;
+
+ ///
+ /// \brief Initialize the deformable object
+ ///
+ bool initialize() override;
+
+ ///
+ /// \brief Initialize the kinematic state of the body
+ ///
+ void initializeState();
+ void initializeState(const Vectord& p, const Vectord& v);
+
+ ///
+ /// \brief Set/Get dynamical model
+ ///
+ //void setDynamicalModel(std::shared_ptr<DynamicalModel<VectorizedState>> dynaDefModel) override;
+
+ ///
+ /// \brief Get the vector that holds the contact forces
+ ///
+ Vectord& getContactForce();
+
+ ///
+ /// \brief Get the vector of current displacements
+ ///
+ const Vectord& getDisplacements() const;
+
+ ///
+ /// \brief Get the vector of displacements from previous time step
+ ///
+ const Vectord& getPrevDisplacements() const;
+
+ ///
+ /// \brief Get the vector of current velocities
+ ///
+ const Vectord& getVelocities() const;
+
+ ///
+ /// \brief Get the vector of velocities from previous time step
+ ///
+ const Vectord& getPrevVelocities() const;
+
+ ///
+ /// \brief Get the vector of current accelerations
+ ///
+ const Vectord& getAccelerations() const;
+
+ ///
+ /// \brief Get the vector of accelerations from previous time step
+ ///
+ const Vectord& getPrevAccelerations() const;
+
+protected:
+ std::shared_ptr<ReducedStVK> m_defModel = nullptr;
+};
+} // imstk
diff --git a/Source/SceneEntities/Objects/imstkRigidObject.cpp b/Source/SceneEntities/Objects/imstkRigidObject.cpp
index 407520854b7fb416a8c924a1d2b758b6a99bd8c6..fa2ebba781ae404919be4cf353ecb80b111c0cd0 100644
--- a/Source/SceneEntities/Objects/imstkRigidObject.cpp
+++ b/Source/SceneEntities/Objects/imstkRigidObject.cpp
@@ -18,28 +18,32 @@
limitations under the License.
=========================================================================*/
+
#include "imstkRigidObject.h"
#include "imstkLogger.h"
+#include "imstkRigidBodyModel.h"
namespace imstk
{
bool
RigidObject::initialize()
{
- auto m_rigidBodyModel = std::dynamic_pointer_cast<RigidBodyModel>(m_dynamicalModel);
- if (m_rigidBodyModel)
- {
- /*if (!m_rigidBodyModel->getRigidBodyWorld())
- {
- LOG(WARNING) << "RigidObject::initialize() - Rigid body world not specified!";
- return false;
- }*/
- return DynamicObject::initialize();
- }
- else
+ m_rigidBodyModel = std::dynamic_pointer_cast<RigidBodyModel>(m_dynamicalModel);
+ if (m_rigidBodyModel == nullptr)
{
- LOG(WARNING) << "RigidObject::initialize() - Dynamics pointer cast failure";
+ LOG(FATAL) << "Dynamics pointer cast failure in RigidObject::initialize()";
return false;
}
+
+ DynamicObject::initialize();
+ m_rigidBodyModel->initialize();
+
+ return true;
+}
+
+void
+RigidObject::addForce(const Vec3d& force, const Vec3d& pos, bool wakeup)
+{
+ m_rigidBodyModel->addForce(force, pos, wakeup);
}
} // imstk
diff --git a/Source/SceneEntities/Objects/imstkRigidObject.h b/Source/SceneEntities/Objects/imstkRigidObject.h
index 7e0ff3a2810f87506bc782d6108e36521b841abb..eb24b27f24ab84c3a3ec4b9f9703d5ffd29efca8 100644
--- a/Source/SceneEntities/Objects/imstkRigidObject.h
+++ b/Source/SceneEntities/Objects/imstkRigidObject.h
@@ -21,12 +21,12 @@
#pragma once
-// imstk
#include "imstkDynamicObject.h"
-#include "imstkRigidBodyModel.h"
namespace imstk
{
+class RigidBodyModel;
+
///
/// \class RigidObject
///
@@ -35,20 +35,14 @@ namespace imstk
class RigidObject : public DynamicObject
{
public:
-
- ///
- /// \brief Constructor
- ///
explicit RigidObject(const std::string& name) : DynamicObject(name)
{
m_type = Type::Rigid;
}
- ///
- /// \brief Destructor
- ///
~RigidObject() = default;
+public:
///
/// \brief Initialize the rigid scene object
///
@@ -57,24 +51,11 @@ public:
///
/// \brief Add local force at a position relative to object
///
- void addForce(const Vec3d& force, const Vec3d& pos, bool wakeup = true)
- {
- getRigidBodyModel()->addForce(force, pos, wakeup);
- }
+ void addForce(const Vec3d& force, const Vec3d& pos, bool wakeup = true);
- ///
- /// \brief Get/Set rigid body model
- ///
- void setRigidBodyModel(std::shared_ptr<RigidBodyModel> rbModel) { m_dynamicalModel = rbModel; };
- std::shared_ptr<RigidBodyModel> getRigidBodyModel() const { return std::dynamic_pointer_cast<RigidBodyModel>(m_dynamicalModel); };
-
- void reset() override
- {
- m_dynamicalModel->resetToInitialState();
- this->updateGeometries();
- }
+ std::shared_ptr<RigidBodyModel> getRigidBodyModel() const { return m_rigidBodyModel; }
protected:
- //std::shared_ptr<RigidBodyModel> m_rigidBodyModel; ///> PBD mathematical model
+ std::shared_ptr<RigidBodyModel> m_rigidBodyModel;
};
} // imstk
\ No newline at end of file
diff --git a/Source/SceneEntities/Objects/imstkSPHObject.cpp b/Source/SceneEntities/Objects/imstkSPHObject.cpp
index f5896db40469425e42833a82ca5ab554a6c1645a..9be7701c33be9051e53d9ece65162713da930191 100644
--- a/Source/SceneEntities/Objects/imstkSPHObject.cpp
+++ b/Source/SceneEntities/Objects/imstkSPHObject.cpp
@@ -26,15 +26,22 @@ namespace imstk
{
SPHObject::SPHObject(const std::string& name) : DynamicObject(name)
{
- this->m_type = SceneObject::Type::SPH;
+ this->m_type = Type::SPH;
}
bool
SPHObject::initialize()
{
- m_SPHModel = std::dynamic_pointer_cast<SPHModel>(this->m_dynamicalModel);
- LOG_IF(FATAL, (!m_SPHModel)) << "Invalid SPH model";
+ m_SPHModel = std::dynamic_pointer_cast<SPHModel>(m_dynamicalModel);
+ if (m_SPHModel == nullptr)
+ {
+ LOG(FATAL) << "Dynamics pointer cast failure in SPHObject::initialize()";
+ return false;
+ }
- return m_SPHModel->initialize();
+ DynamicObject::initialize();
+ m_SPHModel->initialize();
+
+ return true;
}
} // end namespace imstk
diff --git a/Source/SceneEntities/Objects/imstkSPHObject.h b/Source/SceneEntities/Objects/imstkSPHObject.h
index c123cadb9c953c888d27de7b2e78cabf2ec9e593..d6f8936633e1e84e0043e18ae2c87b4cdc604987 100644
--- a/Source/SceneEntities/Objects/imstkSPHObject.h
+++ b/Source/SceneEntities/Objects/imstkSPHObject.h
@@ -30,22 +30,17 @@ class SPHModel;
///
/// \class SPHObject
///
-/// \brief Base class for scene objects that move and/or deform under position
-/// based dynamics formulation
+/// \brief Base class for scene objects that move and/or deform under
+/// smooth particle hydrodynamics
///
class SPHObject : public DynamicObject
{
public:
- ///
- /// \brief Constructor
- ///
explicit SPHObject(const std::string& name);
- ///
- /// \brief Destructor
- ///
virtual ~SPHObject() override = default;
+public:
///
/// \brief Initialize the SPH scene object
///
@@ -54,9 +49,9 @@ public:
///
/// \brief Get the SPH model of the object
///
- const std::shared_ptr<SPHModel>& getSPHModel() const { assert(m_SPHModel); return m_SPHModel; }
+ std::shared_ptr<SPHModel> getSPHModel() const { assert(m_SPHModel); return m_SPHModel; }
protected:
- std::shared_ptr<SPHModel> m_SPHModel;
+ std::shared_ptr<SPHModel> m_SPHModel = nullptr;
};
} // end namespace imstk
diff --git a/Source/SceneEntities/Objects/imstkSceneObject.cpp b/Source/SceneEntities/Objects/imstkSceneObject.cpp
index 65c82cc3cbef5701b0c6e0177f48a571c0d4f9ba..34aaad5cdab3eb2bb8f9127175c61946f598b847 100644
--- a/Source/SceneEntities/Objects/imstkSceneObject.cpp
+++ b/Source/SceneEntities/Objects/imstkSceneObject.cpp
@@ -20,12 +20,16 @@
=========================================================================*/
#include "imstkSceneObject.h"
+#include "imstkTaskGraph.h"
namespace imstk
{
-SceneObject::SceneObject(const std::string& name) : m_name(name), SceneEntity()
+SceneObject::SceneObject(const std::string& name) :
+ m_type(Type::Visual), m_name(name),
+ m_taskGraph(std::make_shared<TaskGraph>("SceneObject_" + name + "_Source", "SceneObject_" + name + "_Sink")), SceneEntity()
{
- m_type = Type::Visual;
+ m_updateNode = m_taskGraph->addFunction("SceneObject_" + name + "_Update", std::bind(&SceneObject::update, this));
+ m_updateGeometryNode = m_taskGraph->addFunction("SceneObject_" + name + "_UpdateGeometry", std::bind(&SceneObject::updateGeometries, this));
}
std::shared_ptr<Geometry>
@@ -51,57 +55,18 @@ SceneObject::setVisualGeometry(std::shared_ptr<Geometry> geometry)
}
}
-std::shared_ptr<Geometry>
-SceneObject::getMasterGeometry() const
-{
- return this->getVisualGeometry();
-}
-
-std::shared_ptr<VisualModel>
-SceneObject::getVisualModel(unsigned int index)
-{
- return m_visualModels[index];
-}
-
void
-SceneObject::addVisualModel(std::shared_ptr<VisualModel> visualModel)
-{
- m_visualModels.push_back(visualModel);
-}
-
-const std::vector<std::shared_ptr<VisualModel>>&
-SceneObject::getVisualModels()
-{
- return m_visualModels;
-}
-
-size_t
-SceneObject::getNumVisualModels()
-{
- return m_visualModels.size();
-}
-
-const SceneObject::Type&
-SceneObject::getType() const
-{
- return m_type;
-}
-
-void
-SceneObject::setType(SceneObject::Type type)
-{
- m_type = type;
-}
-
-const std::string&
-SceneObject::getName() const
+SceneObject::initGraphEdges()
{
- return m_name;
+ m_taskGraph->clearEdges();
+ initGraphEdges(m_taskGraph->getSource(), m_taskGraph->getSink());
}
void
-SceneObject::setName(const std::string& name)
+SceneObject::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink)
{
- m_name = name;
+ m_taskGraph->addEdge(source, m_updateNode);
+ m_taskGraph->addEdge(m_updateNode, m_updateGeometryNode);
+ m_taskGraph->addEdge(m_updateGeometryNode, sink);
}
} // imstk
diff --git a/Source/SceneEntities/Objects/imstkSceneObject.h b/Source/SceneEntities/Objects/imstkSceneObject.h
index cb6ce62df8f4eda987eec780d64267b769673f61..d848c62e829de94f0fcd7f541694e28112e77fe9 100644
--- a/Source/SceneEntities/Objects/imstkSceneObject.h
+++ b/Source/SceneEntities/Objects/imstkSceneObject.h
@@ -29,8 +29,10 @@
namespace imstk
{
-class Geometry;
class DeviceClient;
+class Geometry;
+class TaskGraph;
+class TaskNode;
///
/// \class SceneObject
@@ -48,6 +50,7 @@ public:
Colliding,
Rigid,
FEMDeformable,
+ ReducedFEMDeformable,
Pbd,
SPH
};
@@ -62,16 +65,22 @@ public:
///
virtual ~SceneObject() = default;
+public:
///
/// \brief Get the type of the object
///
- const Type& getType() const;
+ const Type& getType() const { return m_type; }
+
+ ///
+ /// \brief Get the computational graph
+ ///
+ std::shared_ptr<TaskGraph> getTaskGraph() const { return m_taskGraph; }
///
/// \brief Get/Set the custom name of the scene object
///
- const std::string& getName() const;
- void setName(const std::string& name);
+ const std::string& getName() const { return m_name; }
+ void setName(const std::string& name) { m_name = name; }
///
/// \brief DEPRECATED: Get/Set geometry used for viewing
@@ -84,23 +93,38 @@ public:
///
/// \brief Get/add visual model
///
- std::shared_ptr<VisualModel> getVisualModel(unsigned int index=0);
- void addVisualModel(std::shared_ptr<VisualModel> visualModel);
+ std::shared_ptr<VisualModel> getVisualModel(unsigned int index) { return m_visualModels[index]; }
+ void addVisualModel(std::shared_ptr<VisualModel> visualModel) { m_visualModels.push_back(visualModel); }
///
/// \brief Get all visual models
///
- const std::vector<std::shared_ptr<VisualModel>>& getVisualModels();
+ const std::vector<std::shared_ptr<VisualModel>>& getVisualModels() { return m_visualModels; }
///
/// \brief Get number of visual models
///
- size_t getNumVisualModels();
+ size_t getNumVisualModels() { return m_visualModels.size(); }
///
/// \brief Get the master geometry
///
- virtual std::shared_ptr<Geometry> getMasterGeometry() const;
+ virtual std::shared_ptr<Geometry> getMasterGeometry() const { return this->getVisualGeometry(); }
+
+ ///
+ /// \brief Returns the computational node for updating
+ ///
+ std::shared_ptr<TaskNode> getUpdateNode() const { return m_updateNode; }
+
+ ///
+ /// \brief Returns the computational node for updating geometry
+ ///
+ std::shared_ptr<TaskNode> getUpdateGeometryNode() const { return m_updateGeometryNode; }
+
+ ///
+ /// \brief
+ ///
+ virtual void update() { }
///
/// \brief
@@ -112,20 +136,32 @@ public:
///
virtual bool initialize() { return true; }
+ ///
+ /// \brief Initializes the edges of the SceneObject's computational graph
+ ///
+ void initGraphEdges();
+
///
/// \brief
///
- virtual void reset() {}
+ virtual void reset() { }
protected:
///
- /// \brief Assigns the type of the object
+ /// \brief Setup connectivity of the compute graph
///
- void setType(Type type);
+ virtual void initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink);
+protected:
Type m_type; ///> Type of the scene object
std::string m_name; ///> Custom name of the scene object
std::vector<std::shared_ptr<VisualModel>> m_visualModels; ///> Visual objects for rendering
+ std::shared_ptr<TaskGraph> m_taskGraph = nullptr; ///> Computational Graph
+
+private:
+ // Dissallow reassignment of these in subclasses
+ std::shared_ptr<TaskNode> m_updateNode = nullptr;
+ std::shared_ptr<TaskNode> m_updateGeometryNode = nullptr;
};
using VisualObject = SceneObject;
diff --git a/Source/SimulationManager/VTKRenderer/imstkOpenVRCommand.cpp b/Source/SimulationManager/VTKRenderer/imstkOpenVRCommand.cpp
index 04c24daf515ba8e005d3cbe73fc8167a9c0c15eb..2ba7f86c8b2cc2a950e5bf49e5b43eb856b14211 100644
--- a/Source/SimulationManager/VTKRenderer/imstkOpenVRCommand.cpp
+++ b/Source/SimulationManager/VTKRenderer/imstkOpenVRCommand.cpp
@@ -26,7 +26,6 @@ limitations under the License.
#include "imstkOpenVRCommand.h"
#include "imstkSimulationManager.h"
#include "VTKRenderer/imstkVTKRenderer.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
namespace imstk
diff --git a/Source/SimulationManager/VTKRenderer/imstkVTKInteractorStyle.cpp b/Source/SimulationManager/VTKRenderer/imstkVTKInteractorStyle.cpp
index 4dbbe40f9f620db31d77c151b640ebc0b2f895d9..78a57f8beb24b49ad386afc5ebea67b14c728be7 100644
--- a/Source/SimulationManager/VTKRenderer/imstkVTKInteractorStyle.cpp
+++ b/Source/SimulationManager/VTKRenderer/imstkVTKInteractorStyle.cpp
@@ -23,7 +23,6 @@
#include "imstkVTKInteractorStyle.h"
#include "imstkSimulationManager.h"
#include "imstkVTKTextStatusManager.h"
-#include "imstkCollisionGraph.h"
#include "imstkVTKRenderer.h"
#include "imstkCamera.h"
#include "imstkScene.h"
diff --git a/Source/SimulationManager/VTKRenderer/imstkVTKTextStatusManager.cpp b/Source/SimulationManager/VTKRenderer/imstkVTKTextStatusManager.cpp
index 2f5ca2738730ab2a61be419e2eb3a8416de834f1..102cda91fc654ef868e3504f74ad45319ce0d594 100644
--- a/Source/SimulationManager/VTKRenderer/imstkVTKTextStatusManager.cpp
+++ b/Source/SimulationManager/VTKRenderer/imstkVTKTextStatusManager.cpp
@@ -22,7 +22,6 @@
#include "imstkSimulationManager.h"
#include "imstkVTKTextStatusManager.h"
#include "imstkVTKInteractorStyle.h"
-#include "imstkCollisionGraph.h"
#include "imstkLogger.h"
#include "imstkCamera.h"
diff --git a/Source/SimulationManager/VTKRenderer/imstkVTKViewer.cpp b/Source/SimulationManager/VTKRenderer/imstkVTKViewer.cpp
index 3f4052a82501e7040fc5d14cef6a1bcfcea1b76c..e53ca868bf6ea18f80a3479c8f6749eae1ae272e 100644
--- a/Source/SimulationManager/VTKRenderer/imstkVTKViewer.cpp
+++ b/Source/SimulationManager/VTKRenderer/imstkVTKViewer.cpp
@@ -22,7 +22,6 @@
#include "imstkVTKViewer.h"
#include "imstkVTKRenderDelegate.h"
#include "imstkVTKInteractorStyle.h"
-#include "imstkCollisionGraph.h"
#include "imstkVTKRenderer.h"
#include "imstkCamera.h"
#include "imstkLogger.h"
@@ -31,12 +30,15 @@
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
+#include <vtkCallbackCommand.h>
+
+#include "imstkParallelUtils.h"
namespace imstk
{
-VTKViewer::VTKViewer(SimulationManager* manager /*= nullptr*/, bool enableVR /*= false*/)
+VTKViewer::VTKViewer(SimulationManager* manager /*= nullptr*/, bool enableVR /*= false*/) :
+ m_enableVR(enableVR)
{
- m_enableVR = enableVR;
// init render window / interactor / command based
// depending on if we enable VR or not
if (!m_enableVR)
@@ -76,6 +78,11 @@ VTKViewer::VTKViewer(SimulationManager* manager /*= nullptr*/, bool enableVR /*=
m_vtkRenderWindow->AddObserver(vtkCommand::StartEvent, m_openVRCommand, 1.0);
}
#endif
+
+ // Setup callback for timer on the interactor
+ timerCallbackCommand = vtkSmartPointer<vtkCallbackCommand>::New();
+ timerCallbackCommand->SetCallback(timerCallback);
+ timerCallbackCommand->SetClientData(this);
}
void
@@ -181,6 +188,16 @@ VTKViewer::startRenderingLoop()
{
m_vtkRenderWindow->GetInteractor()->Initialize();
m_vtkRenderWindow->GetInteractor()->CreateOneShotTimer(0);
+
+ // If the Scene wants benchmarking hookup timer to update the table
+ auto vtkRen = std::dynamic_pointer_cast<VTKRenderer>(getActiveRenderer());
+ vtkRen->setTimeTableVisibility(m_activeScene->getConfig()->taskTimingEnabled);
+ if (m_activeScene->getConfig()->taskTimingEnabled)
+ {
+ m_vtkRenderWindow->GetInteractor()->AddObserver(vtkCommand::TimerEvent, timerCallbackCommand);
+ m_vtkRenderWindow->GetInteractor()->CreateRepeatingTimer(500);
+ }
+
m_vtkRenderWindow->SetWindowName(m_config->m_windowName.c_str());
m_vtkRenderWindow->GetInteractor()->Start();
m_vtkRenderWindow->GetInteractor()->DestroyTimer();
@@ -236,4 +253,18 @@ VTKViewer::getTextStatusManager()
{
return m_vtkInteractorStyle->getTextStatusManager();
}
+
+void
+VTKViewer::timerCallback(vtkObject* caller, long unsigned int vtkNotUsed(eventId), void* clientData, void* vtkNotUsed(callData))
+{
+ VTKViewer* self = static_cast<VTKViewer*>(clientData);
+
+ if (self->getActiveScene()->getConfig()->taskTimingEnabled)
+ {
+ auto vtkRen = std::dynamic_pointer_cast<VTKRenderer>(self->getActiveRenderer());
+ self->getActiveScene()->lockComputeTimes();
+ vtkRen->setTimeTable(self->getActiveScene()->getTaskComputeTimes());
+ self->getActiveScene()->unlockComputeTimes();
+ }
+}
} // imstk
diff --git a/Source/SimulationManager/VTKRenderer/imstkVTKViewer.h b/Source/SimulationManager/VTKRenderer/imstkVTKViewer.h
index 9ea6a5650cdb293ee61f84e1ff003b091204e291..812d7fd829ca9b73dd460a33a2753f95e784ef02 100644
--- a/Source/SimulationManager/VTKRenderer/imstkVTKViewer.h
+++ b/Source/SimulationManager/VTKRenderer/imstkVTKViewer.h
@@ -32,6 +32,7 @@
#include "vtkOpenVRRenderWindowInteractor.h"
#endif
+class vtkCallbackCommand;
class vtkRenderWindow;
namespace imstk
@@ -109,10 +110,14 @@ public:
const std::shared_ptr<VTKTextStatusManager>& getTextStatusManager();
protected:
+ static void timerCallback(vtkObject* caller, long unsigned int eventId, void* clientData, void* callData);
+
vtkSmartPointer<vtkRenderWindow> m_vtkRenderWindow;
std::shared_ptr<VTKInteractorStyle> m_vtkInteractorStyle;
bool m_enableVR;
+ vtkSmartPointer<vtkCallbackCommand> timerCallbackCommand;
+
#ifdef iMSTK_ENABLE_VR
vtkSmartPointer<OpenVRCommand> m_openVRCommand;
#endif
diff --git a/Source/SimulationManager/imstkSceneManager.cpp b/Source/SimulationManager/imstkSceneManager.cpp
index 452fc8e52a21a7cd5ff31a1448f88b5117b9c501..d77823acb907d050d66349eebbec1c560cbd7531 100644
--- a/Source/SimulationManager/imstkSceneManager.cpp
+++ b/Source/SimulationManager/imstkSceneManager.cpp
@@ -23,7 +23,6 @@
#include "imstkScene.h"
#include "imstkSceneManager.h"
//#include "imstkCameraController.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
namespace imstk
diff --git a/Source/SimulationManager/imstkSimulationManager.cpp b/Source/SimulationManager/imstkSimulationManager.cpp
index a6b72f5cd23717e526fa4c3bcea612ce09516338..fa0caca444250f4b1ec44cb9ee0a90f32468427f 100644
--- a/Source/SimulationManager/imstkSimulationManager.cpp
+++ b/Source/SimulationManager/imstkSimulationManager.cpp
@@ -20,19 +20,17 @@
=========================================================================*/
#include "imstkSimulationManager.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
-#include "imstkThreadManager.h"
-#include "imstkSceneManager.h"
-#include "imstkViewer.h"
-#include "imstkTimer.h"
#include "imstkLogger.h"
#include "imstkScene.h"
+#include "imstkSceneManager.h"
+#include "imstkThreadManager.h"
+#include "imstkTimer.h"
+#include "imstkViewer.h"
+#include "imstkVTKRenderer.h"
#include <string>
-#include "imstkLogger.h"
-
namespace imstk
{
SimulationManager::SimulationManager(const std::shared_ptr<SimManagerConfig> config) : m_config(config)
diff --git a/Source/SimulationManager/imstkViewer.cpp b/Source/SimulationManager/imstkViewer.cpp
index d12ea424849953579acedf545871886a03b35b42..d19cf8d536069d16a86f983fde80cb1f6e1211e5 100644
--- a/Source/SimulationManager/imstkViewer.cpp
+++ b/Source/SimulationManager/imstkViewer.cpp
@@ -21,7 +21,6 @@
#include "imstkViewer.h"
#include "imstkScene.h"
-#include "imstkCollisionGraph.h"
#include "imstkCamera.h"
#include "imstkLogger.h"
#include "imstkScreenCaptureUtility.h"
diff --git a/Source/Solvers/CMakeLists.txt b/Source/Solvers/CMakeLists.txt
index 14b17c8a3dd2ecff8eb109f6dc8d1c4efdcd8a50..9f45108fb8cb52421166d8cfc28e347a680b2927 100644
--- a/Source/Solvers/CMakeLists.txt
+++ b/Source/Solvers/CMakeLists.txt
@@ -5,7 +5,6 @@ include(imstkAddLibrary)
imstk_add_library( Solvers
DEPENDS
Constraints
- SceneEntities
)
#-----------------------------------------------------------------------------
diff --git a/Source/Solvers/imstkDirectLinearSolver.cpp b/Source/Solvers/imstkDirectLinearSolver.cpp
index 77b4018ee1db170783bed698d08968edaaf57dce..1b82a4eb8a063a23f78be90caf956db0cad20986 100644
--- a/Source/Solvers/imstkDirectLinearSolver.cpp
+++ b/Source/Solvers/imstkDirectLinearSolver.cpp
@@ -59,14 +59,20 @@ setSystem(std::shared_ptr<LinearSystem<SparseMatrixd>> newSystem)
void
DirectLinearSolver<SparseMatrixd>::solve(const Vectord& rhs, Vectord& x)
{
- if(!m_linearSystem) LOG(FATAL) << "Linear system has not been set";
+ if (!m_linearSystem)
+ {
+ LOG(FATAL) << "Linear system has not been set";
+ }
x = m_solver.solve(rhs);
}
void
DirectLinearSolver<SparseMatrixd>::solve(Vectord& x)
{
- if(!m_linearSystem) LOG(FATAL) << "Linear system has not been set";
+ if (!m_linearSystem)
+ {
+ LOG(FATAL) << "Linear system has not been set";
+ }
x.setZero();
auto b = m_linearSystem->getRHSVector();
@@ -76,14 +82,20 @@ DirectLinearSolver<SparseMatrixd>::solve(Vectord& x)
void
DirectLinearSolver<Matrixd>::solve(const Vectord& rhs, Vectord& x)
{
- if(!m_linearSystem) LOG(FATAL) << "Linear system has not been set";
+ if (!m_linearSystem)
+ {
+ LOG(FATAL) << "Linear system has not been set";
+ }
x = m_solver.solve(rhs);
}
void
DirectLinearSolver<Matrixd>::solve(Vectord& x)
{
- if(!m_linearSystem) LOG(FATAL) << "Linear system has not been set";
+ if (!m_linearSystem)
+ {
+ LOG(FATAL) << "Linear system has not been set";
+ }
x.setZero();
auto b = m_linearSystem->getRHSVector();
diff --git a/Source/Solvers/imstkDirectLinearSolver.h b/Source/Solvers/imstkDirectLinearSolver.h
index e586d811f03ec1e8fb0b21dd0cdc961d23616017..baaf133e5b79247b15e27777415aaf6e0dd0461e 100644
--- a/Source/Solvers/imstkDirectLinearSolver.h
+++ b/Source/Solvers/imstkDirectLinearSolver.h
@@ -47,7 +47,7 @@ public:
/// \brief Default constructor/destructor.
///
// DirectLinearSolver() = delete;
- DirectLinearSolver() {}
+ DirectLinearSolver() {}
~DirectLinearSolver() {};
///
diff --git a/Source/Solvers/imstkNewtonSolver.cpp b/Source/Solvers/imstkNewtonSolver.cpp
index 7632722119b1635cff0efc2ee94d6dd36e8faf57..28bf71a30b630c900b32351805baa8206d6022eb 100644
--- a/Source/Solvers/imstkNewtonSolver.cpp
+++ b/Source/Solvers/imstkNewtonSolver.cpp
@@ -30,7 +30,7 @@
namespace imstk
{
-template <>
+template<>
NewtonSolver<SparseMatrixd>::NewtonSolver() :
m_linearSolver(std::make_shared<ConjugateGradient>()),
m_forcingTerm(0.9),
@@ -43,7 +43,7 @@ NewtonSolver<SparseMatrixd>::NewtonSolver() :
{
}
-template <>
+template<>
NewtonSolver<Matrixd>::NewtonSolver() :
m_linearSolver(std::make_shared<DirectLinearSolver<Matrixd>>()),
m_forcingTerm(0.9),
@@ -56,8 +56,7 @@ NewtonSolver<Matrixd>::NewtonSolver() :
{
}
-
-template <typename SystemMatrix>
+template<typename SystemMatrix>
void
NewtonSolver<SystemMatrix>::solveGivenState(Vectord& x)
{
@@ -101,7 +100,7 @@ NewtonSolver<SystemMatrix>::solveGivenState(Vectord& x)
}
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
void
NewtonSolver<SystemMatrix>::solve()
{
@@ -112,14 +111,14 @@ NewtonSolver<SystemMatrix>::solve()
}
size_t iterNum;
- const auto& u = this->m_nonLinearSystem->getUnknownVector();
- Vectord du = u; // make this a class member in future
- double error0 = MAX_D;
+ const auto& u = this->m_nonLinearSystem->getUnknownVector();
+ Vectord du = u; // make this a class member in future
+ double error0 = MAX_D;
double epsilon = m_relativeTolerance * m_relativeTolerance;
for (iterNum = 0; iterNum < m_maxIterations; ++iterNum)
{
- double error = updateJacobian(u);
+ double error = updateJacobian(u);
if (iterNum == 0)
{
@@ -144,7 +143,7 @@ NewtonSolver<SystemMatrix>::solve()
}
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
double
NewtonSolver<SystemMatrix>::updateJacobian(const Vectord& x)
{
@@ -171,7 +170,7 @@ NewtonSolver<SystemMatrix>::updateJacobian(const Vectord& x)
return std::sqrt(b.dot(b));
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
void
NewtonSolver<SystemMatrix>::updateForcingTerm(const double ratio, const double stopTolerance, const double fnorm)
{
@@ -188,36 +187,28 @@ NewtonSolver<SystemMatrix>::updateForcingTerm(const double ratio, const double s
m_forcingTerm = std::max(std::min(eta, m_etaMax), 0.5 * stopTolerance / fnorm);
}
-template <typename SystemMatrix>
-void
-NewtonSolver<SystemMatrix>::setLinearSolver(std::shared_ptr<NewtonSolver::LinearSolverType> newLinearSolver)
-{
- m_linearSolver = newLinearSolver;
-}
-
-template <typename SystemMatrix>
+template<typename SystemMatrix>
// std::shared_ptr<NewtonSolver<SystemMatrix>::LinearSolverType>
auto
-NewtonSolver<SystemMatrix>::getLinearSolver() const -> std::shared_ptr<LinearSolverType>
+NewtonSolver<SystemMatrix>::getLinearSolver() const->std::shared_ptr<LinearSolverType>
{
return m_linearSolver;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
void
NewtonSolver<SystemMatrix>::setAbsoluteTolerance(const double aTolerance)
{
m_absoluteTolerance = aTolerance;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
double
NewtonSolver<SystemMatrix>::getAbsoluteTolerance() const
{
return m_absoluteTolerance;
}
-
template class NewtonSolver<SparseMatrixd>;
template class NewtonSolver<Matrixd>;
} // imstk
diff --git a/Source/Solvers/imstkNewtonSolver.h b/Source/Solvers/imstkNewtonSolver.h
index b06e0d977f2441602f3347abe80838107a1d1f30..2dded1606af66aec5fecab42a955738777bba738 100644
--- a/Source/Solvers/imstkNewtonSolver.h
+++ b/Source/Solvers/imstkNewtonSolver.h
@@ -33,7 +33,7 @@ namespace imstk
/// root of F or fails. Global convergence is achieved using a line search sub-process
/// and the Armijo rule
///
-template <typename SystemMatrix>
+template<typename SystemMatrix>
class NewtonSolver : public NonLinearSolver<SystemMatrix>
{
public:
@@ -74,7 +74,10 @@ public:
///
/// \param newLinearSolver Linear solver pointer
///
- void setLinearSolver(std::shared_ptr<LinearSolverType> newLinearSolver);
+ void setLinearSolver(std::shared_ptr<LinearSolverType> newLinearSolver)
+ {
+ m_linearSolver = newLinearSolver;
+ }
///
/// \brief Get LinearSolver
@@ -204,7 +207,7 @@ public:
virtual void setToSemiImplicit() override
{
this->m_isSemiImplicit = true;
- m_maxIterations = 1;
+ m_maxIterations = 1;
}
private:
diff --git a/Source/Solvers/imstkNonLinearSolver.cpp b/Source/Solvers/imstkNonLinearSolver.cpp
index c0f7f8b80454b93ae0af5609d1b68650c8b8b073..6eac4676c94c171ba1a6e480f8ee1160401c8ffa 100644
--- a/Source/Solvers/imstkNonLinearSolver.cpp
+++ b/Source/Solvers/imstkNonLinearSolver.cpp
@@ -24,7 +24,7 @@
namespace imstk
{
-template <typename SystemMatrix>
+template<typename SystemMatrix>
NonLinearSolver<SystemMatrix>::NonLinearSolver() : m_sigma(std::array<double, 2>
{
{ 0.1, 0.5 }
@@ -38,7 +38,7 @@ NonLinearSolver<SystemMatrix>::NonLinearSolver() : m_sigma(std::array<double, 2>
};
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
double
NonLinearSolver<SystemMatrix>::armijo(const Vectord& dx, Vectord& x, const double previousFnorm)
{
@@ -99,7 +99,7 @@ NonLinearSolver<SystemMatrix>::armijo(const Vectord& dx, Vectord& x, const doubl
return currentFnorm;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
void
NonLinearSolver<SystemMatrix>::parabolicModel(const std::array<double, 3>& fnorm, std::array<double, 3>& lambda)
{
@@ -134,69 +134,62 @@ NonLinearSolver<SystemMatrix>::parabolicModel(const std::array<double, 3>& fnorm
lambda[0] = newLambda;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
void
NonLinearSolver<SystemMatrix>::setSigma(const std::array<double, 2>& newSigma)
{
m_sigma = newSigma;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
const std::array<double, 2>&
NonLinearSolver<SystemMatrix>::getSigma() const
{
return m_sigma;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
void
NonLinearSolver<SystemMatrix>::setAlpha(const double newAlpha)
{
m_alpha = newAlpha;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
double
NonLinearSolver<SystemMatrix>::getAlpha() const
{
return m_alpha;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
void
NonLinearSolver<SystemMatrix>::setArmijoMax(const size_t newArmijoMax)
{
m_armijoMax = newArmijoMax;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
size_t
NonLinearSolver<SystemMatrix>::getArmijoMax() const
{
return m_armijoMax;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
void
NonLinearSolver<SystemMatrix>::setSystem(std::shared_ptr<NonLinearSystem<SystemMatrix>> newSystem)
{
m_nonLinearSystem = newSystem;
}
-template <typename SystemMatrix>
+template<typename SystemMatrix>
std::shared_ptr<NonLinearSystem<SystemMatrix>>
NonLinearSolver<SystemMatrix>::getSystem() const
{
return m_nonLinearSystem;
}
-template <typename SystemMatrix>
-void
-NonLinearSolver<SystemMatrix>::setUpdateIterate(const NonLinearSolver<SystemMatrix>::UpdateIterateType& newUpdateIterate)
-{
- m_updateIterate = newUpdateIterate;
-}
-
template class NonLinearSolver<SparseMatrixd>;
template class NonLinearSolver<Matrixd>;
} // imstk
diff --git a/Source/Solvers/imstkNonLinearSolver.h b/Source/Solvers/imstkNonLinearSolver.h
index 3902fb310a54ca81f1e11733ed556ce8efcc15de..f576ea458b2376f5c8574c01976adee1bf2af6ef 100644
--- a/Source/Solvers/imstkNonLinearSolver.h
+++ b/Source/Solvers/imstkNonLinearSolver.h
@@ -35,7 +35,7 @@ namespace imstk
///
/// \brief Base class for non-linear solvers
///
-template <typename SystemMatrix>
+template<typename SystemMatrix>
class NonLinearSolver : public SolverBase
{
public:
@@ -114,7 +114,10 @@ public:
///
/// \param newUpdateIterate Function used to update iterates. Default: x+=dx.
///
- void setUpdateIterate(const UpdateIterateType& newUpdateIterate);
+ void setUpdateIterate(const UpdateIterateType& newUpdateIterate)
+ {
+ m_updateIterate = newUpdateIterate;
+ }
///
/// \brief Set the Newton solver to be fully implicit
@@ -133,12 +136,12 @@ public:
}
protected:
- std::array<double, 2> m_sigma; ///< Safeguarding bounds for the line search
- double m_alpha; ///< Parameter to measure decrease
- size_t m_armijoMax; ///< Maximum number of step length reductions
+ std::array<double, 2> m_sigma; ///< Safeguarding bounds for the line search
+ double m_alpha; ///< Parameter to measure decrease
+ size_t m_armijoMax; ///< Maximum number of step length reductions
std::shared_ptr<NonLinearSystem<SystemMatrix>> m_nonLinearSystem; ///< System of non-linear equations
- UpdateIterateType m_updateIterate; ///< Update iteration function
- bool m_isSemiImplicit = true; ///> Semi-Implicit solver
+ UpdateIterateType m_updateIterate; ///< Update iteration function
+ bool m_isSemiImplicit = true; ///> Semi-Implicit solver
};
} // imstk
diff --git a/Source/Solvers/imstkNonLinearSystem.cpp b/Source/Solvers/imstkNonLinearSystem.cpp
index 92c688c54b8a1ac4b4e0409814c7fdfb58b6bc80..0ce7bc971b8f0d85199110ca511ba94aad6d975f 100644
--- a/Source/Solvers/imstkNonLinearSystem.cpp
+++ b/Source/Solvers/imstkNonLinearSystem.cpp
@@ -26,33 +26,33 @@
namespace imstk
{
-template <typename Matrix>
+template<typename Matrix>
NonLinearSystem<Matrix>::NonLinearSystem(const VectorFunctionType& F, const MatrixFunctionType& dF) : m_F(F), m_dF(dF)
{
}
-template <typename Matrix>
+template<typename Matrix>
void
NonLinearSystem<Matrix>::setFunction(const VectorFunctionType& function)
{
m_F = function;
}
-template <typename Matrix>
+template<typename Matrix>
void
NonLinearSystem<Matrix>::setJacobian(const MatrixFunctionType& function)
{
m_dF = function;
}
-template <typename Matrix>
+template<typename Matrix>
const Vectord&
NonLinearSystem<Matrix>::evaluateF(const Vectord& x, const bool isSemiImplicit)
{
return m_F(x, isSemiImplicit);
}
-template <typename Matrix>
+template<typename Matrix>
const Matrix&
NonLinearSystem<Matrix>::evaluateJacobian(const Vectord& x)
{
@@ -61,5 +61,4 @@ NonLinearSystem<Matrix>::evaluateJacobian(const Vectord& x)
template class NonLinearSystem<SparseMatrixd>;
template class NonLinearSystem<Matrixd>;
-
} //imstk
diff --git a/Source/Solvers/imstkNonLinearSystem.h b/Source/Solvers/imstkNonLinearSystem.h
index d631063b9b287bb562a671059b09ea35e92ca1bf..2a49cc24362b31b3c083113b11f83c76d012bac7 100644
--- a/Source/Solvers/imstkNonLinearSystem.h
+++ b/Source/Solvers/imstkNonLinearSystem.h
@@ -31,7 +31,7 @@ namespace imstk
///
/// \brief Base class for a multi-variable nonlinear system
///
-template <typename Matrix>
+template<typename Matrix>
class NonLinearSystem
{
public:
diff --git a/Source/Solvers/imstkPbdSolver.cpp b/Source/Solvers/imstkPbdSolver.cpp
index 83cbc529fbc3b47bff087936aacd81d2a1bd4bd2..a03ff5351351d8448fa6371394dc1600eaaffe29 100644
--- a/Source/Solvers/imstkPbdSolver.cpp
+++ b/Source/Solvers/imstkPbdSolver.cpp
@@ -19,39 +19,111 @@
=========================================================================*/
-#include "imstkPbdObject.h"
#include "imstkPbdSolver.h"
-#include "imstkPbdCollisionConstraint.h"
#include "imstkParallelUtils.h"
namespace imstk
{
+PbdSolver::PbdSolver() :
+ m_partitionedConstraints(std::make_shared<std::vector<PBDConstraintVector>>()),
+ m_constraints(std::make_shared<PBDConstraintVector>()),
+ m_positions(std::make_shared<StdVectorOfVec3d>()),
+ m_invMasses(std::make_shared<StdVectorOfReal>())
+{
+}
+
void
PbdSolver::solve()
{
- m_pbdObject->integratePosition();
- m_pbdObject->solveConstraints();
- resolveCollisionConstraints();
- m_pbdObject->updateVelocity();
+ // Solve the constraints and partitioned constraints
+ StdVectorOfVec3d& currPositions = *m_positions;
+ const StdVectorOfReal& invMasses = *m_invMasses;
+
+ const PBDConstraintVector& constraints = *m_constraints;
+ const std::vector<PBDConstraintVector>& partitionedConstraints = *m_partitionedConstraints;
+
+ // zero out the Lagrange multiplier
+ for (size_t j = 0; j < constraints.size(); ++j)
+ {
+ constraints[j]->zeroOutLambda();
+ }
+
+ for (size_t j = 0; j < partitionedConstraints.size(); j++)
+ {
+ const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
+ ParallelUtils::parallelFor(constraintPartition.size(),
+ [&](const size_t idx) { constraintPartition[idx]->zeroOutLambda(); }
+ );
+ }
+
+ unsigned int i = 0;
+ while (i++ < m_iterations)
+ {
+ for (size_t j = 0; j < constraints.size(); ++j)
+ {
+ constraints[j]->projectConstraint(invMasses, m_dt, m_solverType, currPositions);
+ }
+
+ for (size_t j = 0; j < partitionedConstraints.size(); j++)
+ {
+ const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
+
+ ParallelUtils::parallelFor(constraintPartition.size(),
+ [&](const size_t idx)
+ {
+ constraintPartition[idx]->projectConstraint(invMasses, m_dt, m_solverType, currPositions);
+ });
+ // Sequential
+ /*for (size_t k = 0; k < constraintPartition.size(); k++)
+ {
+ constraintPartition[k]->solvePositionConstraint(currPositions, invMasses);
+ }*/
+ }
+ }
+}
+
+PbdCollisionSolver::PbdCollisionSolver() :
+ m_collisionConstraints(std::make_shared<std::list<PBDCollisionConstraintVector*>>()),
+ m_collisionConstraintsData(std::make_shared<std::list<CollisionConstraintData>>())
+{
+}
+
+void
+PbdCollisionSolver::addCollisionConstraints(PBDCollisionConstraintVector* constraints,
+ std::shared_ptr<StdVectorOfVec3d> posA, std::shared_ptr<StdVectorOfReal> invMassA,
+ std::shared_ptr<StdVectorOfVec3d> posB, std::shared_ptr<StdVectorOfReal> invMassB)
+{
+ m_collisionConstraints->push_back(constraints);
+ m_collisionConstraintsData->push_back({ posA, invMassA, posB, invMassB });
}
void
-PbdSolver::resolveCollisionConstraints()
+PbdCollisionSolver::solve()
{
- if (m_PBDConstraints.size() > 0)
+ // Solve collision constraints
+ if (m_collisionConstraints->size() > 0)
{
- uint32_t maxIter = 3u;
- uint32_t i = 0;
- while (++i < maxIter)
+ unsigned int i = 0;
+ while (i++ < m_collisionIterations)
{
- for (const auto constraintList : m_PBDConstraints)
+ std::list<CollisionConstraintData>::iterator colDataIter = m_collisionConstraintsData->begin();
+ for (auto constraintList : *m_collisionConstraints)
{
- for (size_t k = 0; k < constraintList->size(); ++k)
+ CollisionConstraintData colData = *colDataIter;
+ StdVectorOfVec3d& posA = *colData.m_posA;
+ const StdVectorOfReal& invMassA = *colData.m_invMassA;
+ StdVectorOfVec3d& posB = *colData.m_posB;
+ const StdVectorOfReal& invMassB = *colData.m_invMassB;
+ const PBDCollisionConstraintVector& constraints = *constraintList;
+ for (size_t j = 0; j < constraints.size(); j++)
{
- (*constraintList)[k]->solvePositionConstraint();
+ constraints[j]->projectConstraint(invMassA, invMassB, posA, posB);
}
+ colDataIter++;
}
}
+ m_collisionConstraints->clear();
+ m_collisionConstraintsData->clear();
}
}
} // end namespace imstk
diff --git a/Source/Solvers/imstkPbdSolver.h b/Source/Solvers/imstkPbdSolver.h
index ef20dbdf5ce286c95b4bdf42810f46a5de65713a..f7012d6b5766bd9e6315db03f85812299793909b 100644
--- a/Source/Solvers/imstkPbdSolver.h
+++ b/Source/Solvers/imstkPbdSolver.h
@@ -21,67 +21,143 @@
#pragma once
+#include "imstkPbdCollisionConstraint.h"
+#include "imstkPbdConstraint.h"
#include "imstkSolverBase.h"
-#include "imstkGraph.h"
-
-#include <unordered_set>
+#include "imstkLogger.h"
namespace imstk
{
-class PbdObject;
-class PbdCollisionConstraint;
+struct CollisionConstraintData
+{
+ CollisionConstraintData(std::shared_ptr<StdVectorOfVec3d> posA,
+ std::shared_ptr<StdVectorOfReal> invMassA,
+ std::shared_ptr<StdVectorOfVec3d> posB,
+ std::shared_ptr<StdVectorOfReal> invMassB) :
+ m_posA(posA), m_invMassA(invMassA), m_posB(posB), m_invMassB(invMassB)
+ {
+ }
+
+ std::shared_ptr<StdVectorOfVec3d> m_posA = nullptr;
+ std::shared_ptr<StdVectorOfReal> m_invMassA = nullptr;
+ std::shared_ptr<StdVectorOfVec3d> m_posB = nullptr;
+ std::shared_ptr<StdVectorOfReal> m_invMassB = nullptr;
+};
+
///
/// \class PbdSolver
///
/// \brief Position Based Dynamics solver
+/// This solver can operate on both partitioned constraints (unordered_set of vector'd constraints) in parallel
+/// and sequentially on vector'd constraints. It requires a set of constraints, positions, and invMasses.
///
class PbdSolver : public SolverBase
{
-using PBDConstraintVector = std::vector<PbdCollisionConstraint*>;
-
public:
///
/// \brief Constructors/Destructor
///
- PbdSolver() = default;
+ PbdSolver();
virtual ~PbdSolver() override = default;
- PbdSolver(const PbdSolver& other) = delete;
- PbdSolver& operator=(const PbdSolver& other) = delete;
+public:
+ ///
+ /// \brief Set Iterations. The number of nonlinear iterations.
+ ///
+ void setIterations(const size_t iterations) { this->m_iterations = iterations; }
+
+ ///
+ /// \brief Sets the partioned constraints the solver should solve for
+ /// These will be solved in parallel
+ ///
+ void setPartitionedConstraints(std::shared_ptr<std::vector<PBDConstraintVector>> partitionedConstraints) { this->m_partitionedConstraints = partitionedConstraints; }
+
+ ///
+ /// \brief Sets the constraints the solver should solve for
+ /// These wil be solved sequentially
+ ///
+ void setConstraints(std::shared_ptr<PBDConstraintVector> constraints) { this->m_constraints = constraints; }
///
- /// \brief Set MaxIterations. The maximum number of nonlinear iterations.
+ /// \brief Sets the positions the solver should solve with
///
- void setMaxIterations(const size_t newMaxIterations) { this->m_maxIterations = newMaxIterations; }
+ void setPositions(std::shared_ptr<StdVectorOfVec3d> positions) { this->m_positions = positions; }
///
- /// \brief Get MaxIterations. Returns current maximum nonlinear iterations.
+ /// \brief Sets the invMasses the solver should solve with
///
- size_t getMaxIterations() const { return this->m_maxIterations; }
+ void setInvMasses(std::shared_ptr<StdVectorOfReal> invMasses) { this->m_invMasses = invMasses; }
///
- /// \brief
+ /// \brief Set time step
///
- void setPbdObject(const std::shared_ptr<PbdObject>& pbdObj) { m_pbdObject = pbdObj; }
+ void setTimeStep(const double dt) { m_dt = dt; }
+
+ ///
+ /// \brief Get Iterations. Returns current nonlinear iterations.
+ ///
+ size_t getIterations() const { return this->m_iterations; }
+
+ ///
+ /// \brief Set the PBD solver type
+ ///
+ void setSolverType(const PbdConstraint::SolverType& type)
+ {
+ if (type == PbdConstraint::SolverType::GCD)
+ {
+ LOG(WARNING) << "GCD is NOT implemented yet, use xPBD instead";
+ m_solverType = PbdConstraint::SolverType::xPBD;
+ return;
+ }
+
+ m_solverType = type;
+ }
///
/// \brief Solve the non linear system of equations G(x)=0 using Newton's method.
///
void solve() override;
+private:
+ size_t m_iterations = 20; ///> Number of NL Gauss-Seidel iterations for regular constraints
+ double m_dt; ///> time step
+
+ std::shared_ptr<std::vector<PBDConstraintVector>> m_partitionedConstraints = nullptr; ///> Set of vector'd/partitioned pbd constraints
+ std::shared_ptr<PBDConstraintVector> m_constraints = nullptr; ///> Vector of constraints
+
+ std::shared_ptr<StdVectorOfVec3d> m_positions = nullptr;
+ std::shared_ptr<StdVectorOfReal> m_invMasses = nullptr;
+ PbdConstraint::SolverType m_solverType = PbdConstraint::SolverType::xPBD;
+};
+
+class PbdCollisionSolver : SolverBase
+{
+public:
+ PbdCollisionSolver();
+ virtual ~PbdCollisionSolver() override = default;
+
+public:
+ ///
+ /// \brief Get CollisionIterations
+ ///
+ size_t getCollisionIterations() const { return this->m_collisionIterations; }
+
///
/// \brief Add the global collision contraints to this solver
///
- void addCollisionConstraints(PBDConstraintVector* constraints) { m_PBDConstraints.insert(constraints); }
+ void addCollisionConstraints(PBDCollisionConstraintVector* constraints,
+ std::shared_ptr<StdVectorOfVec3d> posA, std::shared_ptr<StdVectorOfReal> invMassA,
+ std::shared_ptr<StdVectorOfVec3d> posB, std::shared_ptr<StdVectorOfReal> invMassB);
///
- /// \brief Solve the global collision contraints charged to this solver
+ /// \brief Solve the non linear system of equations G(x)=0 using Newton's method.
///
- void resolveCollisionConstraints();
+ void solve() override;
private:
- size_t m_maxIterations = 20; ///< Maximum number of NL Gauss-Seidel iterations
- std::unordered_set<PBDConstraintVector*> m_PBDConstraints; ///< Collision contraints charged to this solver
- std::shared_ptr<PbdObject> m_pbdObject;
+ size_t m_collisionIterations = 5; ///> Number of NL Gauss-Seidel iterations for collision constraints
+
+ std::shared_ptr<std::list<PBDCollisionConstraintVector*>> m_collisionConstraints = nullptr; ///< Collision contraints charged to this solver
+ std::shared_ptr<std::list<CollisionConstraintData>> m_collisionConstraintsData = nullptr;
};
} // imstk
diff --git a/Source/Solvers/imstkSPHSolver.h b/Source/Solvers/imstkSPHSolver.h
deleted file mode 100644
index 58f08edf73e468683864cbaebae71588208f4ae7..0000000000000000000000000000000000000000
--- a/Source/Solvers/imstkSPHSolver.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & 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.txt
-
- 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.
-
-=========================================================================*/
-
-#pragma once
-
-#include "imstkSolverBase.h"
-#include "imstkSPHObject.h"
-#include "imstkSPHModel.h"
-
-namespace imstk
-{
-///
-/// \class SPHSolver
-/// \brief SPH solver
-///
-class SPHSolver : public SolverBase
-{
-public:
- SPHSolver() = default;
- virtual ~SPHSolver() override = default;
-
- SPHSolver(const SPHSolver& other) = delete;
- SPHSolver& operator=(const SPHSolver& other) = delete;
-
- ///
- /// \brief Set the simulation object
- ///
- void setSPHObject(const std::shared_ptr<SPHObject>& obj)
- { assert(obj); m_SPHObject = obj; }
-
- ///
- /// \brief Advance one time step
- ///
- virtual void solve() override
- {
- const auto& SPHModel = m_SPHObject->getSPHModel();
- LOG_IF(FATAL, (!SPHModel)) << "SPH model has not been initialized";
- SPHModel->advanceTimeStep();
- }
-
-private:
- std::shared_ptr<SPHObject> m_SPHObject;
-};
-} // end namespace imstk
diff --git a/Source/Solvers/imstkSolverBase.h b/Source/Solvers/imstkSolverBase.h
index 4db160b9ad3e75938bad0a36a5e7c82219b21992..6249e0b0c412196e331a2b49fc52188f4d398343 100644
--- a/Source/Solvers/imstkSolverBase.h
+++ b/Source/Solvers/imstkSolverBase.h
@@ -28,11 +28,10 @@ namespace imstk
///
class SolverBase
{
-public:
- ///
- /// \brief Default Constructor/Destructor
- ///
+protected:
SolverBase() = default;
+
+public:
virtual ~SolverBase() = default;
virtual void solve() = 0;
diff --git a/Source/apiUtilities/imstkAPIUtilities.cpp b/Source/apiUtilities/imstkAPIUtilities.cpp
index 82bfcb51e895fd2093cfd7cf062fa32cf12ff387..3dcf882c6757e8e78c1157ca62b889715539bc17 100644
--- a/Source/apiUtilities/imstkAPIUtilities.cpp
+++ b/Source/apiUtilities/imstkAPIUtilities.cpp
@@ -210,6 +210,7 @@ printUPS(std::shared_ptr<SceneManager> sceneManager)
return;
}
+ sceneManager->enableFrameCount();
sceneManager->setPostUpdateCallback([&sceneManager](Module* module)
{
std::cout << "\r" << module->getName() << " running at "
diff --git a/Source/apiUtilities/imstkAPIUtilities.h b/Source/apiUtilities/imstkAPIUtilities.h
index f39515b79aa620b9c7a23580a178117a2fe608cc..8a929f125554ba9c91a75f12b5e0320696cfa25e 100644
--- a/Source/apiUtilities/imstkAPIUtilities.h
+++ b/Source/apiUtilities/imstkAPIUtilities.h
@@ -33,7 +33,7 @@ class PointSet;
class SceneManager;
class Scene;
class FEMDeformableBodyModel;
-template <typename Matrix>
+template<typename Matrix>
class NonLinearSystem;
class CollidingObject;
class Graph;