ENH: Add collision Mesh to sphere and plane coll. detection classes

 Add collision Mesh to sphere and plane coll. detection classes based on brute force approach.

Base/Collision/CollisionData/imstkCollisionData.h

@@ -42,13 +42,13 @@ struct PositionDirectionCollisionData
 };
 
 ///
-/// \struct MeshToAnalyticalPointDepthCollisionData
+/// \struct MeshToAnalyticalCollisionData
 ///
 /// \brief Mesh to analytical point-penetration depth collision data
 ///
-struct MeshToAnalyticalPointDepthCollisionData
+struct MeshToAnalyticalCollisionData
 {
-    int nodeId;
+    size_t nodeId;
     Vec3d penetrationVector;
 };
 
@@ -126,12 +126,12 @@ public:
         EEColData.clear();
     }
 
-    std::vector<PositionDirectionCollisionData> PDColData; //!< Position Direction collision data
-    std::vector<VertexTriangleCollisionData> VTColData;    //!< Vertex Triangle collision data
-    std::vector<TriangleVertexCollisionData> TVColData;    //!< Triangle Vertex collision data
-    std::vector<EdgeEdgeCollisionData> EEColData;          //!< Edge Edge collision data
+    std::vector<PositionDirectionCollisionData> PDColData; ///< Position Direction collision data
+    std::vector<VertexTriangleCollisionData> VTColData;    ///< Vertex Triangle collision data
+    std::vector<TriangleVertexCollisionData> TVColData;    ///< Triangle Vertex collision data
+    std::vector<EdgeEdgeCollisionData> EEColData;          ///< Edge Edge collision data
 
-    std::vector<MeshToAnalyticalPointDepthCollisionData> PDMeshAnaColData;
+    std::vector<MeshToAnalyticalCollisionData> MAColData;  ///< Mesh to analytical collision data
 };
 
 }

Base/Collision/CollisionDetection/imstkCollisionDetection.cpp

@@ -23,6 +23,8 @@
 
 #include "imstkPlaneToSphereCD.h"
 #include "imstkSphereToSphereCD.h"
+#include "imstkMeshToSphereCD.h"
+#include "imstkMeshToPlaneCD.h"
 #include "imstkMeshToMeshCD.h"
 
 #include "imstkCollidingObject.h"
@@ -32,7 +34,8 @@
 
 #include <g3log/g3log.hpp>
 
