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