BOOL sTrimeshCapsuleColliderData::_cldTestSeparatingAxesOfCapsule(
const dVector3 &v0,
const dVector3 &v1,
const dVector3 &v2,
uint8 flags)
{
// calculate caps centers in absolute space
dVector3 vCp0;
vCp0[0] = m_vCapsulePosition[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp0[1] = m_vCapsulePosition[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp0[2] = m_vCapsulePosition[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
dVector3 vCp1;
vCp1[0] = m_vCapsulePosition[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp1[1] = m_vCapsulePosition[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp1[2] = m_vCapsulePosition[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
// reset best axis
m_iBestAxis = 0;
// reset best depth
m_fBestDepth = -MAX_REAL;
// reset separating axis vector
dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
// Epsilon value for checking axis vector length
const dReal fEpsilon = 1e-6f;
// Translate triangle to Cc cord.
SUBTRACT(v0, m_vCapsulePosition, m_vV0);
SUBTRACT(v1, m_vCapsulePosition, m_vV1);
SUBTRACT(v2, m_vCapsulePosition, m_vV2);
// We begin to test for 19 separating axis now
// I wonder does it help if we employ the method like ISA-GJK???
// Or at least we should do experiment and find what axis will
// be most likely to be separating axis to check it first.
// Original
// axis m_vN
//vAxis = -m_vN;
vAxis[0] = - m_vN[0];
vAxis[1] = - m_vN[1];
vAxis[2] = - m_vN[2];
if (!_cldTestAxis(v0, v1, v2, vAxis, 1, TRUE))
{
return FALSE;
}
if (flags & dxTriMeshData::kEdge0)
{
// axis CxE0 - Edge 0
dCROSS(vAxis,=,m_vCapsuleAxis,m_vE0);
//vAxis = dCROSS( m_vCapsuleAxis cross vE0 );
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) {
return FALSE;
}
}
}
void calculate( void )
{
int j;
N4 = N + 4;
for (j = 0; j < 10*5; ++j) {
memdiv5[j][0] = j/5;
memdiv5[j][1] = 10*(j - memdiv5[j][0]*5);
}
for (j = 0; j < 10*25; ++j) {
memdiv25[j][0] = j/25;
memdiv25[j][1] = 10*(j - memdiv25[j][0]*25);
}
for (j = 0; j < 10*239; ++j) {
memdiv239[j][0] = j/239;
memdiv239[j][1] = 10*(j - memdiv239[j][0]*239);
}
SET( a, 0 );
SET( b, 0 );
for( j = 2 * N4 + 1; j >= 3; j -= 2 )
{
SET( c, 1 );
DIVIDE( c, j );
SUBTRACT( a, c, a );
DIVIDE25( a );
SUBTRACT( b, c, b );
DIVIDE239( b );
DIVIDE239( b );
progress();
}
SET( c, 1 );
SUBTRACT( a, c, a );
DIVIDE5( a );
SUBTRACT( b, c, b );
DIVIDE239( b );
MULTIPLY( a, 4 );
SUBTRACT( a, a, b );
MULTIPLY( a, 4 );
progress();
}
inline void MACD(int short_window, int long_window, int smooth_window, dvector &input, dvector &macd, dvector &macd_signal, dvector &macd_hist)
{
std::vector<double> short_emas;
std::vector<double> long_emas;
short_emas.reserve(input.size());
long_emas.reserve(input.size());
macd.reserve(input.size());
macd_signal.reserve(input.size());
macd_hist.reserve(input.size());
EMA(short_window,input,short_emas);
EMA(long_window,input,long_emas);
SUBTRACT(short_emas,long_emas,macd);
EMA(smooth_window,macd,macd_signal);
SUBTRACT(macd,macd_signal,macd_hist);
}
// find two closest points on two lines
static BOOL _cldClosestPointOnTwoLines( dVector3 vPoint1, dVector3 vLenVec1,
dVector3 vPoint2, dVector3 vLenVec2,
dReal &fvalue1, dReal &fvalue2)
{
// calulate denominator
dVector3 vp;
SUBTRACT(vPoint2,vPoint1,vp);
dReal fuaub = dDOT(vLenVec1,vLenVec2);
dReal fq1 = dDOT(vLenVec1,vp);
dReal fq2 = -dDOT(vLenVec2,vp);
dReal fd = 1.0f - fuaub * fuaub;
// if denominator is positive
if (fd > 0.0f) {
// calculate points of closest approach
fd = 1.0f/fd;
fvalue1 = (fq1 + fuaub*fq2)*fd;
fvalue2 = (fuaub*fq1 + fq2)*fd;
return TRUE;
// otherwise
} else {
// lines are parallel
fvalue1 = 0.0f;
fvalue2 = 0.0f;
return FALSE;
}
}
// helper for less key strokes
inline void _CalculateAxis(const dVector3& v1,
const dVector3& v2,
const dVector3& v3,
const dVector3& v4,
dVector3& r)
{
dVector3 t1;
dVector3 t2;
SUBTRACT(v1,v2,t1);
dCROSS(t2,=,t1,v3);
dCROSS(r,=,t2,v4);
}
void calculate( void )
{
int j;
N4 = N + 4;
SET( a, 0 );
SET( b, 0 );
for( j = 2 * N4 + 1; j >= 3; j -= 2 )
{
SET( c, 1 );
DIVIDE( c, j );
SUBTRACT( a, c, a );
DIVIDE25(a);//DIVIDE( a, 25 );
SUBTRACT( b, c, b );
DIVIDE239(b);//DIVIDE( b, 239 );
DIVIDE239(b);//DIVIDE( b, 239 );
progress();
}
SET( c, 1 );
SUBTRACT( a, c, a );
DIVIDE5(a); //DIVIDE( a, 5 );
SUBTRACT( b, c, b );
DIVIDE239(b);//DIVIDE( b, 239 );
MULTIPLY( a, 4 );
SUBTRACT( a, a, b );
MULTIPLY( a, 4 );
progress();
}
// clip and generate contacts
static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
// if we have edge/edge intersection
if ( iBestAxis > 4 ) {
dVector3 vub,vPb,vPa;
SET(vPa,vHullBoxPos);
// calculate point on box edge
for( int i=0; i<3; i++) {
dVector3 vRotCol;
GETCOL(mHullBoxRot,i,vRotCol);
dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;
vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0];
vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1];
vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2];
}
int iEdge = (iBestAxis-5)%3;
// decide which edge is on triangle
if ( iEdge == 0 ) {
SET(vPb,v0);
SET(vub,vE0);
} else if ( iEdge == 1) {
SET(vPb,v2);
SET(vub,vE1);
} else {
SET(vPb,v1);
SET(vub,vE2);
}
// setup direction parameter for face edge
dNormalize3(vub);
dReal fParam1, fParam2;
// setup direction parameter for box edge
dVector3 vua;
int col=(iBestAxis-5)/3;
GETCOL(mHullBoxRot,col,vua);
// find two closest points on both edges
_cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 );
vPa[0] += vua[0]*fParam1;
vPa[1] += vua[1]*fParam1;
vPa[2] += vua[2]*fParam1;
vPb[0] += vub[0]*fParam2;
vPb[1] += vub[1]*fParam2;
vPb[2] += vub[2]*fParam2;
// calculate collision point
dVector3 vPntTmp;
ADD(vPa,vPb,vPntTmp);
vPntTmp[0]*=0.5f;
vPntTmp[1]*=0.5f;
vPntTmp[2]*=0.5f;
// generate contact point between two closest points
#ifdef ORIG
if (ctContacts < (iFlags & 0x0ffff)) {
dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
Contact->depth = fBestDepth;
SET(Contact->normal,vBestNormal);
SET(Contact->pos,vPntTmp);
Contact->g1 = Geom1;
Contact->g2 = Geom2;
ctContacts++;
}
#endif
GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
vPntTmp, vBestNormal, fBestDepth, ctContacts);
// if triangle is the referent face then clip box to triangle face
} else if ( iBestAxis == 1 ) {
dVector3 vNormal2;
vNormal2[0]=-vBestNormal[0];
vNormal2[1]=-vBestNormal[1];
vNormal2[2]=-vBestNormal[2];
// vNr is normal in box frame, pointing from triangle to box
dMatrix3 mTransposed;
mTransposed[0*4+0]=mHullBoxRot[0*4+0];
mTransposed[0*4+1]=mHullBoxRot[1*4+0];
mTransposed[0*4+2]=mHullBoxRot[2*4+0];
mTransposed[1*4+0]=mHullBoxRot[0*4+1];
mTransposed[1*4+1]=mHullBoxRot[1*4+1];
mTransposed[1*4+2]=mHullBoxRot[2*4+1];
mTransposed[2*4+0]=mHullBoxRot[0*4+2];
mTransposed[2*4+1]=mHullBoxRot[1*4+2];
mTransposed[2*4+2]=mHullBoxRot[2*4+2];
dVector3 vNr;
vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2];
vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2];
vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2];
dVector3 vAbsNormal;
vAbsNormal[0] = dFabs( vNr[0] );
vAbsNormal[1] = dFabs( vNr[1] );
vAbsNormal[2] = dFabs( vNr[2] );
// get closest face from box
int iB0, iB1, iB2;
if (vAbsNormal[1] > vAbsNormal[0]) {
if (vAbsNormal[1] > vAbsNormal[2]) {
iB1 = 0; iB0 = 1; iB2 = 2;
} else {
iB1 = 0; iB2 = 1; iB0 = 2;
}
} else {
if (vAbsNormal[0] > vAbsNormal[2]) {
iB0 = 0; iB1 = 1; iB2 = 2;
} else {
iB1 = 0; iB2 = 1; iB0 = 2;
}
}
// Here find center of box face we are going to project
dVector3 vCenter;
dVector3 vRotCol;
GETCOL(mHullBoxRot,iB0,vRotCol);
if (vNr[iB0] > 0) {
vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0];
vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1];
vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2];
} else {
vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0];
vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1];
vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2];
}
// Here find 4 corner points of box
dVector3 avPoints[4];
dVector3 vRotCol2;
GETCOL(mHullBoxRot,iB1,vRotCol);
GETCOL(mHullBoxRot,iB2,vRotCol2);
for(int x=0;x<3;x++) {
avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
}
// clip Box face with 4 planes of triangle (1 face plane, 3 egde planes)
dVector3 avTempArray1[9];
dVector3 avTempArray2[9];
dVector4 plPlane;
int iTempCnt1=0;
int iTempCnt2=0;
// zeroify vectors - necessary?
for(int i=0; i<9; i++) {
avTempArray1[i][0]=0;
avTempArray1[i][1]=0;
avTempArray1[i][2]=0;
avTempArray2[i][0]=0;
avTempArray2[i][1]=0;
avTempArray2[i][2]=0;
}
// Normal plane
dVector3 vTemp;
vTemp[0]=-vN[0];
vTemp[1]=-vN[1];
vTemp[2]=-vN[2];
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane );
// Plane p0
dVector3 vTemp2;
SUBTRACT(v1,v0,vTemp2);
dCROSS(vTemp,=,vN,vTemp2);
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
// Plane p1
SUBTRACT(v2,v1,vTemp2);
dCROSS(vTemp,=,vN,vTemp2);
dNormalize3(vTemp);
SUBTRACT(v0,v2,vTemp2);
CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp));
_cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
// Plane p2
SUBTRACT(v0,v2,vTemp2);
dCROSS(vTemp,=,vN,vTemp2);
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
// END of clipping polygons
// for each generated contact point
for ( int i=0; i<iTempCnt2; i++ ) {
// calculate depth
dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]);
// clamp depth to zero
if (fTempDepth > 0) {
fTempDepth = 0;
}
dVector3 vPntTmp;
ADD(avTempArray2[i],v0,vPntTmp);
#ifdef ORIG
if (ctContacts < (iFlags & 0x0ffff)) {
dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
Contact->depth = -fTempDepth;
SET(Contact->normal,vBestNormal);
SET(Contact->pos,vPntTmp);
Contact->g1 = Geom1;
Contact->g2 = Geom2;
ctContacts++;
}
#endif
GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
vPntTmp, vBestNormal, -fTempDepth, ctContacts);
}
//dAASSERT(ctContacts>0);
// if box face is the referent face, then clip triangle on box face
} else { // 2 <= if iBestAxis <= 4
static BOOL _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
// reset best axis
iBestAxis = 0;
iExitAxis = -1;
fBestDepth = MAXVALUE;
// calculate edges
SUBTRACT(v1,v0,vE0);
SUBTRACT(v2,v0,vE1);
SUBTRACT(vE1,vE0,vE2);
// calculate poly normal
dCROSS(vN,=,vE0,vE1);
// extract box axes as vectors
dVector3 vA0,vA1,vA2;
GETCOL(mHullBoxRot,0,vA0);
GETCOL(mHullBoxRot,1,vA1);
GETCOL(mHullBoxRot,2,vA2);
// box halfsizes
dReal fa0 = vBoxHalfSize[0];
dReal fa1 = vBoxHalfSize[1];
dReal fa2 = vBoxHalfSize[2];
// calculate relative position between box and triangle
dVector3 vD;
SUBTRACT(v0,vHullBoxPos,vD);
// calculate length of face normal
dReal fNLen = LENGTHOF( vN );
dVector3 vL;
dReal fp0, fp1, fp2, fR, fD;
// Test separating axes for intersection
// ************************************************
// Axis 1 - Triangle Normal
SET(vL,vN);
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0;
fR=fa0*dFabs( dDOT(vN,vA0) ) + fa1 * dFabs( dDOT(vN,vA1) ) + fa2 * dFabs( dDOT(vN,vA2) );
if( !_cldTestNormal( fp0, fR, vL, 1) ) {
iExitAxis=1;
return FALSE;
}
// ************************************************
// Test Faces
// ************************************************
// Axis 2 - Box X-Axis
SET(vL,vA0);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA0,vE0);
fp2 = fp0 + dDOT(vA0,vE1);
fR = fa0;
if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 2) ) {
iExitAxis=2;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 3 - Box Y-Axis
SET(vL,vA1);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA1,vE0);
fp2 = fp0 + dDOT(vA1,vE1);
fR = fa1;
if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 3) ) {
iExitAxis=3;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 4 - Box Z-Axis
SET(vL,vA2);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA2,vE0);
fp2 = fp0 + dDOT(vA2,vE1);
fR = fa2;
if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 4) ) {
iExitAxis=4;
return FALSE;
}
// ************************************************
// Test Edges
// ************************************************
// Axis 5 - Box X-Axis cross Edge0
dCROSS(vL,=,vA0,vE0);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA0,vN);
fR = fa1 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA1,vE0));
if( !_cldTestEdge( fp1, fp2, fR, fD, vL, 5) ) {
iExitAxis=5;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 6 - Box X-Axis cross Edge1
dCROSS(vL,=,vA0,vE1);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA0,vN);
fp2 = fp0;
fR = fa1 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA1,vE1));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 6) ) {
iExitAxis=6;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 7 - Box X-Axis cross Edge2
dCROSS(vL,=,vA0,vE2);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA0,vN);
fp2 = fp0 - dDOT(vA0,vN);
fR = fa1 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA1,vE2));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 7) ) {
iExitAxis=7;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 8 - Box Y-Axis cross Edge0
dCROSS(vL,=,vA1,vE0);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA1,vN);
fR = fa0 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA0,vE0));
if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 8) ) {
iExitAxis=8;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 9 - Box Y-Axis cross Edge1
dCROSS(vL,=,vA1,vE1);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA1,vN);
fp2 = fp0;
fR = fa0 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA0,vE1));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 9) ) {
iExitAxis=9;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 10 - Box Y-Axis cross Edge2
dCROSS(vL,=,vA1,vE2);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA1,vN);
fp2 = fp0 - dDOT(vA1,vN);
fR = fa0 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA0,vE2));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 10) ) {
iExitAxis=10;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 11 - Box Z-Axis cross Edge0
dCROSS(vL,=,vA2,vE0);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA2,vN);
fR = fa0 * dFabs(dDOT(vA1,vE0)) + fa1 * dFabs(dDOT(vA0,vE0));
if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 11) ) {
iExitAxis=11;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 12 - Box Z-Axis cross Edge1
dCROSS(vL,=,vA2,vE1);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA2,vN);
fp2 = fp0;
fR = fa0 * dFabs(dDOT(vA1,vE1)) + fa1 * dFabs(dDOT(vA0,vE1));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 12) ) {
iExitAxis=12;
return FALSE;
}
// ************************************************
// ************************************************
// Axis 13 - Box Z-Axis cross Edge2
dCROSS(vL,=,vA2,vE2);
fD = dDOT(vL,vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA2,vN);
fp2 = fp0 - dDOT(vA2,vN);
fR = fa0 * dFabs(dDOT(vA1,vE2)) + fa1 * dFabs(dDOT(vA0,vE2));
if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 13) ) {
iExitAxis=13;
return FALSE;
}
// ************************************************
return TRUE;
}
// test one mesh triangle on intersection with capsule
void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule(
const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
uint8 flags)
{
// calculate edges
SUBTRACT(v1,v0,m_vE0);
SUBTRACT(v2,v1,m_vE1);
SUBTRACT(v0,v2,m_vE2);
dVector3 _minus_vE0;
SUBTRACT(v0,v1,_minus_vE0);
// calculate poly normal
dCalcVectorCross3(m_vN,m_vE1,_minus_vE0);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(m_vN))
{
return;
}
// create plane from triangle
dReal plDistance = -dCalcVectorDot3(v0,m_vN);
dVector4 plTrianglePlane;
CONSTRUCTPLANE(plTrianglePlane,m_vN,plDistance);
// calculate capsule distance to plane
dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,m_vCapsulePosition);
// Capsule must be over positive side of triangle
if (fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/)
{
// if not don't generate contacts
return;
}
dVector3 vPnt0;
SET (vPnt0,v0);
dVector3 vPnt1;
SET (vPnt1,v1);
dVector3 vPnt2;
SET (vPnt2,v2);
if (fDistanceCapsuleCenterToPlane < 0 )
{
SET (vPnt0,v0);
SET (vPnt1,v2);
SET (vPnt2,v1);
}
// do intersection test and find best separating axis
if (!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags))
{
// if not found do nothing
return;
}
// if best separation axis is not found
if (m_iBestAxis == 0 )
{
// this should not happen (we should already exit in that case)
dIASSERT(FALSE);
// do nothing
return;
}
// calculate caps centers in absolute space
dVector3 vCposTrans;
vCposTrans[0] = m_vCapsulePosition[0] + m_vNormal[0]*m_vCapsuleRadius;
vCposTrans[1] = m_vCapsulePosition[1] + m_vNormal[1]*m_vCapsuleRadius;
vCposTrans[2] = m_vCapsulePosition[2] + m_vNormal[2]*m_vCapsuleRadius;
dVector3 vCEdgePoint0;
vCEdgePoint0[0] = vCposTrans[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[1] = vCposTrans[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[2] = vCposTrans[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
dVector3 vCEdgePoint1;
vCEdgePoint1[0] = vCposTrans[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[1] = vCposTrans[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[2] = vCposTrans[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
// transform capsule edge points into triangle space
vCEdgePoint0[0] -= vPnt0[0];
vCEdgePoint0[1] -= vPnt0[1];
vCEdgePoint0[2] -= vPnt0[2];
vCEdgePoint1[0] -= vPnt0[0];
vCEdgePoint1[1] -= vPnt0[1];
vCEdgePoint1[2] -= vPnt0[2];
dVector4 plPlane;
dVector3 _minus_vN;
_minus_vN[0] = -m_vN[0];
_minus_vN[1] = -m_vN[1];
_minus_vN[2] = -m_vN[2];
// triangle plane
CONSTRUCTPLANE(plPlane,_minus_vN,0);
//plPlane = Plane4f( -m_vN, 0);
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
// plane with edge 0
dVector3 vTemp;
dCalcVectorCross3(vTemp,m_vN,m_vE0);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE1);
CONSTRUCTPLANE(plPlane, vTemp, -(dCalcVectorDot3(m_vE0,vTemp)-REAL(1e-5)));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE2);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) {
return;
}
// return capsule edge points into absolute space
vCEdgePoint0[0] += vPnt0[0];
vCEdgePoint0[1] += vPnt0[1];
vCEdgePoint0[2] += vPnt0[2];
vCEdgePoint1[0] += vPnt0[0];
vCEdgePoint1[1] += vPnt0[1];
vCEdgePoint1[2] += vPnt0[2];
// calculate depths for both contact points
SUBTRACT(vCEdgePoint0,m_vCapsulePosition,vTemp);
dReal fDepth0 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
SUBTRACT(vCEdgePoint1,m_vCapsulePosition,vTemp);
dReal fDepth1 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
// clamp depths to zero
if (fDepth0 < 0)
{
fDepth0 = 0.0f;
}
if (fDepth1 < 0 )
{
fDepth1 = 0.0f;
}
// Cached contacts's data
// contact 0
dIASSERT(m_ctContacts < (m_iFlags & NUMC_MASK)); // Do not call function if there is no room to store result
m_gLocalContacts[m_ctContacts].fDepth = fDepth0;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint0);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
if (m_ctContacts < (m_iFlags & NUMC_MASK)) {
// contact 1
m_gLocalContacts[m_ctContacts].fDepth = fDepth1;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint1);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
}
}
BOOL sTrimeshCapsuleColliderData::_cldTestSeparatingAxesOfCapsule(
const dVector3 &v0,
const dVector3 &v1,
const dVector3 &v2,
uint8 flags)
{
// calculate caps centers in absolute space
dVector3 vCp0;
vCp0[0] = m_vCapsulePosition[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp0[1] = m_vCapsulePosition[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp0[2] = m_vCapsulePosition[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
dVector3 vCp1;
vCp1[0] = m_vCapsulePosition[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp1[1] = m_vCapsulePosition[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp1[2] = m_vCapsulePosition[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
// reset best axis
m_iBestAxis = 0;
// reset best depth
m_fBestDepth = -MAX_REAL;
// reset separating axis vector
dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
// Epsilon value for checking axis vector length
const dReal fEpsilon = 1e-6f;
// Translate triangle to Cc cord.
SUBTRACT(v0, m_vCapsulePosition, m_vV0);
SUBTRACT(v1, m_vCapsulePosition, m_vV1);
SUBTRACT(v2, m_vCapsulePosition, m_vV2);
// We begin to test for 19 separating axis now
// I wonder does it help if we employ the method like ISA-GJK???
// Or at least we should do experiment and find what axis will
// be most likely to be separating axis to check it first.
// Original
// axis m_vN
//vAxis = -m_vN;
vAxis[0] = - m_vN[0];
vAxis[1] = - m_vN[1];
vAxis[2] = - m_vN[2];
if (!_cldTestAxis(v0, v1, v2, vAxis, 1, TRUE))
{
return FALSE;
}
if (flags & dxTriMeshData::kEdge0)
{
// axis CxE0 - Edge 0
dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE0);
//vAxis = dCalcVectorCross3( m_vCapsuleAxis cross vE0 );
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge1)
{
// axis CxE1 - Edge 1
dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE1);
//vAxis = ( m_vCapsuleAxis cross m_vE1 );
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 3)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge2)
{
// axis CxE2 - Edge 2
//vAxis = ( m_vCapsuleAxis cross m_vE2 );
dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE2);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 4)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge0)
{
// first capsule point
// axis ((Cp0-V0) x E0) x E0
_CalculateAxis(vCp0,v0,m_vE0,m_vE0,vAxis);
// vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 5)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge1)
{
// axis ((Cp0-V1) x E1) x E1
_CalculateAxis(vCp0,v1,m_vE1,m_vE1,vAxis);
//vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 6)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge2)
{
// axis ((Cp0-V2) x E2) x E2
_CalculateAxis(vCp0,v2,m_vE2,m_vE2,vAxis);
//vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 7)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge0)
{
// second capsule point
// axis ((Cp1-V0) x E0) x E0
_CalculateAxis(vCp1,v0,m_vE0,m_vE0,vAxis);
//vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 8)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge1)
{
// axis ((Cp1-V1) x E1) x E1
_CalculateAxis(vCp1,v1,m_vE1,m_vE1,vAxis);
//vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 9)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge2)
{
// axis ((Cp1-V2) x E2) x E2
_CalculateAxis(vCp1,v2,m_vE2,m_vE2,vAxis);
//vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 10)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert0)
{
// first vertex on triangle
// axis ((V0-Cp0) x C) x C
_CalculateAxis(v0,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis);
//vAxis = ( ( v0-vCp0 ) cross m_vCapsuleAxis ) cross m_vCapsuleAxis;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 11)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert1)
{
// second vertex on triangle
// axis ((V1-Cp0) x C) x C
_CalculateAxis(v1,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis);
//vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 12)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert2)
{
// third vertex on triangle
// axis ((V2-Cp0) x C) x C
_CalculateAxis(v2,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis);
//vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 13)) {
return FALSE;
}
}
}
// Test as separating axes direction vectors between each triangle
// edge and each capsule's cap center
if (flags & dxTriMeshData::kVert0)
{
// first triangle vertex and first capsule point
//vAxis = v0 - vCp0;
SUBTRACT(v0,vCp0,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 14)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert1)
{
// second triangle vertex and first capsule point
//vAxis = v1 - vCp0;
SUBTRACT(v1,vCp0,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 15)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert2)
{
// third triangle vertex and first capsule point
//vAxis = v2 - vCp0;
SUBTRACT(v2,vCp0,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 16)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert0)
{
// first triangle vertex and second capsule point
//vAxis = v0 - vCp1;
SUBTRACT(v0,vCp1,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 17)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert1)
{
// second triangle vertex and second capsule point
//vAxis = v1 - vCp1;
SUBTRACT(v1,vCp1,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 18)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert2)
{
// third triangle vertex and second capsule point
//vAxis = v2 - vCp1;
SUBTRACT(v2,vCp1,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 19)) {
return FALSE;
}
}
}
return TRUE;
}
bool sTrimeshBoxColliderData::_cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
// reset best axis
m_iBestAxis = 0;
m_iExitAxis = -1;
m_fBestDepth = MAXVALUE;
// calculate edges
SUBTRACT(v1,v0,m_vE0);
SUBTRACT(v2,v0,m_vE1);
SUBTRACT(m_vE1,m_vE0,m_vE2);
// calculate poly normal
dCROSS(m_vN,=,m_vE0,m_vE1);
// calculate length of face normal
dReal fNLen = LENGTHOF(m_vN);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!fNLen) {
return false;
}
// extract box axes as vectors
dVector3 vA0,vA1,vA2;
GETCOL(m_mHullBoxRot,0,vA0);
GETCOL(m_mHullBoxRot,1,vA1);
GETCOL(m_mHullBoxRot,2,vA2);
// box halfsizes
dReal fa0 = m_vBoxHalfSize[0];
dReal fa1 = m_vBoxHalfSize[1];
dReal fa2 = m_vBoxHalfSize[2];
// calculate relative position between box and triangle
dVector3 vD;
SUBTRACT(v0,m_vHullBoxPos,vD);
dVector3 vL;
dReal fp0, fp1, fp2, fR, fD;
// Test separating axes for intersection
// ************************************************
// Axis 1 - Triangle Normal
SET(vL,m_vN);
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0;
fR=fa0*dFabs( dDOT(m_vN,vA0) ) + fa1 * dFabs( dDOT(m_vN,vA1) ) + fa2 * dFabs( dDOT(m_vN,vA2) );
if (!_cldTestNormal(fp0, fR, vL, 1)) {
m_iExitAxis=1;
return false;
}
// ************************************************
// Test Faces
// ************************************************
// Axis 2 - Box X-Axis
SET(vL,vA0);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA0,m_vE0);
fp2 = fp0 + dDOT(vA0,m_vE1);
fR = fa0;
if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 2)) {
m_iExitAxis=2;
return false;
}
// ************************************************
// ************************************************
// Axis 3 - Box Y-Axis
SET(vL,vA1);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA1,m_vE0);
fp2 = fp0 + dDOT(vA1,m_vE1);
fR = fa1;
if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 3)) {
m_iExitAxis=3;
return false;
}
// ************************************************
// ************************************************
// Axis 4 - Box Z-Axis
SET(vL,vA2);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 + dDOT(vA2,m_vE0);
fp2 = fp0 + dDOT(vA2,m_vE1);
fR = fa2;
if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 4)) {
m_iExitAxis=4;
return false;
}
// ************************************************
// Test Edges
// ************************************************
// Axis 5 - Box X-Axis cross Edge0
dCROSS(vL,=,vA0,m_vE0);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA0,m_vN);
fR = fa1 * dFabs(dDOT(vA2,m_vE0)) + fa2 * dFabs(dDOT(vA1,m_vE0));
if (!_cldTestEdge(fp1, fp2, fR, fD, vL, 5)) {
m_iExitAxis=5;
return false;
}
// ************************************************
// ************************************************
// Axis 6 - Box X-Axis cross Edge1
dCROSS(vL,=,vA0,m_vE1);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA0,m_vN);
fp2 = fp0;
fR = fa1 * dFabs(dDOT(vA2,m_vE1)) + fa2 * dFabs(dDOT(vA1,m_vE1));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 6)) {
m_iExitAxis=6;
return false;
}
// ************************************************
// ************************************************
// Axis 7 - Box X-Axis cross Edge2
dCROSS(vL,=,vA0,m_vE2);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA0,m_vN);
fp2 = fp0 - dDOT(vA0,m_vN);
fR = fa1 * dFabs(dDOT(vA2,m_vE2)) + fa2 * dFabs(dDOT(vA1,m_vE2));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 7)) {
m_iExitAxis=7;
return false;
}
// ************************************************
// ************************************************
// Axis 8 - Box Y-Axis cross Edge0
dCROSS(vL,=,vA1,m_vE0);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA1,m_vN);
fR = fa0 * dFabs(dDOT(vA2,m_vE0)) + fa2 * dFabs(dDOT(vA0,m_vE0));
if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 8)) {
m_iExitAxis=8;
return false;
}
// ************************************************
// ************************************************
// Axis 9 - Box Y-Axis cross Edge1
dCROSS(vL,=,vA1,m_vE1);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA1,m_vN);
fp2 = fp0;
fR = fa0 * dFabs(dDOT(vA2,m_vE1)) + fa2 * dFabs(dDOT(vA0,m_vE1));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 9)) {
m_iExitAxis=9;
return false;
}
// ************************************************
// ************************************************
// Axis 10 - Box Y-Axis cross Edge2
dCROSS(vL,=,vA1,m_vE2);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA1,m_vN);
fp2 = fp0 - dDOT(vA1,m_vN);
fR = fa0 * dFabs(dDOT(vA2,m_vE2)) + fa2 * dFabs(dDOT(vA0,m_vE2));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 10)) {
m_iExitAxis=10;
return false;
}
// ************************************************
// ************************************************
// Axis 11 - Box Z-Axis cross Edge0
dCROSS(vL,=,vA2,m_vE0);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0;
fp2 = fp0 + dDOT(vA2,m_vN);
fR = fa0 * dFabs(dDOT(vA1,m_vE0)) + fa1 * dFabs(dDOT(vA0,m_vE0));
if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 11)) {
m_iExitAxis=11;
return false;
}
// ************************************************
// ************************************************
// Axis 12 - Box Z-Axis cross Edge1
dCROSS(vL,=,vA2,m_vE1);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA2,m_vN);
fp2 = fp0;
fR = fa0 * dFabs(dDOT(vA1,m_vE1)) + fa1 * dFabs(dDOT(vA0,m_vE1));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 12)) {
m_iExitAxis=12;
return false;
}
// ************************************************
// ************************************************
// Axis 13 - Box Z-Axis cross Edge2
dCROSS(vL,=,vA2,m_vE2);
fD = dDOT(vL,m_vN)/fNLen;
fp0 = dDOT(vL,vD);
fp1 = fp0 - dDOT(vA2,m_vN);
fp2 = fp0 - dDOT(vA2,m_vN);
fR = fa0 * dFabs(dDOT(vA1,m_vE2)) + fa1 * dFabs(dDOT(vA0,m_vE2));
if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 13)) {
m_iExitAxis=13;
return false;
}
// ************************************************
return true;
}
int pointblankrange_calculate(lpPointBlankRange pointblankrange)
{
Vector dr, r, tv, v, w;
double mv, vm, elev, mh, vz, bw;
double dt, eq, t, mach, drg;
int i, itcnt, mhr;
/* Set values that don't change for the pointblankrange... */
if (pointblankrange->alc)
atmos_standardalt(pointblankrange->atmos);
else
atmos_atmos(pointblankrange->atmos);
mach = pointblankrange->atmos->mach;
eq = pointblankrange->atmos->density/ATMOS_DENSSTD;
vz = INTOFT(pointblankrange->vital_zone);
bw = pointblankrange->bullet_weight;
itcnt = 0;
pointblankrange->found = 0;
elev = vz/(250.0*PBR_DX);
while ((!pointblankrange->found) && (itcnt < PBR_MAXIMUM_ITCNT))
{
vm = pointblankrange->muzzle_velocity;
pointblankrange->muzzle_energy = PBR_ENERGY(bw, vm);
t = 0.0;
mh = INTOFT(-pointblankrange->sight_height);
r = vector(0.0, mh, 0.0);
dr = vector(0.0, 0.0, 0.0);
v.x = vm*cos(elev);
v.y = vm*sin(elev);
v.z = 0.0;
for (i = 0; (r.y > -vz); i++)
{
if (vm < PBR_ABSMINVX) break;
vm = LENGTH(v);
dt = 0.5*PBR_DX/v.x;
drg = eq*vm*bc_getdrag(pointblankrange->bc, vm/mach);
tv = SUBTRACT(v, MULTIPLY(dt, SUBTRACT(MULTIPLY(drg, v), PBR_GRAVITY)));
vm = LENGTH(tv);
dt = PBR_DX/tv.x;
drg = eq*vm*bc_getdrag(pointblankrange->bc, vm/mach);
v = SUBTRACT(v, MULTIPLY(dt, SUBTRACT(MULTIPLY(drg, tv), PBR_GRAVITY)));
dr = vector(PBR_DX, v.y*dt, v.z*dt);
r = ADD(r, dr);
t = t + LENGTH(dr)/vm;
vm = LENGTH(v);
if (r.y > 0.0) pointblankrange->pbzero = i;
pointblankrange->pbrange = i;
if (r.y > mh)
{
mh = r.y;
mhr = i;
}
}
pointblankrange->found = (fabs(mh - vz) < PBR_MAXIMUM_ERROR);
if (!pointblankrange->found)
{
elev = elev - (mh - vz)/(mhr*PBR_DX);
}
itcnt++;
pointblankrange->terminal_energy = PBR_ENERGY(bw, vm);
}
return 0;
}