internal override void processCollision( btCollisionObjectWrapper body0Wrap , ref btTransform body0Transform , btCollisionObjectWrapper body1Wrap , ref btTransform body1Transform , btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { btCollisionObjectWrapper col0ObjWrap = body0Wrap; btCollisionObjectWrapper col1ObjWrap = body1Wrap; Debug.Assert( col0ObjWrap.getCollisionShape().isCompound() ); Debug.Assert( col1ObjWrap.getCollisionShape().isCompound() ); btCompoundShape compoundShape0 = (btCompoundShape)( col0ObjWrap.getCollisionShape() ); btCompoundShape compoundShape1 = (btCompoundShape)( col1ObjWrap.getCollisionShape() ); btDbvt tree0 = compoundShape0.getDynamicAabbTree(); btDbvt tree1 = compoundShape1.getDynamicAabbTree(); if( tree0 == null || tree1 == null ) { base.processCollision( body0Wrap, ref body0Transform, body1Wrap, ref body1Transform, dispatchInfo, resultOut ); return; } ///btCompoundShape might have changed: ////make sure the internal child collision algorithm caches are still valid if( ( compoundShape0.getUpdateRevision() != m_compoundShapeRevision0 ) || ( compoundShape1.getUpdateRevision() != m_compoundShapeRevision1 ) ) { ///clear all removeChildAlgorithms(); m_compoundShapeRevision0 = compoundShape0.getUpdateRevision(); m_compoundShapeRevision1 = compoundShape1.getUpdateRevision(); } ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; btManifoldArray manifoldArray = new btManifoldArray(); btSimplePairArray pairs = m_childCollisionAlgorithmCache.getOverlappingPairArray(); for( i = 0; i < pairs.Count; i++ ) { if( pairs[i].m_userPointer != null ) { btCollisionAlgorithm algo = (btCollisionAlgorithm)pairs[i].m_userPointer; algo.getAllContactManifolds( manifoldArray ); for( int m = 0; m < manifoldArray.Count; m++ ) { if( manifoldArray[m].m_cachedPoints != 0 ) { resultOut.setPersistentManifold( manifoldArray[m] ); resultOut.refreshContactPoints(); resultOut.setPersistentManifold( null ); } } manifoldArray.Count =( 0 ); } } } btCompoundCompoundLeafCallback callback = new btCompoundCompoundLeafCallback ( col0ObjWrap, col1ObjWrap,this.m_dispatcher, dispatchInfo, resultOut, this.m_childCollisionAlgorithmCache, m_sharedManifold); btTransform xform; body0Transform.inverseTimes( ref body1Transform, out xform ); MycollideTT( tree0.m_root, tree1.m_root, ref xform, callback ); //Console.WriteLine("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); //remove non-overlapping child pairs { Debug.Assert( m_removePairs.Count == 0 ); //iterate over all children, perform an AABB check inside ProcessChildShape btSimplePairArray pairs = m_childCollisionAlgorithmCache.getOverlappingPairArray(); int i; //btManifoldArray manifoldArray; btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; for( i = 0; i < pairs.Count; i++ ) { if( pairs[i].m_userPointer != null ) { btCollisionAlgorithm algo = (btCollisionAlgorithm)pairs[i].m_userPointer; { btCollisionShape childShape0 = null; btTransform newChildWorldTrans0; //btTransform orgInterpolationTrans0; childShape0 = compoundShape0.getChildShape( pairs[i].m_indexA ); //orgInterpolationTrans0 = col0ObjWrap.m_worldTransform; btTransform childTrans0 = compoundShape0.getChildTransform( pairs[i].m_indexA ); body0Transform.Apply( ref childTrans0, out newChildWorldTrans0 ); childShape0.getAabb( ref newChildWorldTrans0, out aabbMin0, out aabbMax0 ); } { btCollisionShape childShape1 = null; btTransform newChildWorldTrans1; childShape1 = compoundShape1.getChildShape( pairs[i].m_indexB ); btTransform childTrans1 = compoundShape1.getChildTransform( pairs[i].m_indexB ); body1Transform.Apply( ref childTrans1, out newChildWorldTrans1 ); childShape1.getAabb( ref newChildWorldTrans1, out aabbMin1, out aabbMax1 ); } if( !btAabbUtil.TestAabbAgainstAabb2( ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1 ) ) { //algo.~btCollisionAlgorithm(); m_dispatcher.freeCollisionAlgorithm( algo ); m_removePairs.Add( new btSimplePair( pairs[i].m_indexA, pairs[i].m_indexB ) ); } } } for( i = 0; i < m_removePairs.Count; i++ ) { m_childCollisionAlgorithmCache.removeOverlappingPair( m_removePairs[i].m_indexA, m_removePairs[i].m_indexB ); } m_removePairs.Clear(); } }
static void MycollideTT( btDbvt.btDbvtNode root0, btDbvt.btDbvtNode root1, ref btTransform xform, btCompoundCompoundLeafCallback callback ) { if( root0 != null && root1 != null ) { int depth = 1; int treshold = btDbvt.DOUBLE_STACKSIZE - 4; btList<btDbvt.sStkNN> stkStack = new btList<btDbvt.sStkNN>( btDbvt.DOUBLE_STACKSIZE ); stkStack[0].Initialize( root0, root1 ); do { btDbvt.sStkNN p = stkStack[--depth]; if( MyIntersect( p.a.volume, p.b.volume, ref xform ) ) { if( depth > treshold ) { stkStack.Capacity = ( stkStack.Count * 2 ); treshold = stkStack.Count - 4; } if( p.a.IsInternal() ) { if( p.b.IsInternal() ) { stkStack[depth++].Initialize( p.a._children0, p.b._children0 ); stkStack[depth++].Initialize( p.a._children1, p.b._children0 ); stkStack[depth++].Initialize( p.a._children0, p.b._children1 ); stkStack[depth++].Initialize( p.a._children1, p.b._children1 ); } else { stkStack[depth++].Initialize( p.a._children0, p.b ); stkStack[depth++].Initialize( p.a._children1, p.b ); } } else { if( p.b.IsInternal() ) { stkStack[depth++].Initialize( p.a, p.b._children0 ); stkStack[depth++].Initialize( p.a, p.b._children1 ); } else { callback.Process( p.a, p.b ); } } } } while( depth != 0 ); } }