public virtual void Initialize(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool isSwapped) { base.Initialize(ci, body0, body1); m_isSwapped = isSwapped; m_sharedManifold = ci.GetManifold(); m_ownsManifold = false; CollisionObject colObj = m_isSwapped ? body1 : body0; Debug.Assert(colObj.GetCollisionShape().IsCompound()); CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape()); m_compoundShapeRevision = compoundShape.GetUpdateRevision(); PreallocateChildAlgorithms(body0, body1); }
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { //resultOut = null; CollisionObject colObj = m_isSwapped ? body1 : body0; CollisionObject otherObj = m_isSwapped ? body0 : body1; Debug.Assert(colObj.GetCollisionShape().IsCompound()); CompoundShape compoundShape = (CompoundShape)(colObj.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(body0, body1); } Dbvt tree = compoundShape.GetDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps using (CompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get()) { callback.Initialize(colObj, otherObj, m_dispatcher, dispatchInfo, resultOut, this, m_childCollisionAlgorithms, 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 { m_manifoldArray.Clear(); for (int i = 0; i < m_childCollisionAlgorithms.Count; i++) { if (m_childCollisionAlgorithms[i] != null) { m_childCollisionAlgorithms[i].GetAllContactManifolds(m_manifoldArray); for (int m = 0; m < m_manifoldArray.Count; m++) { if (m_manifoldArray[m].GetNumContacts() > 0) { resultOut.SetPersistentManifold(m_manifoldArray[m]); resultOut.RefreshContactPoints(); resultOut.SetPersistentManifold(null);//??necessary? } } m_manifoldArray.Clear(); } } } if (tree != null) { IndexedVector3 localAabbMin; IndexedVector3 localAabbMax; IndexedMatrix otherInCompoundSpace; //otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform()); otherInCompoundSpace = colObj.GetWorldTransform().Inverse() * otherObj.GetWorldTransform(); otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace, out localAabbMin, out localAabbMax); DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax); //process all children, that overlap with the given AABB bounds Dbvt.CollideTV(tree.m_root, ref bounds, callback, tree.CollideTVStack, ref tree.CollideTVCount); } else { //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.Count; for (int 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; m_manifoldArray.Clear(); CollisionShape childShape = null; IndexedMatrix orgTrans; IndexedMatrix orgInterpolationTrans; IndexedMatrix newChildWorldTrans; for (int i = 0; i < numChildren; i++) { if (m_childCollisionAlgorithms[i] != null) { childShape = compoundShape.GetChildShape(i); //if not longer overlapping, remove the algorithm orgTrans = colObj.GetWorldTransform(); orgInterpolationTrans = colObj.GetInterpolationWorldTransform(); IndexedMatrix childTrans = compoundShape.GetChildTransform(i); newChildWorldTrans = orgTrans * childTrans; //perform an AABB check first IndexedVector3 aabbMin0; IndexedVector3 aabbMax0; IndexedVector3 aabbMin1; IndexedVector3 aabbMax1; childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0); otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1); if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1)) { m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]); m_childCollisionAlgorithms[i] = null; } } } } } }