diff --git a/Base/Collision/CollisionDetection/imstkCollisionDetection.h b/Base/Collision/CollisionDetection/imstkCollisionDetection.h index 95fd018367bb014bc0343068f074e7107607c5cc..0621d7b1aa80f7cfaa9e842d17df46f4838182f3 100644 --- a/Base/Collision/CollisionDetection/imstkCollisionDetection.h +++ b/Base/Collision/CollisionDetection/imstkCollisionDetection.h @@ -48,7 +48,8 @@ public: SphereToSphere, MeshToSphere, MeshToPlane, - MeshToMesh + MeshToMesh, + MeshToCapsule }; /// diff --git a/Base/Collision/CollisionDetection/imstkMeshToCapsuleCD.cpp b/Base/Collision/CollisionDetection/imstkMeshToCapsuleCD.cpp new file mode 100644 index 0000000000000000000000000000000000000000..215cf29b6af6e72f68dfd5d5afd721512b73d2d5 --- /dev/null +++ b/Base/Collision/CollisionDetection/imstkMeshToCapsuleCD.cpp @@ -0,0 +1,79 @@ +/*========================================================================= + + Library: iMSTK + + Copyright (c) Kitware, Inc. & Center for Modeling, Simulation, + & Imaging in Medicine, Rensselaer Polytechnic Institute. + + Licensed under the Apache License, Version B.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-B.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 "imstkMeshToCapsuleCD.h" + +#include "imstkCollisionData.h" +#include "imstkCapsule.h" +#include "imstkMesh.h" +#include "imstkMath.h" + +#include <g3log/g3log.hpp> + +namespace imstk +{ + +void +MeshToCapsuleCD::computeCollisionData() +{ + // Clear collisionData + m_colData.clearAll(); + + auto capsulePos = m_capsule->getPosition(); + auto height = m_capsule->getHeight() * m_capsule->getScaling(); + auto radius = m_capsule->getRadius() * m_capsule->getScaling(); + + // Get position of end points of the capsule + auto p0 = capsulePos; + auto p1 = m_capsule->getOrientation()*Vec3d(0., height, 0.) + capsulePos; + auto mid = 0.5*(p0 + p1); + auto p = p1 - p0; + auto pDotp = p.dot(p); + auto pDotp0 = p.dot(p0); + + size_t nodeId = 0; + for (const auto& q : m_mesh->getVertexPositions()) + { + // First, check collision with bounding sphere + if ((mid - q).norm() > (radius + height*0.5)) + { + nodeId++; + continue; + } + + // Do the actual check + auto alpha = (q.dot(p) - pDotp0) / pDotp; + auto closestPoint = p0 + p*alpha; + + // If the point is inside the bounding sphere then the closest point + // should be inside the capsule + auto dist = (closestPoint - q).norm(); + if (dist <= radius) + { + auto direction = (closestPoint - q) / dist; + auto pointOnCapsule = closestPoint - radius*direction; + m_colData.MAColData.push_back({ nodeId, p - pointOnCapsule }); + } + nodeId++; + } +} + +} // imstk \ No newline at end of file diff --git a/Base/Collision/CollisionDetection/imstkMeshToCapsuleCD.h b/Base/Collision/CollisionDetection/imstkMeshToCapsuleCD.h new file mode 100644 index 0000000000000000000000000000000000000000..cbf7c7bfc9ce56586596f98ecd341a00e260496f --- /dev/null +++ b/Base/Collision/CollisionDetection/imstkMeshToCapsuleCD.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 imstkMeshToCapsuleCD_h +#define imstkMeshToCapsuleCD_h + +#include <memory> + +#include "imstkCollisionDetection.h" + +namespace imstk +{ + +class Mesh; +class Capsule; +class CollisionData; + +/// +/// \class MeshToCapsuleCD +/// +/// \brief Mesh to Capsule collision detection +/// +class MeshToCapsuleCD : public CollisionDetection +{ +public: + + /// + /// \brief Constructor + /// + MeshToCapsuleCD(std::shared_ptr<Mesh> mesh, + std::shared_ptr<Capsule> capsule, + CollisionData& colData) : + CollisionDetection(CollisionDetection::Type::MeshToCapsule, + colData), + m_mesh(mesh), + m_capsule(capsule){} + + /// + /// \brief Destructor + /// + ~MeshToCapsuleCD() = default; + + /// + /// \brief Detect collision and compute collision data + /// + void computeCollisionData() override; + +private: + + std::shared_ptr<Mesh> m_mesh; ///> Mesh + std::shared_ptr<Capsule> m_capsule; ///> Capsule +}; + +} + +#endif // ifndef imstkMeshToCapsuleCD_h \ No newline at end of file