PhysX 3.4, APEX 1.4 patch release @23879214

1 files changed, 764 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/SceneQuery/src/SqIncrementalAABBTree.cpp b/PhysX_3.4/Source/SceneQuery/src/SqIncrementalAABBTree.cpp
new file mode 100644
index 00000000..be5fccf4
--- /dev/null
+++ b/PhysX_3.4/Source/SceneQuery/src/SqIncrementalAABBTree.cpp
@@ -0,0 +1,764 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2017 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "foundation/PxMemory.h"
+#include "SqIncrementalAABBTree.h"
+#include "SqAABBTree.h"
+#include "SqAABBTreeUpdateMap.h"
+#include "SqBounds.h"
+#include "PsVecMath.h"
+#include "PsFPU.h"
+
+using namespace physx;
+using namespace Sq;
+using namespace shdfnd::aos;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+IncrementalAABBTree::IncrementalAABBTree():
+	mIndicesPool("AABBTreeIndicesPool", 256),
+	mNodesPool("AABBTreeNodesPool", 256	),
+	mRoot(NULL)
+{
+	
+}
+
+IncrementalAABBTree::~IncrementalAABBTree()
+{
+	release();
+}
+
+void IncrementalAABBTree::release()
+{
+	if(mRoot)
+	{
+		releaseNode(mRoot);
+		mRoot = NULL;
+	}
+}
+
+void IncrementalAABBTree::releaseNode(IncrementalAABBTreeNode* node)
+{
+	PX_ASSERT(node);
+	if(node->isLeaf())
+	{
+		mIndicesPool.deallocate(node->mIndices);
+	}
+	else
+	{
+		releaseNode(node->mChilds[0]);
+		releaseNode(node->mChilds[1]);
+	}
+	if(!node->mParent)
+	{
+		mNodesPool.deallocate(reinterpret_cast<IncrementalAABBTreeNodePair*>(node));
+		return;
+	}
+	if(node->mParent->mChilds[1] == node)
+	{
+		mNodesPool.deallocate(reinterpret_cast<IncrementalAABBTreeNodePair*>(node->mParent->mChilds[0]));
+	}
+}
+
+
+// check if node is inside the given bounds
+PX_FORCE_INLINE static bool nodeInsideBounds(const Vec4V& nodeMin, const Vec4V& nodeMax, const Vec4V& parentMin, const Vec4V& parentMax)
+{
+	return !(Ps::IntBool(V4AnyGrtr3(parentMin, nodeMin)) || Ps::IntBool(V4AnyGrtr3(nodeMax, parentMax)));
+}
+
+// update the node parent hierarchy, when insert happen, we can early exit when the node is inside its parent
+// no further update is needed
+PX_FORCE_INLINE static void updateHierarchyAfterInsert(IncrementalAABBTreeNode* node)
+{
+	IncrementalAABBTreeNode* parent = node->mParent;
+	IncrementalAABBTreeNode* testNode = node;
+	while(parent)
+	{
+		// check if we can early exit
+		if(!nodeInsideBounds(testNode->mBVMin, testNode->mBVMax, parent->mBVMin, parent->mBVMax))
+		{
+			parent->mBVMin = V4Min(parent->mChilds[0]->mBVMin, parent->mChilds[1]->mBVMin);
+			parent->mBVMax = V4Max(parent->mChilds[0]->mBVMax, parent->mChilds[1]->mBVMax);
+		}
+		else
+			break;
+		testNode = parent;
+		parent = parent->mParent;
+	}
+}
+
+// add an index into the leaf indices list and update the node bounds
+PX_FORCE_INLINE static void addPrimitiveIntoNode(IncrementalAABBTreeNode* node, const PoolIndex index,  const Vec4V& minV,  const Vec4V& maxV)
+{
+	PX_ASSERT(node->isLeaf());
+	AABBTreeIndices& nodeIndices = *node->mIndices;
+	PX_ASSERT(nodeIndices.nbIndices < NB_OBJECTS_PER_NODE);
+
+	// store the new handle
+	nodeIndices.indices[nodeIndices.nbIndices++] = index;
+
+	// increase the node bounds
+	node->mBVMin = V4Min(node->mBVMin, minV);
+	node->mBVMax = V4Max(node->mBVMax, maxV);
+
+	updateHierarchyAfterInsert(node);
+}
+
+// check if node does intersect with given bounds
+PX_FORCE_INLINE static bool nodeIntersection(IncrementalAABBTreeNode& node, const Vec4V& minV,  const Vec4V& maxV)
+{
+	return !(Ps::IntBool(V4AnyGrtr3(node.mBVMin, maxV)) || Ps::IntBool(V4AnyGrtr3(minV, node.mBVMax)));
+}
+
+// traversal strategy
+PX_FORCE_INLINE static PxU32 traversalDirection(IncrementalAABBTreeNode& child0, IncrementalAABBTreeNode& child1, const Vec4V& testCenterV)
+{
+	// traverse in the direction of a node which is closer
+	// we compare the node and object centers
+	const Vec4V centerCh0V = V4Add(child0.mBVMax, child0.mBVMin);
+	const Vec4V centerCh1V = V4Add(child1.mBVMax, child1.mBVMin);
+
+	const Vec4V ch0D = V4Sub(testCenterV, centerCh0V);
+	const Vec4V ch1D = V4Sub(testCenterV, centerCh1V);
+
+	const BoolV con = FIsGrtr(V4Dot3(ch0D, ch0D), V4Dot3(ch1D, ch1D));
+	return (BAllEqTTTT(con) == 1) ? PxU32(1) : PxU32(0);
+}
+
+// remove an index from the leaf
+PX_FORCE_INLINE static void removePrimitiveFromNode(IncrementalAABBTreeNode* node, const PoolIndex index)
+{
+	AABBTreeIndices& indices = *node->mIndices;
+	PX_ASSERT(indices.nbIndices > 1);
+
+	for (PxU32 i = indices.nbIndices; i--; )
+	{
+		if(node->mIndices->indices[i] == index)
+		{
+			node->mIndices->indices[i] = node->mIndices->indices[--indices.nbIndices];
+			return;
+		}
+	}
+	// if handle was not found something is wrong here
+	PX_ASSERT(0);
+}
+
+// check if bounds are equal with given node min/max
+PX_FORCE_INLINE static bool boundsEqual(const Vec4V& testMin, const Vec4V& testMax, const Vec4V& nodeMin, const Vec4V& nodeMax)
+{
+	return (Ps::IntBool(V4AllEq(nodeMin, testMin)) && Ps::IntBool(V4AllEq(testMax, nodeMax)));
+}
+
+// update the node hierarchy bounds when remove happen, we can early exit if the bounds are equal and no bounds update
+// did happen
+PX_FORCE_INLINE static void updateHierarchyAfterRemove(IncrementalAABBTreeNode* node, const PxBounds3* bounds)
+{	
+	if(node->isLeaf())
+	{		
+		const AABBTreeIndices& indices = *node->mIndices;
+		PX_ASSERT(indices.nbIndices > 0);
+
+		Vec4V bvMin = V4LoadU(&bounds[indices.indices[0]].minimum.x);
+		Vec4V bvMax = V4LoadU(&bounds[indices.indices[0]].maximum.x);
+		for(PxU32 i = 1; i < indices.nbIndices; i++)
+		{
+			const Vec4V minV = V4LoadU(&bounds[indices.indices[i]].minimum.x);
+			const Vec4V maxV = V4LoadU(&bounds[indices.indices[i]].maximum.x);
+
+			bvMin = V4Min(bvMin, minV);
+			bvMax = V4Max(bvMax, maxV);
+		}
+
+		node->mBVMin = V4ClearW(bvMin);
+		node->mBVMax = V4ClearW(bvMax);
+	}
+	else
+	{
+		node->mBVMin = V4Min(node->mChilds[0]->mBVMin, node->mChilds[1]->mBVMin);
+		node->mBVMax = V4Max(node->mChilds[0]->mBVMax, node->mChilds[1]->mBVMax);
+	}
+
+	IncrementalAABBTreeNode* parent = node->mParent;
+	while(parent)
+	{
+		const Vec4V newMinV = V4Min(parent->mChilds[0]->mBVMin, parent->mChilds[1]->mBVMin);
+		const Vec4V newMaxV = V4Max(parent->mChilds[0]->mBVMax, parent->mChilds[1]->mBVMax);
+
+		const bool earlyExit = boundsEqual(newMinV, newMaxV, parent->mBVMin, parent->mBVMax);
+		if(earlyExit)
+			break;
+
+		parent->mBVMin = newMinV;
+		parent->mBVMax = newMaxV;
+
+		parent = parent->mParent;
+	}
+}
+
+// split the leaf node along the most significant axis
+IncrementalAABBTreeNode* IncrementalAABBTree::splitLeafNode(IncrementalAABBTreeNode* node, const PoolIndex index,  const Vec4V& minV,  const Vec4V& maxV, const PxBounds3* bounds)
+{
+	PX_ASSERT(node->isLeaf());
+
+	IncrementalAABBTreeNode* returnNode = NULL;
+
+	// create new pairs of nodes, parent will remain the node (the one we split_
+	IncrementalAABBTreeNode* child0 = reinterpret_cast<IncrementalAABBTreeNode*>(mNodesPool.allocate());
+	IncrementalAABBTreeNode* child1 = child0 + 1;	
+	AABBTreeIndices* newIndices = mIndicesPool.allocate();
+
+	// get the split axis
+	PX_ALIGN(16, PxVec4) vars;
+	PX_ALIGN(16, PxVec4) center;
+	const float half = 0.5f;
+	const FloatV halfV = FLoad(half);
+	const Vec4V newMinV = V4Min(node->mBVMin, minV);
+	const Vec4V newMaxV = V4Max(node->mBVMax, maxV);
+	const Vec4V centerV = V4Scale(V4Add(newMaxV, newMinV), halfV);
+	const Vec4V varsV = V4Sub(newMaxV, newMinV);
+	V4StoreA(varsV, &vars.x);
+	V4StoreA(centerV, &center.x);
+	const PxU32 axis = Ps::largestAxis(PxVec3(vars.x, vars.y, vars.z));
+
+	// setup parent
+	child0->mParent = node;
+	child1->mParent = node;
+	child0->mIndices = node->mIndices;
+	child0->mChilds[1] = NULL;
+	child1->mIndices = newIndices;
+	child1->mChilds[1] = NULL;
+
+	AABBTreeIndices& child0Indices = *child0->mIndices;	// the original node indices
+	AABBTreeIndices& child1Indices = *child1->mIndices; // new empty indices
+	child1Indices.nbIndices = 0;
+
+	// split the node
+	for(PxU32 i = child0Indices.nbIndices; i--;)
+	{
+		const PxBounds3& primitiveBounds = bounds[child0Indices.indices[i]];
+		const float pCenter = primitiveBounds.getCenter(axis);
+		if(center[axis] > pCenter)
+		{
+			// move to new node
+			child1Indices.indices[child1Indices.nbIndices++] = child0Indices.indices[i];						
+			child0Indices.nbIndices--;
+			child0Indices.indices[i] = child0Indices.indices[child0Indices.nbIndices];
+		}
+	}
+
+	// check where to put the new node, if there is still a free space
+	if(child0Indices.nbIndices == 0 || child1Indices.nbIndices == NB_OBJECTS_PER_NODE)
+	{
+		child0Indices.nbIndices = 1;
+		child0Indices.indices[0] = index;
+		returnNode = child0;
+	}
+	else
+	{
+		if(child0Indices.nbIndices == NB_OBJECTS_PER_NODE)
+		{
+			child1Indices.nbIndices = 1;
+			child1Indices.indices[0] = index;
+			returnNode = child1;
+		}
+		else
+		{
+			const PxBounds3& primitiveBounds = bounds[index];
+			const float pCenter = primitiveBounds.getCenter(axis);
+			if(center[axis] > pCenter)
+			{
+				// move to new node
+				child1Indices.indices[child1Indices.nbIndices++] = index;
+				returnNode = child1;
+			}
+			else
+			{
+				// move to old node
+				child0Indices.indices[child0Indices.nbIndices++] = index;	
+				returnNode = child0;
+			}
+		}
+	}
+
+	// update bounds for the new nodes
+	Vec4V bvMin = V4LoadU(&bounds[child0Indices.indices[0]].minimum.x);
+	Vec4V bvMax = V4LoadU(&bounds[child0Indices.indices[0]].maximum.x);	
+	for(PxU32 i = 1; i < child0Indices.nbIndices; i++)
+	{
+		const Vec4V nodeMinV = V4LoadU(&bounds[child0Indices.indices[i]].minimum.x);
+		const Vec4V nodeMaxV = V4LoadU(&bounds[child0Indices.indices[i]].maximum.x);	
+
+		bvMin = V4Min(bvMin, nodeMinV);
+		bvMax = V4Max(bvMax, nodeMaxV);
+	}
+	child0->mBVMin = V4ClearW(bvMin);
+	child0->mBVMax = V4ClearW(bvMax);	
+
+	bvMin = V4LoadU(&bounds[child1Indices.indices[0]].minimum.x);
+	bvMax = V4LoadU(&bounds[child1Indices.indices[0]].maximum.x);	
+	for(PxU32 i = 1; i < child1Indices.nbIndices; i++)
+	{
+		const Vec4V nodeMinV = V4LoadU(&bounds[child1Indices.indices[i]].minimum.x);
+		const Vec4V nodeMaxV = V4LoadU(&bounds[child1Indices.indices[i]].maximum.x);	
+
+		bvMin = V4Min(bvMin, nodeMinV);
+		bvMax = V4Max(bvMax, nodeMaxV);
+	}
+	child1->mBVMin = V4ClearW(bvMin);
+	child1->mBVMax = V4ClearW(bvMax);
+
+	// node parent is the same, setup the new childs
+	node->mChilds[0] = child0;
+	node->mChilds[1] = child1;
+	node->mBVMin = newMinV;
+	node->mBVMax = newMaxV;
+
+	updateHierarchyAfterInsert(node);
+
+	PX_ASSERT(returnNode);
+	return returnNode;
+}
+
+
+// insert new bounds into tree
+IncrementalAABBTreeNode* IncrementalAABBTree::insert(const PoolIndex index, const PxBounds3* bounds, bool& split)
+{	
+	PX_SIMD_GUARD;
+
+	// get the bounds, reset the W value
+	const Vec4V minV = V4ClearW(V4LoadU(&bounds[index].minimum.x));
+	const Vec4V maxV = V4ClearW(V4LoadU(&bounds[index].maximum.x));	
+
+	split = false;
+
+	// check if tree is empty
+	if(!mRoot)
+	{
+		// make it a leaf
+		AABBTreeIndices* indices = mIndicesPool.construct(index);
+		mRoot = reinterpret_cast<IncrementalAABBTreeNode*> (mNodesPool.allocate());
+		mRoot->mBVMin = minV;
+		mRoot->mBVMax = maxV;
+		mRoot->mIndices = indices;
+		mRoot->mChilds[1] = NULL;
+		mRoot->mParent = NULL;
+		
+		return mRoot;
+	}
+	else
+	{
+		// check if root is a leaf
+		if(mRoot->isLeaf())
+		{
+			// if we still can insert the primitive into the leaf, or we need to split			
+			if(mRoot->getNbPrimitives() < NB_OBJECTS_PER_NODE)
+			{
+				// simply add the primitive into the current leaf
+				addPrimitiveIntoNode(mRoot, index, minV, maxV);
+				return mRoot;
+			}
+			else
+			{
+				// need to split the node
+				IncrementalAABBTreeNode* retNode = splitLeafNode(mRoot, index, minV, maxV, bounds);
+				mRoot = retNode->mParent;
+				split = true;
+				return retNode;
+			}
+		}
+		else
+		{
+			const Vec4V testCenterV = V4Add(maxV, minV);
+			// we dont need to modify root, lets traverse the tree to find the right spot
+			PxU32 traversalIndex = traversalDirection(*mRoot->mChilds[0], *mRoot->mChilds[1], testCenterV);
+			IncrementalAABBTreeNode* baseNode = mRoot->mChilds[traversalIndex];
+			while(!baseNode->isLeaf())
+			{
+				Ps::prefetchLine(baseNode->mChilds[0]->mChilds[0]);
+				Ps::prefetchLine(baseNode->mChilds[1]->mChilds[0]);
+
+				traversalIndex = traversalDirection(*baseNode->mChilds[0], *baseNode->mChilds[1], testCenterV);
+				baseNode = baseNode->mChilds[traversalIndex];
+			}
+			
+			// if we still can insert the primitive into the leaf, or we need to split			
+			if(baseNode->getNbPrimitives() < NB_OBJECTS_PER_NODE)
+			{
+				// simply add the primitive into the current leaf
+				addPrimitiveIntoNode(baseNode, index, minV, maxV);
+				return baseNode;
+			}
+			else
+			{
+				// split
+				IncrementalAABBTreeNode* retNode = splitLeafNode(baseNode, index, minV, maxV, bounds);				
+				split = true;
+				return retNode;
+			}
+		}
+	}
+}
+
+// update the index, do a full remove/insert update
+IncrementalAABBTreeNode* IncrementalAABBTree::update(IncrementalAABBTreeNode* node, const PoolIndex index, const PxBounds3* bounds, bool& split, IncrementalAABBTreeNode*& removedNode)
+{
+	PX_SIMD_GUARD;
+
+	removedNode = remove(node, index, bounds);
+	return insert(index, bounds, split);	
+}
+
+// update the index, faster version with a lazy update of objects that moved just a bit
+IncrementalAABBTreeNode* IncrementalAABBTree::updateFast(IncrementalAABBTreeNode* node, const PoolIndex index, const PxBounds3* bounds, bool& split, IncrementalAABBTreeNode*& removedNode)
+{
+	PX_SIMD_GUARD;
+
+	const Vec4V minV = V4ClearW(V4LoadU(&bounds[index].minimum.x));
+	const Vec4V maxV = V4ClearW(V4LoadU(&bounds[index].maximum.x));
+
+	// for update fast, we dont care if the tree gets slowly unbalanced, we are building a new tree already
+	if(nodeIntersection(*node, minV, maxV))
+	{
+		updateHierarchyAfterRemove(node, bounds);
+		return node;
+	}
+	else
+	{
+		removedNode = remove(node, index, bounds);	
+		return insert(index, bounds, split);	
+	}
+}
+
+// remove primitive from the tree, return a node if it moved to its parent
+IncrementalAABBTreeNode* IncrementalAABBTree::remove(IncrementalAABBTreeNode* node, const PoolIndex index, const PxBounds3* bounds)
+{
+	PX_SIMD_GUARD;
+	PX_ASSERT(node->isLeaf());
+	// if we just remove the primitive from the list
+	if(node->getNbPrimitives() > 1)
+	{
+		removePrimitiveFromNode(node, index);
+
+		// update the hierarchy		
+		updateHierarchyAfterRemove(node, bounds);
+		return NULL;
+	}
+	else
+	{
+		// if root node and the last primitive remove root
+		if(node == mRoot)
+		{
+			mNodesPool.deallocate(reinterpret_cast<IncrementalAABBTreeNodePair*>(node));
+			mRoot = NULL;
+			return NULL;
+		}
+		else
+		{
+			// create new parent and remove the current leaf
+			IncrementalAABBTreeNode* parent = node->mParent;
+			IncrementalAABBTreeNodePair* removedPair = reinterpret_cast<IncrementalAABBTreeNodePair*>(parent->mChilds[0]);
+			PX_ASSERT(!parent->isLeaf());
+
+			// copy the remaining child into parent
+			IncrementalAABBTreeNode* remainingChild = (parent->mChilds[0] == node) ? parent->mChilds[1] : parent->mChilds[0];
+			parent->mBVMax = remainingChild->mBVMax;
+			parent->mBVMin = remainingChild->mBVMin;
+			if(remainingChild->isLeaf())
+			{
+				parent->mIndices = remainingChild->mIndices;
+				parent->mChilds[1] = NULL;
+			}
+			else
+			{
+				parent->mChilds[0] = remainingChild->mChilds[0];
+				parent->mChilds[0]->mParent = parent;
+				parent->mChilds[1] = remainingChild->mChilds[1];
+				parent->mChilds[1]->mParent = parent;
+			}
+
+			if(parent->mParent)
+			{
+				updateHierarchyAfterRemove(parent->mParent, bounds);
+			}
+
+			mIndicesPool.deallocate(node->mIndices);
+			mNodesPool.deallocate(removedPair);	
+			return parent;
+		}
+	}
+}
+
+// fixup the indices
+void IncrementalAABBTree::fixupTreeIndices(IncrementalAABBTreeNode* node, const PoolIndex index, const PoolIndex newIndex)
+{
+	PX_ASSERT(node->isLeaf());
+
+	AABBTreeIndices& indices = *node->mIndices;
+	for(PxU32 i = 0; i < indices.nbIndices; i++)
+	{
+		if(indices.indices[i] == index)			
+		{
+			indices.indices[i] = newIndex;
+			return;
+		}
+	}	
+	PX_ASSERT(0);
+}
+
+// shift node
+static void shiftNode(IncrementalAABBTreeNode* node, const Vec4V& shiftV)
+{
+	node->mBVMax = V4Sub(node->mBVMax, shiftV);
+	node->mBVMin = V4Sub(node->mBVMin, shiftV);
+
+	if(!node->isLeaf())
+	{
+		shiftNode(node->mChilds[0], shiftV);
+		shiftNode(node->mChilds[1], shiftV);
+	}
+}
+
+// shift origin
+void IncrementalAABBTree::shiftOrigin(const PxVec3& shift)
+{
+	if(mRoot)
+	{
+		const Vec4V shiftV = V4ClearW(V4LoadU(&shift.x));
+
+		shiftNode(mRoot, shiftV);
+	}
+}
+
+static void checkNode(IncrementalAABBTreeNode* node, IncrementalAABBTreeNode* parent, const PxBounds3* bounds, PoolIndex maxIndex, PxU32& numIndices)
+{
+	PX_ASSERT(node->mParent == parent);
+	PX_ASSERT(!parent->isLeaf());
+	PX_ASSERT(parent->mChilds[0] == node || parent->mChilds[1] == node);
+
+	ASSERT_ISVALIDVEC3V(node->mBVMin);
+	ASSERT_ISVALIDVEC3V(node->mBVMax);
+
+	if(!node->isLeaf())
+	{
+		PX_ASSERT(nodeInsideBounds(node->mChilds[0]->mBVMin, node->mChilds[0]->mBVMax, node->mBVMin, node->mBVMax));
+		PX_ASSERT(nodeInsideBounds(node->mChilds[1]->mBVMin, node->mChilds[1]->mBVMax, node->mBVMin, node->mBVMax));
+
+		const Vec4V testMinV = V4Min(parent->mChilds[0]->mBVMin, parent->mChilds[1]->mBVMin);
+		const Vec4V testMaxV = V4Max(parent->mChilds[0]->mBVMax, parent->mChilds[1]->mBVMax);
+
+		PX_UNUSED(testMinV);
+		PX_UNUSED(testMaxV);
+		PX_ASSERT(boundsEqual(testMinV, testMaxV, node->mBVMin, node->mBVMax));
+
+		checkNode(node->mChilds[0], node, bounds, maxIndex, numIndices);
+		checkNode(node->mChilds[1], node, bounds, maxIndex, numIndices);
+	}
+	else
+	{
+		const AABBTreeIndices& indices = *node->mIndices;
+		PX_ASSERT(indices.nbIndices);
+		Vec4V testMinV = V4ClearW(V4LoadU(&bounds[indices.indices[0]].minimum.x));
+		Vec4V testMaxV = V4ClearW(V4LoadU(&bounds[indices.indices[0]].maximum.x));
+		for(PxU32 i = 0; i < indices.nbIndices; i++)
+		{
+			PX_ASSERT(indices.indices[i] < maxIndex);
+			numIndices++;
+
+			const Vec4V minV = V4ClearW(V4LoadU(&bounds[indices.indices[i]].minimum.x));
+			const Vec4V maxV = V4ClearW(V4LoadU(&bounds[indices.indices[i]].maximum.x));
+
+			testMinV = V4Min(testMinV, minV);
+			testMaxV = V4Max(testMaxV, maxV);
+
+			PX_ASSERT(nodeInsideBounds(minV, maxV, node->mBVMin, node->mBVMax));
+		}
+
+		PX_ASSERT(boundsEqual(testMinV, testMaxV, node->mBVMin, node->mBVMax));
+	}
+}
+
+void IncrementalAABBTree::hierarchyCheck(PoolIndex maxIndex, const PxBounds3* bounds)
+{
+	PxU32 numHandles = 0;
+	if(mRoot && !mRoot->isLeaf())
+	{
+		checkNode(mRoot->mChilds[0], mRoot, bounds, maxIndex, numHandles);
+		checkNode(mRoot->mChilds[1], mRoot, bounds, maxIndex, numHandles);
+
+		PX_ASSERT(numHandles == maxIndex);
+	}	
+}
+
+void IncrementalAABBTree::checkTreeLeaf(IncrementalAABBTreeNode* leaf, PoolIndex h)
+{
+	PX_ASSERT(leaf->isLeaf());
+
+	const AABBTreeIndices& indices = *leaf->mIndices;
+	bool found = false;
+	for(PxU32 i = 0; i < indices.nbIndices; i++)
+	{
+		if(indices.indices[i] == h)
+		{
+			found = true;
+			break;
+		}
+	}
+	PX_UNUSED(found);
+	PX_ASSERT(found);
+}
+
+// build the tree from given bounds
+bool IncrementalAABBTree::build(AABBTreeBuildParams& params, Ps::Array<IncrementalAABBTreeNode*>& mapping)
+{
+	// Init stats
+	BuildStats stats;
+	const PxU32 nbPrimitives = params.mNbPrimitives;
+	if (!nbPrimitives)
+		return false;
+
+	// Init stats
+	stats.setCount(1);
+
+	// Initialize indices. This list will be modified during build.
+	PxU32* indices = reinterpret_cast<PxU32*>(PX_ALLOC(sizeof(PxU32)*nbPrimitives, "AABB tree indices"));
+	// Identity permutation
+	for (PxU32 i = 0; i<nbPrimitives; i++)
+		indices[i] = i;
+
+	// Allocate a pool of nodes
+	mNodeAllocator.init(nbPrimitives, params.mLimit);
+
+	// Compute box centers only once and cache them
+	params.mCache = reinterpret_cast<PxVec3*>(PX_ALLOC(sizeof(PxVec3)*(nbPrimitives + 1), "cache"));
+	const float half = 0.5f;
+	const FloatV halfV = FLoad(half);
+	for (PxU32 i = 0; i<nbPrimitives; i++)
+	{
+		const Vec4V curMinV = V4LoadU(&params.mAABBArray[i].minimum.x);
+		const Vec4V curMaxV = V4LoadU(&params.mAABBArray[i].maximum.x);
+		const Vec4V centerV = V4Scale(V4Add(curMaxV, curMinV), halfV);
+		V4StoreU(centerV, &params.mCache[i].x);
+	}
+
+	// Build the hierarchy
+	mNodeAllocator.mPool->_buildHierarchy(params, stats, mNodeAllocator, indices);
+
+	PX_FREE_AND_RESET(params.mCache);
+
+	IncrementalAABBTreeNode** treeNodes = reinterpret_cast<IncrementalAABBTreeNode**>(PX_ALLOC(sizeof(IncrementalAABBTreeNode*)*(stats.getCount()), "temp node helper array"));
+	PxMemSet(treeNodes, 0, sizeof(IncrementalAABBTreeNode*)*(stats.getCount()));
+
+	clone(mapping, indices, treeNodes);
+	mRoot = treeNodes[0];
+	mRoot->mParent = NULL;
+	
+	PX_FREE_AND_RESET(indices);
+	PX_FREE_AND_RESET(treeNodes);
+
+	mNodeAllocator.release();
+	return true;
+}
+
+// clone the tree, the tree is computed in the NodeAllocator, similar to AABBTree flatten
+void IncrementalAABBTree::clone(Ps::Array<IncrementalAABBTreeNode*>& mapping, const PxU32* _indices, IncrementalAABBTreeNode** treeNodes)
+{		
+	PxU32 offset = 0;
+	const PxU32 nbSlabs = mNodeAllocator.mSlabs.size();
+	for (PxU32 s = 0; s<nbSlabs; s++)
+	{
+		const NodeAllocator::Slab& currentSlab = mNodeAllocator.mSlabs[s];
+
+		AABBTreeBuildNode* pool = currentSlab.mPool;
+		for (PxU32 i = 0; i < currentSlab.mNbUsedNodes; i++)
+		{
+			IncrementalAABBTreeNode* destNode = treeNodes[offset];
+			if(!destNode)
+			{
+				destNode = reinterpret_cast<IncrementalAABBTreeNode*>(mNodesPool.allocate());
+				treeNodes[offset] = destNode;
+			}
+
+			destNode->mBVMin = V4ClearW(V4LoadU(&pool[i].mBV.minimum.x));
+			destNode->mBVMax = V4ClearW(V4LoadU(&pool[i].mBV.maximum.x));
+
+			if (pool[i].isLeaf())
+			{
+				AABBTreeIndices* indices = mIndicesPool.allocate();
+				destNode->mIndices = indices;
+				destNode->mChilds[1] = NULL;
+				indices->nbIndices = pool[i].getNbPrimitives();
+				PX_ASSERT(indices->nbIndices <= 16);
+				const PxU32* sourceIndices = _indices + pool[i].mNodeIndex;
+				for (PxU32 iIndices = 0; iIndices < indices->nbIndices; iIndices++)
+				{
+					const PxU32 sourceIndex = sourceIndices[iIndices];
+					indices->indices[iIndices] = sourceIndex;
+					PX_ASSERT(sourceIndex < mapping.size());
+					mapping[sourceIndex] = destNode;
+				}
+			}
+			else
+			{
+				PX_ASSERT(pool[i].mPos);
+				PxU32 localNodeIndex = 0xffffffff;
+				PxU32 nodeBase = 0;
+				for (PxU32 j = 0; j<nbSlabs; j++)
+				{
+					if (pool[i].mPos >= mNodeAllocator.mSlabs[j].mPool && pool[i].mPos < mNodeAllocator.mSlabs[j].mPool + mNodeAllocator.mSlabs[j].mNbUsedNodes)
+					{
+						localNodeIndex = PxU32(pool[i].mPos - mNodeAllocator.mSlabs[j].mPool);
+						break;
+					}
+					nodeBase += mNodeAllocator.mSlabs[j].mNbUsedNodes;
+				}
+				const PxU32 nodeIndex = nodeBase + localNodeIndex;
+
+				IncrementalAABBTreeNode* child0 = treeNodes[nodeIndex];
+				IncrementalAABBTreeNode* child1 = treeNodes[nodeIndex + 1];
+				if(!child0)
+				{
+					PX_ASSERT(!child1);
+					child0 = reinterpret_cast<IncrementalAABBTreeNode*>(mNodesPool.allocate());
+					child1 = child0 + 1;
+					treeNodes[nodeIndex] = child0;
+					treeNodes[nodeIndex + 1] = child1;
+				}
+
+				destNode->mChilds[0] = child0;
+				destNode->mChilds[1] = child1;
+				child0->mParent = destNode;
+				child1->mParent = destNode;
+			}
+			offset++;
+		}
+	}
+}
+
+