public static void Run()
{
{
btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 3, 4, 5 ) );
btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) );
btTransform convexInPlaneTrans;
btTransform tmp;
planeObjWorld.inverse( out tmp );
tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
//convexInPlaneTrans = planeObjWorld( convexWorldTransform;
//PrintTransform( &convexInPlaneTrans );
}
{
btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 5, 2, 1 ) );
btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) );
btTransform convexInPlaneTrans;
btTransform tmp;
planeObjWorld.inverse( out tmp );
tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
//convexInPlaneTrans = planeObjWorld( convexWorldTransform;
//PrintTransform( &convexInPlaneTrans );
btQuaternion perturbeRot = new btQuaternion( 0.1, 0.5, 0.25, 0.8 );
perturbeRot.normalize();
btTransform planeObjWrapTrans = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 4, 7, 2 ) );
planeObjWrapTrans.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
//now perturbe the convex-world transform
btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot );
btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe );
convexWorldTransform.m_basis = tmpPerturbe;
btTransform planeInConvex;
convexWorldTransform.inverseTimes( ref planeObjWrapTrans, out planeInConvex );
PrintTransform( ref planeInConvex );
}
Console.Read();
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
btCollisionObjectWrapper colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
btCollisionObjectWrapper otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
Debug.Assert( colObjWrap.getCollisionShape().isCompound() );
btCompoundShape compoundShape = (btCompoundShape)( colObjWrap.getCollisionShape() );
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if( compoundShape.getUpdateRevision() != m_compoundShapeRevision )
{
///clear and update all
removeChildAlgorithms();
preallocateChildAlgorithms( body0Wrap, body1Wrap );
m_compoundShapeRevision = compoundShape.getUpdateRevision();
}
if( m_childCollisionAlgorithms.Count == 0 )
return;
btDbvt tree = compoundShape.getDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
btCompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get();
callback.Initialize( colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, m_childCollisionAlgorithms.InternalArray, m_sharedManifold);
///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();
for( i = 0; i < m_childCollisionAlgorithms.Count; i++ )
{
if( m_childCollisionAlgorithms[i] != null )
{
m_childCollisionAlgorithms[i].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 );//??necessary?
}
}
manifoldArray.Count = ( 0 );
}
}
}
if( tree != null )
{
btVector3 localAabbMin, localAabbMax;
btTransform otherInCompoundSpace;
if( m_isSwapped )
body1Transform.inverseTimes(ref body0Transform, out otherInCompoundSpace);
else
body0Transform.inverseTimes( ref body1Transform, out otherInCompoundSpace );
otherObjWrap.getCollisionShape().getAabb( ref otherInCompoundSpace, out localAabbMin, out localAabbMax );
btDbvt.btDbvtVolume bounds = btDbvt.btDbvtVolume.FromMM( ref localAabbMin, ref localAabbMax );
//process all children, that overlap with the given AABB bounds
btDbvt.CollideTV( tree.m_root, ref bounds, callback );
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
int i;
for( i = 0; i < numChildren; i++ )
{
callback.ProcessChildShape( compoundShape.getChildShape( i ), i );
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
int i;
//btManifoldArray manifoldArray;
btCollisionShape childShape = null;
//btITransform orgTrans;
btTransform newChildWorldTrans;
btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
for( i = 0; i < numChildren; i++ )
{
if( m_childCollisionAlgorithms[i] != null )
{
childShape = compoundShape.getChildShape( i );
//if not longer overlapping, remove the algorithm
//orgTrans = colObjWrap.m_worldTransform;
//btTransform childTrans = compoundShape.getChildTransform( i );
( ( m_isSwapped ) ? body1Transform : body0Transform ).Apply( ref compoundShape.m_children.InternalArray[i].m_transform, out newChildWorldTrans );
//perform an AABB check first
childShape.getAabb( ref newChildWorldTrans, out aabbMin0, out aabbMax0 );
if( m_isSwapped )
otherObjWrap.m_shape.getAabb( ref body0Transform, out aabbMin1, out aabbMax1 );
else
otherObjWrap.m_shape.getAabb( ref body1Transform, out aabbMin1, out aabbMax1 );
if( !btAabbUtil.TestAabbAgainstAabb2( ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1 ) )
{
//m_childCollisionAlgorithms[i].~btCollisionAlgorithm();
m_dispatcher.freeCollisionAlgorithm( m_childCollisionAlgorithms[i] );
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
//
static void Initialize( btConvexShape shape0, ref btTransform wtrs0,
btConvexShape shape1, ref btTransform wtrs1,
out btGjkEpaSolver2.sResults results,
tShape shape,
bool withmargins )
{
/* Results */
results.normal = btVector3.xAxis;
results.witness0 =
results.witness1 = btVector3.Zero;
results.status = btGjkEpaSolver2.sResults.eStatus.Separated;
results.distance = 0;
/* Shape */
shape.m_shape0 = shape0;
shape.m_shape1 = shape1;
wtrs1.m_basis.transposeTimes( ref wtrs0.m_basis, out shape.m_toshape1 );
wtrs0.inverseTimes( ref wtrs1, out shape.m_toshape0 );
shape.EnableMargin( withmargins );
}
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();
}
}
void collideSingleContact( bool usePertube, ref btQuaternion perturbeRot
, btCollisionObjectWrapper convexObjWrap
, ref btTransform convexTransform
, btCollisionObjectWrapper planeObjWrap
, ref btTransform planeTransform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut
, ref btVector3 planeNormal, double planeConstant
)
{
//btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
//btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;
btConvexShape convexShape = (btConvexShape)convexObjWrap.getCollisionShape();
btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape();
bool hasCollision = false;
//planeNormal = planeShape.getPlaneNormal().Copy( out planeNormal );
//double planeConstant = planeShape.getPlaneConstant();
btTransform convexWorldTransform = convexTransform;
//btTransform planeWorldTransform = planeObjWrap.m_worldTransform;
btTransform convexInPlaneTrans;
planeTransform.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
if( usePertube )
{
//now perturbe the convex-world transform
btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot );
btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe );
convexWorldTransform.m_basis = tmpPerturbe;
//convexWorldTransform.getBasis() *= btMatrix3x3( perturbeRot );
}
btTransform planeInConvex;
convexTransform.inverseTimes( ref planeObjWrap.m_collisionObject.m_worldTransform, out planeInConvex );
btVector3 tmp, tmp2;
planeNormal.Invert( out tmp );
planeInConvex.m_basis.Apply( ref tmp, out tmp2 );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp2, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( ref planeNormal, -distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; planeTransform.Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld );
hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold();
resultOut.setPersistentManifold( m_manifoldPtr );
if( hasCollision )
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB; planeTransform.m_basis.Apply( ref planeNormal, out normalOnSurfaceB );
btScalar.Dbg( "Convex plane adds point " + normalOnSurfaceB + " " + vtxInPlaneWorld + " " + distance.ToString( "g17" ) );
resultOut.addContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance );
}
}