-namespace imstk {
+namespace imstk
+{
 
 std::shared_ptr<CollisionDetection>
 CollisionDetection::make_collision_detection(const Type& type,
@@ -55,7 +58,8 @@ CollisionDetection::make_collision_detection(const Type& type,
             return nullptr;
         }
         return std::make_shared<PlaneToSphereCD>(plane, sphere, colData);
-    }break;
+    }
+    break;
     case Type::SphereToSphere:
     {
         auto sphereA = std::dynamic_pointer_cast<Sphere>(objA->getCollidingGeometry());
@@ -69,7 +73,38 @@ CollisionDetection::make_collision_detection(const Type& type,
             return nullptr;
         }
         return std::make_shared<SphereToSphereCD>(sphereA, sphereB, colData);
-    }break;
+    }
+    break;
+    case Type::MeshToSphere:
+    {
+        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<MeshToSphereCD>(mesh, sphere, colData);
+    }
+    break;
+    case Type::MeshToPlane:
+    {
+        auto mesh = std::dynamic_pointer_cast<Mesh>(objA->getCollidingGeometry());
+        auto plane = std::dynamic_pointer_cast<Plane>(objB->getCollidingGeometry());
+
+        // Geometries check
+        if (mesh == nullptr || plane == nullptr)
+        {
+            LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
+                << "invalid object geometries for SphereToSphere collision detection.";
+            return nullptr;
+        }
+        return std::make_shared<MeshToPlaneCD>(mesh, plane, colData);
+    }
+    break;
     case Type::MeshToMesh:
     {
         auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
@@ -83,7 +118,8 @@ CollisionDetection::make_collision_detection(const Type& type,
             return nullptr;
         }
         return std::make_shared<MeshToMeshCD>(meshA, meshB, colData);
-    }break;
+    }
+    break;
     default:
     {
         LOG(WARNING) << "CollisionDetection::make_collision_detection error: type not implemented.";

Base/Collision/CollisionDetection/imstkCollisionDetection.h

@@ -33,7 +33,7 @@ class CollisionData;
 ///
 /// \class CollisionDetection
 ///
-/// \brief Base class for all collsion detection classes
+/// \brief Base class for all collision detection classes
 ///
 class CollisionDetection
 {
@@ -46,6 +46,8 @@ public:
     {
         PlaneToSphere,
         SphereToSphere,
+        MeshToSphere,
+        MeshToPlane,
         MeshToMesh
     };
 
@@ -64,6 +66,7 @@ public:
         m_type(type),
         m_colData(colData)
     {}
+    CollisionDetection() = delete;
 
     ///
     /// \brief Destructor

Base/Collision/CollisionDetection/imstkMeshToPlaneCD.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version B.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-B.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 "imstkMeshToPlaneCD.h"
+
+#include "imstkCollidingObject.h"
+#include "imstkCollisionData.h"
+#include "imstkPlane.h"
+#include "imstkMesh.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk
+{
+
+void
+MeshToPlaneCD::computeCollisionData()
+{
+    // Clear collisionData
+    m_colData.clearAll();
+
+    // Get plane properties
+    auto planePos = m_plane->getPosition();
+
+    size_t nodeId = 0;
+    for (const auto& p : m_mesh->getVertexPositions())
+    {
+        auto peneDistance = (planePos - p).dot(m_plane->getNormal());
+        if (peneDistance <= 0.0)
+        {
+            m_colData.MAColData.push_back({ nodeId, m_plane->getNormal() * -peneDistance });
+        }
+        nodeId++;
+    }
+
+}
+
+} // imstk
\ No newline at end of file

Base/Collision/CollisionDetection/imstkMeshToPlaneCD.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 imstkMeshToPlaneCD_h
+#define imstkMeshToPlaneCD_h
+
+#include <memory>
+
+#include "imstkCollisionDetection.h"
+
+namespace imstk
+{
+
+class Mesh;
+class Plane;
+class CollisionData;
+
+///
+/// \class MeshToPlaneCD
+///
+/// \brief Mesh to plane collision detection
+///
+class MeshToPlaneCD : public CollisionDetection
+{
+public:
+
+    ///
+    /// \brief Constructor
+    ///
+    MeshToPlaneCD(std::shared_ptr<Mesh> mesh,
+                  std::shared_ptr<Plane> plane,
+                  CollisionData& colData) :
+				  CollisionDetection(CollisionDetection::Type::MeshToSphere,
+                  colData),
+				  m_mesh(mesh),
+				  m_plane(plane){}
+
+    ///
+    /// \brief Destructor
+    ///
+    ~MeshToPlaneCD() = default;
+
+    ///
+    /// \brief Detect collision and compute collision data
+    ///
+    void computeCollisionData() override;
+
+private:
+
+    std::shared_ptr<Mesh> m_mesh;      ///> Mesh
+    std::shared_ptr<Plane> m_plane;    ///> Plane
+};
+
+}
+
+#endif // ifndef imstkMeshToPlaneCD_h

Base/Collision/CollisionDetection/imstkMeshToSphereCD.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version B.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-B.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 "imstkMeshToSphereCD.h"
+
+#include "imstkCollidingObject.h"
+#include "imstkCollisionData.h"
+#include "imstkSphere.h"
+#include "imstkMesh.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk
+{
+
+void
+MeshToSphereCD::computeCollisionData()
+{
+    // Clear collisionData
+    m_colData.clearAll();
+
+    // 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)
+        {
+            auto direction = (spherePos - p)/dist;
+            auto pointOnSphere = spherePos - radius*direction;
+            m_colData.MAColData.push_back({ nodeId, p - pointOnSphere });
+        }
+        nodeId++;
+    }
+
+}
+
+} // imstk
\ No newline at end of file

Base/Collision/CollisionDetection/imstkMeshToSphereCD.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 imstkMeshToSphereCD_h
+#define imstkMeshToSphereCD_h
+
+#include <memory>
+
+#include "imstkCollisionDetection.h"
+
+namespace imstk
+{
+
+class Mesh;
+class Sphere;
+class CollisionData;
+
+///
+/// \class MeshToSphereCD
+///
+/// \brief Mesh to sphere collision detection
+///
+class MeshToSphereCD : public CollisionDetection
+{
+public:
+
+    ///
+    /// \brief Constructor
+    ///
+    MeshToSphereCD(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
+    ///
+    ~MeshToSphereCD() = default;
+
+    ///
+    /// \brief Detect collision and compute collision data
+    ///
+    void computeCollisionData() override;
+
+private:
+
+    std::shared_ptr<Mesh> m_mesh;       ///> Mesh
+    std::shared_ptr<Sphere> m_sphere;  ///> Sphere
+};
+
+}
+
+#endif // ifndef imstkMeshToSphereCD_h