Commit 42a321fb authored by Ye Han's avatar Ye Han 🐼 Committed by Andrew Wilson
Browse files

ENH: Add SurfaceMesh to Capsule CD

parent 91d09f63
......@@ -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();
......
......@@ -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)
{
......
......@@ -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
......
/*=========================================================================
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
/*=========================================================================
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;
};
}
......@@ -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)
......
Markdown is supported
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