An update will be applied January 25th, between 12PM and 1:00PM EST (UTC -5:00). The site may be slow during that time.

Commit 4ece7524 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

Merge branch 'ParallelCollisionDetectionAndHandling' into 'master'

Parallel collision detection and handling

See merge request !323
parents 9f4fd3be daa8aab7
Pipeline #139760 passed with stage
in 0 seconds
......@@ -4,7 +4,7 @@ project(iMSTK VERSION 0.0.1 LANGUAGES C CXX)
if(UNIX AND NOT APPLE)
set(LINUX TRUE)
endif()
#-----------------------------------------------------------------------------
# CTest/Dashboards
#-----------------------------------------------------------------------------
......@@ -18,7 +18,7 @@ set(CMAKE_MODULE_PATH
${CMAKE_CURRENT_SOURCE_DIR}/CMake/Utilities
${CMAKE_MODULE_PATH}
)
set(${PROJECT_NAME}_CMAKE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/CMake)
set(${PROJECT_NAME}_CMAKE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/CMake)
#-----------------------------------------------------------------------------
# Set a default build type if none was specified
......@@ -106,7 +106,7 @@ if(${PROJECT_NAME}_SUPERBUILD)
imstk_define_dependency(glm)
if(${PROJECT_NAME}_USE_Vulkan)
imstk_define_dependency(glfw)
imstk_define_dependency(glfw)
imstk_define_dependency(gli)
endif()
......@@ -135,10 +135,10 @@ if(${PROJECT_NAME}_SUPERBUILD)
if(APPLE OR LINUX)
message("Warning: Building iMSTK WITHOUT audio support!")
set(${PROJECT_NAME}_AUDIO_ENABLED OFF)
set(${PROJECT_NAME}_AUDIO_ENABLED OFF)
else()
set(${PROJECT_NAME}_AUDIO_ENABLED "Windows OS detected: Building iMSTK with AUDIO support." ON)
imstk_define_dependency(SFML)
set(${PROJECT_NAME}_AUDIO_ENABLED "Windows OS detected: Building iMSTK with AUDIO support." ON)
imstk_define_dependency(SFML)
endif()
option(${PROJECT_NAME}_USE_ODE "Build iMSTK with ODE support." OFF)
......@@ -190,9 +190,9 @@ endif()
find_program(Uncrustify_EXECUTABLE uncrustify)
include(SetupUncrustifyConfig)
if(Uncrustify_EXECUTABLE)
include(imstkAddUncrustifyCustomTarget)
include(imstkAddUncrustifyCustomTarget)
else(Uncrustify_EXECUTABLE)
message(WARNING "uncrustify not found! Cannot run code-style test.")
message(WARNING "uncrustify not found! Cannot run code-style test.")
endif(Uncrustify_EXECUTABLE)
# Assimp
......@@ -292,6 +292,8 @@ if(BUILD_TESTING)
endif()
# TBB
# Define __TBB_NO_IMPLICIT_LINKAGE so that MSVC will not always look for tbb_debug in debug mode
add_definitions(-D__TBB_NO_IMPLICIT_LINKAGE)
find_package(tbb REQUIRED)
add_definitions(${TBB_DEFINITIONS})
include_directories( ${TBB_INCLUDE_DIRS} )
......
......@@ -89,7 +89,7 @@ public:
///
/// \brief Destructor
///
~CollisionDetection() = default;
virtual ~CollisionDetection() = default;
///
/// \brief Detect collision and compute collision data (pure virtual)
......
......@@ -25,11 +25,19 @@
#include "imstkCapsule.h"
#include "imstkPointSet.h"
#include "imstkMath.h"
#include <g3log/g3log.hpp>
#include "imstkParallelUtils.h"
namespace imstk
{
PointSetToCapsuleCD::PointSetToCapsuleCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Capsule> capsule,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::PointSetToCapsule, colData),
m_pointSet(pointSet),
m_capsule(capsule)
{
}
void
PointSetToCapsuleCD::computeCollisionData()
{
......@@ -49,30 +57,33 @@ PointSetToCapsuleCD::computeCollisionData()
auto pDotp = p.dot(p);
auto pDotp0 = p.dot(p0);
size_t nodeId = 0;
for (const auto& q : m_pointSet->getVertexPositions())
{
// First, check collision with bounding sphere
if ((mid - q).norm() > (radius + length * 0.5))
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_pointSet->getVertexPositions().size(),
[&](const size_t idx)
{
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++;
}
const auto q = m_pointSet->getVertexPosition(idx);
// First, check collision with bounding sphere
if ((mid - q).norm() > (radius + length * 0.5))
{
return;
}
// 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;
lock.lock();
m_colData->MAColData.push_back({ idx, p - pointOnCapsule });
lock.unlock();
}
});
}
} // imstk
......@@ -46,16 +46,12 @@ public:
///
PointSetToCapsuleCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Capsule> capsule,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::PointSetToCapsule,
colData),
m_pointSet(pointSet),
m_capsule(capsule){}
std::shared_ptr<CollisionData> colData);
///
/// \brief Destructor
///
~PointSetToCapsuleCD() = default;
virtual ~PointSetToCapsuleCD() override = default;
///
/// \brief Detect collision and compute collision data
......@@ -69,4 +65,4 @@ private:
};
}
#endif // ifndef imstkPointSetToCapsuleCD_h
\ No newline at end of file
#endif // ifndef imstkPointSetToCapsuleCD_h
......@@ -25,32 +25,42 @@
#include "imstkCollisionData.h"
#include "imstkPlane.h"
#include "imstkPointSet.h"
#include <g3log/g3log.hpp>
#include "imstkParallelUtils.h"
namespace imstk
{
void
PointSetToPlaneCD::computeCollisionData()
PointSetToPlaneCD::PointSetToPlaneCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Plane> plane,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSphere, colData),
m_pointSet(pointSet),
m_plane(plane)
{
}
void PointSetToPlaneCD::computeCollisionData()
{
// Clear collisionData
m_colData->clearAll();
// Get plane properties
auto planePos = m_plane->getPosition();
// TODO: Fix this issue of extra computation in future
auto planeNormal = m_plane->getNormal();
size_t nodeId = 0;
for (const auto& p : m_pointSet->getVertexPositions())
{
auto peneDistance = (p - planePos).dot(planeNormal);
if (peneDistance <= 0.0)
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_pointSet->getVertexPositions().size(),
[&](const size_t idx)
{
m_colData->MAColData.push_back({ nodeId, planeNormal * peneDistance });
}
nodeId++;
}
const auto p = m_pointSet->getVertexPosition(idx);
const auto penetrationDist = (p - planePos).dot(planeNormal);
if (penetrationDist <= 0.0)
{
const auto penetrationDir = planeNormal * penetrationDist;
lock.lock();
m_colData->MAColData.push_back({ idx, penetrationDir });
lock.unlock();
}
});
}
} // imstk
......@@ -46,16 +46,12 @@ public:
///
PointSetToPlaneCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Plane> plane,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSphere,
colData),
m_pointSet(pointSet),
m_plane(plane){}
std::shared_ptr<CollisionData> colData);
///
/// \brief Destructor
///
~PointSetToPlaneCD() = default;
virtual ~PointSetToPlaneCD() override = default;
///
/// \brief Detect collision and compute collision data
......
......@@ -25,11 +25,19 @@
#include "imstkCollisionData.h"
#include "imstkSphere.h"
#include "imstkPointSet.h"
#include <g3log/g3log.hpp>
#include "imstkParallelUtils.h"
namespace imstk
{
PointSetToSphereCD::PointSetToSphereCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Sphere> sphere,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSphere, colData),
m_pointSet(pointSet),
m_sphere(sphere)
{
}
void
PointSetToSphereCD::computeCollisionData()
{
......@@ -37,20 +45,27 @@ PointSetToSphereCD::computeCollisionData()
m_colData->clearAll();
// Get sphere properties
auto spherePos = m_sphere->getPosition();
auto radius = m_sphere->getRadius();
size_t nodeId = 0;
for (const auto& p : m_pointSet->getVertexPositions())
{
auto dist = (spherePos - p).norm();
if (dist <= radius)
const auto sphereCenter = m_sphere->getPosition();
const auto sphereRadius = m_sphere->getRadius();
const auto sphereRadiusSqr = sphereRadius * sphereRadius;
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_pointSet->getVertexPositions().size(),
[&](const size_t idx)
{
auto direction = (spherePos - p) / dist;
auto pointOnSphere = spherePos - radius * direction;
m_colData->MAColData.push_back({ nodeId, p - pointOnSphere });
}
nodeId++;
}
const auto p = m_pointSet->getVertexPosition(idx);
const auto pc = sphereCenter - p;
const auto distSqr = pc.squaredNorm();
if (distSqr <= sphereRadiusSqr && distSqr > Real(1e-12))
{
const auto direction = pc / std::sqrt(distSqr);
const auto pointOnSphere = sphereCenter - sphereRadius * direction;
const auto penetrationDir = p - pointOnSphere;
lock.lock();
m_colData->MAColData.push_back({ idx, penetrationDir });
lock.unlock();
}
});
}
} // imstk
......@@ -46,16 +46,12 @@ public:
///
PointSetToSphereCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Sphere> sphere,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSphere,
colData),
m_pointSet(pointSet),
m_sphere(sphere){}
std::shared_ptr<CollisionData> colData);
///
/// \brief Destructor
///
~PointSetToSphereCD() = default;
virtual ~PointSetToSphereCD() override = default;
///
/// \brief Detect collision and compute collision data
......
......@@ -23,9 +23,19 @@ limitations under the License.
#include "imstkCollisionData.h"
#include "imstkPointSet.h"
#include "imstkSphere.h"
#include "imstkParallelUtils.h"
namespace imstk
{
PointSetToSpherePickingCD::PointSetToSpherePickingCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Sphere> sphere,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSphere, colData),
m_pointSet(pointSet),
m_sphere(sphere)
{
}
void
PointSetToSpherePickingCD::computeCollisionData()
{
......@@ -41,15 +51,24 @@ PointSetToSpherePickingCD::computeCollisionData()
auto spherePos = m_sphere->getPosition();
auto radius = m_sphere->getRadius() * m_sphere->getScaling();
size_t nodeId = 0;
for (const auto& p : m_pointSet->getVertexPositions())
{
auto dist = (spherePos - p).norm();
if (dist <= radius)
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_pointSet->getVertexPositions().size(),
[&](const size_t idx)
{
m_colData->NodePickData.push_back({ spherePos - p, nodeId, 0 });
}
nodeId++;
}
const auto p = m_pointSet->getVertexPosition(idx);
auto dist = (spherePos - p).norm();
if (dist <= radius)
{
lock.lock();
m_colData->NodePickData.push_back({ spherePos - p, idx, 0 });
lock.unlock();
}
});
}
void PointSetToSpherePickingCD::setDeviceTrackerAndButton(const std::shared_ptr<DeviceTracker> devTracker,
const unsigned int )
{
m_deviceTracker = devTracker;
}
}
\ No newline at end of file
} // imstk
......@@ -47,16 +47,12 @@ public:
///
PointSetToSpherePickingCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Sphere> sphere,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSphere,
colData),
m_pointSet(pointSet),
m_sphere(sphere){}
std::shared_ptr<CollisionData> colData);
///
/// \brief Destructor
///
~PointSetToSpherePickingCD() = default;
virtual ~PointSetToSpherePickingCD() override = default;
///
/// \brief Detect collision and compute collision data
......@@ -67,7 +63,7 @@ public:
/// \brief Set device tracker and the id of the button
///
void setDeviceTrackerAndButton(const std::shared_ptr<imstk::DeviceTracker> devTracker,
const unsigned int buttonId = 0){ m_deviceTracker = devTracker; }
const unsigned int buttonId = 0);
private:
std::shared_ptr<PointSet> m_pointSet; ///> PointSet
......
......@@ -23,8 +23,7 @@
#include "imstkCollisionData.h"
#include "imstkMath.h"
#include <g3log/g3log.hpp>
#include "imstkParallelUtils.h"
namespace imstk
{
......@@ -40,8 +39,6 @@ TetraToTetraCD::TetraToTetraCD(std::shared_ptr<TetrahedralMesh> meshA,
void
TetraToTetraCD::findCollisionsForMeshWithinHashTable(const std::shared_ptr<TetrahedralMesh> mesh, size_t idOffset)
{
Vec3d min, max; //bounding box of a tetrahedron
Vec3d vPos;
const auto eps = MACHINE_PRECISION;
const double eps2 = 1e-10;
......@@ -51,57 +48,63 @@ TetraToTetraCD::findCollisionsForMeshWithinHashTable(const std::shared_ptr<Tetra
auto nodesMeshA = m_meshA->getVertexPositions();
auto nodesMeshB = m_meshB->getVertexPositions();
for (size_t tId = 0; tId < mesh->getNumTetrahedra(); ++tId) //TODO: parallelize!
{
TetrahedralMesh::TetraArray vInd = mesh->getTetrahedronVertices(tId);
for (int i = 0; i < 4; i++) //if idOffset!=0 ?
{
vInd[i] += idOffset;
}
mesh->computeTetrahedronBoundingBox(tId, min, max);
std::vector<size_t> collP = m_hashTable.getPointsInAABB(min, max);
assert(collP.size() >= 4);
if (collP.size() > 4)
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(mesh->getNumTetrahedra(),
[&](const size_t tId)
{
for (size_t vId : collP)
TetrahedralMesh::TetraArray vInd = mesh->getTetrahedronVertices(tId);
for (size_t i = 0; i < 4; ++i) //if idOffset!=0 ?
{
//vertex does not belong to this tetrahedron
if (vId != vInd[0] &&
vId != vInd[1] &&
vId != vInd[2] &&
vId != vInd[3])
vInd[i] += idOffset;
}
Vec3d min, max; //bounding box of a tetrahedron
mesh->computeTetrahedronBoundingBox(tId, min, max);
std::vector<size_t> collP = m_hashTable.getPointsInAABB(min, max);
assert(collP.size() >= 4);
if (collP.size() > 4)
{
for (size_t vId : collP)
{
//this determines vertex belonging part of the penetration type
//and gets vertex position
if (vId < m_meshA->getNumVertices())
{
vPos = nodesMeshA[vId];
cType = static_cast<PointTetrahedronCollisionData::CollisionType>((cType & 1) + 0);
}
else
{
vId -= m_meshA->getNumVertices();
vPos = nodesMeshB[vId];
cType = static_cast<PointTetrahedronCollisionData::CollisionType>((cType & 1) + 2);
}
TetrahedralMesh::WeightsArray bCoord; //barycentric coordinates of the vertex in tetrahedron
mesh->computeBarycentricWeights(tId, vPos, bCoord);
if (bCoord[0] >= -eps &&
bCoord[1] >= -eps &&
bCoord[2] >= -eps &&
bCoord[3] >= -eps)
//vertex does not belong to this tetrahedron
if (vId != vInd[0] &&
vId != vInd[1] &&
vId != vInd[2] &&
vId != vInd[3])
{
auto coordSum = bCoord[0] + bCoord[1] + bCoord[2] + bCoord[3];
assert(coordSum <= 1 + eps2 && coordSum >= 1 - eps2);
PointTetrahedronCollisionData ptColl = { cType, vId, tId, bCoord };
m_colData->PTColData.push_back(ptColl);
}
} //if not this tetrahedron
} //for vertices
}
} //for tetrahedra
//this determines vertex belonging part of the penetration type and gets vertex position
Vec3d vPos;
if (vId < m_meshA->getNumVertices())
{
vPos = nodesMeshA[vId];
cType = static_cast<PointTetrahedronCollisionData::CollisionType>((cType & 1) + 0);
}
else
{
vId -= m_meshA->getNumVertices();
vPos = nodesMeshB[vId];
cType = static_cast<PointTetrahedronCollisionData::CollisionType>((cType & 1) + 2);
}
TetrahedralMesh::WeightsArray bCoord; //barycentric coordinates of the vertex in tetrahedron
mesh->computeBarycentricWeights(tId, vPos, bCoord);
if (bCoord[0] >= -eps &&
bCoord[1] >= -eps &&
bCoord[2] >= -eps &&
bCoord[3] >= -eps)
{
auto coordSum = bCoord[0] + bCoord[1] + bCoord[2] + bCoord[3];
assert(coordSum <= 1.0 + eps2 && coordSum >= 1.0 - eps2);
lock.lock();
m_colData->PTColData.push_back({ cType, vId, tId, bCoord });
lock.unlock();
}
} //if not this tetrahedron
} //for vertices
}
}); //for tetrahedra
}
void
......
......@@ -54,7 +54,7 @@ public:
///
/// \brief Destructor
///
~TetraToTetraCD() = default;
virtual ~TetraToTetraCD() override = default;
///
/// \brief Detect collision and compute collision data
......
......@@ -26,6 +26,7 @@
#include "imstkCollisionData.h"
#include "imstkDeviceTracker.h"
#include "imstkMath.h"
#include "imstkParallelUtils.h"
#include <g3log/g3log.hpp>
......@@ -47,29 +48,31 @@ BoneDrillingCH::BoneDrillingCH(const Side& side,
}
// Initialize bone density values
m_nodalDensity.reserve(boneMesh->getNumVertices());
for (int i = 0; i < boneMesh->getNumVertices(); ++i)
{
m_nodalDensity.push_back(m_initialBoneDensity);
}
m_nodeRemovalStatus.reserve(boneMesh->getNumVertices());
for (int i = 0; i < boneMesh->getNumVertices(); ++i)
{
m_nodeRemovalStatus.push_back(false);
}
m_nodalCardinalSet.resize(boneMesh->getNumVertices());
m_nodalCardinalSet.reserve(boneMesh->getNumVertices());
for (int i = 0; i < boneMesh->getNumVertices(); ++i)
{
std::vector<int> row;
std::vector<size_t> row;