ENH: Adds collision detection of mesh nodal picking

Adds a sphere to mesh collision detection.
The radius of the sphere is small to use for picking.

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
 };
 
 }

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.";

Base/Collision/CollisionDetection/imstkCollisionDetection.h

@@ -50,7 +50,8 @@ public:
         MeshToSphere,
         MeshToPlane,
         MeshToMesh,
-        MeshToCapsule
+        MeshToCapsule,
+        MeshToSpherePicking
     };
 
     ///

Base/Collision/CollisionDetection/imstkMeshToSpherePickingCD.cpp

+/*=========================================================================
+
+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

Base/Collision/CollisionDetection/imstkMeshToSpherePickingCD.h

+/*=========================================================================
+
+   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