/*
		internal void Initialize( btCollisionAlgorithmConstructionInfo ci)
		{
			base.Initialize( ci );
        }
		*/

		internal override void processCollision( btCollisionObjectWrapper a
			, ref btTransform body0Transform
			, btCollisionObjectWrapper body1Wrap
			, ref btTransform body1Transform
			, btDispatcherInfo c, btManifoldResult d)
		{
		}
		internal override void processCollision( btCollisionObjectWrapper body0Wrap
			, ref btTransform body0Transform
			, btCollisionObjectWrapper body1Wrap
			, ref btTransform body1Transform
			, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
		{
			//(void)dispatchInfo;

			if( m_manifoldPtr == null )
				return;

			resultOut.setPersistentManifold( m_manifoldPtr );

			btSphereShape sphere0 = (btSphereShape)body0Wrap.getCollisionShape();
			btSphereShape sphere1 = (btSphereShape)body1Wrap.getCollisionShape();

			btVector3 diff; body0Wrap.m_collisionObject.m_worldTransform.m_origin.Sub( ref body1Wrap.m_collisionObject.m_worldTransform.m_origin, out diff );
			double len = diff.length();
			double radius0 = sphere0.getRadius();
			double radius1 = sphere1.getRadius();

#if CLEAR_MANIFOLD
	m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting
#endif

			///iff distance positive, don't generate a new contact
			if( len > ( radius0 + radius1 ) )
			{
#if !CLEAR_MANIFOLD
				resultOut.refreshContactPoints();
#endif //CLEAR_MANIFOLD
				return;
			}
			///distance (negative means penetration)
			double dist = len - ( radius0 + radius1 );

			btVector3 normalOnSurfaceB = btVector3.xAxis;
			if( len > btScalar.SIMD_EPSILON )
			{
				normalOnSurfaceB = diff / len;
			}

			///point on A (worldspace)
			///btVector3 pos0 = col0.getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
			///point on B (worldspace)
			btVector3 pos1; body1Wrap.m_collisionObject.m_worldTransform.m_origin.AddScale( ref normalOnSurfaceB, radius1, out pos1 );

			/// report a contact. internally this will be kept persistent, and contact reduction is done
			resultOut.addContactPoint( ref normalOnSurfaceB, ref pos1, dist );

#if !CLEAR_MANIFOLD
			resultOut.refreshContactPoints();
#endif //CLEAR_MANIFOLD

		}
		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

		}
		//
		// Convex-Convex collision algorithm
		//
		internal override void processCollision( btCollisionObjectWrapper body0Wrap
							, ref btTransform body0Transform
							, btCollisionObjectWrapper body1Wrap
							, ref btTransform body1Transform
							, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
		{

			if( m_manifoldPtr == null )
			{
				//swapped?
				m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject );
				m_ownManifold = true;
			}
			resultOut.setPersistentManifold( m_manifoldPtr );

			//comment-out next line to test multi-contact generation
			//resultOut.getPersistentManifold().clearManifold();


			btConvexShape min0 = (btConvexShape)body0Wrap.getCollisionShape();
			btConvexShape min1 = (btConvexShape)body1Wrap.getCollisionShape();

			btVector3 normalOnB;
			btVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
			if( ( min0.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE )
				&& ( min1.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) )
			{
				btCapsuleShape capsuleA = (btCapsuleShape)min0;
				btCapsuleShape capsuleB = (btCapsuleShape)min1;
				//	btVector3 localScalingA = capsuleA.getLocalScaling();
				//	btVector3 localScalingB = capsuleB.getLocalScaling();

				double threshold = m_manifoldPtr.getContactBreakingThreshold();

				double dist = capsuleCapsuleDistance( out normalOnB, out pointOnBWorld
						, capsuleA.getHalfHeight(), capsuleA.getRadius()
						, capsuleB.getHalfHeight(), capsuleB.getRadius()
						, capsuleA.getUpAxis(), capsuleB.getUpAxis()
						, ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, threshold );

				if( dist < threshold )
				{
					Debug.Assert( normalOnB.length2() >= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) );
					resultOut.addContactPoint( ref normalOnB, ref pointOnBWorld, dist );
				}
				resultOut.refreshContactPoints();
				return;
			}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER




#if USE_SEPDISTANCE_UTIL2
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
	}

	if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0)
#endif //USE_SEPDISTANCE_UTIL2

			{
				btGjkPairDetector.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get();
				input.Initialize();

				btGjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get();
				gjkPairDetector.Initialize( min0, min1, m_simplexSolver, m_pdSolver );
				//TODO: if (dispatchInfo.m_useContinuous)
				gjkPairDetector.setMinkowskiA( min0 );
				gjkPairDetector.setMinkowskiB( min1 );

#if USE_SEPDISTANCE_UTIL2
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
	} else
#endif //USE_SEPDISTANCE_UTIL2
				{
					//if (dispatchInfo.m_convexMaxDistanceUseCPT)
					//{
					//	input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactProcessingThreshold();
					//} else
					//{
					input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactBreakingThreshold();
					//		}

					input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
				}

				input.m_transformA = body0Transform;
				input.m_transformB = body1Transform;


#if USE_SEPDISTANCE_UTIL2
	double sepDist = 0;
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		sepDist = gjkPairDetector.getCachedSeparatingDistance();
		if (sepDist>SIMD_EPSILON)
		{
			sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
			//now perturbe directions to get multiple contact points
			
		}
	}
#endif //USE_SEPDISTANCE_UTIL2

				if( min0.isPolyhedral() && min1.isPolyhedral() )
				{



					btDummyResult dummy = new btDummyResult();

					///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it

					double min0Margin = min0.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min0.getMargin();
					double min1Margin = min1.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min1.getMargin();

					btWithoutMarginResult withoutMargin = new btWithoutMarginResult( resultOut, min0Margin, min1Margin );

					btPolyhedralConvexShape polyhedronA = (btPolyhedralConvexShape)min0;
					btPolyhedralConvexShape polyhedronB = (btPolyhedralConvexShape)min1;
					if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getConvexPolyhedron() != null )
					{
						double threshold = m_manifoldPtr.getContactBreakingThreshold();

						double minDist = -1e30f;
						btVector3 sepNormalWorldSpace;
						bool foundSepAxis = true;

						if( dispatchInfo.m_enableSatConvex )
						{
							foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
								polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
								ref body0Wrap.m_collisionObject.m_worldTransform,
								ref body1Wrap.m_collisionObject.m_worldTransform,
								out sepNormalWorldSpace, resultOut );
						}
						else
						{
#if ZERO_MARGIN
				gjkPairDetector.setIgnoreMargin(true);
				gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else

							gjkPairDetector.getClosestPoints( input, withoutMargin, dispatchInfo.m_debugDraw );
							//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN
							//double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
							//if (l2>SIMD_EPSILON)
							{
								sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1/l2);
																							//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
								minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0.getMargin()+min1.getMargin();

#if ZERO_MARGIN
					foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0;
#else
								foundSepAxis = withoutMargin.m_foundResult && minDist < 0;//-(min0.getMargin()+min1.getMargin());
#endif
							}
						}
						if( foundSepAxis )
						{

							//				Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);

							btPolyhedralContactClipping.clipHullAgainstHull( ref sepNormalWorldSpace, polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
								ref body0Wrap.m_collisionObject.m_worldTransform,
								ref body1Wrap.m_collisionObject.m_worldTransform
								, minDist - threshold, threshold, resultOut );

						}
						if( m_ownManifold )
						{
							resultOut.refreshContactPoints();
						}
						BulletGlobals.ClosestPointInputPool.Free( input );
						BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
						return;

					}
					else
					{
						//we can also deal with convex versus triangle (without connectivity data)
						if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE )
						{

							btVertexArray vertices = new btVertexArray();
							btTriangleShape tri = (btTriangleShape)polyhedronB;
							btVector3 tmp;
							body1Transform.Apply( ref tri.m_vertices1, out tmp );
							vertices.Add( ref tmp );
							body1Transform.Apply( ref tri.m_vertices2, out tmp );
							vertices.Add( ref tmp );
							body1Transform.Apply( ref tri.m_vertices3, out tmp );
							vertices.Add( ref tmp );

							//tri.initializePolyhedralFeatures();

							double threshold = m_manifoldPtr.getContactBreakingThreshold();

							btVector3 sepNormalWorldSpace;
							double minDist = -btScalar.BT_LARGE_FLOAT;
							double maxDist = threshold;

							bool foundSepAxis = false;
							if( false )
							{
								polyhedronB.initializePolyhedralFeatures();
								foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
										polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
										ref body0Wrap.m_collisionObject.m_worldTransform,
										ref body1Wrap.m_collisionObject.m_worldTransform,
										out sepNormalWorldSpace, resultOut );
								//	 Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);
								btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
									, polyhedronA.getConvexPolyhedron(),
									ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );

							}
							else
							{
#if ZERO_MARGIN
					gjkPairDetector.setIgnoreMargin(true);
					gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
								gjkPairDetector.getClosestPoints( input, dummy, dispatchInfo.m_debugDraw );
#endif//ZERO_MARGIN

								double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
								if( l2 > btScalar.SIMD_EPSILON )
								{
									gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );
									//minDist = gjkPairDetector.getCachedSeparatingDistance();
									//maxDist = threshold;
									minDist = gjkPairDetector.getCachedSeparatingDistance() - min0.getMargin() - min1.getMargin();
									//foundSepAxis = true;
									btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
										, polyhedronA.getConvexPolyhedron(),
										ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );
								}
								else
								{
									//sepNormalWorldSpace = btVector3.Zero;
								}
							}


							if( m_ownManifold )
							{
								resultOut.refreshContactPoints();
							}
							BulletGlobals.ClosestPointInputPool.Free( input );
							BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );

							return;
						}

					}


				}

				gjkPairDetector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw );

				//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

				//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
				if( m_numPerturbationIterations != 0
					&& resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold )
				{

					int i;
					btVector3 v0, v1;
					btVector3 sepNormalWorldSpace;
					double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();

					if( l2 > btScalar.SIMD_EPSILON )
					{
						gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );

						btVector3.btPlaneSpace1( ref sepNormalWorldSpace, out v0, out v1 );


						bool perturbeA = true;
						double angleLimit = 0.125f * btScalar.SIMD_PI;
						double perturbeAngle;
						double radiusA = min0.getAngularMotionDisc();
						double radiusB = min1.getAngularMotionDisc();
						if( radiusA < radiusB )
						{
							perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusA;
							perturbeA = true;
						}
						else
						{
							perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusB;
							perturbeA = false;
						}
						if( perturbeAngle > angleLimit )
							perturbeAngle = angleLimit;

						btTransform unPerturbedTransform;
						if( perturbeA )
						{
							unPerturbedTransform = input.m_transformA;
						}
						else
						{
							unPerturbedTransform = input.m_transformB;
						}

						for( i = 0; i < m_numPerturbationIterations; i++ )
						{
							if( v0.length2() > btScalar.SIMD_EPSILON )
							{
								btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle );
								double iterationAngle = i * ( btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations ) );
								btQuaternion rotq = new btQuaternion( ref sepNormalWorldSpace, iterationAngle );


								if( perturbeA )
								{
									btQuaternion tmpq;
									btQuaternion tmpq2;
									rotq.inverse( out tmpq );
									btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
									btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
									btMatrix3x3 m = new btMatrix3x3( ref tmpq );
									btMatrix3x3 m2; body0Transform.getBasis( out m2 );
									btMatrix3x3 m3;
									btMatrix3x3.Mult( ref m, ref m2, out m3 );
									input.m_transformA.setBasis( ref m3 );
									input.m_transformB = body1Transform;
#if DEBUG_CONTACTS
					dispatchInfo.m_debugDraw.drawTransform(input.m_transformA,10.0);
#endif //DEBUG_CONTACTS
								}
								else
								{
									btQuaternion tmpq;
									btQuaternion tmpq2;
									rotq.inverse( out tmpq );
									btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
									btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
									btMatrix3x3 m = new btMatrix3x3( ref tmpq );
									btMatrix3x3 m2; body1Transform.getBasis( out m2 );
									btMatrix3x3 m3;
									btMatrix3x3.Mult( ref m, ref m2, out m3 );

									input.m_transformA = body0Transform;
									input.m_transformB.setBasis( ref m3 );
#if DEBUG_CONTACTS
					dispatchInfo.m_debugDraw.drawTransform(input.m_transformB,10.0);
#endif
								}

								btPerturbedContactResult perturbedResultOut = BulletGlobals.PerturbedContactResultPool.Get();
								if( perturbeA )
									perturbedResultOut.Initialize( resultOut
										, ref input.m_transformA, ref input.m_transformB
										, ref input.m_transformA
										, perturbeA
										, dispatchInfo.m_debugDraw );
								else
									perturbedResultOut.Initialize( resultOut
										, ref input.m_transformA, ref input.m_transformB
										, ref input.m_transformB
												, perturbeA
												, dispatchInfo.m_debugDraw );
								gjkPairDetector.getClosestPoints( input, perturbedResultOut, dispatchInfo.m_debugDraw );
								BulletGlobals.PerturbedContactResultPool.Free( perturbedResultOut );
							}
						}
					}
				}



#if USE_SEPDISTANCE_UTIL2
	if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
	{
		m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
	}
#endif //USE_SEPDISTANCE_UTIL2

				BulletGlobals.ClosestPointInputPool.Free( input );
				BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );

			}

			if( m_ownManifold )
			{
				resultOut.refreshContactPoints();
			}

		}
            internal void Initialize( btManifoldResult originalResult
				, ref btTransform transformA, ref btTransform transformB
				, ref btTransform unPerturbedTransform, bool perturbA, btIDebugDraw debugDrawer )
			{
				m_originalManifoldResult = ( originalResult );
				m_transformA = ( transformA );
				m_transformB = ( transformB );
				m_unPerturbedTransform = ( unPerturbedTransform );
				m_perturbA = ( perturbA );
				m_debugDrawer = ( debugDrawer );
			}
		internal override double calculateTimeOfImpact( btCollisionObject col0, btCollisionObject col1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
		{
			//not yet
			return btScalar.BT_ONE;
		}
		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;
						}
					}
				}
			}
		}
		internal override double calculateTimeOfImpact( btCollisionObject a, btCollisionObject b, btDispatcherInfo c , btManifoldResult d )
		{
			return btScalar.BT_ONE;
		}
		internal override void processCollision( btCollisionObjectWrapper col0Wrap
			, ref btTransform body0Transform
			, btCollisionObjectWrapper col1Wrap
			, ref btTransform body1Transform
			, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
		{
			if( m_manifoldPtr == null )
				return;

			btCollisionObjectWrapper sphereObjWrap = m_swapped ? col1Wrap : col0Wrap;
			btCollisionObjectWrapper triObjWrap = m_swapped ? col0Wrap : col1Wrap;

			btSphereShape sphere = (btSphereShape)sphereObjWrap.getCollisionShape();
			btTriangleShape triangle = (btTriangleShape)triObjWrap.getCollisionShape();

			/// report a contact. internally this will be kept persistent, and contact reduction is done
			resultOut.setPersistentManifold( m_manifoldPtr );
			SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get();
			detector.Initialize( sphere, triangle, m_manifoldPtr.getContactBreakingThreshold());

			btDiscreteCollisionDetectorInterface.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get();

			input.m_maximumDistanceSquared = btScalar.BT_LARGE_FLOAT;///@todo: tighter bounds
			if( m_swapped )
			{
				input.m_transformA = body1Transform;
				input.m_transformB = body0Transform;
			}
			else
			{
				input.m_transformA = body0Transform;
				input.m_transformB = body1Transform;
			}

			bool swapResults = m_swapped;

			detector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw, swapResults );

			BulletGlobals.ClosestPointInputPool.Free( input );
			BulletGlobals.SphereTriangleDetectorPool.Free( detector );
			if( m_ownManifold )
				resultOut.refreshContactPoints();

		}
		internal override void processCollision( btCollisionObjectWrapper body0Wrap
			, ref btTransform body0Transform
			, btCollisionObjectWrapper body1Wrap
			, ref btTransform body1Transform
			, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
		{
			//(void)dispatchInfo;
			if( m_manifoldPtr == null )
				return;
			btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
			btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;

			btConvexShape convexShape = (btConvexShape)convexObjWrap.m_shape;
			btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape();
			//btVector3 planeNormal;
			//planeShape.getPlaneNormal().Copy( out planeNormal );
			double planeConstant = planeShape.getPlaneConstant();
			if( m_swapped )
				collideSingleContact( false, ref btQuaternion.Identity, convexObjWrap
					, ref body1Transform
					, planeObjWrap
					, ref body0Transform
					, dispatchInfo, resultOut
							, ref planeShape.m_planeNormal, planeConstant );
			else
				collideSingleContact( false, ref btQuaternion.Identity, convexObjWrap
					, ref body0Transform
					, planeObjWrap
					, ref body1Transform
					, dispatchInfo, resultOut
							, ref planeShape.m_planeNormal, planeConstant );
			/*
						btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
						btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;

						btConvexShape convexShape = (btConvexShape)convexObjWrap.m_shape;
						btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.m_shape;

						bool hasCollision = false;
						btVector3 planeNormal; planeShape.m_planeNormal.Copy( out planeNormal );
						double planeConstant = planeShape.getPlaneConstant();
						btTransform planeInConvex;
						convexObjWrap.getWorldTransform().inverseTimes( planeObjWrap.getWorldTransform(), out planeInConvex );
						btTransform convexInPlaneTrans;
						planeObjWrap.getWorldTransform().inverseTimes( convexObjWrap.getWorldTransform(), out convexInPlaneTrans );

						btVector3 invPlaneNormal;
						planeNormal.Invert( out invPlaneNormal );
						btVector3 tmp;
						planeInConvex.getBasis().Apply( planeNormal, out tmp );
						btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp, out vtx );
						btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
						double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant );

						btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( planeNormal, -distance, out vtxInPlaneProjected );
						btVector3 vtxInPlaneWorld; planeObjWrap.getWorldTransform().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 = planeObjWrap.getWorldTransform().getBasis() * planeNormal;
							btVector3 pOnB = vtxInPlaneWorld;
							resultOut.addContactPoint( ref normalOnSurfaceB, ref pOnB, distance );
						}
			*/
			//the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
			//they keep on rolling forever because of the additional off-center contact points
			//so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
			if( convexShape.isPolyhedral() && resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold )
			{
				btVector3 v0, v1;
				btVector3.btPlaneSpace1( ref planeShape.m_planeNormal, out v0, out v1 );
				//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

				double angleLimit = 0.125f * btScalar.SIMD_PI;
				double perturbeAngle;
				double radius = convexShape.getAngularMotionDisc();
				perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radius;
				if( perturbeAngle > angleLimit )
					perturbeAngle = angleLimit;

				btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle );
				double interval = btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations );
				for( int i = 0; i < m_numPerturbationIterations; i++ )
				{
					double iterationAngle = i * interval;
					btQuaternion rotq = new btQuaternion( ref planeShape.m_planeNormal, iterationAngle );
					btQuaternion rotqInv;
					rotq.inverse( out rotqInv );
					btQuaternion tmpq, tmpq2;
					rotqInv.Mult( ref perturbeRot, out tmpq );
					tmpq.Mult( ref rotq, out tmpq2 );
					if( m_swapped )
						collideSingleContact( true, ref tmpq2
							, convexObjWrap, ref body1Transform
							, planeObjWrap, ref body0Transform
							, dispatchInfo
										, resultOut, ref planeShape.m_planeNormal, planeConstant );
					else
						collideSingleContact( true, ref tmpq2
							, convexObjWrap, ref body0Transform
							, planeObjWrap, ref body1Transform, dispatchInfo
									, resultOut, ref planeShape.m_planeNormal, planeConstant );
				}
			}

			if( m_ownManifold )
			{
				if( m_manifoldPtr.m_cachedPoints != 0 )
				{
					resultOut.refreshContactPoints();
				}
			}
		}
		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 );
			}
		}
		internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
		{
			//(void)resultOut;
			//(void)dispatchInfo;
			btCollisionObject convexbody = m_isSwapped ? body1 : body0;
			btCollisionObject triBody = m_isSwapped ? body0 : body1;

			//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)

			//only perform CCD above a certain threshold, this prevents blocking on the long run
			//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
			double squareMot0 = btVector3.btDelLength2( ref convexbody.m_interpolationWorldTransform.m_origin, ref convexbody.m_worldTransform.m_origin );
			if( squareMot0 < convexbody.getCcdSquareMotionThreshold() )
			{
				return btScalar.BT_ONE;
			}

			//ref btVector3 from = convexbody.m_worldTransform.getOrigin();
			//btVector3 to = convexbody.m_interpolationWorldTransform.getOrigin();
			//todo: only do if the motion exceeds the 'radius'

			btTransform triInv; triBody.m_worldTransform.inverse( out triInv );
			btTransform convexFromLocal; triInv.Apply( ref convexbody.m_worldTransform, out convexFromLocal );
			btTransform convexToLocal; triInv.Apply( ref convexbody.m_interpolationWorldTransform, out convexToLocal );

			if( triBody.getCollisionShape().isConcave() )
			{
				btVector3 rayAabbMin = convexFromLocal.m_origin;
				rayAabbMin.setMin( ref convexToLocal.m_origin );
				btVector3 rayAabbMax = convexFromLocal.m_origin;
				rayAabbMax.setMax( ref convexToLocal.m_origin );
				double ccdRadius0 = convexbody.getCcdSweptSphereRadius();
				rayAabbMin.AddScale( ref btVector3.One, -ccdRadius0, out rayAabbMin );
				rayAabbMax.AddScale( ref btVector3.One, ccdRadius0, out rayAabbMax );

				double curHitFraction = btScalar.BT_ONE; //is this available?
				LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback ( ref convexFromLocal, ref convexToLocal,
					convexbody.getCcdSweptSphereRadius(), curHitFraction);

				raycastCallback.m_hitFraction = convexbody.getHitFraction();

				btCollisionObject concavebody = triBody;

				btConcaveShape triangleMesh = (btConcaveShape)concavebody.getCollisionShape();

				if( triangleMesh != null )
				{
					triangleMesh.processAllTriangles( raycastCallback, ref rayAabbMin, ref rayAabbMax );
				}



				if( raycastCallback.m_hitFraction < convexbody.getHitFraction() )
				{
					convexbody.setHitFraction( raycastCallback.m_hitFraction );
					return raycastCallback.m_hitFraction;
				}
			}

			return btScalar.BT_ONE;

		}
		internal override void processCollision( btCollisionObjectWrapper body0Wrap
			, ref btTransform body0Transform
			, btCollisionObjectWrapper body1Wrap
			, ref btTransform body1Transform
			, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
		{


			btCollisionObjectWrapper convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
			btCollisionObjectWrapper triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;

			if( triBodyWrap.getCollisionShape().isConcave() )
			{
				btConcaveShape concaveShape = (btConcaveShape)triBodyWrap.getCollisionShape();

				if( convexBodyWrap.getCollisionShape().isConvex() )
				{
					double collisionMarginTriangle = concaveShape.getMargin();

					resultOut.setPersistentManifold( m_btConvexTriangleCallback.m_manifoldPtr );
					if( m_isSwapped )
						m_btConvexTriangleCallback.setTimeStepAndCounters( collisionMarginTriangle, dispatchInfo
								, convexBodyWrap, ref body1Transform
								, triBodyWrap, ref body0Transform
								, resultOut );
					else
						m_btConvexTriangleCallback.setTimeStepAndCounters( collisionMarginTriangle, dispatchInfo
								, convexBodyWrap, ref body0Transform
								, triBodyWrap, ref body1Transform
								, resultOut );

					m_btConvexTriangleCallback.m_manifoldPtr.setBodies( convexBodyWrap.m_collisionObject, triBodyWrap.m_collisionObject );

					concaveShape.processAllTriangles( m_btConvexTriangleCallback, ref m_btConvexTriangleCallback.m_aabbMin, ref m_btConvexTriangleCallback.m_aabbMax );

					resultOut.refreshContactPoints();

					m_btConvexTriangleCallback.clearWrapperData();

				}

			}

		}
		public void setTimeStepAndCounters( double collisionMarginTriangle
			, btDispatcherInfo dispatchInfo, btCollisionObjectWrapper convexBodyWrap
			, ref btTransform convexTransform
			, btCollisionObjectWrapper triBodyWrap
			, ref btTransform triBodyTransform
			, btManifoldResult resultOut )
		{
			m_convexBodyWrap = convexBodyWrap;
			m_triBodyWrap = triBodyWrap;

			m_dispatchInfoPtr = dispatchInfo;
			m_collisionMarginTriangle = collisionMarginTriangle;
			m_resultOut = resultOut;

			//recalc aabbs
			btTransform convexInTriangleSpace;
			btTransform invTrans;
			triBodyTransform.inverse( out invTrans );
			invTrans.Apply( ref convexTransform, out convexInTriangleSpace );
			btCollisionShape convexShape = m_convexBodyWrap.getCollisionShape();
			//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody.m_collisionShape);
			convexShape.getAabb( ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax );
			double extraMargin = collisionMarginTriangle;
			btVector3 extra = new btVector3( extraMargin );
			m_aabbMax += extra;
			m_aabbMin -= extra;
		}
Example #15
0
		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;

		}
			internal btCompoundCompoundLeafCallback( btCollisionObjectWrapper compound1ObjWrap,
											btCollisionObjectWrapper compound0ObjWrap,
											btDispatcher dispatcher,
											btDispatcherInfo dispatchInfo,
											btManifoldResult resultOut,
											btHashedSimplePairCache childAlgorithmsCache,
											btPersistentManifold sharedManifold )
			{
				m_numOverlapPairs = ( 0 );
				m_compound0ColObjWrap = ( compound1ObjWrap );
				m_compound1ColObjWrap = ( compound0ObjWrap );
				m_dispatcher = ( dispatcher );
				m_dispatchInfo = ( dispatchInfo );
				m_resultOut = ( resultOut );
				m_childCollisionAlgorithmCache = ( childAlgorithmsCache );
				m_sharedManifold = ( sharedManifold );

			}
		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();
			}

		}
			internal void Initialize( btCollisionObjectWrapper compoundObjWrap, btCollisionObjectWrapper otherObjWrap, btDispatcher dispatcher, btDispatcherInfo dispatchInfo, btManifoldResult resultOut, btCollisionAlgorithm[] childCollisionAlgorithms, btPersistentManifold sharedManifold )
			{
				m_compoundColObjWrap = ( compoundObjWrap );
				m_otherObjWrap = ( otherObjWrap );
				m_dispatcher = ( dispatcher );
				m_dispatchInfo = ( dispatchInfo );
				m_resultOut = ( resultOut );
				m_childCollisionAlgorithms = ( childCollisionAlgorithms );
				m_sharedManifold = ( sharedManifold );
			}
		internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
		{
			Debug.Assert( false );
			return 0;

		}
		internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
		{
			Debug.Assert( false );
			//needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
			btCollisionObject colObj = m_isSwapped ? body1 : body0;
			btCollisionObject otherObj = m_isSwapped ? body0 : body1;

			Debug.Assert( colObj.getCollisionShape().isCompound() );

			btCompoundShape compoundShape = (btCompoundShape)( colObj.getCollisionShape() );

			//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
			//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
			//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
			//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
			//then use each overlapping node AABB against Tree0
			//and vise versa.

			double hitFraction = btScalar.BT_ONE;

			int numChildren = m_childCollisionAlgorithms.Count;
			int i;
			//btTransform orgTrans;
			double frac;
			for( i = 0; i < numChildren; i++ )
			{
				//btCollisionShape  childShape = compoundShape.getChildShape(i);

				//backup
				//orgTrans = colObj.m_worldTransform;

				btTransform childTrans = compoundShape.getChildTransform( i );
				//btTransform	newChildWorldTrans = orgTrans*childTrans ;
				//colObj.setWorldTransform( orgTrans * childTrans );
				btTransform tmp;
				colObj.m_worldTransform.Apply( ref childTrans, out tmp );
				colObj.m_worldTransform = tmp;
				//btCollisionShape  tmpShape = colObj.getCollisionShape();
				//colObj.internalSetTemporaryCollisionShape( childShape );
				frac = m_childCollisionAlgorithms[i].calculateTimeOfImpact( colObj, otherObj, dispatchInfo, resultOut );
				if( frac < hitFraction )
				{
					hitFraction = frac;
				}
				//revert back
				//colObj.internalSetTemporaryCollisionShape( tmpShape);
				colObj.setWorldTransform( ref colObj.m_worldTransform );
			}
			return hitFraction;

		}
		internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut)
		{
			//not yet
			return 1;
		}