diff --git a/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp b/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp index 8e46d6a290fdd6cc0c2144b870adde406179bde7..a25ab29957bf090021440d7a7a2e83d8749dc4fe 100644 --- a/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp +++ b/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp @@ -21,6 +21,7 @@ #include "imstkCollisionDetection.h" +#include "imstkUnidirectionalPlaneToSphereCD.h" #include "imstkPlaneToSphereCD.h" #include "imstkSphereToSphereCD.h" #include "imstkMeshToSphereCD.h" @@ -45,6 +46,21 @@ CollisionDetection::make_collision_detection(const Type& type, { switch (type) { + case Type::UnidirectionalPlaneToSphere: + { + auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry()); + auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry()); + + // Geometries check + if (plane == nullptr || sphere == nullptr) + { + LOG(WARNING) << "CollisionDetection::make_collision_detection error: " + << "invalid object geometries for UnidirectionalPlaneToSphere collision detection."; + return nullptr; + } + return std::make_shared<UnidirectionalPlaneToSphereCD>(plane, sphere, colData); + } + break; case Type::PlaneToSphere: { auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry()); diff --git a/Base/Collision/CollisionDetection/imstkCollisionDetection.h b/Base/Collision/CollisionDetection/imstkCollisionDetection.h index 0621d7b1aa80f7cfaa9e842d17df46f4838182f3..769ae81635e843e07f4e651731ddb99770b82cce 100644 --- a/Base/Collision/CollisionDetection/imstkCollisionDetection.h +++ b/Base/Collision/CollisionDetection/imstkCollisionDetection.h @@ -44,6 +44,7 @@ public: /// enum class Type { + UnidirectionalPlaneToSphere, PlaneToSphere, SphereToSphere, MeshToSphere, diff --git a/Base/Collision/CollisionDetection/imstkUnidirectionalPlaneToSphereCD.cpp b/Base/Collision/CollisionDetection/imstkUnidirectionalPlaneToSphereCD.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f029f6d14f56098c87dd8d609c725a5536963f7f --- /dev/null +++ b/Base/Collision/CollisionDetection/imstkUnidirectionalPlaneToSphereCD.cpp @@ -0,0 +1,67 @@ +/*========================================================================= + + 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 "imstkUnidirectionalPlaneToSphereCD.h" + +#include "imstkCollidingObject.h" +#include "imstkCollisionData.h" +#include "imstkPlane.h" +#include "imstkSphere.h" + +#include <g3log/g3log.hpp> + +namespace imstk { + +void +UnidirectionalPlaneToSphereCD::computeCollisionData() +{ + // Clear collisionData + m_colData.clearAll(); + + // Get geometry properties + const Vec3d sphereBPos = m_sphereB->getPosition(); + const double r = m_sphereB->getRadius() * m_sphereB->getScaling(); + const Vec3d planeAPos = m_planeA->getPosition(); + const Vec3d n = m_planeA->getNormal(); + + // Compute shortest distance + double d = (sphereBPos-planeAPos).dot(n); + + // Compute penetration depth + // Half-space defined by the normal of the plane is considered as "outside". + double penetrationDepth = r-d; + if (penetrationDepth <= 0.0) + { + return; + } + + // Compute collision points + Vec3d planeAColPt = sphereBPos - n*d; + Vec3d sphereBColPt = sphereBPos - n*r; + + // Compute the direction from plane to sphere + Vec3d dirAtoB = (d > 0.0 ? 1.0 : -1.0)*n; + + // Set collisionData + m_colData.PDColData.push_back({planeAColPt, sphereBColPt, dirAtoB, penetrationDepth}); +} + +} //iMSTK diff --git a/Base/Collision/CollisionDetection/imstkUnidirectionalPlaneToSphereCD.h b/Base/Collision/CollisionDetection/imstkUnidirectionalPlaneToSphereCD.h new file mode 100644 index 0000000000000000000000000000000000000000..ebb076d9f9a3a044e0a7ba83fb62d42c4e9c6140 --- /dev/null +++ b/Base/Collision/CollisionDetection/imstkUnidirectionalPlaneToSphereCD.h @@ -0,0 +1,74 @@ +/*========================================================================= + + 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 imstkUnidirectionalPlaneToSphereCD_h +#define imstkUnidirectionalPlaneToSphereCD_h + +#include <memory> + +#include "imstkCollisionDetection.h" + +namespace imstk +{ + +class Plane; +class Sphere; +class CollisionData; + +/// +/// \class PlaneToSphereCD +/// +/// \brief Plane to sphere collision detection +/// +class UnidirectionalPlaneToSphereCD : public CollisionDetection +{ +public: + + /// + /// \brief Constructor + /// + UnidirectionalPlaneToSphereCD(std::shared_ptr<Plane> planeA, + std::shared_ptr<Sphere> sphereB, + CollisionData& colData) : + CollisionDetection(CollisionDetection::Type::UnidirectionalPlaneToSphere, colData), + m_planeA(planeA), + m_sphereB(sphereB) + {} + + /// + /// \brief Destructor + /// + ~UnidirectionalPlaneToSphereCD() = default; + + /// + /// \brief Detect collision and compute collision data + /// + void computeCollisionData() override; + +private: + + std::shared_ptr<Plane> m_planeA; ///> + std::shared_ptr<Sphere> m_sphereB; ///> +}; + +} + +#endif // ifndef imstkUnidirectionalToSphereCD_h