public void collideTV(Node root, Dispatcher dispatcher, DbvtAabbMm volume, short collisionFilterGroup, short collisionFilterMask, ICollide policy)
{
if (root != null)
{
List <Node> stack = new List <Node>(SIMPLE_STACKSIZE);
stack.Add(root);
do
{
Node n = stack[stack.Count - 1];
stack.RemoveAt(stack.Count - 1);
if (DbvtAabbMm.Intersect(n.volume, volume))
{
if (n.isinternal())
{
stack.Add(n.childs[0]);
stack.Add(n.childs[1]);
}
else
{
if (!dispatcher.needsCollision(collisionFilterGroup, collisionFilterMask, n.data.collisionFilterGroup, n.data.collisionFilterMask))
{
continue;
}
policy.Process(n);
}
}
} while (stack.Count > 0);
}
}
public void rayTestInternal(Node root, Dispatcher dispatcher, VInt3 rayFrom, VInt3 rayTo, VInt3 aabbMin, VInt3 aabbMax, short collisionFilterGroup, short collisionFilterMask, ICollide policy)
{
if (root != null)
{
List <Node> stack = new List <Node>(DOUBLE_STACKSIZE);
stack.Add(root);
VInt3[] bounds = new VInt3[2];
do
{
Node node = stack[stack.Count - 1];
stack.RemoveAt(stack.Count - 1);
VFixedPoint tmin = VFixedPoint.One, tmax = VFixedPoint.One;
bool result1 = AabbUtils.RayAabb2(rayFrom, rayTo, node.volume.Mins() - aabbMax, node.volume.Maxs() - aabbMin, ref tmin, ref tmax);
if (result1)
{
if (node.isinternal())
{
stack.Add(node.childs[0]);
stack.Add(node.childs[1]);
}
else
{
if (!dispatcher.needsCollision(collisionFilterGroup, collisionFilterMask, node.data.collisionFilterGroup, node.data.collisionFilterMask))
{
continue;
}
policy.Process(node);
}
}
} while (stack.Count > 0);
}
}
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 void EnumNodes(DbvtNode root, ICollide collideable)
{
collideable.Process(root);
if (root.IsInternal())
{
EnumNodes(root._children[0], collideable);
EnumNodes(root._children[1], collideable);
}
}
public static void collideTT(Node root0, Node root1, Dispatcher dispatcher, ICollide policy)
{
if (root0 != null && root1 != null)
{
List <sStkNN> stack = new List <sStkNN>(DOUBLE_STACKSIZE);
stack.Add(new sStkNN(root0, root1));
do
{
sStkNN p = stack[stack.Count - 1];
stack.RemoveAt(stack.Count - 1);
if (p.a == p.b)
{
if (p.a.isinternal())
{
stack.Add(new sStkNN(p.a.childs[0], p.a.childs[0]));
stack.Add(new sStkNN(p.a.childs[1], p.a.childs[1]));
stack.Add(new sStkNN(p.a.childs[0], p.a.childs[1]));
}
}
else if (DbvtAabbMm.Intersect(p.a.volume, p.b.volume))
{
if (p.a.isinternal())
{
if (p.b.isinternal())
{
stack.Add(new sStkNN(p.a.childs[0], p.b.childs[0]));
stack.Add(new sStkNN(p.a.childs[1], p.b.childs[0]));
stack.Add(new sStkNN(p.a.childs[0], p.b.childs[1]));
stack.Add(new sStkNN(p.a.childs[1], p.b.childs[1]));
}
else
{
stack.Add(new sStkNN(p.a.childs[0], p.b));
stack.Add(new sStkNN(p.a.childs[1], p.b));
}
}
else
{
if (p.b.isinternal())
{
stack.Add(new sStkNN(p.a, p.b.childs[0]));
stack.Add(new sStkNN(p.a, p.b.childs[1]));
}
else
{
if (!dispatcher.needsCollision(p.a.data.collisionFilterGroup, p.a.data.collisionFilterMask, p.b.data.collisionFilterGroup, p.b.data.collisionFilterMask))
{
continue;
}
policy.Process(p.a, p.b);
}
}
}
}while (stack.Count > 0);
}
}
public void RayTestInternal(DbvtNode root,
ref IndexedVector3 rayFrom,
ref IndexedVector3 rayTo,
ref IndexedVector3 rayDirectionInverse,
bool[] signs,
float lambda_max,
ref IndexedVector3 aabbMin,
ref IndexedVector3 aabbMax,
ICollide policy)
{
using (DbvtStackDataBlock stackDataBlock = BulletGlobals.DbvtStackDataBlockPool.Get())
{
// (void) rayTo;
//DBVT_CHECKTYPE
if (root != null)
{
IndexedVector3 resultNormal = new IndexedVector3(0, 1, 0);
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack[0] = root;
do
{
DbvtNode node = stackDataBlock.stack[--depth];
stackDataBlock.bounds[0] = node.volume.Mins() - aabbMax;
stackDataBlock.bounds[1] = node.volume.Maxs() - aabbMin;
float tmin = 1.0f, lambda_min = 0.0f;
bool result1 = AabbUtil2.RayAabb2(ref rayFrom, ref rayDirectionInverse, signs, stackDataBlock.bounds, out tmin, lambda_min, lambda_max);
if (result1)
{
if (node.IsInternal())
{
if (depth > treshold)
{
stackDataBlock.stack.Resize(stackDataBlock.stack.Count * 2);
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children[0];
stackDataBlock.stack[depth++] = node._children[1];
}
else
{
policy.Process(node);
}
}
} while (depth != 0);
}
}
}
public static void CollideTT(DbvtNode root0, DbvtNode root1, ICollide collideable)
{
CollideTTCount++;
Debug.Assert(CollideTTCount < 2);
CollideTTStack.Clear();
if (root0 != null && root1 != null)
{
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 4;
CollideTTStack[0] = new sStkNN(root0, root1);
do
{
sStkNN p = CollideTTStack[--depth];
if (depth > treshold)
{
CollideTTStack.Resize(CollideTTStack.Count * 2);
treshold = CollideTTStack.Count - 4;
}
if (p.a == p.b)
{
if (p.a.IsInternal())
{
CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.a._children[0]);
CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.a._children[1]);
CollideTTStack[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())
{
CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.b._children[0]);
CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.b._children[0]);
CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.b._children[1]);
CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.b._children[1]);
}
else
{
CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.b);
CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.b);
}
}
else
{
if (p.b.IsInternal())
{
CollideTTStack[depth++] = new sStkNN(p.a, p.b._children[0]);
CollideTTStack[depth++] = new sStkNN(p.a, p.b._children[1]);
}
else
{
collideable.Process(p.a, p.b);
}
}
}
} while (depth > 0);
}
CollideTTCount--;
}
public static void EnumNodes(DbvtNode root, ICollide collideable)
{
collideable.Process(root);
if (root.IsInternal())
{
EnumNodes(root._children[0], collideable);
EnumNodes(root._children[1], collideable);
}
}
public static void rayTest( btDbvtNode root,
ref btVector3 rayFrom,
ref btVector3 rayTo,
ICollide policy )
{
using( btDbvtStackDataBlock stackDataBlock = new btDbvtStackDataBlock() )
{
if( root != null )
{
btVector3 rayDir = ( rayTo - rayFrom );
rayDir.normalize();
///what about division by zero? -. just set rayDirection[i] to INF/BT_LARGE_FLOAT
btVector3 rayDirectionInverse = new btVector3(
rayDir.x == 0.0f ? btScalar.BT_LARGE_FLOAT : 1.0f / rayDir.x,
rayDir.y == 0.0f ? btScalar.BT_LARGE_FLOAT : 1.0f / rayDir.y,
rayDir.z == 0.0f ? btScalar.BT_LARGE_FLOAT : 1.0f / rayDir.z );
stackDataBlock.signs[0] = rayDirectionInverse.x < 0.0f ? (uint)1 :0;
stackDataBlock.signs[1] = rayDirectionInverse.y < 0.0f ? (uint)1 :0;
stackDataBlock.signs[2] = rayDirectionInverse.z < 0.0f ? (uint)1 :0;
btVector3 tmp; rayTo.Sub( ref rayFrom, out tmp );
double lambda_max = btVector3.dot( ref rayDir, ref tmp );
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack.Count = ( DOUBLE_STACKSIZE );
stackDataBlock.stack[0] = root;
do
{
btDbvtNode node = stackDataBlock.stack[--depth];
stackDataBlock.bounds0 = node.volume._min;
stackDataBlock.bounds1 = node.volume._max;
double tmin = 1.0f, lambda_min = 0.0f;
bool result1 = btAabbUtil.btRayAabb2( ref rayFrom, ref rayDirectionInverse, stackDataBlock.signs
, ref stackDataBlock.bounds0
, ref stackDataBlock.bounds1
, out tmin, lambda_min, lambda_max );
#if COMPARE_BTRAY_AABB2
double param=1.0f;
bool result2 = AabbUtil.RayAabb(ref rayFrom,ref rayTo,node.volume.Mins(),node.volume.Maxs(),param,resultNormal);
Debug.Assert(result1 == result2);
#endif //TEST_BTRAY_AABB2
if( result1 )
{
if( node.IsInternal() )
{
if( depth > treshold )
{
stackDataBlock.stack.Count = ( stackDataBlock.stack.Count * 2 );
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children0;
stackDataBlock.stack[depth++] = node._children1;
}
else
{
policy.Process( node );
}
}
} while( depth != 0 );
}
}
}
public static void CollideTTpersistentStack( btDbvtNode root0,
btDbvtNode root1,
ICollide collideable )
{
//CollideTT(root0, root1, collideable);
//return;
if( root0 != null && root1 != null )
{
int depth = 1;
int treshold = m_stkStack.Length - 4;
//m_stkStack.Capacity = DOUBLE_STACKSIZE;
m_stkStack[0].Initialize( root0, root1 );
do
{
sStkNN p = m_stkStack[--depth];
if( depth > treshold )
{
sStkNN[] newArray = new sStkNN[m_stkStack.Length * 2];
for( int n = 0; n < depth; n++ )
newArray[n] = m_stkStack[n];
m_stkStack = newArray;
treshold = newArray.Length - 4;
}
if( p.a == p.b )
{
if( p.a.IsInternal() )
{
m_stkStack[depth++].Initialize( p.a._children0, p.a._children0 );
m_stkStack[depth++].Initialize( p.a._children1, p.a._children1 );
m_stkStack[depth++].Initialize( p.a._children0, p.a._children1 );
}
}
else if( btDbvtVolume.Intersect( ref p.a.volume, ref p.b.volume ) )
{
if( p.a.IsInternal() )
{
if( p.b.IsInternal() )
{
m_stkStack[depth++].Initialize( p.a._children0, p.b._children0 );
m_stkStack[depth++].Initialize( p.a._children1, p.b._children0 );
m_stkStack[depth++].Initialize( p.a._children0, p.b._children1 );
m_stkStack[depth++].Initialize( p.a._children1, p.b._children1 );
}
else
{
m_stkStack[depth++].Initialize( p.a._children0, p.b );
m_stkStack[depth++].Initialize( p.a._children1, p.b );
}
}
else
{
if( p.b.IsInternal() )
{
m_stkStack[depth++].Initialize( p.a, p.b._children0 );
m_stkStack[depth++].Initialize( p.a, p.b._children1 );
}
else
{
collideable.Process( p.a, p.b );
}
}
}
} while( depth > 0 );
}
}
public static void CollideTT( btDbvtNode root0, btDbvtNode root1, ICollide collideable )
{
CollideTTCount++;
Debug.Assert( CollideTTCount < 2 );
CollideTTStack.Clear();
if( root0 != null && root1 != null )
{
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 4;
CollideTTStack[0].Initialize( root0, root1 );
do
{
sStkNN p = CollideTTStack[--depth];
if( depth > treshold )
{
CollideTTStack.Count = ( CollideTTStack.Count * 2 );
treshold = CollideTTStack.Count - 4;
}
if( p.a == p.b )
{
if( p.a.IsInternal() )
{
CollideTTStack[depth++].Initialize( p.a._children0, p.a._children0 );
CollideTTStack[depth++].Initialize( p.a._children1, p.a._children1 );
CollideTTStack[depth++].Initialize( p.a._children0, p.a._children1 );
}
}
else if( btDbvtVolume.Intersect( ref p.a.volume, ref p.b.volume ) )
{
if( p.a.IsInternal() )
{
if( p.b.IsInternal() )
{
CollideTTStack[depth++].Initialize( p.a._children0, p.b._children0 );
CollideTTStack[depth++].Initialize( p.a._children1, p.b._children0 );
CollideTTStack[depth++].Initialize( p.a._children0, p.b._children1 );
CollideTTStack[depth++].Initialize( p.a._children1, p.b._children1 );
}
else
{
CollideTTStack[depth++].Initialize( p.a._children0, p.b );
CollideTTStack[depth++].Initialize( p.a._children1, p.b );
}
}
else
{
if( p.b.IsInternal() )
{
CollideTTStack[depth++].Initialize( p.a, p.b._children0 );
CollideTTStack[depth++].Initialize( p.a, p.b._children1 );
}
else
{
collideable.Process( p.a, p.b );
}
}
}
} while( depth > 0 );
}
CollideTTCount--;
}
public static void EnumLeaves( btDbvtNode root, ICollide collideable )
{
if( root.IsInternal() )
{
EnumLeaves( root._children0, collideable );
EnumLeaves( root._children1, collideable );
}
else
{
collideable.Process( root );
}
}
public void RayTestInternal(DbvtNode root,
ref IndexedVector3 rayFrom,
ref IndexedVector3 rayTo,
ref IndexedVector3 rayDirectionInverse,
bool[] signs,
float lambda_max,
ref IndexedVector3 aabbMin,
ref IndexedVector3 aabbMax,
ICollide policy)
{
using (DbvtStackDataBlock stackDataBlock = BulletGlobals.DbvtStackDataBlockPool.Get())
{
// (void) rayTo;
//DBVT_CHECKTYPE
if (root != null)
{
IndexedVector3 resultNormal = new IndexedVector3(0, 1, 0);
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack[0] = root;
do
{
DbvtNode node = stackDataBlock.stack[--depth];
stackDataBlock.bounds[0] = node.volume.Mins() - aabbMax;
stackDataBlock.bounds[1] = node.volume.Maxs() - aabbMin;
float tmin = 1.0f, lambda_min = 0.0f;
bool result1 = AabbUtil2.RayAabb2(ref rayFrom, ref rayDirectionInverse, signs, stackDataBlock.bounds, out tmin, lambda_min, lambda_max);
if (result1)
{
if (node.IsInternal())
{
if (depth > treshold)
{
stackDataBlock.stack.Resize(stackDataBlock.stack.Count * 2);
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children[0];
stackDataBlock.stack[depth++] = node._children[1];
}
else
{
policy.Process(node);
}
}
} while (depth != 0);
}
}
}
public static void RayTest(DbvtNode root,
ref IndexedVector3 rayFrom,
ref IndexedVector3 rayTo,
ICollide policy)
{
using (DbvtStackDataBlock stackDataBlock = BulletGlobals.DbvtStackDataBlockPool.Get())
{
if (root != null)
{
IndexedVector3 rayDir = (rayTo - rayFrom);
rayDir.Normalize();
///what about division by zero? -. just set rayDirection[i] to INF/BT_LARGE_FLOAT
IndexedVector3 rayDirectionInverse = new IndexedVector3(
rayDir.X == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.X,
rayDir.Y == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.Y,
rayDir.Z == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.Z);
stackDataBlock.signs[0] = rayDirectionInverse.X < 0.0f;
stackDataBlock.signs[1] = rayDirectionInverse.Y < 0.0f;
stackDataBlock.signs[2] = rayDirectionInverse.Z < 0.0f;
float lambda_max = IndexedVector3.Dot(rayDir, (rayTo - rayFrom));
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack.Resize(DOUBLE_STACKSIZE);
stackDataBlock.stack[0] = root;
do
{
DbvtNode node = stackDataBlock.stack[--depth];
stackDataBlock.bounds[0] = node.volume.Mins();
stackDataBlock.bounds[1] = node.volume.Maxs();
float tmin = 1.0f, lambda_min = 0.0f;
bool result1 = AabbUtil2.RayAabb2(ref rayFrom, ref rayDirectionInverse, stackDataBlock.signs, stackDataBlock.bounds, out tmin, lambda_min, lambda_max);
#if COMPARE_BTRAY_AABB2
float param=1.0f;
bool result2 = AabbUtil.RayAabb(ref rayFrom,ref rayTo,node.volume.Mins(),node.volume.Maxs(),param,resultNormal);
Debug.Assert(result1 == result2);
#endif //TEST_BTRAY_AABB2
if (result1)
{
if (node.IsInternal())
{
if (depth > treshold)
{
stackDataBlock.stack.Resize(stackDataBlock.stack.Count * 2);
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children[0];
stackDataBlock.stack[depth++] = node._children[1];
}
else
{
policy.Process(node);
}
}
} while (depth != 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);
}
}
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);
}
}
public static void RayTest(DbvtNode root,
ref IndexedVector3 rayFrom,
ref IndexedVector3 rayTo,
ICollide policy)
{
using (DbvtStackDataBlock stackDataBlock = BulletGlobals.DbvtStackDataBlockPool.Get())
{
if (root != null)
{
IndexedVector3 rayDir = (rayTo - rayFrom);
rayDir.Normalize();
///what about division by zero? -. just set rayDirection[i] to INF/BT_LARGE_FLOAT
IndexedVector3 rayDirectionInverse = new IndexedVector3(
rayDir.X == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.X,
rayDir.Y == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.Y,
rayDir.Z == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.Z);
stackDataBlock.signs[0] = rayDirectionInverse.X < 0.0f;
stackDataBlock.signs[1] = rayDirectionInverse.Y < 0.0f;
stackDataBlock.signs[2] = rayDirectionInverse.Z < 0.0f;
float lambda_max = IndexedVector3.Dot(rayDir, (rayTo - rayFrom));
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack.Resize(DOUBLE_STACKSIZE);
stackDataBlock.stack[0] = root;
do
{
DbvtNode node = stackDataBlock.stack[--depth];
stackDataBlock.bounds[0] = node.volume.Mins();
stackDataBlock.bounds[1] = node.volume.Maxs();
float tmin = 1.0f, lambda_min = 0.0f;
bool result1 = AabbUtil2.RayAabb2(ref rayFrom, ref rayDirectionInverse, stackDataBlock.signs, stackDataBlock.bounds, out tmin, lambda_min, lambda_max);
#if COMPARE_BTRAY_AABB2
float param = 1.0f;
bool result2 = AabbUtil.RayAabb(ref rayFrom, ref rayTo, node.volume.Mins(), node.volume.Maxs(), param, resultNormal);
Debug.Assert(result1 == result2);
#endif //TEST_BTRAY_AABB2
if (result1)
{
if (node.IsInternal())
{
if (depth > treshold)
{
stackDataBlock.stack.Resize(stackDataBlock.stack.Count * 2);
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children[0];
stackDataBlock.stack[depth++] = node._children[1];
}
else
{
policy.Process(node);
}
}
} while (depth != 0);
}
}
}
public static void rayTestInternal( btDbvtNode root,
ref btVector3 rayFrom,
ref btVector3 rayTo,
ref btVector3 rayDirectionInverse,
uint[] signs,
double lambda_max,
ref btVector3 aabbMin,
ref btVector3 aabbMax,
ICollide policy )
{
using( btDbvtStackDataBlock stackDataBlock = BulletGlobals.DbvtStackDataBlockPool.Get() )
{
// (void) rayTo;
//DBVT_CHECKTYPE
if( root != null )
{
btVector3 resultNormal = new btVector3( 0, 1, 0 );
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack[0] = root;
do
{
btDbvtNode node = stackDataBlock.stack[--depth];
node.volume._min.Sub( ref aabbMax, out stackDataBlock.bounds0 );
node.volume._max.Sub( ref aabbMin, out stackDataBlock.bounds1 );
double tmin = 1.0f, lambda_min = 0.0f;
bool result1 = btAabbUtil.btRayAabb2( ref rayFrom, ref rayDirectionInverse, signs
, ref stackDataBlock.bounds0
, ref stackDataBlock.bounds1
, out tmin, lambda_min, lambda_max );
if( result1 )
{
if( node.IsInternal() )
{
if( depth > treshold )
{
stackDataBlock.stack.Count = ( stackDataBlock.stack.Count * 2 );
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children0;
stackDataBlock.stack[depth++] = node._children1;
}
else
{
policy.Process( node );
}
}
} while( depth != 0 );
}
}
}
public static void CollideTV( btDbvtNode root, ref btDbvtVolume volume, ICollide collideable )
{
CollideTVCount++;
Debug.Assert( CollideTVCount < 2 );
CollideTVStack.Clear();
if( root != null )
{
CollideTVStack.Push( root );
do
{
btDbvtNode n = CollideTVStack.Pop();
if( btDbvtVolume.Intersect( ref n.volume, ref volume ) )
{
if( n.IsInternal() )
{
CollideTVStack.Push( n._children0 );
CollideTVStack.Push( n._children1 );
}
else
{
collideable.Process( n );
}
}
} while( CollideTVStack.Count > 0 );
}
CollideTVCount--;
}
public static void CollideTV(DbvtNode root, ref DbvtAabbMm volume, ICollide collideable, Stack<DbvtNode> collideTVStack, ref int collideTVCount)
{
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--;
}
