internal override double calculateTimeOfImpact( btCollisionObject col0, btCollisionObject col1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
//(void)resultOut;
//(void)dispatchInfo;
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///col0.m_worldTransform,
double resultFraction = btScalar.BT_ONE;
btVector3 tmp;
col0.m_interpolationWorldTransform.m_origin.Sub( ref col0.m_worldTransform.m_origin, out tmp );
double squareMot0 = ( tmp ).length2();
col1.m_interpolationWorldTransform.m_origin.Sub( ref col1.m_worldTransform.m_origin, out tmp );
double squareMot1 = ( tmp ).length2();
if( squareMot0 < col0.getCcdSquareMotionThreshold() &&
squareMot1 < col1.getCcdSquareMotionThreshold() )
return resultFraction;
if( disableCcd )
return btScalar.BT_ONE;
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
/// Convex0 against sphere for Convex1
{
btConvexShape convex0 = (btConvexShape)col0.getCollisionShape();
using( btSphereShape sphere1 = BulletGlobals.SphereShapePool.Get() )
{
sphere1.Initialize( col1.getCcdSweptSphereRadius() ); //todo: allow non-zero sphere sizes, for better approximation
btConvexCast.CastResult result = BulletGlobals.CastResultPool.Get();
btVoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
btGjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get();
ccd1.Initialize( convex0, sphere1, voronoiSimplex );
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if( ccd1.calcTimeOfImpact( ref col0.m_worldTransform, ref col0.m_interpolationWorldTransform,
ref col1.m_worldTransform, ref col1.m_interpolationWorldTransform, result ) )
{
//store result.m_fraction in both bodies
if( col0.getHitFraction() > result.m_fraction )
col0.setHitFraction( result.m_fraction );
if( col1.getHitFraction() > result.m_fraction )
col1.setHitFraction( result.m_fraction );
if( resultFraction > result.m_fraction )
resultFraction = result.m_fraction;
}
BulletGlobals.GjkConvexCastPool.Free( ccd1 );
}
}
/// Sphere (for convex0) against Convex1
{
btConvexShape convex1 = (btConvexShape)( col1.getCollisionShape() );
using( btSphereShape sphere0 = BulletGlobals.SphereShapePool.Get() )
{
sphere0.Initialize( col0.getCcdSweptSphereRadius() ); //todo: allow non-zero sphere sizes, for better approximation
btConvexCast.CastResult result = BulletGlobals.CastResultPool.Get();
btVoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
btGjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get();
ccd1.Initialize( sphere0, convex1, voronoiSimplex );
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if( ccd1.calcTimeOfImpact( ref col0.m_worldTransform, ref col0.m_interpolationWorldTransform,
ref col1.m_worldTransform, ref col1.m_interpolationWorldTransform, result ) )
{
//store result.m_fraction in both bodies
if( col0.getHitFraction() > result.m_fraction )
col0.setHitFraction( result.m_fraction );
if( col1.getHitFraction() > result.m_fraction )
col1.setHitFraction( result.m_fraction );
if( resultFraction > result.m_fraction )
resultFraction = result.m_fraction;
}
BulletGlobals.GjkConvexCastPool.Free( ccd1 );
}
}
return resultFraction;
}
void updateSingleAabb( btCollisionObject colObj )
{
btVector3 minAabb, maxAabb;
colObj.getCollisionShape().getAabb( ref colObj.m_worldTransform, out minAabb, out maxAabb );
//need to increase the aabb for contact thresholds
btVector3 contactThreshold = new btVector3( btPersistentManifold.gContactBreakingThreshold, btPersistentManifold.gContactBreakingThreshold, btPersistentManifold.gContactBreakingThreshold );
minAabb.Sub( ref contactThreshold, out minAabb );
maxAabb.Add( ref contactThreshold, out maxAabb );
//minAabb -= contactThreshold;
//maxAabb += contactThreshold;
if( getDispatchInfo().m_useContinuous
&& colObj.getInternalType() == btCollisionObject.CollisionObjectTypes.CO_RIGID_BODY
&& !colObj.isStaticOrKinematicObject() )
{
btVector3 minAabb2, maxAabb2;
colObj.getCollisionShape().getAabb( ref colObj.m_interpolationWorldTransform, out minAabb2, out maxAabb2 );
minAabb2.Sub( ref contactThreshold, out minAabb2 );
maxAabb2.Add( ref contactThreshold, out maxAabb2 );
minAabb.setMin( ref minAabb2 );
maxAabb.setMax( ref maxAabb2 );
}
btBroadphaseInterface bp = (btBroadphaseInterface)m_broadphasePairCache;
//moving objects should be moderately sized, probably something wrong if not
if( colObj.isStaticObject()
|| ( btVector3.BetweenLength2( ref minAabb, ref maxAabb ) < (double)( 1e12 ) ) )
{
bp.setAabb( colObj.getBroadphaseHandle(), ref minAabb, ref maxAabb, m_dispatcher1 );
}
else
{
//something went wrong, investigate
//this assert is unwanted in 3D modelers (danger of loosing work)
colObj.setActivationState( ActivationState.DISABLE_SIMULATION );
if( reportMe && m_debugDrawer != null )
{
reportMe = false;
m_debugDrawer.reportErrorWarning( "Overflow in AABB, object removed from simulation" );
m_debugDrawer.reportErrorWarning( "If you can reproduce this, please email [email protected]\n" );
m_debugDrawer.reportErrorWarning( "Please include above information, your Platform, version of OS.\n" );
m_debugDrawer.reportErrorWarning( "Thanks.\n" );
}
}
}
public virtual void addCollisionObject( btCollisionObject collisionObject
, btBroadphaseProxy.CollisionFilterGroups collisionFilterGroup = btBroadphaseProxy.CollisionFilterGroups.DefaultFilter
, btBroadphaseProxy.CollisionFilterGroups collisionFilterMask = btBroadphaseProxy.CollisionFilterGroups.AllFilter )
{
Debug.Assert( collisionObject != null );
//check that the object isn't already added
Debug.Assert( !m_collisionObjects.Contains( collisionObject ) );
m_collisionObjects.Add( collisionObject );
//calculate new AABB
btVector3 minAabb;
btVector3 maxAabb;
collisionObject.getCollisionShape().getAabb( ref collisionObject.m_worldTransform, out minAabb, out maxAabb );
BroadphaseNativeTypes type = collisionObject.getCollisionShape().getShapeType();
collisionObject.setBroadphaseHandle( getBroadphase().createProxy(
ref minAabb,
ref maxAabb,
type,
collisionObject,
collisionFilterGroup,
collisionFilterMask,
m_dispatcher1, null
) );
}
///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
///it reports one or more contact points (including the one with deepest penetration)
void contactPairTest( btCollisionObject colObjA, btCollisionObject colObjB, ContactResultCallback resultCallback )
{
btCollisionObjectWrapper obA = BulletGlobals.CollisionObjectWrapperPool.Get();
obA.Initialize( null, colObjA.getCollisionShape(), colObjA, -1, -1 );
btCollisionObjectWrapper obB = BulletGlobals.CollisionObjectWrapperPool.Get();
obB.Initialize( null, colObjB.getCollisionShape(), colObjB, -1, -1 );
btCollisionAlgorithm algorithm = m_dispatcher1.findAlgorithm( obA, obB, null );
if( algorithm != null )
{
btBridgedManifoldResult contactPointResult = BulletGlobals.BridgedManifoldResultPool.Get();
contactPointResult.Initialize( obA, obB, resultCallback );
//discrete collision detection query
algorithm.processCollision( obA, ref obA.m_collisionObject.m_worldTransform
, obB, ref obB.m_collisionObject.m_worldTransform
, getDispatchInfo(), contactPointResult );
//algorithm.~btCollisionAlgorithm();
m_dispatcher1.freeCollisionAlgorithm( algorithm );
BulletGlobals.BridgedManifoldResultPool.Free( contactPointResult );
}
BulletGlobals.CollisionObjectWrapperPool.Free( obA );
BulletGlobals.CollisionObjectWrapperPool.Free( obB );
}
///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
void contactTest( btCollisionObject colObj, ContactResultCallback resultCallback )
{
btVector3 aabbMin, aabbMax;
colObj.getCollisionShape().getAabb( ref colObj.m_worldTransform, out aabbMin, out aabbMax );
btSingleContactCallback contactCB = new btSingleContactCallback( colObj, this, resultCallback );
m_broadphasePairCache.aabbTest( ref aabbMin, ref aabbMax, contactCB );
}
internal override btPersistentManifold getNewManifold( btCollisionObject body0, btCollisionObject body1 )
{
gNumManifold++;
//Debug.Assert(gNumManifold < 65535);
//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
double contactBreakingThreshold = ( m_dispatcherFlags & DispatcherFlags.CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD ) != 0 ?
btScalar.btMin( body0.getCollisionShape().getContactBreakingThreshold( btPersistentManifold.gContactBreakingThreshold ), body1.getCollisionShape().getContactBreakingThreshold( btPersistentManifold.gContactBreakingThreshold ) )
: btPersistentManifold.gContactBreakingThreshold;
double contactProcessingThreshold = btScalar.btMin( body0.getContactProcessingThreshold(), body1.getContactProcessingThreshold() );
btPersistentManifold manifold = BulletGlobals.PersistentManifoldPool.Get();
manifold.Initialize( body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold );
manifold.m_index1a = m_manifoldsPtr.Count;
btScalar.Dbg( DbgFlag.Manifolds, "add a manifold (getNewManifold)" );
m_manifoldsPtr.Add( manifold );
return manifold;
}