From d48ea5bb797037069d641da41da0f195f0124491 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:48 +0000 Subject: one more for the gipper --- .../ode-0.9/ode/src/collision_trimesh_distance.cpp | 1255 ++++++++++++++++++++ 1 file changed, 1255 insertions(+) create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp') diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp new file mode 100644 index 0000000..717c763 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp @@ -0,0 +1,1255 @@ +// This file contains some code based on the code from Magic Software. +// That code is available under a Free Source License Agreement +// that can be found at http://www.magic-software.com/License/free.pdf + +#include +#include +#include +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +//------------------------------------------------------------------------------ +/** + @brief Finds the shortest distance squared between a point and a triangle. + + @param pfSParam Barycentric coordinate of triangle at point closest to p (u) + @param pfTParam Barycentric coordinate of triangle at point closest to p (v) + @return Shortest distance squared. + + The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v + + Taken from: + Magic Software, Inc. + http://www.magic-software.com +*/ +dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, + const dVector3 triEdge0, const dVector3 triEdge1, + dReal* pfSParam, dReal* pfTParam ) +{ + dVector3 kDiff; + Vector3Subtract( triOrigin, p, kDiff ); + dReal fA00 = dDOT( triEdge0, triEdge0 ); + dReal fA01 = dDOT( triEdge0, triEdge1 ); + dReal fA11 = dDOT( triEdge1, triEdge1 ); + dReal fB0 = dDOT( kDiff, triEdge0 ); + dReal fB1 = dDOT( kDiff, triEdge1 ); + dReal fC = dDOT( kDiff, kDiff ); + dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01)); + dReal fS = fA01*fB1-fA11*fB0; + dReal fT = fA01*fB0-fA00*fB1; + dReal fSqrDist; + + if ( fS + fT <= fDet ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4 + { + if ( fB0 < REAL(0.0) ) + { + fT = REAL(0.0); + if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + else // region 3 + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + else if ( fT < REAL(0.0) ) // region 5 + { + fT = REAL(0.0); + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else // region 0 + { + // minimum at interior point + if ( fDet == REAL(0.0) ) + { + fS = REAL(0.0); + fT = REAL(0.0); + fSqrDist = dInfinity; + } + else + { + dReal fInvDet = REAL(1.0)/fDet; + fS *= fInvDet; + fT *= fInvDet; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + } + else + { + dReal fTmp0, fTmp1, fNumer, fDenom; + + if ( fS < REAL(0.0) ) // region 2 + { + fTmp0 = fA01 + fB0; + fTmp1 = fA11 + fB1; + if ( fTmp1 > fTmp0 ) + { + fNumer = fTmp1 - fTmp0; + fDenom = fA00-REAL(2.0)*fA01+fA11; + if ( fNumer >= fDenom ) + { + fS = REAL(1.0); + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = fNumer/fDenom; + fT = REAL(1.0) - fS; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + else + { + fS = REAL(0.0); + if ( fTmp1 <= REAL(0.0) ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + else if ( fT < REAL(0.0) ) // region 6 + { + fTmp0 = fA01 + fB1; + fTmp1 = fA00 + fB0; + if ( fTmp1 > fTmp0 ) + { + fNumer = fTmp1 - fTmp0; + fDenom = fA00-REAL(2.0)*fA01+fA11; + if ( fNumer >= fDenom ) + { + fT = REAL(1.0); + fS = REAL(0.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = fNumer/fDenom; + fS = REAL(1.0) - fT; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + else + { + fT = REAL(0.0); + if ( fTmp1 <= REAL(0.0) ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + } + else // region 1 + { + fNumer = fA11 + fB1 - fA01 - fB0; + if ( fNumer <= REAL(0.0) ) + { + fS = REAL(0.0); + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fDenom = fA00-REAL(2.0)*fA01+fA11; + if ( fNumer >= fDenom ) + { + fS = REAL(1.0); + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = fNumer/fDenom; + fT = REAL(1.0) - fS; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + } + } + + if ( pfSParam ) + *pfSParam = (float)fS; + + if ( pfTParam ) + *pfTParam = (float)fT; + + return dReal(fabs(fSqrDist)); +} + +//------------------------------------------------------------------------------ +/** + @brief Finds the shortest distance squared between two line segments. + @param pfSegP0 t value for seg1 where the shortest distance between + the segments exists. + @param pfSegP0 t value for seg2 where the shortest distance between + the segments exists. + @return Shortest distance squared. + + Taken from: + Magic Software, Inc. + http://www.magic-software.com +*/ +dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, + const dVector3 seg2Origin, const dVector3 seg2Direction, + dReal* pfSegP0, dReal* pfSegP1 ) +{ + const dReal gs_fTolerance = 1e-05f; + dVector3 kDiff, kNegDiff, seg1NegDirection; + Vector3Subtract( seg1Origin, seg2Origin, kDiff ); + Vector3Negate( kDiff, kNegDiff ); + dReal fA00 = dDOT( seg1Direction, seg1Direction ); + Vector3Negate( seg1Direction, seg1NegDirection ); + dReal fA01 = dDOT( seg1NegDirection, seg2Direction ); + dReal fA11 = dDOT( seg2Direction, seg2Direction ); + dReal fB0 = dDOT( kDiff, seg1Direction ); + dReal fC = dDOT( kDiff, kDiff ); + dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01)); + dReal fB1, fS, fT, fSqrDist, fTmp; + + if ( fDet >= gs_fTolerance ) + { + // line segments are not parallel + fB1 = dDOT( kNegDiff, seg2Direction ); + fS = fA01*fB1-fA11*fB0; + fT = fA01*fB0-fA00*fB1; + + if ( fS >= REAL(0.0) ) + { + if ( fS <= fDet ) + { + if ( fT >= REAL(0.0) ) + { + if ( fT <= fDet ) // region 0 (interior) + { + // minimum at two interior points of 3D lines + dReal fInvDet = REAL(1.0)/fDet; + fS *= fInvDet; + fT *= fInvDet; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + else // region 3 (side) + { + fT = REAL(1.0); + fTmp = fA01+fB0; + if ( fTmp >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else if ( -fTmp >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp); + } + else + { + fS = -fTmp/fA00; + fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; + } + } + } + else // region 7 (side) + { + fT = REAL(0.0); + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + } + else + { + if ( fT >= REAL(0.0) ) + { + if ( fT <= fDet ) // region 1 (side) + { + fS = REAL(1.0); + fTmp = fA01+fB1; + if ( fTmp >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( -fTmp >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); + } + else + { + fT = -fTmp/fA11; + fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; + } + } + else // region 2 (corner) + { + fTmp = fA01+fB0; + if ( -fTmp <= fA00 ) + { + fT = REAL(1.0); + if ( fTmp >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fS = -fTmp/fA00; + fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; + } + } + else + { + fS = REAL(1.0); + fTmp = fA01+fB1; + if ( fTmp >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( -fTmp >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); + } + else + { + fT = -fTmp/fA11; + fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; + } + } + } + } + else // region 8 (corner) + { + if ( -fB0 < fA00 ) + { + fT = REAL(0.0); + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else + { + fS = REAL(1.0); + fTmp = fA01+fB1; + if ( fTmp >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( -fTmp >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); + } + else + { + fT = -fTmp/fA11; + fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; + } + } + } + } + } + else + { + if ( fT >= REAL(0.0) ) + { + if ( fT <= fDet ) // region 5 (side) + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + else // region 4 (corner) + { + fTmp = fA01+fB0; + if ( fTmp < REAL(0.0) ) + { + fT = REAL(1.0); + if ( -fTmp >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp); + } + else + { + fS = -fTmp/fA00; + fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; + } + } + else + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + } + else // region 6 (corner) + { + if ( fB0 < REAL(0.0) ) + { + fT = REAL(0.0); + if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + } + } + else + { + // line segments are parallel + if ( fA01 > REAL(0.0) ) + { + // direction vectors form an obtuse angle + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB0 <= fA00 ) + { + fS = -fB0/fA00; + fT = REAL(0.0); + fSqrDist = fB0*fS+fC; + } + else + { + //fB1 = -kDiff % seg2.m; + fB1 = dDOT( kNegDiff, seg2Direction ); + fS = REAL(1.0); + fTmp = fA00+fB0; + if ( -fTmp >= fA01 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fA01+fB0+fB1); + } + else + { + fT = -fTmp/fA01; + fSqrDist = fA00+REAL(2.0)*fB0+fC+fT*(fA11*fT+REAL(2.0)*(fA01+fB1)); + } + } + } + else + { + // direction vectors form an acute angle + if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( fB0 <= REAL(0.0) ) + { + fS = -fB0/fA00; + fT = REAL(0.0); + fSqrDist = fB0*fS+fC; + } + else + { + fB1 = dDOT( kNegDiff, seg2Direction ); + fS = REAL(0.0); + if ( fB0 >= -fA01 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB0/fA01; + fSqrDist = fC+fT*(REAL(2.0)*fB1+fA11*fT); + } + } + } + } + + if ( pfSegP0 ) + *pfSegP0 = fS; + + if ( pfSegP1 ) + *pfSegP1 = fT; + + return dReal(fabs(fSqrDist)); +} + +//------------------------------------------------------------------------------ +/** + @brief Finds the shortest distance squared between a line segment and + a triangle. + + @param pfSegP t value for the line segment where the shortest distance between + the segment and the triangle occurs. + So the point along the segment that is the shortest distance + away from the triangle can be obtained by (seg.end - seg.start) * t. + @param pfTriP0 Barycentric coordinate of triangle at point closest to seg (u) + @param pfTriP1 Barycentric coordinate of triangle at point closest to seg (v) + @return Shortest distance squared. + + The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v + + Taken from: + Magic Software, Inc. + http://www.magic-software.com +*/ +dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, + const dVector3 triOrigin, + const dVector3 triEdge0, const dVector3 triEdge1, + dReal* pfSegP, dReal* pfTriP0, dReal* pfTriP1 ) +{ + const dReal gs_fTolerance = 1e-06f; + dVector3 segDirection, segNegDirection, kDiff, kNegDiff; + Vector3Subtract( segEnd, segOrigin, segDirection ); + Vector3Negate( segDirection, segNegDirection ); + Vector3Subtract( triOrigin, segOrigin, kDiff ); + Vector3Negate( kDiff, kNegDiff ); + dReal fA00 = dDOT( segDirection, segDirection ); + dReal fA01 = dDOT( segNegDirection, triEdge0 ); + dReal fA02 = dDOT( segNegDirection, triEdge1 ); + dReal fA11 = dDOT( triEdge0, triEdge0 ); + dReal fA12 = dDOT( triEdge0, triEdge1 ); + dReal fA22 = dDOT( triEdge1, triEdge1 ); + dReal fB0 = dDOT( kNegDiff, segDirection ); + dReal fB1 = dDOT( kDiff, triEdge0 ); + dReal fB2 = dDOT( kDiff, triEdge1 ); + + dVector3 kTriSegOrigin, kTriSegDirection, kPt; + dReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0; + + // Set up for a relative error test on the angle between ray direction + // and triangle normal to determine parallel/nonparallel status. + dVector3 kN; + dCROSS( kN, =, triEdge0, triEdge1 ); + dReal fNSqrLen = dDOT( kN, kN ); + dReal fDot = dDOT( segDirection, kN ); + bool bNotParallel = (fDot*fDot >= gs_fTolerance*fA00*fNSqrLen); + + if ( bNotParallel ) + { + dReal fCof00 = fA11*fA22-fA12*fA12; + dReal fCof01 = fA02*fA12-fA01*fA22; + dReal fCof02 = fA01*fA12-fA02*fA11; + dReal fCof11 = fA00*fA22-fA02*fA02; + dReal fCof12 = fA02*fA01-fA00*fA12; + dReal fCof22 = fA00*fA11-fA01*fA01; + dReal fInvDet = REAL(1.0)/(fA00*fCof00+fA01*fCof01+fA02*fCof02); + dReal fRhs0 = -fB0*fInvDet; + dReal fRhs1 = -fB1*fInvDet; + dReal fRhs2 = -fB2*fInvDet; + + fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2; + fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2; + fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2; + + if ( fR < REAL(0.0) ) + { + if ( fS+fT <= REAL(1.0) ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4m + { + // min on face s=0 or t=0 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fS0 ); + fT0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 3m + { + // min on face s=0 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR,&fT ); + fS = REAL(0.0); + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + else if ( fT < REAL(0.0) ) // region 5m + { + // min on face t=0 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 0m + { + // min on face r=0 + fSqrDist = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS, &fT ); + fR = REAL(0.0); + } + } + else + { + if ( fS < REAL(0.0) ) // region 2m + { + // min on face s=0 or s+t=1 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else if ( fT < REAL(0.0) ) // region 6m + { + // min on face t=0 or s+t=1 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 1m + { + // min on face s+t=1 or r=0 + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(1.0) - fT; + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + } + else if ( fR <= REAL(1.0) ) + { + if ( fS+fT <= REAL(1.0) ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4 + { + // min on face s=0 or t=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fS0 ); + fT0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 3 + { + // min on face s=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + } + } + else if ( fT < REAL(0.0) ) // region 5 + { + // min on face t=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + } + else // region 0 + { + // global minimum is interior, done + fSqrDist = REAL(0.0); + } + } + else + { + if ( fS < REAL(0.0) ) // region 2 + { + // min on face s=0 or s+t=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else if ( fT < REAL(0.0) ) // region 6 + { + // min on face t=0 or s+t=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 1 + { + // min on face s+t=1 + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(1.0) - fT; + } + } + } + else // fR > 1 + { + if ( fS+fT <= REAL(1.0) ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4p + { + // min on face s=0 or t=0 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fS0 ); + fT0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 3p + { + // min on face s=0 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + else if ( fT < REAL(0.0) ) // region 5p + { + // min on face t=0 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 0p + { + // min face on r=1 + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS, &fT ); + fR = REAL(1.0); + } + } + else + { + if ( fS < REAL(0.0) ) // region 2p + { + // min on face s=0 or s+t=1 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else if ( fT < REAL(0.0) ) // region 6p + { + // min on face t=0 or s+t=1 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 1p + { + // min on face s+t=1 or r=1 + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(1.0) - fT; + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + } + } + else + { + // segment and triangle are parallel + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, &fR, &fS ); + fT = REAL(0.0); + + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + + if ( pfSegP ) + *pfSegP = fR; + + if ( pfTriP0 ) + *pfTriP0 = fS; + + if ( pfTriP1 ) + *pfTriP1 = fT; + + return fSqrDist; +} -- cgit v1.1