Commit 0186ac28 authored by Andrew Wilson's avatar Andrew Wilson Committed by Andrew Wilson
Browse files

BUG: Fix issue in MeshToMeshBruteForce, handles extra edge case, bary moved...

BUG: Fix issue in MeshToMeshBruteForce, handles extra edge case, bary moved and swapped with eigen vecs, comments added
parent d1c7b0c9
Pipeline #240719 failed with stage
......@@ -38,6 +38,8 @@
#include "imstkVisualModel.h"
#include "imstkVTKViewer.h"
#include "imstkPlane.h"
using namespace imstk;
// mesh file name
......@@ -215,9 +217,9 @@ main()
scene->addSceneObject(fluidObj);
imstkNew<CollidingObject> floorObj("Floor");
std::shared_ptr<SurfaceMesh> floorMesh = createCollidingSurfaceMesh();
floorObj->setVisualGeometry(floorMesh);
floorObj->setCollidingGeometry(floorMesh);
std::shared_ptr<SurfaceMesh> floorGeom = createCollidingSurfaceMesh();
floorObj->setVisualGeometry(floorGeom);
floorObj->setCollidingGeometry(floorGeom);
scene->addSceneObject(floorObj);
// Collisions
......
......@@ -23,7 +23,6 @@
#include "imstkMath.h"
#include "imstkTypes.h"
#include "imstkGeometryUtilities.h"
#include <algorithm>
#include <array>
......@@ -776,13 +775,15 @@ testSphereToTriangle(const Vec3d& spherePt, const double sphereRadius,
}
}
///
/// \brief Tests if a point is inside a tetrahedron
///
inline bool
testPointInsideTet(const std::array<Vec3d, 4>& inputTetVerts, const Vec3d& p)
testPointToTetrahedron(const std::array<Vec3d, 4>& inputTetVerts, const Vec3d& p)
{
std::array<double, 4> bCoord;
GeometryUtils::computePointBarycentricCoordinates(inputTetVerts, p, bCoord);
const Vec4d bCoord = baryCentric(p, inputTetVerts[0], inputTetVerts[1], inputTetVerts[2], inputTetVerts[3]);
constexpr const double eps = VERY_SMALL_EPSILON;
constexpr const double eps = IMSTK_DOUBLE_EPS;
if (bCoord[0] >= -eps
&& bCoord[1] >= -eps
&& bCoord[2] >= -eps
......@@ -795,8 +796,7 @@ testPointInsideTet(const std::array<Vec3d, 4>& inputTetVerts, const Vec3d& p)
}
///
/// \brief Tests if the segment intersects any of the triangle faces of the tet
/// \todo: Could be faster with SAT directly applied here
/// \brief Tests if the segment intersects any of the triangle faces of the tet (GJK)
///
inline bool
testTetToSegment(
......@@ -812,8 +812,8 @@ testTetToSegment(
}
}
// test if both points lie inside the tetrahedron
if (testPointInsideTet(inputTetVerts, x1) && testPointInsideTet(inputTetVerts, x2))
// If either point lies inside the tetrahedron (handles completely inside case)
if (testPointToTetrahedron(inputTetVerts, x1) || testPointToTetrahedron(inputTetVerts, x2))
{
return true;
}
......@@ -862,6 +862,45 @@ testTetToSegment(
return firstFound;
}
///
/// \brief ray OBB intersection with intersection point
/// \param ray origin
/// \param ray direction
/// \param box to world
/// \param world to box
/// \param half length/extents
/// \param intersection points (first component gives entry point, second gives exit)
///
inline bool
testRayToObb(const Vec3d& rayOrigin, const Vec3d& rayDir,
const Mat4d& worldToBox, Vec3d extents,
Vec2d& iPt)
{
// convert from world to box space
const Vec3d rd = (worldToBox * Vec4d(rayDir[0], rayDir[1], rayDir[2], 0.0)).head<3>();
const Vec3d ro = (worldToBox * Vec4d(rayOrigin[0], rayOrigin[1], rayOrigin[2], 1.0)).head<3>();
// ray-box intersection in box space
const Vec3d m = Vec3d(1.0, 1.0, 1.0).cwiseQuotient(rd);
const Vec3d s = Vec3d((rd[0] < 0.0) ? 1.0 : -1.0,
(rd[1] < 0.0) ? 1.0 : -1.0,
(rd[2] < 0.0) ? 1.0 : -1.0);
const Vec3d t1 = m.cwiseProduct(-ro + s.cwiseProduct(extents));
const Vec3d t2 = m.cwiseProduct(-ro - s.cwiseProduct(extents));
const double tN = std::max(std::max(t1[0], t1[1]), t1[2]);
const double tF = std::min(std::min(t2[0], t2[1]), t2[2]);
// Does not enter
if (tN > tF || tF < 0.0)
{
return false;
}
iPt = Vec2d(tN, tF); // Parameterized along the ray
return true;
}
///
/// \brief Compute closest distance from a point to a segment x1-x2
///
......
......@@ -20,12 +20,10 @@
=========================================================================*/
#include "imstkMeshToMeshBruteForceCD.h"
#include "imstkCollisionData.h"
#include "imstkCollisionUtils.h"
#include "imstkLineMesh.h"
#include "imstkSurfaceMesh.h"
#include <stdint.h>
#include <unordered_set>
namespace imstk
......@@ -233,15 +231,15 @@ polySignedDist(const Vec3d& pos, const SurfMeshData& surfMeshData,
const Vec3d& x2 = surfMeshData.vertices[cell[1]];
const Vec3d& x3 = surfMeshData.vertices[cell[2]];
int caseType;
const Vec3d closestPtOnTri = CollisionUtils::closestPointOnTriangle(pos, x1, x2, x3, caseType);
int ptOnTriangleCaseType;
const Vec3d closestPtOnTri = CollisionUtils::closestPointOnTriangle(pos, x1, x2, x3, ptOnTriangleCaseType);
const double sqrDist = (closestPtOnTri - pos).squaredNorm();
if (sqrDist < minSqrDist)
{
minSqrDist = sqrDist;
closestPt = closestPtOnTri;
closestCell = j;
closestCellCase = caseType;
closestCellCase = ptOnTriangleCaseType;
}
}
......@@ -321,6 +319,7 @@ MeshToMeshBruteForceCD::computeCollisionDataAB(
// Broad phase collision
if (doBroadPhaseCollisionCheck(geomA, geomB))
{
auto pointSet = std::dynamic_pointer_cast<PointSet>(geomA);
auto surfMesh = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
surfMesh->computeTrianglesNormals();
surfMesh->computeVertexNeighborTriangles();
......@@ -328,9 +327,9 @@ MeshToMeshBruteForceCD::computeCollisionDataAB(
// Narrow phase
if (m_generateVertexTriangleContacts)
{
if (m_vertexInside.size() < surfMesh->getNumVertices())
if (m_vertexInside.size() < pointSet->getNumVertices())
{
m_vertexInside = std::vector<bool>(surfMesh->getNumVertices(), false);
m_vertexInside = std::vector<bool>(pointSet->getNumVertices(), false);
}
else
{
......@@ -389,8 +388,8 @@ MeshToMeshBruteForceCD::vertexToTriangleTest(
elemB.idCount = 1;
elemB.cellType = IMSTK_VERTEX;
elementsA.safeAppend(elemA);
elementsB.safeAppend(elemB);
elementsA.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
m_vertexInside[i] = true;
}
else if (caseType == 1)
......@@ -406,8 +405,8 @@ MeshToMeshBruteForceCD::vertexToTriangleTest(
elemB.idCount = 2;
elemB.cellType = IMSTK_EDGE;
elementsA.safeAppend(elemA);
elementsB.safeAppend(elemB);
elementsA.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
m_vertexInside[i] = true;
}
else if (caseType == 2)
......@@ -424,8 +423,8 @@ MeshToMeshBruteForceCD::vertexToTriangleTest(
elemB.idCount = 3;
elemB.cellType = IMSTK_TRIANGLE;
elementsA.safeAppend(elemA);
elementsB.safeAppend(elemB);
elementsA.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
m_vertexInside[i] = true;
}
}
......@@ -514,8 +513,8 @@ MeshToMeshBruteForceCD::lineMeshEdgeToTriangleTest(
elemB.idCount = 2;
elemB.cellType = IMSTK_EDGE;
elementsA.safeAppend(elemA);
elementsB.safeAppend(elemB);
elementsA.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
}
}
}
......@@ -618,8 +617,8 @@ MeshToMeshBruteForceCD::surfMeshEdgeToTriangleTest(
elemB.idCount = 2;
elemB.cellType = IMSTK_EDGE;
elementsA.safeAppend(elemA);
elementsB.safeAppend(elemB);
elementsA.unsafeAppend(elemA);
elementsB.unsafeAppend(elemB);
hashedEdges.insert(edgePair);
}
......@@ -650,6 +649,12 @@ MeshToMeshBruteForceCD::doBroadPhaseCollisionCheck(
Vec3d min2, max2;
mesh2->computeBoundingBox(min2, max2);
// Padding here helps with thin vs thin geometry
min1 -= m_padding;
max1 += m_padding;
min2 -= m_padding;
max2 += m_padding;
return CollisionUtils::testAABBToAABB(
min1[0], max1[0],
min1[1], max1[1],
......
......@@ -78,6 +78,12 @@ public:
///
void setGenerateVertexTriangleContacts(bool genVertexTriangleContacts) { m_generateVertexTriangleContacts = genVertexTriangleContacts; }
///
/// \brief Set padding to the broad phase
///
void setPadding(const Vec3d& padding) { m_padding = padding; }
const Vec3d& getPadding() const { return m_padding; }
protected:
///
/// \brief Compute collision data for AB simulatenously
......@@ -119,5 +125,6 @@ private:
bool m_generateVertexTriangleContacts = true;
std::vector<bool> m_vertexInside;
Vec3d m_padding = Vec3d(0.001, 0.001, 0.001);
};
}
\ No newline at end of file
......@@ -68,8 +68,7 @@ TetraToPointSetCD::computeCollisionDataAB(
Vec3d vPos = verticesMeshB[vertexIdB];
// Now compute if the point is actually within the tet
TetrahedralMesh::WeightsArray bCoord; // Barycentric coordinates of the vertex in tetrahedron
tetMesh->computeBarycentricWeights(tetIdA, vPos, bCoord);
const Vec4d bCoord = tetMesh->computeBarycentricWeights(tetIdA, vPos);
if (bCoord[0] >= -eps
&& bCoord[1] >= -eps
&& bCoord[2] >= -eps
......@@ -131,8 +130,7 @@ TetraToPointSetCD::computeCollisionDataA(
Vec3d vPos = verticesMeshB[vertexIdB];
// Now compute if the point is actually within the tet
TetrahedralMesh::WeightsArray bCoord; // Barycentric coordinates of the vertex in tetrahedron
tetMesh->computeBarycentricWeights(tetIdA, vPos, bCoord);
const Vec4d bCoord = tetMesh->computeBarycentricWeights(tetIdA, vPos);
if (bCoord[0] >= -eps
&& bCoord[1] >= -eps
&& bCoord[2] >= -eps
......@@ -188,8 +186,7 @@ TetraToPointSetCD::computeCollisionDataB(
Vec3d vPos = verticesMeshB[vertexIdB];
// Now compute if the point is actually within the tet
TetrahedralMesh::WeightsArray bCoord; // Barycentric coordinates of the vertex in tetrahedron
tetMesh->computeBarycentricWeights(tetIdA, vPos, bCoord);
const Vec4d bCoord = tetMesh->computeBarycentricWeights(tetIdA, vPos);
if (bCoord[0] >= -eps
&& bCoord[1] >= -eps
&& bCoord[2] >= -eps
......
......@@ -267,7 +267,7 @@ baryCentric(const Vec2d& p, const Vec2d& a, const Vec2d& b, const Vec2d& c)
}
///
/// \brief Compute bary centric coordinates (u,v,w) given triangle in 3d space (and point p on triangle)
/// \brief Compute bary centric coordinates (u,v,w) of point p, given 3 points in 3d space (a,b,c)
///
static Vec3d
baryCentric(const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c)
......@@ -287,6 +287,31 @@ baryCentric(const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c)
return Vec3d(u, v, w);
}
///
/// \brief Compute bary centric coordinates (u,v,w,x) of point p, given 4 points in 3d space (a,b,c,d)
///
static Vec4d
baryCentric(const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c, const Vec3d& d)
{
Mat4d A;
A << a[0], a[1], a[2], 1.0,
b[0], b[1], b[2], 1.0,
c[0], c[1], c[2], 1.0,
d[0], d[1], d[2], 1.0;
Vec4d weights;
double det = A.determinant(); // Signed volume
for (int i = 0; i < 4; ++i)
{
Mat4d B = A;
B(i, 0) = p[0];
B(i, 1) = p[1];
B(i, 2) = p[2];
weights[i] = B.determinant() / det;
}
return weights;
}
///
/// \brief Cantors pairing function, take two ints and produce a unique one
/// The resulting numbers are close for two close A's and B's
......
......@@ -30,7 +30,8 @@ namespace imstk
///
/// \class PbdConstantDensityConstraint
///
/// \brief Implements the constant density constraint to simulate fluids
/// \brief Implements the constant density constraint to simulate fluids.
/// This constraint is global and applied to all vertices passed in during projection
///
class PbdConstantDensityConstraint : public PbdConstraint
{
......@@ -80,12 +81,7 @@ private:
double wPoly6(const Vec3d& pi, const Vec3d& pj);
///
/// \brief Smoothing kernel Spiky for gradient calculation
///
double wSpiky(const Vec3d& pi, const Vec3d& pj);
///
/// \brief
/// \brief Gradient of density kernel
///
Vec3d gradSpiky(const Vec3d& pi, const Vec3d& pj);
......
......@@ -41,7 +41,8 @@
namespace imstk
{
///
/// \brief PbdConstraintFunctor take input geometry and produce constraints
/// \brief PbdConstraintFunctor take input geometry and produce constraints.
/// It exists to allow extensible constraint generation
///
struct PbdConstraintFunctor
{
......
......@@ -55,7 +55,7 @@ public:
void setPosition(const double x, const double y, const double z);
///
/// \brief Get the local or globa orientation (post transformed)
/// \brief Get the local or global orientation (post transformed)
///
Quatd getOrientation(DataType type = DataType::PostTransform);
......
......@@ -236,18 +236,16 @@ TetrahedralMesh::extractSurfaceMesh()
return surfMesh;
}
void
TetrahedralMesh::computeBarycentricWeights(const size_t& tetId, const Vec3d& pos,
WeightsArray& weights) const
Vec4d
TetrahedralMesh::computeBarycentricWeights(const size_t& tetId, const Vec3d& pos) const
{
const Vec4i& tetIndices = (*m_tetrahedraIndices)[tetId];
std::array<Vec3d, 4> v;
for (size_t i = 0; i < 4; ++i)
{
v[i] = getVertexPosition(tetIndices[i]);
}
GeometryUtils::computePointBarycentricCoordinates(v, pos, weights);
const VecDataArray<double, 3>& vertices = *m_vertexPositions;
const VecDataArray<int, 4>& tetraIndices = *m_tetrahedraIndices;
return baryCentric(pos,
vertices[tetraIndices[tetId][0]],
vertices[tetraIndices[tetId][1]],
vertices[tetraIndices[tetId][2]],
vertices[tetraIndices[tetId][3]]);
}
void
......
......@@ -36,9 +36,6 @@ class SurfaceMesh;
///
class TetrahedralMesh : public VolumetricMesh
{
public:
using WeightsArray = std::array<double, 4>;
public:
///
/// \brief Constructor
......@@ -81,7 +78,7 @@ public:
///
/// \brief compute the barycentric weights of a given point in 3D space for a given the tetrahedra
///
void computeBarycentricWeights(const size_t& tetId, const Vec3d& pos, WeightsArray& weights) const;
Vec4d computeBarycentricWeights(const size_t& tetId, const Vec3d& pos) const;
///
/// \brief Compute the bounding box of a given tetrahedron
......
......@@ -1597,7 +1597,7 @@ GeometryUtils::createTetrahedralMeshCover(std::shared_ptr<SurfaceMesh> surfMesh,
const int tetId0 = numDiv * hexId;
const int tetId1 = tetId0 + numDiv;
static TetrahedralMesh::WeightsArray weights = { 0.0, 0.0, 0.0, 0.0 };
static Vec4d weights = Vec4d::Zero();
// loop over the tets to find the enclosing tets
for (int id = tetId0; id < tetId1; ++id)
......@@ -1606,7 +1606,7 @@ GeometryUtils::createTetrahedralMeshCover(std::shared_ptr<SurfaceMesh> surfMesh,
{
continue;
}
uniformMesh->computeBarycentricWeights(id, xyz, weights);
weights = uniformMesh->computeBarycentricWeights(id, xyz);
if ((weights[0] >= 0) && (weights[1] >= 0) && (weights[2] >= 0) && (weights[3] >= 0))
{
......
......@@ -242,23 +242,6 @@ std::shared_ptr<TetrahedralMesh> createUniformMesh(const Vec3d& aabbMin, const V
///
std::shared_ptr<TetrahedralMesh> createTetrahedralMeshCover(std::shared_ptr<SurfaceMesh> surfMesh, const int nx, const int ny, int nz);
inline void
computePointBarycentricCoordinates(const std::array<Vec3d, 4>& v, const Vec3d& p, std::array<double, 4>& weights)
{
Mat4d A;
A << v[0][0], v[0][1], v[0][2], 1, v[1][0], v[1][1], v[1][2], 1, v[2][0], v[2][1], v[2][2], 1, v[3][0], v[3][1], v[3][2], 1;
double det = A.determinant();
for (int i = 0; i < 4; ++i)
{
Mat4d B = A;
B(i, 0) = p[0];
B(i, 1) = p[1];
B(i, 2) = p[2];
weights[i] = B.determinant() / det;
}
}
///
/// \brief Enumeration for reordering method
///
......
......@@ -72,8 +72,7 @@ TetraTriangleMap::compute()
}
// Compute the weights
TetrahedralMesh::WeightsArray weights = { 0.0, 0.0, 0.0, 0.0 };
tetMesh->computeBarycentricWeights(closestTetId, surfVertPos, weights);
const Vec4d weights = tetMesh->computeBarycentricWeights(closestTetId, surfVertPos);
m_verticesEnclosingTetraId[vertexIdx] = closestTetId; // store nearest tetrahedron
m_verticesWeights[vertexIdx] = weights; // store weights
......@@ -215,9 +214,8 @@ TetraTriangleMap::findClosestTetrahedron(const Vec3d& pos) const
size_t
TetraTriangleMap::findEnclosingTetrahedron(const Vec3d& pos) const
{
const auto tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_master);
TetrahedralMesh::WeightsArray weights = { 0.0, 0.0, 0.0, 0.0 };
size_t enclosingTetrahedron = std::numeric_limits<size_t>::max();
const auto tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_master);
size_t enclosingTetrahedron = std::numeric_limits<size_t>::max();
for (int idx = 0; idx < tetMesh->getNumTetrahedra(); ++idx)
{
......@@ -232,7 +230,7 @@ TetraTriangleMap::findEnclosingTetrahedron(const Vec3d& pos) const
continue;
}
tetMesh->computeBarycentricWeights(idx, pos, weights);
const Vec4d weights = tetMesh->computeBarycentricWeights(idx, pos);
if (weights[0] >= 0 && weights[1] >= 0 && weights[2] >= 0 && weights[3] >= 0)
{
......
......@@ -23,8 +23,6 @@
#include "imstkGeometryMap.h"
#include <array>
namespace imstk
{
template<typename T, int N> class VecDataArray;
......@@ -106,9 +104,9 @@ protected:
///
size_t findClosestTetrahedron(const Vec3d& pos) const;
std::vector<std::array<double, 4>> m_verticesWeights; ///> weights
std::vector<Vec4d> m_verticesWeights; ///> weights
std::vector<size_t> m_verticesEnclosingTetraId; ///> Enclosing tetrahedra to interpolate the weights upon
std::vector<size_t> m_verticesEnclosingTetraId; ///> Enclosing tetrahedra to interpolate the weights upon
std::vector<Vec3d> m_bBoxMin;
std::vector<Vec3d> m_bBoxMax;
......
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