/// 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 );
}
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap, ref btTransform body1Transform, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
if( m_manifoldPtr == null )
return;
btBoxShape box0 = (btBoxShape)body0Wrap.m_shape;
btBoxShape box1 = (btBoxShape)body1Wrap.m_shape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.setPersistentManifold( m_manifoldPtr );
#if !USE_PERSISTENT_CONTACTS
m_manifoldPtr.clearManifold();
#endif //USE_PERSISTENT_CONTACTS
btDiscreteCollisionDetectorInterface.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput();
input.m_maximumDistanceSquared = btScalar.BT_LARGE_FLOAT;
input.m_transformA = body0Transform;
input.m_transformB = body1Transform;
//btBoxBoxDetector detector = BulletGlobals.
// new btBoxBoxDetectors( box0, box1 );
btBoxBoxDetector.getClosestPoints( box0, box1, input, resultOut, dispatchInfo.m_debugDraw );
#if USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
}
/// 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;
}
