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 virtual void AabbTest(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, IBroadphaseAabbCallback aabbCallback)
{
BroadphaseAabbTester callback = new BroadphaseAabbTester(aabbCallback);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax);
Dbvt.CollideTV(m_sets[0].m_root, ref bounds, callback);
Dbvt.CollideTV(m_sets[1].m_root, ref bounds, callback);
}
public void CreateAabbTreeFromChildren()
{
if (m_dynamicAabbTree == null)
{
m_dynamicAabbTree = new Dbvt();
for (int index = 0; index < m_children.Count; index++)
{
CompoundShapeChild child = m_children[index];
//extend the local aabbMin/aabbMax
IndexedVector3 localAabbMin, localAabbMax;
child.m_childShape.GetAabb(ref child.m_transform, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (object)index);
}
}
}
public void UpdateChildTransform(int childIndex, ref IndexedMatrix newChildTransform, bool shouldRecalculateLocalAabb)
{
m_children[childIndex].m_transform = newChildTransform;
if (m_dynamicAabbTree != null)
{
///update the dynamic aabb tree
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
m_children[childIndex].m_childShape.GetAabb(ref newChildTransform, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//int index = m_children.Count - 1;
m_dynamicAabbTree.Update(m_children[childIndex].m_treeNode, ref bounds);
}
if (shouldRecalculateLocalAabb)
{
RecalculateLocalAabb();
}
}
public virtual BroadphaseProxy CreateProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
DbvtProxy proxy = new DbvtProxy(ref aabbMin, ref aabbMax, userPtr, collisionFilterGroup, collisionFilterMask);
DbvtAabbMm aabb = DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax);
//bproxy.aabb = btDbvtAabbMm::FromMM(aabbMin,aabbMax);
proxy.stage = m_stageCurrent;
proxy.m_uniqueId = ++m_gid;
proxy.leaf = m_sets[0].Insert(ref aabb, proxy);
ListAppend(proxy, ref m_stageRoots[m_stageCurrent]);
if (!m_deferedcollide)
{
DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get();
collider.Initialize(this);
collider.proxy = proxy;
Dbvt.CollideTV(m_sets[0].m_root, ref aabb, collider);
Dbvt.CollideTV(m_sets[1].m_root, ref aabb, collider);
BulletGlobals.DbvtTreeColliderPool.Free(collider);
}
return(proxy);
}
///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
///it is not part of the btBroadphaseInterface but specific to btDbvtBroadphase.
///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
///http://code.google.com/p/bullet/issues/detail?id=223
public void SetAabbForceUpdate(BroadphaseProxy absproxy, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, IDispatcher dispatcher)
{
DbvtProxy proxy = absproxy as DbvtProxy;
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax);
bool docollide = false;
if (proxy.stage == STAGECOUNT)
{/* fixed . dynamic set */
m_sets[1].Remove(proxy.leaf);
proxy.leaf = m_sets[0].Insert(ref bounds, proxy);
docollide = true;
}
else
{/* dynamic set */
++m_updates_call;
/* Teleporting */
m_sets[0].Update(proxy.leaf, ref bounds);
++m_updates_done;
docollide = true;
}
ListRemove(proxy, ref m_stageRoots[proxy.stage]);
proxy.m_aabbMin = aabbMin;
proxy.m_aabbMax = aabbMax;
proxy.stage = m_stageCurrent;
ListAppend(proxy, ref m_stageRoots[m_stageCurrent]);
if (docollide)
{
m_needcleanup = true;
if (!m_deferedcollide)
{
DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get();
collider.Initialize(this);
Dbvt.CollideTTpersistentStack(m_sets[1].m_root, proxy.leaf, collider);
Dbvt.CollideTTpersistentStack(m_sets[0].m_root, proxy.leaf, collider);
BulletGlobals.DbvtTreeColliderPool.Free(collider);
}
}
}
public virtual void SetAabb(BroadphaseProxy absproxy, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, IDispatcher dispatcher)
{
DbvtProxy proxy = absproxy as DbvtProxy;
DbvtAabbMm aabb = DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax);
#if DBVT_BP_PREVENTFALSEUPDATE
if (NotEqual(ref aabb, proxy, leaf.volume))
#endif
{
bool docollide = false;
if (proxy.stage == STAGECOUNT)
{/* fixed . dynamic set */
m_sets[1].Remove(proxy.leaf);
proxy.leaf = m_sets[0].Insert(ref aabb, proxy);
docollide = true;
}
else
{/* dynamic set */
++m_updates_call;
if (DbvtAabbMm.Intersect(ref proxy.leaf.volume, ref aabb))
{/* Moving */
IndexedVector3 delta = aabbMin - proxy.m_aabbMin;
IndexedVector3 velocity = (((proxy.m_aabbMax - proxy.m_aabbMin) / 2f) * m_prediction);
if (delta.X < 0)
{
velocity.X = -velocity.X;
}
if (delta.Y < 0)
{
velocity.Y = -velocity.Y;
}
if (delta.Z < 0)
{
velocity.Z = -velocity.Z;
}
if (
#if DBVT_BP_MARGIN
m_sets[0].Update(proxy.leaf, ref aabb, ref velocity, DBVT_BP_MARGIN)
#else
m_sets[0].update(proxy.leaf, aabb, ref velocity)
#endif
)
{
++m_updates_done;
docollide = true;
}
}
else
{/* Teleporting */
m_sets[0].Update(proxy.leaf, ref aabb);
++m_updates_done;
docollide = true;
}
}
ListRemove(proxy, ref m_stageRoots[proxy.stage]);
proxy.m_aabbMin = aabbMin;
proxy.m_aabbMax = aabbMax;
proxy.stage = m_stageCurrent;
ListAppend(proxy, ref m_stageRoots[m_stageCurrent]);
if (docollide)
{
m_needcleanup = true;
if (!m_deferedcollide)
{
DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get();
collider.Initialize(this);
Dbvt.CollideTTpersistentStack(m_sets[1].m_root, proxy.leaf, collider);
Dbvt.CollideTTpersistentStack(m_sets[0].m_root, proxy.leaf, collider);
BulletGlobals.DbvtTreeColliderPool.Free(collider);
}
}
}
}
public void Collide(IDispatcher dispatcher)
{
BulletGlobals.StartProfile("BroadphaseCollide");
//SPC(m_profiling.m_total);
/* optimize */
m_sets[0].OptimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
if (m_fixedleft > 0)
{
int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100;
m_sets[1].OptimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100);
m_fixedleft = Math.Max(0, m_fixedleft - count);
}
/* dynamic . fixed set */
m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
DbvtProxy current = m_stageRoots[m_stageCurrent];
if (current != null)
{
DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get();
collider.Initialize(this);
do
{
DbvtProxy next = current.links[1];
ListRemove(current, ref m_stageRoots[current.stage]);
ListAppend(current, ref m_stageRoots[STAGECOUNT]);
#if DBVT_BP_ACCURATESLEEPING
m_paircache.removeOverlappingPairsContainingProxy(current, dispatcher);
collider.proxy = current;
btDbvt::collideTV(m_sets[0].m_root, current.aabb, collider);
btDbvt::collideTV(m_sets[1].m_root, current.aabb, collider);
#endif
m_sets[0].Remove(current.leaf);
DbvtAabbMm curAabb = DbvtAabbMm.FromMM(ref current.m_aabbMin, ref current.m_aabbMax);
current.leaf = m_sets[1].Insert(ref curAabb, current);
current.stage = STAGECOUNT;
current = next;
} while (current != null);
m_fixedleft = m_sets[1].m_leaves;
BulletGlobals.DbvtTreeColliderPool.Free(collider);
m_needcleanup = true;
}
/* collide dynamics */
{
DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get();
collider.Initialize(this);
if (m_deferedcollide)
{
//Stopwatch fdCollideStopwatch = new Stopwatch();
//fdCollideStopwatch.Start();
//SPC(m_profiling.m_fdcollide);
Dbvt.CollideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
//fdCollideStopwatch.Stop();
//m_profiling.m_fdcollide += (ulong)fdCollideStopwatch.ElapsedMilliseconds;
}
if (m_deferedcollide)
{
//Stopwatch ddCollideStopwatch = new Stopwatch();
//ddCollideStopwatch.Start();
//SPC(m_profiling.m_ddcollide);
Dbvt.CollideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
//ddCollideStopwatch.Stop();
//m_profiling.m_ddcollide += (ulong)ddCollideStopwatch.ElapsedMilliseconds;
}
BulletGlobals.DbvtTreeColliderPool.Free(collider);
}
/* clean up */
if (m_needcleanup)
{
Stopwatch cleanupStopwatch = new Stopwatch();
cleanupStopwatch.Start();
//SPC(m_profiling.m_cleanup);
IList <BroadphasePair> pairs = m_paircache.GetOverlappingPairArray();
if (pairs.Count > 0)
{
int ni = Math.Min(pairs.Count, Math.Max(m_newpairs, (pairs.Count * m_cupdates) / 100));
for (int i = 0; i < ni; ++i)
{
BroadphasePair p = pairs[(m_cid + i) % pairs.Count];
DbvtProxy pa = p.m_pProxy0 as DbvtProxy;
DbvtProxy pb = p.m_pProxy1 as DbvtProxy;
if (!DbvtAabbMm.Intersect(ref pa.leaf.volume, ref pb.leaf.volume))
{
#if DBVT_BP_SORTPAIRS
if (pa.m_uniqueId > pb.m_uniqueId)
{
btSwap(pa, pb);
}
#endif
m_paircache.RemoveOverlappingPair(pa, pb, dispatcher);
--ni; --i;
}
}
if (pairs.Count > 0)
{
m_cid = (m_cid + ni) % pairs.Count;
}
else
{
m_cid = 0;
}
}
cleanupStopwatch.Stop();
//m_profiling.m_cleanup += (ulong)cleanupStopwatch.ElapsedMilliseconds;
}
++m_pid;
m_newpairs = 1;
m_needcleanup = false;
if (m_updates_call > 0)
{
m_updates_ratio = m_updates_done / (float)m_updates_call;
}
else
{
m_updates_ratio = 0;
}
m_updates_done /= 2;
m_updates_call /= 2;
BulletGlobals.StopProfile();
}
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;
}
}
}
}
}
}
