706 lines
33 KiB
C++
706 lines
33 KiB
C++
#include "Stdafx.h"
|
||
#include "EtCollisionFunc.h"
|
||
#include "EtCollisionPrimitive.h"
|
||
#include "EtComputeDist.h"
|
||
|
||
#ifdef _DEBUG
|
||
#define new new(_NORMAL_BLOCK,__FILE__,__LINE__)
|
||
#endif
|
||
|
||
bool TestLineToTriangle( EtVector3 &Origin, EtVector3 &Direction, EtVector3 &Point1, EtVector3 &Point2, EtVector3 &Point3,
|
||
float &fDist, float &fBary1, float &fBary2 )
|
||
{
|
||
float fDeterminant;
|
||
EtVector3 Edge1, Edge2;
|
||
EtVector3 PVector, TVector, QVector;
|
||
|
||
Edge1 = Point2 - Point1;
|
||
Edge2 = Point3 - Point1;
|
||
EtVec3Cross( &PVector, &Direction, &Edge2 );
|
||
fDeterminant = EtVec3Dot( &Edge1, &PVector );
|
||
|
||
fDist = FLT_MAX;
|
||
if( fDeterminant > 0 )
|
||
{
|
||
TVector = Origin - Point1;
|
||
}
|
||
else
|
||
{
|
||
TVector = Point1 - Origin;
|
||
fDeterminant = -fDeterminant;
|
||
}
|
||
if( fDeterminant < NEAR_ZERO )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fBary1 = EtVec3Dot( &TVector, &PVector );
|
||
if( ( fBary1 < 0.0f ) || ( fBary1 > fDeterminant ) )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
EtVec3Cross( &QVector, &TVector, &Edge1 );
|
||
fBary2 = EtVec3Dot( &Direction, &QVector );
|
||
if( ( fBary2 < 0.0f ) || ( fBary2 + fBary1 > fDeterminant ) )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fDist = EtVec3Dot( &Edge2, &QVector );
|
||
fDist /= fDeterminant;
|
||
fBary1 /= fDeterminant;
|
||
fBary2 /= fDeterminant;
|
||
|
||
return true;
|
||
}
|
||
|
||
bool TestLineToBox( EtVector3 &Origin, EtVector3 &Direction, SAABox &Box, float &fDist )
|
||
{
|
||
bool bInside;
|
||
|
||
fDist = -FLT_MAX;
|
||
bInside = true;
|
||
if( Box.Min.x - Origin.x > NEAR_ZERO )
|
||
{
|
||
if( Direction.x <= 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
else
|
||
{
|
||
bInside = false;
|
||
fDist = max( fDist, ( Box.Min.x - Origin.x ) / Direction.x );
|
||
}
|
||
}
|
||
else if( Origin.x - Box.Max.x > NEAR_ZERO )
|
||
{
|
||
if( Direction.x >= 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
else
|
||
{
|
||
bInside = false;
|
||
fDist = max( fDist, ( Box.Max.x - Origin.x ) / Direction.x );
|
||
}
|
||
}
|
||
|
||
if( Box.Min.y - Origin.y > NEAR_ZERO )
|
||
{
|
||
if( Direction.y <= 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
else
|
||
{
|
||
bInside = false;
|
||
fDist = max( fDist, ( Box.Min.y - Origin.y ) / Direction.y );
|
||
}
|
||
}
|
||
else if( Origin.y - Box.Max.y > NEAR_ZERO )
|
||
{
|
||
if( Direction.y >= 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
else
|
||
{
|
||
bInside = false;
|
||
fDist = max( fDist, ( Box.Max.y - Origin.y ) / Direction.y );
|
||
}
|
||
}
|
||
|
||
if( Box.Min.z - Origin.z > NEAR_ZERO )
|
||
{
|
||
if( Direction.z <= 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
else
|
||
{
|
||
bInside = false;
|
||
fDist = max( fDist, ( Box.Min.z - Origin.z ) / Direction.z );
|
||
}
|
||
}
|
||
else if( Origin.z - Box.Max.z > NEAR_ZERO )
|
||
{
|
||
if(Direction.z >= 0.0f)
|
||
{
|
||
return false;
|
||
}
|
||
else
|
||
{
|
||
bInside = false;
|
||
fDist = max( fDist, ( Box.Max.z - Origin.z ) / Direction.z );
|
||
}
|
||
}
|
||
|
||
if( bInside )
|
||
{
|
||
fDist = 0.0f;
|
||
return true;
|
||
}
|
||
|
||
EtVector3 Hit;
|
||
|
||
Hit = Origin + Direction * fDist;
|
||
const float BOX_SIDE_THRESHOLD = 0.1f;
|
||
if( Hit.x > Box.Min.x - BOX_SIDE_THRESHOLD && Hit.x < Box.Max.x + BOX_SIDE_THRESHOLD &&
|
||
Hit.y > Box.Min.y - BOX_SIDE_THRESHOLD && Hit.y < Box.Max.y + BOX_SIDE_THRESHOLD &&
|
||
Hit.z > Box.Min.z - BOX_SIDE_THRESHOLD && Hit.z < Box.Max.z + BOX_SIDE_THRESHOLD)
|
||
return true;
|
||
|
||
return false;
|
||
}
|
||
|
||
bool TestSegmentToSphere( SSegment &Segment, SSphere &Sphere )
|
||
{
|
||
float fDistSq, fSegParam;
|
||
|
||
fDistSq = DistPointToSegment( Sphere.Center, Segment, fSegParam );
|
||
|
||
return fDistSq <= Sphere.fRadius * Sphere.fRadius;
|
||
}
|
||
|
||
bool TestSegmentToOBB( SSegment &Segment, SOBB &Box )
|
||
{
|
||
float fAWdU[ 3 ], fADdU[ 3 ], fAWxDdU[ 3 ], fRhs;
|
||
EtVector3 HalfDir = 0.5f * Segment.vDirection;
|
||
EtVector3 Center = Segment.vOrigin + HalfDir;
|
||
|
||
EtVector3 Diff = Center - Box.Center;
|
||
|
||
fAWdU[ 0 ] = ( float )FastAbs(( float )( EtVec3Dot( &HalfDir, &Box.Axis[ 0 ] ) ) );
|
||
fADdU[ 0 ] = ( float )FastAbs(( float )( EtVec3Dot( &Diff, &Box.Axis[ 0 ] ) ) );
|
||
fRhs = Box.Extent[ 0 ] + fAWdU[ 0 ];
|
||
if ( fADdU[ 0 ] > fRhs )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fAWdU[ 1 ] = ( float )FastAbs(( float )( EtVec3Dot( &HalfDir, &Box.Axis[ 1 ] ) ) );
|
||
fADdU[ 1 ] = ( float )FastAbs(( float )( EtVec3Dot( &Diff, &Box.Axis[ 1 ] ) ) );
|
||
fRhs = Box.Extent[ 1 ] + fAWdU[ 1 ];
|
||
if ( fADdU[ 1 ] > fRhs )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fAWdU[ 2 ] = ( float )FastAbs(( float )( EtVec3Dot( &HalfDir, &Box.Axis[ 2 ] ) ) );
|
||
fADdU[ 2 ] = ( float )FastAbs(( float )( EtVec3Dot( &Diff, &Box.Axis[ 2 ] ) ) );
|
||
fRhs = Box.Extent[ 2 ] + fAWdU[ 2 ];
|
||
if ( fADdU[ 2 ] > fRhs )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
EtVector3 WxD;
|
||
EtVec3Cross( &WxD, &HalfDir, &Diff );
|
||
|
||
fAWxDdU[ 0 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 0 ] ) ) );
|
||
fRhs = Box.Extent[ 1 ]*fAWdU[ 2 ] + Box.Extent[ 2 ]*fAWdU[ 1 ];
|
||
if ( fAWxDdU[ 0 ] > fRhs )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fAWxDdU[ 1 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 1 ] ) ) );
|
||
fRhs = Box.Extent[ 0 ]*fAWdU[ 2 ] + Box.Extent[ 2 ]*fAWdU[ 0 ];
|
||
if ( fAWxDdU[ 1 ] > fRhs )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fAWxDdU[ 2 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 2 ] ) ) );
|
||
fRhs = Box.Extent[ 0 ]*fAWdU[ 1 ] + Box.Extent[ 1 ]*fAWdU[ 0 ];
|
||
if ( fAWxDdU[ 2 ] > fRhs )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool TestSegmentToCapsule( SSegment &Segment, SCapsule &Capsule )
|
||
{
|
||
float fDistSq, fSegParam1, fSegParam2;
|
||
|
||
fDistSq = DistSegmentToSegment( Segment, Capsule.Segment, fSegParam1, fSegParam2 );
|
||
|
||
return fDistSq <= Capsule.fRadius * Capsule.fRadius;
|
||
}
|
||
|
||
bool TestLineToSphere( EtVector3 &Origin, EtVector3 &Direction, SSphere &Sphere )
|
||
{
|
||
EtVector3 OriginToCenter;
|
||
float fDot, fLength;
|
||
|
||
OriginToCenter = Sphere.Center - Origin;
|
||
fLength = EtVec3LengthSq( &OriginToCenter );
|
||
EtVec3Normalize( &Direction, &Direction );
|
||
fDot = EtVec3Dot( &OriginToCenter, &Direction );
|
||
if( fDot < 0 )
|
||
{
|
||
return false;
|
||
}
|
||
if( Sphere.fRadius * Sphere.fRadius > fLength - fDot * fDot )
|
||
{
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
|
||
bool TestLineToPlane( EtVector3 &Origin, EtVector3 &Direction, EtVector4 &Plane, EtVector3 &IntersectPoint )
|
||
{
|
||
float fDot;
|
||
|
||
fDot = EtVec3Dot( ( EtVector3 * )&Plane, &Direction );
|
||
if( FastAbs( fDot ) < NEAR_ZERO )
|
||
{
|
||
return false;
|
||
}
|
||
fDot = ( Plane.w - ( Plane.x * Origin.x + Plane.y * Origin.y + Plane.z * Origin.z ) ) / fDot;
|
||
if( fDot < 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
IntersectPoint = Origin + Direction * fDot;
|
||
|
||
return true;
|
||
}
|
||
|
||
bool TestEdgeToPlane( EtVector3 &Start, EtVector3 &End, EtVector4 &Plane, EtVector3 &IntersectPoint )
|
||
{
|
||
EtVector3 Direction;
|
||
float fDot;
|
||
|
||
Direction = End - Start;
|
||
fDot = EtVec3Dot( ( EtVector3 * )&Plane, &Direction );
|
||
// fDot = Plane.x * Direction.x + Plane.y * Direction.y + Plane.z * Direction.z;
|
||
if( FastAbs( fDot ) < NEAR_ZERO )
|
||
{
|
||
return false;
|
||
}
|
||
fDot = ( Plane.w - ( Plane.x * Start.x + Plane.y * Start.y + Plane.z * Start.z ) ) / fDot;
|
||
if( ( fDot < -NEAR_ZERO ) || ( fDot > 1.0f + NEAR_ZERO ) )
|
||
{
|
||
return false;
|
||
}
|
||
IntersectPoint = Start + Direction * fDot;
|
||
|
||
return true;
|
||
}
|
||
|
||
bool IsBehindPoint( EtVector4 &Plane, EtVector3 &Point )
|
||
{
|
||
return Plane.x * Point.x + Plane.y * Point.y + Plane.z * Point.z - Plane.w > NEAR_ZERO;
|
||
}
|
||
|
||
bool IsNearPoint( EtVector3 &Point1, EtVector3 &Point2, float fError )
|
||
{
|
||
if( FastAbs( Point1.x - Point2.x ) > fError )
|
||
{
|
||
return false;
|
||
}
|
||
if( FastAbs( Point1.y - Point2.y ) > fError )
|
||
{
|
||
return false;
|
||
}
|
||
if( FastAbs( Point1.z - Point2.z ) > fError )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool TestBoxToSphere( SAABox &Box, SSphere &Sphere )
|
||
{
|
||
EtVector3 vBoxCenter, vBoxExtent, vDiff;
|
||
float fAx, fAy, fAz, fDx, fDy, fDz, fSqrtRadius;
|
||
|
||
vBoxExtent = ( Box.Max - Box.Min ) * 0.5f;
|
||
vBoxCenter = ( Box.Max + Box.Min ) * 0.5f;
|
||
vDiff = Sphere.Center - vBoxCenter;
|
||
|
||
fAx = vDiff.x;
|
||
fAy = vDiff.y;
|
||
fAz = vDiff.z;
|
||
fDx = fAx - vBoxExtent.x;
|
||
fDy = fAy - vBoxExtent.y;
|
||
fDz = fAz - vBoxExtent.z;
|
||
|
||
if ( fAx <= vBoxExtent[ 0 ] )
|
||
{
|
||
if ( fAy <= vBoxExtent[ 1 ] )
|
||
{
|
||
if ( fAz <= vBoxExtent[ 2 ] )
|
||
{
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
return fDz <= Sphere.fRadius;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if ( fAz <= vBoxExtent[ 2 ] )
|
||
{
|
||
return fDy <= Sphere.fRadius;
|
||
}
|
||
else
|
||
{
|
||
fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
|
||
return fDy * fDy + fDz * fDz <= fSqrtRadius;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if ( fAy <= vBoxExtent[ 1 ] )
|
||
{
|
||
if ( fAz <= vBoxExtent[ 2 ] )
|
||
{
|
||
return fDx <= Sphere.fRadius;
|
||
}
|
||
else
|
||
{
|
||
fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
|
||
return fDx*fDx + fDz*fDz <= fSqrtRadius;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if ( fAz <= vBoxExtent[ 2 ] )
|
||
{
|
||
fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
|
||
return fDx*fDx + fDy*fDy <= fSqrtRadius;
|
||
}
|
||
else
|
||
{
|
||
fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
|
||
return fDx*fDx + fDy*fDy + fDz*fDz <= fSqrtRadius;
|
||
}
|
||
}
|
||
}
|
||
|
||
/* EtVector3 Center, Extent;
|
||
|
||
Center = ( Box.Max + Box.Min ) * 0.5f;
|
||
Extent = ( Box.Max - Box.Min ) * 0.5f;
|
||
|
||
if( fabs( Center.x - Sphere.Center.x ) > ( Extent.x + Sphere.fRadius ) )
|
||
{
|
||
return false;
|
||
}
|
||
if( fabs( Center.y - Sphere.Center.y ) > ( Extent.y + Sphere.fRadius ) )
|
||
{
|
||
return false;
|
||
}
|
||
if( fabs( Center.z - Sphere.Center.z ) > ( Extent.z + Sphere.fRadius ) )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
float fRootRadius;
|
||
|
||
fRootRadius = Sphere.fRadius * 0.707106f;
|
||
if( fabs( Center.x - Sphere.Center.x ) < ( Extent.x + fRootRadius ) )
|
||
{
|
||
return true;
|
||
}
|
||
if( fabs( Center.y - Sphere.Center.y ) > ( Extent.y + fRootRadius ) )
|
||
{
|
||
return true;
|
||
}
|
||
if( fabs( Center.z - Sphere.Center.z ) > ( Extent.z + fRootRadius ) )
|
||
{
|
||
return true;
|
||
}
|
||
|
||
return false;*/
|
||
}
|
||
|
||
bool TestSphereToSphere( SSphere &Sphere1, SSphere &Sphere2 )
|
||
{
|
||
float fLengthSq;
|
||
|
||
fLengthSq = EtVec3LengthSq( &( Sphere1.Center - Sphere2.Center ) );
|
||
if( fLengthSq > ( Sphere1.fRadius + Sphere2.fRadius ) * ( Sphere1.fRadius + Sphere2.fRadius ) )
|
||
{
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool TestSphereToCapsule( SSphere &Sphere, SCapsule &Capsule, float &fSegParam )
|
||
{
|
||
float fLengthSq;
|
||
|
||
fLengthSq = DistPointToSegment( Sphere.Center, Capsule.Segment, fSegParam );
|
||
|
||
return fLengthSq <= ( Capsule.fRadius + Sphere.fRadius ) * ( Capsule.fRadius + Sphere.fRadius );
|
||
}
|
||
|
||
bool TestBoxToBox( SAABox &Box1, SAABox &Box2 )
|
||
{
|
||
EtVector3 Center1, Center2;
|
||
EtVector3 Extent1, Extent2;
|
||
|
||
Center1 = ( Box1.Max + Box1.Min ) * 0.5f;
|
||
Center2 = ( Box2.Max + Box2.Min ) * 0.5f;
|
||
Extent1 = ( Box1.Max - Box1.Min ) * 0.5f;
|
||
Extent2 = ( Box2.Max - Box2.Min ) * 0.5f;
|
||
|
||
if( FastAbs( Center1.x - Center2.x ) > ( Extent1.x + Extent2.x ) )
|
||
{
|
||
return false;
|
||
}
|
||
if( FastAbs( Center1.y - Center2.y ) > ( Extent1.y + Extent2.y ) )
|
||
{
|
||
return false;
|
||
}
|
||
if( FastAbs( Center1.z - Center2.z ) > ( Extent1.z + Extent2.z ) )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool TestCircleToCircle( SCircle &Circle1, SCircle &Circle2 )
|
||
{
|
||
float fRadiusSum;
|
||
|
||
fRadiusSum = Circle1.fRadius + Circle2.fRadius;
|
||
if( EtVec2LengthSq( &( Circle1.Center - Circle2.Center ) ) < fRadiusSum * fRadiusSum )
|
||
{
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
bool TestCircleToBox2D( SCircle &Circle, SAABox2D &Box )
|
||
{
|
||
int i;
|
||
float fRadiusSq;
|
||
DNVector(EtVector2) vecVertex;
|
||
Box.GetVertices( vecVertex );
|
||
fRadiusSq = Circle.fRadius * Circle.fRadius;
|
||
for( i = 0; i < ( int )vecVertex.size(); i++ )
|
||
{
|
||
if( EtVec2LengthSq( &( vecVertex[ i ] - Circle.Center ) ) < fRadiusSq )
|
||
{
|
||
return true;
|
||
}
|
||
}
|
||
if( Box.IsInside( Circle ) )
|
||
{
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
bool TestLineToOBB( EtVector3 &Origin, EtVector3 &Direction, SOBB &Box )
|
||
{
|
||
float fWdU[ 3 ], fAWdU[ 3 ], fDdU[ 3 ], fADdU[ 3 ], fAWxDdU[ 3 ], fRhs;
|
||
|
||
EtVector3 Diff = Origin - Box.Center;
|
||
|
||
fWdU[ 0 ] = EtVec3Dot( &Direction, &Box.Axis[ 0 ]);
|
||
fAWdU[ 0 ] = ( float )FastAbs( ( float )(fWdU[ 0 ]) );
|
||
fDdU[ 0 ] = EtVec3Dot( &Diff, &Box.Axis[ 0 ] );
|
||
fADdU[ 0 ] = ( float )FastAbs( ( float )(fDdU[ 0 ]) );
|
||
if ( fADdU[ 0 ] > Box.Extent[ 0 ] && fDdU[ 0 ] * fWdU[ 0 ] >= 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fWdU[ 1 ] = EtVec3Dot( &Direction, &Box.Axis[ 1 ]);
|
||
fAWdU[ 1 ] = ( float )FastAbs( ( float )(fWdU[ 1 ]) );
|
||
fDdU[ 1 ] = EtVec3Dot( &Diff, &Box.Axis[ 1 ]);
|
||
fADdU[ 1 ] = ( float )FastAbs( ( float )(fDdU[ 1 ]) );
|
||
if ( fADdU[ 1 ] > Box.Extent[ 1 ] && fDdU[ 1 ] * fWdU[ 1 ] >= 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fWdU[ 2 ] = EtVec3Dot( &Direction, &Box.Axis[ 2 ]);
|
||
fAWdU[ 2 ] = ( float )FastAbs( ( float )(fWdU[ 2 ]) );
|
||
fDdU[ 2 ] = EtVec3Dot( &Diff, &Box.Axis[ 2 ]);
|
||
fADdU[ 2 ] = ( float )FastAbs( ( float )(fDdU[ 2 ]) );
|
||
if ( fADdU[ 2 ] > Box.Extent[ 2 ] && fDdU[ 2 ] * fWdU[ 2 ] >= 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
EtVector3 WxD;
|
||
EtVec3Cross( &WxD, &Direction, &Diff );
|
||
|
||
fAWxDdU[ 0 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 0 ] ) ) );
|
||
fRhs = Box.Extent[ 1 ] * fAWdU[ 2 ] + Box.Extent[ 2 ] * fAWdU[ 1 ];
|
||
if ( fAWxDdU[ 0 ] > fRhs )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fAWxDdU[ 1 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 1 ] ) ) );
|
||
fRhs = Box.Extent[ 0 ] * fAWdU[ 2 ] + Box.Extent[ 2 ] * fAWdU[ 0 ];
|
||
if ( fAWxDdU[ 1 ] > fRhs )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fAWxDdU[ 2 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 2 ] ) ) );
|
||
fRhs = Box.Extent[ 0 ] * fAWdU[ 1 ] + Box.Extent[ 1 ] * fAWdU[ 0 ];
|
||
if ( fAWxDdU[ 2 ] > fRhs )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool TestOBBToOBB( SOBB &Box1, SOBB &Box2 )
|
||
{
|
||
EtVector3 Min, Max;
|
||
float fExtentDiagonal[3];
|
||
|
||
Box1.GetDiagonal( Min, Max, Box2.Axis[ 0 ] );
|
||
fExtentDiagonal[ 0 ] = FastAbs( EtVec3Dot( &( ( Max - Min ) *0.5f ), Box2.Axis ) );
|
||
|
||
Box1.GetDiagonal( Min, Max, Box2.Axis[ 1 ] );
|
||
fExtentDiagonal[ 1 ] = FastAbs( EtVec3Dot( &( ( Max - Min ) * 0.5f ), Box2.Axis + 1 ) );
|
||
|
||
Box1.GetDiagonal( Min, Max, Box2.Axis[ 2 ] );
|
||
fExtentDiagonal[ 2 ] = FastAbs( EtVec3Dot( &( ( Max - Min ) * 0.5f ), Box2.Axis + 2 ) );
|
||
|
||
EtVector3 CenterDist;
|
||
|
||
CenterDist = Box1.Center - Box2.Center;
|
||
if( Box2.Extent[ 0 ] + fExtentDiagonal[ 0 ] < FastAbs( EtVec3Dot( &CenterDist, Box2.Axis ) ) )
|
||
{
|
||
return false;
|
||
}
|
||
if( Box2.Extent[ 1 ] + fExtentDiagonal[ 1 ] < FastAbs( EtVec3Dot( &CenterDist, Box2.Axis + 1 ) ) )
|
||
{
|
||
return false;
|
||
}
|
||
if( Box2.Extent[ 2 ] + fExtentDiagonal[ 2 ] < FastAbs( EtVec3Dot( &CenterDist, Box2.Axis + 2 ) ) )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool TestOBBToSphere( SOBB &Box, SSphere &Sphere )
|
||
{
|
||
EtVector3 Diff = Sphere.Center - Box.Center;
|
||
|
||
float fAx = FastAbs( EtVec3Dot( &Diff, Box.Axis ) );
|
||
float fAy = FastAbs( EtVec3Dot( &Diff, Box.Axis + 1 ) );
|
||
float fAz = FastAbs( EtVec3Dot( &Diff, Box.Axis + 2 ) );
|
||
float fDx = fAx - Box.Extent[ 0 ];
|
||
float fDy = fAy - Box.Extent[ 1 ];
|
||
float fDz = fAz - Box.Extent[ 2 ];
|
||
float fSqrtRadius;
|
||
|
||
if ( fAx <= Box.Extent[ 0 ] )
|
||
{
|
||
if ( fAy <= Box.Extent[ 1 ] )
|
||
{
|
||
if ( fAz <= Box.Extent[ 2 ] )
|
||
{
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
return fDz <= Sphere.fRadius;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if ( fAz <= Box.Extent[ 2 ] )
|
||
{
|
||
return fDy <= Sphere.fRadius;
|
||
}
|
||
else
|
||
{
|
||
fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
|
||
return fDy * fDy + fDz * fDz <= fSqrtRadius;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if ( fAy <= Box.Extent[ 1 ] )
|
||
{
|
||
if ( fAz <= Box.Extent[ 2 ] )
|
||
{
|
||
return fDx <= Sphere.fRadius;
|
||
}
|
||
else
|
||
{
|
||
fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
|
||
return fDx*fDx + fDz*fDz <= fSqrtRadius;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if ( fAz <= Box.Extent[ 2 ] )
|
||
{
|
||
fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
|
||
return fDx*fDx + fDy*fDy <= fSqrtRadius;
|
||
}
|
||
else
|
||
{
|
||
fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
|
||
return fDx*fDx + fDy*fDy + fDz*fDz <= fSqrtRadius;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*bool TestOBBToCapsule( SOBB &Box, SCapsule &Capsule, float &fSegParam )
|
||
{
|
||
float fLengthSq;
|
||
float fBoxParam0, fBoxParam1, fBoxParam2;
|
||
|
||
fLengthSq = DistSegToOBB( Capsule.Segment, Box, fSegParam, fBoxParam0, fBoxParam1, fBoxParam2 );
|
||
|
||
return fLengthSq <= Capsule.fRadius * Capsule.fRadius;
|
||
}*/
|
||
|
||
bool TestCapsuleToCapsule( SCapsule &Capsule1, SCapsule &Capsule2, float &fSegParam1, float &fSegParam2 )
|
||
{
|
||
float fLengthSq;
|
||
|
||
fLengthSq = DistSegmentToSegment( Capsule1.Segment, Capsule2.Segment, fSegParam1, fSegParam2 );
|
||
|
||
return fLengthSq <= ( Capsule1.fRadius + Capsule2.fRadius ) * ( Capsule1.fRadius + Capsule2.fRadius );
|
||
}
|
||
|
||
bool CalcFrustumPlane( EtVector4 &Out, float fX, float fY, float fZ, float fW )
|
||
{
|
||
float fLength, fInvLength;
|
||
|
||
fLength = sqrtf( fX * fX + fY * fY + fZ * fZ );
|
||
if( fLength < NEAR_ZERO )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
fInvLength = 1.0f / fLength;
|
||
Out = EtVector4( -fX * fInvLength, -fY * fInvLength, -fZ * fInvLength, fW * fInvLength );
|
||
|
||
return true;
|
||
}
|
||
|
||
|