Commit 696b072f authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

ENH: Add Brute for mesh-to-mesh collision detection

Refactored from PBD pipeline
parent fb2a5ac3
......@@ -83,7 +83,7 @@ struct TriangleVertexCollisionData
size_t vertexIdB;
float time;
TriangleVertexCollisionData(size_t fIdA, size_t vIdB, float t = -1)
TriangleVertexCollisionData(const size_t fIdA, const size_t vIdB, const float t = -1)
{
triIdA = fIdA;
vertexIdB = vIdB;
......@@ -102,7 +102,7 @@ struct EdgeEdgeCollisionData
std::pair<size_t, size_t> edgeIdB;
float time;
EdgeEdgeCollisionData(size_t eA_v1, size_t eA_v2, size_t eB_v1, size_t eB_v2, float t = -1)
EdgeEdgeCollisionData(const size_t eA_v1, const size_t eA_v2, const size_t eB_v1, const size_t eB_v2, const float t = -1)
{
edgeIdA = std::pair<size_t, size_t>(eA_v1, eA_v2);
edgeIdB = std::pair<size_t, size_t>(eB_v1, eB_v2);
......@@ -138,6 +138,26 @@ struct PickingCollisionData
bool touchStatus;
};
///
/// \struct PointTetrahedronCollisionData
///
/// \brief Point-tetrahedron collision data
///
struct
{
enum CollisionType
{
aPenetratingA = 0, // A self-penetration
aPenetratingB = 1, // vertex is from mesh A, tetrahedron is from mesh B
bPenetratingA = 2, // vertex is from mesh B, tetrahedron is from mesh A
bPenetratingB = 3 // B self-penetration
} collisionType;
size_t vertexId;
size_t tetreahedronId;
using WeightsArray = std::array<double, 4>;
WeightsArray BarycentricCoordinates;
};
///
/// \struct CollisionData
///
......
......@@ -40,10 +40,10 @@
namespace imstk
{
std::shared_ptr<CollisionDetection>
CollisionDetection::make_collision_detection(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData &colData)
CollisionDetection::makeCollisionDetectionObject(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData &colData)
{
switch (type)
{
......
......@@ -51,16 +51,18 @@ public:
PointSetToPlane,
MeshToMesh,
PointSetToCapsule,
PointSetToSpherePicking
PointSetToSpherePicking,
MeshToMeshBruteForce,
Custom
};
///
/// \brief Static factory for collision detection sub classes
///
static std::shared_ptr<CollisionDetection> make_collision_detection(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colData);
static std::shared_ptr<CollisionDetection> makeCollisionDetectionObject(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colData);
///
/// \brief Constructor
......@@ -93,8 +95,8 @@ public:
protected:
Type m_type; ///< Collision detection algorithm type
CollisionData& m_colData; ///< Collision data
Type m_type = Type::Custom; ///< Collision detection algorithm type
CollisionData& m_colData; ///< Collision data
};
}
......
/*=========================================================================
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 "imstkMeshToMeshBruteforceCD.h"
#include "imstkCollidingObject.h"
#include "imstkCollisionData.h"
#include "imstkSurfaceMesh.h"
#include "imstkPointSet.h"
#include "imstkLineMesh.h"
#include "imstkGeometry.h"
#include "imstkIntersectionTestUtils.h"
#include <g3log/g3log.hpp>
namespace imstk
{
void
MeshToMeshBruteForceCD::computeCollisionData()
{
// Clear collisionData
m_colData.clearAll();
// Broad phase collision
doBroadPhaseCollisionCheck();
// Narrow phase collision
const auto mesh2 = std::static_pointer_cast<SurfaceMesh>(m_object2);
if (m_object1->getType() == Geometry::Type::LineMesh)
{
auto mesh1 = std::static_pointer_cast<LineMesh>(m_object1);
// brute force, use BVH or spatial grid would be much better
// point
for (size_t i = 0; i < mesh1->getNumVertices(); ++i)
{
const Vec3d p = mesh1->getVertexPosition(i);
const auto elements = mesh2->getTrianglesVertices();
for (size_t j = 0; j < elements.size(); ++j)
{
const SurfaceMesh::TriangleArray& e = elements[j];
const Vec3d p0 = mesh2->getVertexPosition(e[0]);
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (testPointToTriAABB(p[0], p[1], p[2],
p0[0], p0[1], p0[2],
p1[0], p1[1], p1[2],
p2[0], p2[1], p2[2],
m_proximityTolerance,
m_proximityTolerance))
{
TriangleVertexCollisionData d(j, i);
m_colData.TVColData.push_back(d);
}
}
}
const auto numLines = mesh1->getNumLines();
const auto numVertices = mesh2->getNumVertices();
std::vector<std::vector<bool>> E2(numVertices, std::vector<bool>(numVertices, 1));
const auto triangleVertices = mesh2->getTrianglesVertices();
for (int k = 0; k < numLines; ++k)
{
const auto nodes = mesh1->getLinesVertices()[k];
const size_t i1 = nodes[0];
const size_t i2 = nodes[1];
const Vec3d P = mesh1->getVertexPosition(i1);
const Vec3d Q = mesh1->getVertexPosition(i2);
for (size_t j = 0; j < triangleVertices.size(); ++j)
{
const SurfaceMesh::TriangleArray& e = triangleVertices[j];
const Vec3d p0 = mesh2->getVertexPosition(e[0]);
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p0[0], p0[1], p0[2],
p1[0], p1[1], p1[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[0], e[1]);
m_colData.EEColData.push_back(d);
}
E2[e[0]][e[1]] = 0;
}
if (E2[e[1]][e[2]] && E2[e[2]][e[1]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p1[0], p1[1], p1[2],
p2[0], p2[1], p2[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[1], e[2]);
m_colData.EEColData.push_back(d);
}
E2[e[1]][e[2]] = 0;
}
if (E2[e[2]][e[0]] && E2[e[0]][e[2]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p2[0], p2[1], p2[2],
p0[0], p0[1], p0[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[2], e[0]);
m_colData.EEColData.push_back(d);
}
E2[e[2]][e[0]] = 0;
}
}
}
}
else if (m_object1->getType() == Geometry::Type::PointSet)
{
const auto mesh1 = std::static_pointer_cast<PointSet>(m_object1);
// brute force, use BVH or spatial grid would be much better
// point
for (size_t i = 0; i < mesh1->getNumVertices(); ++i)
{
const auto p = mesh1->getVertexPosition(i);
const auto triangleVerts = mesh2->getTrianglesVertices();
for (size_t j = 0; j < triangleVerts.size(); ++j)
{
const auto& e = triangleVerts[j];
const Vec3d p0 = mesh2->getVertexPosition(e[0]);
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (testPointToTriAABB(p[0], p[1], p[2],
p0[0], p0[1], p0[2],
p1[0], p1[1], p1[2],
p2[0], p2[1], p2[2], m_proximityTolerance, m_proximityTolerance))
{
TriangleVertexCollisionData d(i, j);
m_colData.TVColData.push_back(d);
}
}
}
}
else
{
const auto mesh1 = std::static_pointer_cast<SurfaceMesh>(m_object1);
// brute force, use BVH or spatial grid would be much better
// point
for (size_t i = 0; i < mesh1->getNumVertices(); ++i)
{
const Vec3d p = mesh1->getVertexPosition(i);
const auto triangleVerts = mesh2->getTrianglesVertices();
for (size_t j = 0; j < triangleVerts.size(); ++j)
{
const SurfaceMesh::TriangleArray& e = triangleVerts[j];
const Vec3d p0 = mesh2->getVertexPosition(e[0]);
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (testPointToTriAABB(p[0], p[1], p[2],
p0[0], p0[1], p0[2],
p1[0], p1[1], p1[2],
p2[0], p2[1], p2[2], m_proximityTolerance, m_proximityTolerance))
{
TriangleVertexCollisionData d(i, j);
m_colData.TVColData.push_back(d);
}
}
}
// edge
// since we don't have edge structure, the following is not good
const auto nV = mesh1->getNumVertices();
std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
const auto nV2 = mesh2->getNumVertices();
std::vector<std::vector<bool>> E2(nV2, std::vector<bool>(nV2, 1));
const auto elements = mesh1->getTrianglesVertices();
for (size_t k = 0; k < elements.size(); ++k)
{
const SurfaceMesh::TriangleArray& tri = elements[k];
size_t i1 = tri[0];
size_t i2 = tri[1];
if (E[i1][i2] && E[i2][i1])
{
const Vec3d P = mesh1->getVertexPosition(i1);
const Vec3d Q = mesh1->getVertexPosition(i2);
const auto elements2 = mesh2->getTrianglesVertices();
for (size_t j = 0; j < elements2.size(); ++j)
{
const SurfaceMesh::TriangleArray& e = elements2[j];
const Vec3d p0 = mesh2->getVertexPosition(e[0]);
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p0[0], p0[1], p0[2],
p1[0], p1[1], p1[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[0], e[1]);
m_colData.EEColData.push_back(d);
E2[e[0]][e[1]] = 0;
}
}
if (E2[e[1]][e[2]] && E2[e[2]][e[1]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p1[0], p1[1], p1[2],
p2[0], p2[1], p2[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[1], e[2]);
m_colData.EEColData.push_back(d);
E2[e[1]][e[2]] = 0;
}
}
if (E2[e[2]][e[0]] && E2[e[0]][e[2]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p2[0], p2[1], p2[2],
p0[0], p0[1], p0[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[2], e[0]);
m_colData.EEColData.push_back(d);
E2[e[2]][e[0]] = 0;
}
}
}
E[i1][i2] = 0;
}
i1 = tri[1];
i2 = tri[2];
if (E[i1][i2] && E[i2][i1])
{
const Vec3d P = mesh1->getVertexPosition(i1);
const Vec3d Q = mesh1->getVertexPosition(i2);
const auto elements2 = mesh2->getTrianglesVertices();
for (size_t j = 0; j < elements2.size(); ++j)
{
const SurfaceMesh::TriangleArray& e = elements2[j];
const Vec3d p0 = mesh2->getVertexPosition(e[0]);
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p0[0], p0[1], p0[2],
p1[0], p1[1], p1[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[0], e[1]);
m_colData.EEColData.push_back(d);
E2[e[0]][e[1]] = 0;
}
}
if (E2[e[1]][e[2]] && E2[e[2]][e[1]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p1[0], p1[1], p1[2],
p2[0], p2[1], p2[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[1], e[2]);
m_colData.EEColData.push_back(d);
E2[e[1]][e[2]] = 0;
}
}
if (E2[e[2]][e[0]] && E2[e[0]][e[2]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p2[0], p2[1], p2[2],
p0[0], p0[1], p0[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[2], e[0]);
m_colData.EEColData.push_back(d);
E2[e[2]][e[0]] = 0;
}
}
}
E[i1][i2] = 0;
}
i1 = tri[2];
i2 = tri[0];
if (E[i1][i2] && E[i2][i1])
{
const Vec3d P = mesh1->getVertexPosition(i1);
const Vec3d Q = mesh1->getVertexPosition(i2);
const auto elements2 = mesh2->getTrianglesVertices();
for (size_t j = 0; j < elements2.size(); ++j)
{
const SurfaceMesh::TriangleArray& e = elements2[j];
const Vec3d p0 = mesh2->getVertexPosition(e[0]);
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p0[0], p0[1], p0[2],
p1[0], p1[1], p1[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[0], e[1]);
m_colData.EEColData.push_back(d);
E2[e[0]][e[1]] = 0;
}
}
if (E2[e[1]][e[2]] && E2[e[2]][e[1]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p1[0], p1[1], p1[2],
p2[0], p2[1], p2[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[1], e[2]);
m_colData.EEColData.push_back(d);
E2[e[1]][e[2]] = 0;
}
}
if (E2[e[2]][e[0]] && E2[e[0]][e[2]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
Q[0], Q[1], Q[2],
p2[0], p2[1], p2[2],
p0[0], p0[1], p0[2], m_proximityTolerance, m_proximityTolerance))
{
EdgeEdgeCollisionData d(i1, i2, e[2], e[0]);
m_colData.EEColData.push_back(d);
E2[e[2]][e[0]] = 0;
}
}
}
E[i1][i2] = 0;
}
}
}
}
bool
MeshToMeshBruteForceCD::doBroadPhaseCollisionCheck() const
{
const auto mesh1 = std::static_pointer_cast<PointSet>(m_object1);
const auto mesh2 = std::static_pointer_cast<PointSet>(m_object2);
Vec3d min1, max1;
mesh1->computeBoundingBox(min1, max1);
Vec3d min2, max2;
mesh2->computeBoundingBox(min2, max2);
return testAABBToAABB(min1[0] - m_proximityTolerance,
max1[0] + m_proximityTolerance,
min1[1] - m_proximityTolerance,
max1[1] + m_proximityTolerance,
min1[2] - m_proximityTolerance,
max1[2] + m_proximityTolerance,
min2[0] - m_proximityTolerance,
max2[0] + m_proximityTolerance,
min2[1] - m_proximityTolerance,
max2[1] + m_proximityTolerance,
min2[2] - m_proximityTolerance,
max2[2] + m_proximityTolerance);
}
} // imstk
/*=========================================================================
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 imstkMeshToMeshBruteforceCD_h
#define imstkMeshToMeshBruteforceCD_h
#include <memory>
#include "imstkCollisionDetection.h"
namespace imstk
{
class Geometry;
class SurfaceMesh;
class CollisionData;
///
/// \class MeshToMeshBruteForceCD
///
/// \brief Mesh to mesh collision with brute force strategy
///
class MeshToMeshBruteForceCD : public CollisionDetection
{
public:
///
/// \brief Constructor
///
MeshToMeshBruteForceCD(std::shared_ptr<Geometry> obj1,
std::shared_ptr<SurfaceMesh> obj2,
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::MeshToMeshBruteForce,
colData),
m_object1(obj1),
m_object2(obj2){}