diff --git a/Base/Collision/CollisionData/imstkCollisionData.h b/Base/Collision/CollisionData/imstkCollisionData.h
index bc93c0c08b27cf69cf4f0af617e6ee381802548c..a68537e97a06880abab1c400d4b78921ebcc55a7 100644
--- a/Base/Collision/CollisionData/imstkCollisionData.h
+++ b/Base/Collision/CollisionData/imstkCollisionData.h
@@ -130,6 +130,14 @@ struct PointTetrahedronCollisionData
WeightsArray BarycentricCoordinates;
};
+struct PickingCollisionData
+{
+ // map of node and point position
+ Vec3d ptPos;
+ size_t nodeId;
+ bool touchStatus;
+};
+
///
/// \struct CollisionData
///
@@ -147,6 +155,7 @@ public:
EEColData.clear();
MAColData.clear();
PTColData.clear();
+ NodePickData.clear();
}
std::vector<PositionDirectionCollisionData> PDColData; ///< Position Direction collision data
@@ -155,6 +164,7 @@ public:
std::vector<EdgeEdgeCollisionData> EEColData; ///< Edge Edge collision data
std::vector<MeshToAnalyticalCollisionData> MAColData; ///< Mesh to analytical collision data
std::vector<PointTetrahedronCollisionData> PTColData; ///< Point Tetrahedron collision data
+ std::vector<PickingCollisionData> NodePickData; ///< List of points that are picked
};
}
diff --git a/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp b/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp
index 0404115c54065771313bc6c49d3770ee7d278d6d..15473ad81373979c3f76e46393907d675b62c8cd 100644
--- a/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp
+++ b/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp
@@ -27,6 +27,7 @@
#include "imstkMeshToSphereCD.h"
#include "imstkMeshToPlaneCD.h"
#include "imstkMeshToMeshCD.h"
+#include "imstkMeshToSpherePickingCD.h"
#include "imstkCollidingObject.h"
#include "imstkPlane.h"
@@ -136,6 +137,21 @@ CollisionDetection::make_collision_detection(const Type& type,
return std::make_shared<MeshToMeshCD>(meshA, meshB, colData);
}
break;
+ case Type::MeshToSpherePicking:
+ {
+ auto mesh = std::dynamic_pointer_cast<Mesh>(objA->getCollidingGeometry());
+ auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
+
+ // Geometries check
+ if (mesh == nullptr || sphere == nullptr)
+ {
+ LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
+ << "invalid object geometries for SphereToSphere collision detection.";
+ return nullptr;
+ }
+ return std::make_shared<MeshToSpherePickingCD>(mesh, sphere, colData);
+ }
+ break;
default:
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: type not implemented.";
diff --git a/Base/Collision/CollisionDetection/imstkCollisionDetection.h b/Base/Collision/CollisionDetection/imstkCollisionDetection.h
index 90a8f3c2fa05cc1cda18b8bcdacd18618732a639..6864fbaae1a223a504625b8677b93dbce49a70fc 100644
--- a/Base/Collision/CollisionDetection/imstkCollisionDetection.h
+++ b/Base/Collision/CollisionDetection/imstkCollisionDetection.h
@@ -50,7 +50,8 @@ public:
MeshToSphere,
MeshToPlane,
MeshToMesh,
- MeshToCapsule
+ MeshToCapsule,
+ MeshToSpherePicking
};
///
diff --git a/Base/Collision/CollisionDetection/imstkMeshToSpherePickingCD.cpp b/Base/Collision/CollisionDetection/imstkMeshToSpherePickingCD.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..95e721d75870b2f507cf4e36105baf6415c8b29a
--- /dev/null
+++ b/Base/Collision/CollisionDetection/imstkMeshToSpherePickingCD.cpp
@@ -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.
+
+=========================================================================*/
+
+#include "imstkMeshToSpherePickingCD.h"
+#include "imstkCollisionData.h"
+#include "imstkMesh.h"
+#include "imstkSphere.h"
+
+namespace imstk
+{
+
+void
+MeshToSpherePickingCD::computeCollisionData()
+{
+ // Clear collisionData
+ m_colData.clearAll();
+
+ if (!m_deviceTracker || !m_deviceTracker->getDeviceClient()->getButton(m_buttonId))
+ {
+ return;
+ }
+
+ // Get sphere properties
+ auto spherePos = m_sphere->getPosition();
+ auto radius = m_sphere->getRadius()*m_sphere->getScaling();
+
+ size_t nodeId = 0;
+ for (const auto& p : m_mesh->getVertexPositions())
+ {
+ auto dist = (spherePos - p).norm();
+ if (dist <= radius)
+ {
+ m_colData.NodePickData.push_back({ spherePos - p, nodeId, 0 });
+ }
+ nodeId++;
+ }
+}
+
+}
\ No newline at end of file
diff --git a/Base/Collision/CollisionDetection/imstkMeshToSpherePickingCD.h b/Base/Collision/CollisionDetection/imstkMeshToSpherePickingCD.h
new file mode 100644
index 0000000000000000000000000000000000000000..7c2c71123f44458662d239366c51c4ca27a16c4e
--- /dev/null
+++ b/Base/Collision/CollisionDetection/imstkMeshToSpherePickingCD.h
@@ -0,0 +1,83 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef imstkMeshToSpherePickingCD_h
+#define imstkMeshToSpherePickingCD_h
+
+#include <memory>
+
+#include "imstkCollisionDetection.h"
+#include "imstkDeviceTracker.h"`
+
+namespace imstk
+{
+
+class Mesh;
+class Sphere;
+class CollisionData;
+
+///
+/// \class MeshToSpherePickingCD
+///
+/// \brief Mesh to sphere collision detection while picking
+///
+class MeshToSpherePickingCD : public CollisionDetection
+{
+public:
+
+ ///
+ /// \brief Constructor
+ ///
+ MeshToSpherePickingCD(std::shared_ptr<Mesh> mesh,
+ std::shared_ptr<Sphere> sphere,
+ CollisionData& colData) :
+ CollisionDetection(CollisionDetection::Type::MeshToSphere,
+ colData),
+ m_mesh(mesh),
+ m_sphere(sphere){}
+
+ ///
+ /// \brief Destructor
+ ///
+ ~MeshToSpherePickingCD() = default;
+
+ ///
+ /// \brief Detect collision and compute collision data
+ ///
+ void computeCollisionData() override;
+
+ ///
+ /// \brief Set device tracker and the id of the button
+ ///
+ void setDeviceTrackerAndButton(const std::shared_ptr<imstk::DeviceTracker> devTracker,
+ const unsigned int buttonId = 0){ m_deviceTracker = devTracker; }
+private:
+
+ std::shared_ptr<Mesh> m_mesh; ///> Mesh
+ std::shared_ptr<Sphere> m_sphere; ///> Sphere
+
+ std::shared_ptr<imstk::DeviceTracker> m_deviceTracker; ///> Device tracker to get the button status
+ unsigned int m_buttonId = 0; ///> button id
+};
+
+}
+
+#endif // ifndef imstkMeshToSphereCD_h
diff --git a/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp b/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp
index cf8348b623b2ba711973a777d59ac22a1aee06dd..8d4f1aec9190c8eb4dd4f1d7b69c2181140aa494 100644
--- a/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp
+++ b/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp
@@ -23,6 +23,8 @@
#include "imstkPenaltyCH.h"
#include "imstkVirtualCouplingCH.h"
+#include "imstkPickingCH.h"
+#include "imstkDeformableObject.h"
#include <g3log/g3log.hpp>
@@ -48,10 +50,34 @@ CollisionHandling::make_collision_handling(const Type& type,
{
case Type::None:
return nullptr;
+
case Type::Penalty:
+ if (objA->getType() == SceneObject::Type::Visual)
+ {
+ LOG(WARNING) << "CollisionHandling::make_collision_handling error: "
+ << "penalty collision handling only implemented for colliding objects.";
+ return nullptr;
+ }
+
return std::make_shared<PenaltyCH>(side, colData, objA);
+
case Type::VirtualCoupling:
+
return std::make_shared<VirtualCouplingCH>(side, colData, objA );
+
+ case Type::NodalPicking:
+
+ if (objA->getType() == SceneObject::Type::Visual)
+ {
+ LOG(WARNING) << "CollisionHandling::make_collision_handling error: "
+ << "penalty collision handling only implemented for colliding objects.";
+ return nullptr;
+ }
+ if (auto defObj = std::dynamic_pointer_cast<DeformableObject>(objA))
+ {
+ return std::make_shared<PickingCH>(side, colData, defObj);
+ }
+
default:
LOG(WARNING) << "CollisionHandling::make_collision_handling error: type not implemented.";
return nullptr;
diff --git a/Base/Collision/CollisionHandling/imstkCollisionHandling.h b/Base/Collision/CollisionHandling/imstkCollisionHandling.h
index 2725248e7b1998e0fde62e5517a2088a0fe8ff41..556782cb4f44d13b8d7d2d32e18dd603808c0aa3 100644
--- a/Base/Collision/CollisionHandling/imstkCollisionHandling.h
+++ b/Base/Collision/CollisionHandling/imstkCollisionHandling.h
@@ -49,7 +49,8 @@ public:
{
None,
Penalty,
- VirtualCoupling
+ VirtualCoupling,
+ NodalPicking
};
///
diff --git a/Base/Collision/CollisionHandling/imstkPickingCH.cpp b/Base/Collision/CollisionHandling/imstkPickingCH.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa0d8f8b84ec972a07d19473dc1689d058e37808
--- /dev/null
+++ b/Base/Collision/CollisionHandling/imstkPickingCH.cpp
@@ -0,0 +1,81 @@
+/*=========================================================================
+
+Library: iMSTK
+
+Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+& Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0.txt
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+=========================================================================*/
+
+#include "imstkPickingCH.h"
+#include "imstkDeformableObject.h"
+
+namespace imstk
+{
+
+void
+PickingCH::computeContactForces()
+{
+ if (m_object)
+ {
+ this->addPickConstraints(m_object);
+ }
+ else
+ {
+ LOG(WARNING) << "PickingCH::handleCollision error: "
+ << "no picking collision handling available the object";
+ }
+}
+
+void
+PickingCH::addPickConstraints(std::shared_ptr<DeformableObject> deformableObj)
+{
+ m_DynamicLinearProjConstraints->clear();
+
+ if (m_colData.NodePickData.empty())
+ {
+ return;
+ }
+
+ if (deformableObj == nullptr)
+ {
+ LOG(WARNING) << "PenaltyRigidCH::addPickConstraints error: "
+ << " not a deformable object.";
+ return;
+ }
+
+ const auto& Uprev = deformableObj->getDisplacements();
+ const auto& Vprev = deformableObj->getVelocities();
+
+ auto PhysTetMesh = std::dynamic_pointer_cast<Mesh>(deformableObj->getPhysicsGeometry());
+ auto dT = std::dynamic_pointer_cast<FEMDeformableBodyModel>(m_object->getDynamicalModel())->getTimeIntegrator()->getTimestepSize();
+
+ // If collision data, append LPC constraints
+ for (const auto& CD : m_colData.NodePickData)
+ {
+ auto nodeDof = 3 * CD.nodeId;
+ auto vprev = Vec3d(Vprev(nodeDof), Vprev(nodeDof + 1), Vprev(nodeDof + 2));
+ auto uprev = Vec3d(Uprev(nodeDof), Uprev(nodeDof + 1), Uprev(nodeDof + 2));
+ auto x = (CD.ptPos + PhysTetMesh->getVertexPosition(CD.nodeId) -
+ PhysTetMesh->getInitialVertexPosition(CD.nodeId) - uprev) / dT - vprev;
+
+ auto pickProjector = LinearProjectionConstraint(CD.nodeId, true);
+ pickProjector.setProjectorToDirichlet(CD.nodeId, x);
+
+ m_DynamicLinearProjConstraints->push_back(pickProjector);
+ }
+}
+
+}
\ No newline at end of file
diff --git a/Base/Collision/CollisionHandling/imstkPickingCH.h b/Base/Collision/CollisionHandling/imstkPickingCH.h
new file mode 100644
index 0000000000000000000000000000000000000000..88113eba1e36bb98674a442d875677e8032dae26
--- /dev/null
+++ b/Base/Collision/CollisionHandling/imstkPickingCH.h
@@ -0,0 +1,96 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef imstkPickingCH_h
+#define imstkPenaltyCH_h
+
+// std library
+#include <memory>
+
+// imstk
+#include "imstkCollisionHandling.h"
+#include "imstkNonlinearSystem.h"
+
+namespace imstk
+{
+
+class CollidingObject;
+class DeformableObject;
+class CollisionData;
+
+///
+/// \class PickingCH
+///
+/// \brief Implements nodal picking
+///
+class PickingCH : public CollisionHandling
+{
+public:
+
+ ///
+ /// \brief Constructor
+ ///
+ PickingCH(const Side& side,
+ const CollisionData& colData,
+ std::shared_ptr<DeformableObject> obj):
+ CollisionHandling(Type::NodalPicking, side, colData),
+ m_object(obj){}
+
+ PickingCH() = delete;
+
+ ///
+ /// \brief Destructor
+ ///
+ ~PickingCH() = default;
+
+ ///
+ /// \brief Compute forces based on collision data
+ ///
+ void computeContactForces() override;
+
+ ///
+ /// \brief Add LPC constraints for the node that is picked
+ ///
+ void addPickConstraints(std::shared_ptr<DeformableObject> deformableObj);
+
+ /// \brief Get the vector denoting the filter
+ ///
+ void setDynamicLinearProjectors(std::vector<LinearProjectionConstraint>* f)
+ {
+ m_DynamicLinearProjConstraints = f;
+ }
+
+ /// \brief Get the vector denoting the filter
+ ///
+ std::vector<LinearProjectionConstraint>& getDynamicLinearProjectors()
+ {
+ return *m_DynamicLinearProjConstraints;
+ }
+
+private:
+
+ std::shared_ptr<DeformableObject> m_object; ///> Deformable object
+ std::vector<LinearProjectionConstraint> *m_DynamicLinearProjConstraints;
+};
+
+}
+
+#endif // ifndef imstkPenaltyCH_h
\ No newline at end of file
diff --git a/Base/Collision/imstkInteractionPair.cpp b/Base/Collision/imstkInteractionPair.cpp
index f58d7a0c24b37b12f59b77dd538dd1b5348bb56c..e2137f7536db8179a331be489f9003286a4d71f6 100644
--- a/Base/Collision/imstkInteractionPair.cpp
+++ b/Base/Collision/imstkInteractionPair.cpp
@@ -23,7 +23,8 @@
#include <g3log/g3log.hpp>
-namespace imstk {
+namespace imstk
+{
InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
std::shared_ptr<CollidingObject> B,
@@ -40,7 +41,7 @@ InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
return;
}
- // Check if objects are differents
+ // Check if objects are different
if (A == B)
{
LOG(WARNING) << "InteractionPair error: object cannot interact with itself.";
@@ -89,6 +90,36 @@ InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
m_valid = true;
}
+InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
+ std::shared_ptr<CollidingObject> B,
+ std::shared_ptr<CollisionDetection> CD,
+ std::shared_ptr<CollisionHandling> CHA,
+ std::shared_ptr<CollisionHandling> CHB)
+{
+ m_valid = false;
+
+ // Check that objects exist
+ if (A == nullptr || B == nullptr)
+ {
+ LOG(WARNING) << "InteractionPair error: invalid objects (nullptr).";
+ return;
+ }
+
+ // Check if objects are different
+ if (A == B)
+ {
+ LOG(WARNING) << "InteractionPair error: object cannot interact with itself.";
+ return;
+ }
+
+ m_objects = ObjectsPair(A, B);
+ m_colDetect = CD;
+ m_colHandlingA = CHA;
+ m_colHandlingB = CHB;
+ m_colData = CD->getCollisionData();
+ m_valid = true;
+}
+
void
InteractionPair::computeCollisionData()
{
diff --git a/Base/Collision/imstkInteractionPair.h b/Base/Collision/imstkInteractionPair.h
index 77418fc4e16b6d2302dfa12b230d5edea861f744..cae2709f45451c1725d0d5f3844bf73d3f9a16ac 100644
--- a/Base/Collision/imstkInteractionPair.h
+++ b/Base/Collision/imstkInteractionPair.h
@@ -55,6 +55,12 @@ public:
CollisionHandling::Type CHAType,
CollisionHandling::Type CHBType);
+ InteractionPair(std::shared_ptr<CollidingObject> A,
+ std::shared_ptr<CollidingObject> B,
+ std::shared_ptr<CollisionDetection> CD,
+ std::shared_ptr<CollisionHandling> CHA,
+ std::shared_ptr<CollisionHandling> CHB);
+
///
/// \brief Destructor
///
diff --git a/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.cpp b/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.cpp
index 84a19ed66e359fefe64b6eb67e1513a298e4b054..4d3cd593c0d26708d3f20903a0c982aea5548a5f 100644
--- a/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.cpp
+++ b/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.cpp
@@ -43,11 +43,12 @@ LinearProjectionConstraint::setProjection(const size_t& nodeId, const Vec3d& p,
}
void
-LinearProjectionConstraint::setProjectorToDirichlet(const size_t& nodeId)
+LinearProjectionConstraint::setProjectorToDirichlet(const unsigned int& nodeId, const Vec3d z)
{
m_nodeId = nodeId;
m_projection = Mat3d::Zero();
m_isFixedConstraint = true;
+ m_value = z;
}
void
diff --git a/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.h b/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.h
index 78f3f24a94a92cadd2892c792aaaa0c80a28ba67..a2683d8bb33a62e71617e0a1e1b85e2d180558f1 100644
--- a/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.h
+++ b/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.h
@@ -55,6 +55,7 @@ public:
/// \brief Set the projector to simulate Dirichlet conditions
///
void setProjectorToDirichlet(const size_t& nodeId);
+ void setProjectorToDirichlet(const unsigned int& nodeId, const Vec3d z);
///
/// \brief Reset the linear projector
@@ -64,37 +65,30 @@ public:
///
/// \brief Set the value in the restricted subspace
///
- void setValue(const Vec3d& v)
- {
- m_value = v;
- }
+ inline void setValue(const Vec3d& v) { m_value = v; }
///
/// \brief Get the projector
///
- inline const Mat3d& getProjector() const
- {
- return m_projection;
- }
+ inline const Mat3d& getProjector() const { return m_projection; }
///
/// \brief Get the value
///
- inline const Vec3d& getValue() const
- {
- return m_value;
- }
+ inline const Vec3d& getValue() const { return m_value; }
///
/// \brief Get the node id
///
- inline const size_t& getNodeId() const
- {
- return m_nodeId;
- }
+ inline const size_t& getNodeId() const { return m_nodeId; }
+
+ ///
+ /// \brief Returns true if the constraint is fixed
+ ///
+ inline bool isFixed() const { return m_isFixedConstraint; }
private:
- size_t m_nodeId; ///> Node id
+ size_t m_nodeId; ///> Node id
bool m_isFixedConstraint = false; ///> Flag to know if that node is fixed
Mat3d m_projection = Mat3d::Identity(); ///> Orthogonal projector
Vec3d m_value = Vec3d(0., 0., 0.); ///> Value in the subspace: range(I-m_projector)
diff --git a/Base/Core/imstkAPIUtilities.h b/Base/Core/imstkAPIUtilities.h
index 41adcc7311390d94f2821cb497cae0e564f6aa86..3daa959cb104c54e5c772b45756b21ea3ccbf529 100644
--- a/Base/Core/imstkAPIUtilities.h
+++ b/Base/Core/imstkAPIUtilities.h
@@ -209,7 +209,6 @@ createNonLinearSystem(std::shared_ptr<imstk::FEMDeformableBodyModel> dynaModel)
{
linProj.push_back(LinearProjectionConstraint(i, true));
}
- nlSystem->setLinearProjectors(linProj);
nlSystem->setUnknownVector(dynaModel->getUnknownVec());
nlSystem->setUpdateFunction(dynaModel->getUpdateFunction());
nlSystem->setUpdatePreviousStatesFunction(dynaModel->getUpdatePrevStateFunction());
diff --git a/Base/Scene/imstkCollisionGraph.cpp b/Base/Scene/imstkCollisionGraph.cpp
index 99e3d3eb6fe22accb3cbc891562b92a25b898311..b79c3ac9ef478fd18da033b915088045a4e44275 100644
--- a/Base/Scene/imstkCollisionGraph.cpp
+++ b/Base/Scene/imstkCollisionGraph.cpp
@@ -62,12 +62,50 @@ CollisionGraph::addInteractionPair(CollidingObjectPtr A,
return intPair;
}
+
//TODO: Refactor -> PBD only
-void CollisionGraph::addInteractionPair(std::shared_ptr<PbdInteractionPair> pair)
+void
+CollisionGraph::addInteractionPair(std::shared_ptr<PbdInteractionPair> pair)
{
m_interactionPbdPairList.push_back(pair);
}
+std::shared_ptr<InteractionPair>
+CollisionGraph::addInteractionPair(CollidingObjectPtr A,
+ CollidingObjectPtr B,
+ CollisionDetectionPtr CD,
+ CollisionHandlingPtr CHA,
+ CollisionHandlingPtr CHB)
+{
+ // Check that interaction pair does not exist
+ if (this->getInteractionPair(A, B) != nullptr)
+ {
+ LOG(WARNING) << "CollisionGraph::addInteractionPair error: interaction already defined for "
+ << A->getName() << " & " << B->getName() << ".";
+ return nullptr;
+ }
+
+ // Create interaction pair
+ auto intPair = std::make_shared<InteractionPair>(A, B, CD, CHA, CHB);
+
+ // Check validity
+ if (!intPair->isValid())
+ {
+ LOG(WARNING) << "CollisionGraph::addInteractionPair error: could not create interaction for "
+ << A->getName() << " & " << B->getName() << " with those parameters.";
+ intPair.reset();
+ return nullptr;
+ }
+
+ // Populate book-keeping
+ m_interactionPairList.push_back(intPair);
+ m_interactionPairMap[A].push_back(intPair);
+ m_interactionPairMap[B].push_back(intPair);
+
+ // Return interaction pair
+ return intPair;
+}
+
bool
CollisionGraph::removeInteractionPair(CollidingObjectPtr A, CollidingObjectPtr B)
{
diff --git a/Base/Scene/imstkCollisionGraph.h b/Base/Scene/imstkCollisionGraph.h
index e2f4d5b3086a9924c9401095c3f34324cbb3d073..a735ed928f48e240746a33e7650f4a2f178d7067 100644
--- a/Base/Scene/imstkCollisionGraph.h
+++ b/Base/Scene/imstkCollisionGraph.h
@@ -44,7 +44,9 @@ namespace imstk
class CollisionGraph
{
public:
- using CollidingObjectPtr = std::shared_ptr<CollidingObject>;
+ using CollidingObjectPtr = std::shared_ptr < CollidingObject > ;
+ using CollisionHandlingPtr = std::shared_ptr < CollisionHandling > ;
+ using CollisionDetectionPtr = std::shared_ptr < CollisionDetection > ;
using InteractionPairPtr = std::shared_ptr<InteractionPair>;
///
@@ -67,6 +69,12 @@ public:
CollisionHandling::Type CHBType);
//TODO: Refactor -> PBD only
+ InteractionPairPtr addInteractionPair(CollidingObjectPtr A,
+ CollidingObjectPtr B,
+ CollisionDetectionPtr CD,
+ CollisionHandlingPtr CHA,
+ CollisionHandlingPtr CHB);
+
void addInteractionPair(std::shared_ptr<PbdInteractionPair> pair);
///
@@ -95,8 +103,8 @@ public:
protected:
std::vector<std::shared_ptr<PbdInteractionPair>> m_interactionPbdPairList; //TODO: Refactor -> PBD only
- std::vector<InteractionPairPtr> m_interactionPairList; //!< All interaction pairs in the collision graph
- std::unordered_map<CollidingObjectPtr, std::vector<InteractionPairPtr>> m_interactionPairMap; //!< Map of interaction pairs per colliding object
+ std::vector<InteractionPairPtr> m_interactionPairList; ///< All interaction pairs in the collision graph
+ std::unordered_map<CollidingObjectPtr, std::vector<InteractionPairPtr>> m_interactionPairMap; ///< Map of interaction pairs per colliding object
};
}
diff --git a/Base/Solvers/imstkConjugateGradient.cpp b/Base/Solvers/imstkConjugateGradient.cpp
index 793f453b529352aa064c94900a8bc939f8937d37..2ba8bd96627588efce34e875006a5728b4cbae37 100644
--- a/Base/Solvers/imstkConjugateGradient.cpp
+++ b/Base/Solvers/imstkConjugateGradient.cpp
@@ -68,7 +68,7 @@ ConjugateGradient::solve(Vectord& x)
return;
}
- if (m_linearSystem->getLinearProjectors().size() == 0)
+ if (m_FixedLinearProjConstraints->size() == 0)
{
x = m_cgSolver.solve(m_linearSystem->getRHSVector());
}
@@ -81,16 +81,17 @@ ConjugateGradient::solve(Vectord& x)
void
ConjugateGradient::modifiedCGSolve(Vectord& x)
{
- const auto &linearProjectors = m_linearSystem->getLinearProjectors();
const auto &b = m_linearSystem->getRHSVector();
const auto &A = m_linearSystem->getMatrix();
// Set the initial guess to zero
x.setZero();
- applyLinearProjectionFilter(x, linearProjectors, true);
+ applyLinearProjectionFilter(x, *m_DynamicLinearProjConstraints, true);
+ applyLinearProjectionFilter(x, *m_FixedLinearProjConstraints, true);
auto res = b;
- applyLinearProjectionFilter(res, linearProjectors, false);
+ applyLinearProjectionFilter(res, *m_DynamicLinearProjConstraints, false);
+ applyLinearProjectionFilter(res, *m_FixedLinearProjConstraints, false);
auto c = res;
auto delta = res.dot(res);
auto deltaPrev = delta;
@@ -103,7 +104,8 @@ ConjugateGradient::modifiedCGSolve(Vectord& x)
while (delta > eps)
{
q = A * c;
- applyLinearProjectionFilter(q, linearProjectors, false);
+ applyLinearProjectionFilter(q, *m_DynamicLinearProjConstraints, false);
+ applyLinearProjectionFilter(q, *m_FixedLinearProjConstraints, false);
dotval = c.dot(q);
if (dotval != 0.0)
{
@@ -120,7 +122,8 @@ ConjugateGradient::modifiedCGSolve(Vectord& x)
delta = res.dot(res);
c *= delta / deltaPrev;
c += res;
- applyLinearProjectionFilter(c, linearProjectors, false);
+ applyLinearProjectionFilter(c, *m_DynamicLinearProjConstraints, false);
+ applyLinearProjectionFilter(c, *m_FixedLinearProjConstraints, false);
if (++iterNum >= m_maxIterations)
{
diff --git a/Base/Solvers/imstkConjugateGradient.h b/Base/Solvers/imstkConjugateGradient.h
index d9dd6a035ebf559f8a017c862b798955a91909a5..6a27c2a0c1d3e58fa2b3239a00a0b6aae24f4e08 100644
--- a/Base/Solvers/imstkConjugateGradient.h
+++ b/Base/Solvers/imstkConjugateGradient.h
@@ -98,6 +98,34 @@ public:
/// \brief Apply a filter to the vector supplied
///
void applyLinearProjectionFilter(Vectord& x, const std::vector<LinearProjectionConstraint>& linProj, const bool setVal);
+
+ /// \brief Get the vector denoting the filter
+ ///
+ void setLinearProjectors(std::vector<LinearProjectionConstraint>* f)
+ {
+ m_FixedLinearProjConstraints = f;
+ }
+
+ /// \brief Get the vector denoting the filter
+ ///
+ std::vector<LinearProjectionConstraint>& getLinearProjectors()
+ {
+ return *m_FixedLinearProjConstraints;
+ }
+
+ /// \brief Get the vector denoting the filter
+ ///
+ void setDynamicLinearProjectors(std::vector<LinearProjectionConstraint>* f)
+ {
+ m_DynamicLinearProjConstraints = f;
+ }
+
+ /// \brief Get the vector denoting the filter
+ ///
+ std::vector<LinearProjectionConstraint>& getDynamicLinearProjectors()
+ {
+ return *m_DynamicLinearProjConstraints;
+ }
private:
///
@@ -107,6 +135,9 @@ private:
///> Pointer to the Eigen's Conjugate gradient solver
Eigen::ConjugateGradient<SparseMatrixd> m_cgSolver;
+
+ std::vector<LinearProjectionConstraint> *m_FixedLinearProjConstraints;
+ std::vector<LinearProjectionConstraint> *m_DynamicLinearProjConstraints;
};
} // imstk
diff --git a/Base/Solvers/imstkNewtonSolver.cpp b/Base/Solvers/imstkNewtonSolver.cpp
index 8938b39178ca6c249c3884017cafd56a7256954c..a01e566881651116eef52f22ad0724f976a88993 100644
--- a/Base/Solvers/imstkNewtonSolver.cpp
+++ b/Base/Solvers/imstkNewtonSolver.cpp
@@ -144,7 +144,7 @@ NewtonSolver::updateJacobian(const Vectord& x)
auto &b = m_nonLinearSystem->m_F(x, m_isSemiImplicit);
auto linearSystem = std::make_shared<LinearSolverType::LinearSystemType>(A, b);
- linearSystem->setLinearProjectors(m_nonLinearSystem->getLinearProjectors());
+ //linearSystem->setLinearProjectors(m_nonLinearSystem->getLinearProjectors()); // TODO: Left for near future reference. Clear in future.
m_linearSolver->setSystem(linearSystem);
return b.dot(b);
diff --git a/Base/Solvers/imstkNonlinearSystem.h b/Base/Solvers/imstkNonlinearSystem.h
index b6a5d54808b50a4e609a5e273f9d6c79a58559aa..95d04c3e1b68f52bfa85252ec642651249e7dc46 100644
--- a/Base/Solvers/imstkNonlinearSystem.h
+++ b/Base/Solvers/imstkNonlinearSystem.h
@@ -92,7 +92,7 @@ namespace imstk
/// \brief Get the vector denoting the filter
///
- void setLinearProjectors(std::vector<LinearProjectionConstraint>& f)
+ /*void setLinearProjectors(std::vector<LinearProjectionConstraint>& f)
{
m_LinearProjConstraints = &f;
}
@@ -102,7 +102,7 @@ namespace imstk
std::vector<LinearProjectionConstraint>& getLinearProjectors()
{
return *m_LinearProjConstraints;
- }
+ }*/
///
/// \brief Set the update function
@@ -120,6 +120,20 @@ namespace imstk
m_FUpdatePrevState = updateFunc;
}
+ /// \brief Get the vector denoting the filter
+ ///
+ /*void setDynamicLinearProjectors(std::vector<LinearProjectionConstraint>* f)
+ {
+ m_DynamicLinearProjConstraints = f;
+ }
+
+ /// \brief Get the vector denoting the filter
+ ///
+ std::vector<LinearProjectionConstraint>& getDynamicLinearProjectors()
+ {
+ return *m_DynamicLinearProjConstraints;
+ }*/
+
public:
VectorFunctionType m_F; ///> Nonlinear function
MatrixFunctionType m_dF; ///> Gradient of the Nonlinear function with respect to the unknown vector
@@ -127,7 +141,8 @@ public:
UpdateFunctionType m_FUpdate;
UpdatePrevStateFunctionType m_FUpdatePrevState;
- std::vector<LinearProjectionConstraint> *m_LinearProjConstraints;
+ /*std::vector<LinearProjectionConstraint> *m_LinearProjConstraints;
+ std::vector<LinearProjectionConstraint> *m_DynamicLinearProjConstraints;*/
};
} // imstk
diff --git a/Examples/Sandbox/main.cpp b/Examples/Sandbox/main.cpp
index f224938bf0b363aa68d2db5e984fb44335f103b8..15b3441535d02e203de6acb89ce6db55fbe9b07f 100644
--- a/Examples/Sandbox/main.cpp
+++ b/Examples/Sandbox/main.cpp
@@ -75,6 +75,8 @@
#include "imstkMeshToPlaneCD.h"
#include "imstkMeshToSphereCD.h"
#include "imstkVirtualCouplingCH.h"
+#include "imstkMeshToSpherePickingCD.h"
+#include "imstkPickingCH.h"
// logger
#include "g3log/g3log.hpp"
@@ -130,6 +132,7 @@ void liverToolInteraction();
void testCapsule();
void testVirtualCoupling();
void testGeometryTransforms();
+void testPicking();
int main()
{
@@ -162,7 +165,7 @@ int main()
//testOneToOneNodalMap();
//testSurfaceMeshOptimizer();
//testAnalyticalGeometry();
- testGeometryTransforms();
+ //testGeometryTransforms();
/*------------------
@@ -174,6 +177,7 @@ int main()
//testDeformableBody();
//testDeformableBodyCollision();
//liverToolInteraction();
+ //testPicking();
/*------------------
@@ -188,11 +192,11 @@ int main()
Test devices, controllers
------------------*/
//testObjectController();
- testTwoFalcons();
+ //testTwoFalcons();
//testCameraController();
//testTwoOmnis();
//testLapToolController();
-
+ testPicking();
/*------------------
Test Misc.
@@ -352,7 +356,7 @@ void testMultiObjectWithTextures()
object1->setVisualGeometry(surfaceMesh1); // change to any mesh created above
scene->addSceneObject(object1);
}
-
+
// Run
sdk->setCurrentScene(scene);
sdk->startSimulation(true);
@@ -664,7 +668,7 @@ void testCameraController()
#ifdef iMSTK_USE_OPENHAPTICS
- auto client = std::make_shared<imstk::HDAPIDeviceClient>("PHANToM 1");
+ auto client = std::make_shared<imstk::HDAPIDeviceClient>("Default Device");
// Device Server
auto server = std::make_shared<imstk::HDAPIDeviceServer>();
@@ -1256,11 +1260,9 @@ void testDeformableBody()
for (auto i : dynaModel->getFixNodeIds())
{
auto s = LinearProjectionConstraint(i, false);
- s.setProjectorToDirichlet(i);
- s.setValue(Vec3d(0.001, 0, 0));
+ s.setProjectorToDirichlet(i, Vec3d(0.001, 0, 0));
projList.push_back(s);
}
- nlSystem->setLinearProjectors(projList);
nlSystem->setUnknownVector(dynaModel->getUnknownVec());
nlSystem->setUpdateFunction(dynaModel->getUpdateFunction());
@@ -1271,9 +1273,9 @@ void testDeformableBody()
// create a non-linear solver and add to the scene
auto nlSolver = std::make_shared<NewtonSolver>();
+ cgLinSolver->setLinearProjectors(&projList);
nlSolver->setLinearSolver(cgLinSolver);
nlSolver->setSystem(nlSystem);
- //nlSolver->setToFullyImplicit();
scene->addNonlinearSolver(nlSolver);
// Display UPS
@@ -2286,7 +2288,6 @@ void testDeformableBodyCollision()
{
linProj.push_back(LinearProjectionConstraint(id, true));
}
- nlSystem->setLinearProjectors(linProj);
nlSystem->setUnknownVector(dynaModel->getUnknownVec());
nlSystem->setUpdateFunction(dynaModel->getUpdateFunction());
nlSystem->setUpdatePreviousStatesFunction(dynaModel->getUpdatePrevStateFunction());
@@ -2294,6 +2295,7 @@ void testDeformableBodyCollision()
// create a non-linear solver and add to the scene
auto nlSolver = std::make_shared<NewtonSolver>();
auto cgLinSolver = std::make_shared<ConjugateGradient>();// create a linear solver to be used in the NL solver
+ cgLinSolver->setLinearProjectors(&linProj);
nlSolver->setLinearSolver(cgLinSolver);
nlSolver->setSystem(nlSystem);
scene->addNonlinearSolver(nlSolver);
@@ -2386,7 +2388,6 @@ void liverToolInteraction()
{
linProj.push_back(LinearProjectionConstraint(id, true));
}
- nlSystem->setLinearProjectors(linProj);
nlSystem->setUnknownVector(dynaModel->getUnknownVec());
nlSystem->setUpdateFunction(dynaModel->getUpdateFunction());
nlSystem->setUpdatePreviousStatesFunction(dynaModel->getUpdatePrevStateFunction());
@@ -2394,6 +2395,7 @@ void liverToolInteraction()
// create a non-linear solver and add to the scene
auto nlSolver = std::make_shared<NewtonSolver>();
auto cgLinSolver = std::make_shared<ConjugateGradient>();// create a linear solver to be used in the NL solver
+ cgLinSolver->setLinearProjectors(&linProj);
nlSolver->setLinearSolver(cgLinSolver);
nlSolver->setSystem(nlSystem);
//nlSolver->setToFullyImplicit();
@@ -2574,3 +2576,132 @@ void testGeometryTransforms()
sdk->setCurrentScene(scene);
sdk->startSimulation(false);
}
+
+void testPicking()
+{
+ // SDK and Scene
+ auto sdk = std::make_shared<imstk::SimulationManager>();
+ auto scene = sdk->createNewScene("Picking");
+
+ //----------------------------------------------------------
+ // Create plane visual scene object
+ //----------------------------------------------------------
+ auto planeObj = apiutils::createVisualAnalyticalSceneObject(
+ imstk::Geometry::Type::Plane, scene, "VisualPlane", 100, Vec3d(0., -20., 0.));
+
+ //----------------------------------------------------------
+ // Create Nidus FE deformable scene object
+ //----------------------------------------------------------
+ // Load a tetrahedral mesh
+ auto tetMesh = imstk::MeshIO::read(iMSTK_DATA_ROOT"/oneTet/oneTet.veg");
+
+ if (!tetMesh)
+ {
+ LOG(WARNING) << "Could not read mesh from file.";
+ return;
+ }
+ // Extract the surface mesh
+ auto volTetMesh = std::dynamic_pointer_cast<imstk::TetrahedralMesh>(tetMesh);
+ if (!volTetMesh)
+ {
+ LOG(WARNING) << "Dynamic pointer cast from imstk::Mesh to imstk::TetrahedralMesh failed!";
+ return;
+ }
+ auto surfMesh = std::make_shared<imstk::SurfaceMesh>();
+ volTetMesh->extractSurfaceMesh(surfMesh);
+
+ // Construct one to one nodal map based on the above meshes
+ auto oneToOneNodalMap = std::make_shared<imstk::OneToOneMap>();
+ oneToOneNodalMap->setMaster(tetMesh);
+ oneToOneNodalMap->setSlave(surfMesh);
+ oneToOneNodalMap->compute();
+
+ // Configure the dynamic model
+ auto dynaModel = std::make_shared<FEMDeformableBodyModel>();
+ dynaModel->configure(iMSTK_DATA_ROOT"/oneTet/oneTet.config");
+ dynaModel->initialize(volTetMesh);
+
+ // Create and add Backward Euler time integrator
+ auto timeIntegrator = std::make_shared<BackwardEuler>(0.01);
+ dynaModel->setTimeIntegrator(timeIntegrator);
+
+ // Configure Scene Object
+ auto physicsObj = std::make_shared<DeformableObject>("deformableObj");
+ physicsObj->setVisualGeometry(surfMesh);
+ physicsObj->setCollidingGeometry(volTetMesh);
+ physicsObj->setPhysicsGeometry(volTetMesh);
+ physicsObj->setPhysicsToVisualMap(oneToOneNodalMap);
+ physicsObj->setDynamicalModel(dynaModel);
+ physicsObj->initialize();
+ scene->addSceneObject(physicsObj);
+
+ //----------------------------------------------------------
+ // Create a nonlinear system and its solver
+ //----------------------------------------------------------
+ auto nlSystem = std::make_shared<NonLinearSystem>(dynaModel->getFunction(), dynaModel->getFunctionGradient());
+ std::vector<LinearProjectionConstraint> linProj;
+ for (auto id : dynaModel->getFixNodeIds())
+ {
+ linProj.push_back(LinearProjectionConstraint(id, true));
+ }
+ nlSystem->setUnknownVector(dynaModel->getUnknownVec());
+ nlSystem->setUpdateFunction(dynaModel->getUpdateFunction());
+ nlSystem->setUpdatePreviousStatesFunction(dynaModel->getUpdatePrevStateFunction());
+ std::vector<LinearProjectionConstraint> dynLinProj;
+
+ // create a non-linear solver and add to the scene
+ auto nlSolver = std::make_shared<NewtonSolver>();
+ auto cgLinSolver = std::make_shared<ConjugateGradient>();// create a linear solver to be used in the NL solver
+ cgLinSolver->setLinearProjectors(&linProj);
+ cgLinSolver->setDynamicLinearProjectors(&dynLinProj);
+ nlSolver->setLinearSolver(cgLinSolver);
+ nlSolver->setSystem(nlSystem);
+ scene->addNonlinearSolver(nlSolver);
+
+ //----------------------------------------------------------
+ // Create object controller
+ //----------------------------------------------------------
+#ifdef iMSTK_USE_OPENHAPTICS
+
+ // Device clients
+ auto client = std::make_shared<imstk::HDAPIDeviceClient>("Default Device");
+
+ // Device Server
+ auto server = std::make_shared<imstk::HDAPIDeviceServer>();
+ server->addDeviceClient(client);
+ sdk->addModule(server);
+
+ // Sphere0
+ auto sphereForPickObj = apiutils::createCollidingAnalyticalSceneObject(
+ imstk::Geometry::Type::Sphere, scene, "Sphere0", 1, Vec3d(0., 0., 0.));
+
+ auto pickTrackingCtrl = std::make_shared<imstk::DeviceTracker>(client);
+ //pickTrackingCtrl->setTranslationOffset(Vec3d(0., 0., 24.));
+
+ auto pickController = std::make_shared<imstk::SceneObjectController>(sphereForPickObj, pickTrackingCtrl);
+ scene->addObjectController(pickController);
+
+ CollisionData coldata;
+ auto sphereGeo = std::dynamic_pointer_cast<Sphere>(sphereForPickObj->getCollidingGeometry());
+
+ // Create collision detection for picking
+ auto pickingCD = std::make_shared<MeshToSpherePickingCD>(volTetMesh, sphereGeo, coldata);
+ pickingCD->setDeviceTrackerAndButton(pickTrackingCtrl, 0);
+
+ // Create contact handling for picking
+ auto pickingCH = std::make_shared<PickingCH>(CollisionHandling::Side::A, coldata, physicsObj);
+ pickingCH->setDynamicLinearProjectors(&dynLinProj);
+
+ // Create collision pair
+ scene->getCollisionGraph()->addInteractionPair(physicsObj, sphereForPickObj, pickingCD, pickingCH, nullptr);
+#endif
+
+ // Set Camera configuration
+ auto cam = scene->getCamera();
+ auto camPosition = imstk::Vec3d(0, 40, 80);
+ cam->setPosition(camPosition);
+ cam->setFocalPoint(imstk::Vec3d(0, 0, 0));
+ // Run
+ sdk->setCurrentScene(scene);
+ sdk->startSimulation(true);
+}