Commit c880f974 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

Merge branch 'sphereToCylinderCD' into 'master'

ENH: Add collision between cylinder and sphere

See merge request !185
parents c4390700 51c270ce
......@@ -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
......
......@@ -137,6 +137,7 @@ void testVirtualCoupling();
void testGeometryTransforms();
void testPicking();
void testBoneDrilling();
void testVirtualCouplingCylinder();
int main()
{
......@@ -169,7 +170,7 @@ int main()
//testOneToOneNodalMap();
//testSurfaceMeshOptimizer();
//testAnalyticalGeometry();
testGeometryTransforms();
//testGeometryTransforms();
/*------------------
......@@ -207,8 +208,9 @@ int main()
------------------*/
//testScenesManagement();
//testVectorPlotters();
//testVirtualCoupling();
testBoneDrilling();
testVirtualCoupling();
//testBoneDrilling();
testVirtualCouplingCylinder();
return 0;
......@@ -2494,7 +2496,7 @@ void testVirtualCoupling()
#ifdef iMSTK_USE_OPENHAPTICS
// Device clients
auto client = std::make_shared<imstk::HDAPIDeviceClient>("Default Device");
auto client = std::make_shared<imstk::HDAPIDeviceClient>("PHANToM 1");
// Device Server
auto server = std::make_shared<imstk::HDAPIDeviceServer>();
......@@ -2799,3 +2801,81 @@ void testBoneDrilling()
sdk->setCurrentScene(scene);
sdk->startSimulation(false);
}
// test virtual coupling for cylinder to sphere collision
void testVirtualCouplingCylinder()
{
// SDK and Scene
auto sdk = std::make_shared<imstk::SimulationManager>();
auto scene = sdk->createNewScene("VirtualCouplingCylinderSphere");
// Create a plane in the scene (visual)
auto planeGeom = std::make_shared<imstk::Plane>();
planeGeom->setWidth(10);
planeGeom->setPosition(0.0, -50, 0.0);
auto planeObj = std::make_shared<imstk::VisualObject>("Plane");
planeObj->setVisualGeometry(planeGeom);
scene->addSceneObject(planeObj);
// Create the virtual coupling object controller
#ifdef iMSTK_USE_OPENHAPTICS
// Device clients
auto client = std::make_shared<imstk::HDAPIDeviceClient>("PHANToM 1");
// Device Server
auto server = std::make_shared<imstk::HDAPIDeviceServer>();
server->addDeviceClient(client);
sdk->addModule(server);
// Device tracker
auto deviceTracker = std::make_shared<imstk::DeviceTracker>(client);
// Create a virtual coupling object
auto visualGeom = std::make_shared<imstk::Sphere>();
visualGeom->setRadius(5.);
auto collidingGeom = std::make_shared<imstk::Sphere>();
collidingGeom->setRadius(5);
auto virtualCouplingSphereObj = std::make_shared<CollidingObject>("VirtualCouplingObject");
virtualCouplingSphereObj->setCollidingGeometry(collidingGeom);
virtualCouplingSphereObj->setVisualGeometry(visualGeom);
scene->addSceneObject(virtualCouplingSphereObj);
// Create colliding cylinder scene object
auto CylinderGeom = std::make_shared<Cylinder>();
//CylinderGeom->setRadius(20.);
//CylinderGeom->setLength(40.);
auto CylinderObj = std::make_shared<CollidingObject>("Cylinder");
CylinderObj->setVisualGeometry(CylinderGeom);
CylinderObj->setCollidingGeometry(CylinderGeom);
scene->addSceneObject(CylinderObj);
// Create and add virtual coupling object controller in the scene
auto objController = std::make_shared<imstk::SceneObjectController>(virtualCouplingSphereObj,
deviceTracker);
scene->addObjectController(objController);
// Create a collision graph
auto graph = scene->getCollisionGraph();
auto pair = graph->addInteractionPair(CylinderObj, virtualCouplingSphereObj,
CollisionDetection::Type::SphereToCylinder,
CollisionHandling::Type::None,
CollisionHandling::Type::VirtualCoupling);
// Customize collision handling algorithm
auto colHandlingAlgo = std::dynamic_pointer_cast<VirtualCouplingCH>(pair->getCollisionHandlingB());
colHandlingAlgo->setStiffness(5e-1);
colHandlingAlgo->setDamping(0.005);
#endif
// Move Camera
auto cam = scene->getCamera();
cam->setPosition(imstk::Vec3d(200, 200, 200));
cam->setFocalPoint(imstk::Vec3d(0, 0, 0));
//Run
sdk->setCurrentScene(scene);
sdk->startSimulation(false);
}
Supports Markdown
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