public static void Swap(ref DbvtAabbMm a, ref DbvtAabbMm b)
{
DbvtAabbMm temp = b;
b = a;
a = temp;
}
public static void CollideTV(DbvtNode root, ref DbvtAabbMm volume, ICollide collideable)
{
CollideTVCount++;
Debug.Assert(CollideTVCount < 2);
CollideTVStack.Clear();
if (root != null)
{
CollideTVStack.Push(root);
do
{
DbvtNode n = CollideTVStack.Pop();
if (DbvtAabbMm.Intersect(ref n.volume, ref volume))
{
if (n.IsInternal())
{
CollideTVStack.Push(n._children[0]);
CollideTVStack.Push(n._children[1]);
}
else
{
collideable.Process(n);
}
}
} while (CollideTVStack.Count > 0);
}
CollideTVCount--;
}
public static DbvtAabbMm Merge(ref DbvtAabbMm a, ref DbvtAabbMm b)
{
DbvtAabbMm res = new DbvtAabbMm();
Merge(ref a, ref b, ref res);
return(res);
}
// volume+edge lengths
public static float Size(ref DbvtAabbMm a)
{
IndexedVector3 edges = a.Lengths();
return(edges.X * edges.Y * edges.Z +
edges.X + edges.Y + edges.Z);
}
public virtual void GetBroadphaseAabb(out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
DbvtAabbMm bounds = new DbvtAabbMm();
if (!m_sets[0].Empty())
{
if (!m_sets[1].Empty())
{
DbvtAabbMm.Merge(ref m_sets[0].m_root.volume, ref m_sets[1].m_root.volume, ref bounds);
}
else
{
bounds = m_sets[0].m_root.volume;
}
}
else if (!m_sets[1].Empty())
{
bounds = m_sets[1].m_root.volume;
}
else
{
IndexedVector3 temp = IndexedVector3.Zero;
bounds = DbvtAabbMm.FromCR(ref temp, 0);
}
aabbMin = bounds.Mins();
aabbMax = bounds.Maxs();
}
//
public static void BottomUp(Dbvt pdbvt, ObjectArray <DbvtNode> leaves)
{
while (leaves.Count > 1)
{
float minsize = float.MaxValue;
int[] minidx = { -1, -1 };
for (int i = 0; i < leaves.Count; ++i)
{
for (int j = i + 1; j < leaves.Count; ++j)
{
DbvtAabbMm mergeResults = DbvtAabbMm.Merge(ref leaves[i].volume, ref leaves[j].volume);
float sz = Size(ref mergeResults);
if (sz < minsize)
{
minsize = sz;
minidx[0] = i;
minidx[1] = j;
}
}
}
DbvtNode[] n = { leaves[minidx[0]], leaves[minidx[1]] };
DbvtNode p = CreateNode(pdbvt, null, ref n[0].volume, ref n[1].volume, null);
p._children[0] = n[0];
p._children[1] = n[1];
n[0].parent = p;
n[1].parent = p;
leaves[minidx[0]] = p;
leaves.Swap(minidx[1], leaves.Count - 1);
leaves.PopBack();
}
}
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 DbvtNode Insert(ref DbvtAabbMm box, Object data)
{
DbvtNode leaf = CreateNode(this, null, ref box, data);
InsertLeaf(this, Root, leaf);
++m_leaves;
return(leaf);
}
public static void Merge(ref DbvtAabbMm a, ref DbvtAabbMm b, ref DbvtAabbMm r)
{
//r = a;
//SetMin(ref r._min, ref b._min);
//SetMax(ref r._max, ref b._max);
MathUtil.VectorMin(ref a._min, ref b._min, out r._min);
MathUtil.VectorMax(ref a._max, ref b._max, out r._max);
}
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 static bool Intersect(DbvtAabbMm a, ref IndexedVector3 b)
{
return((b.X >= a._min.X) &&
(b.Y >= a._min.Y) &&
(b.Z >= a._min.Z) &&
(b.X <= a._max.X) &&
(b.Y <= a._max.Y) &&
(b.Z <= a._max.Z));
}
public static bool Intersect(ref DbvtAabbMm a, ref DbvtAabbMm b)
{
return((a._min.X <= b._max.X) &&
(a._max.X >= b._min.X) &&
(a._min.Y <= b._max.Y) &&
(a._max.Y >= b._min.Y) &&
(a._min.Z <= b._max.Z) &&
(a._max.Z >= b._min.Z));
}
public bool Contain(ref DbvtAabbMm a)
{
return((_min.X <= a._min.X) &&
(_min.Y <= a._min.Y) &&
(_min.Z <= a._min.Z) &&
(_max.X >= a._max.X) &&
(_max.Y >= a._max.Y) &&
(_max.Z >= a._max.Z));
}
public static bool NotEqual(ref DbvtAabbMm a, ref DbvtAabbMm b)
{
return((a._min.X != b._min.X) ||
(a._min.Y != b._min.Y) ||
(a._min.Z != b._min.Z) ||
(a._max.X != b._max.X) ||
(a._max.Y != b._max.Y) ||
(a._max.Z != b._max.Z));
}
public bool Update(DbvtNode leaf, ref DbvtAabbMm volume, ref IndexedVector3 velocity)
{
if (leaf.volume.Contain(ref volume))
{
return(false);
}
volume.SignedExpand(velocity);
Update(leaf, ref volume);
return(true);
}
public bool Update(DbvtNode leaf, ref DbvtAabbMm volume, float margin)
{
if (leaf.volume.Contain(ref volume))
{
return(false);
}
volume.Expand(new IndexedVector3(margin));
Update(leaf, ref volume);
return(true);
}
//
public static DbvtNode CreateNode(Dbvt pdbvt,
DbvtNode parent,
ref DbvtAabbMm volume,
Object data)
{
DbvtNode node = CreateNode(pdbvt, parent, data);
node.volume = volume;
return(node);
}
//
public static DbvtAabbMm Bounds(ObjectArray <DbvtNode> leafs)
{
DbvtAabbMm volume = leafs[0].volume;
for (int i = 1, ni = leafs.Count; i < ni; ++i)
{
DbvtAabbMm.Merge(ref volume, ref leafs[i].volume, ref volume);
}
return(volume);
}
//
public static DbvtNode CreateNode(Dbvt pdbvt,
DbvtNode parent,
ref DbvtAabbMm volume0,
ref DbvtAabbMm volume1,
Object data)
{
DbvtNode node = CreateNode(pdbvt, parent, data);
DbvtAabbMm.Merge(ref volume0, ref volume1, ref node.volume);
return(node);
}
public DbvtNode(Dbvt tree, DbvtNode aparent, ref DbvtAabbMm avolume, Object adata)
: this()
{
volume = avolume;
parent = aparent;
data = adata;
if (data is int)
{
dataAsInt = (int)data;
}
}
public static void InsertLeaf(Dbvt pdbvt, DbvtNode root, DbvtNode leaf)
{
if (pdbvt.Root == null)
{
pdbvt.Root = leaf;
leaf.parent = null;
}
else
{
if (!root.IsLeaf())
{
do
{
root = root._children[DbvtAabbMm.Select(ref leaf.volume,
ref root._children[0].volume,
ref root._children[1].volume)];
} while (!root.IsLeaf());
}
DbvtNode prev = root.parent;
DbvtAabbMm mergeResults = DbvtAabbMm.Merge(ref leaf.volume, ref root.volume);
DbvtNode node = CreateNode2(pdbvt, prev, ref mergeResults, null);
if (prev != null)
{
prev._children[IndexOf(root)] = node;
node._children[0] = root;
root.parent = node;
node._children[1] = leaf;
leaf.parent = node;
do
{
if (!prev.volume.Contain(ref node.volume))
{
DbvtAabbMm.Merge(ref prev._children[0].volume, ref prev._children[1].volume, ref prev.volume);
}
else
{
break;
}
node = prev;
} while (null != (prev = node.parent));
}
else
{
node._children[0] = root;
root.parent = node;
node._children[1] = leaf;
leaf.parent = node;
pdbvt.Root = node;
}
}
}
public static DbvtNode CreateNode2(Dbvt tree, DbvtNode aparent, ref DbvtAabbMm avolume, Object adata)
{
DbvtNode node = BulletGlobals.DbvtNodePool.Get();
node.volume = avolume;
node.parent = aparent;
node.data = adata;
node._children[0] = null;
node._children[1] = null;
if (node.data is int)
{
Debug.Assert(false);
node.dataAsInt = (int)node.data;
}
return(node);
}
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 static DbvtNode RemoveLeaf(Dbvt pdbvt, DbvtNode leaf)
{
if (leaf == pdbvt.Root)
{
pdbvt.Root = null;
return(null);
}
else
{
DbvtNode parent = leaf.parent;
DbvtNode prev = parent.parent;
DbvtNode sibling = parent._children[1 - IndexOf(leaf)];
if (prev != null)
{
prev._children[IndexOf(parent)] = sibling;
sibling.parent = prev;
DeleteNode(pdbvt, parent);
while (prev != null)
{
DbvtAabbMm pb = prev.volume;
DbvtAabbMm.Merge(ref prev._children[0].volume, ref prev._children[1].volume, ref prev.volume);
if (DbvtAabbMm.NotEqual(ref pb, ref prev.volume))
{
sibling = prev;
prev = prev.parent;
}
else
{
break;
}
}
return(prev != null ? prev : pdbvt.Root);
}
else
{
pdbvt.Root = sibling;
sibling.parent = null;
DeleteNode(pdbvt, parent);
return(pdbvt.Root);
}
}
}
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 void Update(DbvtNode leaf, ref DbvtAabbMm volume)
{
DbvtNode root = RemoveLeaf(this, leaf);
if (root != null)
{
if (m_lkhd >= 0)
{
for (int i = 0; (i < m_lkhd) && (root.parent != null); ++i)
{
root = root.parent;
}
}
else
{
root = Root;
}
}
leaf.volume = volume;
InsertLeaf(this, root, leaf);
}
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 static DbvtNode TopDown(Dbvt pdbvt, ObjectArray <DbvtNode> leaves, int bu_treshold)
{
if (leaves.Count > 1)
{
if (leaves.Count > bu_treshold)
{
DbvtAabbMm vol = Bounds(leaves);
IndexedVector3 org = vol.Center();
ObjectArray <DbvtNode>[] sets = { new ObjectArray <DbvtNode>(), new ObjectArray <DbvtNode>() };
int bestaxis = -1;
int bestmidp = leaves.Count;
int[] a1 = new int[] { 0, 0 };
int[] a2 = new int[] { 0, 0 };
int[] a3 = new int[] { 0, 0 };
int[][] splitcount = new int[][] { a1, a2, a3 };
int i;
for (i = 0; i < leaves.Count; ++i)
{
IndexedVector3 x = leaves[i].volume.Center() - org;
for (int j = 0; j < 3; ++j)
{
++splitcount[j][IndexedVector3.Dot(x, axis[j]) > 0 ? 1 : 0];
}
}
for (i = 0; i < 3; ++i)
{
if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
{
int midp = (int)Math.Abs((splitcount[i][0] - splitcount[i][1]));
if (midp < bestmidp)
{
bestaxis = i;
bestmidp = midp;
}
}
}
if (bestaxis >= 0)
{
sets[0].EnsureCapacity(splitcount[bestaxis][0]);
sets[1].EnsureCapacity(splitcount[bestaxis][1]);
Split(leaves, sets[0], sets[1], ref org, ref axis[bestaxis]);
}
else
{
sets[0].EnsureCapacity(leaves.Count / 2 + 1);
sets[1].EnsureCapacity(leaves.Count / 2);
for (int i2 = 0, ni = leaves.Count; i2 < ni; ++i2)
{
sets[i2 & 1].Add(leaves[i2]);
}
}
DbvtNode node = CreateNode(pdbvt, null, ref vol, null);
node._children[0] = TopDown(pdbvt, sets[0], bu_treshold);
node._children[1] = TopDown(pdbvt, sets[1], bu_treshold);
node._children[0].parent = node;
node._children[1].parent = node;
return(node);
}
else
{
BottomUp(pdbvt, leaves);
return(leaves[0]);
}
}
return(leaves[0]);
}
public static void CollideTTpersistentStack(DbvtNode root0,
DbvtNode root1,
ICollide collideable)
{
//CollideTT(root0, root1, collideable);
//return;
if (root0 != null && root1 != null)
{
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 4;
m_stkStack.Resize(DOUBLE_STACKSIZE);
m_stkStack[0] = new sStkNN(root0, root1);
do
{
sStkNN p = m_stkStack[--depth];
if (depth > treshold)
{
m_stkStack.Resize(m_stkStack.Count * 2);
treshold = m_stkStack.Count - 4;
}
if (p.a == p.b)
{
if (p.a.IsInternal())
{
m_stkStack[depth++] = new sStkNN(p.a._children[0], p.a._children[0]);
m_stkStack[depth++] = new sStkNN(p.a._children[1], p.a._children[1]);
m_stkStack[depth++] = new sStkNN(p.a._children[0], p.a._children[1]);
}
}
else if (DbvtAabbMm.Intersect(ref p.a.volume, ref p.b.volume))
{
if (p.a.IsInternal())
{
if (p.b.IsInternal())
{
m_stkStack[depth++] = new sStkNN(p.a._children[0], p.b._children[0]);
m_stkStack[depth++] = new sStkNN(p.a._children[1], p.b._children[0]);
m_stkStack[depth++] = new sStkNN(p.a._children[0], p.b._children[1]);
m_stkStack[depth++] = new sStkNN(p.a._children[1], p.b._children[1]);
}
else
{
m_stkStack[depth++] = new sStkNN(p.a._children[0], p.b);
m_stkStack[depth++] = new sStkNN(p.a._children[1], p.b);
}
}
else
{
if (p.b.IsInternal())
{
m_stkStack[depth++] = new sStkNN(p.a, p.b._children[0]);
m_stkStack[depth++] = new sStkNN(p.a, p.b._children[1]);
}
else
{
collideable.Process(p.a, p.b);
}
}
}
} while (depth > 0);
}
}