diff --git a/Base/Collision/CMakeLists.txt b/Base/Collision/CMakeLists.txt
index e543c13eb2aab49b8bd875b648bd1e4a2fd64c87..a32c0446a69c32ed4c92b2358faa0766e424e8fe 100644
--- a/Base/Collision/CMakeLists.txt
+++ b/Base/Collision/CMakeLists.txt
@@ -4,6 +4,7 @@
include(imstkAddLibrary)
imstk_add_library( Collision
DEPENDS
+ Geometry
SceneElements
)
diff --git a/Base/Collision/CollisionData/imstkCollisionData.h b/Base/Collision/CollisionData/imstkCollisionData.h
index 1490dbf508e1e24cc539601e23b9feebf68b5371..31949cb31696400b06d6d127649370a64a8d586d 100644
--- a/Base/Collision/CollisionData/imstkCollisionData.h
+++ b/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
};
}
diff --git a/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp b/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp
index b158f036f4076ab00518adafa4e0753ef67b68fe..c0b316a4224137cc4bee3cb73365451aa8fa169f 100644
--- a/Base/Collision/CollisionDetection/imstkCollisionDetection.cpp
+++ b/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;
+}
}
diff --git a/Base/Collision/CollisionDetection/imstkPlaneToSphereCD.cpp b/Base/Collision/CollisionDetection/imstkPlaneToSphereCD.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..75e59286656db06140198efdcc0ca5cb638bdd4f
--- /dev/null
+++ b/Base/Collision/CollisionDetection/imstkPlaneToSphereCD.cpp
@@ -0,0 +1,89 @@
+/*=========================================================================
+
+ 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;
+
+}
+
+}
diff --git a/Base/Collision/CollisionDetection/imstkPlaneToSphereCD.h b/Base/Collision/CollisionDetection/imstkPlaneToSphereCD.h
new file mode 100644
index 0000000000000000000000000000000000000000..0609ac327e0970ad7d3fed29721fa6b8e4032684
--- /dev/null
+++ b/Base/Collision/CollisionDetection/imstkPlaneToSphereCD.h
@@ -0,0 +1,59 @@
+/*=========================================================================
+
+ 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