ENH: Implement PlaneToSphereCD

Base/Collision/CMakeLists.txt

@@ -4,6 +4,7 @@
 include(imstkAddLibrary)
 imstk_add_library( Collision
   DEPENDS
+    Geometry
     SceneElements
   )
 

Base/Collision/CollisionData/imstkCollisionData.h

@@ -26,10 +26,17 @@
 
 namespace imstk {
 
-struct VertexNormalCollisionData
+struct PositionDirectionCollisionData
+{
+    Vec3d position;
+    Vec3d direction;
+    double penetrationDepth;
+};
+
+struct VertexDirectionCollisionData
 {
     size_t vertexId;
-    Vec3d normal;
+    Vec3d direction;
     double penetrationDepth;
 };
 
@@ -49,9 +56,10 @@ struct EdgeEdgeCollisionData
 
 struct CollisionData
 {
-    std::vector<VertexNormalCollisionData> VNColData;   //!< Vertex Normal collision data
-    std::vector<VertexTriangleCollisionData> VTColData; //!< Vertex Triangle collision data
-    std::vector<EdgeEdgeCollisionData> EEColData;       //!< Edge Edge collision data
+    std::vector<PositionDirectionCollisionData> PDColData; //!< Position Direction collision data
+    std::vector<VertexDirectionCollisionData> VDColData;   //!< Vertex Direction collision data
+    std::vector<VertexTriangleCollisionData> VTColData;    //!< Vertex Triangle collision data
+    std::vector<EdgeEdgeCollisionData> EEColData;          //!< Edge Edge collision data
 };
 }
 

Base/Collision/CollisionDetection/imstkCollisionDetection.cpp

@@ -21,6 +21,11 @@
 
 #include "imstkCollisionDetection.h"
 
+#include "imstkPlaneToSphereCD.h"
+
+#include "imstkCollidingObject.h"
+#include "imstkGeometry.h"
+
 #include <g3log/g3log.hpp>
 
 namespace imstk {
@@ -30,8 +35,31 @@ CollisionDetection::make_collision_detection(const Type& type,
                                              std::shared_ptr<CollidingObject> objA,
                                              std::shared_ptr<CollidingObject> objB)
 {
-    LOG(WARNING) << "CollisionDetection::make_collision_detection not implemented.";
-    return nullptr;
+    switch (type)
+    {
+    case Type::PlaneToSphere:
+    {
+        if (objA->getCollidingGeometry()->getType() != Geometry::Type::Plane ||
+            objB->getCollidingGeometry()->getType() != Geometry::Type::Sphere)
+        {
+            LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
+                         << "invalid object geometries for PlaneToSphere collision detection.";
+            return nullptr;
+        }
+        return std::make_shared<PlaneToSphereCD>();
+    }break;
+    default:
+    {
+        LOG(WARNING) << "CollisionDetection::make_collision_detection error: type not implemented.";
+        return nullptr;
+    }
+    }
+
 }
 
+const CollisionDetection::Type&
+CollisionDetection::getType() const
+{
+    return m_type;
+}
 }

Base/Collision/CollisionDetection/imstkPlaneToSphereCD.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 "imstkPlaneToSphereCD.h"
+
+#include "imstkCollidingObject.h"
+#include "imstkCollisionData.h"
+#include "imstkPlane.h"
+#include "imstkSphere.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk {
+
+void
+PlaneToSphereCD::computeCollisionData(std::shared_ptr<CollidingObject> objA,
+                                      std::shared_ptr<CollidingObject> objB,
+                                      CollisionData& colDataA,
+                                      CollisionData& colDataB)
+{
+    auto planeGeom = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
+    auto sphereGeom = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
+
+    // Geometries check
+    if (planeGeom == nullptr || sphereGeom == nullptr)
+    {
+        LOG(WARNING) << "PlaneToSphereCD::computeCollisionData error: invalid geometries.";
+        return;
+    }
+
+    // Get geometry properties
+    Vec3d sP = sphereGeom->getPosition();
+    double r = sphereGeom->getRadius();
+    Vec3d pP = planeGeom->getPosition();
+    Vec3d n = planeGeom->getNormal();
+
+    // Compute shortest distance
+    double d = (sP-pP).dot(n);
+
+    // Define sphere to plane direction
+    Vec3d dir = -n;
+    if( d < 0 )
+    {
+        d = -d;
+        dir = n;
+    }
+
+    // Return if no penetration
+    double penetrationDepth = r-d;
+    if ( penetrationDepth < 0)
+    {
+        return;
+    }
+
+    // Compute collision points
+    Vec3d pC = sP + dir*d;
+    Vec3d sC = sP + dir*r;
+
+    // Set collisionData
+    PositionDirectionCollisionData cdA = {pC, -dir, penetrationDepth};
+    PositionDirectionCollisionData cdB = {sC, dir, penetrationDepth};
+    colDataA.PDColData.push_back({pC, dir, penetrationDepth});
+    colDataB.PDColData.push_back(cdB);
+
+    LOG(DEBUG) << "Collision between "
+               << objA->getName() << " & " << objB->getName()
+               << " : " << penetrationDepth;
+
+}
+
+}

Base/Collision/CollisionDetection/imstkPlaneToSphereCD.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 imstkPlaneToSphereCD_h
+#define imstkPlaneToSphereCD_h
+
+#include "imstkCollisionDetection.h"
+
+#include <memory>
+
+namespace imstk {
+
+class CollidingObject;
+class CollisionData;
+
+class PlaneToSphereCD : public CollisionDetection
+{
+public:
+
+    ///
+    /// \brief Constructor
+    ///
+    PlaneToSphereCD() : CollisionDetection(CollisionDetection::Type::PlaneToSphere) {}
+
+    ///
+    /// \brief Destructor
+    ///
+    ~PlaneToSphereCD() = default;
+
+    ///
+    /// \brief Detect collision and compute collision data
+    ///
+    void computeCollisionData(std::shared_ptr<CollidingObject> objA,
+                              std::shared_ptr<CollidingObject> objB,
+                              CollisionData& colDataA,
+                              CollisionData& colDataB) override;
+
+};
+}
+
+#endif // ifndef imstkPlaneToSphereCD_h