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; } } }
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; }