Commit 245398ea authored by Andrew Wilson's avatar Andrew Wilson Committed by Andrew Wilson
Browse files

BUG: Tuned PBDPickingExample to use SurfaceMeshToCapsule, switch to for...

BUG: Tuned PBDPickingExample to use SurfaceMeshToCapsule, switch to for instead of parallelFor for CD
parent 42a321fb
......@@ -117,13 +117,14 @@ makeClothObj(const std::string& name,
// Setup the Parameters
imstkNew<PBDModelConfig> pbdParams;
pbdParams->enableConstraint(PbdConstraint::Type::Distance, 1000.0);
pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 10.0);
pbdParams->enableConstraint(PbdConstraint::Type::Distance, 4000.0);
pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 100.0);
pbdParams->m_fixedNodeIds = { 0, static_cast<size_t>(nCols) - 1 };
pbdParams->m_uniformMassValue = width * height / ((double)nRows * (double)nCols);
pbdParams->m_gravity = Vec3d(0.0, -20.0, 0.0);
pbdParams->m_gravity = Vec3d(0.0, -140.0, 0.0);
pbdParams->m_dt = 0.01;
pbdParams->m_iterations = 4;
pbdParams->m_iterations = 5;
pbdParams->m_viscousDampingCoeff = 0.01;
// Setup the Model
imstkNew<PbdModel> pbdModel;
......@@ -202,7 +203,7 @@ main()
objLowerJaw->setCollidingGeometry(geomLowerJaw);
scene->addSceneObject(objLowerJaw);
std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", 50.0, 50.0, 31, 31);
std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", 50.0, 50.0, 15, 15);
scene->addSceneObject(clothObj);
// Create and add virtual coupling object controller in the scene
......@@ -211,8 +212,8 @@ main()
scene->addController(controller);
// Add collision for both jaws of the tool
auto upperJawCollision = std::make_shared<PbdObjectCollision>(clothObj, objUpperJaw, "PointSetToCapsuleCD");
auto lowerJawCollision = std::make_shared<PbdObjectCollision>(clothObj, objLowerJaw, "PointSetToCapsuleCD");
auto upperJawCollision = std::make_shared<PbdObjectCollision>(clothObj, objUpperJaw, "SurfaceMeshToCapsuleCD");
auto lowerJawCollision = std::make_shared<PbdObjectCollision>(clothObj, objLowerJaw, "SurfaceMeshToCapsuleCD");
scene->getCollisionGraph()->addInteraction(upperJawCollision);
scene->getCollisionGraph()->addInteraction(lowerJawCollision);
......@@ -248,7 +249,7 @@ main()
driver->addModule(server);
driver->addModule(viewer);
driver->addModule(sceneManager);
driver->setDesiredDt(0.008);
driver->setDesiredDt(0.005);
// Add mouse and keyboard controls to the viewer
{
......
......@@ -21,12 +21,9 @@
#include "imstkSurfaceMeshToCapsuleCD.h"
#include "imstkCapsule.h"
#include "imstkCollisionData.h"
#include "imstkCollisionUtils.h"
#include "imstkSurfaceMesh.h"
#include <stdio.h>
namespace imstk
{
SurfaceMeshToCapsuleCD::SurfaceMeshToCapsuleCD()
......@@ -58,127 +55,126 @@ SurfaceMeshToCapsuleCD::computeCollisionDataAB(
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)
for (int i = 0; i < indices.size(); 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)
{
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 = 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
{
spherePos = segmentPointB;
// 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.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
}
else // parallel
else if (caseType == 2) // Triangle vs point on sphere
{
spherePos = (segmentPointA + segmentPointB) / 2;
// 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.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
}
// 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)
else if (caseType == 3)
{
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);
}
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.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
}
}, vertices.size() > 1000);
}
}
}
}
\ No newline at end of file
......@@ -30,7 +30,6 @@ namespace imstk
///
/// \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
{
......
......@@ -52,103 +52,102 @@ SurfaceMeshToSphereCD::computeCollisionDataAB(
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)
for (int i = 0; i < indices.size(); i++)
{
const Vec3i& cell = indices[i];
const Vec3d& x1 = vertices[cell[0]];
const Vec3d& x2 = vertices[cell[1]];
const Vec3d& x3 = vertices[cell[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)
{
const Vec3i& cell = indices[i];
const Vec3d& x1 = vertices[cell[0]];
const Vec3d& x2 = vertices[cell[1]];
const Vec3d& x3 = vertices[cell[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
{
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);
}
// 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.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
}
}, vertices.size() > 100);
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.unsafeAppend(elemA);
elementsB.unsafeAppend(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.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
}
}
}
}
}
\ No newline at end of file
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