Commit e7c01ead authored by Andrew Wilson's avatar Andrew Wilson 🐘
Browse files

Merge branch 'PBD-cloth-cutting' into 'master'

Pbd SurfaceMesh cutting using surface mesh and analytical geometries

See merge request !519
parents f61d1121 b6ee6d88
Pipeline #222324 created
###########################################################################
#
# 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.
#
###########################################################################
if(iMSTK_USE_OpenHaptics)
project(Example-PBDCutting)
#-----------------------------------------------------------------------------
# Create executable
#-----------------------------------------------------------------------------
imstk_add_executable(${PROJECT_NAME} PBDCuttingExample.cpp)
#-----------------------------------------------------------------------------
# Add the target to Examples folder
#-----------------------------------------------------------------------------
SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/Haptics)
#-----------------------------------------------------------------------------
# Link libraries to executable
#-----------------------------------------------------------------------------
target_link_libraries(${PROJECT_NAME} SimulationManager apiUtilities Filtering)
endif()
/*=========================================================================
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 "imstkHapticDeviceClient.h"
#include "imstkHapticDeviceManager.h"
#include "imstkKeyboardDeviceClient.h"
#include "imstkKeyboardSceneControl.h"
#include "imstkLight.h"
#include "imstkLogger.h"
#include "imstkMouseSceneControl.h"
#include "imstkNew.h"
#include "imstkPbdModel.h"
#include "imstkPbdObject.h"
#include "imstkPbdObjectCuttingPair.h"
#include "imstkRenderMaterial.h"
#include "imstkScene.h"
#include "imstkSceneManager.h"
#include "imstkSceneObjectController.h"
#include "imstkSimulationManager.h"
#include "imstkSurfaceMesh.h"
#include "imstkVisualModel.h"
#include "imstkVTKViewer.h"
using namespace imstk;
// Parameters to play with
const double width = 50.0;
const double height = 50.0;
const int nRows = 12;
const int nCols = 12;
///
/// \brief Creates cloth geometry
///
static std::shared_ptr<SurfaceMesh>
makeClothGeometry(const double width,
const double height,
const int nRows,
const int nCols)
{
imstkNew<SurfaceMesh> clothMesh("Cloth_SurfaceMesh");
imstkNew<VecDataArray<double, 3>> verticesPtr(nRows * nCols);
VecDataArray<double, 3>& vertices = *verticesPtr.get();
const double dy = width / static_cast<double>(nCols - 1);
const double dx = height / static_cast<double>(nRows - 1);
for (int i = 0; i < nRows; i++)
{
for (int j = 0; j < nCols; j++)
{
vertices[i * nCols + j] = Vec3d(dx * static_cast<double>(i), 1.0, dy * static_cast<double>(j));
}
}
// Add connectivity data
imstkNew<VecDataArray<int, 3>> indicesPtr;
VecDataArray<int, 3>& indices = *indicesPtr.get();
for (int i = 0; i < nRows - 1; i++)
{
for (int j = 0; j < nCols - 1; j++)
{
const int index1 = i * nCols + j;
const int index2 = index1 + nCols;
const int index3 = index1 + 1;
const int index4 = index2 + 1;
// Interleave [/][\] pattern
if (i % 2 ^ j % 2)
{
indices.push_back(Vec3i(index1, index2, index3));
indices.push_back(Vec3i(index4, index3, index2));
}
else
{
indices.push_back(Vec3i(index2, index4, index1));
indices.push_back(Vec3i(index4, index3, index1));
}
}
}
clothMesh->initialize(verticesPtr, indicesPtr);
return clothMesh;
}
///
/// \brief Creates cloth object
///
static std::shared_ptr<PbdObject>
makeClothObj(const std::string& name,
const double width,
const double height,
const int nRows,
const int nCols)
{
imstkNew<PbdObject> clothObj(name);
// Setup the Geometry
std::shared_ptr<SurfaceMesh> clothMesh(makeClothGeometry(width, height, nRows, nCols));
// Setup the Parameters
imstkNew<PBDModelConfig> pbdParams;
pbdParams->enableConstraint(PbdConstraint::Type::Distance, 1.0e3);
pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 1.0e3);
pbdParams->m_fixedNodeIds = { 0, static_cast<size_t>(nCols) - 1 };
pbdParams->m_uniformMassValue = width * height / ((double)nRows * (double)nCols);
pbdParams->m_gravity = Vec3d(0.0, -9.8, 0.0);
pbdParams->m_defaultDt = 0.005;
pbdParams->m_iterations = 5;
// Setup the Model
imstkNew<PbdModel> pbdModel;
pbdModel->setModelGeometry(clothMesh);
pbdModel->configure(pbdParams);
// Setup the VisualModel
imstkNew<RenderMaterial> material;
material->setBackFaceCulling(false);
material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
imstkNew<VisualModel> visualModel(clothMesh);
visualModel->setRenderMaterial(material);
// Setup the Object
clothObj->addVisualModel(visualModel);
clothObj->setPhysicsGeometry(clothMesh);
clothObj->setCollidingGeometry(clothMesh);
clothObj->setDynamicalModel(pbdModel);
return clothObj;
}
///
/// \brief This example demonstrates the concept of PBD cutting
/// for haptic interaction. NOTE: Requires GeoMagic Touch device
///
int
main()
{
// Setup logger (write to file and stdout)
Logger::startLogger();
// Scene
imstkNew<Scene> scene("PBDCutting");
// Create a cutting plane object in the scene
std::shared_ptr<SurfaceMesh> cutGeom(makeClothGeometry(40, 40, 2, 2));
cutGeom->setTranslation(Vec3d(-10, -20, 0));
cutGeom->updatePostTransformData();
imstkNew<CollidingObject> cutObj("CuttingObject");
cutObj->setVisualGeometry(cutGeom);
cutObj->setCollidingGeometry(cutGeom);
cutObj->getVisualModel(0)->getRenderMaterial()->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
scene->addSceneObject(cutObj);
// Create a pbd cloth object in the scene
std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", width, height, nRows, nCols);
scene->addSceneObject(clothObj);
// Add interaction pair for pbd cutting
imstkNew<PbdObjectCuttingPair> cuttingPair(clothObj, cutObj);
// Device Server
imstkNew<HapticDeviceManager> server;
std::shared_ptr<HapticDeviceClient> client = server->makeDeviceClient();
// Create the virtual coupling object controller
imstkNew<SceneObjectController> controller(cutObj, client);
scene->addController(controller);
// Camera
scene->getActiveCamera()->setPosition(Vec3d(1.0, 1.0, 1.0) * 100.0);
scene->getActiveCamera()->setFocalPoint(Vec3d(0, -50, 0));
// Light
imstkNew<DirectionalLight> light("light");
light->setFocalPoint(Vec3d(5.0, -8.0, -5.0));
light->setIntensity(1.0);
scene->addLight(light);
//Run the simulation
{
// Setup a viewer to render in its own thread
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);
sceneManager->pause(); // Start simulation paused
imstkNew<SimulationManager> driver;
driver->addModule(server);
driver->addModule(viewer);
driver->addModule(sceneManager);
driver->setDesiredDt(0.001);
// 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);
}
// Queue keypress to be called after scene thread
queueConnect<KeyEvent>(viewer->getKeyboardDevice(), &KeyboardDeviceClient::keyPress, sceneManager, [&](KeyEvent* e)
{
// When i is pressed replace the PBD cloth with a cut one
if (e->m_key == 'i' && e->m_keyPressType == KEY_PRESS)
{
cuttingPair->apply();
}
});
driver->start();
}
return 0;
}
......@@ -158,7 +158,6 @@ main()
// Scene
imstkNew<Scene> scene("PBDPicking");
scene->getConfig()->writeTaskGraph = true;
// Create the virtual coupling object controller
......@@ -177,7 +176,7 @@ main()
geomShaft->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
geomShaft->setTranslation(Vec3d(0.0, 0.0, 10.0));
imstkNew<CollidingObject> objShaft("ShaftObject");
objShaft->setVisualGeometry(geomShaft);
objShaft->setVisualGeometry(pivotSurfMesh);
objShaft->setCollidingGeometry(geomShaft);
scene->addSceneObject(objShaft);
......@@ -187,7 +186,7 @@ main()
geomUpperJaw->setRadius(2.0);
geomUpperJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
imstkNew<CollidingObject> objUpperJaw("UpperJawObject");
objUpperJaw->setVisualGeometry(geomUpperJaw);
objUpperJaw->setVisualGeometry(upperSurfMesh);
objUpperJaw->setCollidingGeometry(geomUpperJaw);
scene->addSceneObject(objUpperJaw);
......@@ -197,7 +196,7 @@ main()
geomLowerJaw->setRadius(2.0);
geomLowerJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
imstkNew<CollidingObject> objLowerJaw("LowerJawObject");
objLowerJaw->setVisualGeometry(geomLowerJaw);
objLowerJaw->setVisualGeometry(lowerSurfMesh);
objLowerJaw->setCollidingGeometry(geomLowerJaw);
scene->addSceneObject(objLowerJaw);
......
......@@ -99,6 +99,7 @@ main()
// 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);
......
......@@ -91,13 +91,19 @@ LaparoscopicToolController::update(const double dt)
// TRS decompose and set upper/lower jaw geometries
{
m_upperJaw->getVisualGeometry()->setTransform(m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawVisualTransform);
m_upperJaw->getCollidingGeometry()->setTransform(m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawCollidingTransform);
const Mat4d upperWorldTransform = m_controllerWorldTransform * m_upperJawLocalTransform;
m_upperJaw->getVisualGeometry()->setTransform(upperWorldTransform * m_upperJawVisualTransform);
m_upperJaw->getCollidingGeometry()->setTransform(upperWorldTransform * m_upperJawCollidingTransform);
}
{
m_lowerJaw->getVisualGeometry()->setTransform(m_controllerWorldTransform * m_lowerJawLocalTransform * m_upperJawVisualTransform);
m_lowerJaw->getCollidingGeometry()->setTransform(m_controllerWorldTransform * m_lowerJawLocalTransform * m_lowerJawCollidingTransform);
const Mat4d lowerWorldTransform = m_controllerWorldTransform * m_lowerJawLocalTransform;
m_lowerJaw->getVisualGeometry()->setTransform(lowerWorldTransform * m_upperJawVisualTransform);
m_lowerJaw->getCollidingGeometry()->setTransform(lowerWorldTransform * m_lowerJawCollidingTransform);
}
m_shaft->getVisualGeometry()->postModified();
m_lowerJaw->getVisualGeometry()->postModified();
m_upperJaw->getVisualGeometry()->postModified();
}
void
......
......@@ -36,6 +36,9 @@
#include "imstkTaskGraph.h"
#include "imstkTetrahedralMesh.h"
#include <map>
#include <set>
namespace imstk
{
PbdModel::PbdModel() : DynamicalModel(DynamicalModelType::PositionBasedDynamics),
......@@ -520,18 +523,24 @@ PbdModel::initializeDihedralConstraints(const double stiffness)
rs.resize(static_cast<size_t>(it - rs.begin()));
if (rs.size() > 1)
{
size_t idx = (rs[0] == k) ? 1 : 0;
const auto& tri = elements[rs[idx]];
size_t idx = (rs[0] == k) ? 1 : 0;
const auto& tri0 = elements[k];
const auto& tri1 = elements[rs[idx]];
size_t idx0 = 0;
size_t idx1 = 0;
for (size_t i = 0; i < 3; ++i)
{
if (tri[i] != tri[0] && tri[i] != tri[1])
if (tri0[i] != i1 && tri0[i] != i2)
{
idx = i;
break;
idx0 = tri0[i];
}
if (tri1[i] != i1 && tri1[i] != i2)
{
idx1 = tri1[i];
}
}
auto c = std::make_shared<PbdDihedralConstraint>();
c->initConstraint(*m_initialState->getPositions(), tri[2], tri[idx], tri[0], tri[1], stiffness);
c->initConstraint(*m_initialState->getPositions(), idx0, idx1, i1, i2, stiffness);
m_constraints->push_back(std::move(c));
}
}
......@@ -570,6 +579,199 @@ PbdModel::initializeConstantDensityConstraint(const double stiffness)
return true;
}
void
PbdModel::removeConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
{
// constraint removal predicate
auto removeConstraint =
[&](std::shared_ptr<PbdConstraint> constraint)
{
for (auto i : constraint->getVertexIds())
{
if (vertices->find(i) != vertices->end())
{
return true;
}
}
return false;
};
// remove constraints from constraint-partition maps and constraint vectors.
m_constraints->erase(std::remove_if(m_constraints->begin(), m_constraints->end(), removeConstraint),
m_constraints->end());
for (auto& pc : *m_partitionedConstraints)
{
pc.erase(std::remove_if(pc.begin(), pc.end(), removeConstraint), pc.end());
}
}
void
PbdModel::addConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
{
// check if constraint type matches the mesh type
CHECK(m_mesh->getTypeName() == "SurfaceMesh")
<< "Add element constraints does not support current mesh type.";
const auto& triMesh = std::static_pointer_cast<SurfaceMesh>(m_mesh);
const auto nV = triMesh->getNumVertices();
const auto elements = triMesh->getTriangleIndices();
std::vector<std::vector<size_t>> onering(nV);
// build onering
for (auto& vertOnering : onering)
{
vertOnering.reserve(10);
}
for (int k = 0; k < elements->size(); ++k)
{
auto& tri = (*elements)[k];
onering[tri[0]].push_back(k);
onering[tri[1]].push_back(k);
onering[tri[2]].push_back(k);
}
for (auto& vertOnering : onering)
{
std::sort(vertOnering.begin(), vertOnering.end());
}
// functions for adding constraints
auto addDistanceConstraint =
[&](size_t i1, size_t i2, double stiffness)
{
auto c = std::make_shared<PbdDistanceConstraint>();
c->initConstraint(*m_initialState->getPositions(), i1, i2, stiffness);
m_constraints->push_back(std::move(c));
};
auto addAreaConstraint =
[&](size_t k, double stiffness)
{
auto& tri = (*elements)[k];
auto c = std::make_shared<PbdAreaConstraint>();
c->initConstraint(*m_initialState->getPositions(), tri[0], tri[1], tri[2], stiffness);
m_constraints->push_back(std::move(c));
};
auto addDihedralConstraint =
[&](size_t t0, size_t t1, size_t i1, size_t i2, double stiffness)
{
const auto& tri0 = (*elements)[t0];
const auto& tri1 = (*elements)[t1];
size_t idx0 = 0;
size_t idx1 = 0;
for (size_t i = 0; i < 3; ++i)
{
if (tri0[i] != i1 && tri0[i] != i2)
{
idx0 = tri0[i];
}
if (tri1[i] != i1 && tri1[i] != i2)
{
idx1 = tri1[i];
}
}
auto c = std::make_shared<PbdDihedralConstraint>();
c->initConstraint(*m_initialState->getPositions(), idx0, idx1, i1, i2, stiffness);
m_constraints->push_back(std::move(c));
};
// count constraints to be added for pre-allocation
std::set<std::pair<size_t, size_t>> distanceSet;
std::unordered_set<size_t> areaSet;
std::map<std::pair<size_t, size_t>, std::pair<size_t, size_t>> dihedralSet;
for (auto& constraint : m_parameters->m_regularConstraints)
{
switch (constraint.first)
{
case PbdConstraint::Type::Distance:
for (const auto& vertIdx : *vertices)
{
for (const auto& triIdx : onering[vertIdx])
{
const auto& tri = (*elements)[triIdx];
size_t i1 = 0;
size_t i2 = 0;
for (size_t i = 0; i < 3; i++)
{
if (tri[i] == vertIdx)
{
i1 = tri[(i + 1) % 3];
i2 = tri[(i + 2) % 3];
break;
}
}
auto pair1 = std::make_pair(std::min(vertIdx, i1), std::max(vertIdx, i1));
auto pair2 = std::make_pair(std::min(vertIdx, i2), std::max(vertIdx, i2));
distanceSet.insert(pair1);
distanceSet.insert(pair2);
}
}
break;
case PbdConstraint::Type::Area:
for (const auto& vertIdx : *vertices)
{
for (const auto& triIdx : onering[vertIdx])
{
areaSet.insert(triIdx);
}
}
break;
case PbdConstraint::Type::Dihedral:
for (const auto& vertIdx : *vertices)
{
for (const auto& triIdx : onering[vertIdx])
{
const auto& tri = (*elements)[triIdx];
for (size_t i = 0; i < 3; i++)
{
size_t j = (i + 1) % 3;
size_t i0 = tri[i];
size_t i1 = tri[j];
if (i0 > i1)
{
std::swap(i0, i1);
}
auto& r0 = onering[i0];
auto& r1 = onering[i1];
std::vector<size_t> rs(2);
auto it = std::set_intersection(r0.begin(), r0.end(), r1.begin(), r1.end(), rs.begin());
rs.resize(static_cast<size_t>(it - rs.begin()));
if (rs.size() > 1)
{
dihedralSet[std::make_pair(i0, i1)] = std::make_pair(rs[0], rs[1]);
}
}
}
}
break;
default:
break;
}
}
// add constraints
m_constraints->reserve(m_constraints->size() + distanceSet.size() + areaSet.size() + dihedralSet.size());
for (auto& constraint : m_parameters->m_regularConstraints)
{
switch (constraint.first)
{
case PbdConstraint::Type::Distance:
for (auto& c : distanceSet)
{
addDistanceConstraint(c.first, c.second, constraint.second);
}
case PbdConstraint::Type::Area:
for (auto& c : areaSet)
{
addAreaConstraint(c, constraint.second);
}
case PbdConstraint::Type::Dihedral:
for (auto& c : dihedralSet)
{
addDihedralConstraint(c.second.first, c.second.second, c.first.first, c.first.second, constraint.second);
}
}
}
}