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