ENH: Add SurfaceMesh to Capsule CD

Examples/PBD/PBDClothCollision/pbdClothCollisionExample.cpp

@@ -42,6 +42,7 @@
 #include "imstkSimulationManager.h"
 #include "imstkSphere.h"
 #include "imstkSurfaceMesh.h"
+#include "imstkSurfaceMeshToCapsuleCD.h"
 #include "imstkSurfaceMeshToSphereCD.h"
 #include "imstkVisualModel.h"
 #include "imstkVTKViewer.h"
@@ -346,6 +347,25 @@ main()
                 scene->buildTaskGraph();
                 scene->reset();
             }
+            // Switch to sphere vs surface and reset
+            else if (e->m_key == '6')
+            {
+                for (int i = 0; i < 4; i++)
+                {
+                    collisionObj->getVisualModel(i)->hide();
+                }
+                collisionObj->getVisualModel(0)->show();
+                collisionObj->setCollidingGeometry(capsule);
+
+                auto capsuleCD = std::make_shared<SurfaceMeshToCapsuleCD>();
+                capsuleCD->setInputGeometryA(clothObj->getCollidingGeometry());
+                capsuleCD->setInputGeometryB(capsule);
+                pbdInteraction->setCollisionDetection(capsuleCD);
+                pbdInteraction->getCollisionHandlingA()->setInputCollisionData(capsuleCD->getCollisionData());
+
+                scene->buildTaskGraph();
+                scene->reset();
+            }
             });
 
         driver->start();

Source/CollisionDetection/CollisionDetection/imstkCollisionUtils.cpp

@@ -171,6 +171,37 @@ pointTriangleClosestDistance(const Vec3d& point, const Vec3d& x1, const Vec3d& x
     }
 }
 
