/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
/// You don't want your game ever to lock-up.
void computeClosestPoints( ref btTransform transA, ref btTransform transB, btPointCollector pointCollector)
{
if( m_convexB1 != null)
{
m_simplexSolver.reset();
btGjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get();
gjk.Initialize( m_convexA, m_convexB1, m_convexA.getShapeType(), m_convexB1.getShapeType(), m_convexA.getMargin(), m_convexB1.getMargin(), m_simplexSolver, m_penetrationDepthSolver);
btGjkPairDetector.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput();
input.m_transformA = transA;
input.m_transformB = transB;
gjk.getClosestPoints( input, pointCollector, null );
BulletGlobals.GjkPairDetectorPool.Free( gjk );
}
else
{
//convex versus plane
btConvexShape convexShape = m_convexA;
btStaticPlaneShape planeShape = m_planeShape;
btVector3 planeNormal; planeShape.getPlaneNormal( out planeNormal );
double planeConstant = planeShape.getPlaneConstant();
//btTransform convexWorldTransform = transA;
btTransform convexInPlaneTrans;
btTransform tmpInv;
transB.inverse( out tmpInv );
tmpInv.Apply( ref transA, out convexInPlaneTrans );
btTransform planeInConvex;
convexInPlaneTrans.inverse( out tmpInv );
tmpInv.Apply( ref transB, out planeInConvex );
//planeInConvex = convexWorldTransform.inverse() * transB;
btVector3 tmp;
planeInConvex.m_basis.Mult( ref planeNormal, out tmp );
tmp.Invert( out tmp );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected;// = vtxInPlane - distance * planeNormal;
vtxInPlane.SubScale( ref planeNormal, distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; transB.Apply(ref vtxInPlaneProjected, out vtxInPlaneWorld );
btVector3 normalOnSurfaceB; transB.m_basis.Apply( ref planeNormal, out normalOnSurfaceB );
pointCollector.addContactPoint(
ref normalOnSurfaceB,
ref vtxInPlaneWorld,
distance );
}
}
/// cast a convex against another convex object
internal override bool calcTimeOfImpact(
ref btTransform fromA,
ref btTransform toA,
ref btTransform fromB,
ref btTransform toB,
CastResult result)
{
m_simplexSolver.reset();
/// compute linear velocity for this interval, to interpolate
//assume no rotation/angular velocity, assert here?
btVector3 linVelA, linVelB;
toA.m_origin.Sub( ref fromA.m_origin, out linVelA );
toB.m_origin.Sub( ref fromB.m_origin, out linVelB );
double radius = (double)( 0.001 );
double lambda = btScalar.BT_ZERO;
btVector3 v = btVector3.xAxis;
int maxIter = MAX_ITERATIONS;
btVector3 n = btVector3.Zero;
bool hasResult = false;
btVector3 c;
btVector3 r; linVelA.Sub( ref linVelB, out r );
double lastLambda = lambda;
//double epsilon = (double)(0.001);
int numIter = 0;
//first solution, using GJK
// result.drawCoordSystem(sphereTr);
btPointCollector pointCollector = new btPointCollector();
btGjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get();
gjk.Initialize( m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver);
btGjkPairDetector.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA.T;
input.m_transformB = fromB.T;
gjk.getClosestPoints( input, pointCollector, null );
hasResult = pointCollector.m_hasResult;
c = pointCollector.m_pointInWorld;
if( hasResult )
{
double dist;
dist = pointCollector.m_distance;
n = pointCollector.m_normalOnBInWorld;
//not close enough
while( dist > radius )
{
numIter++;
if( numIter > maxIter )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false; //todo: report a failure
}
double dLambda = btScalar.BT_ZERO;
double projectedLinearVelocity = r.dot( ref n );
dLambda = dist / ( projectedLinearVelocity );
lambda = lambda - dLambda;
if( lambda > btScalar.BT_ONE )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
if( lambda < btScalar.BT_ZERO )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
//todo: next check with relative epsilon
if( lambda <= lastLambda )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
//n.setValue(0,0,0);
//break;
}
lastLambda = lambda;
//interpolate to next lambda
result.DebugDraw( lambda );
btVector3 tmp;
btVector3.setInterpolate3( out tmp, ref fromA.m_origin, ref toA.m_origin, lambda );
input.m_transformA.setOrigin( ref tmp );
btVector3.setInterpolate3( out tmp, ref fromB.m_origin, ref toB.m_origin, lambda );
input.m_transformB.setOrigin( ref tmp );
gjk.getClosestPoints( input, pointCollector, null );
if( pointCollector.m_hasResult )
{
if( pointCollector.m_distance < btScalar.BT_ZERO )
{
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
}
else
{
//??
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
}
//is n normalized?
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if( n.dot( ref r ) >= -result.m_allowedPenetration )
{
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return true;
}
BulletGlobals.GjkPairDetectorPool.Free( gjk );
return false;
}
internal override bool calcTimeOfImpact(
ref btTransform fromA,
ref btTransform toA,
ref btTransform fromB,
ref btTransform toB,
CastResult result)
{
/// compute linear and angular velocity for this interval, to interpolate
btVector3 linVelA, angVelA, linVelB, angVelB;
btTransformUtil.calculateVelocity( ref fromA,ref toA, btScalar.BT_ONE, out linVelA, out angVelA );
btTransformUtil.calculateVelocity( ref fromB, ref toB, btScalar.BT_ONE, out linVelB, out angVelB );
double boundingRadiusA = m_convexA.getAngularMotionDisc();
double boundingRadiusB = m_convexB1 != null ? m_convexB1.getAngularMotionDisc() : 0;
double maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
btVector3 relLinVel = ( linVelB - linVelA );
double relLinVelocLength = btVector3.btDelLength( ref linVelB,ref linVelA );
if( ( relLinVelocLength + maxAngularProjectedVelocity ) == 0 )
return false;
double lambda = btScalar.BT_ZERO;
btVector3 v = btVector3.xAxis;
int maxIter = MAX_ITERATIONS;
btVector3 n = btVector3.Zero;
bool hasResult = false;
btVector3 c;
double lastLambda = lambda;
//double epsilon = (double)(0.001);
int numIter = 0;
//first solution, using GJK
double radius = 0.001f;
// result.drawCoordSystem(sphereTr);
btPointCollector pointCollector1 = new btPointCollector();
{
computeClosestPoints( ref fromA, ref fromB, pointCollector1 );
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
}
if( hasResult )
{
double dist;
dist = pointCollector1.m_distance + result.m_allowedPenetration;
n = pointCollector1.m_normalOnBInWorld;
double projectedLinearVelocity = relLinVel.dot( n );
if( ( projectedLinearVelocity + maxAngularProjectedVelocity ) <= btScalar.SIMD_EPSILON )
return false;
//not close enough
while( dist > radius )
{
if( result.m_debugDrawer != null )
{
result.m_debugDrawer.drawSphere( ref c, 0.2f, ref btVector3.One );
}
double dLambda = btScalar.BT_ZERO;
projectedLinearVelocity = relLinVel.dot( n );
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if( ( projectedLinearVelocity + maxAngularProjectedVelocity ) <= btScalar.SIMD_EPSILON )
return false;
dLambda = dist / ( projectedLinearVelocity + maxAngularProjectedVelocity );
lambda = lambda + dLambda;
if( lambda > btScalar.BT_ONE )
return false;
if( lambda < btScalar.BT_ZERO )
return false;
//todo: next check with relative epsilon
if( lambda <= lastLambda )
{
return false;
}
lastLambda = lambda;
//interpolate to next lambda
btTransform interpolatedTransA, interpolatedTransB, relativeTrans;
btTransformUtil.integrateTransform( ref fromA, ref linVelA, ref angVelA, lambda, out interpolatedTransA );
btTransformUtil.integrateTransform( ref fromB, ref linVelB, ref angVelB, lambda, out interpolatedTransB );
interpolatedTransB.inverseTimes( ref interpolatedTransA, out relativeTrans );
if( result.m_debugDrawer != null )
{
result.m_debugDrawer.drawSphere( ref interpolatedTransA.m_origin, 0.2f, ref btVector3.xAxis );
}
result.DebugDraw( lambda );
btPointCollector pointCollector = new btPointCollector();
computeClosestPoints( ref interpolatedTransA, ref interpolatedTransB, pointCollector );
if( pointCollector.m_hasResult )
{
dist = pointCollector.m_distance + result.m_allowedPenetration;
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
}
else
{
result.reportFailure( -1, numIter );
return false;
}
numIter++;
if( numIter > maxIter )
{
result.reportFailure( -2, numIter );
return false;
}
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
return false;
}
