TEST(TriangleMesh3, ClosestDistance) {
std::string objStr = getCubeTriMesh3x3x3Obj();
std::istringstream objStream(objStr);
TriangleMesh3 mesh;
mesh.readObj(&objStream);
const auto bruteForceSearch = [&](const Vector3D& pt) {
double minDist = kMaxD;
for (size_t i = 0; i < mesh.numberOfTriangles(); ++i) {
Triangle3 tri = mesh.triangle(i);
auto localResult = tri.closestDistance(pt);
if (localResult < minDist) {
minDist = localResult;
}
}
return minDist;
};
size_t numSamples = getNumberOfSamplePoints3();
for (size_t i = 0; i < numSamples; ++i) {
auto actual = mesh.closestDistance(getSamplePoints3()[i]);
auto expected = bruteForceSearch(getSamplePoints3()[i]);
EXPECT_DOUBLE_EQ(expected, actual);
}
}
TEST(TriangleMesh3, ClosestNormal) {
std::string objStr = getSphereTriMesh5x5Obj();
std::istringstream objStream(objStr);
TriangleMesh3 mesh;
mesh.readObj(&objStream);
const auto bruteForceSearch = [&](const Vector3D& pt) {
double minDist2 = kMaxD;
Vector3D result;
for (size_t i = 0; i < mesh.numberOfTriangles(); ++i) {
Triangle3 tri = mesh.triangle(i);
auto localResult = tri.closestNormal(pt);
auto closestPt = tri.closestPoint(pt);
double localDist2 = pt.distanceSquaredTo(closestPt);
if (localDist2 < minDist2) {
minDist2 = localDist2;
result = localResult;
}
}
return result;
};
size_t numSamples = getNumberOfSamplePoints3();
for (size_t i = 0; i < numSamples; ++i) {
auto actual = mesh.closestNormal(getSamplePoints3()[i]);
auto expected = bruteForceSearch(getSamplePoints3()[i]);
EXPECT_VECTOR3_NEAR(expected, actual, 1e-9);
}
}
TEST(TriangleMesh3, ClosestIntersection) {
std::string objStr = getCubeTriMesh3x3x3Obj();
std::istringstream objStream(objStr);
TriangleMesh3 mesh;
mesh.readObj(&objStream);
size_t numSamples = getNumberOfSamplePoints3();
const auto bruteForceTest = [&](const Ray3D& ray) {
SurfaceRayIntersection3 result{};
for (size_t i = 0; i < mesh.numberOfTriangles(); ++i) {
Triangle3 tri = mesh.triangle(i);
auto localResult = tri.closestIntersection(ray);
if (localResult.distance < result.distance) {
result = localResult;
}
}
return result;
};
for (size_t i = 0; i < numSamples; ++i) {
Ray3D ray(getSamplePoints3()[i], getSampleDirs3()[i]);
auto actual = mesh.closestIntersection(ray);
auto expected = bruteForceTest(ray);
EXPECT_DOUBLE_EQ(expected.distance, actual.distance);
EXPECT_VECTOR3_EQ(expected.point, actual.point);
EXPECT_VECTOR3_EQ(expected.normal, actual.normal);
EXPECT_EQ(expected.isIntersecting, actual.isIntersecting);
}
}
TEST(TriangleMesh3, Intersects) {
std::string objStr = getCubeTriMesh3x3x3Obj();
std::istringstream objStream(objStr);
TriangleMesh3 mesh;
mesh.readObj(&objStream);
size_t numSamples = getNumberOfSamplePoints3();
const auto bruteForceTest = [&](const Ray3D& ray) {
for (size_t i = 0; i < mesh.numberOfTriangles(); ++i) {
Triangle3 tri = mesh.triangle(i);
if (tri.intersects(ray)) {
return true;
}
}
return false;
};
for (size_t i = 0; i < numSamples; ++i) {
Ray3D ray(getSamplePoints3()[i], getSampleDirs3()[i]);
bool actual = mesh.intersects(ray);
bool expected = bruteForceTest(ray);
EXPECT_EQ(expected, actual);
}
}
//----------------------------------------------------------------------------
static bool TriangleIntersectCone (const Triangle3& rkTri,
const Cone3& rkCone)
{
for (int iT = 1; iT < gs_iMaxSample; iT++)
{
Real fT = iT/Real(gs_iMaxSample);
for (int iS = 1; iS+iT < gs_iMaxSample; iS++)
{
Real fS = iS/Real(gs_iMaxSample);
Vector3 kP = rkTri.Origin()+fS*rkTri.Edge0()+fT*rkTri.Edge1();
if ( PointInsideCone(kP,rkCone) )
return true;
}
}
return false;
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Triangle3& rkTri0, const Triangle3& rkTri1,
Real* pfTri0P0, Real* pfTri0P1, Real* pfTri1P0, Real* pfTri1P1)
{
Real fS, fT, fU, fV, fS0, fT0, fU0, fV0, fSqrDist, fSqrDist0;
Segment3 kSeg;
// compare edges of tri0 against all of tri1
kSeg.Origin() = rkTri0.Origin();
kSeg.Direction() = rkTri0.Edge0();
fSqrDist = SqrDistance(kSeg,rkTri1,&fS,&fU,&fV);
fT = 0.0f;
kSeg.Direction() = rkTri0.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkTri1,&fT0,&fU0,&fV0);
fS0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = kSeg.Origin() + rkTri0.Edge0();
kSeg.Direction() = kSeg.Direction() - rkTri0.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkTri1,&fT0,&fU0,&fV0);
fS0 = 1.0f-fT0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
// compare edges of tri1 against all of tri0
kSeg.Origin() = rkTri1.Origin();
kSeg.Direction() = rkTri1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkTri0,&fU0,&fS0,&fT0);
fV0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Direction() = rkTri1.Edge1();
fSqrDist0 = SqrDistance(kSeg,rkTri0,&fV0,&fS0,&fT0);
fU0 = 0.0f;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
kSeg.Origin() = kSeg.Origin() + rkTri1.Edge0();
kSeg.Direction() = kSeg.Direction() - rkTri1.Edge0();
fSqrDist0 = SqrDistance(kSeg,rkTri0,&fV0,&fS0,&fT0);
fU0 = 1.0f-fV0;
if ( fSqrDist0 < fSqrDist )
{
fSqrDist = fSqrDist0;
fS = fS0;
fT = fT0;
fU = fU0;
fV = fV0;
}
if ( pfTri0P0 )
*pfTri0P0 = fS;
if ( pfTri0P1 )
*pfTri0P1 = fT;
if ( pfTri1P0 )
*pfTri1P0 = fU;
if ( pfTri1P1 )
*pfTri1P1 = fV;
return Math::FAbs(fSqrDist);
}
//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Triangle3& rkTri, const Cone3& rkCone)
{
// NOTE. The following quantities computed in this function can be
// precomputed and stored in the cone and triangle classes as an
// optimization.
// 1. The cone squared cosine.
// 2. The triangle squared edge lengths |E0|^2 and |E1|^2.
// 3. The third triangle edge E2 = E1 - E0 and squared length |E2|^2.
// 4. The triangle normal N = Cross(E0,E1) or unitized normal
// N = Cross(E0,E1)/Length(Cross(E0,E1)).
// triangle is <P0,P1,P2>, edges are E0 = P1-P0, E1=P2-P0
int iOnConeSide = 0;
Real fP0Test, fP1Test, fP2Test, fAdE, fEdE, fEdD, fC1, fC2;
Real fCosSqr = rkCone.CosAngle()*rkCone.CosAngle();
// test vertex P0
Vector3 kDiff0 = rkTri.Origin() - rkCone.Vertex();
Real fAdD0 = rkCone.Axis().Dot(kDiff0);
if ( fAdD0 >= 0.0f )
{
// P0 is on cone side of plane
fP0Test = fAdD0*fAdD0 - fCosSqr*(kDiff0.Dot(kDiff0));
if ( fP0Test >= 0.0f )
{
// P0 is inside the cone
return true;
}
else
{
// P0 is outside the cone, but on cone side of plane
iOnConeSide |= 1;
}
}
// else P0 is not on cone side of plane
#ifdef MGC_DEBUG_TRICONE
assert( !PointInsideCone(rkTri.Origin(),rkCone) );
#endif
// test vertex P1
Vector3 kDiff1 = kDiff0 + rkTri.Edge0();
Real fAdD1 = rkCone.Axis().Dot(kDiff1);
if ( fAdD1 >= 0.0f )
{
// P1 is on cone side of plane
fP1Test = fAdD1*fAdD1 - fCosSqr*(kDiff1.Dot(kDiff1));
if ( fP1Test >= 0.0f )
{
// P1 is inside the cone
return true;
}
else
{
// P1 is outside the cone, but on cone side of plane
iOnConeSide |= 2;
}
}
// else P1 is not on cone side of plane
#ifdef MGC_DEBUG_TRICONE
assert( !PointInsideCone(rkTri.Origin()+rkTri.Edge0(),rkCone) );
#endif
// test vertex P2
Vector3 kDiff2 = kDiff0 + rkTri.Edge1();
Real fAdD2 = rkCone.Axis().Dot(kDiff2);
if ( fAdD2 >= 0.0f )
{
// P2 is on cone side of plane
fP2Test = fAdD2*fAdD2 - fCosSqr*(kDiff2.Dot(kDiff2));
if ( fP2Test >= 0.0f )
{
// P2 is inside the cone
return true;
}
else
{
// P2 is outside the cone, but on cone side of plane
iOnConeSide |= 4;
}
}
// else P2 is not on cone side of plane
#ifdef MGC_DEBUG_TRICONE
assert( !PointInsideCone(rkTri.Origin()+rkTri.Edge1(),rkCone) );
#endif
// test edge <P0,P1> = E0
if ( iOnConeSide & 3 )
{
fAdE = fAdD1 - fAdD0;
fEdE = rkTri.Edge0().Dot(rkTri.Edge0());
fC2 = fAdE*fAdE - fCosSqr*fEdE;
if ( fC2 < 0.0f )
{
fEdD = rkTri.Edge0().Dot(kDiff0);
fC1 = fAdE*fAdD0 - fCosSqr*fEdD;
if ( iOnConeSide & 1 )
{
if ( iOnConeSide & 2 )
{
// <P0,P1> fully on cone side of plane, fC0 = fP0Test
if ( 0.0f <= fC1 && fC1 <= -fC2
&& fC1*fC1 >= fP0Test*fC2 )
{
return true;
}
}
else
{
// P0 on cone side (Dot(A,P0-V) >= 0),
// P1 on opposite side (Dot(A,P1-V) <= 0)
// (Dot(A,E0) <= 0), fC0 = fP0Test
if ( 0.0f <= fC1 && fC2*fAdD0 <= fC1*fAdE
&& fC1*fC1 >= fP0Test*fC2 )
{
return true;
}
}
}
else
{
// P1 on cone side (Dot(A,P1-V) >= 0),
// P0 on opposite side (Dot(A,P0-V) <= 0)
// (Dot(A,E0) >= 0), fC0 = fP0Test (needs calculating)
if ( fC1 <= -fC2 && fC2*fAdD0 <= fC1*fAdE )
{
fP0Test = fAdD0*fAdD0 - fCosSqr*(kDiff0.Dot(kDiff0));
if ( fC1*fC1 >= fP0Test*fC2 )
return true;
}
}
}
}
// else <P0,P1> does not intersect cone half space
#ifdef MGC_DEBUG_TRICONE
assert( !EdgeIntersectCone(rkTri.Origin(),rkTri.Edge0(),rkCone) );
#endif
// test edge <P0,P2> = E1
if ( iOnConeSide & 5 )
{
fAdE = fAdD2 - fAdD0;
fEdE = rkTri.Edge1().Dot(rkTri.Edge1());
fC2 = fAdE*fAdE - fCosSqr*fEdE;
if ( fC2 < 0.0f )
{
fEdD = rkTri.Edge1().Dot(kDiff0);
fC1 = fAdE*fAdD0 - fCosSqr*fEdD;
if ( iOnConeSide & 1 )
{
if ( iOnConeSide & 4 )
{
// <P0,P2> fully on cone side of plane, fC0 = fP0Test
if ( 0.0f <= fC1 && fC1 <= -fC2
&& fC1*fC1 >= fP0Test*fC2 )
{
return true;
}
}
else
{
// P0 on cone side (Dot(A,P0-V) >= 0),
// P2 on opposite side (Dot(A,P2-V) <= 0)
// (Dot(A,E1) <= 0), fC0 = fP0Test
if ( 0.0f <= fC1 && fC2*fAdD0 <= fC1*fAdE
&& fC1*fC1 >= fP0Test*fC2 )
{
return true;
}
}
}
else
{
// P2 on cone side (Dot(A,P2-V) >= 0),
// P0 on opposite side (Dot(A,P0-V) <= 0)
// (Dot(A,E1) >= 0), fC0 = fP0Test (needs calculating)
if ( fC1 <= -fC2 && fC2*fAdD0 <= fC1*fAdE )
{
fP0Test = fAdD0*fAdD0 - fCosSqr*(kDiff0.Dot(kDiff0));
if ( fC1*fC1 >= fP0Test*fC2 )
return true;
}
}
}
}
// else <P0,P2> does not intersect cone half space
#ifdef MGC_DEBUG_TRICONE
assert( !EdgeIntersectCone(rkTri.Origin(),rkTri.Edge1(),rkCone) );
#endif
// test edge <P1,P2> = E1-E0 = E2
if ( iOnConeSide & 6 )
{
Vector3 kE2 = rkTri.Edge1() - rkTri.Edge0();
fAdE = fAdD2 - fAdD1;
fEdE = kE2.Dot(kE2);
fC2 = fAdE*fAdE - fCosSqr*fEdE;
if ( fC2 < 0.0f )
{
fEdD = kE2.Dot(kDiff1);
fC1 = fAdE*fAdD1 - fCosSqr*fEdD;
if ( iOnConeSide & 2 )
{
if ( iOnConeSide & 4 )
{
// <P1,P2> fully on cone side of plane, fC0 = fP1Test
if ( 0.0f <= fC1 && fC1 <= -fC2
&& fC1*fC1 >= fP1Test*fC2 )
{
return true;
}
}
else
{
// P1 on cone side (Dot(A,P1-V) >= 0),
// P2 on opposite side (Dot(A,P2-V) <= 0)
// (Dot(A,E2) <= 0), fC0 = fP1Test
if ( 0.0f <= fC1 && fC2*fAdD1 <= fC1*fAdE
&& fC1*fC1 >= fP1Test*fC2 )
{
return true;
}
}
}
else
{
// P2 on cone side (Dot(A,P2-V) >= 0),
// P1 on opposite side (Dot(A,P1-V) <= 0)
// (Dot(A,E2) >= 0), fC0 = fP1Test (needs calculating)
if ( fC1 <= -fC2 && fC2*fAdD1 <= fC1*fAdE )
{
fP1Test = fAdD1*fAdD1 - fCosSqr*(kDiff1.Dot(kDiff1));
if ( fC1*fC1 >= fP1Test*fC2 )
return true;
}
}
}
}
// else <P1,P2> does not intersect cone half space
#ifdef MGC_DEBUG_TRICONE
assert( !EdgeIntersectCone(rkTri.Origin()+rkTri.Edge0(),rkTri.Edge1() -
rkTri.Edge0(),rkCone) );
#endif
// Test triangle <P0,P1,P2>. It is enough to handle only the case when
// at least one Pi is on the cone side of the plane. In this case and
// after the previous testing, if the triangle intersects the cone, the
// set of intersection must contain the point of intersection between
// the cone axis and the triangle.
if ( iOnConeSide > 0 )
{
Vector3 kN = rkTri.Edge0().Cross(rkTri.Edge1());
Real fNdA = kN.Dot(rkCone.Axis());
Real fNdD = kN.Dot(kDiff0);
Vector3 kU = fNdD*rkCone.Axis() - fNdA*kDiff0;
Vector3 kNcU = kN.Cross(kU);
Real fNcUdE0 = kNcU.Dot(rkTri.Edge0()), fNcUdE1, fNcUdE2, fNdN;
if ( fNdA >= 0.0f )
{
if ( fNcUdE0 <= 0.0f )
{
fNcUdE1 = kNcU.Dot(rkTri.Edge1());
if ( fNcUdE1 >= 0.0f )
{
fNcUdE2 = fNcUdE1 - fNcUdE0;
fNdN = kN.SquaredLength();
if ( fNcUdE2 <= fNdA*fNdN )
return true;
}
}
}
else
{
if ( fNcUdE0 >= 0.0f )
{
fNcUdE1 = kNcU.Dot(rkTri.Edge1());
if ( fNcUdE1 <= 0.0f )
{
fNcUdE2 = fNcUdE1 - fNcUdE0;
fNdN = kN.SquaredLength();
if ( fNcUdE2 >= fNdA*fNdN )
return true;
}
}
}
}
#ifdef MGC_DEBUG_TRICONE
assert( !TriangleIntersectCone(rkTri,rkCone) );
#endif
return false;
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Vector3& rkPoint, const Triangle3& rkTri,
Real* pfSParam, Real* pfTParam)
{
Vector3 kDiff = rkTri.Origin() - rkPoint;
Real fA00 = rkTri.Edge0().SquaredLength();
Real fA01 = rkTri.Edge0().Dot(rkTri.Edge1());
Real fA11 = rkTri.Edge1().SquaredLength();
Real fB0 = kDiff.Dot(rkTri.Edge0());
Real fB1 = kDiff.Dot(rkTri.Edge1());
Real fC = kDiff.SquaredLength();
Real fDet = Math::FAbs(fA00*fA11-fA01*fA01);
Real fS = fA01*fB1-fA11*fB0;
Real fT = fA01*fB0-fA00*fB1;
Real fSqrDist;
if ( fS + fT <= fDet )
{
if ( fS < 0.0f )
{
if ( fT < 0.0f ) // region 4
{
if ( fB0 < 0.0f )
{
fT = 0.0f;
if ( -fB0 >= fA00 )
{
fS = 1.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = -fB0/fA00;
fSqrDist = fB0*fS+fC;
}
}
else
{
fS = 0.0f;
if ( fB1 >= 0.0f )
{
fT = 0.0f;
fSqrDist = fC;
}
else if ( -fB1 >= fA11 )
{
fT = 1.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fT = -fB1/fA11;
fSqrDist = fB1*fT+fC;
}
}
}
else // region 3
{
fS = 0.0f;
if ( fB1 >= 0.0f )
{
fT = 0.0f;
fSqrDist = fC;
}
else if ( -fB1 >= fA11 )
{
fT = 1.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fT = -fB1/fA11;
fSqrDist = fB1*fT+fC;
}
}
}
else if ( fT < 0.0f ) // region 5
{
fT = 0.0f;
if ( fB0 >= 0.0f )
{
fS = 0.0f;
fSqrDist = fC;
}
else if ( -fB0 >= fA00 )
{
fS = 1.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = -fB0/fA00;
fSqrDist = fB0*fS+fC;
}
}
else // region 0
{
// minimum at interior point
Real fInvDet = 1.0f/fDet;
fS *= fInvDet;
fT *= fInvDet;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
else
{
Real fTmp0, fTmp1, fNumer, fDenom;
if ( fS < 0.0f ) // region 2
{
fTmp0 = fA01 + fB0;
fTmp1 = fA11 + fB1;
if ( fTmp1 > fTmp0 )
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-2.0f*fA01+fA11;
if ( fNumer >= fDenom )
{
fS = 1.0f;
fT = 0.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = fNumer/fDenom;
fT = 1.0f - fS;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
else
{
fS = 0.0f;
if ( fTmp1 <= 0.0f )
{
fT = 1.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else if ( fB1 >= 0.0f )
{
fT = 0.0f;
fSqrDist = fC;
}
else
{
fT = -fB1/fA11;
fSqrDist = fB1*fT+fC;
}
}
}
else if ( fT < 0.0f ) // region 6
{
fTmp0 = fA01 + fB1;
fTmp1 = fA00 + fB0;
if ( fTmp1 > fTmp0 )
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-2.0f*fA01+fA11;
if ( fNumer >= fDenom )
{
fT = 1.0f;
fS = 0.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fT = fNumer/fDenom;
fS = 1.0f - fT;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
else
{
fT = 0.0f;
if ( fTmp1 <= 0.0f )
{
fS = 1.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else if ( fB0 >= 0.0f )
{
fS = 0.0f;
fSqrDist = fC;
}
else
{
fS = -fB0/fA00;
fSqrDist = fB0*fS+fC;
}
}
}
else // region 1
{
fNumer = fA11 + fB1 - fA01 - fB0;
if ( fNumer <= 0.0f )
{
fS = 0.0f;
fT = 1.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fDenom = fA00-2.0f*fA01+fA11;
if ( fNumer >= fDenom )
{
fS = 1.0f;
fT = 0.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = fNumer/fDenom;
fT = 1.0f - fS;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
}
}
if ( pfSParam )
*pfSParam = fS;
if ( pfTParam )
*pfTParam = fT;
return Math::FAbs(fSqrDist);
}
bool Wml::FindIntersection (const Triangle3<Real>& rkTri,
const Vector3<Real>& rkTriVelocity, const Sphere3<Real>& rkSphere,
const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
int& riQuantity, Vector3<Real> akP[6])
{
// triangle vertices
Vector3<Real> akV[3] =
{
rkTri.Origin(),
rkTri.Origin() + rkTri.Edge0(),
rkTri.Origin() + rkTri.Edge1()
};
// triangle edges
Vector3<Real> akE[3] =
{
akV[1] - akV[0],
akV[2] - akV[1],
akV[0] - akV[2]
};
// triangle normal
Vector3<Real> kN = akE[1].Cross(akE[0]);
// sphere center projection on triangle normal
Real fNdC = kN.Dot(rkSphere.Center());
// Radius projected length in normal direction. This defers the square
// root to normalize kN until absolutely needed.
Real fNormRadiusSqr =
kN.SquaredLength()*rkSphere.Radius()*rkSphere.Radius();
// triangle projection on triangle normal
Real fNdT = kN.Dot(akV[0]);
// Distance from sphere to triangle along the normal
Real fDist = fNdC - fNdT;
// normals for the plane formed by edge i and the triangle normal
Vector3<Real> akExN[3] =
{
akE[0].Cross(kN),
akE[1].Cross(kN),
akE[2].Cross(kN)
};
Segment3<Real> kSeg;
if ( fDist*fDist <= fNormRadiusSqr )
{
// sphere currently intersects the plane of the triangle
// see which edges the sphere center is inside/outside of
bool bInside[3];
for (int i = 0; i < 3; i++ )
{
bInside[i] = ( akExN[i].Dot(rkSphere.Center()) >=
akExN[i].Dot(akV[i]) );
}
if ( bInside[0] )
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// triangle inside sphere
return false;
}
else // !bInside[2]
{
// potential intersection with edge 2
kSeg.Origin() = akV[2];
kSeg.Direction() = akE[2];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with edge 1
kSeg.Origin() = akV[1];
kSeg.Direction() = akE[1];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with edges 1,2
return FindTriSphrCoplanarIntersection(2,akV,kN,akExN[2],
akE[2],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
fTMax,riQuantity,akP);
}
}
}
else // !bInside[0]
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// potential intersection with edge 0
kSeg.Origin() = akV[0];
kSeg.Direction() = akE[0];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with edges 2,0
return FindTriSphrCoplanarIntersection(0,akV,kN,akExN[0],
akE[0],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
fTMax,riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with edges 0,1
return FindTriSphrCoplanarIntersection(1,akV,kN,akExN[1],
akE[1],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// we should not get here
assert( false );
return false;
}
}
}
}
else
{
// sphere does not currently intersect the plane of the triangle
// sphere moving, triangle stationary
Vector3<Real> kVel = rkSphVelocity - rkTriVelocity;
// Find point of intersection of the sphere and the triangle
// plane. Where this point occurs on the plane relative to the
// triangle determines the potential kind of intersection.
kN.Normalize();
// Point on sphere we care about intersecting the triangle plane
Vector3<Real> kSpherePoint;
// Which side of the triangle is the sphere on?
if ( fNdC > fNdT )
{
// positive side
if ( kVel.Dot(kN) >= (Real)0.0 )
{
// moving away, easy out
return false;
}
kSpherePoint = rkSphere.Center() - rkSphere.Radius()*kN;
}
else
{
// negative side
if ( kVel.Dot(kN) <= (Real)0.0 )
{
// moving away, easy out
return false;
}
kSpherePoint = rkSphere.Center() + rkSphere.Radius()*kN;
}
// find intersection of velocity ray and triangle plane
// project ray and plane onto the plane normal
Real fPlane = kN.Dot(akV[0]);
Real fPoint = kN.Dot(kSpherePoint);
Real fVel = kN.Dot(kVel);
Real fTime = (fPlane - fPoint)/fVel;
// where this intersects
Vector3<Real> kIntrPoint = kSpherePoint + fTime*kVel;
// see which edges this intersection point is inside/outside of
bool bInside[3];
for (int i = 0; i < 3; i++ )
bInside[i] = (akExN[i].Dot(kIntrPoint) >= akExN[i].Dot(akV[i]));
if ( bInside[0] )
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// intersects face at time fTime
if ( fTime > fTMax )
{
// intersection after tMax
return false;
}
else
{
rfTFirst = fTime;
riQuantity = 1;
// kIntrPoint is the point in space, assuming that
// TriVel is 0. Re-adjust the point to where it
// should be, were it not.
akP[0] = kIntrPoint + fTime*rkTriVelocity;
return true;
}
}
else // !bInside[2]
{
// potential intersection with edge 2
kSeg.Origin() = akV[2];
kSeg.Direction() = akE[2];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with edge 1
kSeg.Origin() = akV[1];
kSeg.Direction() = akE[1];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with vertex 2
return FindSphereVertexIntersection(akV[2],rkSphere,
rkSphVelocity,rkTriVelocity,rfTFirst,fTMax,
riQuantity,akP);
}
}
}
else // !bInside[0]
{
if ( bInside[1] )
{
if ( bInside[2] )
{
// potential intersection with edge 0
kSeg.Origin() = akV[0];
kSeg.Direction() = akE[0];
return FindIntersection(kSeg,rkTriVelocity,rkSphere,
rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
}
else // !bInside[2]
{
// potential intersection with vertex 0
return FindSphereVertexIntersection(akV[0],rkSphere,
rkSphVelocity,rkTriVelocity,rfTFirst,fTMax,
riQuantity,akP);
}
}
else // !bInside[1]
{
if ( bInside[2] )
{
// potential intersection with vertex 1
return FindSphereVertexIntersection(akV[1],rkSphere,
rkSphVelocity,rkTriVelocity,rfTFirst,fTMax,
riQuantity,akP);
}
else // !bInside[2]
{
// we should not get here
assert( false );
return false;
}
}
}
}
}
