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