public void AddChildShape(ref IndexedMatrix 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.GetShapeType();
child.m_childMargin = shape.GetMargin();
//extend the local aabbMin/aabbMax
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
shape.GetAabb(ref localTransform, out localAabbMin, out 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 AddChildShape(ref IndexedMatrix 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.GetShapeType();
child.m_childMargin = shape.GetMargin();
//extend the local aabbMin/aabbMax
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
shape.GetAabb(ref localTransform, out localAabbMin, out 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);
}
protected void GImpactVsShapeFindPairs(
ref IndexedMatrix trans0,
ref IndexedMatrix trans1,
GImpactShapeInterface shape0,
CollisionShape shape1,
ObjectArray <int> collided_primitives)
{
AABB boxshape = new AABB();
if (shape0.HasBoxSet())
{
IndexedMatrix trans1to0 = trans0.Inverse();
//trans1to0 *= trans1;
trans1to0 = trans1to0 * trans1;
//trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0);
shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max);
}
#endif
shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives);
}
else
{
shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max);
AABB boxshape0 = new AABB();
int i = shape0.GetNumChildShapes();
while (i-- != 0)
{
shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);
if (boxshape.HasCollision(ref boxshape0))
{
collided_primitives.Add(i);
}
}
}
}
public virtual void GetPrimitiveBox(int prim_index, out AABB primbox)
{
IndexedMatrix prim_trans;
if (m_compoundShape.ChildrenHasTransform())
{
prim_trans = m_compoundShape.GetChildTransform(prim_index);
}
else
{
prim_trans = IndexedMatrix.Identity;
}
CollisionShape shape = m_compoundShape.GetChildShape(prim_index);
shape.GetAabb(prim_trans, out primbox.m_min, out primbox.m_max);
}
public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
m_dispatchInfoPtr = dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle;
m_resultOut = resultOut;
//recalc aabbs
//IndexedMatrix convexInTriangleSpace = MathUtil.bulletMatrixMultiply(IndexedMatrix.Invert(m_triBody.getWorldTransform()) , m_convexBody.getWorldTransform());
IndexedMatrix convexInTriangleSpace = m_triBody.GetWorldTransform().Inverse() * m_convexBody.GetWorldTransform();
CollisionShape convexShape = m_convexBody.GetCollisionShape();
convexShape.GetAabb(ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax);
float extraMargin = collisionMarginTriangle;
IndexedVector3 extra = new IndexedVector3(extraMargin);
m_aabbMax += extra;
m_aabbMin -= extra;
}
protected void GImpactVsShapeFindPairs(
ref IndexedMatrix trans0,
ref IndexedMatrix trans1,
GImpactShapeInterface shape0,
CollisionShape shape1,
ObjectArray<int> collided_primitives)
{
AABB boxshape = new AABB();
if (shape0.HasBoxSet())
{
IndexedMatrix trans1to0 = trans0.Inverse();
//trans1to0 *= trans1;
trans1to0 = trans1to0 * trans1;
//trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0);
shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max);
}
shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives);
}
else
{
shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max);
AABB boxshape0 = new AABB();
int i = shape0.GetNumChildShapes();
while (i-- != 0)
{
shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);
if (boxshape.HasCollision(ref boxshape0))
{
collided_primitives.Add(i);
}
}
}
}
public void ProcessChildShape(CollisionShape childShape, int index)
{
Debug.Assert(index >= 0);
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
Debug.Assert(index < compoundShape.GetNumChildShapes());
//backup
IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
IndexedMatrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(index);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(), out aabbMin1, out 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);
if (m_childCollisionAlgorithms[index] == m_parent)
{
int ibreak = 0;
}
}
///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))
{
IndexedVector3 worldAabbMin = IndexedVector3.Zero, worldAabbMax = IndexedVector3.Zero;
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new IndexedVector3(1, 1, 1));
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new IndexedVector3(1, 1, 1));
}
//revert back transform
m_compoundColObj.InternalSetTemporaryCollisionShape(tmpShape);
m_compoundColObj.SetWorldTransform(ref orgTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans);
}
}
public void ProcessChildShape(CollisionShape childShape, int index)
{
Debug.Assert(index >= 0);
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
Debug.Assert(index < compoundShape.GetNumChildShapes());
//backup
IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
IndexedMatrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(index);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(), out aabbMin1, out 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);
if (m_childCollisionAlgorithms[index] == m_parent)
{
int ibreak = 0;
}
}
///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))
{
IndexedVector3 worldAabbMin = IndexedVector3.Zero, worldAabbMax = IndexedVector3.Zero;
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new IndexedVector3(1, 1, 1));
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new IndexedVector3(1, 1, 1));
}
//revert back transform
m_compoundColObj.InternalSetTemporaryCollisionShape(tmpShape);
m_compoundColObj.SetWorldTransform(ref orgTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans);
}
}
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;
}
}
}
}
}
}