OptimizedBvh.java

/*
 * Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
 *
 * Bullet Continuous Collision Detection and Physics Library
 * Copyright (c) 2003-2008 Erwin Coumans  http://www.bulletphysics.com/
 *
 * This software is provided 'as-is', without any express or implied warranty.
 * In no event will the authors be held liable for any damages arising from
 * the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose, 
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package com.bulletphysics.collision.shapes;

import com.bulletphysics.linearmath.AabbUtil2;
import com.bulletphysics.linearmath.MiscUtil;
import com.bulletphysics.linearmath.VectorUtil;
import com.bulletphysics.util.ObjectArrayList;
import com.bulletphysics.util.Stack;
import com.bulletphysics.util.Supplier;

import java.io.Serializable;
import javax.vecmath.Vector3f;

// JAVA NOTE: OptimizedBvh still from 2.66, update it for 2.70b1

/**
 * OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU, GPU in future).
 * 
 * @author jezek2
 */
public class OptimizedBvh implements Serializable {
    private final static Supplier<OptimizedBvhNode> NEW_OPTIMIZED_BVH_NODE_SUPPLIER = new Supplier<OptimizedBvhNode>() {
      @Override
      public OptimizedBvhNode get() {
       return new OptimizedBvhNode();
      }
      
    };
  
	private static final long serialVersionUID = 1L;

	//protected final BulletStack stack = BulletStack.get();
	
	private static final boolean DEBUG_TREE_BUILDING = false;
	private static int gStackDepth = 0;
	private static int gMaxStackDepth = 0;
	
	private static int maxIterations = 0;
	
	// Note: currently we have 16 bytes per quantized node
	public static final int MAX_SUBTREE_SIZE_IN_BYTES = 2048;

	// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
	// actually) triangles each (since the sign bit is reserved
	public static final int MAX_NUM_PARTS_IN_BITS = 10;

	////////////////////////////////////////////////////////////////////////////

	private final ObjectArrayList<OptimizedBvhNode> leafNodes = new ObjectArrayList<OptimizedBvhNode>();
	private final ObjectArrayList<OptimizedBvhNode> contiguousNodes = new ObjectArrayList<OptimizedBvhNode>();

	private QuantizedBvhNodes quantizedLeafNodes = new QuantizedBvhNodes();
	private QuantizedBvhNodes quantizedContiguousNodes = new QuantizedBvhNodes();
	
	private int curNodeIndex;

	// quantization data
	private boolean useQuantization;
	private final Vector3f bvhAabbMin = new Vector3f();
	private final Vector3f bvhAabbMax = new Vector3f();
	private final Vector3f bvhQuantization = new Vector3f();
	
	protected TraversalMode traversalMode = TraversalMode.STACKLESS;
	protected final ObjectArrayList<BvhSubtreeInfo> SubtreeHeaders = new ObjectArrayList<BvhSubtreeInfo>();
	// This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
	protected int subtreeHeaderCount;

	// two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
	// this might be refactored into a virtual, it is usually not calculated at run-time
	public void setInternalNodeAabbMin(int nodeIndex, Vector3f aabbMin) {
		if (useQuantization) {
			quantizedContiguousNodes.setQuantizedAabbMin(nodeIndex, quantizeWithClamp(aabbMin));
		}
		else {
			contiguousNodes.getQuick(nodeIndex).aabbMinOrg.set(aabbMin);
		}
	}

	public void setInternalNodeAabbMax(int nodeIndex, Vector3f aabbMax) {
		if (useQuantization) {
			quantizedContiguousNodes.setQuantizedAabbMax(nodeIndex, quantizeWithClamp(aabbMax));
		}
		else {
			contiguousNodes.getQuick(nodeIndex).aabbMaxOrg.set(aabbMax);
		}
	}
	
	public Vector3f getAabbMin(int nodeIndex) {
		if (useQuantization) {
			Vector3f tmp = new Vector3f();
			unQuantize(tmp, quantizedLeafNodes.getQuantizedAabbMin(nodeIndex));
			return tmp;
		}

		// non-quantized
		return leafNodes.getQuick(nodeIndex).aabbMinOrg;
	}

	public Vector3f getAabbMax(int nodeIndex) {
		if (useQuantization) {
			Vector3f tmp = new Vector3f();
			unQuantize(tmp, quantizedLeafNodes.getQuantizedAabbMax(nodeIndex));
			return tmp;
		}
		
		// non-quantized
		return leafNodes.getQuick(nodeIndex).aabbMaxOrg;
	}

	public void setQuantizationValues(Vector3f aabbMin, Vector3f aabbMax) {
		setQuantizationValues(aabbMin, aabbMax, 1f);
	}
	
	public void setQuantizationValues(Vector3f aabbMin, Vector3f aabbMax, float quantizationMargin) {
		// enlarge the AABB to avoid division by zero when initializing the quantization values
	    Stack stack = Stack.enter();
		Vector3f clampValue = stack.allocVector3f();
		clampValue.set(quantizationMargin,quantizationMargin,quantizationMargin);
		bvhAabbMin.sub(aabbMin, clampValue);
		bvhAabbMax.add(aabbMax, clampValue);
		Vector3f aabbSize = stack.allocVector3f();
		aabbSize.sub(bvhAabbMax, bvhAabbMin);
		bvhQuantization.set(65535f, 65535f, 65535f);
		VectorUtil.div(bvhQuantization, bvhQuantization, aabbSize);
		stack.leave();
	}
	
