Commit 9fb1f933 authored by Nghia Truong's avatar Nghia Truong

REFAC: Rename IntersectionTestUtils to CollisionUtils and add more function to...

REFAC: Rename IntersectionTestUtils to CollisionUtils and add more function to test segment-triangle intersections and compute point-triangle closest distance
parent c85ade66
......@@ -19,46 +19,18 @@
=========================================================================*/
#pragma once
#include <algorithm>
#include "imstkCollisionUtils.h"
namespace imstk
{
///
/// \brief Do ranges [a,b] and [c,d] intersect?
///
inline bool
isIntersect(const double& a, const double& b, const double& c, const double& d)
namespace CollisionUtils
{
return ((a <= d && a >= c) || (c <= b && c >= a)) ? true : false;
}
///
/// \brief Check if two AABBs are intersecting
///
inline bool
testAABBToAABB(const double& min1_x, const double& max1_x,
const double& min1_y, const double& max1_y,
const double& min1_z, const double& max1_z,
const double& min2_x, const double& max2_x,
const double& min2_y, const double& max2_y,
const double& min2_z, const double& max2_z)
{
return (isIntersect(min1_x, max1_x, min2_x, max2_x)
&& isIntersect(min1_y, max1_y, min2_y, max2_y)
&& isIntersect(min1_z, max1_z, min2_z, max2_z));
}
///
/// \brief Check if two lines are intersecting with AABB intersection test
///
inline bool
testLineToLineAABB(const double& x1, const double& y1, const double& z1,
const double& x2, const double& y2, const double& z2,
const double& x3, const double& y3, const double& z3,
const double& x4, const double& y4, const double& z4,
const double& prox1, const double& prox2)
bool
testLineToLineAABB(const double x1, const double y1, const double z1,
const double x2, const double y2, const double z2,
const double x3, const double y3, const double z3,
const double x4, const double y4, const double z4,
const double prox1 /*= VERY_SMALL_EPSILON_D*/, const double prox2 /*= VERY_SMALL_EPSILON_D*/)
{
double min1_x, max1_x, min1_y, max1_y, min1_z, max1_z;
......@@ -135,25 +107,109 @@ testLineToLineAABB(const double& x1, const double& y1, const double& z1,
min2_y - prox2, max2_y + prox2, min2_z - prox2, max2_z + prox2);
}
///
/// \brief Check if triangle and point are intersecting with AABB test
///
inline bool
testPointToTriAABB(const double& x1, const double& y1, const double& z1,
const double& x2, const double& y2, const double& z2,
const double& x3, const double& y3, const double& z3,
const double& x4, const double& y4, const double& z4,
const double& prox1, const double& prox2)
bool
segmentIntersectsTriangle(const Vec3r& pA, const Vec3r& pB,
const Vec3r& v0, const Vec3r& v1, const Vec3r& v2)
{
static const Real EPSILON = 1e-8;
Vec3r dirAB = pB - pA;
const Real lAB = dirAB.norm();
if (lAB < 1e-8)
{
return false;
}
dirAB /= lAB;
Vec3r edge1 = v1 - v0;
Vec3r edge2 = v2 - v0;
Vec3r h = dirAB.cross(edge2);
Real a = edge1.dot(h);
if (a > -EPSILON && a < EPSILON)
{
return false; // This ray is parallel to this triangle.
}
Real f = Real(1.0) / a;
Vec3r s = pA - v0;
Real u = f * s.dot(h);
if (u < Real(0) || u > Real(1.0))
{
return false;
}
Vec3r q = s.cross(edge1);
Real v = f * dirAB.dot(q);
if (v < Real(0) || u + v > Real(1.0))
{
return false;
}
// At this stage we can compute t to find out where the intersection point is on the line.
Real t = f * edge2.dot(q);
// ray - triangle intersection
return (t > EPSILON && t + EPSILON < lAB);
}
Real
pointSegmentClosestDistance(const Vec3r& point, const Vec3r& x1, const Vec3r& x2)
{
auto min_x = std::min(x2, std::min(x3, x4));
auto max_x = std::max(x2, std::max(x3, x4));
auto min_y = std::min(y2, std::min(y3, y4));
auto max_y = std::max(y2, std::max(y3, y4));
auto min_z = std::min(z2, std::min(z3, z4));
auto max_z = std::max(z2, std::max(z3, z4));
return testAABBToAABB(x1 - prox1, x1 + prox1, y1 - prox1, y1 + prox1,
z1 - prox1, z1 + prox1, min_x - prox2, max_x + prox2,
min_y - prox2, max_y + prox2, min_z - prox2, max_z + prox2);
Vec3r dx = x2 - x1;
Real m2 = dx.squaredNorm();
if (m2 < Real(1e-20))
{
return (point - x1).norm();
}
// find parameter value of closest point on segment
Real s12 = dx.dot(x2 - point) / m2;
if (s12 < 0)
{
s12 = 0;
}
else if (s12 > 1.0)
{
s12 = 1.0;
}
return (point - s12 * x1 + (1.0 - s12) * x2).norm();
}
Real
pointTriangleClosestDistance(const Vec3r& point, const Vec3r& x1, const Vec3r& x2, const Vec3r& x3)
{
// first find barycentric coordinates of closest point on infinite plane
Vec3r x13(x1 - x3), x23(x2 - x3), xp3(point - x3);
Real m13 = x13.squaredNorm(), m23 = x23.squaredNorm(), d = x13.dot(x23);
Real invdet = 1.0 / std::max(m13 * m23 - d * d, 1e-30);
Real a = x13.dot(xp3), b = x23.dot(xp3);
// Barycentric coordinates
Real w23 = invdet * (m23 * a - d * b);
Real w31 = invdet * (m13 * b - d * a);
Real w12 = 1.0 - w23 - w31;
if (w23 >= 0 && w31 >= 0 && w12 >= 0) // inside the triangle
{
return (point - w23 * x1 + w31 * x2 + w12 * x3).norm();
}
else
{
if (w23 > 0) //this rules out edge 2-3
{
return std::min(pointSegmentClosestDistance(point, x1, x2),
pointSegmentClosestDistance(point, x1, x3));
}
else if (w31 > 0) //this rules out edge 1-3
{
return std::min(pointSegmentClosestDistance(point, x1, x2),
pointSegmentClosestDistance(point, x2, x3));
}
else //w12 must be >0, ruling out edge 1-2
{
return std::min(pointSegmentClosestDistance(point, x1, x3),
pointSegmentClosestDistance(point, x2, x3));
}
}
}
} // namespace CollisionUtils
} // 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.
=========================================================================*/
#pragma once
#include "imstkMath.h"
#include <algorithm>
namespace imstk
{
namespace CollisionUtils
{
///
/// \brief Do ranges [a,b] and [c,d] intersect?
///
inline bool
isIntersect(const double a, const double b, const double c, const double d)
{
return ((a <= d && a >= c) || (c <= b && c >= a)) ? true : false;
}
///
/// \brief Check if two AABBs are intersecting
///
inline bool
testAABBToAABB(const double min1_x, const double max1_x,
const double min1_y, const double max1_y,
const double min1_z, const double max1_z,
const double min2_x, const double max2_x,
const double min2_y, const double max2_y,
const double min2_z, const double max2_z)
{
return (isIntersect(min1_x, max1_x, min2_x, max2_x)
&& isIntersect(min1_y, max1_y, min2_y, max2_y)
&& isIntersect(min1_z, max1_z, min2_z, max2_z));
}
///
/// \brief Check if triangle and point are intersecting with AABB test
///
inline bool
testPointToTriAABB(const double x1, const double y1, const double z1,
const double x2, const double y2, const double z2,
const double x3, const double y3, const double z3,
const double x4, const double y4, const double z4,
const double prox1, const double prox2)
{
const auto min_x = std::min({ x2, x3, x4 });
const auto max_x = std::max({ x2, x3, x4 });
const auto min_y = std::min({ y2, y3, y4 });
const auto max_y = std::max({ y2, y3, y4 });
const auto min_z = std::min({ z2, z3, z4 });
const auto max_z = std::max({ z2, z3, z4 });
return testAABBToAABB(x1 - prox1, x1 + prox1, y1 - prox1, y1 + prox1,
z1 - prox1, z1 + prox1, min_x - prox2, max_x + prox2,
min_y - prox2, max_y + prox2, min_z - prox2, max_z + prox2);
}
///
/// \brief Check if two lines are intersecting with AABB intersection test
/// \param x1/y1/z1 Coordinates of the first end-point of the first line
/// \param x2/y2/z2 Coordinates of the second end-point of the first line
/// \param x3/y3/z3 Coordinates of the first end-point of the second line
/// \param x4/y4/z4 Coordinates of the second end-point of the second line
/// \param prox1 Round-off precision for the test
/// \param prox2 Round-off precision for the test
///
bool testLineToLineAABB(const double x1, const double y1, const double z1,
const double x2, const double y2, const double z2,
const double x3, const double y3, const double z3,
const double x4, const double y4, const double z4,
const double prox1 = VERY_SMALL_EPSILON_D, const double prox2 = VERY_SMALL_EPSILON_D);
///
/// \brief Check if two lines are intersecting with AABB intersection test
/// \param p1A The first end-point of the first line
/// \param p1B The second end-point of the first line
/// \param p2A The first end-point of the second line
/// \param p2B The second end-point of the second line
/// \param prox1 Round-off precision for the test
/// \param prox2 Round-off precision for the test
///
inline bool
testLineToLineAABB(const Vec3r& p1A, const Vec3r& p1B,
const Vec3r& p2A, const Vec3r& p2B,
const double prox1 = VERY_SMALL_EPSILON_D, const double prox2 = VERY_SMALL_EPSILON_D)
{
const Real* p1Aptr = &p1A[0];
const Real* p1Bptr = &p1B[0];
const Real* p2Aptr = &p2A[0];
const Real* p2Bptr = &p2B[0];
return testLineToLineAABB(p1Aptr[0], p1Aptr[1], p1Aptr[2],
p1Bptr[0], p1Bptr[1], p1Bptr[2],
p2Aptr[0], p2Aptr[1], p2Aptr[2],
p2Bptr[0], p2Bptr[1], p2Bptr[2],
prox1, prox2);
}
///
/// \brief Test if the segment define by points pA-pB intersects with triangle using Moller–Trumbore algorithm
///
bool segmentIntersectsTriangle(const Vec3r& pA, const Vec3r& pB,
const Vec3r& v0, const Vec3r& v1, const Vec3r& v2);
///
/// \brief Compute closest distance from a point to a segment x1-x2
///
Real pointSegmentClosestDistance(const Vec3r& point, const Vec3r& x1, const Vec3r& x2);
///
/// \brief Compute closest distance from a point to a triangle x1-x2-x3
///
Real pointTriangleClosestDistance(const Vec3r& point, const Vec3r& x1, const Vec3r& x2, const Vec3r& x3);
} // namespace CollisionUtils
} // imstk
......@@ -27,12 +27,21 @@
#include "imstkPointSet.h"
#include "imstkLineMesh.h"
#include "imstkGeometry.h"
#include "imstkIntersectionTestUtils.h"
#include "imstkCollisionUtils.h"
#include <g3log/g3log.hpp>
namespace imstk
{
MeshToMeshBruteForceCD::MeshToMeshBruteForceCD(std::shared_ptr<Geometry> obj1,
std::shared_ptr<SurfaceMesh> obj2,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::MeshToMeshBruteForce, colData),
m_object1(obj1),
m_object2(obj2)
{
}
void
MeshToMeshBruteForceCD::computeCollisionData()
{
......@@ -64,15 +73,14 @@ MeshToMeshBruteForceCD::computeCollisionData()
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (testPointToTriAABB(p[0], p[1], p[2],
if (CollisionUtils::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);
m_colData->TVColData.safeAppend({ static_cast<unsigned int>(j), static_cast<unsigned int>(i), 0.0 });
}
}
}
......@@ -100,39 +108,36 @@ MeshToMeshBruteForceCD::computeCollisionData()
if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[0], e[1] }, 0.0 });
}
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],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[1], e[2] }, 0.0 });
}
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],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[2], e[0] }, 0.0 });
}
E2[e[2]][e[0]] = 0;
}
......@@ -157,13 +162,12 @@ MeshToMeshBruteForceCD::computeCollisionData()
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (testPointToTriAABB(p[0], p[1], p[2],
if (CollisionUtils::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);
m_colData->TVColData.safeAppend({ static_cast<unsigned int>(j), static_cast<unsigned int>(i), 0.0 });
}
}
}
......@@ -186,13 +190,12 @@ MeshToMeshBruteForceCD::computeCollisionData()
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (testPointToTriAABB(p[0], p[1], p[2],
if (CollisionUtils::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);
m_colData->TVColData.safeAppend({ static_cast<unsigned int>(j), static_cast<unsigned int>(i), 0.0 });
}
}
}
......@@ -226,42 +229,36 @@ MeshToMeshBruteForceCD::computeCollisionData()
if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[0], e[1] }, 0.0 });
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],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[1], e[2] }, 0.0 });
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],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[2], e[0] }, 0.0 });
E2[e[2]][e[0]] = 0;
}
}
......@@ -286,42 +283,36 @@ MeshToMeshBruteForceCD::computeCollisionData()
if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[0], e[1] }, 0.0 });
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],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[1], e[2] }, 0.0 });
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],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[2], e[0] }, 0.0 });
E2[e[2]][e[0]] = 0;
}
}
......@@ -345,42 +336,36 @@ MeshToMeshBruteForceCD::computeCollisionData()
if (E2[e[0]][e[1]] && E2[e[1]][e[0]])
{
if (testLineToLineAABB(P[0], P[1], P[2],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[0], e[1] }, 0.0 });
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],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[1], e[2] }, 0.0 });
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],
if (CollisionUtils::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);
m_colData->EEColData.safeAppend({ { i1, i2 }, { e[2], e[0] }, 0.0 });
E2[e[2]][e[0]] = 0;
}
}
......@@ -403,7 +388,7 @@ MeshToMeshBruteForceCD::doBroadPhaseCollisionCheck() const
Vec3d min2, max2;
mesh2->computeBoundingBox(min2, max2);
return testAABBToAABB(min1[0] - m_proximityTolerance,