Commit 7623094f authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

ENH: Add collision between cylinder and sphere

parent 01803ad5
......@@ -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());
......
......@@ -45,6 +45,7 @@ public:
{
UnidirectionalPlaneToSphere,
BidirectionalPlaneToSphere,
SphereToCylinder,
SphereToSphere,
MeshToSphere,
MeshToPlane,
......
/*=========================================================================
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
/*=========================================================================
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
......@@ -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
......@@ -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
......
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment