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