ENH: Add collision between cylinder and sphere

Base/Collision/CollisionDetection/imstkCollisionDetection.cpp

@@ -27,6 +27,7 @@
 #include "imstkMeshToSphereCD.h"
 #include "imstkMeshToPlaneCD.h"
 #include "imstkMeshToMeshCD.h"
+#include "imstkSphereCylinderCD.h"
 #include "imstkMeshToSpherePickingCD.h"
 
 #include "imstkCollidingObject.h"
@@ -91,6 +92,21 @@ CollisionDetection::make_collision_detection(const Type& type,
         return std::make_shared<SphereToSphereCD>(sphereA, sphereB, colData);
     }
     break;
+    case Type::SphereToCylinder:
+    {
+        auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
+        auto cylinder = std::dynamic_pointer_cast<Cylinder>(objA->getCollidingGeometry());
+
+        // Geometries check
+        if (sphere == nullptr || cylinder == nullptr)
+        {
+            LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
+                         << "invalid object geometries for SphereToCylinder collision detection.";
+            return nullptr;
+        }
+        return std::make_shared<SphereCylinderCD>(sphere, cylinder, colData);
+    }
+    break;
     case Type::MeshToSphere:
     {
         auto mesh = std::dynamic_pointer_cast<Mesh>(objA->getCollidingGeometry());

Base/Collision/CollisionDetection/imstkCollisionDetection.h

@@ -45,6 +45,7 @@ public:
     {
         UnidirectionalPlaneToSphere,
         BidirectionalPlaneToSphere,
+        SphereToCylinder,
         SphereToSphere,
         MeshToSphere,
         MeshToPlane,

Base/Collision/CollisionDetection/imstkSphereCylinderCD.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 imstkSphereCylinderCD_h
+#define imstkSphereCylinderCD_h
+
+#include <memory>
+
+#include "imstkCollisionDetection.h"
+#include "imstkCylinder.h"
+
+namespace imstk
+{
+class Sphere;
+class CollisionData;
+
+///
+/// \class SphereCylinderCD
+///
+/// \brief Sphere-Cylinder collision detection
+///
+class SphereCylinderCD : public CollisionDetection
+{
+public:
+
+    ///
+    /// \brief Constructor
+    ///
+    SphereCylinderCD(std::shared_ptr<Sphere> sphere,
+                     std::shared_ptr<Cylinder> cylinder,
+                     CollisionData& colData) :
+        CollisionDetection(CollisionDetection::Type::SphereToCylinder, colData),
+        m_cylinder(cylinder),
+        m_sphere(sphere)
+    {}
+
+    ///
+    /// \brief Destructor
+    ///
+    ~SphereCylinderCD() = default;
+
+    ///
+    /// \brief Detect collision and compute collision data
+    ///
+    void computeCollisionData() override;
+
+private:
+
+    std::shared_ptr<Cylinder> m_cylinder;    ///> colliding cylinder
+    std::shared_ptr<Sphere> m_sphere;        ///> colliding sphere
+};
+}
+
+#endif // ifndef imstkSphereCylinderCD_h

Base/Collision/CollisionDetection/imstkSphererCylinderCD.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 "imstkSphereCylinderCD.h"
+
+#include "imstkCollidingObject.h"
+#include "imstkCollisionData.h"
+#include "imstkSphere.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk {
+void
+SphereCylinderCD::computeCollisionData()
+{
+    // Clear collisionData
+    m_colData.clearAll();
+
+    // Get geometry properties
+    const Vec3d spherePos = m_sphere->getPosition();
+    const double rSphere = m_sphere->getRadius();
+
+    const Vec3d cylinderPos = m_cylinder->getPosition();
+    const Vec3d cylinderAxis = m_cylinder->getOrientationAxis();
+    const double rCylinder = m_cylinder->getRadius();
+
+    // Compute shortest distance
+    Vec3d distVec = (spherePos - cylinderPos) - cylinderAxis*(spherePos - cylinderPos).dot(cylinderAxis);
+    Vec3d n = -distVec / distVec.norm();
+
+    // Compute penetration depth
+    double penetrationDepth = distVec.norm() - rSphere - rCylinder;
+    if (penetrationDepth > 0.0)
+    {
+        return;
+    }
+
+    // Compute collision points
+    Vec3d sphereColPt = spherePos + rSphere*n;
+    Vec3d cylinderColPt = cylinderPos + cylinderAxis*(spherePos - cylinderPos).dot(cylinderAxis) + n * rCylinder;
+
+    // Set collisionData
+    m_colData.PDColData.push_back({ sphereColPt, cylinderColPt, n, penetrationDepth });
+}
+} //imstk

Base/Collision/CollisionHandling/imstkVirtualCouplingCH.cpp

@@ -32,6 +32,8 @@ namespace imstk
 void
 VirtualCouplingCH::computeContactForces()
 {
+    m_offset.setZero();
+
     const auto collidingGeometry = std::static_pointer_cast<AnalyticalGeometry>(m_object->getCollidingGeometry());
     const auto visualGeometry =  std::static_pointer_cast<AnalyticalGeometry>(m_object->getVisualGeometry());
 
@@ -56,7 +58,8 @@ VirtualCouplingCH::computeContactForces()
     visualGeometry->setPosition(visualObjPos);
 
     // Spring force
-    Vec3d force = m_stiffness * (visualObjPos - collidingObjPos);
+    m_offset = (visualObjPos - collidingObjPos);
+    Vec3d force = m_stiffness * m_offset;
 
     // Damping force
     const double dt = 0.1; // Time step size to calculate the object velocity
@@ -69,4 +72,10 @@ VirtualCouplingCH::computeContactForces()
     m_initialStep = false;
     m_prevPos = collidingObjPos;
 }
+
+imstk::Vec3d
+VirtualCouplingCH::getOffset()
+{
+    return m_offset;
+}
 }// iMSTK

Base/Collision/CollisionHandling/imstkVirtualCouplingCH.h

@@ -72,11 +72,17 @@ public:
     inline double getDamping() const { return m_damping; }
     inline void setDamping(const double damping) { m_damping = damping; }
 
+    ///
+    /// \brief Get the offset of the
+    ///
+    Vec3d getOffset();
+
 private:
 
     std::shared_ptr<CollidingObject> m_object;  ///> colliding object
     bool m_initialStep = true;                  ///> Number of times steps
     Vec3d m_prevPos;                            ///> Previous position of the colliding object
+    Vec3d m_offset;
 
     double m_stiffness = 5e-01;                 ///> Stiffness coefficient use to compute spring force
     double m_damping = 0.005;                   ///> Damping coefficient used to compute damping force

Base/Geometry/Analytic/imstkCylinder.h

@@ -81,10 +81,10 @@ protected:
     void applyScaling(const double s) override;
     void updatePostTransformData() override;
 
-    double m_radius = 1.0;              ///> Radius of the cylinder
-    double m_length = 1.0;              ///> Length of the cylinder
-    double m_radiusPostTransform = 1.0; ///> Radius of the cylinder once transform applied
-    double m_lengthPostTransform = 1.0; ///> Length of the cylinder once transform applied
+    double m_radius = 0.7;              ///> Radius of the cylinder
+    double m_length = 10.;              ///> Length of the cylinder
+    double m_radiusPostTransform = 0.7; ///> Radius of the cylinder oncee transform applied
+    double m_lengthPostTransform = 10.; ///> Length of the cylinder onc transform applied
 };
 } // imstk