1563 lines
44 KiB
C++
1563 lines
44 KiB
C++
#include "Stdafx.h"
|
|
#include "EtComputeDist.h"
|
|
|
|
#ifdef _DEBUG
|
|
#define new new(_NORMAL_BLOCK,__FILE__,__LINE__)
|
|
#endif
|
|
|
|
// 한 직선과 한 점을 가지고, CCW인지 CW인지를 판단한다. 음수면 CCW. 같은 선상에 있으면 0리턴.
|
|
float CCW( EtVector2 &Line1, EtVector2 &Line2, EtVector2 &Point )
|
|
{
|
|
return ((Line1.x - Point.x) * (Line2.y - Point.y)) - ((Line1.y - Point.y) * (Line2.x - Point.x));
|
|
}
|
|
|
|
// 교차할때만 true, 그 외 나머지 경우 false
|
|
bool IntrLine2DToLine2D( EtVector2 &Line1, EtVector2 &Line2, EtVector2 &Line3, EtVector2 &Line4 )
|
|
{
|
|
if( (CCW(Line1, Line2, Line3) * CCW(Line1, Line2, Line4)) < 0 && (CCW(Line3, Line4, Line1) * CCW(Line3, Line4, Line2)) < 0 )
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
// 와일드매직 코드를 보면 DistSegmentToSegment에 대응되는 2D용 선분 거리 검사코드가 있긴 한데,
|
|
// 직관적인 코드로 짜보려고, 직접 만들어본다. PressDir의 경우 필요없으면 사용하지 않아도 무방함.
|
|
float DistLine2DToLine2D( EtVector2 &Line1, EtVector2 &Line2, EtVector2 &Line3, EtVector2 &Line4, EtVector2 &PressDir )
|
|
{
|
|
// 두 선분이 교차하면 0.0f 리턴
|
|
if( IntrLine2DToLine2D(Line1, Line2, Line3, Line4) )
|
|
return 0.0f;
|
|
else
|
|
{
|
|
// 두 선분 각각 선분과 양 끝점의 거리를 재서 가장 짧은 것을 리턴.
|
|
EtVector2 vDir1, vDir2, vDir3, vDir4;
|
|
float fDist1 = DistPointToLine2D( Line3, Line1, Line2, vDir1 );
|
|
float fDist2 = DistPointToLine2D( Line4, Line1, Line2, vDir2 );
|
|
// 거리가 짧은 것을 기억.
|
|
if( fDist1 > fDist2 ) {
|
|
vDir1 = vDir2;
|
|
fDist1 = fDist2;
|
|
}
|
|
|
|
// 타겟선분에서 더 짧은게 나온다면 PressDir을 반대로 해준다.
|
|
float fDist3 = DistPointToLine2D( Line1, Line3, Line4, vDir3 );
|
|
float fDist4 = DistPointToLine2D( Line2, Line3, Line4, vDir4 );
|
|
if( fDist3 > fDist4 ) {
|
|
vDir3 = vDir4;
|
|
fDist3 = fDist4;
|
|
}
|
|
vDir3 *= -1.0f;
|
|
|
|
// 거리가 가장 짧은 것을 리턴.
|
|
if( fDist1 > fDist3 ) {
|
|
vDir1 = vDir3;
|
|
fDist1 = fDist3;
|
|
}
|
|
PressDir = vDir1;
|
|
return fDist1;
|
|
}
|
|
}
|
|
|
|
float DistPointToLine2D( EtVector2 &Point, EtVector2 &Line1, EtVector2 &Line2, EtVector2 &PressDir )
|
|
{
|
|
EtVector2 vDiff;
|
|
EtVector2 vOrig = Line1;
|
|
EtVector2 vDir = Line2 - Line1;
|
|
float fDot;
|
|
|
|
vDiff = Point - vOrig;
|
|
fDot = EtVec2Dot( &vDir, &vDiff );
|
|
|
|
if(fDot <= 0.0f)
|
|
{
|
|
fDot = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
float fLengthSq;
|
|
|
|
fLengthSq = EtVec2LengthSq( &vDir );
|
|
if( fDot >= fLengthSq )
|
|
{
|
|
fDot = 1.0f;
|
|
vDiff -= vDir;
|
|
}
|
|
else
|
|
{
|
|
fDot /= fLengthSq;
|
|
vDiff -= fDot * vDir;
|
|
}
|
|
}
|
|
EtVec2Normalize( &PressDir, &vDiff );
|
|
return EtVec2Length( &vDiff );
|
|
}
|
|
|
|
float DistPointToSegment( EtVector3 &Point, SSegment &Segment, float &fSegParam )
|
|
{
|
|
EtVector3 Diff;
|
|
|
|
Diff = Point - Segment.vOrigin;
|
|
fSegParam = EtVec3Dot( &Segment.vDirection, &Diff );
|
|
|
|
if(fSegParam <= 0.0f)
|
|
{
|
|
fSegParam = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
float fLengthSq;
|
|
|
|
fLengthSq = EtVec3LengthSq( &Segment.vDirection );
|
|
if( fSegParam >= fLengthSq )
|
|
{
|
|
fSegParam = 1.0f;
|
|
Diff -= Segment.vDirection;
|
|
}
|
|
else
|
|
{
|
|
fSegParam /= fLengthSq;
|
|
Diff -= fSegParam * Segment.vDirection;
|
|
}
|
|
}
|
|
|
|
return EtVec3LengthSq( &Diff );
|
|
}
|
|
|
|
float DistPointToOBB( EtVector3 &Point, SCollisionBox &Box, float &fBoxParam0, float &fBoxParam1, float &fBoxParam2 )
|
|
{
|
|
float fSqrDistance = 0.0f;
|
|
|
|
// Compute the squared distance in the box's coordinate system.
|
|
EtVector3 Diff = Point - Box.vCenter, Closest;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
float fDelta;
|
|
Closest[ i ] = EtVec3Dot( &Diff, Box.vAxis + i );
|
|
if(Closest[ i ] < -Box.fExtent[ i ])
|
|
{
|
|
fDelta = Closest[ i ] + Box.fExtent[ i ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Closest[ i ] = -Box.fExtent[ i ];
|
|
}
|
|
else if(Closest[ i ] > Box.fExtent[ i ])
|
|
{
|
|
fDelta = Closest[ i ] - Box.fExtent[ i ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Closest[ i ] = Box.fExtent[ i ];
|
|
}
|
|
}
|
|
|
|
fBoxParam0 = Closest[ 0 ];
|
|
fBoxParam1 = Closest[ 1 ];
|
|
fBoxParam2 = Closest[ 2 ];
|
|
|
|
return fSqrDistance;
|
|
}
|
|
|
|
float DistSegmentToSegment( SSegment &Segment1, SSegment &Segment2, float &fSeg0Param, float &fSeg1Param )
|
|
{
|
|
const float fTolerance = 1e-06f;
|
|
float fTmp;
|
|
float fA, fB, fC, fD, fE, fF;
|
|
EtVector3 Diff;
|
|
|
|
Diff = Segment1.vOrigin - Segment2.vOrigin;
|
|
fA = EtVec3Dot( &Segment1.vDirection, &Segment1.vDirection );
|
|
fB = -EtVec3Dot( &Segment1.vDirection, &Segment2.vDirection );
|
|
fC = EtVec3Dot( &Segment2.vDirection, &Segment2.vDirection );
|
|
fD = EtVec3Dot( &Segment1.vDirection, &Diff );
|
|
fE = -EtVec3Dot( &Segment2.vDirection, &Diff );
|
|
fF = EtVec3Dot( &Diff, &Diff );
|
|
|
|
// fA*fC-fB*fB = |Cross(Dir0, Dir1)|^2 >= 0
|
|
float fDet;
|
|
EtVector3 Cross;
|
|
|
|
fDet = fabs( fA * fC - fB * fB );
|
|
EtVec3Cross( &Cross, &Segment1.vDirection, &Segment2.vDirection );
|
|
if( EtVec3LengthSq( &Cross ) > fTolerance )
|
|
{
|
|
// line segments are not parallel
|
|
fSeg0Param = fB * fE - fC * fD;
|
|
fSeg1Param = fB * fD - fA * fE;
|
|
|
|
if( fSeg0Param >= 0 )
|
|
{
|
|
if( fSeg0Param <= fDet )
|
|
{
|
|
if( fSeg1Param >= 0 )
|
|
{
|
|
if( fSeg1Param <= fDet ) // region 0 (interior)
|
|
{
|
|
// minimum at two interior points of 3D lines
|
|
float fInvDet;
|
|
|
|
fInvDet = 1.0f / fDet;
|
|
fSeg0Param *= fInvDet;
|
|
fSeg1Param *= fInvDet;
|
|
return fabs( fSeg0Param * ( fA * fSeg0Param + fB * fSeg1Param + 2 * fD ) +
|
|
fSeg1Param * (fB * fSeg0Param + fC * fSeg1Param + 2 * fE ) + fF);
|
|
}
|
|
else // region 3 (side)
|
|
{
|
|
fSeg1Param = 1;
|
|
fTmp = fB + fD;
|
|
if( fTmp >= 0 )
|
|
{
|
|
fSeg0Param = 0;
|
|
return fabs( fC + 2 * fE + fF );
|
|
}
|
|
else if( -fTmp >= fA )
|
|
{
|
|
fSeg0Param = 1;
|
|
return fabs( fA + fC + fF + 2 * ( fE + fTmp ) );
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = -fTmp / fA;
|
|
return fabs( fTmp * fSeg0Param + fC + 2 * fE + fF );
|
|
}
|
|
}
|
|
}
|
|
else // region 7 (side)
|
|
{
|
|
fSeg1Param = 0;
|
|
if( fD >= 0 )
|
|
{
|
|
fSeg0Param = 0;
|
|
return fabs( fF );
|
|
}
|
|
else if( -fD >= fA )
|
|
{
|
|
fSeg0Param = 1;
|
|
return fabs( fA + 2 * fD + fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = -fD / fA;
|
|
return fabs( fD * fSeg0Param + fF );
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( fSeg1Param >= 0 )
|
|
{
|
|
if( fSeg1Param <= fDet ) // region 1 (side)
|
|
{
|
|
fSeg0Param = 1;
|
|
fTmp = fB + fE;
|
|
if( fTmp >= 0 )
|
|
{
|
|
fSeg1Param = 0;
|
|
return fabs( fA + 2 * fD + fF );
|
|
}
|
|
else if( -fTmp >= fC )
|
|
{
|
|
fSeg1Param = 1;
|
|
return fabs( fA + fC + fF + 2 * ( fD + fTmp ) );
|
|
}
|
|
else
|
|
{
|
|
fSeg1Param = -fTmp / fC;
|
|
return fabs( fTmp * fSeg1Param + fA + 2 * fD + fF );
|
|
}
|
|
}
|
|
else // region 2 (corner)
|
|
{
|
|
fTmp = fB + fD;
|
|
if( -fTmp <= fA )
|
|
{
|
|
fSeg1Param = 1;
|
|
if( fTmp >= 0 )
|
|
{
|
|
fSeg0Param = 0;
|
|
return fabs( fC + 2 * fE + fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = -fTmp / fA;
|
|
return fabs( fTmp * fSeg0Param + fC + 2 * fE + fF );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = 1;
|
|
fTmp = fB + fE;
|
|
if( -fTmp >= fC )
|
|
{
|
|
fSeg1Param = 1;
|
|
return fabs( fA + fC + fF + 2 * ( fD + fTmp ) );
|
|
}
|
|
else if( fTmp > 0 )
|
|
{
|
|
fSeg1Param = 0;
|
|
return fabs( fA + 2 * fD + fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg1Param = -fTmp / fC;
|
|
return fabs( fTmp * fSeg1Param + fA + 2 * fD + fF );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else // region 8 (corner)
|
|
{
|
|
if( -fD < fA )
|
|
{
|
|
fSeg1Param = 0;
|
|
if( fD >= 0 )
|
|
{
|
|
fSeg0Param = 0;
|
|
return fabs( fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = -fD / fA;
|
|
return fabs( fD * fSeg0Param + fF );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = 1;
|
|
fTmp = fB + fE;
|
|
if( fTmp >= 0 )
|
|
{
|
|
fSeg1Param = 0;
|
|
return fabs( fA + 2 * fD + fF );
|
|
}
|
|
else if(-fTmp >= fC)
|
|
{
|
|
fSeg1Param = 1;
|
|
return fabs( fA + fC + fF + 2 * ( fD + fTmp ) );
|
|
}
|
|
else
|
|
{
|
|
fSeg1Param = -fTmp / fC;
|
|
return fabs( fTmp * fSeg1Param + fA + 2 * fD + fF );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( fSeg1Param >= 0 )
|
|
{
|
|
if( fSeg1Param <= fDet ) // region 5 (side)
|
|
{
|
|
fSeg0Param = 0;
|
|
if( fE >= 0 )
|
|
{
|
|
fSeg1Param = 0;
|
|
return fabs( fF );
|
|
}
|
|
else if( -fE >= fC )
|
|
{
|
|
fSeg1Param = 1;
|
|
return fabs( fC + 2 * fE + fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg1Param = -fE / fC;
|
|
return fabs( fE * fSeg1Param + fF );
|
|
}
|
|
}
|
|
else // region 4 (corner)
|
|
{
|
|
fTmp = fB + fD;
|
|
if( fTmp < 0 )
|
|
{
|
|
fSeg1Param = 1;
|
|
if( -fTmp >= fA )
|
|
{
|
|
fSeg0Param = 1;
|
|
return fabs( fA + fC + fF + 2 * ( fE + fTmp ) );
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = -fTmp / fA;
|
|
return fabs( fTmp * fSeg0Param + fC + 2 * fE + fF );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = 0;
|
|
fTmp = fC + fE;
|
|
if( fTmp <= 0 )
|
|
{
|
|
fSeg1Param = 1;
|
|
return fabs( fC + 2 * fE + fF );
|
|
}
|
|
else if( fE >= 0 )
|
|
{
|
|
fSeg1Param = 0;
|
|
return fabs( fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg1Param = -fE / fC;
|
|
return fabs( fE * fSeg1Param + fF );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else // region 6 (corner)
|
|
{
|
|
if( fD < 0 )
|
|
{
|
|
fSeg1Param = 0;
|
|
if( -fD >= fA )
|
|
{
|
|
fSeg0Param = 1;
|
|
return fabs( fA + 2 * fD + fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = -fD / fA;
|
|
return fabs( fD * fSeg0Param + fF );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = 0;
|
|
if( fE >= 0 )
|
|
{
|
|
fSeg1Param = 0;
|
|
return fabs( fF );
|
|
}
|
|
else if( -fE >= fC )
|
|
{
|
|
fSeg1Param = 1;
|
|
return fabs( fC + 2 * fE + fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg1Param = -fE / fC;
|
|
return fabs( fE * fSeg1Param + fF );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// line segments are parallel
|
|
if( fB > 0 )
|
|
{
|
|
// direction vectors form an obtuse angle
|
|
if( fD >= 0 )
|
|
{
|
|
fSeg0Param = 0;
|
|
fSeg1Param = 0;
|
|
return fabs( fF );
|
|
}
|
|
else if( -fD <= fA )
|
|
{
|
|
fSeg0Param = -fD / fA;
|
|
fSeg1Param = 0;
|
|
return fabs( fD * fSeg0Param + fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = 1;
|
|
fTmp = fA + fD;
|
|
if( -fTmp >= fB )
|
|
{
|
|
fSeg1Param = 1;
|
|
return fabs( fA + fC + fF + 2 * ( fB + fD + fE ) );
|
|
}
|
|
else
|
|
{
|
|
fSeg1Param = -fTmp / fB;
|
|
return fabs( fA + 2 * fD + fF + fSeg1Param * ( fC * fSeg1Param + 2 * ( fB + fE ) ) );
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// direction vectors form an acute angle
|
|
if( -fD >= fA )
|
|
{
|
|
fSeg0Param = 1;
|
|
fSeg1Param = 0;
|
|
return fabs( fA + 2 * fD + fF );
|
|
}
|
|
else if(fD <= 0)
|
|
{
|
|
fSeg0Param = -fD / fA;
|
|
fSeg1Param = 0;
|
|
return fabs( fD * fSeg0Param + fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg0Param = 0;
|
|
if( fD >= -fB )
|
|
{
|
|
fSeg1Param = 1;
|
|
return fabs( fC + 2 * fE + fF );
|
|
}
|
|
else
|
|
{
|
|
fSeg1Param = -fD / fB;
|
|
return fabs( fF + fSeg1Param * ( 2 * fE + fC * fSeg1Param ) );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
float DistSegToBox( SSegment &Segment, SCollisionBox &Box, float &fSegParam,
|
|
float &fBoxParam0, float &fBoxParam1, float &fBoxParam2 )
|
|
{
|
|
// compute coordinates of line in box coordinate system
|
|
EtVector3 Diff = Segment.vOrigin - Box.vCenter;
|
|
EtVector3 DiffDotAxis, Direction;
|
|
|
|
DiffDotAxis.x = EtVec3Dot( &Diff, Box.vAxis );
|
|
DiffDotAxis.y = EtVec3Dot( &Diff, Box.vAxis + 1 );
|
|
DiffDotAxis.z = EtVec3Dot( &Diff, Box.vAxis + 2 );
|
|
|
|
Direction.x = EtVec3Dot( &Segment.vDirection, Box.vAxis );
|
|
Direction.y = EtVec3Dot( &Segment.vDirection, Box.vAxis + 1 );
|
|
Direction.z = EtVec3Dot( &Segment.vDirection, Box.vAxis + 2 );
|
|
|
|
// Apply reflections so that direction vector has nonnegative components.
|
|
int i;
|
|
bool bReflect[ 3 ];
|
|
for ( i = 0; i < 3; i++ )
|
|
{
|
|
if( Direction[ i ] < 0.0f )
|
|
{
|
|
DiffDotAxis[ i ] = -DiffDotAxis[ i ];
|
|
Direction[ i ] = -Direction[ i ];
|
|
bReflect[ i ] = true;
|
|
}
|
|
else
|
|
{
|
|
bReflect[ i ] = false;
|
|
}
|
|
}
|
|
|
|
float fSqrDistance = 0.0f;
|
|
fSegParam = 0.0f; // parameter for closest point on line
|
|
|
|
if( Direction.x > 0.0f )
|
|
{
|
|
if( Direction.y > 0.0f )
|
|
{
|
|
if( Direction.z > 0.0f ) // (+,+,+)
|
|
{
|
|
CaseNoZeros( Box, DiffDotAxis, Direction, fSegParam, fSqrDistance );
|
|
}
|
|
else // (+,+,0)
|
|
{
|
|
Case0( Box, 0, 1, 2, DiffDotAxis, Direction, fSegParam, fSqrDistance );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( Direction.z > 0.0f ) // (+,0,+)
|
|
{
|
|
Case0( Box, 0, 2, 1, DiffDotAxis, Direction, fSegParam, fSqrDistance );
|
|
}
|
|
else // (+,0,0)
|
|
{
|
|
Case00( Box, 0, 1, 2, DiffDotAxis, Direction, fSegParam, fSqrDistance );
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( Direction.y > 0.0f )
|
|
{
|
|
if( Direction.z > 0.0f ) // (0,+,+)
|
|
{
|
|
Case0( Box, 1, 2, 0, DiffDotAxis, Direction, fSegParam, fSqrDistance );
|
|
}
|
|
else // (0,+,0)
|
|
{
|
|
Case00( Box, 1, 0, 2, DiffDotAxis, Direction, fSegParam, fSqrDistance );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( Direction.z > 0.0f ) // (0,0,+)
|
|
{
|
|
Case00( Box, 2, 0, 1, DiffDotAxis, Direction, fSegParam, fSqrDistance );
|
|
}
|
|
else // (0,0,0)
|
|
{
|
|
Case000( Box, DiffDotAxis, fSqrDistance );
|
|
}
|
|
}
|
|
}
|
|
|
|
if( fSegParam >= 0.0f )
|
|
{
|
|
if( fSegParam <= 1.0f )
|
|
{
|
|
for ( i = 0; i < 3; i++ )
|
|
{
|
|
// undo the reflections applied previously
|
|
if( bReflect[ i ] )
|
|
{
|
|
DiffDotAxis[ i ] = -DiffDotAxis[ i ];
|
|
}
|
|
}
|
|
fBoxParam0 = DiffDotAxis[ 0 ];
|
|
fBoxParam1 = DiffDotAxis[ 1 ];
|
|
fBoxParam2 = DiffDotAxis[ 2 ];
|
|
}
|
|
else
|
|
{
|
|
fSegParam = 1.0f;
|
|
fSqrDistance = DistPointToOBB( Segment.vOrigin + Segment.vDirection, Box, fBoxParam0, fBoxParam1, fBoxParam2 );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fSegParam = 0.0f;
|
|
fSqrDistance = DistPointToOBB( Segment.vOrigin, Box, fBoxParam0, fBoxParam1, fBoxParam2 );
|
|
}
|
|
|
|
return fSqrDistance;
|
|
}
|
|
|
|
void Face( SCollisionBox &Box, int i0, int i1, int i2, EtVector3 &Point, EtVector3 &Direction, EtVector3 &PmE,
|
|
float &fLParam, float &fSqrDistance )
|
|
{
|
|
EtVector3 ExtentPoint;
|
|
float fLSqr, fInv, fTmp, fParam, fT, fDelta;
|
|
|
|
ExtentPoint[ i1 ] = Point[ i1 ] + Box.fExtent[ i1 ];
|
|
ExtentPoint[ i2 ] = Point[ i2 ] + Box.fExtent[ i2 ];
|
|
if( Direction[ i0 ] * ExtentPoint[ i1 ] >= Direction[ i1 ] * PmE[ i0 ] )
|
|
{
|
|
if( Direction[ i0 ] * ExtentPoint[ i2 ] >= Direction[ i2 ] * PmE[ i0 ] )
|
|
{
|
|
// v[ i1 ] >= -e[ i1 ], v[ i2 ] >= -e[ i2 ] (distance = 0)
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
fInv = 1.0f / Direction[ i0 ];
|
|
Point[ i1 ] -= Direction[ i1 ] * PmE[ i0 ] * fInv;
|
|
Point[ i2 ] -= Direction[ i2 ] * PmE[ i0 ] * fInv;
|
|
fLParam = -PmE[ i0 ] * fInv;
|
|
}
|
|
else
|
|
{
|
|
// v[ i1 ] >= -e[ i1 ], v[ i2 ] < -e[ i2 ]
|
|
fLSqr = Direction[ i0 ] * Direction[ i0 ] + Direction[ i2 ] * Direction[ i2 ];
|
|
fTmp = fLSqr * ExtentPoint[ i1 ] - Direction[ i1 ] * (Direction[ i0 ] * PmE[ i0 ] + Direction[ i2 ] * ExtentPoint[ i2 ]);
|
|
if( fTmp <= 2.0f * fLSqr * Box.fExtent[ i1 ] )
|
|
{
|
|
fT = fTmp / fLSqr;
|
|
fLSqr += Direction[ i1 ] * Direction[ i1 ];
|
|
fTmp = ExtentPoint[ i1 ] - fT;
|
|
fDelta = Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * fTmp + Direction[ i2 ] * ExtentPoint[ i2 ];
|
|
fParam = -fDelta / fLSqr;
|
|
fSqrDistance += PmE[ i0 ] * PmE[ i0 ] + fTmp * fTmp + ExtentPoint[ i2 ] * ExtentPoint[ i2 ] + fDelta * fParam;
|
|
|
|
fLParam = fParam;
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
Point[ i1 ] = fT - Box.fExtent[ i1 ];
|
|
Point[ i2 ] = -Box.fExtent[ i2 ];
|
|
}
|
|
else
|
|
{
|
|
fLSqr += Direction[ i1 ] * Direction[ i1 ];
|
|
fDelta = Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * PmE[ i1 ] + Direction[ i2 ] * ExtentPoint[ i2 ];
|
|
fParam = -fDelta / fLSqr;
|
|
fSqrDistance += PmE[ i0 ] * PmE[ i0 ] + PmE[ i1 ] * PmE[ i1 ] + ExtentPoint[ i2 ] * ExtentPoint[ i2 ] + fDelta * fParam;
|
|
|
|
fLParam = fParam;
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
Point[ i1 ] = Box.fExtent[ i1 ];
|
|
Point[ i2 ] = -Box.fExtent[ i2 ];
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( Direction[ i0 ] * ExtentPoint[ i2 ] >= Direction[ i2 ] * PmE[ i0 ] )
|
|
{
|
|
// v[ i1 ] < -e[ i1 ], v[ i2 ] >= -e[ i2 ]
|
|
fLSqr = Direction[ i0 ] * Direction[ i0 ] + Direction[ i1 ] * Direction[ i1 ];
|
|
fTmp = fLSqr * ExtentPoint[ i2 ] - Direction[ i2 ] * (Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * ExtentPoint[ i1 ]);
|
|
if( fTmp <= 2.0f * fLSqr * Box.fExtent[ i2 ] )
|
|
{
|
|
fT = fTmp / fLSqr;
|
|
fLSqr += Direction[ i2 ] * Direction[ i2 ];
|
|
fTmp = ExtentPoint[ i2 ] - fT;
|
|
fDelta = Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * ExtentPoint[ i1 ] + Direction[ i2 ] * fTmp;
|
|
fParam = -fDelta / fLSqr;
|
|
fSqrDistance += PmE[ i0 ] * PmE[ i0 ] + ExtentPoint[ i1 ] * ExtentPoint[ i1 ] + fTmp * fTmp + fDelta * fParam;
|
|
|
|
fLParam = fParam;
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
Point[ i1 ] = -Box.fExtent[ i1 ];
|
|
Point[ i2 ] = fT - Box.fExtent[ i2 ];
|
|
}
|
|
else
|
|
{
|
|
fLSqr += Direction[ i2 ] * Direction[ i2 ];
|
|
fDelta = Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * ExtentPoint[ i1 ] + Direction[ i2 ] * PmE[ i2 ];
|
|
fParam = -fDelta / fLSqr;
|
|
fSqrDistance += PmE[ i0 ] * PmE[ i0 ] + ExtentPoint[ i1 ] * ExtentPoint[ i1 ] + PmE[ i2 ] * PmE[ i2 ] + fDelta * fParam;
|
|
|
|
fLParam = fParam;
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
Point[ i1 ] = -Box.fExtent[ i1 ];
|
|
Point[ i2 ] = Box.fExtent[ i2 ];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// v[ i1 ] < -e[ i1 ], v[ i2 ] < -e[ i2 ]
|
|
fLSqr = Direction[ i0 ] * Direction[ i0 ]+Direction[ i2 ] * Direction[ i2 ];
|
|
fTmp = fLSqr * ExtentPoint[ i1 ] - Direction[ i1 ] * (Direction[ i0 ] * PmE[ i0 ] + Direction[ i2 ] * ExtentPoint[ i2 ]);
|
|
if( fTmp >= (float)0.0 )
|
|
{
|
|
// v[ i1 ]-edge is closest
|
|
if( fTmp <= 2.0f * fLSqr * Box.fExtent[ i1 ] )
|
|
{
|
|
fT = fTmp / fLSqr;
|
|
fLSqr += Direction[ i1 ] * Direction[ i1 ];
|
|
fTmp = ExtentPoint[ i1 ] - fT;
|
|
fDelta = Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * fTmp + Direction[ i2 ] * ExtentPoint[ i2 ];
|
|
fParam = -fDelta / fLSqr;
|
|
fSqrDistance += PmE[ i0 ] * PmE[ i0 ] + fTmp * fTmp + ExtentPoint[ i2 ] * ExtentPoint[ i2 ] + fDelta * fParam;
|
|
|
|
fLParam = fParam;
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
Point[ i1 ] = fT - Box.fExtent[ i1 ];
|
|
Point[ i2 ] = -Box.fExtent[ i2 ];
|
|
}
|
|
else
|
|
{
|
|
fLSqr += Direction[ i1 ] * Direction[ i1 ];
|
|
fDelta = Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * PmE[ i1 ] + Direction[ i2 ] * ExtentPoint[ i2 ];
|
|
fParam = -fDelta / fLSqr;
|
|
fSqrDistance += PmE[ i0 ] * PmE[ i0 ] + PmE[ i1 ] * PmE[ i1 ] + ExtentPoint[ i2 ] * ExtentPoint[ i2 ] + fDelta * fParam;
|
|
|
|
fLParam = fParam;
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
Point[ i1 ] = Box.fExtent[ i1 ];
|
|
Point[ i2 ] = -Box.fExtent[ i2 ];
|
|
}
|
|
return;
|
|
}
|
|
|
|
fLSqr = Direction[ i0 ] * Direction[ i0 ] + Direction[ i1 ] * Direction[ i1 ];
|
|
fTmp = fLSqr * ExtentPoint[ i2 ] - Direction[ i2 ] * (Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * ExtentPoint[ i1 ]);
|
|
if( fTmp >= 0.0f )
|
|
{
|
|
// v[ i2 ]-edge is closest
|
|
if( fTmp <= 2.0f * fLSqr * Box.fExtent[ i2 ] )
|
|
{
|
|
fT = fTmp / fLSqr;
|
|
fLSqr += Direction[ i2 ] * Direction[ i2 ];
|
|
fTmp = ExtentPoint[ i2 ] - fT;
|
|
fDelta = Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * ExtentPoint[ i1 ] + Direction[ i2 ] * fTmp;
|
|
fParam = -fDelta / fLSqr;
|
|
fSqrDistance += PmE[ i0 ] * PmE[ i0 ] + ExtentPoint[ i1 ] * ExtentPoint[ i1 ] + fTmp * fTmp + fDelta * fParam;
|
|
|
|
fLParam = fParam;
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
Point[ i1 ] = -Box.fExtent[ i1 ];
|
|
Point[ i2 ] = fT - Box.fExtent[ i2 ];
|
|
}
|
|
else
|
|
{
|
|
fLSqr += Direction[ i2 ] * Direction[ i2 ];
|
|
fDelta = Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * ExtentPoint[ i1 ] + Direction[ i2 ] * PmE[ i2 ];
|
|
fParam = -fDelta / fLSqr;
|
|
fSqrDistance += PmE[ i0 ] * PmE[ i0 ] + ExtentPoint[ i1 ] * ExtentPoint[ i1 ] + PmE[ i2 ] * PmE[ i2 ] + fDelta * fParam;
|
|
|
|
fLParam = fParam;
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
Point[ i1 ] = -Box.fExtent[ i1 ];
|
|
Point[ i2 ] = Box.fExtent[ i2 ];
|
|
}
|
|
return;
|
|
}
|
|
|
|
// (v[ i1 ],v[ i2 ])-corner is closest
|
|
fLSqr += Direction[ i2 ] * Direction[ i2 ];
|
|
fDelta = Direction[ i0 ] * PmE[ i0 ] + Direction[ i1 ] * ExtentPoint[ i1 ] + Direction[ i2 ] * ExtentPoint[ i2 ];
|
|
fParam = -fDelta / fLSqr;
|
|
fSqrDistance += PmE[ i0 ] * PmE[ i0 ] + ExtentPoint[ i1 ] * ExtentPoint[ i1 ] + ExtentPoint[ i2 ] * ExtentPoint[ i2 ] + fDelta * fParam;
|
|
|
|
fLParam = fParam;
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
Point[ i1 ] = -Box.fExtent[ i1 ];
|
|
Point[ i2 ] = -Box.fExtent[ i2 ];
|
|
}
|
|
}
|
|
}
|
|
|
|
void CaseNoZeros( SCollisionBox &Box, EtVector3 &Point, EtVector3 &Direction, float &fLParam, float &fSqrDistance )
|
|
{
|
|
EtVector3 PmE;
|
|
|
|
PmE.x = Point.x - Box.fExtent[ 0 ];
|
|
PmE.y = Point.y - Box.fExtent[ 1 ];
|
|
PmE.z = Point.z - Box.fExtent[ 2 ];
|
|
|
|
float fProdDxPy = Direction.x * PmE.y;
|
|
float fProdDyPx = Direction.y * PmE.x;
|
|
float fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;
|
|
|
|
if( fProdDyPx >= fProdDxPy )
|
|
{
|
|
fProdDzPx = Direction.z * PmE.x;
|
|
fProdDxPz = Direction.x * PmE.z;
|
|
if( fProdDzPx >= fProdDxPz )
|
|
{
|
|
// line intersects x = e0
|
|
Face( Box, 0, 1, 2, Point, Direction, PmE, fLParam, fSqrDistance );
|
|
}
|
|
else
|
|
{
|
|
// line intersects z = e2
|
|
Face( Box, 2, 0, 1, Point, Direction, PmE, fLParam, fSqrDistance );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fProdDzPy = Direction.z * PmE.y;
|
|
fProdDyPz = Direction.y * PmE.z;
|
|
if( fProdDzPy >= fProdDyPz )
|
|
{
|
|
// line intersects y = e1
|
|
Face( Box, 1, 2, 0, Point, Direction, PmE, fLParam, fSqrDistance );
|
|
}
|
|
else
|
|
{
|
|
// line intersects z = e2
|
|
Face( Box, 2, 0, 1, Point, Direction, PmE, fLParam, fSqrDistance );
|
|
}
|
|
}
|
|
}
|
|
|
|
void Case0( SCollisionBox &Box, int i0, int i1, int i2, EtVector3 &Point, EtVector3 &Direction, float &fLParam, float &fSqrDistance )
|
|
{
|
|
float fPmE0 = Point[ i0 ] - Box.fExtent[ i0 ];
|
|
float fPmE1 = Point[ i1 ] - Box.fExtent[ i1 ];
|
|
float fProd0 = Direction[ i1 ] * fPmE0;
|
|
float fProd1 = Direction[ i0 ] * fPmE1;
|
|
float fDelta, fInvLSqr, fInv;
|
|
|
|
if( fProd0 >= fProd1 )
|
|
{
|
|
// line intersects P[ i0 ] = e[ i0 ]
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
|
|
float fPpE1 = Point[ i1 ] + Box.fExtent[ i1 ];
|
|
fDelta = fProd0 - Direction[ i0 ] * fPpE1;
|
|
if( fDelta >= 0.0f )
|
|
{
|
|
fInvLSqr = 1.0f / ( Direction[ i0 ] * Direction[ i0 ]+Direction[ i1 ] * Direction[ i1 ] );
|
|
fSqrDistance += fDelta * fDelta * fInvLSqr;
|
|
Point[ i1 ] = -Box.fExtent[ i1 ];
|
|
fLParam = -( Direction[ i0 ] * fPmE0+Direction[ i1 ] * fPpE1 ) * fInvLSqr;
|
|
}
|
|
else
|
|
{
|
|
fInv = 1.0f / Direction[ i0 ];
|
|
Point[ i1 ] -= fProd0 * fInv;
|
|
fLParam = -fPmE0 * fInv;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// line intersects P[ i1 ] = e[ i1 ]
|
|
Point[ i1 ] = Box.fExtent[ i1 ];
|
|
|
|
float fPpE0 = Point[ i0 ] + Box.fExtent[ i0 ];
|
|
fDelta = fProd1 - Direction[ i1 ] * fPpE0;
|
|
if( fDelta >= 0.0f )
|
|
{
|
|
fInvLSqr = 1.0f / ( Direction[ i0 ] * Direction[ i0 ]+Direction[ i1 ] * Direction[ i1 ] );
|
|
fSqrDistance += fDelta * fDelta * fInvLSqr;
|
|
Point[ i0 ] = -Box.fExtent[ i0 ];
|
|
fLParam = -( Direction[ i0 ] * fPpE0+Direction[ i1 ] * fPmE1 ) * fInvLSqr;
|
|
}
|
|
else
|
|
{
|
|
fInv = ( 1.0f ) / Direction[ i1 ];
|
|
Point[ i0 ] -= fProd1 * fInv;
|
|
fLParam = -fPmE1 * fInv;
|
|
}
|
|
}
|
|
|
|
if( Point[ i2 ] < -Box.fExtent[ i2 ] )
|
|
{
|
|
fDelta = Point[ i2 ] + Box.fExtent[ i2 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point[ i2 ] = -Box.fExtent[ i2 ];
|
|
}
|
|
else if( Point[ i2 ] > Box.fExtent[ i2 ] )
|
|
{
|
|
fDelta = Point[ i2 ] - Box.fExtent[ i2 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point[ i2 ] = Box.fExtent[ i2 ];
|
|
}
|
|
}
|
|
|
|
void Case00( SCollisionBox &Box, int i0, int i1, int i2, EtVector3 &Point, EtVector3 &Direction, float &fLParam, float &fSqrDistance )
|
|
{
|
|
float fDelta;
|
|
|
|
fLParam = ( Box.fExtent[ i0 ] - Point[ i0 ] ) / Direction[ i0 ];
|
|
|
|
Point[ i0 ] = Box.fExtent[ i0 ];
|
|
|
|
if( Point[ i1 ] < -Box.fExtent[ i1 ] )
|
|
{
|
|
fDelta = Point[ i1 ] + Box.fExtent[ i1 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point[ i1 ] = -Box.fExtent[ i1 ];
|
|
}
|
|
else if( Point[ i1 ] > Box.fExtent[ i1 ] )
|
|
{
|
|
fDelta = Point[ i1 ] - Box.fExtent[ i1 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point[ i1 ] = Box.fExtent[ i1 ];
|
|
}
|
|
|
|
if( Point[ i2 ] < -Box.fExtent[ i2 ] )
|
|
{
|
|
fDelta = Point[ i2 ] + Box.fExtent[ i2 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point[ i2 ] = -Box.fExtent[ i2 ];
|
|
}
|
|
else if( Point[ i2 ] > Box.fExtent[ i2 ] )
|
|
{
|
|
fDelta = Point[ i2 ] - Box.fExtent[ i2 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point[ i2 ] = Box.fExtent[ i2 ];
|
|
}
|
|
}
|
|
|
|
void Case000( SCollisionBox &Box, EtVector3 &Point, float &fSqrDistance )
|
|
{
|
|
float fDelta;
|
|
|
|
if( Point.x < -Box.fExtent[ 0 ] )
|
|
{
|
|
fDelta = Point.x + Box.fExtent[ 0 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point.x = -Box.fExtent[ 0 ];
|
|
}
|
|
else if( Point.x > Box.fExtent[ 0 ] )
|
|
{
|
|
fDelta = Point.x - Box.fExtent[ 0 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point.x = Box.fExtent[ 0 ];
|
|
}
|
|
|
|
if( Point.y < -Box.fExtent[ 1 ] )
|
|
{
|
|
fDelta = Point.y + Box.fExtent[ 1 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point.y = -Box.fExtent[ 1 ];
|
|
}
|
|
else if( Point.y > Box.fExtent[ 1 ] )
|
|
{
|
|
fDelta = Point.y - Box.fExtent[ 1 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point.y = Box.fExtent[ 1 ];
|
|
}
|
|
|
|
if( Point.z < -Box.fExtent[ 2 ] )
|
|
{
|
|
fDelta = Point.z + Box.fExtent[ 2 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point.z = -Box.fExtent[ 2 ];
|
|
}
|
|
else if( Point.z > Box.fExtent[ 2 ] )
|
|
{
|
|
fDelta = Point.z - Box.fExtent[ 2 ];
|
|
fSqrDistance += fDelta * fDelta;
|
|
Point.z = Box.fExtent[ 2 ];
|
|
}
|
|
}
|
|
|
|
#define COMPARE_MIN( fTmpMinD, fMinD, fTmpSeg, fSeg, fTmpTri0, fTri0, fTmpTri1, fTri1 ) \
|
|
{ \
|
|
if( fTmpMinD < fMinD ) \
|
|
{ \
|
|
fMinD = fTmpMinD; \
|
|
fSeg = fTmpSeg; \
|
|
fTri0 = fTmpTri0; \
|
|
fTri1 = fTmpTri1; \
|
|
} \
|
|
}
|
|
|
|
float DistPointToTriangle( EtVector3 &vPoint, SCollisionTriangle &Triangle, float &fTriParam0, float &fTriParam1 )
|
|
{
|
|
EtVector3 vDiff = Triangle.vOrigin - vPoint;
|
|
float fA = EtVec3Dot( &Triangle.vEdge1, &Triangle.vEdge1 );
|
|
float fB = EtVec3Dot( &Triangle.vEdge1, &Triangle.vEdge2 );
|
|
float fC = EtVec3Dot( &Triangle.vEdge2, &Triangle.vEdge2 );
|
|
float fD = EtVec3Dot( &vDiff, &Triangle.vEdge1 );
|
|
float fE = EtVec3Dot( &vDiff, &Triangle.vEdge2 );
|
|
float fF = EtVec3Dot( &vDiff, &vDiff );
|
|
float fDet = fabs( fA * fC - fB * fB );
|
|
|
|
fTriParam0 = fB * fE - fC * fD;
|
|
fTriParam1 = fB * fD - fA * fE;
|
|
|
|
if( fTriParam0 + fTriParam1 <= fDet )
|
|
{
|
|
if( fTriParam0 < 0.0f )
|
|
{
|
|
if( fTriParam1 < 0.0f ) // region 4
|
|
{
|
|
if( fD < 0.0f )
|
|
{
|
|
fTriParam1 = 0.0f;
|
|
if( -fD >= fA )
|
|
{
|
|
fTriParam0 = 1.0f;
|
|
return fabs( fA + 2.0f * fD + fF );
|
|
}
|
|
else
|
|
{
|
|
fTriParam0 = -fD / fA;
|
|
return fabs( fD * fTriParam0 + fF );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fTriParam0 = 0.0f;
|
|
if( fE >= 0.0f )
|
|
{
|
|
fTriParam1 = 0.0f;
|
|
return fabs(fF);
|
|
}
|
|
else if( -fE >= fC )
|
|
{
|
|
fTriParam1 = 1.0f;
|
|
return fabs( fC + 2.0f * fE + fF );
|
|
}
|
|
else
|
|
{
|
|
fTriParam1 = -fE / fC;
|
|
return fabs( fE * fTriParam1 + fF );
|
|
}
|
|
}
|
|
}
|
|
else // region 3
|
|
{
|
|
fTriParam0 = 0.0f;
|
|
if( fE >= 0.0f )
|
|
{
|
|
fTriParam1 = 0.0f;
|
|
return fabs( fF );
|
|
}
|
|
else if( -fE >= fC )
|
|
{
|
|
fTriParam1 = 1.0f;
|
|
return fabs( fC + 2.0f * fE + fF );
|
|
}
|
|
else
|
|
{
|
|
fTriParam1 = -fE / fC;
|
|
return fabs( fE * fTriParam1 + fF );
|
|
}
|
|
}
|
|
}
|
|
else if( fTriParam1 < 0.0f ) // region 5
|
|
{
|
|
fTriParam1 = 0.0f;
|
|
if( fD >= 0.0f )
|
|
{
|
|
fTriParam0 = 0.0f;
|
|
return fabs( fF );
|
|
}
|
|
else if( -fD >= fA )
|
|
{
|
|
fTriParam0 = 1.0f;
|
|
return fabs( fA + 2.0f * fD + fF );
|
|
}
|
|
else
|
|
{
|
|
fTriParam0 = -fD / fA;
|
|
return fabs( fD * fTriParam0 + fF );
|
|
}
|
|
}
|
|
else // region 0
|
|
{
|
|
// minimum at interior point
|
|
float fInvDet = 1.0f / fDet;
|
|
fTriParam0 *= fInvDet;
|
|
fTriParam1 *= fInvDet;
|
|
return fabs( fTriParam0 * ( fA * fTriParam0 + fB * fTriParam1 + 2 * fD ) + fTriParam1 * ( fB * fTriParam0 + fC * fTriParam1 + 2 * fE ) + fF );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
float fTmp0;
|
|
float fTmp1;
|
|
float fNumer;
|
|
float fDenom;
|
|
|
|
if( fTriParam0 < 0.0f ) // region 2
|
|
{
|
|
fTmp0 = fB + fD;
|
|
fTmp1 = fC + fE;
|
|
if( fTmp1 > fTmp0 )
|
|
{
|
|
fNumer = fTmp1 - fTmp0;
|
|
fDenom = fA - 2.0f * fB + fC;
|
|
if( fNumer >= fDenom )
|
|
{
|
|
fTriParam0 = 1.0f;
|
|
fTriParam1 = 0.0f;
|
|
return fabs( fA + 2.0f * fD + fF );
|
|
}
|
|
else
|
|
{
|
|
fTriParam0 = fNumer / fDenom;
|
|
fTriParam1 = 1.0f - fTriParam0;
|
|
return fabs( fC + 2.0f * fE + fF - fNumer * fTriParam0 );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fTriParam0 = 0.0f;
|
|
if( fTmp1 <= 0.0f )
|
|
{
|
|
fTriParam1 = 1.0f;
|
|
return fabs( fC + 2.0f * fE + fF );
|
|
}
|
|
else if( fE >= 0.0f )
|
|
{
|
|
fTriParam1 = 0.0f;
|
|
return fabs( fF );
|
|
}
|
|
else
|
|
{
|
|
fTriParam1 = -fE / fC;
|
|
return fabs( fE * fTriParam1 + fF );
|
|
}
|
|
}
|
|
}
|
|
else if( fTriParam1 < 0.0f ) // region 6
|
|
{
|
|
fTmp0 = fB + fE;
|
|
fTmp1 = fA + fD;
|
|
if( fTmp1 > fTmp0 )
|
|
{
|
|
fNumer = fTmp1 - fTmp0;
|
|
fDenom = fA - 2.0f * fB + fC;
|
|
if( fNumer >= fDenom )
|
|
{
|
|
fTriParam1 = 1.0f;
|
|
fTriParam0 = 0.0f;
|
|
return fabs( fC + 2.0f * fE + fF );
|
|
}
|
|
else
|
|
{
|
|
fTriParam1 = fNumer / fDenom;
|
|
fTriParam0 = 1.0f - fTriParam1;
|
|
return fabs( fA + 2.0f * fD + fF - fNumer * fTriParam1 );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fTriParam1 = 0.0f;
|
|
if( fTmp1 <= 0.0f )
|
|
{
|
|
fTriParam0 = 1.0f;
|
|
return fabs( fA + 2.0f * fD + fF );
|
|
}
|
|
else if( fD >= 0.0f )
|
|
{
|
|
fTriParam0 = 0.0f;
|
|
return fabs( fF );
|
|
}
|
|
else
|
|
{
|
|
fTriParam0 = -fD / fA;
|
|
return fabs( fD * fTriParam0 + fF );
|
|
}
|
|
}
|
|
}
|
|
else // region 1
|
|
{
|
|
fNumer = ( fC + fE ) - ( fB + fD );
|
|
if(fNumer <= 0)
|
|
{
|
|
fTriParam0 = 0.0f;
|
|
fTriParam1 = 1.0f;
|
|
return fabs( fC + 2.0f * fE + fF );
|
|
}
|
|
else
|
|
{
|
|
fDenom = fA - 2.0f * fB + fC;
|
|
if( fNumer >= fDenom )
|
|
{
|
|
fTriParam0 = 1.0f;
|
|
fTriParam1 = 0.0f;
|
|
return fabs( fA + 2.0f * fD + fF );
|
|
}
|
|
else
|
|
{
|
|
fTriParam0 = fNumer / fDenom;
|
|
fTriParam1 = 1.0f - fTriParam0;
|
|
return fabs( fC + 2.0f * fE + fF - fNumer * fTriParam0 );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
float DistSegmentToTriangle( SSegment &Segment, SCollisionTriangle &Triangle, float &fSegParm, float &fTriParm0, float &fTriParm1 )
|
|
{
|
|
const float fTolerance = 1e-05f;
|
|
SSegment TriangleSeg;
|
|
EtVector3 vPoint;
|
|
float fMin, fMin0, fTmpSegParm, fTmpTriParm0, fTmpTriParm1;
|
|
|
|
EtVector3 vDiff = Triangle.vOrigin - Segment.vOrigin;
|
|
float fA00 = EtVec3Dot( &Segment.vDirection, &Segment.vDirection );
|
|
float fA01 = -EtVec3Dot( &Segment.vDirection, &Triangle.vEdge1 );
|
|
float fA02 = -EtVec3Dot( &Segment.vDirection, &Triangle.vEdge2 );
|
|
float fA11 = EtVec3Dot( &Triangle.vEdge1, &Triangle.vEdge1 );
|
|
float fA12 = EtVec3Dot( &Triangle.vEdge1, &Triangle.vEdge2 );
|
|
float fA22 = EtVec3Dot( &Triangle.vEdge2, &Triangle.vEdge2 );
|
|
float fB0 = -EtVec3Dot( &vDiff, &Segment.vDirection );
|
|
float fB1 = EtVec3Dot( &vDiff, &Triangle.vEdge1 );
|
|
float fB2 = EtVec3Dot( &vDiff, &Triangle.vEdge2 );
|
|
|
|
EtVector3 vPlaneNormal;
|
|
EtVec3Cross( &vPlaneNormal, &Triangle.vEdge1, &Triangle.vEdge2 );
|
|
|
|
if( fabs( EtVec3Dot( &vPlaneNormal, &Segment.vDirection ) ) > ( fTolerance * ( EtVec3Length( &vPlaneNormal ) * EtVec3Length( &Segment.vDirection ) ) ) )
|
|
{
|
|
float fCof00 = fA11 * fA22 - fA12 * fA12;
|
|
float fCof01 = fA02 * fA12 - fA01 * fA22;
|
|
float fCof02 = fA01 * fA12 - fA02 * fA11;
|
|
float fCof11 = fA00 * fA22 - fA02 * fA02;
|
|
float fCof12 = fA02 * fA01 - fA00 * fA12;
|
|
float fCof22 = fA00 * fA11 - fA01 * fA01;
|
|
|
|
float fDet = fA00 * fCof00 + fA01 * fCof01 + fA02 * fCof02;
|
|
float fInvDet = 1.0f / fDet;
|
|
|
|
float fRhs0 = -fB0 * fInvDet;
|
|
float fRhs1 = -fB1 * fInvDet;
|
|
float fRhs2 = -fB2 * fInvDet;
|
|
|
|
fSegParm = fCof00 * fRhs0 + fCof01 * fRhs1 + fCof02 * fRhs2;
|
|
fTriParm0 = fCof01 * fRhs0 + fCof11 * fRhs1 + fCof12 * fRhs2;
|
|
fTriParm1 = fCof02 * fRhs0 + fCof12 * fRhs1 + fCof22 * fRhs2;
|
|
|
|
if( fSegParm < 0.0f )
|
|
{
|
|
if( fTriParm0 + fTriParm1 <= 1.0f )
|
|
{
|
|
if( fTriParm0 < 0.0f )
|
|
{
|
|
if( fTriParm1 < 0.0f )
|
|
{
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 0.0f;
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm0 );
|
|
fTmpTriParm1 = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
fMin0 = DistPointToTriangle( Segment.vOrigin, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else
|
|
{
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 0.0f;
|
|
fMin0 = DistPointToTriangle( Segment.vOrigin, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
}
|
|
else if( fTriParm1 < 0.0f )
|
|
{
|
|
// min on face TP1=0 or SP=0
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm0 );
|
|
fTriParm1 = 0.0f;
|
|
fMin0 = DistPointToTriangle( Segment.vOrigin, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else // region 0m, min face on SP=0
|
|
{
|
|
fMin = DistPointToTriangle( Segment.vOrigin, Triangle, fTriParm0, fTriParm1) ;
|
|
fSegParm = 0.0f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( fTriParm0 < 0.0f ) // region 2m
|
|
{
|
|
// min on face TP0=0 or TP0+TP1=1 or SP=0
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 0.0f;
|
|
TriangleSeg.vOrigin = Triangle.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = Triangle.vEdge2-Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm1);
|
|
fTmpTriParm0 = 1.0f - fTmpTriParm1;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
fMin0 = DistPointToTriangle( Segment.vOrigin, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else if(fTriParm1 < 0.0f) // region 6m
|
|
{
|
|
// min on face TP1=0 or TP0+TP1=1 or SP=0
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm0 );
|
|
fTriParm1 = 0.0f;
|
|
TriangleSeg.vOrigin = Triangle.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = Triangle.vEdge2-Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm1 );
|
|
fTmpTriParm0 = 1.0f - fTmpTriParm1;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
fMin0 = DistPointToTriangle( Segment.vOrigin, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else // region 1m, min on face TP0+TP1=1 or SP=0
|
|
{
|
|
TriangleSeg.vOrigin = Triangle.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = Triangle.vEdge2-Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 1.0f - fTriParm1;
|
|
fMin0 = DistPointToTriangle( Segment.vOrigin, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
}
|
|
}
|
|
else if( fSegParm <= 1.0f )
|
|
{
|
|
if( fTriParm0 + fTriParm1 <= 1.0f )
|
|
{
|
|
if( fTriParm0 < 0.0f )
|
|
{
|
|
if( fTriParm1 < 0.0f ) // region 4
|
|
{
|
|
// min on face TP0=0 or TP1=0
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 0.0f;
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm0) ;
|
|
fTmpTriParm1 = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else // region 3, min on face TP0=0
|
|
{
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 0.0f;
|
|
}
|
|
}
|
|
else if( fTriParm1 < 0.0f ) // region 5, min on face TP1=0
|
|
{
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm0 );
|
|
fTriParm1 = 0.0f;
|
|
}
|
|
else // region 0, global minimum is interior, so done
|
|
{
|
|
fMin = 0.0f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( fTriParm0 < 0.0f ) // region 2
|
|
{
|
|
// min on face TP0=0 or TP0+TP1=1
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 0.0f;
|
|
TriangleSeg.vOrigin = Triangle.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = Triangle.vEdge2 - Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm1 );
|
|
fTmpTriParm0 = 1.0f - fTmpTriParm1;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else if(fTriParm1 < 0.0f) // region 6
|
|
{
|
|
// min on face TP1=0 or TP0+TP1=1
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm0 );
|
|
fTriParm1 = 0.0f;
|
|
TriangleSeg.vOrigin = Triangle.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = Triangle.vEdge2 - Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm1 );
|
|
fTmpTriParm0 = 1.0f - fTmpTriParm1;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else // region 1, min on face TP0+TP1=1
|
|
{
|
|
TriangleSeg.vOrigin = Triangle.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = Triangle.vEdge2 - Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 1.0f - fTriParm1;
|
|
}
|
|
}
|
|
}
|
|
else // fSegParm > 1
|
|
{
|
|
if( fTriParm0 + fTriParm1 <= 1.0f )
|
|
{
|
|
if( fTriParm0 < 0.0f )
|
|
{
|
|
if( fTriParm1 < 0.0f ) // region 4p
|
|
{
|
|
// min on face TP0=0 or TP1=0 or SP=1
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 0.0f;
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm0 );
|
|
fTmpTriParm1 = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
vPoint = Segment.vOrigin + Segment.vDirection;
|
|
fMin0 = DistPointToTriangle( vPoint, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 1.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else // region 3p, min on face TP0=0 or SP=1
|
|
{
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 0.0f;
|
|
vPoint = Segment.vOrigin + Segment.vDirection;
|
|
fMin0 = DistPointToTriangle( vPoint, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 1.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
}
|
|
else if( fTriParm1 < 0.0f ) // region 5p
|
|
{
|
|
// min on face TP1=0 or SP=1
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm0 );
|
|
fTriParm1 = 0.0f;
|
|
vPoint = Segment.vOrigin + Segment.vDirection;
|
|
fMin0 = DistPointToTriangle( vPoint, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 1.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else // region 0p, min face on SP=1
|
|
{
|
|
vPoint = Segment.vOrigin + Segment.vDirection;
|
|
fMin = DistPointToTriangle( vPoint, Triangle, fTriParm0, fTriParm1 );
|
|
fSegParm = 1.0f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( fTriParm0 < 0.0f ) // region 2p
|
|
{
|
|
// min on face TP0=0 or TP0+TP1=1 or SP=1
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 0.0f;
|
|
TriangleSeg.vOrigin = Triangle.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = Triangle.vEdge2 - Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm1 );
|
|
fTmpTriParm0 = 1.0f - fTmpTriParm1;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
vPoint = Segment.vOrigin + Segment.vDirection;
|
|
fMin0 = DistPointToTriangle( vPoint, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 1.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else if( fTriParm1 < 0.0f ) // region 6p
|
|
{
|
|
// min on face TP1=0 or TP0+TP1=1 or SP=1
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm0 );
|
|
fTriParm1 = 0.0f;
|
|
TriangleSeg.vOrigin = Triangle.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = Triangle.vEdge2 - Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm1 );
|
|
fTmpTriParm0 = 1.0f - fTmpTriParm1;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
vPoint = Segment.vOrigin + Segment.vDirection;
|
|
fMin0 = DistPointToTriangle( vPoint, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 1.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
else // region 1p, min on face TP0+TP1=1 or SP=1
|
|
{
|
|
TriangleSeg.vOrigin = Triangle.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = Triangle.vEdge2 - Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm1 );
|
|
fTriParm0 = 1.0f - fTriParm1;
|
|
vPoint = Segment.vOrigin + Segment.vDirection;
|
|
fMin0 = DistPointToTriangle( vPoint, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 1.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
}
|
|
}
|
|
}
|
|
|
|
return fMin;
|
|
}
|
|
|
|
// Segment and triangle are parallel.
|
|
TriangleSeg.vOrigin = Triangle.vOrigin;
|
|
TriangleSeg.vDirection = Triangle.vEdge1;
|
|
fMin = DistSegmentToSegment( Segment, TriangleSeg, fSegParm, fTriParm0 );
|
|
fTriParm1 = 0.0f;
|
|
|
|
TriangleSeg.vDirection = Triangle.vEdge2;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm1 );
|
|
fTmpTriParm0 = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
|
|
TriangleSeg.vOrigin = TriangleSeg.vOrigin + Triangle.vEdge1;
|
|
TriangleSeg.vDirection = TriangleSeg.vDirection - Triangle.vEdge1;
|
|
fMin0 = DistSegmentToSegment( Segment, TriangleSeg, fTmpSegParm, fTmpTriParm1 );
|
|
fTmpTriParm0 = 1.0f - fTmpTriParm1;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
|
|
fMin0 = DistPointToTriangle( Segment.vOrigin, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 0.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
|
|
vPoint = Segment.vOrigin + Segment.vDirection;
|
|
fMin0 = DistPointToTriangle( vPoint, Triangle, fTmpTriParm0, fTmpTriParm1 );
|
|
fTmpSegParm = 1.0f;
|
|
COMPARE_MIN( fMin0, fMin, fTmpSegParm, fSegParm, fTmpTriParm0, fTriParm0, fTmpTriParm1, fTriParm1 );
|
|
|
|
return fMin;
|
|
}
|