public void AddChildShape(ref Matrix localTransform, CollisionShape shape) { m_updateRevision++; //m_childTransforms.push_back(localTransform); //m_childShapes.push_back(shape); CompoundShapeChild child = new CompoundShapeChild(); child.m_transform = localTransform; child.m_childShape = shape; child.m_childShapeType = shape.ShapeType; child.m_childMargin = shape.Margin; //extend the local aabbMin/aabbMax Vector3 localAabbMin = new Vector3(); Vector3 localAabbMax = new Vector3(); shape.GetAabb(ref localTransform, ref localAabbMin, ref localAabbMax); MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin); MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax); if (m_dynamicAabbTree != null) { DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax); int index = m_children.Count; child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (Object)index); } m_children.Add(child); }
public void ProcessChildShape(CollisionShape childShape,int index) { CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape()); //backup Matrix orgTrans = m_compoundColObj.GetWorldTransform(); Matrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform(); Matrix childTrans = compoundShape.GetChildTransform(index); Matrix newChildWorldTrans = MathUtil.BulletMatrixMultiply(ref orgTrans,ref childTrans); //perform an AABB check first Vector3 aabbMin0 = Vector3.Zero; Vector3 aabbMax0 = Vector3.Zero; Vector3 aabbMin1 = Vector3.Zero; Vector3 aabbMax1 = Vector3.Zero; childShape.GetAabb(ref newChildWorldTrans,ref aabbMin0,ref aabbMax0); m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(),ref aabbMin1,ref aabbMax1); if (AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0,ref aabbMax0,ref aabbMin1,ref aabbMax1)) { m_compoundColObj.SetWorldTransform(ref newChildWorldTrans); m_compoundColObj.SetInterpolationWorldTransform(ref newChildWorldTrans); //the contactpoint is still projected back using the original inverted worldtrans CollisionShape tmpShape = m_compoundColObj.GetCollisionShape(); m_compoundColObj.InternalSetTemporaryCollisionShape( childShape ); if (m_childCollisionAlgorithms[index] == null) { m_childCollisionAlgorithms[index] = m_dispatcher.FindAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold); } ///detect swapping case if (m_resultOut.GetBody0Internal() == m_compoundColObj) { m_resultOut.SetShapeIdentifiersA(-1, index); } else { m_resultOut.SetShapeIdentifiersB(-1, index); } m_childCollisionAlgorithms[index].ProcessCollision(m_compoundColObj,m_otherObj,m_dispatchInfo, m_resultOut); if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0)) { Vector3 worldAabbMin = Vector3.Zero, worldAabbMax = Vector3.Zero; m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new Vector3(1, 1, 1)); m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new Vector3(1, 1, 1)); } //revert back transform m_compoundColObj.InternalSetTemporaryCollisionShape( tmpShape); m_compoundColObj.SetWorldTransform( ref orgTrans ); m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans); } }