public static void FetchLeafs( btDbvt pdbvt, btDbvtNode root, btList<btDbvtNode> leafs, int depth )
{
if( root.IsInternal() && depth != 0 )
{
FetchLeafs( pdbvt, root._children0, leafs, depth - 1 );
FetchLeafs( pdbvt, root._children1, leafs, depth - 1 );
DeleteNode( pdbvt, root );
}
else
{
leafs.Add( root );
}
}
//
public static void BottomUp( btDbvt pdbvt, btList<btDbvtNode> leaves )
{
while( leaves.Count > 1 )
{
double minsize = double.MaxValue;
int minidx0 = -1;
int minidx1 = -1;
for( int i = 0; i < leaves.Count; ++i )
{
for( int j = i + 1; j < leaves.Count; ++j )
{
btDbvtVolume mergeResults = btDbvtVolume.Merge( ref leaves[i].volume, ref leaves[j].volume );
double sz = Size( ref mergeResults );
if( sz < minsize )
{
minsize = sz;
minidx0 = i;
minidx1 = j;
}
}
}
btDbvtNode n0 = leaves[minidx0];
btDbvtNode n1 = leaves[minidx1];
btDbvtNode p = CreateNode( pdbvt, null, ref n0.volume, ref n1.volume, null );
p._children0 = n0;
p._children1 = n1;
n0.parent = p;
n1.parent = p;
leaves[minidx0] = p;
leaves.Swap( minidx1, leaves.Count - 1 );
leaves.Count--;// PopBack();
}
}
public static void InsertLeaf( btDbvt pdbvt, btDbvtNode root, btDbvtNode leaf )
{
if( pdbvt.Root == null )
{
pdbvt.Root = leaf;
leaf.parent = null;
}
else
{
if( !root.IsLeaf() )
{
do
{
if( btDbvtVolume.Select( ref leaf.volume,
ref root._children0.volume,
ref root._children1.volume ) == 0 )
root = root._children0;
else
root = root._children1;
} while( !root.IsLeaf() );
}
btDbvtNode prev = root.parent;
btDbvtVolume mergeResults = btDbvtVolume.Merge( ref leaf.volume, ref root.volume );
btDbvtNode node = CreateNode2( pdbvt, prev, ref mergeResults, null );
if( prev != null )
{
if( IndexOf( root ) == 0 )
prev._children0 = node;
else
prev._children1 = node;
node._children0 = root;
root.parent = node;
node._children1 = leaf;
leaf.parent = node;
do
{
if( !prev.volume.Contain( ref node.volume ) )
{
btDbvtVolume.Merge( ref prev._children0.volume, ref prev._children1.volume, out prev.volume );
}
else
{
break;
}
node = prev;
} while( null != ( prev = node.parent ) );
}
else
{
node._children0 = root;
root.parent = node;
node._children1 = leaf;
leaf.parent = node;
pdbvt.Root = node;
}
}
}
public void Process( btDbvt.btDbvtNode n, btDbvt.btDbvtNode n2 ) { }
//
public static btDbvtNode CreateNode( btDbvt pdbvt,
btDbvtNode parent,
ref btDbvtVolume volume,
Object data )
{
btDbvtNode node = CreateNode( pdbvt, parent, data );
node.volume = volume;
return ( node );
}
public static void DeleteNode( btDbvt pdbvt, btDbvtNode node )
{
//btAlignedFree(pdbvt.m_free);
//pdbvt.m_free = node;
node.Reset();
BulletGlobals.DbvtNodePool.Free( node );
}
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 );
}
}
public static btDbvtNode CreateNode( btDbvt pdbvt, btDbvtNode parent, Object data )
{
btDbvtNode node = BulletGlobals.DbvtNodePool.Get();
node.parent = parent;
node.data = data;
if( node.data is int )
{
//Debug.Assert(false);
node.dataAsInt = (int)node.data;
}
node._children0 = null;
node._children1 = null;
return ( node );
}
public override void Process( btDbvt.btDbvtNode leaf0, btDbvt.btDbvtNode leaf1 )
{
m_numOverlapPairs++;
int childIndex0 = leaf0.dataAsInt;
int childIndex1 = leaf1.dataAsInt;
Debug.Assert( childIndex0 >= 0 );
Debug.Assert( childIndex1 >= 0 );
btCompoundShape compoundShape0 = (btCompoundShape)( m_compound0ColObjWrap.getCollisionShape() );
Debug.Assert( childIndex0 < compoundShape0.getNumChildShapes() );
btCompoundShape compoundShape1 = (btCompoundShape)( m_compound1ColObjWrap.getCollisionShape() );
Debug.Assert( childIndex1 < compoundShape1.getNumChildShapes() );
btCollisionShape childShape0 = compoundShape0.getChildShape( childIndex0 );
btCollisionShape childShape1 = compoundShape1.getChildShape( childIndex1 );
//backup
btTransform orgTrans0 = m_compound0ColObjWrap.m_collisionObject.m_worldTransform;
btTransform childTrans0 = compoundShape0.getChildTransform( childIndex0 );
btTransform newChildWorldTrans0; orgTrans0.Apply( ref childTrans0, out newChildWorldTrans0 );
btTransform orgTrans1 = m_compound1ColObjWrap.m_collisionObject.m_worldTransform;
btTransform childTrans1 = compoundShape1.getChildTransform( childIndex1 );
btTransform newChildWorldTrans1; orgTrans1.Apply( ref childTrans1, out newChildWorldTrans1 );
//perform an AABB check first
btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
childShape0.getAabb( ref newChildWorldTrans0, out aabbMin0, out aabbMax0 );
childShape1.getAabb( ref newChildWorldTrans1, out aabbMin1, out aabbMax1 );
if( gCompoundCompoundChildShapePairCallback != null )
{
if( !gCompoundCompoundChildShapePairCallback( childShape0, childShape1 ) )
return;
}
if( btAabbUtil.TestAabbAgainstAabb2( ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1 ) )
{
using( btCollisionObjectWrapper compoundWrap0 = BulletGlobals.CollisionObjectWrapperPool.Get()
, compoundWrap1 = BulletGlobals.CollisionObjectWrapperPool.Get() )
{
compoundWrap0.Initialize( this.m_compound0ColObjWrap, childShape0, m_compound0ColObjWrap.m_collisionObject, -1, childIndex0 );
compoundWrap1.Initialize( this.m_compound1ColObjWrap, childShape1, m_compound1ColObjWrap.m_collisionObject, -1, childIndex1 );
btSimplePair pair = m_childCollisionAlgorithmCache.findPair( childIndex0, childIndex1 );
btCollisionAlgorithm colAlgo = null;
if( pair != null )
{
colAlgo = (btCollisionAlgorithm)pair.m_userPointer;
}
else
{
colAlgo = m_dispatcher.findAlgorithm( compoundWrap0, compoundWrap1, m_sharedManifold );
pair = m_childCollisionAlgorithmCache.addOverlappingPair( childIndex0, childIndex1 );
Debug.Assert( pair != null );
pair.m_userPointer = colAlgo;
}
Debug.Assert( colAlgo != null );
btCollisionObjectWrapper tmpWrap0;
btCollisionObjectWrapper tmpWrap1;
tmpWrap0 = m_resultOut.m_body0Wrap;
tmpWrap1 = m_resultOut.m_body1Wrap;
m_resultOut.m_body0Wrap=( compoundWrap0 );
m_resultOut.m_body1Wrap=( compoundWrap1 );
m_resultOut.setShapeIdentifiersA( -1, childIndex0 );
m_resultOut.setShapeIdentifiersB( -1, childIndex1 );
colAlgo.processCollision( compoundWrap0, ref newChildWorldTrans0, compoundWrap1, ref newChildWorldTrans1, m_dispatchInfo, m_resultOut );
m_resultOut.m_body0Wrap=( tmpWrap0 );
m_resultOut.m_body1Wrap=( tmpWrap1 );
}
}
}
static bool MyIntersect( btDbvt.btDbvtVolume a,
btDbvt.btDbvtVolume b, ref btTransform xform )
{
btVector3 newmin, newmax;
btAabbUtil.btTransformAabb( ref b._min, ref b._max, 0, ref xform, out newmin, out newmax );
btDbvt.btDbvtVolume newb = btDbvt.btDbvtVolume.FromMM( ref newmin, ref newmax );
return btDbvt.btDbvtVolume.Intersect( ref a, ref newb );
}
public bool AllLeaves( btDbvt.btDbvtNode n )
{
return true;
}
public bool Descent( btDbvt.btDbvtNode n )
{
return true;
}
public void Process( btDbvt.btDbvtNode n, double f )
{
Process( n );
}
public static btDbvtNode TopDown( btDbvt pdbvt, btList<btDbvtNode> leaves, int bu_treshold )
{
if( leaves.Count > 1 )
{
if( leaves.Count > bu_treshold )
{
btDbvtVolume vol; Bounds( leaves, out vol );
btVector3 org = vol.Center();
btList<btDbvtNode> sets0 = new btList<btDbvtNode>();
btList<btDbvtNode> sets1 = new btList<btDbvtNode>();
int bestaxis = -1;
int bestmidp = leaves.Count;
int[] a1 = new int[] { 0, 0 };
int[] a2 = new int[] { 0, 0 };
int[] a3 = new int[] { 0, 0 };
int[][] splitcount = new int[][] { a1, a2, a3 };
int i;
for( i = 0; i < leaves.Count; ++i )
{
btVector3 x = leaves[i].volume.Center() - org;
for( int j = 0; j < 3; ++j )
{
++splitcount[j][btVector3.dot( ref x, ref axis[j] ) > 0 ? 1 : 0];
}
}
for( i = 0; i < 3; ++i )
{
if( ( splitcount[i][0] > 0 ) && ( splitcount[i][1] > 0 ) )
{
int midp = (int)Math.Abs( ( splitcount[i][0] - splitcount[i][1] ) );
if( midp < bestmidp )
{
bestaxis = i;
bestmidp = midp;
}
}
}
if( bestaxis >= 0 )
{
sets0.Capacity = ( splitcount[bestaxis][0] );
sets1.Capacity = ( splitcount[bestaxis][1] );
Split( leaves, sets0, sets1, ref org, ref axis[bestaxis] );
}
else
{
sets0.Capacity = ( leaves.Count / 2 + 1 );
sets1.Capacity = ( leaves.Count / 2 );
for( int i2 = 0, ni = leaves.Count; i2 < ni; ++i2 )
{
if( (i2 & 1)== 0 )
sets0.Add( leaves[i2] );
else
sets1.Add( leaves[i2] );
}
}
btDbvtNode node = CreateNode( pdbvt, null, ref vol, null );
node._children0 = TopDown( pdbvt, sets0, bu_treshold );
node._children1 = TopDown( pdbvt, sets1, bu_treshold );
node._children0.parent = node;
node._children1.parent = node;
return ( node );
}
else
{
BottomUp( pdbvt, leaves );
return ( leaves[0] );
}
}
return ( leaves[0] );
}
public btCompoundShape( bool enableDynamicAabbTree = true, int initialChildCapacity = 0 )
{
m_localAabbMin = btVector3.Max;
m_localAabbMax = btVector3.Min;
m_dynamicAabbTree = ( null );
m_updateRevision = ( 1 );
m_collisionMargin = ( btScalar.BT_ZERO );
m_localScaling = btVector3.One;
m_shapeType = BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE;
if( enableDynamicAabbTree )
{
m_dynamicAabbTree = new btDbvt();
}
m_children.Capacity = ( initialChildCapacity );
}
public static btDbvtNode CreateNode( btDbvt pdbvt, btDbvtNode parent, int data )
{
btDbvtNode node = BulletGlobals.DbvtNodePool.Get();
node.parent = parent;
node.data = null;
node.dataAsInt = data;
node._children0 = null;
node._children1 = null;
return ( node );
}
void createAabbTreeFromChildren()
{
if( m_dynamicAabbTree == null )
{
m_dynamicAabbTree = new btDbvt();
for( int index = 0; index < m_children.Count; index++ )
{
btCompoundShapeChild child = m_children[index];
//extend the local aabbMin/aabbMax
btVector3 localAabbMin, localAabbMax;
child.m_childShape.getAabb( ref child.m_transform, out localAabbMin, out localAabbMax );
btDbvt.btDbvtVolume bounds = btDbvt.btDbvtVolume.FromMM( ref localAabbMin, ref localAabbMax );
int index2 = index;
child.m_node = m_dynamicAabbTree.insert( ref bounds, index2 );
}
}
}
public static btDbvtNode CreateNode2( btDbvt tree, btDbvtNode aparent, ref btDbvtVolume avolume, Object adata )
{
btDbvtNode node = BulletGlobals.DbvtNodePool.Get();
node.volume = avolume;
node.parent = aparent;
node.data = adata;
node._children0 = null;
node._children1 = null;
if( node.data is int )
{
Debug.Assert( false );
node.dataAsInt = (int)node.data;
}
return node;
}
public static btDbvtNode RemoveLeaf( btDbvt pdbvt, btDbvtNode leaf )
{
if( leaf == pdbvt.Root )
{
pdbvt.Root = null;
return null;
}
else
{
btDbvtNode parent = leaf.parent;
btDbvtNode prev = parent.parent;
btDbvtNode sibling = ((1-IndexOf(leaf))==0?parent._children0:parent._children1);
if( prev != null )
{
if( IndexOf( parent ) == 0 )
prev._children0 = sibling;
else
prev._children1 = sibling;
sibling.parent = prev;
DeleteNode( pdbvt, parent );
while( prev != null )
{
btDbvtVolume pb = prev.volume;
btDbvtVolume.Merge( ref prev._children0.volume, ref prev._children1.volume, out prev.volume );
if( btDbvtVolume.NotEqual( ref pb, ref prev.volume ) )
{
sibling = prev;
prev = prev.parent;
}
else
{
break;
}
}
return ( prev != null ? prev : pdbvt.Root );
}
else
{
pdbvt.Root = sibling;
sibling.parent = null;
DeleteNode( pdbvt, parent );
return ( pdbvt.Root );
}
}
}
//
public static btDbvtNode CreateNode( btDbvt pdbvt,
btDbvtNode parent,
ref btDbvtVolume volume0,
ref btDbvtVolume volume1,
Object data )
{
btDbvtNode node = CreateNode( pdbvt, parent, data );
btDbvtVolume.Merge( ref volume0, ref volume1, out node.volume );
return ( node );
}
public btDbvtNode( btDbvt tree, btDbvtNode aparent, ref btDbvtVolume avolume, Object adata )
: this()
{
volume = avolume;
parent = aparent;
data = adata;
if( data is int )
{
dataAsInt = (int)data;
}
}
public static void RecurseDeleteNode( btDbvt pdbvt, btDbvtNode node )
{
if( !node.IsLeaf() )
{
RecurseDeleteNode( pdbvt, node._children0 );
RecurseDeleteNode( pdbvt, node._children1 );
}
if( node == pdbvt.m_root )
{
pdbvt.m_root = null;
}
DeleteNode( pdbvt, node );
}
//
// depth is defaulted to -1
public static void FetchLeafs( btDbvt pdbvt, btDbvtNode root, btList<btDbvtNode> leafs )
{
FetchLeafs( pdbvt, root, leafs, -1 );
}
public override void Process( btDbvt.btDbvtNode leaf )
{
int index = leaf.dataAsInt;
btCompoundShape compoundShape = (btCompoundShape)( m_compoundColObjWrap.getCollisionShape() );
btCollisionShape childShape = compoundShape.getChildShape( index );
#if false
if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw.getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btVector3 worldAabbMin,worldAabbMax;
btTransform orgTrans = m_compoundColObjWrap.getWorldTransform();
btTransformAabb(leaf.volume.Mins(),leaf.volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
m_dispatchInfo.m_debugDraw.drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
}
#endif
ProcessChildShape( childShape, index );
}
public void Process( btDbvt.btDbvtNode leaf )
{
Process( leaf.dataAsInt );
}
