From fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:07 +0000 Subject: dont ask --- .../ode-0.9/ode/src/collision_trimesh_distance.cpp | 1255 -------------------- 1 file changed, 1255 deletions(-) delete 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 deleted file mode 100644 index 717c763..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp +++ /dev/null @@ -1,1255 +0,0 @@ -// 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