	public void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) {
		if (useQuantization) {
			quantizedContiguousNodes.setEscapeIndexOrTriangleIndex(nodeIndex, -escapeIndex);
		}
		else {
			contiguousNodes.getQuick(nodeIndex).escapeIndex = escapeIndex;
		}
	}

	public void mergeInternalNodeAabb(int nodeIndex, Vector3f newAabbMin, Vector3f newAabbMax) {
		if (useQuantization) {
			long quantizedAabbMin;
			long quantizedAabbMax;

			quantizedAabbMin = quantizeWithClamp(newAabbMin);
			quantizedAabbMax = quantizeWithClamp(newAabbMax);
			for (int i = 0; i < 3; i++) {
				if (quantizedContiguousNodes.getQuantizedAabbMin(nodeIndex, i) > QuantizedBvhNodes.getCoord(quantizedAabbMin, i)) {
					quantizedContiguousNodes.setQuantizedAabbMin(nodeIndex, i, QuantizedBvhNodes.getCoord(quantizedAabbMin, i));
				}

				if (quantizedContiguousNodes.getQuantizedAabbMax(nodeIndex, i) < QuantizedBvhNodes.getCoord(quantizedAabbMax, i)) {
					quantizedContiguousNodes.setQuantizedAabbMax(nodeIndex, i, QuantizedBvhNodes.getCoord(quantizedAabbMax, i));
				}
			}
		}
		else {
			// non-quantized
			VectorUtil.setMin(contiguousNodes.getQuick(nodeIndex).aabbMinOrg, newAabbMin);
			VectorUtil.setMax(contiguousNodes.getQuick(nodeIndex).aabbMaxOrg, newAabbMax);
		}
	}
	
	public void swapLeafNodes(int i, int splitIndex) {
		if (useQuantization) {
			quantizedLeafNodes.swap(i, splitIndex);
		}
		else {
			// JAVA NOTE: changing reference instead of copy
			OptimizedBvhNode tmp = leafNodes.getQuick(i);
			leafNodes.setQuick(i, leafNodes.getQuick(splitIndex));
			leafNodes.setQuick(splitIndex, tmp);
		}
	}

	public void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex) {
		if (useQuantization) {
			quantizedContiguousNodes.set(internalNode, quantizedLeafNodes, leafNodeIndex);
		}
		else {
			contiguousNodes.getQuick(internalNode).set(leafNodes.getQuick(leafNodeIndex));
		}
	}

	private static class NodeTriangleCallback extends InternalTriangleIndexCallback {
		public ObjectArrayList<OptimizedBvhNode> triangleNodes;
		
		public NodeTriangleCallback(ObjectArrayList<OptimizedBvhNode> triangleNodes) {
			this.triangleNodes = triangleNodes;
		}

		private final Vector3f aabbMin = new Vector3f(), aabbMax = new Vector3f();
		
		public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
			OptimizedBvhNode node = new OptimizedBvhNode();
			aabbMin.set(1e30f, 1e30f, 1e30f);
			aabbMax.set(-1e30f, -1e30f, -1e30f);
			VectorUtil.setMin(aabbMin, triangle[0]);
			VectorUtil.setMax(aabbMax, triangle[0]);
			VectorUtil.setMin(aabbMin, triangle[1]);
			VectorUtil.setMax(aabbMax, triangle[1]);
			VectorUtil.setMin(aabbMin, triangle[2]);
			VectorUtil.setMax(aabbMax, triangle[2]);

			// with quantization?
			node.aabbMinOrg.set(aabbMin);
			node.aabbMaxOrg.set(aabbMax);

			node.escapeIndex = -1;

			// for child nodes
			node.subPart = partId;
			node.triangleIndex = triangleIndex;
			triangleNodes.add(node);
		}
	}

	private static class QuantizedNodeTriangleCallback extends InternalTriangleIndexCallback {
		//protected final BulletStack stack = BulletStack.get();
		
		public QuantizedBvhNodes triangleNodes;
		public OptimizedBvh optimizedTree; // for quantization

		public QuantizedNodeTriangleCallback(QuantizedBvhNodes triangleNodes, OptimizedBvh tree) {
			this.triangleNodes = triangleNodes;
			this.optimizedTree = tree;
		}
		
		public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
			// The partId and triangle index must fit in the same (positive) integer
			assert (partId < (1 << MAX_NUM_PARTS_IN_BITS));
			assert (triangleIndex < (1 << (31 - MAX_NUM_PARTS_IN_BITS)));
			// negative indices are reserved for escapeIndex
			assert (triangleIndex >= 0);

			Stack stack = Stack.enter();
			int nodeId = triangleNodes.add();
			Vector3f aabbMin = stack.allocVector3f(), aabbMax = stack.allocVector3f();
			aabbMin.set(1e30f, 1e30f, 1e30f);
			aabbMax.set(-1e30f, -1e30f, -1e30f);
			VectorUtil.setMin(aabbMin, triangle[0]);
			VectorUtil.setMax(aabbMax, triangle[0]);
			VectorUtil.setMin(aabbMin, triangle[1]);
			VectorUtil.setMax(aabbMax, triangle[1]);
			VectorUtil.setMin(aabbMin, triangle[2]);
			VectorUtil.setMax(aabbMax, triangle[2]);

			// PCK: add these checks for zero dimensions of aabb
			final float MIN_AABB_DIMENSION = 0.002f;
			final float MIN_AABB_HALF_DIMENSION = 0.001f;
			if (aabbMax.x - aabbMin.x < MIN_AABB_DIMENSION) {
				aabbMax.x = (aabbMax.x + MIN_AABB_HALF_DIMENSION);
				aabbMin.x = (aabbMin.x - MIN_AABB_HALF_DIMENSION);
			}
			if (aabbMax.y - aabbMin.y < MIN_AABB_DIMENSION) {
				aabbMax.y = (aabbMax.y + MIN_AABB_HALF_DIMENSION);
				aabbMin.y = (aabbMin.y - MIN_AABB_HALF_DIMENSION);
			}
			if (aabbMax.z - aabbMin.z < MIN_AABB_DIMENSION) {
				aabbMax.z = (aabbMax.z + MIN_AABB_HALF_DIMENSION);
				aabbMin.z = (aabbMin.z - MIN_AABB_HALF_DIMENSION);
			}

			triangleNodes.setQuantizedAabbMin(nodeId, optimizedTree.quantizeWithClamp(aabbMin));
			triangleNodes.setQuantizedAabbMax(nodeId, optimizedTree.quantizeWithClamp(aabbMax));

			triangleNodes.setEscapeIndexOrTriangleIndex(nodeId, (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex);
			stack.leave();
		}
	}
	
	public void build(StridingMeshInterface triangles, boolean useQuantizedAabbCompression, Vector3f _aabbMin, Vector3f _aabbMax) {
		this.useQuantization = useQuantizedAabbCompression;

		// NodeArray	triangleNodes;

		int numLeafNodes = 0;

		if (useQuantization) {
			// initialize quantization values
			setQuantizationValues(_aabbMin, _aabbMax);

			QuantizedNodeTriangleCallback callback = new QuantizedNodeTriangleCallback(quantizedLeafNodes, this);

			triangles.internalProcessAllTriangles(callback, bvhAabbMin, bvhAabbMax);

			// now we have an array of leafnodes in m_leafNodes
			numLeafNodes = quantizedLeafNodes.size();

			quantizedContiguousNodes.resize(2 * numLeafNodes);
		}
		else {
		    Stack stack = Stack.enter();
			NodeTriangleCallback callback = new NodeTriangleCallback(leafNodes);

			Vector3f aabbMin = stack.allocVector3f();
			aabbMin.set(-1e30f, -1e30f, -1e30f);
			Vector3f aabbMax = stack.allocVector3f();
			aabbMax.set(1e30f, 1e30f, 1e30f);

			triangles.internalProcessAllTriangles(callback, aabbMin, aabbMax);

			// now we have an array of leafnodes in m_leafNodes
			numLeafNodes = leafNodes.size();

			// TODO: check
			//contiguousNodes.resize(2*numLeafNodes);
			MiscUtil.resize(contiguousNodes, 2 * numLeafNodes, NEW_OPTIMIZED_BVH_NODE_SUPPLIER);
			stack.leave();
		}

		curNodeIndex = 0;

		buildTree(0, numLeafNodes);

		//  if the entire tree is small then subtree size, we need to create a header info for the tree
		if (useQuantization && SubtreeHeaders.size() == 0) {
			BvhSubtreeInfo subtree = new BvhSubtreeInfo();
			SubtreeHeaders.add(subtree);

			subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, 0);
			subtree.rootNodeIndex = 0;
			subtree.subtreeSize = quantizedContiguousNodes.isLeafNode(0) ? 1 : quantizedContiguousNodes.getEscapeIndex(0);
		}

		// PCK: update the copy of the size
		subtreeHeaderCount = SubtreeHeaders.size();

		// PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
		quantizedLeafNodes.clear();
		leafNodes.clear();
	}
	
	public void refit(StridingMeshInterface meshInterface) {
		if (useQuantization) {
			// calculate new aabb
		    Stack stack = Stack.enter();
			Vector3f aabbMin = stack.allocVector3f(), aabbMax = stack.allocVector3f();
			meshInterface.calculateAabbBruteForce(aabbMin, aabbMax);

			setQuantizationValues(aabbMin, aabbMax);

			updateBvhNodes(meshInterface, 0, curNodeIndex, 0);

			// now update all subtree headers

			int i;
			for (i = 0; i < SubtreeHeaders.size(); i++) {
				BvhSubtreeInfo subtree = SubtreeHeaders.getQuick(i);
				subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, subtree.rootNodeIndex);
			}
			stack.leave();
		}
		else {
			// JAVA NOTE: added for testing, it's too slow for practical use
			build(meshInterface, false, null, null);
		}
	}
	
	public void refitPartial(StridingMeshInterface meshInterface, Vector3f aabbMin, Vector3f aabbMax) {
		throw new UnsupportedOperationException();
//		// incrementally initialize quantization values
//		assert (useQuantization);
//
//		btAssert(aabbMin.getX() > m_bvhAabbMin.getX());
//		btAssert(aabbMin.getY() > m_bvhAabbMin.getY());
//		btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ());
//
//		btAssert(aabbMax.getX() < m_bvhAabbMax.getX());
//		btAssert(aabbMax.getY() < m_bvhAabbMax.getY());
//		btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ());
//
//		///we should update all quantization values, using updateBvhNodes(meshInterface);
//		///but we only update chunks that overlap the given aabb
//
//		unsigned short	quantizedQueryAabbMin[3];
//		unsigned short	quantizedQueryAabbMax[3];
//
//		quantizeWithClamp(&quantizedQueryAabbMin[0],aabbMin);
//		quantizeWithClamp(&quantizedQueryAabbMax[0],aabbMax);
//
//		int i;
//		for (i=0;i<this->m_SubtreeHeaders.size();i++)
//		{
//			btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
//
//			//PCK: unsigned instead of bool
//			unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
//			if (overlap != 0)
//			{
//				updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
//
//				subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
//			}
//		}
	}
	
	public void updateBvhNodes(StridingMeshInterface meshInterface, int firstNode, int endNode, int index) {
		assert (useQuantization);

		int curNodeSubPart = -1;
		Stack stack = Stack.enter();
		
		Vector3f[] triangleVerts/*[3]*/ = new Vector3f[] { stack.allocVector3f(), stack.allocVector3f(), stack.allocVector3f() };
		Vector3f aabbMin = stack.allocVector3f(), aabbMax = stack.allocVector3f();
		Vector3f meshScaling = meshInterface.getScaling(stack.allocVector3f());

		VertexData data = null;

		for (int i = endNode - 1; i >= firstNode; i--) {
			QuantizedBvhNodes curNodes = quantizedContiguousNodes;
			int curNodeId = i;

			if (curNodes.isLeafNode(curNodeId)) {
				// recalc aabb from triangle data
				int nodeSubPart = curNodes.getPartId(curNodeId);
				int nodeTriangleIndex = curNodes.getTriangleIndex(curNodeId);
				if (nodeSubPart != curNodeSubPart) {
					if (curNodeSubPart >= 0) {
						meshInterface.unLockReadOnlyVertexBase(curNodeSubPart);
					}
					data = meshInterface.getLockedReadOnlyVertexIndexBase(nodeSubPart);
				}
				//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,

				data.getTriangle(nodeTriangleIndex*3, meshScaling, triangleVerts);

				aabbMin.set(1e30f, 1e30f, 1e30f);
				aabbMax.set(-1e30f, -1e30f, -1e30f);
				VectorUtil.setMin(aabbMin, triangleVerts[0]);
				VectorUtil.setMax(aabbMax, triangleVerts[0]);
				VectorUtil.setMin(aabbMin, triangleVerts[1]);
				VectorUtil.setMax(aabbMax, triangleVerts[1]);
				VectorUtil.setMin(aabbMin, triangleVerts[2]);
				VectorUtil.setMax(aabbMax, triangleVerts[2]);

				curNodes.setQuantizedAabbMin(curNodeId, quantizeWithClamp(aabbMin));
				curNodes.setQuantizedAabbMax(curNodeId, quantizeWithClamp(aabbMax));
			}
			else {
				// combine aabb from both children

				//quantizedContiguousNodes
				int leftChildNodeId = i + 1;

				int rightChildNodeId = quantizedContiguousNodes.isLeafNode(leftChildNodeId) ? i + 2 : i + 1 + quantizedContiguousNodes.getEscapeIndex(leftChildNodeId);

				for (int i2 = 0; i2 < 3; i2++) {
					curNodes.setQuantizedAabbMin(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMin(leftChildNodeId, i2));
					if (curNodes.getQuantizedAabbMin(curNodeId, i2) > quantizedContiguousNodes.getQuantizedAabbMin(rightChildNodeId, i2)) {
						curNodes.setQuantizedAabbMin(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMin(rightChildNodeId, i2));
					}

					curNodes.setQuantizedAabbMax(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMax(leftChildNodeId, i2));
					if (curNodes.getQuantizedAabbMax(curNodeId, i2) < quantizedContiguousNodes.getQuantizedAabbMax(rightChildNodeId, i2)) {
						curNodes.setQuantizedAabbMax(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMax(rightChildNodeId, i2));
					}
				}
			}
		}

		if (curNodeSubPart >= 0) {
			meshInterface.unLockReadOnlyVertexBase(curNodeSubPart);
		}
		stack.leave();
	}
	
	protected void buildTree(int startIndex, int endIndex) {
		//#ifdef DEBUG_TREE_BUILDING
		if (DEBUG_TREE_BUILDING) {
			gStackDepth++;
			if (gStackDepth > gMaxStackDepth) {
				gMaxStackDepth = gStackDepth;
			}
		}
		//#endif //DEBUG_TREE_BUILDING

		int splitAxis, splitIndex, i;
		int numIndices = endIndex - startIndex;
		int curIndex = curNodeIndex;

		assert (numIndices > 0);

		if (numIndices == 1) {
			//#ifdef DEBUG_TREE_BUILDING
			if (DEBUG_TREE_BUILDING) {
				gStackDepth--;
			}
			//#endif //DEBUG_TREE_BUILDING

			assignInternalNodeFromLeafNode(curNodeIndex, startIndex);

			curNodeIndex++;
			return;
		}
		// calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.

		splitAxis = calcSplittingAxis(startIndex, endIndex);

		splitIndex = sortAndCalcSplittingIndex(startIndex, endIndex, splitAxis);

		int internalNodeIndex = curNodeIndex;

		Stack stack = Stack.enter();
		Vector3f tmp1 = stack.allocVector3f();
		tmp1.set(-1e30f, -1e30f, -1e30f);
		setInternalNodeAabbMax(curNodeIndex, tmp1);
		Vector3f tmp2 = stack.allocVector3f();
		tmp2.set(1e30f, 1e30f, 1e30f);
		setInternalNodeAabbMin(curNodeIndex, tmp2);

		for (i = startIndex; i < endIndex; i++) {
			mergeInternalNodeAabb(curNodeIndex, getAabbMin(i), getAabbMax(i));
		}

		curNodeIndex++;

		//internalNode->m_escapeIndex;

		int leftChildNodexIndex = curNodeIndex;

		//build left child tree
		buildTree(startIndex, splitIndex);

		int rightChildNodexIndex = curNodeIndex;
		// build right child tree
		buildTree(splitIndex, endIndex);

		//#ifdef DEBUG_TREE_BUILDING
		if (DEBUG_TREE_BUILDING) {
			gStackDepth--;
		}
		//#endif //DEBUG_TREE_BUILDING

		int escapeIndex = curNodeIndex - curIndex;

		if (useQuantization) {
			// escapeIndex is the number of nodes of this subtree
			int sizeQuantizedNode = QuantizedBvhNodes.getNodeSize();
			int treeSizeInBytes = escapeIndex * sizeQuantizedNode;
			if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) {
				updateSubtreeHeaders(leftChildNodexIndex, rightChildNodexIndex);
			}
		}

		setInternalNodeEscapeIndex(internalNodeIndex, escapeIndex);
		stack.leave();
	}

	protected boolean testQuantizedAabbAgainstQuantizedAabb(long aabbMin1, long aabbMax1, long aabbMin2, long aabbMax2) {
		int aabbMin1_0 = QuantizedBvhNodes.getCoord(aabbMin1, 0);
		int aabbMin1_1 = QuantizedBvhNodes.getCoord(aabbMin1, 1);
		int aabbMin1_2 = QuantizedBvhNodes.getCoord(aabbMin1, 2);

		int aabbMax1_0 = QuantizedBvhNodes.getCoord(aabbMax1, 0);
		int aabbMax1_1 = QuantizedBvhNodes.getCoord(aabbMax1, 1);
		int aabbMax1_2 = QuantizedBvhNodes.getCoord(aabbMax1, 2);

		int aabbMin2_0 = QuantizedBvhNodes.getCoord(aabbMin2, 0);
		int aabbMin2_1 = QuantizedBvhNodes.getCoord(aabbMin2, 1);
		int aabbMin2_2 = QuantizedBvhNodes.getCoord(aabbMin2, 2);

		int aabbMax2_0 = QuantizedBvhNodes.getCoord(aabbMax2, 0);
		int aabbMax2_1 = QuantizedBvhNodes.getCoord(aabbMax2, 1);
		int aabbMax2_2 = QuantizedBvhNodes.getCoord(aabbMax2, 2);

		boolean overlap = true;
		overlap = (aabbMin1_0 > aabbMax2_0 || aabbMax1_0 < aabbMin2_0) ? false : overlap;
		overlap = (aabbMin1_2 > aabbMax2_2 || aabbMax1_2 < aabbMin2_2) ? false : overlap;
		overlap = (aabbMin1_1 > aabbMax2_1 || aabbMax1_1 < aabbMin2_1) ? false : overlap;
		return overlap;
	}

	protected void updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex) {
		assert (useQuantization);

		//btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex];
		int leftSubTreeSize = quantizedContiguousNodes.isLeafNode(leftChildNodexIndex) ? 1 : quantizedContiguousNodes.getEscapeIndex(leftChildNodexIndex);
		int leftSubTreeSizeInBytes = leftSubTreeSize * QuantizedBvhNodes.getNodeSize();

		//btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex];
		int rightSubTreeSize = quantizedContiguousNodes.isLeafNode(rightChildNodexIndex) ? 1 : quantizedContiguousNodes.getEscapeIndex(rightChildNodexIndex);
		int rightSubTreeSizeInBytes = rightSubTreeSize * QuantizedBvhNodes.getNodeSize();

		if (leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) {
			BvhSubtreeInfo subtree = new BvhSubtreeInfo();
			SubtreeHeaders.add(subtree);

			subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, leftChildNodexIndex);
			subtree.rootNodeIndex = leftChildNodexIndex;
			subtree.subtreeSize = leftSubTreeSize;
		}

		if (rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) {
			BvhSubtreeInfo subtree = new BvhSubtreeInfo();
			SubtreeHeaders.add(subtree);

			subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, rightChildNodexIndex);
			subtree.rootNodeIndex = rightChildNodexIndex;
			subtree.subtreeSize = rightSubTreeSize;
		}

		// PCK: update the copy of the size
		subtreeHeaderCount = SubtreeHeaders.size();
	}
	
	protected int sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis) {
		int i;
		int splitIndex = startIndex;
		int numIndices = endIndex - startIndex;
		float splitValue;
		Stack stack = Stack.enter();
		Vector3f means = stack.allocVector3f();
		means.set(0f, 0f, 0f);
		Vector3f center = stack.allocVector3f();
		for (i = startIndex; i < endIndex; i++) {
			center.add(getAabbMax(i), getAabbMin(i));
			center.scale(0.5f);
			means.add(center);
		}
		means.scale(1f / (float) numIndices);

		splitValue = VectorUtil.getCoord(means, splitAxis);

		//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
		for (i = startIndex; i < endIndex; i++) {
			//Vector3f center = new Vector3f();
			center.add(getAabbMax(i), getAabbMin(i));
			center.scale(0.5f);

			if (VectorUtil.getCoord(center, splitAxis) > splitValue) {
				// swap
				swapLeafNodes(i, splitIndex);
				splitIndex++;
			}
		}

		// if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
		// otherwise the tree-building might fail due to stack-overflows in certain cases.
		// unbalanced1 is unsafe: it can cause stack overflows
		// bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));

		// unbalanced2 should work too: always use center (perfect balanced trees)	
		// bool unbalanced2 = true;

		// this should be safe too:
		int rangeBalancedIndices = numIndices / 3;
		boolean unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));

		if (unbalanced) {
			splitIndex = startIndex + (numIndices >> 1);
		}

		boolean unbal = (splitIndex == startIndex) || (splitIndex == (endIndex));
		assert (!unbal);
		
		stack.leave();
		return splitIndex;
	}

	protected int calcSplittingAxis(int startIndex, int endIndex) {
		int i;
		Stack stack = Stack.enter();
		Vector3f means = stack.allocVector3f();
		means.set(0f, 0f, 0f);
		Vector3f variance = stack.allocVector3f();
		variance.set(0f, 0f, 0f);
		int numIndices = endIndex - startIndex;

		Vector3f center = stack.allocVector3f();
		for (i = startIndex; i < endIndex; i++) {
			center.add(getAabbMax(i), getAabbMin(i));
			center.scale(0.5f);
			means.add(center);
		}
		means.scale(1f / (float) numIndices);

		Vector3f diff2 = stack.allocVector3f();
		for (i = startIndex; i < endIndex; i++) {
			center.add(getAabbMax(i), getAabbMin(i));
			center.scale(0.5f);
			diff2.sub(center, means);
			//diff2 = diff2 * diff2;
			VectorUtil.mul(diff2, diff2, diff2);
			variance.add(diff2);
		}
		variance.scale(1f / ((float) numIndices - 1));

		int result = VectorUtil.maxAxis(variance);
		stack.leave();
		return result;
	}

	public void reportAabbOverlappingNodex(NodeOverlapCallback nodeCallback, Vector3f aabbMin, Vector3f aabbMax) {
		// either choose recursive traversal (walkTree) or stackless (walkStacklessTree)

		if (useQuantization) {
			// quantize query AABB
			long quantizedQueryAabbMin;
			long quantizedQueryAabbMax;
			quantizedQueryAabbMin = quantizeWithClamp(aabbMin);
			quantizedQueryAabbMax = quantizeWithClamp(aabbMax);

			// JAVA TODO:
			switch (traversalMode) {
				case STACKLESS:
					walkStacklessQuantizedTree(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax, 0, curNodeIndex);
					break;
					
//				case STACKLESS_CACHE_FRIENDLY:
//					walkStacklessQuantizedTreeCacheFriendly(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
//					break;
					
				case RECURSIVE:
					walkRecursiveQuantizedTreeAgainstQueryAabb(quantizedContiguousNodes, 0, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
					break;
					
				default:
					assert (false); // unsupported
			}
		}
		else {
			walkStacklessTree(nodeCallback, aabbMin, aabbMax);
		}
	}
	
	protected void walkStacklessTree(NodeOverlapCallback nodeCallback, Vector3f aabbMin, Vector3f aabbMax) {
		assert (!useQuantization);

		// JAVA NOTE: rewritten
		OptimizedBvhNode rootNode = null;//contiguousNodes.get(0);
		int rootNode_index = 0;

		int escapeIndex, curIndex = 0;
		int walkIterations = 0;
		boolean isLeafNode;
		//PCK: unsigned instead of bool
		//unsigned aabbOverlap;
		boolean aabbOverlap;

		while (curIndex < curNodeIndex) {
			// catch bugs in tree data
			assert (walkIterations < curNodeIndex);

			walkIterations++;

			rootNode = contiguousNodes.getQuick(rootNode_index);

			aabbOverlap = AabbUtil2.testAabbAgainstAabb2(aabbMin, aabbMax, rootNode.aabbMinOrg, rootNode.aabbMaxOrg);
			isLeafNode = (rootNode.escapeIndex == -1);

			// PCK: unsigned instead of bool
			if (isLeafNode && (aabbOverlap/* != 0*/)) {
				nodeCallback.processNode(rootNode.subPart, rootNode.triangleIndex);
			}

			rootNode = null;

			//PCK: unsigned instead of bool
			if ((aabbOverlap/* != 0*/) || isLeafNode) {
				rootNode_index++;
				curIndex++;
			}
			else {
				escapeIndex = /*rootNode*/ contiguousNodes.getQuick(rootNode_index).escapeIndex;
				rootNode_index += escapeIndex;
				curIndex += escapeIndex;
			}
		}
		if (maxIterations < walkIterations) {
			maxIterations = walkIterations;
		}
	}

	protected void walkRecursiveQuantizedTreeAgainstQueryAabb(QuantizedBvhNodes currentNodes, int currentNodeId, NodeOverlapCallback nodeCallback, long quantizedQueryAabbMin, long quantizedQueryAabbMax) {
		assert (useQuantization);

		boolean isLeafNode;
		boolean aabbOverlap;

		aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, currentNodes.getQuantizedAabbMin(currentNodeId), currentNodes.getQuantizedAabbMax(currentNodeId));
		isLeafNode = currentNodes.isLeafNode(currentNodeId);

		if (aabbOverlap) {
			if (isLeafNode) {
				nodeCallback.processNode(currentNodes.getPartId(currentNodeId), currentNodes.getTriangleIndex(currentNodeId));
			}
			else {
				// process left and right children
				int leftChildNodeId = currentNodeId + 1;
				walkRecursiveQuantizedTreeAgainstQueryAabb(currentNodes, leftChildNodeId, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);

				int rightChildNodeId = currentNodes.isLeafNode(leftChildNodeId) ? leftChildNodeId + 1 : leftChildNodeId + currentNodes.getEscapeIndex(leftChildNodeId);
				walkRecursiveQuantizedTreeAgainstQueryAabb(currentNodes, rightChildNodeId, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
			}
		}
	}

	protected void walkStacklessQuantizedTreeAgainstRay(NodeOverlapCallback nodeCallback, Vector3f raySource, Vector3f rayTarget, Vector3f aabbMin, Vector3f aabbMax, int startNodeIndex, int endNodeIndex) {
		assert (useQuantization);
		Stack stack = Stack.enter();
		Vector3f tmp = stack.allocVector3f();

		int curIndex = startNodeIndex;
		int walkIterations = 0;
		int subTreeSize = endNodeIndex - startNodeIndex;

		QuantizedBvhNodes rootNode = quantizedContiguousNodes;
		int rootNode_idx = startNodeIndex;
		int escapeIndex;

		boolean isLeafNode;
		boolean boxBoxOverlap = false;
		boolean rayBoxOverlap = false;

		float lambda_max = 1f;
		//#define RAYAABB2
		//#ifdef RAYAABB2
		Vector3f rayFrom = stack.alloc(raySource);
		Vector3f rayDirection = stack.allocVector3f();
		tmp.sub(rayTarget, raySource);
		rayDirection.normalize(tmp);
		lambda_max = rayDirection.dot(tmp);
		rayDirection.x = 1f / rayDirection.x;
		rayDirection.y = 1f / rayDirection.y;
		rayDirection.z = 1f / rayDirection.z;
//		boolean sign_x = rayDirection.x < 0f;
//		boolean sign_y = rayDirection.y < 0f;
//		boolean sign_z = rayDirection.z < 0f;
		//#endif

		/* Quick pruning by quantized box */
		Vector3f rayAabbMin = stack.alloc(raySource);
		Vector3f rayAabbMax = stack.alloc(raySource);
		VectorUtil.setMin(rayAabbMin, rayTarget);
		VectorUtil.setMax(rayAabbMax, rayTarget);

		/* Add box cast extents to bounding box */
		rayAabbMin.add(aabbMin);
		rayAabbMax.add(aabbMax);

		long quantizedQueryAabbMin;
		long quantizedQueryAabbMax;
		quantizedQueryAabbMin = quantizeWithClamp(rayAabbMin);
		quantizedQueryAabbMax = quantizeWithClamp(rayAabbMax);

		Vector3f bounds_0 = stack.allocVector3f();
		Vector3f bounds_1 = stack.allocVector3f();
		Vector3f normal = stack.allocVector3f();
		float[] param = new float[1];

		while (curIndex < endNodeIndex) {
			
			//#define VISUALLY_ANALYZE_BVH 1
			//#ifdef VISUALLY_ANALYZE_BVH
			//		//some code snippet to debugDraw aabb, to visually analyze bvh structure
			//		static int drawPatch = 0;
			//		//need some global access to a debugDrawer
			//		extern btIDebugDraw* debugDrawerPtr;
			//		if (curIndex==drawPatch)
			//		{
			//			btVector3 aabbMin,aabbMax;
			//			aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
			//			aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
			//			btVector3	color(1,0,0);
			//			debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
			//		}
			//#endif//VISUALLY_ANALYZE_BVH

			// catch bugs in tree data
			assert (walkIterations < subTreeSize);

			walkIterations++;
			// only interested if this is closer than any previous hit
			param[0] = 1f;
			rayBoxOverlap = false;
			boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.getQuantizedAabbMin(rootNode_idx), rootNode.getQuantizedAabbMax(rootNode_idx));
			isLeafNode = rootNode.isLeafNode(rootNode_idx);
			if (boxBoxOverlap) {
				unQuantize(bounds_0, rootNode.getQuantizedAabbMin(rootNode_idx));
				unQuantize(bounds_1, rootNode.getQuantizedAabbMax(rootNode_idx));
				/* Add box cast extents */
				bounds_0.add(aabbMin);
				bounds_1.add(aabbMax);
				//#if 0
				//			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
				//#ifdef RAYAABB2
				//			rayBoxOverlap = AabbUtil2.rayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max);
				//#else
				rayBoxOverlap = AabbUtil2.rayAabb(raySource, rayTarget, bounds_0, bounds_1, param, normal);
				//#endif
			}

			if (isLeafNode && rayBoxOverlap) {
				nodeCallback.processNode(rootNode.getPartId(rootNode_idx), rootNode.getTriangleIndex(rootNode_idx));
			}

			if (rayBoxOverlap || isLeafNode) {
				rootNode_idx++;
				curIndex++;
			}
			else {
				escapeIndex = rootNode.getEscapeIndex(rootNode_idx);
				rootNode_idx += escapeIndex;
				curIndex += escapeIndex;
			}
		}
		
		if (maxIterations < walkIterations) {
			maxIterations = walkIterations;
		}
		stack.leave();
	}

	protected void walkStacklessQuantizedTree(NodeOverlapCallback nodeCallback, long quantizedQueryAabbMin, long quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) {
		assert (useQuantization);

		int curIndex = startNodeIndex;
		int walkIterations = 0;
		int subTreeSize = endNodeIndex - startNodeIndex;

		QuantizedBvhNodes rootNode = quantizedContiguousNodes;
		int rootNode_idx = startNodeIndex;
		int escapeIndex;

		boolean isLeafNode;
		boolean aabbOverlap;

		while (curIndex < endNodeIndex) {
			////#define VISUALLY_ANALYZE_BVH 1
			//#ifdef VISUALLY_ANALYZE_BVH
			////some code snippet to debugDraw aabb, to visually analyze bvh structure
			//static int drawPatch = 0;
			////need some global access to a debugDrawer
			//extern btIDebugDraw* debugDrawerPtr;
			//if (curIndex==drawPatch)
			//{
			//	btVector3 aabbMin,aabbMax;
			//	aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
			//	aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
			//	btVector3	color(1,0,0);
			//	debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
			//}
			//#endif//VISUALLY_ANALYZE_BVH

			// catch bugs in tree data
			assert (walkIterations < subTreeSize);

			walkIterations++;
			aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.getQuantizedAabbMin(rootNode_idx), rootNode.getQuantizedAabbMax(rootNode_idx));
			isLeafNode = rootNode.isLeafNode(rootNode_idx);

			if (isLeafNode && aabbOverlap) {
				nodeCallback.processNode(rootNode.getPartId(rootNode_idx), rootNode.getTriangleIndex(rootNode_idx));
			}

			if (aabbOverlap || isLeafNode) {
				rootNode_idx++;
				curIndex++;
			}
			else {
				escapeIndex = rootNode.getEscapeIndex(rootNode_idx);
				rootNode_idx += escapeIndex;
				curIndex += escapeIndex;
			}
		}

		if (maxIterations < walkIterations) {
			maxIterations = walkIterations;
		}
	}
	
	public void reportRayOverlappingNodex(NodeOverlapCallback nodeCallback, Vector3f raySource, Vector3f rayTarget) {
		boolean fast_path = useQuantization && traversalMode == TraversalMode.STACKLESS;
		Stack stack = Stack.enter();
		if (fast_path) {
			Vector3f tmp = stack.allocVector3f();
			tmp.set(0f, 0f, 0f);
			walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, tmp, tmp, 0, curNodeIndex);
		}
		else {
			/* Otherwise fallback to AABB overlap test */
			Vector3f aabbMin = stack.alloc(raySource);
			Vector3f aabbMax = stack.alloc(raySource);
			VectorUtil.setMin(aabbMin, rayTarget);
			VectorUtil.setMax(aabbMax, rayTarget);
			reportAabbOverlappingNodex(nodeCallback, aabbMin, aabbMax);
		}
		stack.leave();
	}

	public void reportBoxCastOverlappingNodex(NodeOverlapCallback nodeCallback, Vector3f raySource, Vector3f rayTarget, Vector3f aabbMin, Vector3f aabbMax) {
		boolean fast_path = useQuantization && traversalMode == TraversalMode.STACKLESS;
		if (fast_path) {
			walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, curNodeIndex);
		}
		else {
		    Stack stack = Stack.enter();
			/* Slow path:
			Construct the bounding box for the entire box cast and send that down the tree */
			Vector3f qaabbMin = stack.alloc(raySource);
			Vector3f qaabbMax = stack.alloc(raySource);
			VectorUtil.setMin(qaabbMin, rayTarget);
			VectorUtil.setMax(qaabbMax, rayTarget);
			qaabbMin.add(aabbMin);
			qaabbMax.add(aabbMax);
			reportAabbOverlappingNodex(nodeCallback, qaabbMin, qaabbMax);
			stack.leave();
		}
	}
	
	public long quantizeWithClamp(Vector3f point) {
		assert (useQuantization);
		Stack stack = Stack.enter();

		Vector3f clampedPoint = stack.alloc(point);
		VectorUtil.setMax(clampedPoint, bvhAabbMin);
		VectorUtil.setMin(clampedPoint, bvhAabbMax);

		Vector3f v = stack.allocVector3f();
		v.sub(clampedPoint, bvhAabbMin);
		VectorUtil.mul(v, v, bvhQuantization);

		int out0 = (int)(v.x + 0.5f) & 0xFFFF;
		int out1 = (int)(v.y + 0.5f) & 0xFFFF;
		int out2 = (int)(v.z + 0.5f) & 0xFFFF;

		stack.leave();
		return ((long)out0) | (((long)out1) << 16) | (((long)out2) << 32);
	}
	
	public void unQuantize(Vector3f vecOut, long vecIn) {
		int vecIn0 = (int)((vecIn & 0x00000000FFFFL));
		int vecIn1 = (int)((vecIn & 0x0000FFFF0000L) >>> 16);
		int vecIn2 = (int)((vecIn & 0xFFFF00000000L) >>> 32);

		vecOut.x = (float)vecIn0 / (bvhQuantization.x);
		vecOut.y = (float)vecIn1 / (bvhQuantization.y);
		vecOut.z = (float)vecIn2 / (bvhQuantization.z);

		vecOut.add(bvhAabbMin);
	}
	
}