bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v )
{
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
const SimdScalar marginSqrd = margin * margin;
simplexSolver.reset();
int nbIterations = 0;
while ( true )
{
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
SimdVector3 seperatingAxisInB = v * transformB.getBasis();
SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = transformA( pInA );
SimdPoint3 qWorld = transformB( qInB );
SimdVector3 w = pWorld - qWorld;
delta = v.dot( w );
// potential exit, they don't overlap
if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
{
// Convex shapes do not overlap
// Returning true means that Hybrid's result is ok and there's no need to run EPA
penDepth = 0;
return true;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
{
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
//add current vertex to simplex
simplexSolver.addVertex( w, pWorld, qWorld );
//calculate the closest point to the origin (update vector v)
if ( !simplexSolver.closest( v ) )
{
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
SimdScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2();
//are we getting any closer ?
if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance )
{
simplexSolver.backup_closest( v );
squaredDistance = v.length2();
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
{
// Convex Shapes intersect - we need to run EPA
// Returning false means that Hybrid couldn't do anything for us
// and that we need to run EPA to calculate the pen depth
return false;
}
++nbIterations;
}
}
bool calcPenDepth()
{
// Ryn Hybrid Pen Depth and EPA if necessary
SimdVector3 v( 1, 0, 0 );
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
const SimdScalar margin = g_pConvexShapes[ 0 ]->GetMargin() + g_pConvexShapes[ 1 ]->GetMargin();
const SimdScalar marginSqrd = margin * margin;
SimdScalar maxRelErrorSqrd = 1e-3 * 1e-3;
simplexSolver.reset();
while ( true )
{
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * g_convexShapesTransform[ 0 ].getBasis();
SimdVector3 seperatingAxisInB = v * g_convexShapesTransform[ 1 ].getBasis();
SimdVector3 pInA = g_pConvexShapes[ 0 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = g_pConvexShapes[ 1 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = g_convexShapesTransform[ 0 ]( pInA );
SimdPoint3 qWorld = g_convexShapesTransform[ 1 ]( qInB );
SimdVector3 w = pWorld - qWorld;
delta = v.dot( w );
// potential exit, they don't overlap
if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
{
// Convex shapes do not overlap
return false;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if ( ( squaredDistance - delta <= squaredDistance * maxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
{
simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength );
g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength );
return true;
}
//add current vertex to simplex
simplexSolver.addVertex( w, pWorld, qWorld );
//calculate the closest point to the origin (update vector v)
if ( !simplexSolver.closest( v ) )
{
simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength );
g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength );
return true;
}
SimdScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2();
//are we getting any closer ?
if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance )
{
simplexSolver.backup_closest( v );
squaredDistance = v.length2();
simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength );
g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength );
return true;
}
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
{
// Run EPA
break;
}
}
return epaPenDepthSolver.CalcPenDepth( simplexSolver, g_pConvexShapes[ 0 ], g_pConvexShapes[ 1 ],
g_convexShapesTransform[ 0 ], g_convexShapesTransform[ 1 ], v,
g_wWitnesses[ 0 ], g_wWitnesses[ 1 ], 0 );
}
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
{
int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf);
switch (num_verts)
{
case 1:
// Touching contact. Yes, we have a collision,
// but no penetration.
return false;
case 2:
{
// We have a line segment inside the Minkowski sum containing the
// origin. Blow it up by adding three additional support points.
SimdVector3 dir = (yBuf[1] - yBuf[0]).normalized();
int axis = dir.furthestAxis();
static SimdScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;//
SimdQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, SimdScalar(0.5));
SimdMatrix3x3 rot_mat(rot);
SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2));
SimdVector3 aux2 = rot_mat * aux1;
SimdVector3 aux3 = rot_mat * aux2;
pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis()));
qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis()));
yBuf[2] = pBuf[2] - qBuf[2];
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(aux2*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-aux2)*transformB.getBasis()));
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = transformA(convexA->LocalGetSupportingVertex(aux3*transformA.getBasis()));
qBuf[4] = transformB(convexB->LocalGetSupportingVertex((-aux3)*transformB.getBasis()));
yBuf[4] = pBuf[4] - qBuf[4];
if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4]))
{
pBuf[1] = pBuf[4];
qBuf[1] = qBuf[4];
yBuf[1] = yBuf[4];
}
else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4]))
{
pBuf[0] = pBuf[4];
qBuf[0] = qBuf[4];
yBuf[0] = yBuf[4];
}
else
{
// Origin not in initial polytope
return false;
}
num_verts = 4;
break;
}
case 3:
{
// We have a triangle inside the Minkowski sum containing
// the origin. First blow it up.
SimdVector3 v1 = yBuf[1] - yBuf[0];
SimdVector3 v2 = yBuf[2] - yBuf[0];
SimdVector3 vv = v1.cross(v2);
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis()));
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = transformA(convexA->LocalGetSupportingVertex((-vv)*transformA.getBasis()));
qBuf[4] = transformB(convexB->LocalGetSupportingVertex(vv*transformB.getBasis()));
yBuf[4] = pBuf[4] - qBuf[4];
if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4]))
{
pBuf[3] = pBuf[4];
qBuf[3] = qBuf[4];
yBuf[3] = yBuf[4];
}
else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
{
// Origin not in initial polytope
return false;
}
num_verts = 4;
break;
}
}
// We have a tetrahedron inside the Minkowski sum containing
// the origin (if GJK did it's job right ;-)
if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
{
// assert(false);
return false;
}
num_facets = 0;
freeFacet = 0;
ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY);
if (!f0 || f0->getDist2() == SimdScalar(0.0) ||
!f1 || f1->getDist2() == SimdScalar(0.0) ||
!f2 || f2->getDist2() == SimdScalar(0.0) ||
!f3 || f3->getDist2() == SimdScalar(0.0))
{
return false;
}
f0->link(0, f1, 2);
f0->link(1, f3, 2);
f0->link(2, f2, 0);
f1->link(0, f2, 2);
f1->link(1, f3, 0);
f2->link(1, f3, 1);
if (num_facets == 0)
{
return false;
}
// at least one facet on the heap.
ReplaceMeEdgeBuffer edgeBuffer(20);
ReplaceMeFacet *facet = 0;
SimdScalar upper_bound2 = SIMD_INFINITY;
do {
facet = facetHeap[0];
std::pop_heap(&facetHeap[0], &facetHeap[num_facets], myFacetComp);
--num_facets;
if (!facet->isObsolete())
{
assert(facet->getDist2() > SimdScalar(0.0));
if (num_verts == MaxSupportPoints)
{
#ifdef DEBUG
std::cout << "Ouch, no convergence!!!" << std::endl;
#endif
ASSERT_MESSAGE(false,"Error: pendepth calc failed");
break;
}
pBuf[num_verts] = transformA(convexA->LocalGetSupportingVertex((facet->getClosest())*transformA.getBasis()));
qBuf[num_verts] = transformB(convexB->LocalGetSupportingVertex((-facet->getClosest())*transformB.getBasis()));
yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts];
int index = num_verts++;
SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest());
// Make sure the support mapping is OK.
//assert(far_dist2 > SimdScalar(0.0));
//
// this is to avoid problems with implicit-sphere-touching contact
//
if (far_dist2 < SimdScalar(0.0))
{
return false;
}
GEN_set_min(upper_bound2, (far_dist2 * far_dist2) / facet->getDist2());
if (upper_bound2 <= ReplaceMeAccuracy::depth_tolerance * facet->getDist2()
#define CHECK_NEW_SUPPORT
#ifdef CHECK_NEW_SUPPORT
|| yBuf[index] == yBuf[(*facet)[0]]
|| yBuf[index] == yBuf[(*facet)[1]]
|| yBuf[index] == yBuf[(*facet)[2]]
#endif
)
{
break;
}
// Compute the silhouette cast by the new vertex
// Note that the new vertex is on the positive side
// of the current facet, so the current facet is will
// not be in the convex hull. Start local search
// from this facet.
facet->silhouette(yBuf[index], edgeBuffer);
if (edgeBuffer.empty())
{
return false;
}
ReplaceMeEdgeBuffer::const_iterator it = edgeBuffer.begin();
ReplaceMeFacet *firstFacet =
addFacet((*it).getTarget(), (*it).getSource(),
index, facet->getDist2(), upper_bound2);
if (!firstFacet)
{
break;
}
firstFacet->link(0, (*it).getFacet(), (*it).getIndex());
ReplaceMeFacet *lastFacet = firstFacet;
++it;
for (; it != edgeBuffer.end(); ++it)
{
ReplaceMeFacet *newFacet =
addFacet((*it).getTarget(), (*it).getSource(),
index, facet->getDist2(), upper_bound2);
if (!newFacet)
{
break;
}
if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex()))
{
break;
}
if (!newFacet->link(2, lastFacet, 1))
{
break;
}
lastFacet = newFacet;
}
if (it != edgeBuffer.end())
{
break;
}
firstFacet->link(2, lastFacet, 1);
}
}
while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2);
#ifdef DEBUG
std::cout << "#facets left = " << num_facets << std::endl;
#endif
v = facet->getClosest();
pa = facet->getClosestPoint(pBuf);
pb = facet->getClosestPoint(qBuf);
return true;
}
bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
{
// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO
SimdVector3 v( 1, 0, 0 );
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
simplexSolver.reset();
int nbIterations = 0;
while ( true )
{
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" );
SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = v * m_transformB.getBasis();
SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( pInA );
SimdPoint3 qWorld = m_transformB( qInB );
SimdVector3 w = pWorld - qWorld;
delta = v.dot( w );
EPA_DEBUG_ASSERT( ( delta <= 0 ) ,"Shapes are disjoint, EPA should have never been called!" );
if ( delta > 0.f )
return false;
EPA_DEBUG_ASSERT( !simplexSolver.inSimplex( w ) ,"Shapes are disjoint, EPA should have never been called!" );
if (simplexSolver.inSimplex( w ))
return false;
// Add support point to simplex
simplexSolver.addVertex( w, pWorld, qWorld );
bool closestOk = simplexSolver.closest( v );
EPA_DEBUG_ASSERT( closestOk ,"Shapes are disjoint, EPA should have never been called!" );
if (!closestOk)
return false;
SimdScalar prevVSqrd = squaredDistance;
squaredDistance = v.length2();
// Is v converging to v(A-B) ?
EPA_DEBUG_ASSERT( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) ,
"Shapes are disjoint, EPA should have never been called!" );
if (( ( prevVSqrd - squaredDistance ) <= SIMD_EPSILON * prevVSqrd ))
return false;
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
{
break;
}
++nbIterations;
}
SimdPoint3 simplexPoints[ 5 ];
SimdPoint3 wSupportPointsOnA[ 5 ];
SimdPoint3 wSupportPointsOnB[ 5 ];
int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints );
// nbSimplexPoints can't be one because cases where the origin is on the boundary are handled
// by hybrid penetration depth
EPA_DEBUG_ASSERT( ( ( nbSimplexPoints > 1 ) ,( nbSimplexPoints <= 4 ) ) ,
"Hybrid Penetration Depth algorithm failed!" );
int nbPolyhedronPoints = nbSimplexPoints;
#ifndef EPA_POLYHEDRON_USE_PLANES
int initTetraIndices[ 4 ] = { 0, 1, 2, 3 };
#endif
// Prepare initial polyhedron to start EPA from
if ( nbSimplexPoints == 1 )
{
return false;
}
else if ( nbSimplexPoints == 2 )
{
// We have a line segment inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points
SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
d.normalize();
SimdVector3 v1;
SimdVector3 v2;
SimdVector3 v3;
SimdVector3 e1;
SimdScalar absx = abs( d.getX() );
SimdScalar absy = abs( d.getY() );
SimdScalar absz = abs( d.getZ() );
if ( absx < absy )
{
if ( absx < absz )
{
e1.setX( 1 );
}
else
{
e1.setZ( 1 );
}
}
else
{
if ( absy < absz )
{
e1.setY( 1 );
}
else
{
e1.setZ( 1 );
}
}
v1 = d.cross( e1 );
v1.normalize();
v2 = v1.rotate( d, 120 * SIMD_RADS_PER_DEG );
v3 = v2.rotate( d, 120 * SIMD_RADS_PER_DEG );
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 2 ] = pWorld;
wSupportPointsOnB[ 2 ] = qWorld;
simplexPoints[ 2 ] = wSupportPointsOnA[ 2 ] - wSupportPointsOnB[ 2 ];
seperatingAxisInA = v2 * m_transformA.getBasis();
seperatingAxisInB = -v2 * m_transformB.getBasis();
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
pWorld = m_transformA( p );
qWorld = m_transformB( q );
wSupportPointsOnA[ 3 ] = pWorld;
wSupportPointsOnB[ 3 ] = qWorld;
simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
seperatingAxisInA = v3 * m_transformA.getBasis();
seperatingAxisInB = -v3 * m_transformB.getBasis();
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
pWorld = m_transformA( p );
qWorld = m_transformB( q );
wSupportPointsOnA[ 4 ] = pWorld;
wSupportPointsOnB[ 4 ] = qWorld;
simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
#ifndef EPA_POLYHEDRON_USE_PLANES
if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
{
initTetraIndices[ 1 ] = 2;
initTetraIndices[ 2 ] = 3;
initTetraIndices[ 3 ] = 4;
}
else
{
if ( TetrahedronContainsOrigin( simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
{
initTetraIndices[ 0 ] = 1;
initTetraIndices[ 1 ] = 2;
initTetraIndices[ 2 ] = 3;
initTetraIndices[ 3 ] = 4;
}
else
{
// No tetrahedron contains the origin
assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
return false;
}
}
#endif
}
else if ( nbSimplexPoints == 3 )
{
// We have a triangle inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points
SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
SimdVector3 triangleNormal = v0.cross( v1 );
triangleNormal.normalize();
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 3 ] = pWorld;
wSupportPointsOnB[ 3 ] = qWorld;
simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
#ifndef EPA_POLYHEDRON_USE_PLANES
// We place this check here because if the tetrahedron contains the origin
// there is no need to sample another support point
if ( !TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ] ) )
{
#endif
seperatingAxisInA = -triangleNormal * m_transformA.getBasis();
seperatingAxisInB = triangleNormal * m_transformB.getBasis();
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
pWorld = m_transformA( p );
qWorld = m_transformB( q );
wSupportPointsOnA[ 4 ] = pWorld;
wSupportPointsOnB[ 4 ] = qWorld;
simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
#ifndef EPA_POLYHEDRON_USE_PLANES
if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 4 ] ) )
{
initTetraIndices[ 3 ] = 4;
}
else
{
// No tetrahedron contains the origin
assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
return false;
}
}
#endif
}
#ifdef _DEBUG
else if ( nbSimplexPoints == 4 )
{
EPA_DEBUG_ASSERT( TetrahedronContainsOrigin( simplexPoints ) ,"Initial tetrahedron does not contain the origin!" );
}
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
simplexPoints[ initTetraIndices[ 1 ] ],
simplexPoints[ initTetraIndices[ 2 ] ],
simplexPoints[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
wSupportPointsOnA[ initTetraIndices[ 1 ] ],
wSupportPointsOnA[ initTetraIndices[ 2 ] ],
wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
wSupportPointsOnB[ initTetraIndices[ 1 ] ],
wSupportPointsOnB[ initTetraIndices[ 2 ] ],
wSupportPointsOnB[ initTetraIndices[ 3 ] ] };
#endif
#ifdef EPA_POLYHEDRON_USE_PLANES
if ( !m_polyhedron.Create( simplexPoints, wSupportPointsOnA, wSupportPointsOnB, nbPolyhedronPoints ) )
#else
if ( !m_polyhedron.Create( wTetraPoints, wTetraSupportPointsOnA, wTetraSupportPointsOnB, 4 ) )
#endif
{
// Failed to create initial polyhedron
EPA_DEBUG_ASSERT( false ,"Failed to create initial polyhedron!" );
return false;
}
// Add initial faces to priority queue
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
#endif
std::list< EpaFace* >& faces = m_polyhedron.GetFaces();
std::list< EpaFace* >::iterator facesItr( faces.begin() );
while ( facesItr != faces.end() )
{
EpaFace* pFace = *facesItr;
if ( !pFace->m_deleted )
{
//#ifdef EPA_POLYHEDRON_USE_PLANES
// if ( pFace->m_planeDistance >= 0 )
// {
// m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
// assert( false && "Face's plane distance equal or greater than 0!" );
// }
//#endif
if ( pFace->IsAffinelyDependent() )
{
EPA_DEBUG_ASSERT( false ,"One initial face is affinely dependent!" );
return false;
}
if ( pFace->m_vSqrd <= 0 )
{
EPA_DEBUG_ASSERT( false ,"Face containing the origin!" );
return false;
}
if ( pFace->IsClosestPointInternal() )
{
m_faceEntries.push_back( pFace );
std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
}
}
++facesItr;
}
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
#endif
EPA_DEBUG_ASSERT( !m_faceEntries.empty() ,"No faces added to heap!" );
return true;
}
