//int m_padding[3];
public void SetAabbFromQuantizeNode(QuantizedBvhNode quantizedNode)
{
m_quantizedAabbMin = quantizedNode.m_quantizedAabbMin;
m_quantizedAabbMax = quantizedNode.m_quantizedAabbMax;
}
public void Quantize(ref UShortVector3 result, ref Vector3 point, bool isMax)
{
Debug.Assert(m_useQuantization);
Debug.Assert(point.X <= m_bvhAabbMax.X);
Debug.Assert(point.Y <= m_bvhAabbMax.Y);
Debug.Assert(point.Z <= m_bvhAabbMax.Z);
Debug.Assert(point.X >= m_bvhAabbMin.X);
Debug.Assert(point.Y >= m_bvhAabbMin.Y);
Debug.Assert(point.Z >= m_bvhAabbMin.Z);
Vector3 v = (point - m_bvhAabbMin) * m_bvhQuantization;
///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative
///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly)
///@todo: double-check this
if (isMax)
{
result.X = (ushort)(((ushort)(v.X + 1f) | 1));
result.Y = (ushort)(((ushort)(v.Y + 1f) | 1));
result.Z = (ushort)(((ushort)(v.Z + 1f) | 1));
}
else
{
result.X = (ushort)(((ushort)(v.X) & 0xfffe));
result.Y = (ushort)(((ushort)(v.Y) & 0xfffe));
result.Z = (ushort)(((ushort)(v.Z) & 0xfffe));
}
#if DEBUG_CHECK_DEQUANTIZATION
Vector3 newPoint = UnQuantize(result);
if (isMax)
{
if (newPoint.X < point.X)
{
System.Console.WriteLine("unconservative X, diffX = {0}, oldX={1},newX={2}\n", newPoint.X - point.X, newPoint.X, point.X);
}
if (newPoint.Y < point.Y)
{
System.Console.WriteLine("unconservative Y, diffY = {0}, oldY={1},newY={2}\n", newPoint.Y - point.Y, newPoint.Y, point.Y);
}
if (newPoint.Z < point.Z)
{
System.Console.WriteLine("unconservative Z, diffZ = {0}, oldZ={1},newZ={2}\n", newPoint.Z - point.Z, newPoint.Z, point.Z);
}
}
else
{
if (newPoint.X > point.X)
{
System.Console.WriteLine("unconservative X, diffX = {0}, oldX={1},newX={2}\n", newPoint.X - point.X, newPoint.X, point.X);
}
if (newPoint.Y > point.Y)
{
System.Console.WriteLine("unconservative Y, diffY = {0}, oldY={1},newY={2}\n", newPoint.Y - point.Y, newPoint.Y, point.Y);
}
if (newPoint.Z > point.Z)
{
System.Console.WriteLine("unconservative Z, diffZ = {0}, oldZ={1},newZ={2}\n", newPoint.Z - point.Z, newPoint.Z, point.Z);
}
}
#endif //DEBUG_CHECK_DEQUANTIZATION
}
//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
public static bool TestQuantizedAabbAgainstQuantizedAabb(ref UShortVector3 aabbMin1, ref UShortVector3 aabbMax1, ref UShortVector3 aabbMin2, ref UShortVector3 aabbMax2)
{
//return ((aabbMin1[0] <= aabbMax2[0]) && (aabbMax1[0] >= aabbMin2[0])
// & (aabbMin1[2] <= aabbMax2[2]) && (aabbMax1[2] >= aabbMin2[2])
// & (aabbMin1[1] <= aabbMax2[1]) && (aabbMax1[1] >= aabbMin2[1]));
//return (MathUtil.select(val,1, 0));
// MAN - Not sure why this version isn't just replaced by anding all of the above, it's still not conditional as theres a quick ref.
bool overlap = true;
overlap = (aabbMin1.X > aabbMax2.X || aabbMax1.X < aabbMin2.X) ? false : overlap;
overlap = (aabbMin1.Z > aabbMax2.Z || aabbMax1.Z < aabbMin2.Z) ? false : overlap;
overlap = (aabbMin1.Y > aabbMax2.Y || aabbMax1.Y < aabbMin2.Y) ? false : overlap;
return overlap;
}
///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
protected void WalkRecursiveQuantizedTreeAgainstQueryAabb(ref QuantizedBvhNode currentNode, INodeOverlapCallback nodeCallback, ref UShortVector3 quantizedQueryAabbMin, ref UShortVector3 quantizedQueryAabbMax)
{
Debug.Assert(m_useQuantization);
bool isLeafNode;
//PCK: unsigned instead of bool
bool aabbOverlap = false;
//PCK: unsigned instead of bool
aabbOverlap = AabbUtil2.TestQuantizedAabbAgainstQuantizedAabb(ref quantizedQueryAabbMin, ref quantizedQueryAabbMax, ref currentNode.m_quantizedAabbMin, ref currentNode.m_quantizedAabbMax);
isLeafNode = currentNode.IsLeafNode();
//PCK: unsigned instead of bool
if (aabbOverlap)
{
if (isLeafNode)
{
nodeCallback.ProcessNode(currentNode.GetPartId(), currentNode.GetTriangleIndex());
}
else
{
//process left and right children
//QuantizedBvhNode leftChildNode = currentNode + 1;
// Not sure bout thie replacement... but avoids pointer arithmetic
// this is broken ...
int nodeIndex = currentNode.GetTriangleIndex() + 1;
QuantizedBvhNode leftChildNode = m_quantizedContiguousNodes[nodeIndex];
WalkRecursiveQuantizedTreeAgainstQueryAabb(ref leftChildNode, nodeCallback, ref quantizedQueryAabbMin, ref quantizedQueryAabbMax);
int newIndex = leftChildNode.IsLeafNode() ? leftChildNode.GetTriangleIndex() + 1 : leftChildNode.GetTriangleIndex() + leftChildNode.GetEscapeIndex();
QuantizedBvhNode rightChildNode = m_quantizedContiguousNodes[newIndex];
WalkRecursiveQuantizedTreeAgainstQueryAabb(ref rightChildNode, nodeCallback, ref quantizedQueryAabbMin, ref quantizedQueryAabbMax);
}
}
}
///***************************************** expert/internal use only *************************
public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, ref Vector3 aabbMin, ref Vector3 aabbMax)
{
//either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
if (m_useQuantization)
{
///quantize query AABB
UShortVector3 quantizedQueryAabbMin = new UShortVector3();
UShortVector3 quantizedQueryAabbMax = new UShortVector3();
QuantizeWithClamp(ref quantizedQueryAabbMin, ref aabbMin, false);
QuantizeWithClamp(ref quantizedQueryAabbMax, ref aabbMax, true);
switch (m_traversalMode)
{
case TraversalMode.TRAVERSAL_STACKLESS:
if (m_useQuantization)
{
WalkStacklessQuantizedTree(nodeCallback, ref quantizedQueryAabbMin, ref quantizedQueryAabbMax, 0, m_curNodeIndex);
}
else
{
WalkStacklessTree(nodeCallback, ref aabbMin, ref aabbMax);
}
break;
case TraversalMode.TRAVERSAL_STACKLESS_CACHE_FRIENDLY:
WalkStacklessQuantizedTreeCacheFriendly(nodeCallback, ref quantizedQueryAabbMin, ref quantizedQueryAabbMax);
break;
case TraversalMode.TRAVERSAL_RECURSIVE:
{
QuantizedBvhNode rootNode = m_quantizedContiguousNodes[0];
WalkRecursiveQuantizedTreeAgainstQueryAabb(ref rootNode, nodeCallback, ref quantizedQueryAabbMin, ref quantizedQueryAabbMax);
}
break;
default:
//unsupported
Debug.Assert(false);
break;
}
}
else
{
WalkStacklessTree(nodeCallback, ref aabbMin, ref aabbMax);
}
}
protected void WalkStacklessQuantizedTree(INodeOverlapCallback nodeCallback, ref UShortVector3 quantizedQueryAabbMin, ref UShortVector3 quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex)
{
Debug.Assert(m_useQuantization);
int curIndex = startNodeIndex;
int walkIterations = 0;
int subTreeSize = endNodeIndex - startNodeIndex;
//(void)subTreeSize;
QuantizedBvhNode rootNode = m_quantizedContiguousNodes[curIndex];
int escapeIndex = 0;
bool isLeafNode;
//PCK: unsigned instead of bool
bool aabbOverlap = false;
while (curIndex < endNodeIndex)
{
//#define VISUALLY_ANALYZE_BVH 1
#if VISUALLY_ANALYZE_BVH
//some code snippet to debugDraw aabb, to visually analyze bvh structure
int drawPatch = 9;
//need some global access to a debugDrawer
IDebugDraw debugDrawerPtr = BulletGlobals.gDebugDraw;
//IDebugDraw debugDrawerPtr = null;
//if (curIndex == drawPatch&& debugDrawerPtr != null)
if (debugDrawerPtr != null && curIndex == drawPatch)
//if (debugDrawerPtr != null)
{
Vector3 aabbMin,aabbMax;
aabbMin = UnQuantize(ref rootNode.m_quantizedAabbMin);
aabbMax = UnQuantize(ref rootNode.m_quantizedAabbMax);
Vector3 color = new Vector3(1,0,0);
debugDrawerPtr.DrawAabb(ref aabbMin,ref aabbMax,ref color);
//Console.Out.WriteLine(String.Format("min[{0},{1},{2}] max[{3},{4},{5}]\n", aabbMin.X, aabbMin.Y, aabbMin.Z, aabbMax.X, aabbMax.Y, aabbMax.Z));
}
#endif//VISUALLY_ANALYZE_BVH
//unQuantize version with out param
//catch bugs in tree data
Debug.Assert(walkIterations < subTreeSize);
walkIterations++;
//PCK: unsigned instead of bool
aabbOverlap = AabbUtil2.TestQuantizedAabbAgainstQuantizedAabb(ref quantizedQueryAabbMin, ref quantizedQueryAabbMax, ref rootNode.m_quantizedAabbMin, ref rootNode.m_quantizedAabbMax);
isLeafNode = rootNode.IsLeafNode();
if (isLeafNode && aabbOverlap)
{
nodeCallback.ProcessNode(rootNode.GetPartId(), rootNode.GetTriangleIndex());
}
//PCK: unsigned instead of bool
if ((aabbOverlap) || isLeafNode)
{
curIndex++;
}
else
{
escapeIndex = rootNode.GetEscapeIndex();
curIndex += escapeIndex;
}
rootNode = m_quantizedContiguousNodes[curIndex];
}
if (m_maxIterations < walkIterations)
{
m_maxIterations = walkIterations;
}
}
///tree traversal designed for small-memory processors like PS3 SPU
protected void WalkStacklessQuantizedTreeCacheFriendly(INodeOverlapCallback nodeCallback, ref UShortVector3 quantizedQueryAabbMin, ref UShortVector3 quantizedQueryAabbMax)
{
Debug.Assert(m_useQuantization);
for (int i = 0; i < m_SubtreeHeaders.Count; i++)
{
BvhSubtreeInfo subtree = m_SubtreeHeaders[i];
//PCK: unsigned instead of bool
bool overlap = AabbUtil2.TestQuantizedAabbAgainstQuantizedAabb(ref quantizedQueryAabbMin, ref quantizedQueryAabbMax, ref subtree.m_quantizedAabbMin, ref subtree.m_quantizedAabbMax);
if (overlap)
{
WalkStacklessQuantizedTree(nodeCallback, ref quantizedQueryAabbMin, ref quantizedQueryAabbMax,
subtree.m_rootNodeIndex,
subtree.m_rootNodeIndex + subtree.m_subtreeSize);
}
}
}
protected void WalkStacklessQuantizedTreeAgainstRay(INodeOverlapCallback nodeCallback, ref Vector3 raySource, ref Vector3 rayTarget, ref Vector3 aabbMin, ref Vector3 aabbMax, int startNodeIndex, int endNodeIndex)
{
Debug.Assert(m_useQuantization);
int curIndex = startNodeIndex;
int walkIterations = 0;
int subTreeSize = endNodeIndex - startNodeIndex;
//(void)subTreeSize;
QuantizedBvhNode rootNode = m_quantizedContiguousNodes[curIndex];
int escapeIndex = 0;
bool isLeafNode = false;
//PCK: unsigned instead of bool
bool boxBoxOverlap = false;
bool rayBoxOverlap = false;
float lambda_max = 1.0f;
#if RAYAABB2
Vector3 rayDirection = (rayTarget - raySource);
rayDirection.Normalize();
lambda_max = Vector3.Dot(rayDirection, rayTarget - raySource);
///what about division by zero? --> just set rayDirection[i] to 1.0
rayDirection.X = MathUtil.FuzzyZero(rayDirection.X) ? MathUtil.BT_LARGE_FLOAT : 1f / rayDirection.X;
rayDirection.Y = MathUtil.FuzzyZero(rayDirection.Y) ? MathUtil.BT_LARGE_FLOAT : 1f / rayDirection.Y;
rayDirection.Z = MathUtil.FuzzyZero(rayDirection.Z) ? MathUtil.BT_LARGE_FLOAT : 1f / rayDirection.Z;
bool[] sign = new bool[] { rayDirection.X < 0.0f, rayDirection.Y < 0.0f, rayDirection.Z < 0.0f };
#endif
/* Quick pruning by quantized box */
Vector3 rayAabbMin = raySource;
Vector3 rayAabbMax = raySource;
MathUtil.VectorMin(ref rayTarget, ref rayAabbMin);
MathUtil.VectorMax(ref rayTarget, ref rayAabbMax);
/* Add box cast extents to bounding box */
rayAabbMin += aabbMin;
rayAabbMax += aabbMax;
UShortVector3 quantizedQueryAabbMin = new UShortVector3();
UShortVector3 quantizedQueryAabbMax = new UShortVector3();
QuantizeWithClamp(ref quantizedQueryAabbMin, ref rayAabbMin, false);
QuantizeWithClamp(ref quantizedQueryAabbMax, ref rayAabbMax, true);
while (curIndex < endNodeIndex)
{
//#define VISUALLY_ANALYZE_BVH 1
#if VISUALLY_ANALYZE_BVH
//some code snippet to debugDraw aabb, to visually analyze bvh structure
int drawPatch = 3;
//need some global access to a debugDrawer
IDebugDraw debugDrawerPtr = BulletGlobals.gDebugDraw;
//IDebugDraw debugDrawerPtr = null;
//if (curIndex == drawPatch&& debugDrawerPtr != null)
if (debugDrawerPtr != null && curIndex == drawPatch)
{
Vector3 aabbMin2 = Vector3.Zero,aabbMax2 = Vector3.Zero;
aabbMin2 = UnQuantize(ref rootNode.m_quantizedAabbMin);
aabbMax2 = UnQuantize(ref rootNode.m_quantizedAabbMax);
Vector3 color = new Vector3(1f/curIndex,0,0);
debugDrawerPtr.DrawAabb(ref aabbMin2,ref aabbMax2,ref color);
//Console.Out.WriteLine(String.Format("min[{0},{1},{2}] max[{3},{4},{5}]\n", aabbMin.X, aabbMin.Y, aabbMin.Z, aabbMax.X, aabbMax.Y, aabbMax.Z));
}
#endif//VISUALLY_ANALYZE_BVH
//catch bugs in tree data
Debug.Assert(walkIterations < subTreeSize);
walkIterations++;
//PCK: unsigned instead of bool
// only interested if this is closer than any previous hit
float param = 1.0f;
rayBoxOverlap = false;
boxBoxOverlap = AabbUtil2.TestQuantizedAabbAgainstQuantizedAabb(ref quantizedQueryAabbMin, ref quantizedQueryAabbMax, ref rootNode.m_quantizedAabbMin, ref rootNode.m_quantizedAabbMax);
isLeafNode = rootNode.IsLeafNode();
if (boxBoxOverlap)
{
Vector3[] bounds = new Vector3[2];
bounds[0] = UnQuantize(rootNode.m_quantizedAabbMin);
bounds[1] = UnQuantize(rootNode.m_quantizedAabbMax);
/* Add box cast extents */
bounds[0] -= aabbMax;
bounds[1] -= aabbMin;
Vector3 normal;
#if false
bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max);
bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal);
if (ra2 != ra)
{
printf("functions don't match\n");
}
#endif
#if RAYAABB2
///careful with this check: need to check division by zero (above) and fix the unQuantize method
///thanks Joerg/hiker for the reproduction case!
///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
//BT_PROFILE("btRayAabb2");
rayBoxOverlap = AabbUtil2.RayAabb2 (ref raySource, ref rayDirection, sign, bounds, ref param, 0.0f, lambda_max);
#else
rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
#endif
}
if (isLeafNode && rayBoxOverlap)
{
nodeCallback.ProcessNode(rootNode.GetPartId(), rootNode.GetTriangleIndex());
}
//PCK: unsigned instead of bool
if ((rayBoxOverlap) || isLeafNode)
{
curIndex++;
}
else
{
escapeIndex = rootNode.GetEscapeIndex();
curIndex += escapeIndex;
}
rootNode = m_quantizedContiguousNodes[curIndex];
}
if (m_maxIterations < walkIterations)
{
m_maxIterations = walkIterations;
}
}
public void MergeInternalNodeAabb(int nodeIndex, ref Vector3 newAabbMin, ref Vector3 newAabbMax)
{
if (m_useQuantization)
{
UShortVector3 quantizedAabbMin = new UShortVector3();
UShortVector3 quantizedAabbMax = new UShortVector3();
Quantize(ref quantizedAabbMin, ref newAabbMin, false);
Quantize(ref quantizedAabbMax, ref newAabbMax, true);
QuantizedBvhNode node = m_quantizedContiguousNodes[nodeIndex];
node.m_quantizedAabbMin.min(ref quantizedAabbMin);
node.m_quantizedAabbMax.max(ref quantizedAabbMax);
m_quantizedContiguousNodes[nodeIndex]= node;
}
else
{
OptimizedBvhNode node = m_contiguousNodes[nodeIndex];
//non-quantized
MathUtil.VectorMin(ref newAabbMin,ref node.m_aabbMinOrg);
MathUtil.VectorMin(ref newAabbMax, ref node.m_aabbMaxOrg);
m_contiguousNodes[nodeIndex] = node;
}
}
public Vector3 UnQuantize(ref UShortVector3 vecIn)
{
Vector3 vecOut = new Vector3(((float)vecIn.X) / m_bvhQuantization.X,
((float)vecIn.Y) / m_bvhQuantization.Y,
((float)vecIn.Z) / m_bvhQuantization.Z);
vecOut += m_bvhAabbMin;
return vecOut;
}
public Vector3 UnQuantize(UShortVector3 vecIn)
{
return UnQuantize(ref vecIn);
}
//public void quantizeWithClamp(ref UShortVector3 result, ref UShortVector3 point2, bool isMax)
//{
// Debug.Assert(m_useQuantization);
// Vector3 clampedPoint = new Vector3(point2.X, point2.Y, point2.Z);
// MathUtil.vectorMax(ref m_bvhAabbMin, ref clampedPoint);
// MathUtil.vectorMin(ref m_bvhAabbMax, ref clampedPoint);
// quantize(ref result, ref clampedPoint, isMax);
//}
public void QuantizeWithClamp(ref UShortVector3 result, ref Vector3 point2, bool isMax)
{
Debug.Assert(m_useQuantization);
Vector3 clampedPoint = point2;
MathUtil.VectorMax(ref m_bvhAabbMin, ref clampedPoint);
MathUtil.VectorMin(ref m_bvhAabbMax, ref clampedPoint);
Quantize(ref result, ref clampedPoint, isMax);
}