diff --git a/Examples/PBD/PBDTissueStitch/PBDTissueStitchExample.cpp b/Examples/PBD/PBDTissueStitch/PBDTissueStitchExample.cpp
index 41f2673756f81d24cb29d971aa4004a02484208f..860e972e2ad04e37b55f907dfed735ed02178d50 100644
--- a/Examples/PBD/PBDTissueStitch/PBDTissueStitchExample.cpp
+++ b/Examples/PBD/PBDTissueStitch/PBDTissueStitchExample.cpp
@@ -22,11 +22,9 @@
#include "imstkCamera.h"
#include "imstkCapsule.h"
#include "imstkDirectionalLight.h"
-#include "imstkImageData.h"
#include "imstkKeyboardDeviceClient.h"
#include "imstkKeyboardSceneControl.h"
#include "imstkLineMesh.h"
-#include "imstkMeshIO.h"
#include "imstkMouseSceneControl.h"
#include "imstkNew.h"
#include "imstkOneToOneMap.h"
@@ -41,20 +39,17 @@
#include "imstkTetrahedralMesh.h"
#include "imstkVisualModel.h"
#include "imstkVTKViewer.h"
-#include "imstkPbdBaryPointToPointConstraint.h"
-#include "imstkPointPicker.h"
-#include "imstkPbdObjectGrasping.h"
-#include "imstkSphere.h"
#include "imstkHapticDeviceClient.h"
#include "imstkHapticDeviceManager.h"
#include "imstkRigidObjectController.h"
#include "imstkRigidObject2.h"
#include "imstkRigidBodyModel2.h"
#include "imstkRbdConstraint.h"
+#include "imstkPbdObjectStitching.h"
using namespace imstk;
-//#define USE_FEM
+#define USE_FEM
///
/// \brief Creates a tetraheral grid
@@ -173,21 +168,19 @@ makeTissueObj(const std::string& name,
#ifdef USE_FEM
// Use FEMTet constraints (42k - 85k for tissue, but we want
// something much more stretchy to wrap)
- pbdParams->m_femParams->m_YoungModulus = 100000.0;
+ pbdParams->m_femParams->m_YoungModulus = 1000.0;
pbdParams->m_femParams->m_PoissonRatio = 0.4; // 0.48 for tissue
pbdParams->enableFemConstraint(PbdFemConstraint::MaterialType::StVK);
#else
- // Use volume+distance constraints, worse results. More performant (can use larger mesh)
- // Handles inversion better
- pbdParams->enableConstraint(PbdModelConfig::ConstraintGenType::Volume, 100000.0);
- pbdParams->enableConstraint(PbdModelConfig::ConstraintGenType::Distance, 100000.0);
+ pbdParams->enableConstraint(PbdModelConfig::ConstraintGenType::Volume, 0.01);
+ pbdParams->enableConstraint(PbdModelConfig::ConstraintGenType::Distance, 0.4);
#endif
pbdParams->m_doPartitioning = false;
- pbdParams->m_uniformMassValue = 100.0;
- pbdParams->m_gravity = Vec3d(0.0, 0.0, 0.0);
+ pbdParams->m_uniformMassValue = 0.00001;
+ pbdParams->m_gravity = Vec3d(0.0, -1.0, 0.0);;
pbdParams->m_dt = 0.001;
pbdParams->m_iterations = 5;
- pbdParams->m_contactStiffness = 0.1;
+ //pbdParams->m_contactStiffness = 0.4;
pbdParams->m_viscousDampingCoeff = 0.05;
// Fix the borders
@@ -197,7 +190,7 @@ makeTissueObj(const std::string& name,
{
for (int x = 0; x < dim[0]; x++)
{
- if (x == 0 || /*z == 0 ||*/ x == dim[0] - 1 /*|| z == dim[2] - 1*/)
+ if (x == 0)
{
pbdParams->m_fixedNodeIds.push_back(x + dim[0] * (y + dim[1] * z));
}
@@ -212,10 +205,10 @@ makeTissueObj(const std::string& name,
// Setup the material
imstkNew<RenderMaterial> material;
- material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+ material->setDisplayMode(RenderMaterial::DisplayMode::Wireframe);
material->setColor(Color(0.77, 0.53, 0.34));
material->setEdgeColor(Color(0.87, 0.63, 0.44));
- //material->setOpacity(0.5);
+ material->setOpacity(0.5);
// Setup the Object
tissueObj->setVisualGeometry(surfMesh);
@@ -271,6 +264,8 @@ main()
// Setup logger (write to file and stdout)
Logger::startLogger();
+ const double capsuleRadius = 0.02;
+
// Setup the scene
imstkNew<Scene> scene("PbdTissueStitch");
scene->getActiveCamera()->setPosition(0.0012, 0.0451, 0.1651);
@@ -279,13 +274,13 @@ main()
// Setup a tet tissue
std::shared_ptr<PbdObject> tissueObj = makeTissueObj("Tissue",
- Vec3d(0.1, 0.01, 0.07), Vec3i(12, 2, 8), Vec3d(0.0, 0.0, 0.0));
+ Vec3d(0.2, 0.01, 0.07), Vec3i(20, 2, 5), Vec3d(0.1, -0.01 - capsuleRadius, 0.0));
scene->addSceneObject(tissueObj);
auto cdObj = std::make_shared<CollidingObject>("Bone");
auto capsuleGeom = std::make_shared<Capsule>();
- capsuleGeom->setPosition(0.0, 0.03, 0.0);
- capsuleGeom->setRadius(0.01);
+ capsuleGeom->setPosition(0.0, 0.0, 0.0);
+ capsuleGeom->setRadius(capsuleRadius);
capsuleGeom->setLength(0.08);
capsuleGeom->setOrientation(Quatd(Rotd(PI_2, Vec3d(1.0, 0.0, 0.0))));
cdObj->setVisualGeometry(capsuleGeom);
@@ -299,10 +294,15 @@ main()
// Setup CD with a cylinder CD object
auto interaction = std::make_shared<PbdObjectCollision>(tissueObj, cdObj, "SurfaceMeshToCapsuleCD");
+ interaction->setFriction(0.0);
+ interaction->setRestitution(0.0);
scene->addInteraction(interaction);
- auto grasping = std::make_shared<PbdObjectGrasping>(tissueObj);
- scene->addInteraction(grasping);
+ /* auto grasping = std::make_shared<PbdObjectGrasping>(tissueObj);
+ scene->addInteraction(grasping);*/
+
+ auto stitching = std::make_shared<PbdObjectStitching>(tissueObj);
+ scene->addInteraction(stitching);
// Light
imstkNew<DirectionalLight> light1;
@@ -333,8 +333,6 @@ main()
driver->addModule(sceneManager);
driver->setDesiredDt(0.001);
- bool performStitch = false;
-
#ifdef iMSTK_USE_OPENHAPTICS
imstkNew<HapticDeviceManager> hapticManager;
hapticManager->setSleepDelay(0.1); // Delay for 1ms (haptics thread is limited to max 1000hz)
@@ -356,225 +354,81 @@ main()
{
if (e->m_button == 0 && e->m_buttonState == BUTTON_PRESSED)
{
- performStitch = true;
+ auto toolGeom = std::dynamic_pointer_cast<LineMesh>(toolObj->getCollidingGeometry());
+ const Vec3d& v1 = toolGeom->getVertexPosition(0);
+ const Vec3d& v2 = toolGeom->getVertexPosition(1);
+ stitching->beginRayPointStitch(v1, (v2 - v1).normalized());
}
});
+#else
#endif
- // Record the tool position relative to the camera
- // As the camera moves, reapply that relative transform
-
- const std::vector<size_t>& fixedNodes = tissueObj->getPbdModel()->getConfig()->m_fixedNodeIds;
- std::vector<Vec3d> initPositions;
-
- auto tetMesh =
- std::dynamic_pointer_cast<TetrahedralMesh>(tissueObj->getPhysicsGeometry());
- std::shared_ptr<VecDataArray<double, 3>> verticesPtr = tetMesh->getVertexPositions();
- VecDataArray<double, 3>& vertices = *verticesPtr;
- for (size_t i = 0; i < fixedNodes.size(); i++)
- {
- initPositions.push_back(vertices[fixedNodes[i]]);
- }
-
+ // Toggle gravity, perform stitch, & reset
+ double t = 0.0;
connect<KeyEvent>(viewer->getKeyboardDevice(), &KeyboardDeviceClient::keyPress,
[&](KeyEvent* e)
{
if (e->m_key == 'g')
{
- tissueObj->getPbdModel()->getConfig()->m_gravity = Vec3d(0.0, -1.0, 0.0);
- }
-#ifdef iMSTK_USE_OPENHAPTICS
- else if (e->m_key == 's')
- {
- performStitch = true;
- }
-#endif
- });
-
- // Script the movement of the tissues fixed points
- // Move upwards, then inwards
-
- std::shared_ptr<PbdBaryPointToPointConstraint> stitchConstraint = nullptr;
- connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
- {
- const double dt = sceneManager->getDt();
-
- for (size_t i = 0; i < fixedNodes.size(); i++)
- {
- Vec3d initPos = initPositions[i];
- Vec3d& pos = vertices[fixedNodes[i]];
-
- const double dy = std::abs(pos[1] - initPos[1]);
- const double dx = std::abs(pos[0] - initPos[0]);
- if (dy < 0.04)
- {
- pos[1] += 0.01 * dt;
- }
- else if (dx < 0.03)
+ Vec3d& g = tissueObj->getPbdModel()->getConfig()->m_gravity;
+ if (g.norm() > 0.0)
{
- if (initPos[0] < 0.0)
- {
- pos[0] += 0.01 * dt;
- }
- else
- {
- pos[0] -= 0.01 * dt;
- }
- if (initPos[1] < 0.0)
- {
- pos[1] += 0.005 * dt;
- }
+ tissueObj->getPbdModel()->getConfig()->m_gravity = Vec3d(0.0, 0.0, 0.0);
}
else
{
- tissueObj->getPbdModel()->setPointUnfixed(fixedNodes[i]);
+ tissueObj->getPbdModel()->getConfig()->m_gravity = Vec3d(0.0, -1.0, 0.0);
}
}
-
- if (performStitch && stitchConstraint == nullptr)
+ else if (e->m_key == 's')
{
- std::shared_ptr<VecDataArray<int, 4>> indicesPtr = tetMesh->getTetrahedraIndices();
-
- auto velocitiesPtr =
- std::dynamic_pointer_cast<VecDataArray<double, 3>>(tetMesh->getVertexAttribute("Velocities"));
- CHECK(velocitiesPtr != nullptr) << "Trying to stitch with geometry that has no Velocities";
-
- auto invMassesPtr =
- std::dynamic_pointer_cast<DataArray<double>>(tetMesh->getVertexAttribute("InvMass"));
- CHECK(invMassesPtr != nullptr) << "Trying to stitch with geometry that has no InvMass";
-
auto toolGeom = std::dynamic_pointer_cast<LineMesh>(toolObj->getCollidingGeometry());
-
- // Perform line intersection to place a stitch along intersection points in line
- PointPicker picker;
const Vec3d& v1 = toolGeom->getVertexPosition(0);
const Vec3d& v2 = toolGeom->getVertexPosition(1);
- picker.setPickingRay(v1, (v2 - v1).normalized());
- picker.setUseFirstHit(false);
- /*const std::vector<PickData>& pickData = picker.pick(tetMesh);
-
- // Select the intersection points closest together but on opposites sides of the origin
- std::pair<PickData, PickData> closestPair;
- double minDist = IMSTK_DOUBLE_MAX;
- for (size_t i = 0; i < pickData.size(); i++)
- {
- const Vec3d& pickPt1 = pickData[i].pickPoint;
- if (pickPt1[0] > 0.0)
- {
- continue;
- }
- for (size_t j = 0; j < pickData.size(); j++)
- {
- const Vec3d& pickPt2 = pickData[j].pickPoint;
- if (i == j || pickPt2[0] < 0.0)
- {
- continue;
- }
-
- const double dist = (pickPt2 - pickPt1).norm();
- if (dist < minDist)
- {
- minDist = dist;
- closestPair = { pickData[i], pickData[j] };
- }
- }
- }*/
+ stitching->beginRayPointStitch(v1, (v2 - v1).normalized());
+ }
+ else if (e->m_key == 'r')
+ {
+ t = 0.0;
+ }
+ });
- {
- // Perform the picking only on surface data
- std::shared_ptr<SurfaceMesh> surfMesh = tetMesh->extractSurfaceMesh();
- surfMesh->computeTrianglesNormals();
-
- const std::vector<PickData>& pickData = picker.pick(surfMesh);
-
- // ** Warning **, surface triangles are not 100% garunteed to tell inside/out
- // Use angle-weighted psuedonormals as done in MeshToMeshBruteForceCD
- std::shared_ptr<VecDataArray<double, 3>> faceNormalsPtr = surfMesh->getCellNormals();
- const VecDataArray<double, 3>& faceNormals = *faceNormalsPtr;
-
- // Find all neighbor pairs with normals facing each other
- std::vector<std::pair<PickData, PickData>> constraintPair;
- for (size_t i = 0, j = 1; i < pickData.size() - 1; i++, j++)
- {
- const Vec3d& pt_i = pickData[i].pickPoint;
- const Vec3d& pt_j = pickData[j].pickPoint;
- const Vec3d& normal_i = faceNormals[pickData[i].ids[0]];
- const Vec3d& normal_j = faceNormals[pickData[j].ids[0]];
- const Vec3d diff = pt_j - pt_i;
-
- //bool faceOpposite = (normal_i.dot(normal_j) < 0.0);
- std::cout << "diff: " << diff.transpose() << std::endl;
- std::cout << "normal_i: " << normal_i.transpose() << std::endl;
- std::cout << "normal_j: " << normal_j.transpose() << std::endl;
- bool faceInwards = (diff.dot(normal_i) > 0.0) && (diff.dot(normal_j) < 0.0);
-
- // If they face in opposite directions
- if (faceInwards)
- {
- constraintPair.push_back({ pickData[i], pickData[j] });
- }
- }
-
- for (size_t i = 0; i < constraintPair.size(); i++)
- {
- // Get the tet id from the triangle id
- size_t triId1 = constraintPair[i].first.ids[0];
- size_t tetId1 = 0;
- size_t triId2 = constraintPair[i].second.ids[0];
- size_t tetId2 = 0;
-
- std::vector<VertexMassPair> cell1(4);
- for (size_t i = 0; i < 4; i++)
- {
- const int vertexId = (*indicesPtr)[tetId1][i];
- cell1[i] = { &(*verticesPtr)[vertexId], (*invMassesPtr)[vertexId], &(*velocitiesPtr)[vertexId] };
- }
- const Vec4d bcCoord1 = baryCentric(constraintPair[i].first.pickPoint,
- *cell1[0].vertex, *cell1[1].vertex, *cell1[2].vertex, *cell1[3].vertex);
- std::vector<double> weights1 = { bcCoord1[0], bcCoord1[1], bcCoord1[2], bcCoord1[3] };
-
- std::vector<VertexMassPair> cell2(4);
- for (size_t i = 0; i < 4; i++)
- {
- const int vertexId = (*indicesPtr)[tetId2][i];
- cell2[i] = { &(*verticesPtr)[vertexId], (*invMassesPtr)[vertexId], &(*velocitiesPtr)[vertexId] };
- }
- const Vec4d bcCoord2 = baryCentric(constraintPair[i].second.pickPoint,
- *cell2[0].vertex, *cell2[1].vertex, *cell2[2].vertex, *cell2[3].vertex);
- std::vector<double> weights2 = { bcCoord2[0], bcCoord2[1], bcCoord2[2], bcCoord2[3] };
-
- stitchConstraint = std::make_shared<PbdBaryPointToPointConstraint>();
- stitchConstraint->initConstraint(cell1, weights1, cell2, weights2, 0.1, 0.1);
- }
- }
+ // Record the intial positions
+ std::vector<Vec3d> initPositions;
- // Assume tet to tet
- /* std::vector<VertexMassPair> cell1(4);
- for (size_t i = 0; i < 4; i++)
- {
- const int vertexId = (*indicesPtr)[closestPair.first.ids[0]][i];
- cell1[i] = { &(*verticesPtr)[vertexId], (*invMassesPtr)[vertexId], &(*velocitiesPtr)[vertexId] };
- }
- const Vec4d bcCoord1 = baryCentric(closestPair.first.pickPoint,
- *cell1[0].vertex, *cell1[1].vertex, *cell1[2].vertex, *cell1[3].vertex);
- std::vector<double> weights1 = { bcCoord1[0], bcCoord1[1], bcCoord1[2], bcCoord1[3] };
+ auto tetMesh =
+ std::dynamic_pointer_cast<TetrahedralMesh>(tissueObj->getPhysicsGeometry());
+ std::shared_ptr<VecDataArray<double, 3>> verticesPtr = tetMesh->getVertexPositions();
+ VecDataArray<double, 3>& vertices = *verticesPtr;
+ const std::vector<size_t>& fixedNodes = tissueObj->getPbdModel()->getConfig()->m_fixedNodeIds;
+ for (size_t i = 0; i < fixedNodes.size(); i++)
+ {
+ initPositions.push_back(vertices[fixedNodes[i]]);
+ }
- std::vector<VertexMassPair> cell2(4);
- for (size_t i = 0; i < 4; i++)
+ // Script the movement of the tissues fixed points
+ connect<Event>(sceneManager, &SceneManager::postUpdate,
+ [&](Event*)
+ {
+ const double dt = sceneManager->getDt();
+ t += dt;
+ if (t < 9.0)
+ {
+ for (size_t i = 0; i < fixedNodes.size(); i++)
{
- const int vertexId = (*indicesPtr)[closestPair.second.ids[0]][i];
- cell2[i] = { &(*verticesPtr)[vertexId], (*invMassesPtr)[vertexId], &(*velocitiesPtr)[vertexId] };
- }
- const Vec4d bcCoord2 = baryCentric(closestPair.second.pickPoint,
- *cell2[0].vertex, *cell2[1].vertex, *cell2[2].vertex, *cell2[3].vertex);
- std::vector<double> weights2 = { bcCoord2[0], bcCoord2[1], bcCoord2[2], bcCoord2[3] };
+ Vec3d initPos = initPositions[i];
+ Vec3d& pos = vertices[fixedNodes[i]];
- stitchConstraint = std::make_shared<PbdBaryPointToPointConstraint>();
- stitchConstraint->initConstraint(cell1, weights1, cell2, weights2, 0.1, 0.1);*/
+ const double r = (capsuleGeom->getPosition().head<2>() - initPos.head<2>()).norm();
+ pos = Vec3d(-sin(t) * r, -cos(t) * r, initPos[2]);
+ }
}
- if (stitchConstraint != nullptr)
+ else if ( t > 11.0)
{
- stitchConstraint->solvePosition();
+ for (size_t i = 0; i < fixedNodes.size(); i++)
+ {
+ tissueObj->getPbdModel()->setPointUnfixed(fixedNodes[i]);
+ }
}
});
diff --git a/Source/Geometry/Mesh/imstkTetrahedralMesh.cpp b/Source/Geometry/Mesh/imstkTetrahedralMesh.cpp
index 78117da7baffa742c051e329bffcc07277de7df4..da1b8e7398344520c5f5e7cb9de9f26f1e50691e 100644
--- a/Source/Geometry/Mesh/imstkTetrahedralMesh.cpp
+++ b/Source/Geometry/Mesh/imstkTetrahedralMesh.cpp
@@ -80,6 +80,7 @@ TetrahedralMesh::getVolume()
return volume;
}
+#pragma optimize("", off)
std::shared_ptr<SurfaceMesh>
TetrahedralMesh::extractSurfaceMesh()
{
@@ -89,12 +90,11 @@ TetrahedralMesh::extractSurfaceMesh()
const std::array<int, 4> unusedVert = { 3, 2, 1, 0 };
// Find and store the tetrahedral faces that are unique
- const VecDataArray<int, 4>& tetraIndices = *this->getTetrahedraIndices();
- std::shared_ptr<VecDataArray<double, 3>> tetVerticesPtr = getVertexPositions();
- const VecDataArray<double, 3>& tetVertices = *tetVerticesPtr;
- std::shared_ptr<VecDataArray<int, 3>> triIndicesPtr = std::make_shared<VecDataArray<int, 3>>();
- VecDataArray<int, 3>& triIndices = *triIndicesPtr;
- std::vector<size_t> surfaceTriTet; // Map tri to tet id
+ const VecDataArray<int, 4>& tetraIndices = *m_tetrahedraIndices;
+ const VecDataArray<double, 3>& tetVertices = *m_vertexPositions;
+ auto triIndicesPtr = std::make_shared<VecDataArray<int, 3>>();
+ VecDataArray<int, 3>& triIndices = *triIndicesPtr;
+ std::vector<size_t> surfaceTriTet; // Map tri to tet id
// Gives surfaceTri id/faceid -> index of unused vert for face (4 verts per tet, one will be unused)
std::vector<size_t> tetRemainingVert;
@@ -217,7 +217,8 @@ TetrahedralMesh::extractSurfaceMesh()
}
}
- auto triVerticesPtr = std::make_shared<VecDataArray<double, 3>>(static_cast<int>(oldToNewVertId.size()));
+ auto triVerticesPtr =
+ std::make_shared<VecDataArray<double, 3>>(static_cast<int>(oldToNewVertId.size()));
VecDataArray<double, 3>& triVertices = *triVerticesPtr;
for (auto vertIndexPair : oldToNewVertId)
@@ -235,6 +236,7 @@ TetrahedralMesh::extractSurfaceMesh()
surfMesh->initialize(triVerticesPtr, triIndicesPtr);
return surfMesh;
}
+#pragma optimize("", on)
Vec4d
TetrahedralMesh::computeBarycentricWeights(const size_t& tetId, const Vec3d& pos) const
diff --git a/Source/GeometryMappers/imstkSurfaceToTetraMap.cpp b/Source/GeometryMappers/imstkSurfaceToTetraMap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..692261fc67d95aa30bddc565b1a5677bbf02b217
--- /dev/null
+++ b/Source/GeometryMappers/imstkSurfaceToTetraMap.cpp
@@ -0,0 +1,111 @@
+/*=========================================================================
+ Library: iMSTK
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+=========================================================================*/
+
+#include "imstkSurfaceToTetraMap.h"
+#include "imstkTetrahedralMesh.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkVecDataArray.h"
+
+namespace imstk
+{
+SurfaceToTetraMap::SurfaceToTetraMap()
+{
+ setRequiredInputType<TetrahedralMesh>(0);
+ setRequiredInputType<SurfaceMesh>(1);
+}
+SurfaceToTetraMap::SurfaceToTetraMap(
+ std::shared_ptr<Geometry> parent,
+ std::shared_ptr<Geometry> child)
+{
+ setParentGeometry(parent);
+ setChildGeometry(child);
+
+ setRequiredInputType<TetrahedralMesh>(0);
+ setRequiredInputType<SurfaceMesh>(1);
+}
+
+void
+SurfaceToTetraMap::compute()
+{
+ OneToOneMap::compute();
+
+ m_triToTetMap.clear();
+ computeTriToTetMap(m_triToTetMap);
+}
+
+void
+SurfaceToTetraMap::computeTriToTetMap(std::unordered_map<int, int>& triToTetMap)
+{
+ triToTetMap.clear();
+
+ const std::array<Vec3i, 4> facePattern = {
+ Vec3i(0, 1, 2), Vec3i(0, 1, 3), Vec3i(0, 2, 3), Vec3i(1, 2, 3)
+ };
+
+ auto tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(getParentGeometry());
+ auto surfMesh = std::dynamic_pointer_cast<SurfaceMesh>(getChildGeometry());
+
+ //std::shared_ptr<VecDataArray<double, 3>> tetVerticesPtr = tetMesh->getVertexPositions();
+ //const VecDataArray<double, 3>& tetVertices = *tetVerticesPtr;
+ std::shared_ptr<VecDataArray<int, 4>> tetIndicesPtr = tetMesh->getTetrahedraIndices();
+ const VecDataArray<int, 4>& tetIndices = *tetIndicesPtr;
+
+ //std::shared_ptr<VecDataArray<double, 3>> surfVerticesPtr = surfMesh->getVertexPositions();
+ //const VecDataArray<double, 3>& surfVertices = *surfVerticesPtr;
+ std::shared_ptr<VecDataArray<int, 3>> surfIndicesPtr = surfMesh->getTriangleIndices();
+ const VecDataArray<int, 3>& surfIndices = *surfIndicesPtr;
+
+ // Brute force
+ for (int i = 0; i < surfIndices.size(); i++)
+ {
+ const Vec3i& tri = surfIndices[i];
+
+ // Find the corresponding parent id
+ // These are the vertex ids of the tet mesh
+ const int id0 = getParentVertexId(tri[0]);
+ const int id1 = getParentVertexId(tri[1]);
+ const int id2 = getParentVertexId(tri[2]);
+
+ // Hash the triangle with the tet mesh ids
+ TriCell cell_a(id0, id1, id2);
+
+ // Find the corresponding tet
+ for (int j = 0; j < tetIndices.size(); j++)
+ {
+ const Vec4i& tet = tetIndices[j];
+
+ // For every face of the tet
+ for (int k = 0; k < 4; k++)
+ {
+ TriCell cell_b(
+ tet[facePattern[k][0]],
+ tet[facePattern[k][1]],
+ tet[facePattern[k][2]]);
+ if (cell_a == cell_b)
+ {
+ triToTetMap[i] = j;
+ }
+ }
+ }
+ }
+}
+
+int
+SurfaceToTetraMap::getParentTetId(const int triId) const
+{
+ auto citer = m_triToTetMap.find(triId);
+ return (citer != m_triToTetMap.end()) ? citer->second : -1;
+}
+}
\ No newline at end of file
diff --git a/Source/GeometryMappers/imstkSurfaceToTetraMap.h b/Source/GeometryMappers/imstkSurfaceToTetraMap.h
new file mode 100644
index 0000000000000000000000000000000000000000..2c6a207bca4c7be1e47db7f908b5e2caeaf5f919
--- /dev/null
+++ b/Source/GeometryMappers/imstkSurfaceToTetraMap.h
@@ -0,0 +1,57 @@
+/*=========================================================================
+ 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 "imstkOneToOneMap.h"
+#include "imstkMath.h"
+#include "imstkTypes.h"
+
+namespace imstk
+{
+///
+/// \class SurfaceToTetraMap
+///
+/// \brief SurfaceToTetrahedralMap serves as a OneToOneMap but also
+/// maps tets to triangle faces.
+///
+class SurfaceToTetraMap : public OneToOneMap
+{
+public:
+ SurfaceToTetraMap();
+ SurfaceToTetraMap(
+ std::shared_ptr<Geometry> parent,
+ std::shared_ptr<Geometry> child);
+ ~SurfaceToTetraMap() override = default;
+
+ ///
+ /// \brief Compute the map
+ ///
+ void compute() override;
+
+ ///
+ /// \brief Compute tet vertex id to surf vertex id map
+ ///
+ void computeTriToTetMap(std::unordered_map<int, int>& triToTetMap);
+
+ ///
+ /// \brief Get the tet id that contains the triangle
+ ///
+ int getParentTetId(const int triId) const;
+
+public:
+ std::unordered_map<int, int> m_triToTetMap;
+};
+} // namespace imstk
\ No newline at end of file
diff --git a/Source/Scene/imstkPbdObjectGrasping.cpp b/Source/Scene/imstkPbdObjectGrasping.cpp
index 4f38115917e3d1ac3c7320cddd6406d124fbfbed..2fe2c66dc7ffcf22a32fc90e98e95283bdeac171 100644
--- a/Source/Scene/imstkPbdObjectGrasping.cpp
+++ b/Source/Scene/imstkPbdObjectGrasping.cpp
@@ -69,7 +69,7 @@ getElement(const PickData& pickData, const MeshSide& side)
int vertexId = cell[i];
if (side.map != nullptr)
{
- vertexId = static_cast<int>(side.map->getMapIdx(vertexId));
+ vertexId = static_cast<int>(side.map->getParentVertexId(vertexId));
}
results[i] = { vertexId, { &side.vertices[vertexId], side.invMasses[vertexId], &side.velocities[vertexId] } };
}
@@ -81,7 +81,7 @@ getElement(const PickData& pickData, const MeshSide& side)
int vertexId = pickData.ids[i];
if (side.map != nullptr)
{
- vertexId = static_cast<int>(side.map->getMapIdx(vertexId));
+ vertexId = static_cast<int>(side.map->getParentVertexId(vertexId));
}
results[i] = { vertexId, { &side.vertices[vertexId], side.invMasses[vertexId], &side.velocities[vertexId] } };
}
@@ -253,7 +253,7 @@ PbdObjectGrasping::addPickConstraints()
int vertexId = data.ids[0];
if (meshStruct.map != nullptr)
{
- vertexId = static_cast<int>(meshStruct.map->getMapIdx(vertexId));
+ vertexId = static_cast<int>(meshStruct.map->getParentVertexId(vertexId));
}
const Vec3d relativePos = pickGeomRot * (vertices[vertexId] - pickGeomPos);
diff --git a/Source/Scene/imstkPbdObjectStitching.cpp b/Source/Scene/imstkPbdObjectStitching.cpp
index 57b367aadfd623ae78b71ddf9a29175552a47520..c7b4f805d13c650f1ce7dcbfd473096399d4b848 100644
--- a/Source/Scene/imstkPbdObjectStitching.cpp
+++ b/Source/Scene/imstkPbdObjectStitching.cpp
@@ -32,6 +32,7 @@ limitations under the License.
#include "imstkTaskGraph.h"
#include "imstkTetrahedralMesh.h"
#include "imstkVertexPicker.h"
+#include "imstkSurfaceToTetraMap.h"
namespace imstk
{
@@ -68,7 +69,7 @@ getElement(const PickData& pickData, const MeshSide& side)
int vertexId = cell[i];
if (side.map != nullptr)
{
- vertexId = static_cast<int>(side.map->getMapIdx(vertexId));
+ vertexId = static_cast<int>(side.map->getParentVertexId(vertexId));
}
results[i] = { vertexId, { &side.vertices[vertexId], side.invMasses[vertexId], &side.velocities[vertexId] } };
}
@@ -80,7 +81,7 @@ getElement(const PickData& pickData, const MeshSide& side)
int vertexId = pickData.ids[i];
if (side.map != nullptr)
{
- vertexId = static_cast<int>(side.map->getMapIdx(vertexId));
+ vertexId = static_cast<int>(side.map->getParentVertexId(vertexId));
}
results[i] = { vertexId, { &side.vertices[vertexId], side.invMasses[vertexId], &side.velocities[vertexId] } };
}
@@ -88,11 +89,48 @@ getElement(const PickData& pickData, const MeshSide& side)
return results;
}
+static std::vector<double>
+getWeights(const std::vector<VertexMassPair>& cellVerts, const Vec3d& pt)
+{
+ std::vector<double> weights(cellVerts.size());
+ if (cellVerts.size() == IMSTK_TETRAHEDRON)
+ {
+ const Vec4d baryCoord = baryCentric(pt,
+ *cellVerts[0].vertex,
+ *cellVerts[1].vertex,
+ *cellVerts[2].vertex,
+ *cellVerts[3].vertex);
+ weights[0] = baryCoord[0];
+ weights[1] = baryCoord[1];
+ weights[2] = baryCoord[2];
+ weights[3] = baryCoord[3];
+ }
+ else if (cellVerts.size() == IMSTK_TRIANGLE)
+ {
+ const Vec3d baryCoord = baryCentric(pt,
+ *cellVerts[0].vertex, *cellVerts[1].vertex, *cellVerts[2].vertex);
+ weights[0] = baryCoord[0];
+ weights[1] = baryCoord[1];
+ weights[2] = baryCoord[2];
+ }
+ else if (cellVerts.size() == IMSTK_EDGE)
+ {
+ const Vec2d baryCoord = baryCentric(pt, *cellVerts[0].vertex, *cellVerts[1].vertex);
+ weights[0] = baryCoord[0];
+ weights[1] = baryCoord[1];
+ }
+ else if (cellVerts.size() == IMSTK_VERTEX)
+ {
+ weights[0] = 1.0;
+ }
+ return weights;
+};
+
PbdObjectStitching::PbdObjectStitching(std::shared_ptr<PbdObject> obj) :
SceneObject("PbdObjectStitching_" + obj->getName()),
m_objectToStitch(obj), m_pickMethod(std::make_shared<CellPicker>())
{
- m_stitchingNode = std::make_shared<TaskNode>(std::bind(&PbdObjectStitching::updatePicking, this),
+ m_stitchingNode = std::make_shared<TaskNode>(std::bind(&PbdObjectStitching::updateStitching, this),
"PbdStitchingUpdate", true);
m_taskGraph->addNode(m_stitchingNode);
@@ -115,13 +153,6 @@ PbdObjectStitching::beginRayPointStitch(const Vec3d& rayStart, const Vec3d& rayD
LOG(INFO) << "Begin stitch";
}
-void
-PbdObjectStitching::endStitch()
-{
- m_isStitching = false;
- LOG(INFO) << "End stitch";
-}
-
void
PbdObjectStitching::removeStitchConstraints()
{
@@ -131,11 +162,13 @@ PbdObjectStitching::removeStitchConstraints()
void
PbdObjectStitching::addStitchConstraints()
{
+ // PbdModel geometry can only be PointSet
std::shared_ptr<PointSet> pbdPhysicsGeom =
std::dynamic_pointer_cast<PointSet>(m_objectToStitch->getPhysicsGeometry());
- // If the point set to pick hasn't been set yet, default it to the physics geometry
- // This would be the case if the user was mapping a geometry to another
+ // If the geometry to pick hasn't been set yet, default it to the physics geometry
+ // Could be different in cases where user wants to pick a mapped geometry, mapping back
+ // to the physics geometry
std::shared_ptr<PointSet> pointSetToPick = std::dynamic_pointer_cast<PointSet>(m_geomToStitch);
if (m_geomToStitch == nullptr)
{
@@ -186,216 +219,136 @@ PbdObjectStitching::addStitchConstraints()
indicesPtr.get(),
map);
- // Digest the pick data based on grasp mode
- if (m_mode == StitchMode::Vertex)
+ auto getCellVerts = [&](const PickData& data)
{
- /*for (size_t i = 0; i < pickData.size(); i++)
+ const CellTypeId pickedCellType = data.cellType;
+
+ std::vector<std::pair<int, VertexMassPair>> cellIdVerts;
+ if (pickedCellType == IMSTK_TETRAHEDRON)
{
- const PickData& data = pickData[i];
- int vertexId = data.ids[0];
- if (meshStruct.map != nullptr)
- {
- vertexId = static_cast<int>(meshStruct.map->getMapIdx(vertexId));
- }
+ cellIdVerts = getElement<4>(data, meshStruct);
+ }
+ else if (pickedCellType == IMSTK_TRIANGLE)
+ {
+ cellIdVerts = getElement<3>(data, meshStruct);
+ }
+ else if (pickedCellType == IMSTK_EDGE)
+ {
+ cellIdVerts = getElement<2>(data, meshStruct);
+ }
+ std::vector<VertexMassPair> cellVerts(cellIdVerts.size());
+ for (size_t j = 0; j < cellIdVerts.size(); j++)
+ {
+ cellVerts[j] = cellIdVerts[j].second;
+ }
+ return cellVerts;
+ };
- const Vec3d relativePos = pickGeomRot * (vertices[vertexId] - pickGeomPos);
- m_constraintPts.push_back({
- vertices[vertexId],
- relativePos,
- Vec3d::Zero() });
- std::tuple<Vec3d, Vec3d, Vec3d>& cPt = m_constraintPts.back();
+ auto pointPicker = std::dynamic_pointer_cast<PointPicker>(m_pickMethod);
+ pointPicker->setUseFirstHit(false);
- addConstraint(
- { { &vertices[vertexId], invMasses[vertexId], &velocities[vertexId] } }, { 1.0 },
- { { &std::get<0>(cPt), 0.0, &std::get<2>(cPt) } }, { 1.0 },
- m_stiffness, 0.0);
- }*/
- }
- else if (m_mode == StitchMode::Cell || m_mode == StitchMode::RayCell)
+ // Perform the picking only on surface data
+ auto surfMesh = std::dynamic_pointer_cast<SurfaceMesh>(pointSetToPick);
+ auto tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(pointSetToPick);
+ if (tetMesh != nullptr)
{
- //for (size_t i = 0; i < pickData.size(); i++)
- //{
- // const PickData& data = pickData[i];
- // const CellTypeId pickedCellType = data.cellType;
-
- // std::vector<std::pair<int, VertexMassPair>> cellVerts;
- // if (pickedCellType == IMSTK_TETRAHEDRON)
- // {
- // cellVerts = getElement<4>(data, meshStruct);
- // }
- // else if (pickedCellType == IMSTK_TRIANGLE)
- // {
- // cellVerts = getElement<3>(data, meshStruct);
- // }
- // else if (pickedCellType == IMSTK_EDGE)
- // {
- // cellVerts = getElement<2>(data, meshStruct);
- // }
- // else if (pickedCellType == IMSTK_VERTEX)
- // {
- // cellVerts = getElement<1>(data, meshStruct);
- // }
-
- // // Does not resolve duplicate vertices yet
- // // But pbd implicit solve with reprojection avoids issues
- // for (size_t j = 0; j < cellVerts.size(); j++)
- // {
- // const int vertexId = cellVerts[j].first;
-
- // const Vec3d relativePos = pickGeomRot * (vertices[vertexId] - pickGeomPos);
- // m_constraintPts.push_back({
- // vertices[vertexId],
- // relativePos,
- // Vec3d::Zero() });
- // std::tuple<Vec3d, Vec3d, Vec3d>& cPt = m_constraintPts.back();
-
- // addConstraint(
- // { { &vertices[vertexId], invMasses[vertexId], &velocities[vertexId] } }, { 1.0 },
- // { { &std::get<0>(cPt), 0.0, &std::get<2>(cPt) } }, { 1.0 },
- // m_stiffness, 0.0);
- // }
- //}
+ surfMesh = tetMesh->extractSurfaceMesh();
}
- else if (m_mode == StitchMode::RayPoint)
- {
- auto pointPicker = std::dynamic_pointer_cast<PointPicker>(m_pickMethod);
- //const Vec3d& rayStart = pointPicker->getPickRayStart();
- //const Vec3d& rayDir = pointPicker->getPickRayDir();
-
- // Simple heuristic used here. Stitch all points together along the ray with normals
- // facing each other. Any normal pointing in/out can be assumed going inside/outside the mesh
-
- // Other possibility: If given a manfiold mesh get all the intersection points {1,2,3,4}.
- // Assuming we start outside the shape, stitch up 2 & 3. Any points beyond this we ignore
- // If given a non-manifold mesh we stitch up all points found along the ray
-
- // Perform the picking only on surface data
- std::shared_ptr<SurfaceMesh> surfMesh = std::dynamic_pointer_cast<SurfaceMesh>(pointSetToPick);
- if (auto tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(pointSetToPick))
- {
- surfMesh = tetMesh->extractSurfaceMesh();
- }
- surfMesh->computeTrianglesNormals();
+ const std::vector<PickData>& pickData = m_pickMethod->pick(surfMesh);
- const std::vector<PickData>& pickData = m_pickMethod->pick(surfMesh);
+ // Must have at least 2
+ if (pickData.size() < 1)
+ {
+ return;
+ }
- // ** Warning **, surface triangles are not 100% garunteed to tell inside/out
- // Use angle-weighted psuedonormals as done in MeshToMeshBruteForceCD
- std::shared_ptr<VecDataArray<double, 3>> faceNormalsPtr = surfMesh->getCellNormals();
- const VecDataArray<double, 3>& faceNormals = *faceNormalsPtr;
+ // ** Warning **, surface triangles are not 100% garunteed to tell inside/out
+ // Should use angle-weighted psuedonormals as done in MeshToMeshBruteForceCD
+ surfMesh->computeTrianglesNormals();
+ std::shared_ptr<VecDataArray<double, 3>> faceNormalsPtr = surfMesh->getCellNormals();
+ const VecDataArray<double, 3>& faceNormals = *faceNormalsPtr;
+ // Digest the pick data based on grasp mode
+ if (m_mode == StitchMode::RayCell)
+ {
+ }
+ else if (m_mode == StitchMode::RayPoint)
+ {
// Find all neighbor pairs with normals facing each other
+ printf("Pick Datas: %d\n", static_cast<int>(pickData.size()));
std::vector<std::pair<PickData, PickData>> constraintPair;
for (size_t i = 0, j = 1; i < pickData.size() - 1; i++, j++)
{
+ const Vec3d& pt_i = pickData[i].pickPoint;
+ const Vec3d& pt_j = pickData[j].pickPoint;
const Vec3d& normal_i = faceNormals[pickData[i].ids[0]];
const Vec3d& normal_j = faceNormals[pickData[j].ids[0]];
+ const Vec3d diff = pt_j - pt_i;
+
+ //bool faceOpposite = (normal_i.dot(normal_j) < 0.0);
+ bool faceInwards = (diff.dot(normal_i) > 0.0) && (diff.dot(normal_j) < 0.0);
- // If they face in to each other
- if (normal_i.dot(normal_j) < 0.0)
+ // If they face into each other
+ if (faceInwards)
{
constraintPair.push_back({ pickData[i], pickData[j] });
}
}
+ printf("ConstraintPairs: %d\n", static_cast<int>(constraintPair.size()));
- // Now add constraints based on these
- // If a SurfaceMesh just directly add the element
- if (std::dynamic_pointer_cast<SurfaceMesh>(pointSetToPick) != nullptr)
+ if (tetMesh != nullptr && constraintPair.size() > 0)
{
- auto getCellVerts = [&](const PickData& data)
- {
- const CellTypeId pickedCellType = data.cellType;
-
- std::vector<std::pair<int, VertexMassPair>> cellIdVerts;
- if (pickedCellType == IMSTK_TETRAHEDRON)
- {
- cellIdVerts = getElement<4>(data, meshStruct);
- }
- else if (pickedCellType == IMSTK_TRIANGLE)
- {
- cellIdVerts = getElement<3>(data, meshStruct);
- }
- else if (pickedCellType == IMSTK_EDGE)
- {
- cellIdVerts = getElement<2>(data, meshStruct);
- }
- std::vector<VertexMassPair> cellVerts(cellIdVerts.size());
- for (size_t j = 0; j < cellIdVerts.size(); j++)
- {
- cellVerts[j] = cellIdVerts[j].second;
- }
- return cellVerts;
- };
-
- auto getWeights = [](const std::vector<VertexMassPair>& cellVerts, const Vec3d& pt)
- {
- std::vector<double> weights(cellVerts.size());
- if (cellVerts.size() == IMSTK_TETRAHEDRON)
- {
- const Vec4d baryCoord = baryCentric(pt,
- *cellVerts[0].vertex,
- *cellVerts[1].vertex,
- *cellVerts[2].vertex,
- *cellVerts[3].vertex);
- weights[0] = baryCoord[0];
- weights[1] = baryCoord[1];
- weights[2] = baryCoord[2];
- weights[3] = baryCoord[3];
- }
- else if (cellVerts.size() == IMSTK_TRIANGLE)
- {
- const Vec3d baryCoord = baryCentric(pt,
- *cellVerts[0].vertex, *cellVerts[1].vertex, *cellVerts[2].vertex);
- weights[0] = baryCoord[0];
- weights[1] = baryCoord[1];
- weights[2] = baryCoord[2];
- }
- else if (cellVerts.size() == IMSTK_EDGE)
- {
- const Vec2d baryCoord = baryCentric(pt, *cellVerts[0].vertex, *cellVerts[1].vertex);
- weights[0] = baryCoord[0];
- weights[1] = baryCoord[1];
- }
- else if (cellVerts.size() == IMSTK_VERTEX)
- {
- weights[0] = 1.0;
- }
- return weights;
- };
+ // We supplied a extracted SurfaceMesh to the picker so that it would only pick points
+ // along the boundary/surface of the tet mesh. We need to map these back to the tetrahedrons
+ // as the tetrahedrons are needed for the constraints instead of triangles
+ SurfaceToTetraMap mapper;
+ mapper.setParentGeometry(tetMesh);
+ mapper.setChildGeometry(surfMesh);
+ mapper.compute();
for (size_t i = 0; i < constraintPair.size(); i++)
{
- const PickData& pickData1 = constraintPair[i].first;
- const PickData& pickData2 = constraintPair[i].second;
-
- std::vector<VertexMassPair> cellVerts1 = getCellVerts(pickData1);
- std::vector<double> weights1 = getWeights(cellVerts1, pickData1.pickPoint);
- std::vector<VertexMassPair> cellVerts2 = getCellVerts(pickData2);
- std::vector<double> weights2 = getWeights(cellVerts2, pickData2.pickPoint);
-
- // Cell to single point constraint
- addConstraint(
- cellVerts1, weights1,
- cellVerts2, weights2,
- m_stiffness, m_stiffness);
+ PickData& pickData1 = constraintPair[i].first;
+ PickData& pickData2 = constraintPair[i].second;
+
+ // Get the tet id from the triangle id
+ pickData1.ids[0] = mapper.getParentTetId(pickData1.ids[0]);
+ pickData1.idCount = 1;
+ pickData1.cellType = IMSTK_TETRAHEDRON;
+
+ pickData2.ids[0] = mapper.getParentTetId(pickData2.ids[0]);
+ pickData2.idCount = 1;
+ pickData2.cellType = IMSTK_TETRAHEDRON;
+ // Leave pick point the same
}
}
- // If a TetrahedralMesh we need to map our triangle/surface element
- // back to our tetrahedral one
- else if (std::dynamic_pointer_cast<TetrahedralMesh>(pointSetToPick) != nullptr)
+
+ for (size_t i = 0; i < constraintPair.size(); i++)
{
+ const PickData& pickData1 = constraintPair[i].first;
+ const PickData& pickData2 = constraintPair[i].second;
+ std::vector<VertexMassPair> cellVerts1 = getCellVerts(pickData1);
+ std::vector<double> weights1 = getWeights(cellVerts1, pickData1.pickPoint);
+ std::vector<VertexMassPair> cellVerts2 = getCellVerts(pickData2);
+ std::vector<double> weights2 = getWeights(cellVerts2, pickData2.pickPoint);
+
+ // Cell to single point constraint
+ addConstraint(
+ cellVerts1, weights1,
+ cellVerts2, weights2,
+ m_stiffness, m_stiffness);
}
}
}
void
PbdObjectStitching::addConstraint(
- std::vector<VertexMassPair> ptsA,
- std::vector<double> weightsA,
- std::vector<VertexMassPair> ptsB,
- std::vector<double> weightsB,
- double stiffnessA, double stiffnessB)
+ const std::vector<VertexMassPair>& ptsA,
+ const std::vector<double>& weightsA,
+ const std::vector<VertexMassPair>& ptsB,
+ const std::vector<double>& weightsB,
+ const double stiffnessA, const double stiffnessB)
{
auto constraint = std::make_shared<PbdBaryPointToPointConstraint>();
constraint->initConstraint(
@@ -406,7 +359,7 @@ PbdObjectStitching::addConstraint(
}
void
-PbdObjectStitching::updatePicking()
+PbdObjectStitching::updateStitching()
{
m_objectToStitch->updateGeometries();
@@ -414,19 +367,12 @@ PbdObjectStitching::updatePicking()
if (!m_isPrevStitching && m_isStitching)
{
addStitchConstraints();
- }
- // If stopped
- if (!m_isStitching && m_isPrevStitching)
- {
- removeStitchConstraints();
+ m_isStitching = false;
}
// Push back the state
m_isPrevStitching = m_isStitching;
- if (m_isStitching)
- {
- updateConstraints();
- }
+ updateConstraints();
}
void
diff --git a/Source/Scene/imstkPbdObjectStitching.h b/Source/Scene/imstkPbdObjectStitching.h
index 41e94eefe9377d6dff193710efb45d3b80bc904c..51d6818e53451089776c50361d02ac3282693a8b 100644
--- a/Source/Scene/imstkPbdObjectStitching.h
+++ b/Source/Scene/imstkPbdObjectStitching.h
@@ -96,18 +96,12 @@ public:
const Vec3d& rayStart, const Vec3d& rayDir, const double maxDist = -1.0);*/
///
- /// \brief End a sttich (all constraints removed)
- /// \todo: Remove by stitch id
- ///
- void endStitch();
-
- ///
- /// \brief Compute/generate the constraints for picking
+ /// \brief Compute/generate the constraints for stitching
///
void addStitchConstraints();
///
- /// \brief Remove the constraints for picking
+ /// \brief Clears all the stitches
///
void removeStitchConstraints();
@@ -117,11 +111,11 @@ public:
/// pt position = weightA_0 * ptsA_0 + weightA_1 * ptsA_1 + ...
///
virtual void addConstraint(
- std::vector<VertexMassPair> ptsA,
- std::vector<double> weightsA,
- std::vector<VertexMassPair> ptsB,
- std::vector<double> weightsB,
- double stiffnessA, double stiffnessB);
+ const std::vector<VertexMassPair>& ptsA,
+ const std::vector<double>& weightsA,
+ const std::vector<VertexMassPair>& ptsB,
+ const std::vector<double>& weightsB,
+ const double stiffnessA, const double stiffnessB);
///
/// \brief Get/Set the method use for picking, default is CellPicker
@@ -148,7 +142,7 @@ protected:
///
/// \brief Update picking state, this should move grasp points
///
- virtual void updatePicking();
+ virtual void updateStitching();
///
/// \brief Update the constraints used for picking
@@ -169,9 +163,12 @@ protected:
bool m_isStitching = false;
bool m_isPrevStitching = false;
- /// Stiffness of grasp, when 1 the position is completely moved too the grasp point
+ /// Stiffness of stitches, when 1 the position is completely moved too the grasp point
/// when stiffness < 1 it will slowly converge on the grasp point
- double m_stiffness = 0.4;
+ double m_stiffness = 0.1;
+
+ bool m_debugGeometry = false; ///< Display debug points & lines for stitches
+ bool m_retractingStitch = false; ///< Place a stitch that slowly retracts to 0 after placement
std::vector<std::shared_ptr<PbdCollisionConstraint>> m_constraints; ///< List of PBD constraints
};