diff --git a/Examples/FastMarch/CMakeLists.txt b/Examples/FastMarch/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f84730ed20fddef5080b95d5f841b80d0b202966
--- /dev/null
+++ b/Examples/FastMarch/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-FastMarch)
+
+#-----------------------------------------------------------------------------
+# Create executable
+#-----------------------------------------------------------------------------
+imstk_add_executable(${PROJECT_NAME} FastMarchExample.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 Filtering)
diff --git a/Examples/FastMarch/FastMarchExample.cpp b/Examples/FastMarch/FastMarchExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef005ed1a9fcce6a7148bd1ab3681783735b442a
--- /dev/null
+++ b/Examples/FastMarch/FastMarchExample.cpp
@@ -0,0 +1,122 @@
+/*=========================================================================
+
+ 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 "imstkCamera.h"
+#include "imstkDataArray.h"
+#include "imstkFastMarch.h"
+#include "imstkKeyboardSceneControl.h"
+#include "imstkLogger.h"
+#include "imstkMeshIO.h"
+#include "imstkMouseSceneControl.h"
+#include "imstkNew.h"
+#include "imstkScene.h"
+#include "imstkSceneManager.h"
+#include "imstkSceneObject.h"
+#include "imstkSimulationManager.h"
+#include "imstkVisualModel.h"
+#include "imstkVolumeRenderMaterial.h"
+#include "imstkVTKViewer.h"
+
+#include <vtkVolumeProperty.h>
+#include <vtkColorTransferFunction.h>
+#include <vtkPiecewiseFunction.h>
+
+using namespace imstk;
+
+///
+/// \brief This example demonstrates the volume renderer
+///
+int
+main()
+{
+ Logger::startLogger();
+
+ // SDK and Scene
+ imstkNew<Scene> scene("VolumeRendering");
+
+ // Create some blank image
+ imstkNew<ImageData> image;
+ image->allocate(IMSTK_DOUBLE, 1, Vec3i(50, 50, 50));
+ std::shared_ptr<DataArray<double>> scalars = std::dynamic_pointer_cast<DataArray<double>>(image->getScalars());
+ scalars->fill(0.0);
+
+ // Seed voxels
+ imstkNew<FastMarch> fastMarch;
+ fastMarch->setDistThreshold(5.0);
+ fastMarch->setImage(image);
+ std::vector<Vec3i> seeds = { Vec3i(25, 25, 25) };
+ fastMarch->setSeeds(seeds);
+ fastMarch->solve();
+
+ // Create a visual object in the scene for the volume
+ imstkNew<SceneObject> volumeObj("VisualVolume");
+ volumeObj->setVisualGeometry(image);
+ scene->addSceneObject(volumeObj);
+
+ imstkNew<VolumeRenderMaterial> volumeMaterial;
+ vtkNew<vtkColorTransferFunction> colorFunc;
+ colorFunc->AddRGBPoint(0.0, 1.0, 0.0, 0.0);
+ colorFunc->AddRGBPoint(8.0, 0.0, 0.0, 1.0);
+ volumeMaterial->getVolumeProperty()->SetColor(colorFunc);
+ vtkNew<vtkPiecewiseFunction> opacityFunc;
+ opacityFunc->AddPoint(0.0, 0.0);
+ opacityFunc->AddPoint(1.0, 1.0);
+ volumeMaterial->getVolumeProperty()->SetScalarOpacity(opacityFunc);
+ // Apply the preset to the visual object
+ volumeObj->getVisualModel(0)->setRenderMaterial(volumeMaterial);
+
+ scene->getActiveCamera()->setPosition(Vec3d(0.0, -100.0, -100.0));
+ scene->getActiveCamera()->setFocalPoint(Vec3d(25.0, 25.0, 25.0));
+ scene->getActiveCamera()->setViewUp(Vec3d(0.0, 1.0, 0.0));
+
+ // Run the simulation
+ {
+ imstkNew<VTKViewer> viewer("Viewer");
+ viewer->setActiveScene(scene);
+
+ // Setup a scene manager to advance the scene in its own thread
+ imstkNew<SceneManager> sceneManager("Scene Manager");
+ sceneManager->setActiveScene(scene);
+ sceneManager->setExecutionType(Module::ExecutionType::ADAPTIVE);
+
+ imstkNew<SimulationManager> driver;
+ driver->addModule(viewer);
+ driver->addModule(sceneManager);
+
+ // Add mouse and keyboard controls to the viewer
+ {
+ imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice());
+ mouseControl->setSceneManager(sceneManager);
+ viewer->addControl(mouseControl);
+
+ imstkNew<KeyboardSceneControl> keyControl(viewer->getKeyboardDevice());
+ keyControl->setSceneManager(sceneManager);
+ keyControl->setModuleDriver(driver);
+ viewer->addControl(keyControl);
+ }
+
+ driver->start();
+ }
+
+ MeshIO::write(image, "results.nii");
+
+ return 0;
+}
diff --git a/Examples/FemurCut/FemurCutExample.cpp b/Examples/FemurCut/FemurCutExample.cpp
index 199f61cf451381135995bb6cb7a66e82b0eddb20..b7926be9d31ed8ca0e94ecf526a18779b03cd549 100644
--- a/Examples/FemurCut/FemurCutExample.cpp
+++ b/Examples/FemurCut/FemurCutExample.cpp
@@ -25,6 +25,7 @@
#include "imstkHapticDeviceManager.h"
#include "imstkImageData.h"
#include "imstkIsometricMap.h"
+#include "imstkKeyboardDeviceClient.h"
#include "imstkKeyboardSceneControl.h"
#include "imstkLevelSetCH.h"
#include "imstkLevelSetDeformableObject.h"
@@ -50,6 +51,8 @@
#include "imstkVolumeRenderMaterial.h"
#include "imstkVTKViewer.h"
+#include <vtkObject.h>
+
using namespace imstk;
using namespace imstk::expiremental;
@@ -61,27 +64,21 @@ makeLevelsetObj(const std::string& name, std::shared_ptr<LocalMarchingCubes> iso
{
imstkNew<LevelSetDeformableObject> levelsetObj(name);
- std::shared_ptr<ImageData> initLvlsetImage = MeshIO::read<ImageData>(iMSTK_DATA_ROOT "/legs/femurBone_SDF.nii")->cast(IMSTK_DOUBLE);
- const Vec3d& currSpacing = initLvlsetImage->getSpacing();
+ std::shared_ptr<ImageData> initLvlSetImage = MeshIO::read<ImageData>("C:/Users/Andx_/Desktop/femurBoneSolid_SDF.nii")->cast(IMSTK_DOUBLE);
+ const Vec3d& currSpacing = initLvlSetImage->getSpacing();
+
// Note: Anistropic scaling would invalidate the SDF
- initLvlsetImage->setSpacing(currSpacing * 0.001);
- initLvlsetImage->setOrigin(Vec3d(0.0, 0.8, 1.5));
+ initLvlSetImage->setOrigin(Vec3d(0.0, 0.8, 1.5));
// Setup the Parameters
imstkNew<LevelSetModelConfig> lvlSetConfig;
lvlSetConfig->m_sparseUpdate = true;
- lvlSetConfig->m_dt = 0.0001;
- //lvlSetConfig->m_k = 0.8;
-
- // Setup the Model
- imstkNew<LevelSetModel> model;
- model->setModelGeometry(initLvlsetImage);
- model->configure(lvlSetConfig);
+ lvlSetConfig->m_substeps = 30;
// Too many chunks and you'll hit memory constraints quickly
// Too little chunks and the updates for a chunk will take too long
// The chunks must divide the image dimensions (image dim-1 must be divisible by # chunks)
- isoExtract->setInputImage(initLvlsetImage);
+ isoExtract->setInputImage(initLvlSetImage);
isoExtract->setIsoValue(0.0);
isoExtract->setNumberOfChunks(Vec3i(32, 9, 9));
isoExtract->update();
@@ -103,6 +100,7 @@ makeLevelsetObj(const std::string& name, std::shared_ptr<LocalMarchingCubes> iso
const double b = (rand() % 500) / 500.0;
material->setColor(Color(r, g, b));
material->setEdgeColor(Color::Color::Orange);
+ //material->setOpacity(0.7);
surfMeshModel->setRenderMaterial(material);
levelsetObj->addVisualModel(surfMeshModel);
chunksGenerated.insert(i);
@@ -110,10 +108,14 @@ makeLevelsetObj(const std::string& name, std::shared_ptr<LocalMarchingCubes> iso
}
// Setup the Object
- levelsetObj->setPhysicsGeometry(initLvlsetImage);
- std::shared_ptr<SignedDistanceField> sdf = std::make_shared<SignedDistanceField>(initLvlsetImage);
- // Since we scaled the image spacing we should also scale SDF
- sdf->setScale(0.001);
+ imstkNew<SignedDistanceField> sdf(initLvlSetImage);
+
+ // Setup the Model
+ imstkNew<LevelSetModel> model;
+ model->setModelGeometry(sdf);
+ model->configure(lvlSetConfig);
+
+ levelsetObj->setPhysicsGeometry(sdf);
levelsetObj->setCollidingGeometry(sdf);
levelsetObj->setDynamicalModel(model);
@@ -124,27 +126,28 @@ std::shared_ptr<RigidObject2>
makeRigidObj(const std::string& name)
{
imstkNew<RigidBodyModel2> rbdModel;
- rbdModel->getConfig()->m_dt = 0.001;
- rbdModel->getConfig()->m_maxNumIterations = 5;
+ rbdModel->getConfig()->m_maxNumIterations = 8;
rbdModel->getConfig()->m_velocityDamping = 1.0;
rbdModel->getConfig()->m_angularVelocityDamping = 1.0;
- rbdModel->getConfig()->m_maxNumConstraints = 25;
+ rbdModel->getConfig()->m_maxNumConstraints = 20;
// Create the first rbd, plane floor
imstkNew<RigidObject2> rigidObj("Cube");
{
- auto toolMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scalpel/Scalpel_Hull_Subdivided.stl");
+ auto toolMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scalpel/Scalpel_Hull_Subdivided3.stl");
toolMesh->rotate(Vec3d(0.0, 1.0, 0.0), 3.14, Geometry::TransformType::ApplyToData);
toolMesh->rotate(Vec3d(1.0, 0.0, 0.0), -1.57, Geometry::TransformType::ApplyToData);
toolMesh->scale(Vec3d(0.07, 0.07, 0.07), Geometry::TransformType::ApplyToData);
- auto toolVisualMeshHandle = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scalpel/Scalpel_Handle.dae");
+ //auto toolVisualMeshHandle = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scalpel/Scalpel_Handle.dae");
+ auto toolVisualMeshHandle = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scalpel/Scalpel_Hull_Subdivided3.stl");
toolVisualMeshHandle->rotate(Vec3d(0.0, 1.0, 0.0), 3.14, Geometry::TransformType::ApplyToData);
toolVisualMeshHandle->rotate(Vec3d(1.0, 0.0, 0.0), -1.57, Geometry::TransformType::ApplyToData);
toolVisualMeshHandle->scale(Vec3d(0.07, 0.07, 0.07), Geometry::TransformType::ApplyToData);
- auto toolVisualMeshBlade = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scalpel/Scalpel_Blade10.dae");
+ //auto toolVisualMeshBlade = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scalpel/Scalpel_Blade10.dae");
+ auto toolVisualMeshBlade = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scalpel/Scalpel_Hull_Subdivided3.stl");
toolVisualMeshBlade->rotate(Vec3d(0.0, 1.0, 0.0), 3.14, Geometry::TransformType::ApplyToData);
toolVisualMeshBlade->rotate(Vec3d(1.0, 0.0, 0.0), -1.57, Geometry::TransformType::ApplyToData);
toolVisualMeshBlade->scale(Vec3d(0.07, 0.07, 0.07), Geometry::TransformType::ApplyToData);
@@ -154,8 +157,6 @@ makeRigidObj(const std::string& name)
toolMaterial->setShadingModel(RenderMaterial::ShadingModel::PBR);
toolMaterial->setMetalness(0.9f);
toolMaterial->setRoughness(0.4f);
- //toolMaterial->setDisplayMode(RenderMaterial::DisplayMode::Points);
- //toolMaterial->setPointSize(15.0);
toolMaterial->setDiffuseColor(Color(0.7, 0.7, 0.7));
imstkNew<VisualModel> visualModel1(toolVisualMeshHandle);
@@ -173,9 +174,8 @@ makeRigidObj(const std::string& name)
// Hack to add two maps
rigidObj->setPhysicsToCollidingMap(std::make_shared<IsometricMap>(toolMesh, toolVisualMeshBlade));
rigidObj->setDynamicalModel(rbdModel);
- rigidObj->getRigidBody()->m_mass = 1000.0;
- //rigidObj->getRigidBody()->setInertiaFromPointSet(toolMesh, 0.01, false);
- rigidObj->getRigidBody()->m_intertiaTensor = Mat3d::Identity() * 20.0;
+ rigidObj->getRigidBody()->m_mass = 1.0;
+ rigidObj->getRigidBody()->m_intertiaTensor = Mat3d::Identity() * 10000.0;
rigidObj->getRigidBody()->m_initPos = Vec3d(0.0, 1.0, 2.0);
}
return rigidObj;
@@ -189,6 +189,8 @@ makeRigidObj(const std::string& name)
int
main()
{
+ vtkObject::GlobalWarningDisplayOff();
+
// Setup logger (write to file and stdout)
Logger::startLogger();
@@ -212,14 +214,19 @@ main()
scene->addSceneObject(rbdGhostObj);
imstkNew<RigidObjectLevelSetCollisionPair> interaction(rbdObj, lvlSetObj);
- auto colHandlerA = std::dynamic_pointer_cast<RigidBodyCH>(interaction->getCollisionHandlingA());
- colHandlerA->setUseFriction(false);
- colHandlerA->setStiffness(0.0); // inelastic collision
- auto colHandlerB = std::dynamic_pointer_cast<LevelSetCH>(interaction->getCollisionHandlingB());
- colHandlerB->setLevelSetVelocityScaling(0.1);
- colHandlerB->setKernel(3, 1.0);
- //colHandlerB->setLevelSetVelocityScaling(0.0); // Can't push the levelset
- colHandlerB->setUseProportionalVelocity(false);
+ {
+ auto colHandlerA = std::dynamic_pointer_cast<RigidBodyCH>(interaction->getCollisionHandlingA());
+ colHandlerA->setUseFriction(false);
+ colHandlerA->setStiffness(0.05); // inelastic collision
+
+ auto colHandlerB = std::dynamic_pointer_cast<LevelSetCH>(interaction->getCollisionHandlingB());
+ colHandlerB->setLevelSetVelocityScaling(0.05);
+ colHandlerB->setKernel(3, 1.0);
+ //colHandlerB->setLevelSetVelocityScaling(0.0); // Can't push the levelset
+ colHandlerB->setUseProportionalVelocity(true);
+ }
+ std::shared_ptr<CollisionDetection> cd = interaction->getCollisionDetection();
+
scene->getCollisionGraph()->addInteraction(interaction);
// Light (white)
@@ -229,9 +236,9 @@ main()
scene->addLight(whiteLight);
// Adjust camera
- scene->getActiveCamera()->setFocalPoint(0.27, 0.74, 1.53);
- scene->getActiveCamera()->setPosition(0.17, 1.09, 1.89);
- scene->getActiveCamera()->setViewUp(0.17, 0.74, -0.65);
+ scene->getActiveCamera()->setFocalPoint(0.25, 0.83, 1.58);
+ scene->getActiveCamera()->setPosition(0.243, 1.06, 1.95);
+ scene->getActiveCamera()->setViewUp(0.05, 0.86, -0.51);
{
imstkNew<VTKViewer> viewer("Viewer");
@@ -243,29 +250,33 @@ main()
sceneManager->setExecutionType(Module::ExecutionType::ADAPTIVE);
imstkNew<HapticDeviceManager> hapticManager;
- std::shared_ptr<HapticDeviceClient> hapticDeviceClient = hapticManager->makeDeviceClient("Default Device");
+ std::shared_ptr<HapticDeviceClient> hapticDeviceClient = hapticManager->makeDeviceClient();
imstkNew<RigidObjectController> controller(rbdObj, hapticDeviceClient);
-
- controller->setLinearKd(100000.0);
- controller->setAngularKd(550.0);
- controller->setLinearKs(1000000.0);
- controller->setAngularKs(10000.0);
- controller->setForceScaling(0.0001);
-
- controller->setComputeVelocity(true); // The device we are using doesn't produce this quantity, with this flag its computed
- controller->setComputeAngularVelocity(true); // The device we are using doesn't produce this quantity, with this flag its computed
-
- controller->setTranslationScaling(0.0015);
- controller->setTranslationOffset(Vec3d(0.1, 0.9, 1.8));
-
- scene->addController(controller);
+ {
+ controller->setLinearKd(1000.0 * 0.9);
+ controller->setLinearKs(100000.0 * 0.9);
+ controller->setAngularKs(300000000.0);
+ controller->setAngularKd(400000.0);
+ controller->setForceScaling(0.001);
+
+ // The particular device we are using doesn't produce this quantity, with this flag its computed
+ // in code
+ controller->setComputeVelocity(true);
+ controller->setComputeAngularVelocity(true);
+
+ controller->setTranslationScaling(0.0015);
+ controller->setTranslationOffset(Vec3d(0.1, 0.9, 1.6));
+ controller->setSmoothingKernelSize(30);
+
+ scene->addController(controller);
+ }
connect<Event>(sceneManager->getActiveScene(), EventType::Configure, [&](Event*)
{
std::shared_ptr<TaskGraph> taskGraph = sceneManager->getActiveScene()->getTaskGraph();
- // Pipe the changes from the levelset into local marhcing cubes
+ // Pipe the changes from the levelset into local marching cubes
// Compute this before the levelset is evolved
taskGraph->insertBefore(lvlSetObj->getLevelSetModel()->getQuantityEvolveNode(0),
std::make_shared<TaskNode>([&]()
@@ -282,6 +293,7 @@ main()
isoExtract->update();
// Create meshes for chunks if they now contain vertices (and weren't already generated)
+ // You could just create all the chunks, but this saves some memory for internal/empty ones
const Vec3i& numChunks = isoExtract->getNumberOfChunks();
for (int i = 0; i < numChunks[0] * numChunks[1] * numChunks[2]; i++)
{
@@ -297,6 +309,7 @@ main()
const double b = (rand() % 500) / 500.0;
material->setColor(Color(r, g, b));
material->setEdgeColor(Color::Color::Orange);
+ //material->setOpacity(0.7);
surfMeshModel->setRenderMaterial(material);
lvlSetObj->addVisualModel(surfMeshModel);
chunksGenerated.insert(i);
@@ -305,7 +318,8 @@ main()
});
connect<Event>(sceneManager, EventType::PostUpdate, [&](Event*)
{
- rbdObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt();
+ rbdObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt();
+ lvlSetObj->getLevelSetModel()->getConfig()->m_dt = sceneManager->getDt();
// Also apply controller transform to ghost geometry
ghostMesh->setTranslation(controller->getPosition());
@@ -318,7 +332,7 @@ main()
driver->addModule(viewer);
driver->addModule(sceneManager);
driver->addModule(hapticManager);
- driver->setDesiredDt(0.0004);
+ driver->setDesiredDt(0.001); // Little over 1000ups
{
imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice());
diff --git a/Examples/RigidBodyDynamics2/RigidBodyDynamicsExample2.cpp b/Examples/RigidBodyDynamics2/RigidBodyDynamicsExample2.cpp
index 42865ce9222f7c440e45b446d738ec1179e05701..74fb6bbaf6621231632b512915aa5b854efee5b7 100644
--- a/Examples/RigidBodyDynamics2/RigidBodyDynamicsExample2.cpp
+++ b/Examples/RigidBodyDynamics2/RigidBodyDynamicsExample2.cpp
@@ -67,8 +67,8 @@ main()
{
// This model is shared among interacting rigid bodies
imstkNew<RigidBodyModel2> rbdModel;
- rbdModel->getConfig()->m_gravity = Vec3d(0.0, -900.0, 0.0);
- rbdModel->getConfig()->m_maxNumIterations = 40;
+ rbdModel->getConfig()->m_gravity = Vec3d(0.0, -2500.0, 0.0);
+ rbdModel->getConfig()->m_maxNumIterations = 10;
// Create the first rbd, plane floor
imstkNew<CollidingObject> planeObj("Plane");
@@ -150,7 +150,7 @@ main()
auto rbdInteraction = std::make_shared<RigidObjectCollidingCollisionPair>(cubeObj, planeObj, CollisionDetection::Type::PointSetToImplicit);
std::shared_ptr<RigidBodyCH> ch = std::dynamic_pointer_cast<RigidBodyCH>(rbdInteraction->getCollisionHandlingA());
ch->setUseFriction(false);
- ch->setStiffness(0.0); // Completely inelastic
+ ch->setStiffness(0.05);
scene->getCollisionGraph()->addInteraction(rbdInteraction);
scene->getActiveCamera()->setPosition(0.0, 40.0, 40.0);
diff --git a/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.h b/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.h
index b2c17c752859db5e56575a17a5e023141b5bfc5e..31788cf3bc48976e745a7bea86a479d58fd7e3dc 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.h
+++ b/Source/CollisionDetection/CollisionDetection/imstkCollisionDetection.h
@@ -43,6 +43,7 @@ public:
///
/// \brief Type of the collision detection
+ /// \todo: Remove and replace with factory names
///
enum class Type
{
@@ -53,6 +54,7 @@ public:
PointSetToSpherePicking,
PointSetToSurfaceMesh,
PointSetToImplicit,
+ PointSetToImplicitCCD,
// Mesh to mesh (mesh to analytical object = mesh vertices to analytical object)
SurfaceMeshToSurfaceMesh,
diff --git a/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.cpp b/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.cpp
index fd7b979984c8bc71adcc71f6c4347353673346c5..da765b9e1978e4eda426d84fc203a65c8d5bc787 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.cpp
+++ b/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.cpp
@@ -22,29 +22,59 @@ limitations under the License.
#include "imstkImplicitGeometryToPointSetCCD.h"
#include "imstkCollisionData.h"
#include "imstkImageData.h"
-#include "imstkImplicitGeometry.h"
-#include "imstkMath.h"
-#include "imstkPointSet.h"
-#include "imstkSignedDistanceField.h"
-#include "imstkVecDataArray.h"
+#include "imstkSurfaceMesh.h"
namespace imstk
{
ImplicitGeometryToPointSetCCD::ImplicitGeometryToPointSetCCD(std::shared_ptr<ImplicitGeometry> implicitGeomA,
std::shared_ptr<PointSet> pointSetB,
std::shared_ptr<CollisionData> colData) :
- CollisionDetection(CollisionDetection::Type::PointSetToImplicit, colData),
+ CollisionDetection(CollisionDetection::Type::PointSetToImplicitCCD, colData),
m_implicitGeomA(implicitGeomA),
m_pointSetB(pointSetB)
{
centralGrad.setFunction(m_implicitGeomA);
if (m_implicitGeomA->getType() == Geometry::Type::SignedDistanceField)
{
- centralGrad.setDx(std::dynamic_pointer_cast<SignedDistanceField>(m_implicitGeomA)->getImage()->getSpacing() * 0.9);
+ centralGrad.setDx(std::dynamic_pointer_cast<SignedDistanceField>(m_implicitGeomA)->getImage()->getSpacing());
}
- displacements = std::make_shared<VecDataArray<double, 3>>(static_cast<int>(m_pointSetB->getNumVertices()));
- m_pointSetB->setVertexAttribute("displacements", displacements);
+ displacementsPtr = std::make_shared<VecDataArray<double, 3>>(static_cast<int>(m_pointSetB->getNumVertices()));
+ m_pointSetB->setVertexAttribute("displacements", displacementsPtr);
+}
+
+static bool
+findFirstRoot(std::shared_ptr<ImplicitGeometry> implicitGeomA, const Vec3d& start, const Vec3d& end, Vec3d& root)
+{
+ const Vec3d displacement = end - start;
+
+ Vec3d currPos = start;
+ Vec3d prevPos = start;
+ double currDist = implicitGeomA->getFunctionValue(start);
+ double prevDist = currDist;
+
+ // Root find (could be multiple roots, we want the first, so start march from front)
+ // Gradient could be used for SDFs to converge faster but not for levelsets
+ const double stepRatio = 0.01; // 100/5=20, this will fail if object moves 20xwidth of the object
+ const double length = displacement.norm();
+ const double stepLength = length * stepRatio;
+ const Vec3d dir = displacement * (1.0 / length);
+ for (double x = stepLength; x < length; x += stepLength)
+ {
+ prevPos = currPos;
+ currPos = start + dir * x;
+
+ prevDist = currDist;
+ currDist = implicitGeomA->getFunctionValue(currPos);
+
+ if (currDist <= 0.0)
+ {
+ // Pick midpoint
+ root = (prevPos + currPos) * 0.5;
+ return true;
+ }
+ }
+ return false;
}
void
@@ -52,71 +82,114 @@ ImplicitGeometryToPointSetCCD::computeCollisionData()
{
m_colData->clearAll();
+ // We are going to try to catch a contact before updating via marching along
+ // the displacements of every point in the mesh
+
+ // First we project the mesh to the next timepoint (without collision)
+
if (!m_pointSetB->hasVertexAttribute("displacements"))
{
LOG(WARNING) << "ImplicitGeometry to PointSet CCD can't function without displacements on geometry";
return;
}
- const DataArray<double>& displacementsArr = *displacements;
+ const VecDataArray<double, 3>& displacements = *displacementsPtr;
std::shared_ptr<VecDataArray<double, 3>> vertexData = m_pointSetB->getVertexPositions();
- const VecDataArray<double, 3>& vertices = *vertexData;
- ParallelUtils::parallelFor(vertices.size(),
- [&](const size_t idx)
- {
- const Vec3d& pt = vertices[idx];
- const Vec3d displacement = Vec3d(&displacementsArr[idx * 3]);
- const double length = displacement.norm();
-
- Vec3d prevPos = pt;
- Vec3d currPos = pt;
- double prevVal = m_implicitGeomA->getFunctionValue(currPos);
- double currVal = prevVal;
-
- // Check if the starting point of the march is inside
- // This could happen if the collision wasn't fully resolved by the solver (common)
- if (currVal < 0.0)
- {
- PositionDirectionCollisionDataElement elem;
- elem.dirAtoB = -centralGrad(currPos).normalized(); // Contact Normal
- elem.nodeIdx = static_cast<uint32_t>(idx);
- elem.penetrationDepth = std::abs(currVal); // Could also reverse march in gradient direction
- elem.posB = currPos;
- m_colData->PDColData.safeAppend(elem);
- return; // No need to find an other root
- }
+ const VecDataArray<double, 3>& vertices = *vertexData; // Vertices in tentative state
- // If we didn't start inside the object, march along the displacement
- // smapling to see if enter it
- const double stepRatio = 0.05;
- const double stepLength = length * stepRatio;
- const Vec3d dir = displacement * (1.0 / length);
- for (double x = 0.0; x < length; x += stepLength)
- {
- const Vec3d diff = dir * x;
+ for (int i = 0; i < vertices.size(); i++)
+ {
+ const Vec3d& pt = vertices[i];
+ const Vec3d& displacement = displacements[i];
+ const double limit = displacement.norm() * m_depthRatioLimit;
+ const Vec3d prevPt = pt - displacement;
- prevPos = currPos;
- currPos = pt + diff;
+ Vec3d prevPos = prevPt;
+ double prevDist = m_implicitGeomA->getFunctionValue(prevPt);
+ bool prevIsInside = std::signbit(prevDist);
- prevVal = currVal;
- currVal = m_implicitGeomA->getFunctionValue(currPos);
+ Vec3d currPos = pt;
+ double currDist = m_implicitGeomA->getFunctionValue(currPos);
+ bool currIsInside = std::signbit(currDist);
- if (currVal < 0.0)
- {
- // Pick midpoint
- const Vec3d contactPt = (prevPos + currPos) * 0.5;
+ // If both inside
+ if (prevIsInside && currIsInside)
+ {
+ if (prevOuterElementCounter[i] > 0)
+ {
+ // Static or persistant
+ prevOuterElementCounter[i]++;
+ const Vec3d start = prevOuterElement[i]; // The last outside point in its movement history
+ const Vec3d end = pt;
+ Vec3d contactPt = Vec3d::Zero();
+ if (findFirstRoot(m_implicitGeomA, start, end, contactPt))
+ {
PositionDirectionCollisionDataElement elem;
- elem.dirAtoB = -centralGrad(contactPt).normalized(); // Contact Normal
- //elem.dirAtoB = dir;
- elem.nodeIdx = static_cast<uint32_t>(idx);
- elem.penetrationDepth = (contactPt - pt).norm();
+ elem.dirAtoB = -centralGrad(contactPt).normalized(); // -centralGrad gives Outward facing contact normal
+ //elem.dirAtoB = (contactPt - start).normalized();
+ /* {
+ Vec3d gradPos = forwardGrad(contactPt);
+ Vec3d gradNeg = backwardGrad(contactPt);
+ elem.dirAtoB[0] = std::min(gradNeg[0], 0.0) + std::max(gradPos[0], 0.0);
+ elem.dirAtoB[1] = std::min(gradNeg[1], 0.0) + std::max(gradPos[1], 0.0);
+ elem.dirAtoB[2] = std::min(gradNeg[2], 0.0) + std::max(gradPos[2], 0.0);
+ elem.dirAtoB.normalize();
+ }*/
+ elem.nodeIdx = static_cast<uint32_t>(i);
+ //elem.penetrationDepth = (contactPt - pt).norm();
+ elem.penetrationDepth = std::max(0.0, (contactPt - end).dot(elem.dirAtoB));
+ if (elem.penetrationDepth <= limit)
+ {
+ // Could be useful to find the closest point on the shape, as reprojected
+ // contact points could be outside
+ elem.posB = contactPt;
+ m_colData->PDColData.safeAppend(elem);
+ }
+ }
+ }
+ }
+ // If it just entered
+ else if (!prevIsInside && currIsInside)
+ {
+ const Vec3d start = prevPt;
+ const Vec3d end = pt;
+ Vec3d contactPt = Vec3d::Zero();
+ if (findFirstRoot(m_implicitGeomA, start, end, contactPt))
+ {
+ PositionDirectionCollisionDataElement elem;
+ elem.dirAtoB = -centralGrad(contactPt).normalized(); // -centralGrad gives Outward facing contact normal
+ //elem.dirAtoB = (contactPt - start).normalized();
+ /* {
+ Vec3d gradPos = forwardGrad(contactPt);
+ Vec3d gradNeg = backwardGrad(contactPt);
+ elem.dirAtoB[0] = std::max(gradNeg[0], 0.0) + std::min(gradPos[0], 0.0);
+ elem.dirAtoB[1] = std::max(gradNeg[1], 0.0) + std::min(gradPos[1], 0.0);
+ elem.dirAtoB[2] = std::max(gradNeg[2], 0.0) + std::min(gradPos[2], 0.0);
+ elem.dirAtoB.normalize();
+ }*/
+ elem.nodeIdx = static_cast<uint32_t>(i);
+ //elem.penetrationDepth = (contactPt - pt).norm();
+ elem.penetrationDepth = std::max(0.0, (contactPt - end).dot(elem.dirAtoB));
+ if (elem.penetrationDepth <= limit)
+ {
elem.posB = contactPt;
m_colData->PDColData.safeAppend(elem);
-
- return;
}
+ prevOuterElementCounter[i] = 1;
+ // Store the previous exterior point
+ prevOuterElement[i] = start;
}
- });
+ else
+ {
+ prevOuterElementCounter[i] = 0;
+ //printf("Somethings wrong2\n");
+ }
+ }
+ else
+ {
+ prevOuterElementCounter[i] = 0;
+ }
+ }
}
}
\ No newline at end of file
diff --git a/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.h b/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.h
index 9cc52e1ef951e5578783012a96f59ac225e7677a..4093f295b0440c2c4f05d55094927c5a24e6d632 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.h
+++ b/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.h
@@ -30,15 +30,16 @@ template<typename T, int N> class VecDataArray;
class ImplicitGeometry;
class PointSet;
-struct CollisionData;
///
/// \class ImplicitGeometryToPointSetCCD
///
/// \brief ImplicitGeometry to PointSet continous collision detection.
/// This CD method marches along the displacement of the points in the pointset
-/// to converge on the zero crossing of the implicit geometry. Does not produce
-/// time of impacts
+/// to converge on the zero crossing of the implicit geometry. This particular
+/// version is suited for levelsets not SDFs as it caches the history of the contact
+/// to avoid sampling the implicit geometry anywhere but at the surface (it will also
+/// work for SDFs, though better alterations/modifications of this exist for SDFs)
///
class ImplicitGeometryToPointSetCCD : public CollisionDetection
{
@@ -52,7 +53,9 @@ public:
ImplicitGeometryToPointSetCCD(std::shared_ptr<ImplicitGeometry> implicitGeomA,
std::shared_ptr<PointSet> pointSetB,
std::shared_ptr<CollisionData> colData);
+ virtual ~ImplicitGeometryToPointSetCCD() override = default;
+public:
///
/// \brief Detect collision and compute collision data
///
@@ -63,6 +66,12 @@ private:
std::shared_ptr<PointSet> m_pointSetB;
ImplicitFunctionCentralGradient centralGrad;
- std::shared_ptr<VecDataArray<double, 3>> displacements;
+ std::shared_ptr<VecDataArray<double, 3>> displacementsPtr;
+
+ std::unordered_map<int, Vec3d> prevOuterElement;
+ std::unordered_map<int, int> prevOuterElementCounter;
+
+ // Penetration depths are clamped to this ratio * displacement of the vertex
+ double m_depthRatioLimit = 0.3;
};
}
\ No newline at end of file
diff --git a/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCD.cpp b/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCD.cpp
index c9ff41d78defedca1f86159d1d71f0f6203f01f5..b3628e5a213c2db0215f12aae58bb7f7edb754dd 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCD.cpp
+++ b/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCD.cpp
@@ -62,7 +62,7 @@ ImplicitGeometryToPointSetCD::computeCollisionData()
elem.dirAtoB = -centralGrad(pt).normalized(); // Contact Normal
elem.nodeIdx = static_cast<uint32_t>(idx);
elem.penetrationDepth = std::abs(signedDistance);
- elem.posB = pt;
+ elem.posB = pt;// +elem.dirAtoB * elem.penetrationDepth;
m_colData->PDColData.safeAppend(elem);
}
});
diff --git a/Source/CollisionDetection/imstkCDObjectFactory.cpp b/Source/CollisionDetection/imstkCDObjectFactory.cpp
index d73a5944c7a5327de986511ca9a249542bf59836..76fced2e2376aa22425eed531d3c61f41518f9c3 100644
--- a/Source/CollisionDetection/imstkCDObjectFactory.cpp
+++ b/Source/CollisionDetection/imstkCDObjectFactory.cpp
@@ -23,11 +23,12 @@ limitations under the License.
#include "imstkCollisionData.h"
// Points to objects
+#include "imstkImplicitGeometryToPointSetCCD.h"
+#include "imstkImplicitGeometryToPointSetCD.h"
#include "imstkPointSetToCapsuleCD.h"
#include "imstkPointSetToPlaneCD.h"
#include "imstkPointSetToSphereCD.h"
#include "imstkPointSetToSurfaceMeshCD.h"
-#include "imstkImplicitGeometryToPointSetCD.h"
// Mesh to mesh
#include "imstkMeshToMeshBruteForceCD.h"
@@ -137,6 +138,14 @@ makeCollisionDetectionObject(const CollisionDetection::Type type,
auto implicitGeom = std::dynamic_pointer_cast<ImplicitGeometry>(collidingGeometryB);
return std::make_shared<ImplicitGeometryToPointSetCD>(implicitGeom, pointSet, colData);
}
+ case CollisionDetection::Type::PointSetToImplicitCCD:
+ {
+ IMSTK_CHECK_FOR_VALID_GEOMETRIES(collidingGeometryA, collidingGeometryB, PointSet, ImplicitGeometry);
+
+ auto pointSet = std::dynamic_pointer_cast<PointSet>(collidingGeometryA);
+ auto implicitGeom = std::dynamic_pointer_cast<ImplicitGeometry>(collidingGeometryB);
+ return std::make_shared<ImplicitGeometryToPointSetCCD>(implicitGeom, pointSet, colData);
+ }
// Mesh to mesh
case CollisionDetection::Type::SurfaceMeshToSurfaceMesh:
{
diff --git a/Source/CollisionHandling/imstkLevelSetCH.cpp b/Source/CollisionHandling/imstkLevelSetCH.cpp
index de3cd8b54d8383c60ba4f43f81c3abe3a5633bd5..17a276bdb01260ccc385fdb415ac84e2d7cc79f2 100644
--- a/Source/CollisionHandling/imstkLevelSetCH.cpp
+++ b/Source/CollisionHandling/imstkLevelSetCH.cpp
@@ -87,7 +87,7 @@ void
LevelSetCH::processCollisionData()
{
std::shared_ptr<LevelSetModel> lvlSetModel = m_lvlSetObj->getLevelSetModel();
- std::shared_ptr<ImageData> grid = std::dynamic_pointer_cast<ImageData>(lvlSetModel->getModelGeometry());
+ std::shared_ptr<ImageData> grid = std::dynamic_pointer_cast<SignedDistanceField>(lvlSetModel->getModelGeometry())->getImage();
if (grid == nullptr)
{
diff --git a/Source/Controllers/imstkKeyboardControl.h b/Source/Controllers/imstkKeyboardControl.h
index 1c54898e9c918103faf0316ddfd64e7f78de3e66..bfdf682d585df520fe47a34a9eba2cca22a5a9f1 100644
--- a/Source/Controllers/imstkKeyboardControl.h
+++ b/Source/Controllers/imstkKeyboardControl.h
@@ -39,7 +39,7 @@ class KeyEvent;
class KeyboardControl : public DeviceControl
{
public:
- KeyboardControl() { }
+ KeyboardControl() = default;
KeyboardControl(std::shared_ptr<KeyboardDeviceClient> keyDevice);
virtual ~KeyboardControl() override = default;
diff --git a/Source/Controllers/imstkRigidObjectController.cpp b/Source/Controllers/imstkRigidObjectController.cpp
index 0b728cd1c55b50629a098438f3a97dde6c1abde6..cf399ed69467173e5a339dae1f61dd3abf4b67fa 100644
--- a/Source/Controllers/imstkRigidObjectController.cpp
+++ b/Source/Controllers/imstkRigidObjectController.cpp
@@ -20,9 +20,7 @@
=========================================================================*/
#include "imstkRigidObjectController.h"
-#include "imstkCollidingObject.h"
#include "imstkDeviceClient.h"
-#include "imstkGeometry.h"
#include "imstkLogger.h"
#include "imstkRbdConstraint.h"
#include "imstkRigidObject2.h"
@@ -136,19 +134,13 @@ RigidObjectController::applyForces()
if (m_forceSmoothening)
{
m_forces.push_back(force);
+ m_forceSum += force;
if (m_forces.size() > m_smoothingKernelSize)
{
+ m_forceSum -= m_forces.front();
m_forces.pop_front();
}
-
- Vec3d avgForce = Vec3d(0.0, 0.0, 0.0);
- int count = 0;
- for (auto i : m_forces)
- {
- avgForce += i;
- count++;
- }
- avgForce /= count;
+ const Vec3d avgForce = m_forceSum / m_forces.size();
// Render only the spring force (not the other forces the body has)
m_deviceClient->setForce(avgForce);
diff --git a/Source/Controllers/imstkRigidObjectController.h b/Source/Controllers/imstkRigidObjectController.h
index ca33943737d80aec07b62826841d946b7afc891c..ec47c7b86db2dda3cf3dfdf8eb70ad4db359a6d0 100644
--- a/Source/Controllers/imstkRigidObjectController.h
+++ b/Source/Controllers/imstkRigidObjectController.h
@@ -105,16 +105,6 @@ public:
int getSmoothingKernelSize() const { return m_smoothingKernelSize; }
void setSmoothingKernelSize(const int kernelSize) { m_smoothingKernelSize = kernelSize; }
- ///
- /// \brief Return the currently applied force
- ///
- const Vec3d& getForce() const { return fS; }
-
- ///
- /// \brief Return the currently applied torque
- ///
- const Vec3d& getTorque() const { return tS; }
-
public:
///
/// \brief Update controlled scene object using latest tracking information
@@ -128,7 +118,6 @@ public:
protected:
std::shared_ptr<RigidObject2> m_rigidObject;
- std::deque<Vec3d> m_forces;
double m_linearKd = 10000.0; ///> Damping coefficient, linear
double m_angularKd = 300.0; ///> Damping coefficient, rotational
@@ -143,6 +132,8 @@ protected:
bool m_forceSmoothening = true;
int m_smoothingKernelSize = 25;
+ std::deque<Vec3d> m_forces;
+ Vec3d m_forceSum = Vec3d::Zero();
};
}
}
diff --git a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp
index b520cd84c35b49c6bfd280303108f4feea1d8466..dcec520d0ac64798388d40fcbc8136a5084a1196 100644
--- a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp
@@ -20,17 +20,11 @@
=========================================================================*/
#include "imstkLevelSetModel.h"
-#include "imstkTaskGraph.h"
-#include "imstkGeometryUtilities.h"
-#include "imstkLogger.h"
-#include "imstkSignedDistanceField.h"
-#include "imstkParallelFor.h"
-#include "imstkImplicitGeometry.h"
#include "imstkDataArray.h"
#include "imstkImageData.h"
-
-#include <vtkImplicitPolyDataDistance.h>
-#include <vtkPolyData.h>
+#include "imstkLogger.h"
+#include "imstkParallelFor.h"
+#include "imstkTaskGraph.h"
namespace imstk
{
@@ -75,28 +69,29 @@ LevelSetModel::initialize()
m_backwardGrad.setFunction(m_mesh);
m_curvature.setFunction(m_mesh);
- // If dense update, we need a gradient image, which will store forward and backward gradient magnitudes
- if (m_mesh->getType() == Geometry::Type::SignedDistanceField && !m_config->m_sparseUpdate)
+ if (m_mesh->getType() == Geometry::Type::SignedDistanceField)
{
- auto sdfImage = std::dynamic_pointer_cast<SignedDistanceField>(m_mesh)->getImage();
+ auto sdf = std::dynamic_pointer_cast<SignedDistanceField>(m_mesh);
+ std::shared_ptr<ImageData> sdfImage = sdf->getImage();
+ if (!m_config->m_sparseUpdate)
+ {
+ m_gradientMagnitudes = std::make_shared<ImageData>();
+ m_gradientMagnitudes->allocate(IMSTK_DOUBLE, 2, sdfImage->getDimensions(), sdfImage->getSpacing(), sdfImage->getOrigin());
- m_gradientMagnitudes = std::make_shared<ImageData>();
- m_gradientMagnitudes->allocate(IMSTK_DOUBLE, 2, sdfImage->getDimensions(), sdfImage->getSpacing(), sdfImage->getOrigin());
+ /* m_curvatures = std::make_shared<ImageData>();
+ m_curvatures->allocate(IMSTK_DOUBLE, 1, sdfImage->getDimensions(), sdfImage->getSpacing(), sdfImage->getOrigin());*/
- m_curvatures = std::make_shared<ImageData>();
- m_curvatures->allocate(IMSTK_DOUBLE, 1, sdfImage->getDimensions(), sdfImage->getSpacing(), sdfImage->getOrigin());
+ m_velocities = std::make_shared<ImageData>();
+ m_velocities->allocate(IMSTK_DOUBLE, 1, sdfImage->getDimensions(), sdfImage->getSpacing(), sdfImage->getOrigin());
+ //std::fill_n(static_cast<double*>(m_velocities->getScalars()->getVoidPointer()), m_velocities->get
+ }
- m_velocities = std::make_shared<ImageData>();
- m_velocities->allocate(IMSTK_DOUBLE, 1, sdfImage->getDimensions(), sdfImage->getSpacing(), sdfImage->getOrigin());
- //std::fill_n(static_cast<double*>(m_velocities->getScalars()->getVoidPointer()), m_velocities->get
+ const Vec3d actualSpacing = sdf->getImage()->getSpacing();// *sdf->getScale();
+ m_forwardGrad.setDx(Vec3i(1, 1, 1), actualSpacing);
+ m_backwardGrad.setDx(Vec3i(1, 1, 1), actualSpacing);
+ m_curvature.setDx(Vec3i(1, 1, 1), actualSpacing);
}
- m_forwardGrad.setDx(Vec3i(1, 1, 1), std::dynamic_pointer_cast<SignedDistanceField>(m_mesh)->getImage()->getSpacing());
- m_backwardGrad.setDx(Vec3i(1, 1, 1), std::dynamic_pointer_cast<SignedDistanceField>(m_mesh)->getImage()->getSpacing());
- /*forwardGrad.setDx(std::dynamic_pointer_cast<SignedDistanceField>(m_mesh)->getImage()->getSpacing());
- backwardGrad.setDx(std::dynamic_pointer_cast<SignedDistanceField>(m_mesh)->getImage()->getSpacing());*/
- m_curvature.setDx(Vec3i(1, 1, 1), std::dynamic_pointer_cast<SignedDistanceField>(m_mesh)->getImage()->getSpacing());
-
return true;
}
@@ -115,7 +110,8 @@ LevelSetModel::evolve()
auto imageData = std::dynamic_pointer_cast<ImageData>(sdf->getImage());
double* imgPtr = static_cast<double*>(imageData->getVoidPointer());
const Vec3i& dim = imageData->getDimensions();
- const double dt = m_config->m_dt;
+ const Vec3d& spacing = imageData->getSpacing();
+ const double dt = m_config->m_dt / m_config->m_substeps;
//const double k = m_config->m_k;
if (m_config->m_sparseUpdate)
@@ -131,57 +127,58 @@ LevelSetModel::evolve()
nodeUpdates.reserve(m_nodesToUpdate.size());
// Compute gradients
- for (std::unordered_map<size_t, std::tuple<Vec3i, double>>::iterator iter = m_nodesToUpdate.begin(); iter != m_nodesToUpdate.end(); iter++)
+ const double constantVel = m_config->m_constantVelocity;
+ for (int j = 0; j < m_config->m_substeps; j++)
{
- const size_t index = iter->first;
- const Vec3i& coords = std::get<0>(iter->second);
- const double f = std::get<1>(iter->second);
-
- // Gradients
- const Vec3d gradPos = m_forwardGrad(Vec3d(coords[0], coords[1], coords[2]));
- const Vec3d gradNeg = m_backwardGrad(Vec3d(coords[0], coords[1], coords[2]));
+ nodeUpdates.clear();
+ for (std::unordered_map<size_t, std::tuple<Vec3i, double>>::iterator iter = m_nodesToUpdate.begin(); iter != m_nodesToUpdate.end(); iter++)
+ {
+ const size_t index = iter->first;
+ const Vec3i& coords = std::get<0>(iter->second);
+ const double f = std::get<1>(iter->second);
- const double posMag =
- std::pow(std::max(gradNeg[0], 0.0), 2) + std::pow(std::min(gradPos[0], 0.0), 2) +
- std::pow(std::max(gradNeg[1], 0.0), 2) + std::pow(std::min(gradPos[1], 0.0), 2) +
- std::pow(std::max(gradNeg[2], 0.0), 2) + std::pow(std::min(gradPos[2], 0.0), 2);
+ // Gradients
+ const Vec3d gradPos = m_forwardGrad(Vec3d(coords[0], coords[1], coords[2]));
+ const Vec3d gradNeg = m_backwardGrad(Vec3d(coords[0], coords[1], coords[2]));
- const double negMag =
- std::pow(std::min(gradNeg[0], 0.0), 2) + std::pow(std::max(gradPos[0], 0.0), 2) +
- std::pow(std::min(gradNeg[1], 0.0), 2) + std::pow(std::max(gradPos[1], 0.0), 2) +
- std::pow(std::min(gradNeg[2], 0.0), 2) + std::pow(std::max(gradPos[2], 0.0), 2);
+ const double posMag =
+ std::pow(std::max(gradNeg[0], 0.0), 2) + std::pow(std::min(gradPos[0], 0.0), 2) +
+ std::pow(std::max(gradNeg[1], 0.0), 2) + std::pow(std::min(gradPos[1], 0.0), 2) +
+ std::pow(std::max(gradNeg[2], 0.0), 2) + std::pow(std::min(gradPos[2], 0.0), 2);
- // Curvature
- //const double kappa = m_curvature(Vec3d(coords[0], coords[1], coords[2]));
+ const double negMag =
+ std::pow(std::min(gradNeg[0], 0.0), 2) + std::pow(std::max(gradPos[0], 0.0), 2) +
+ std::pow(std::min(gradNeg[1], 0.0), 2) + std::pow(std::max(gradPos[1], 0.0), 2) +
+ std::pow(std::min(gradNeg[2], 0.0), 2) + std::pow(std::max(gradPos[2], 0.0), 2);
- nodeUpdates.push_back(std::tuple<size_t, Vec3i, double, Vec2d, double>(index, coords, f, Vec2d(negMag, posMag), 0.0));
- }
+ // Curvature
+ //const double kappa = m_curvature(Vec3d(coords[0], coords[1], coords[2]));
- // Update levelset
- const double constantVel = m_config->m_constantVelocity;
- for (size_t i = 0; i < nodeUpdates.size(); i++)
- {
- const size_t index = std::get<0>(nodeUpdates[i]);
- //const Vec3i& coords = std::get<1>(nodeUpdates[i]);
- const double vel = std::get<2>(nodeUpdates[i]) + constantVel;
- const Vec2d& g = std::get<3>(nodeUpdates[i]);
- //const double kappa = std::get<4>(nodeUpdates[i]);
-
- // If speed function positive use forward difference (posMag)
- if (vel > 0.0)
- {
- imgPtr[index] += dt * (vel * std::sqrt(g[0]) /*+ kappa*/);
+ nodeUpdates.push_back(std::tuple<size_t, Vec3i, double, Vec2d, double>(index, coords, f, Vec2d(negMag, posMag), 0.0));
}
- // If speed function negative use backward difference (negMag)
- else if (vel < 0.0)
+
+ // Update levelset
+ for (size_t i = 0; i < nodeUpdates.size(); i++)
{
- imgPtr[index] += dt * (vel * std::sqrt(g[1]) /*+ kappa * k*/);
+ const size_t index = std::get<0>(nodeUpdates[i]);
+ const double vel = std::get<2>(nodeUpdates[i]) + constantVel;
+ const Vec2d& g = std::get<3>(nodeUpdates[i]);
+ //const double kappa = std::get<4>(nodeUpdates[i]);
+
+ // If speed function positive use forward difference (posMag)
+ if (vel > 0.0)
+ {
+ imgPtr[index] += dt * (vel * std::sqrt(g[0]) /*+ kappa * k*/);
+ }
+ // If speed function negative use backward difference (negMag)
+ else if (vel < 0.0)
+ {
+ imgPtr[index] += dt * (vel * std::sqrt(g[1]) /*+ kappa * k*/);
+ }
}
}
- if (m_nodesToUpdate.size() > 0)
- {
- m_nodesToUpdate.clear();
- }
+
+ m_nodesToUpdate.clear();
}
else
{
@@ -222,7 +219,6 @@ LevelSetModel::evolve()
}
});
- // Uniform advance
const double constantVel = m_config->m_constantVelocity;
ParallelUtils::parallelFor(dim[0] * dim[1] * dim[2],
[&](const int& i)
diff --git a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h
index 7bae77b492198c970e8d18c3076b07b2170882a2..7263a2ca5f9d3b3ee127011a9a4183aa271cf904 100644
--- a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h
@@ -39,6 +39,7 @@ struct LevelSetModelConfig
bool m_useCurvature = false;
double m_k = 0.05; // Curvature term
double m_constantVelocity = 0.0; // Constant velocity
+ int m_substeps = 10; // How many steps to do every iteration
};
///
@@ -85,7 +86,14 @@ public:
virtual void evolve();
+ ///
+ /// \brief Add an impulse to the velocity field of the levelset
+ /// This actually takes the max of the current and provided
+ ///
void addImpulse(const Vec3i& coord, double f);
+ ///
+ /// \brief Set the impulse in the velocity field, replaces
+ ///
void setImpulse(const Vec3i& coord, double f);
std::shared_ptr<TaskNode> getQuantityEvolveNode(size_t i) const { return m_evolveQuantitiesNodes[i]; }
@@ -115,8 +123,6 @@ protected:
// suspect floating point error
StructuredForwardGradient m_forwardGrad;
StructuredBackwardGradient m_backwardGrad;
- /*ImplicitFunctionForwardGradient forwardGrad;
- ImplicitFunctionBackwardGradient backwardGrad;*/
ImplicitStructuredCurvature m_curvature;
};
diff --git a/Source/Filtering/imstkFastMarch.cpp b/Source/Filtering/imstkFastMarch.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0958ca2e69508f41fc02e26b37b01b2672273b2e
--- /dev/null
+++ b/Source/Filtering/imstkFastMarch.cpp
@@ -0,0 +1,216 @@
+/*=========================================================================
+
+ 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 "imstkFastMarch.h"
+#include "imstkLogger.h"
+#include "imstkVecDataArray.h"
+
+namespace imstk
+{
+void
+FastMarch::solve()
+{
+ // Get the scalars (ensure they're single component doubles)
+ std::shared_ptr<AbstractDataArray> abstractScalars = imageData->getScalars();
+ if (abstractScalars->getScalarType() != IMSTK_DOUBLE || abstractScalars->getNumberOfComponents() != 1)
+ {
+ LOG(WARNING) << "fastMarch only works with single component double images";
+ return;
+ }
+ auto scalars = std::dynamic_pointer_cast<DataArray<double>>(abstractScalars);
+ double* imgPtr = scalars->getPointer();
+ dim = imageData->getDimensions();
+ spacing = imageData->getSpacing();
+ indexShift = dim[0] * dim[1];
+
+ // We maintain a solution in maps, to keep things sparse
+
+ // Sparse container for which nodes are marked visited
+ visited = std::unordered_set<int>();
+ // Sparse container for nodal distances
+ distances = std::unordered_map<int, double>(); // Solved distances
+
+ queue = std::priority_queue<Node, std::vector<Node>, NodeComparator>();
+
+ // Add the initial seeds to the queue
+ for (int i = 0; i < seedVoxels.size(); i++)
+ {
+ Vec3i coord = seedVoxels[i];
+ if (coord[0] < 0 || coord[0] >= dim[0]
+ || coord[1] < 0 || coord[1] >= dim[1]
+ || coord[2] < 0 || coord[2] >= dim[2])
+ {
+ continue;
+ }
+ const int index = static_cast<int>(imageData->getScalarIndex(coord));
+ distances[index] = imgPtr[imageData->getScalarIndex(coord)];
+ queue.push(Node(index, 0.0, coord));
+ }
+
+ // Process every node in order of minimum distance
+ while (!queue.empty())
+ {
+ Node node = queue.top();
+ queue.pop();
+
+ const Vec3i& coord = node.m_coord;
+ const int& nodeId = node.m_nodeId;
+
+ // Termination conditions
+ if (isVisited(nodeId)
+ || getDistance(nodeId) >= distThreshold)
+ {
+ continue;
+ }
+
+ // Mark node as visited (to avoid readdition)
+ visited.insert(nodeId);
+
+ // Update all its neighbor cells (diagonals not considered neighbors)
+ // Right +x
+ int neighborId = nodeId + 1;
+ Vec3i neighborCoord = coord + Vec3i(1, 0, 0);
+ if (neighborCoord[0] < dim[0] && !isVisited(neighborId))
+ {
+ solveNode(neighborCoord, neighborId);
+ }
+ // Left -x
+ neighborId = nodeId - 1;
+ neighborCoord = coord - Vec3i(1, 0, 0);
+ if (neighborCoord[0] >= 0 && !isVisited(neighborId))
+ {
+ solveNode(neighborCoord, neighborId);
+ }
+
+ // Up +y
+ neighborId = nodeId + dim[0];
+ neighborCoord = coord + Vec3i(0, 1, 0);
+ if (neighborCoord[1] < dim[1] && !isVisited(neighborId))
+ {
+ solveNode(neighborCoord, neighborId);
+ }
+ // Down -y
+ neighborId = nodeId - dim[0];
+ neighborCoord = coord - Vec3i(0, 1, 0);
+ if (neighborCoord[1] >= 0 && !isVisited(neighborId))
+ {
+ solveNode(neighborCoord, neighborId);
+ }
+
+ // Forward +z
+ neighborId = nodeId + indexShift;
+ neighborCoord = coord + Vec3i(0, 0, 1);
+ if (neighborCoord[2] < dim[2] && !isVisited(neighborId))
+ {
+ solveNode(neighborCoord, neighborId);
+ }
+ // Backward -z
+ neighborId = nodeId - indexShift;
+ neighborCoord = coord - Vec3i(0, 0, 1);
+ if (neighborCoord[2] >= 0 && !isVisited(neighborId))
+ {
+ solveNode(neighborCoord, neighborId);
+ }
+ }
+
+ // Write the sparse distances to the image
+ for (auto i : distances)
+ {
+ imgPtr[i.first] = i.second;
+ }
+}
+
+void
+FastMarch::solveNode(Vec3i coord, int index)
+{
+ // Compute the min distance in each axes
+ const double dists[6] =
+ {
+ coord[0] - 1 >= 0 ? getDistance(index - 1) : IMSTK_DOUBLE_MAX,
+ coord[0] + 1 < dim[0] ? getDistance(index + 1) : IMSTK_DOUBLE_MAX,
+ coord[1] - 1 >= 0 ? getDistance(index - dim[0]) : IMSTK_DOUBLE_MAX,
+ coord[1] + 1 < dim[1] ? getDistance(index + dim[0]) : IMSTK_DOUBLE_MAX,
+ coord[2] - 1 >= 0 ? getDistance(index - indexShift) : IMSTK_DOUBLE_MAX,
+ coord[2] + 1 < dim[2] ? getDistance(index + indexShift) : IMSTK_DOUBLE_MAX
+ };
+ const double minDist[3] =
+ {
+ std::min(dists[0], dists[1]),
+ std::min(dists[2], dists[3]),
+ std::min(dists[4], dists[5])
+ };
+
+ // Sort so that the min of minDist is first
+ int dimReorder[3] = { 0, 1, 2 };
+ for (int i = 0; i < 3; i++)
+ {
+ for (int j = i + 1; j < 3; j++)
+ {
+ const int dim1 = dimReorder[i];
+ const int dim2 = dimReorder[j];
+ if (minDist[dim1] > minDist[dim2])
+ {
+ std::swap(dimReorder[i], dimReorder[j]);
+ }
+ }
+ }
+
+ double aa = 0.0;
+ double bb = 0.0;
+ double cc = -1.0;
+
+ // For every dimension
+ double solution = IMSTK_DOUBLE_MAX;
+ double discrim = 0.0;
+ // todo: Sort dimensions by minDist, start with smallest (break faster)
+ for (unsigned int i = 0; i < 3; i++)
+ {
+ const double value = minDist[dimReorder[i]];
+ if (solution >= value)
+ {
+ const double spaceFactor = std::sqrt(1.0 / spacing[dimReorder[i]]);
+ aa += spaceFactor;
+ bb += value * spaceFactor;
+ cc += std::pow(value, 2) * spaceFactor;
+
+ discrim = std::pow(bb, 2) - aa * cc;
+ if (discrim < 0.0)
+ {
+ // Whoops
+ return;
+ }
+
+ solution = (std::sqrt(discrim) + bb) / aa;
+ }
+ else
+ {
+ break;
+ }
+ }
+
+ if (solution < IMSTK_DOUBLE_MAX)
+ {
+ // Accept it as the new distance
+ distances[index] = solution;
+ queue.push(Node(index, solution, coord));
+ }
+}
+}
\ No newline at end of file
diff --git a/Source/Filtering/imstkFastMarch.h b/Source/Filtering/imstkFastMarch.h
new file mode 100644
index 0000000000000000000000000000000000000000..d0965da950e21226edc4d7d6a2334c755c051799
--- /dev/null
+++ b/Source/Filtering/imstkFastMarch.h
@@ -0,0 +1,86 @@
+/*=========================================================================
+
+ 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 "imstkImageData.h"
+
+#include <unordered_set>
+#include <queue>
+
+namespace imstk
+{
+class FastMarch
+{
+protected:
+ // Setup Node struct for priority queue
+ struct Node
+ {
+ int m_nodeId;
+ double m_cost;
+ Vec3i m_coord;
+
+ Node(int nodeId, double cost, Vec3i coord) :
+ m_nodeId(nodeId), m_cost(cost), m_coord(coord)
+ {
+ }
+ };
+ struct NodeComparator
+ {
+ bool operator()(const Node& a, const Node& b)
+ {
+ return a.m_cost > b.m_cost;
+ }
+ };
+
+public:
+ bool isVisited(int nodeId) { return visited.count(nodeId) == 0 ? false : true; }
+ double getDistance(int nodeId) { return distances.count(nodeId) == 0 ? IMSTK_DOUBLE_MAX : distances.at(nodeId); }
+
+ void solve();
+
+ void solveNode(Vec3i coord, int index);
+
+ void setSeeds(std::vector<Vec3i> seedVoxels) { this->seedVoxels = seedVoxels; }
+ void setVisited(std::unordered_set<int> visited) { this->visited = visited; }
+ void setDistnaces(std::unordered_map<int, double> distances) { this->distances = distances; }
+ void setImage(std::shared_ptr<ImageData> image) { imageData = image; }
+ void setDistThreshold(double distThreshold) { this->distThreshold = distThreshold; }
+
+protected:
+ // The image to operate on
+ std::shared_ptr<ImageData> imageData;
+ Vec3i dim;
+ Vec3d spacing;
+ int indexShift;
+
+ std::unordered_set<int> visited;
+ std::unordered_map<int, double> distances;
+
+ // The starting voxels
+ std::vector<Vec3i> seedVoxels;
+
+ // Distance to go too
+ double distThreshold;
+
+ std::priority_queue<Node, std::vector<Node>, NodeComparator> queue;
+};
+}
\ No newline at end of file
diff --git a/Source/Geometry/Analytic/imstkCube.h b/Source/Geometry/Analytic/imstkCube.h
index eb3d3470ea06963377afa574718ad9ccc61dc1fc..2ac49400ef44dd3414a62f196d98ccb0624df018 100644
--- a/Source/Geometry/Analytic/imstkCube.h
+++ b/Source/Geometry/Analytic/imstkCube.h
@@ -72,8 +72,14 @@ public:
///
double getFunctionValue(const Vec3d& pos) const override
{
- const Vec3d dmin = pos - m_position - Vec3d(m_width, m_width, m_width) * 0.5;
- const Vec3d dmax = m_position - pos - Vec3d(m_width, m_width, m_width) * 0.5;
+ // Unrotate the point and cube to axes align the cube
+ const Mat3d r = Quatd::FromTwoVectors(m_orientationAxisPostTransform, Vec3d(0.0, 1.0, 0.0)).toRotationMatrix();
+ const Vec3d p = r * pos;
+ const Vec3d center = r * m_positionPostTransform;
+
+ // Then test
+ const Vec3d dmin = p - center - Vec3d(m_widthPostTransform, m_widthPostTransform, m_widthPostTransform) * 0.5;
+ const Vec3d dmax = center - p - Vec3d(m_widthPostTransform, m_widthPostTransform, m_widthPostTransform) * 0.5;
const Vec3d d = dmin.cwiseMax(dmax);
return std::max(std::max(d[0], d[1]), d[2]);
}
diff --git a/Source/Geometry/imstkGeometryUtilities.cpp b/Source/Geometry/imstkGeometryUtilities.cpp
index 3ffea8e3c63e392eb33522d1dc1d41514bac5c47..c48273963825de52bcf983c3d33d8a2758365aab 100644
--- a/Source/Geometry/imstkGeometryUtilities.cpp
+++ b/Source/Geometry/imstkGeometryUtilities.cpp
@@ -20,20 +20,18 @@
=========================================================================*/
#include "imstkGeometryUtilities.h"
-#include "imstkVecDataArray.h"
+#include "imstkCube.h"
#include "imstkHexahedralMesh.h"
#include "imstkImageData.h"
#include "imstkLineMesh.h"
#include "imstkLogger.h"
+#include "imstkMacros.h"
#include "imstkParallelUtils.h"
+#include "imstkSphere.h"
#include "imstkSurfaceMesh.h"
#include "imstkTetrahedralMesh.h"
-#include "imstkCube.h"
-#include "imstkSphere.h"
-#include "imstkPlane.h"
-
-#include "imstkMacros.h"
#include "imstkTypes.h"
+#include "imstkVecDataArray.h"
#include <vtkAppendPolyData.h>
#include <vtkCellData.h>
@@ -45,7 +43,6 @@
#include <vtkFloatArray.h>
#include <vtkImageData.h>
#include <vtkMassProperties.h>
-#include <vtkPlaneSource.h>
#include <vtkPointData.h>
#include <vtkShortArray.h>
#include <vtkSphereSource.h>
@@ -729,7 +726,10 @@ GeometryUtils::toCubeSurfaceMesh(std::shared_ptr<Cube> cube)
vtkNew<vtkTriangleFilter> triangulate;
triangulate->SetInputData(transformCube->GetOutput());
triangulate->Update();
- return copyToSurfaceMesh(triangulate->GetOutput());
+ vtkNew<vtkCleanPolyData> cleanData;
+ cleanData->SetInputData(triangulate->GetOutput());
+ cleanData->Update();
+ return copyToSurfaceMesh(cleanData->GetOutput());
}
std::shared_ptr<SurfaceMesh>
@@ -743,33 +743,20 @@ GeometryUtils::toUVSphereSurfaceMesh(std::shared_ptr<Sphere> sphere,
sphereSource->SetThetaResolution(thetaDivisions);
sphereSource->Update();
- return copyToSurfaceMesh(sphereSource->GetOutput());
-}
-
-std::shared_ptr<SurfaceMesh>
-GeometryUtils::toQuadSurfaceMesh(std::shared_ptr<Plane> plane)
-{
- const Quatd r = Quatd::FromTwoVectors(Vec3d(0.0, 1.0, 0.0), plane->getOrientationAxis());
- const Vec3d i = r._transformVector(Vec3d(1.0, 0.0, 0.0));
- const Vec3d j = r._transformVector(Vec3d(0.0, 0.0, 1.0));
-
- //Vec3d p1 = plane->getPosition() + plane->getWidth() * (i + j);
- Vec3d p2 = plane->getPosition() + plane->getWidth() * (i - j);
- Vec3d p3 = plane->getPosition() + plane->getWidth() * (-i + j);
- Vec3d p4 = plane->getPosition() + plane->getWidth() * (-i - j);
-
- vtkNew<vtkPlaneSource> planeSource;
- planeSource->SetOrigin(p4.data());
- planeSource->SetPoint1(p3.data());
- planeSource->SetPoint2(p2.data());
- planeSource->Update();
+ vtkNew<vtkTransform> transform;
+ transform->SetMatrix(mat4dTranslate(sphere->getPosition()).data());
+ vtkNew<vtkTransformFilter> transformFilter;
+ transformFilter->SetInputData(sphereSource->GetOutput());
+ transformFilter->SetTransform(transform);
+ transformFilter->Update();
vtkNew<vtkTriangleFilter> triangulate;
- triangulate->SetInputData(planeSource->GetOutput());
+ triangulate->SetInputData(transformFilter->GetOutput());
triangulate->Update();
vtkNew<vtkCleanPolyData> cleanData;
- cleanData->SetInputConnection(triangulate->GetOutputPort());
+ cleanData->SetInputData(triangulate->GetOutput());
cleanData->Update();
+
return copyToSurfaceMesh(cleanData->GetOutput());
}
diff --git a/Source/Scene/imstkRigidObjectCollisionPair.cpp b/Source/Scene/imstkRigidObjectCollisionPair.cpp
index 38dc4e788383a3e4d59576d27fa89359b292e9e4..9a049432df890fc3daebde5f9a38b4cc8a0101cf 100644
--- a/Source/Scene/imstkRigidObjectCollisionPair.cpp
+++ b/Source/Scene/imstkRigidObjectCollisionPair.cpp
@@ -22,11 +22,11 @@ limitations under the License.
#include "imstkRigidObjectCollisionPair.h"
#include "imstkCDObjectFactory.h"
#include "imstkCollisionData.h"
+#include "imstkPointSet.h"
#include "imstkRigidBodyCH.h"
+#include "imstkRigidBodyModel2.h"
#include "imstkRigidObject2.h"
#include "imstkTaskGraph.h"
-#include "imstkRigidBodyModel2.h"
-#include "imstkLogger.h"
namespace imstk
{
@@ -74,5 +74,81 @@ RigidObjectCollisionPair::RigidObjectCollisionPair(std::shared_ptr<RigidObject2>
auto ch = std::make_shared<RigidBodyCH>(side, m_colData, obj1, obj2);
setCollisionHandlingAB(ch);
}
+
+void
+RigidObjectCollisionPair::apply()
+{
+ CollisionPair::apply();
+
+ auto obj1 = std::dynamic_pointer_cast<RigidObject2>(m_objects.first);
+ std::shared_ptr<RigidBodyModel2> rbdModel = obj1->getRigidBodyModel2();
+ std::shared_ptr<PointSet> pointSet = std::dynamic_pointer_cast<PointSet>(obj1->getPhysicsGeometry());
+ const bool measureDisplacements = (pointSet != nullptr && pointSet->hasVertexAttribute("displacements"));
+
+ // The tentative body is never actually computed, it should be good to catch the contact
+ // in the next frame
+ if (measureDisplacements)
+ {
+ // 1.) Copy the vertices at the start of the frame
+ obj1->getTaskGraph()->insertBefore(obj1->getRigidBodyModel2()->getComputeTentativeVelocitiesNode(),
+ std::make_shared<TaskNode>([ = ]()
+ {
+ copyVertsToPrevious();
+ }, "CopyVertsToPrevious"));
+
+ // If you were to update to tentative, you'd do it here, then compute displacements
+
+ // 2.) Compute the displacements after updating geometry
+ obj1->getTaskGraph()->insertAfter(obj1->getUpdateGeometryNode(),
+ std::make_shared<TaskNode>([ = ]()
+ {
+ measureDisplacementFromPrevious();
+ }, "ComputeDisplacements"));
+ }
+}
+
+void
+RigidObjectCollisionPair::copyVertsToPrevious()
+{
+ auto obj1 = std::dynamic_pointer_cast<RigidObject2>(m_objects.first);
+ std::shared_ptr<PointSet> pointSet = std::dynamic_pointer_cast<PointSet>(obj1->getPhysicsGeometry());
+
+ if (pointSet != nullptr && pointSet->hasVertexAttribute("displacements"))
+ {
+ std::shared_ptr<VecDataArray<double, 3>> verticesPtr = pointSet->getVertexPositions();
+ VecDataArray<double, 3>& vertices = *verticesPtr;
+ VecDataArray<double, 3>& prevVertices = *m_prevVertices;
+
+ if (prevVertices.size() != vertices.size())
+ {
+ prevVertices.resize(vertices.size());
+ }
+ std::copy_n(vertices.getPointer(), vertices.size(), prevVertices.getPointer());
+ }
+}
+
+void
+RigidObjectCollisionPair::measureDisplacementFromPrevious()
+{
+ auto obj1 = std::dynamic_pointer_cast<RigidObject2>(m_objects.first);
+ std::shared_ptr<PointSet> pointSet = std::dynamic_pointer_cast<PointSet>(obj1->getPhysicsGeometry());
+
+ if (pointSet != nullptr && pointSet->hasVertexAttribute("displacements"))
+ {
+ std::shared_ptr<VecDataArray<double, 3>> displacements =
+ std::dynamic_pointer_cast<VecDataArray<double, 3>>(pointSet->getVertexAttribute("displacements"));
+ VecDataArray<double, 3>& displacementsArr = *displacements;
+
+ std::shared_ptr<VecDataArray<double, 3>> verticesPtr = pointSet->getVertexPositions();
+ VecDataArray<double, 3>& vertices = *verticesPtr;
+ VecDataArray<double, 3>& prevVertices = *m_prevVertices;
+
+ ParallelUtils::parallelFor(displacements->size(),
+ [&](const int i)
+ {
+ displacementsArr[i] = vertices[i] - prevVertices[i];
+ });
+ }
+}
}
}
\ No newline at end of file
diff --git a/Source/Scene/imstkRigidObjectCollisionPair.h b/Source/Scene/imstkRigidObjectCollisionPair.h
index 3fccbcdd10c0fc7da6669a764b661238118d6035..3be8b0db3db3f8f75612499c6b74b37c83880a51 100644
--- a/Source/Scene/imstkRigidObjectCollisionPair.h
+++ b/Source/Scene/imstkRigidObjectCollisionPair.h
@@ -26,6 +26,8 @@ limitations under the License.
namespace imstk
{
+template<typename T, int N> class VecDataArray;
+
namespace expiremental
{
class RigidObject2;
@@ -44,6 +46,16 @@ public:
CollisionDetection::Type cdType);
virtual ~RigidObjectCollisionPair() override = default;
+
+public:
+ void apply() override;
+
+ void copyVertsToPrevious();
+
+ void measureDisplacementFromPrevious();
+
+public:
+ std::shared_ptr<VecDataArray<double, 3>> m_prevVertices;
};
}
}
\ No newline at end of file
diff --git a/Source/Scene/imstkRigidObjectLevelSetCollisionPair.cpp b/Source/Scene/imstkRigidObjectLevelSetCollisionPair.cpp
index b328c76a6013a74584f18845c63237e61b8ccb3c..825ec75c0b689291f39c2d39f4a065e63d5dad8d 100644
--- a/Source/Scene/imstkRigidObjectLevelSetCollisionPair.cpp
+++ b/Source/Scene/imstkRigidObjectLevelSetCollisionPair.cpp
@@ -22,25 +22,24 @@ limitations under the License.
#include "imstkRigidObjectLevelSetCollisionPair.h"
#include "imstkCDObjectFactory.h"
#include "imstkCollisionData.h"
+#include "imstkImplicitGeometry.h"
+#include "imstkImplicitGeometryToPointSetCCD.h"
+#include "imstkImplicitGeometryToPointSetCD.h"
#include "imstkLevelSetCH.h"
#include "imstkLevelSetDeformableObject.h"
#include "imstkLevelSetModel.h"
#include "imstkRigidBodyCH.h"
#include "imstkRigidBodyModel2.h"
#include "imstkRigidObject2.h"
+#include "imstkSurfaceMesh.h"
#include "imstkTaskGraph.h"
-#include "imstkPointSet.h"
-#include "imstkImplicitGeometry.h"
-
-#include "imstkImplicitGeometryToPointSetCD.h"
-#include "imstkImplicitGeometryToPointSetCCD.h"
namespace imstk
{
namespace expiremental
{
RigidObjectLevelSetCollisionPair::RigidObjectLevelSetCollisionPair(std::shared_ptr<RigidObject2> obj1, std::shared_ptr<LevelSetDeformableObject> obj2) :
- CollisionPair(obj1, obj2)
+ CollisionPair(obj1, obj2), m_prevVertices(std::make_shared<VecDataArray<double, 3>>())
{
std::shared_ptr<RigidBodyModel2> rbdModel = obj1->getRigidBodyModel2();
std::shared_ptr<LevelSetModel> lvlSetModel = obj2->getLevelSetModel();
@@ -76,5 +75,80 @@ RigidObjectLevelSetCollisionPair::RigidObjectLevelSetCollisionPair(std::shared_p
setCollisionHandlingA(std::make_shared<RigidBodyCH>(CollisionHandling::Side::A, m_colData, obj1, nullptr, 0.0, 0.0));
setCollisionHandlingB(std::make_shared<LevelSetCH>(CollisionHandling::Side::B, m_colData, obj2, obj1));
}
+
+void
+RigidObjectLevelSetCollisionPair::apply()
+{
+ CollisionPair::apply();
+
+ auto obj1 = std::dynamic_pointer_cast<RigidObject2>(m_objects.first);
+ std::shared_ptr<RigidBodyModel2> rbdModel = obj1->getRigidBodyModel2();
+ std::shared_ptr<PointSet> pointSet = std::dynamic_pointer_cast<PointSet>(obj1->getPhysicsGeometry());
+ const bool measureDisplacements = (pointSet != nullptr && pointSet->hasVertexAttribute("displacements"));
+
+ // The tentative body is never actually computed, it should be good to catch the contact
+ // in the next frame
+ if (measureDisplacements)
+ {
+ // 1.) Copy the vertices at the start of the frame
+ obj1->getTaskGraph()->insertBefore(obj1->getRigidBodyModel2()->getComputeTentativeVelocitiesNode(),
+ std::make_shared<TaskNode>([ = ]()
+ {
+ copyVertsToPrevious();
+ }, "CopyVertsToPrevious"));
+
+ // If you were to update to tentative, you'd do it here, then compute displacements
+
+ // 2.) Compute the displacements after updating geometry
+ std::shared_ptr<TaskNode> computeDisplacements =
+ std::make_shared<TaskNode>(std::bind(&RigidObjectLevelSetCollisionPair::measureDisplacementFromPrevious, this), "ComputeDisplacements");
+ obj1->getTaskGraph()->insertAfter(obj1->getUpdateGeometryNode(),
+ computeDisplacements);
+ }
+}
+
+void
+RigidObjectLevelSetCollisionPair::copyVertsToPrevious()
+{
+ auto obj1 = std::dynamic_pointer_cast<RigidObject2>(m_objects.first);
+ std::shared_ptr<PointSet> pointSet = std::dynamic_pointer_cast<PointSet>(obj1->getPhysicsGeometry());
+
+ if (pointSet != nullptr && pointSet->hasVertexAttribute("displacements"))
+ {
+ std::shared_ptr<VecDataArray<double, 3>> verticesPtr = pointSet->getVertexPositions();
+ VecDataArray<double, 3>& vertices = *verticesPtr;
+ VecDataArray<double, 3>& prevVertices = *m_prevVertices;
+
+ if (prevVertices.size() != vertices.size())
+ {
+ prevVertices.resize(vertices.size());
+ }
+ std::copy_n(vertices.getPointer(), vertices.size(), prevVertices.getPointer());
+ }
+}
+
+void
+RigidObjectLevelSetCollisionPair::measureDisplacementFromPrevious()
+{
+ auto obj1 = std::dynamic_pointer_cast<RigidObject2>(m_objects.first);
+ std::shared_ptr<PointSet> pointSet = std::dynamic_pointer_cast<PointSet>(obj1->getPhysicsGeometry());
+
+ if (pointSet != nullptr && pointSet->hasVertexAttribute("displacements"))
+ {
+ std::shared_ptr<VecDataArray<double, 3>> displacements =
+ std::dynamic_pointer_cast<VecDataArray<double, 3>>(pointSet->getVertexAttribute("displacements"));
+ VecDataArray<double, 3>& displacementsArr = *displacements;
+
+ std::shared_ptr<VecDataArray<double, 3>> verticesPtr = pointSet->getVertexPositions();
+ VecDataArray<double, 3>& vertices = *verticesPtr;
+ VecDataArray<double, 3>& prevVertices = *m_prevVertices;
+
+ ParallelUtils::parallelFor(displacements->size(),
+ [&](const int i)
+ {
+ displacementsArr[i] = vertices[i] - prevVertices[i];
+ });
+ }
+}
}
}
\ No newline at end of file
diff --git a/Source/Scene/imstkRigidObjectLevelSetCollisionPair.h b/Source/Scene/imstkRigidObjectLevelSetCollisionPair.h
index 8bd6b806ea860a53d29ce2148c895174f73be104..a43acdd11a828479ddc5257e70a26b5621640f90 100644
--- a/Source/Scene/imstkRigidObjectLevelSetCollisionPair.h
+++ b/Source/Scene/imstkRigidObjectLevelSetCollisionPair.h
@@ -27,6 +27,7 @@ limitations under the License.
namespace imstk
{
class LevelSetDeformableObject;
+template<typename T, int N> class VecDataArray;
namespace expiremental
{
@@ -43,6 +44,15 @@ class RigidObjectLevelSetCollisionPair : public CollisionPair
public:
RigidObjectLevelSetCollisionPair(std::shared_ptr<RigidObject2> obj1, std::shared_ptr<LevelSetDeformableObject> obj2);
virtual ~RigidObjectLevelSetCollisionPair() override = default;
+
+public:
+ void apply() override;
+
+ void copyVertsToPrevious();
+ void measureDisplacementFromPrevious();
+
+public:
+ std::shared_ptr<VecDataArray<double, 3>> m_prevVertices;
};
}
}
\ No newline at end of file
diff --git a/Source/SceneEntities/Objects/imstkRigidObject2.cpp b/Source/SceneEntities/Objects/imstkRigidObject2.cpp
index 89c221991f6ae36732d88f3018c295b0702baa40..526e830dc098ebd6fc7be2babee1dcef9e125b67 100644
--- a/Source/SceneEntities/Objects/imstkRigidObject2.cpp
+++ b/Source/SceneEntities/Objects/imstkRigidObject2.cpp
@@ -23,9 +23,6 @@
#include "imstkLogger.h"
#include "imstkRbdConstraint.h"
#include "imstkRigidBodyModel2.h"
-#include "imstkPointSet.h"
-#include "imstkVecDataArray.h"
-#include "imstkParallelFor.h"
namespace imstk
{
@@ -68,28 +65,6 @@ RigidObject2::setDynamicalModel(std::shared_ptr<AbstractDynamicalModel> dynaMode
void
RigidObject2::updatePhysicsGeometry()
{
- // Record displacements (useful for CCD)
- std::shared_ptr<PointSet> pointSet = std::dynamic_pointer_cast<PointSet>(m_physicsGeometry);
- if (pointSet != nullptr && pointSet->hasVertexAttribute("displacements"))
- {
- const Vec3d& linearVelocity = m_rigidBody->getVelocity();
- const Vec3d& angularVelocity = m_rigidBody->getAngularVelocity();
-
- std::shared_ptr<VecDataArray<double, 3>> displacements =
- std::dynamic_pointer_cast<VecDataArray<double, 3>>(pointSet->getVertexAttribute("displacements"));
- VecDataArray<double, 3>& displacementsArr = *displacements;
-
- ParallelUtils::parallelFor(displacements->size(), [&](const int i)
- {
- const Vec3d diff = pointSet->getVertexPosition(i) - m_rigidBody->getPosition();
- const Vec3d velocity = linearVelocity + angularVelocity.cross(diff);
- const Vec3d displacement = velocity * m_rigidBodyModel2->getConfig()->m_dt;
- displacementsArr[i][0] = displacement[0];
- displacementsArr[i][1] = displacement[1];
- displacementsArr[i][2] = displacement[2];
- });
- }
-
// Apply the transform back to the geometry
m_physicsGeometry->setTranslation(m_rigidBody->getPosition());
m_physicsGeometry->setRotation(m_rigidBody->getOrientation());
diff --git a/Source/SceneEntities/Objects/imstkRigidObject2.h b/Source/SceneEntities/Objects/imstkRigidObject2.h
index 9cd786ba784586d92672dfc976fad3ae0b8bb571..1a913e365d1746819e318fa4c5af4fc7ab298772 100644
--- a/Source/SceneEntities/Objects/imstkRigidObject2.h
+++ b/Source/SceneEntities/Objects/imstkRigidObject2.h
@@ -35,13 +35,15 @@ struct RigidBody;
///
/// \class RigidObject2
///
-/// \brief Scene objects that are governed by rigid body dynamics
+/// \brief Scene objects that are governed by rigid body dynamics under
+/// the RigidBodyModel2
///
class RigidObject2 : public DynamicObject
{
public:
RigidObject2(const std::string& name) : DynamicObject(name) { }
- virtual ~RigidObject2() override = default;
+
+ virtual ~RigidObject2() = default;
public:
virtual const std::string getTypeName() const override { return "RigidObject2"; }
diff --git a/Source/SimulationManager/imstkKeyboardSceneControl.cpp b/Source/SimulationManager/imstkKeyboardSceneControl.cpp
index c3810b7c836c0097b702ca9c2abe472d68684735..f9cf7617ba666cb27d3973a8bfe11ae2b9d3403f 100644
--- a/Source/SimulationManager/imstkKeyboardSceneControl.cpp
+++ b/Source/SimulationManager/imstkKeyboardSceneControl.cpp
@@ -30,10 +30,6 @@
namespace imstk
{
-KeyboardSceneControl::KeyboardSceneControl()
-{
-}
-
KeyboardSceneControl::KeyboardSceneControl(std::shared_ptr<KeyboardDeviceClient> keyDevice) :
KeyboardControl(keyDevice)
{
@@ -82,6 +78,9 @@ KeyboardSceneControl::OnKeyPress(const char key)
module->setPaused(!paused);
}
}
+
+ // In case the SceneManager is not apart of the driver
+ paused ? sceneManager->resume() : sceneManager->pause();
}
else if (key == 'q' || key == 'Q' || key == 'e' || key == 'E') // end Simulation
{
@@ -94,8 +93,7 @@ KeyboardSceneControl::OnKeyPress(const char key)
for (auto module : driver->getModules())
{
- std::shared_ptr<SceneManager> subManager = std::dynamic_pointer_cast<SceneManager>(module);
- if (subManager != nullptr)
+ if (auto subManager = std::dynamic_pointer_cast<SceneManager>(module))
{
if (simModeOn)
{
@@ -106,8 +104,7 @@ KeyboardSceneControl::OnKeyPress(const char key)
subManager->setMode(SceneManager::Mode::Simulation);
}
}
- std::shared_ptr<VTKViewer> viewer = std::dynamic_pointer_cast<VTKViewer>(module);
- if (viewer != nullptr)
+ if (auto viewer = std::dynamic_pointer_cast<VTKViewer>(module))
{
if (simModeOn)
{
@@ -119,6 +116,8 @@ KeyboardSceneControl::OnKeyPress(const char key)
}
}
}
+
+ simModeOn ? sceneManager->setMode(SceneManager::Mode::Debug) : sceneManager->setMode(SceneManager::Mode::Simulation);
}
else if (key == 'p' || key == 'P') // switch framerate display
{
diff --git a/Source/SimulationManager/imstkKeyboardSceneControl.h b/Source/SimulationManager/imstkKeyboardSceneControl.h
index 1396a8f89d7aebd55d47c7cee098b2919f6ab852..17f7dcbb913567b4eb1dc1178466efe20e36226b 100644
--- a/Source/SimulationManager/imstkKeyboardSceneControl.h
+++ b/Source/SimulationManager/imstkKeyboardSceneControl.h
@@ -40,7 +40,7 @@ class SceneManager;
class KeyboardSceneControl : public KeyboardControl
{
public:
- KeyboardSceneControl();
+ KeyboardSceneControl() = default;
KeyboardSceneControl(std::shared_ptr<KeyboardDeviceClient> keyDevice);
virtual ~KeyboardSceneControl() override = default;