bool _cldTestSeparatingAxes(sData& cData,
const dVector3 &v0,
const dVector3 &v1,
const dVector3 &v2)
{
// calculate edge vectors
dVector3Subtract(v1 ,v0 , cData.vE0);
// cData.vE1 has been calculated before -> so save some cycles here
dVector3Subtract(v0 ,v2 , cData.vE2);
// calculate caps centers in absolute space
dVector3 vCp0;
vCp0[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
vCp0[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
vCp0[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));
dVector3 vCp1;
vCp1[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
vCp1[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
vCp1[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));
// reset best axis
cData.iBestAxis = 0;
dVector3 vAxis;
// axis cData.vNormal
//vAxis = -cData.vNormal;
vAxis[0] = -cData.vNormal[0];
vAxis[1] = -cData.vNormal[1];
vAxis[2] = -cData.vNormal[2];
if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 1, true))
{
return false;
}
// axis CxE0
// vAxis = ( cData.vCylinderAxis cross cData.vE0 );
dVector3Cross(cData.vCylinderAxis, cData.vE0,vAxis);
if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 2))
{
return false;
}
// axis CxE1
// vAxis = ( cData.vCylinderAxis cross cData.vE1 );
dVector3Cross(cData.vCylinderAxis, cData.vE1,vAxis);
if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 3))
{
return false;
}
// axis CxE2
// vAxis = ( cData.vCylinderAxis cross cData.vE2 );
dVector3Cross(cData.vCylinderAxis, cData.vE2,vAxis);
if (!_cldTestAxis( cData ,v0, v1, v2, vAxis, 4))
{
return false;
}
// first vertex on triangle
// axis ((V0-Cp0) x C) x C
//vAxis = ( ( v0-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
_CalculateAxis(v0 , vCp0 , cData.vCylinderAxis , vAxis);
if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 11))
{
return false;
}
// second vertex on triangle
// axis ((V1-Cp0) x C) x C
// vAxis = ( ( v1-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
_CalculateAxis(v1 , vCp0 , cData.vCylinderAxis , vAxis);
if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 12))
{
return false;
}
// third vertex on triangle
// axis ((V2-Cp0) x C) x C
//vAxis = ( ( v2-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
_CalculateAxis(v2 , vCp0 , cData.vCylinderAxis , vAxis);
if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 13))
{
return false;
}
// test cylinder axis
// vAxis = cData.vCylinderAxis;
dVector3Copy(cData.vCylinderAxis , vAxis);
if (!_cldTestAxis(cData , v0, v1, v2, vAxis, 14))
{
return false;
}
// Test top and bottom circle ring of cylinder for separation
dVector3 vccATop;
vccATop[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
vccATop[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
vccATop[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));
dVector3 vccABottom;
vccABottom[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
vccABottom[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
vccABottom[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));
if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v1, 15))
{
return false;
}
if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v1, v2, 16))
{
return false;
}
if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v2, 17))
{
return false;
}
if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v1, 18))
{
return false;
}
if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v1, v2, 19))
{
return false;
}
if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v2, 20))
{
return false;
}
return true;
}
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;
}