+Vec3d
+closestPointOnSegment(const Vec3d& point, const Vec3d& x1, const Vec3d& x2, int& caseType)
+{
+    Vec3d  dx = x2 - x1;
+    double m2 = dx.squaredNorm();
+    if (m2 < Real(1e-20))
+    {
+        caseType = 0;
+        return x1;
+    }
+
+    // find parameter value of closest point on segment
+    double s12 = dx.dot(x2 - point) / m2;
+
+    if (s12 < 0)
+    {
+        s12      = 0;
+        caseType = 1;
+    }
+    else if (s12 > 1.0)
+    {
+        s12      = 1.0;
+        caseType = 0;
+    }
+    else
+    {
+        caseType = 2;
+    }
+    return (s12 * x1 + (1.0 - s12) * x2).eval();
+}
+
 Vec3d
 closestPointOnTriangle(const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c, int& caseType)
 {

Source/CollisionDetection/CollisionDetection/imstkCollisionUtils.h

@@ -680,6 +680,14 @@ testSegmentTriangle(
     return (t > EPSILON && t + EPSILON < lAB);
 }
 
+///
+/// \brief Returns the closest position to point on segment a-b and the case
+/// type=0: x1 is the closest point
+/// type=1: x2 is the closest point
+/// type=2: closest point is on x1-x2
+///
+Vec3d closestPointOnSegment(const Vec3d& point, const Vec3d& x1, const Vec3d& x2, int& caseType);
+
 ///
 /// \brief Returns the position closest to triangle a-b-c and the case
 /// type=0: a is the closest point

Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToCapsuleCD.cpp

+/*=========================================================================
+
+   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 "imstkSurfaceMeshToCapsuleCD.h"
+#include "imstkCapsule.h"
+#include "imstkCollisionData.h"
+#include "imstkCollisionUtils.h"
+#include "imstkSurfaceMesh.h"
+
+#include <stdio.h>
+
+namespace imstk
+{
+SurfaceMeshToCapsuleCD::SurfaceMeshToCapsuleCD()
+{
+    setRequiredInputType<SurfaceMesh>(0);
+    setRequiredInputType<Capsule>(1);
+}
+
+void
+SurfaceMeshToCapsuleCD::computeCollisionDataAB(
+    std::shared_ptr<Geometry>          geomA,
+    std::shared_ptr<Geometry>          geomB,
+    CDElementVector<CollisionElement>& elementsA,
+    CDElementVector<CollisionElement>& elementsB)
+{
+    std::shared_ptr<SurfaceMesh> surfMesh = std::dynamic_pointer_cast<SurfaceMesh>(geomA);
+    std::shared_ptr<Capsule>     capsule  = std::dynamic_pointer_cast<Capsule>(geomB);
+
+    const Vec3d& capsulePos         = capsule->getPosition();
+    const double capsuleRadius      = capsule->getRadius();
+    const double capsuleLength      = capsule->getLength();
+    const Quatd& capsuleOrientation = capsule->getOrientation();
+    const Vec3d& capsulePosA        = capsulePos - 0.5 * capsuleLength * capsuleOrientation.toRotationMatrix().transpose().col(1);
+    const Vec3d& capsulePosB        = capsulePos + (capsulePos - capsulePosA);
+
+    std::shared_ptr<VecDataArray<int, 3>>    indicesPtr  = surfMesh->getTriangleIndices();
+    const VecDataArray<int, 3>&              indices     = *indicesPtr;
+    std::shared_ptr<VecDataArray<double, 3>> verticesPtr = surfMesh->getVertexPositions();
+    const VecDataArray<double, 3>&           vertices    = *verticesPtr;
+
+    // \todo: Doesn't remove duplicate contacts (shared edges), refer to SurfaceMeshCD for easy method to do so
+    ParallelUtils::parallelFor(indices.size(),
+        [&](const int i)
+        {
+            const Vec3i& cell = indices[i];
+            const Vec3d& x1   = vertices[cell[0]];
+            const Vec3d& x2   = vertices[cell[1]];
+            const Vec3d& x3   = vertices[cell[2]];
+
+            // Choose the closest point on capsule axis to create a virtual sphere for CD
+            int unusedCaseType = 0;
+            const Vec3d trianglePointA = CollisionUtils::closestPointOnTriangle(capsulePosA, x1, x2, x3, unusedCaseType);
+            const Vec3d trianglePointB = CollisionUtils::closestPointOnTriangle(capsulePosB, x1, x2, x3, unusedCaseType);
+            const auto segmentPointA   = CollisionUtils::closestPointOnSegment(trianglePointA, capsulePosA, capsulePosB, unusedCaseType);
+            const auto segmentPointB   = CollisionUtils::closestPointOnSegment(trianglePointB, capsulePosA, capsulePosB, unusedCaseType);
+            const auto distanceA       = (segmentPointA - trianglePointA).norm();
+            const auto distanceB       = (segmentPointB - trianglePointB).norm();
+
+            const double sphereRadius = capsuleRadius;
+            Vec3d spherePos(0, 0, 0);
+            if (distanceA < distanceB)
+            {
+                spherePos = segmentPointA;
+            }
+            else if (distanceA > distanceB)
+            {
+                spherePos = segmentPointB;
+            }
+            else // parallel
+            {
+                spherePos = (segmentPointA + segmentPointB) / 2;
+            }
+
+            // This approach does a built in sphere sweep
+            // \todo: Spatial accelerators need to be abstracted
+            const Vec3d centroid = (x1 + x2 + x3) / 3.0;
+
+            // Find the maximal point from centroid for radius
+            const double rSqr1 = (centroid - x1).squaredNorm();
+            const double rSqr2 = (centroid - x2).squaredNorm();
+            const double rSqr3 = (centroid - x3).squaredNorm();
+            const double triangleBoundingRadius = std::sqrt(std::max(rSqr1, std::max(rSqr2, rSqr3)));
+
+            const double distSqr = (centroid - spherePos).squaredNorm();
+            const double rSum    = triangleBoundingRadius + sphereRadius;
+            if (distSqr < rSum * rSum)
+            {
+                Vec3d triangleContactPt;
+                Vec2i edgeContact;
+                int pointContact;
+                int caseType = CollisionUtils::testSphereToTriangle(
+                    spherePos, sphereRadius,
+                    cell, x1, x2, x3,
+                    triangleContactPt,
+                    edgeContact, pointContact);
+                if (caseType == 1) // Edge vs point on sphere
+                {
+                                   // Edge contact
+                    CellIndexElement elemA;
+                    elemA.ids[0]   = edgeContact[0];
+                    elemA.ids[1]   = edgeContact[1];
+                    elemA.idCount  = 2;
+                    elemA.cellType = IMSTK_EDGE;
+
+                    Vec3d contactNormal = (spherePos - triangleContactPt);
+                    const double dist   = contactNormal.norm();
+                    const double penetrationDepth = sphereRadius - dist;
+                    contactNormal /= dist;
+
+                    PointDirectionElement elemB;
+                    elemB.dir = contactNormal;                            // Direction to resolve sphere
+                    elemB.pt  = spherePos - sphereRadius * contactNormal; // Contact point on sphere
+                    elemB.penetrationDepth = penetrationDepth;
+
+                    elementsA.safeAppend(elemA);
+                    elementsB.safeAppend(elemB);
+                }
+                else if (caseType == 2) // Triangle vs point on sphere
+                {
+                                        // Face contact
+                    CellIndexElement elemA;
+                    elemA.ids[0]   = cell[0];
+                    elemA.ids[1]   = cell[1];
+                    elemA.ids[2]   = cell[2];
+                    elemA.idCount  = 3;
+                    elemA.cellType = IMSTK_TRIANGLE;
+
+                    Vec3d contactNormal = (spherePos - triangleContactPt);
+                    const double dist   = contactNormal.norm();
+                    const double penetrationDepth = sphereRadius - dist;
+                    contactNormal /= dist;
+
+                    PointDirectionElement elemB;
+                    elemB.dir = contactNormal;                            // Direction to resolve sphere
+                    elemB.pt  = spherePos - sphereRadius * contactNormal; // Contact point on sphere
+                    elemB.penetrationDepth = penetrationDepth;
+
+                    elementsA.safeAppend(elemA);
+                    elementsB.safeAppend(elemB);
+                }
+                else if (caseType == 3)
+                {
+                    Vec3d contactNormal = (spherePos - triangleContactPt);
+                    const double dist   = contactNormal.norm();
+                    const double penetrationDepth = sphereRadius - dist;
+                    contactNormal /= dist;
+
+                    // Point contact
+                    PointIndexDirectionElement elemA;
+                    elemA.ptIndex = pointContact;
+                    elemA.dir     = -contactNormal; // Direction to resolve point
+                    elemA.penetrationDepth = penetrationDepth;
+
+                    PointDirectionElement elemB;
+                    elemB.pt  = triangleContactPt; // Point on sphere
+                    elemB.dir = contactNormal;     // Direction to resolve point
+                    elemB.penetrationDepth = penetrationDepth;
+
+                    elementsA.safeAppend(elemA);
+                    elementsB.safeAppend(elemB);
+                }
+            }
+        }, vertices.size() > 1000);
+}
+}
\ No newline at end of file

Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToCapsuleCD.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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkCollisionDetectionAlgorithm.h"
+
+namespace imstk
+{
+///
+/// \class SurfaceMeshToCapsuleCD
+///
+/// \brief SurfaceMesh to Capsule collision detection
+/// Generates vertex-triangle, point-edge, and point-point CD data
+/// By default only generates contact data for the pointset.
+///
+class SurfaceMeshToCapsuleCD : public CollisionDetectionAlgorithm
+{
+public:
+    SurfaceMeshToCapsuleCD();
+    virtual ~SurfaceMeshToCapsuleCD() override = default;
+
+    ///
+    /// \brief Returns collision detection type string name
+    ///
+    virtual const std::string getTypeName() const override { return "SurfaceMeshToCapsuleCD"; }
+
+protected:
+    ///
+    /// \brief Compute collision data for AB simulatenously
+    ///
+    virtual void computeCollisionDataAB(
+        std::shared_ptr<Geometry>          geomA,
+        std::shared_ptr<Geometry>          geomB,
+        CDElementVector<CollisionElement>& elementsA,
+        CDElementVector<CollisionElement>& elementsB) override;
+};
+}

Source/CollisionDetection/imstkCDObjectFactory.cpp

@@ -37,6 +37,7 @@ limitations under the License.
 #include "imstkSphereToCylinderCD.h"
 #include "imstkSphereToSphereCD.h"
 #include "imstkSurfaceMesh.h"
+#include "imstkSurfaceMeshToCapsuleCD.h"
 #include "imstkSurfaceMeshToSphereCD.h"
 #include "imstkSurfaceMeshToSurfaceMeshCCD.h"
 #include "imstkSurfaceMeshToSurfaceMeshCD.h"
@@ -74,6 +75,7 @@ std::unordered_map<std::string, std::function<std::shared_ptr<CollisionDetection
     IMSTK_MAKE_CD_CASE(SphereToSphereCD, Sphere, Sphere),
     //IMSTK_MAKE_CD_CASE(SurfaceMeshToSurfaceMeshCCD, SurfaceMesh, SurfaceMesh),
     IMSTK_MAKE_CD_CASE(SurfaceMeshToSurfaceMeshCD, SurfaceMesh, SurfaceMesh),
+    IMSTK_MAKE_CD_CASE(SurfaceMeshToCapsuleCD, SurfaceMesh, Capsule),
     IMSTK_MAKE_CD_CASE(SurfaceMeshToSphereCD, SurfaceMesh, Sphere),
     IMSTK_MAKE_CD_CASE(TetraToPointSetCD, TetrahedralMesh, PointSet),
     IMSTK_MAKE_CD_CASE(UnidirectionalPlaneToSphereCD, Plane, Sphere)