public void Max(ref UShortVector3 a)
{
if (X < a.X)
{
X = a.X;
}
if (Y < a.Y)
{
Y = a.Y;
}
if (Z < a.Z)
{
Z = a.Z;
}
}
public void Min(ref UShortVector3 a)
{
if (X > a.X)
{
X = a.X;
}
if (Y > a.Y)
{
Y = a.Y;
}
if (Z > a.Z)
{
Z = a.Z;
}
}
public static void QuantizeClamp(out UShortVector3 output,
ref IndexedVector3 point,
ref IndexedVector3 min_bound,
ref IndexedVector3 max_bound,
ref IndexedVector3 bvhQuantization)
{
IndexedVector3 clampedPoint = point;
MathUtil.VectorMax(ref min_bound, ref clampedPoint);
MathUtil.VectorMin(ref max_bound, ref clampedPoint);
IndexedVector3 v = (clampedPoint - min_bound) * bvhQuantization;
output = new UShortVector3();
output[0] = (ushort)(v.X + 0.5f);
output[1] = (ushort)(v.Y + 0.5f);
output[2] = (ushort)(v.Z + 0.5f);
}
public static IndexedVector3 Unquantize(
ref UShortVector3 vecIn,
ref IndexedVector3 offset,
ref IndexedVector3 bvhQuantization)
{
IndexedVector3 vecOut = new IndexedVector3(
(float)(vecIn[0]) / (bvhQuantization.X),
(float)(vecIn[1]) / (bvhQuantization.Y),
(float)(vecIn[2]) / (bvhQuantization.Z));
vecOut += offset;
return vecOut;
}
//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;
if(currentNode.m_leftChildIndex > -1 && currentNode.m_leftChildIndex < m_quantizedContiguousNodes.Count)
{
QuantizedBvhNode leftChildNode = m_quantizedContiguousNodes[currentNode.m_leftChildIndex];
WalkRecursiveQuantizedTreeAgainstQueryAabb(ref leftChildNode, nodeCallback, ref quantizedQueryAabbMin, ref quantizedQueryAabbMax);
}
if(currentNode.m_rightChildIndex > -1 && currentNode.m_rightChildIndex < m_quantizedContiguousNodes.Count)
{
//int newIndex = leftChildNode.IsLeafNode() ? leftChildNode.GetTriangleIndex() + 1 : leftChildNode.GetTriangleIndex() + leftChildNode.GetEscapeIndex();
QuantizedBvhNode rightChildNode = m_quantizedContiguousNodes[currentNode.m_rightChildIndex];
WalkRecursiveQuantizedTreeAgainstQueryAabb(ref rightChildNode, nodeCallback, ref quantizedQueryAabbMin, ref quantizedQueryAabbMax);
}
}
}
}
///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 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 = 1;
//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)
{
IndexedVector3 aabbMin, aabbMax;
UnQuantize(ref rootNode.m_quantizedAabbMin, out aabbMin);
UnQuantize(ref rootNode.m_quantizedAabbMax, out aabbMax);
IndexedVector3 color = new IndexedVector3(1, 0, 0);
debugDrawerPtr.DrawAabb(ref aabbMin, ref aabbMax, ref color);
//IndexedVector3 offset = new IndexedVector3(0, 2, 0);
//debugDrawerPtr.DrawAabb(aabbMin+offset, aabbMax+offset, 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;
}
}
//int m_padding[3];
public void SetAabbFromQuantizeNode(QuantizedBvhNode quantizedNode)
{
m_quantizedAabbMin = quantizedNode.m_quantizedAabbMin;
m_quantizedAabbMax = quantizedNode.m_quantizedAabbMax;
}
//public void UnQuantize(UShortVector3 vecIn,out IndexedVector3 vecOut)
//{
// UnQuantize(ref vecIn,out vecOut);
//}
public void UnQuantize(ref UShortVector3 vecIn, out IndexedVector3 vecOut)
{
vecOut = new IndexedVector3(((float)vecIn.X) / m_bvhQuantization.X,
((float)vecIn.Y) / m_bvhQuantization.Y,
((float)vecIn.Z) / m_bvhQuantization.Z);
vecOut += m_bvhAabbMin;
}
//public void quantizeWithClamp(ref UShortVector3 result, ref UShortVector3 point2, bool isMax)
//{
// Debug.Assert(m_useQuantization);
// IndexedVector3 clampedPoint = new IndexedVector3(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(out UShortVector3 result, ref IndexedVector3 point2, bool isMax)
{
Debug.Assert(m_useQuantization);
IndexedVector3 clampedPoint = point2;
MathUtil.VectorMax(ref m_bvhAabbMin, ref clampedPoint);
MathUtil.VectorMin(ref m_bvhAabbMax, ref clampedPoint);
Quantize(out result, ref clampedPoint, isMax);
}
public void Quantize(out UShortVector3 result, ref IndexedVector3 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);
IndexedVector3 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
result = new UShortVector3();
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
IndexedVector3 newPoint;
UnQuantize(ref result, out newPoint);
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
}
public void Max(ref UShortVector3 a)
{
if (X < a.X) X = a.X;
if (Y < a.Y) Y = a.Y;
if (Z < a.Z) Z = a.Z;
}
public void Min(ref UShortVector3 a)
{
if (X > a.X) X = a.X;
if (Y > a.Y) Y = a.Y;
if (Z > a.Z) Z = a.Z;
}
