gt: 		int numIndices = endIndex - startIndex;
context: /*
   * 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:
   *
   * 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 spacegraph.space3d.phys.shape;
  
  import jcog.data.list.FasterList;
  import jcog.math.v3;
  import spacegraph.space3d.phys.math.AabbUtil2;
  import spacegraph.space3d.phys.math.MiscUtil;
  import spacegraph.space3d.phys.math.VectorUtil;
  
  import java.io.Serializable;
  
  
  
  /**
   * 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 static final long serialVersionUID = 1L;
  
  	
  	
  	private static final boolean DEBUG_TREE_BUILDING = false;
  	private static int gStackDepth;
  	private static int gMaxStackDepth;
  	
  	private static int maxIterations;
  	
  	
  	private static final int MAX_SUBTREE_SIZE_IN_BYTES = 2048;
  
  	
  	
  	public static final int MAX_NUM_PARTS_IN_BITS = 10;
  
  	
  
  	private final FasterList<OptimizedBvhNode> leafNodes = new FasterList<>();
  	private final FasterList<OptimizedBvhNode> contiguousNodes = new FasterList<>();
  
  	private final QuantizedBvhNodes quantizedLeafNodes = new QuantizedBvhNodes();
  	private final QuantizedBvhNodes quantizedContiguousNodes = new QuantizedBvhNodes();
  	
  	private int curNodeIndex;
  
  	
  	private boolean useQuantization;
  	private final v3 bvhAabbMin = new v3();
  	private final v3 bvhAabbMax = new v3();
  	private final v3 bvhQuantization = new v3();
  	
  	private final TraversalMode traversalMode = TraversalMode.STACKLESS;
  	private final FasterList<BvhSubtreeInfo> SubtreeHeaders = new FasterList<>();
  	
  	private int subtreeHeaderCount;
  
  	
  	
  	private void setInternalNodeAabbMin(int nodeIndex, v3 aabbMin) {
  		if (useQuantization) {
  			quantizedContiguousNodes.setQuantizedAabbMin(nodeIndex, quantizeWithClamp(aabbMin));
  		}
  		else {
  			
  			contiguousNodes.get(nodeIndex).aabbMinOrg.set(aabbMin);
  		}
  	}
  
  	private void setInternalNodeAabbMax(int nodeIndex, v3 aabbMax) {
  		if (useQuantization) {
  			quantizedContiguousNodes.setQuantizedAabbMax(nodeIndex, quantizeWithClamp(aabbMax));
  		}
  		else {
  			
  			contiguousNodes.get(nodeIndex).aabbMaxOrg.set(aabbMax);
  		}
  	}
  
  	private v3 getAabbMin(int nodeIndex) {
  		if (useQuantization) {
  			v3 tmp = new v3();
  			unQuantize(tmp, quantizedLeafNodes.getQuantizedAabbMin(nodeIndex));
  			return tmp;
  		}
  
  		
  		
  		return leafNodes.get(nodeIndex).aabbMinOrg;
  	}
  
  	private v3 getAabbMax(int nodeIndex) {
  		if (useQuantization) {
  			v3 tmp = new v3();
  			unQuantize(tmp, quantizedLeafNodes.getQuantizedAabbMax(nodeIndex));
  			return tmp;
  		}
  
  		
  		
  		return leafNodes.get(nodeIndex).aabbMaxOrg;
  	}
  
  	private void setQuantizationValues(v3 aabbMin, v3 aabbMax) {
  		setQuantizationValues(aabbMin, aabbMax, 1f);
  	}
  
  	private void setQuantizationValues(v3 aabbMin, v3 aabbMax, float quantizationMargin) {
  		
  		v3 clampValue = new v3();
  		clampValue.set(quantizationMargin,quantizationMargin,quantizationMargin);
  		bvhAabbMin.sub(aabbMin, clampValue);
  		bvhAabbMax.add(aabbMax, clampValue);
  		v3 aabbSize = new v3();
  		aabbSize.sub(bvhAabbMax, bvhAabbMin);
  		bvhQuantization.set(65535f, 65535f, 65535f);
  		VectorUtil.div(bvhQuantization, bvhQuantization, aabbSize);
  	}
  
  	private void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) {
  		if (useQuantization) {
  			quantizedContiguousNodes.setEscapeIndexOrTriangleIndex(nodeIndex, -escapeIndex);
  		}
  		else {
  			
  			contiguousNodes.get(nodeIndex).escapeIndex = escapeIndex;
  		}
  	}
  
  	private void mergeInternalNodeAabb(int nodeIndex, v3 newAabbMin, v3 newAabbMax) {
  		if (useQuantization) {
  
  			long quantizedAabbMin = quantizeWithClamp(newAabbMin);
  			long quantizedAabbMax = quantizeWithClamp(newAabbMax);
  			for (int i = 0; i < 3; i++) {
  				int n = QuantizedBvhNodes.getCoord(quantizedAabbMin, i);
  				if (quantizedContiguousNodes.getQuantizedAabbMin(nodeIndex, i) > n) {
  					quantizedContiguousNodes.setQuantizedAabbMin(nodeIndex, i, n);
  				}
  
  				int m = QuantizedBvhNodes.getCoord(quantizedAabbMax, i);
  				if (quantizedContiguousNodes.getQuantizedAabbMax(nodeIndex, i) < m) {
  					quantizedContiguousNodes.setQuantizedAabbMax(nodeIndex, i, m);
  				}
  			}
  		}
  		else {
  			
  			
  			OptimizedBvhNode cn = contiguousNodes.get(nodeIndex);
  			VectorUtil.setMin(cn.aabbMinOrg, newAabbMin);
  			
  			VectorUtil.setMax(cn.aabbMaxOrg, newAabbMax);
  		}
  	}
  
  	private void swapLeafNodes(int i, int splitIndex) {
  		if (useQuantization) {
  			quantizedLeafNodes.swap(i, splitIndex);
  		}
  		else {
  			
  			
  			OptimizedBvhNode tmp = leafNodes.get(i);
  			
  			leafNodes.setFast(i, leafNodes.get(splitIndex));
  			leafNodes.setFast(splitIndex, tmp);
  		}
  	}
  
  	private void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex) {
  		if (useQuantization) {
  			quantizedContiguousNodes.set(internalNode, quantizedLeafNodes, leafNodeIndex);
  		}
  		else {
  			
  			
  			contiguousNodes.get(internalNode).set(leafNodes.get(leafNodeIndex));
  		}
  	}
  
  	private static class NodeTriangleCallback extends InternalTriangleIndexCallback {
  		final FasterList<OptimizedBvhNode> triangleNodes;
  
  		NodeTriangleCallback(FasterList<OptimizedBvhNode> triangleNodes) {
  			this.triangleNodes = triangleNodes;
  		}
  
  		private final v3 aabbMin = new v3(), aabbMax = new v3();
  
  		@Override
          public void internalProcessTriangleIndex(v3[] 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]);
  
  			
  			node.aabbMinOrg.set(aabbMin);
  			node.aabbMaxOrg.set(aabbMax);
  
  			node.escapeIndex = -1;
  
  			
  			node.subPart = partId;
  			node.triangleIndex = triangleIndex;
  			triangleNodes.add(node);
  		}
  	}
  
  	private static class QuantizedNodeTriangleCallback extends InternalTriangleIndexCallback {
  		
  
  		final QuantizedBvhNodes triangleNodes;
  		final OptimizedBvh optimizedTree;
  
  		QuantizedNodeTriangleCallback(QuantizedBvhNodes triangleNodes, OptimizedBvh tree) {
  			this.triangleNodes = triangleNodes;
  			this.optimizedTree = tree;
  		}
  
  		@Override
          public void internalProcessTriangleIndex(v3[] triangle, int partId, int triangleIndex) {
  			
  			assert (partId < (1 << MAX_NUM_PARTS_IN_BITS));
  			assert (triangleIndex < (1 << (31 - MAX_NUM_PARTS_IN_BITS)));
  			
  			assert (triangleIndex >= 0);
  
  			int nodeId = triangleNodes.add();
  			v3 aabbMin = new v3(), aabbMax = new v3();
  			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]);
  
  			
  			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);
  		}
  	}
  
  	public void build(StridingMeshInterface triangles, boolean useQuantizedAabbCompression, v3 _aabbMin, v3 _aabbMax) {
  		this.useQuantization = useQuantizedAabbCompression;
  
  		
  
  		int numLeafNodes = 0;
  
  		if (useQuantization) {
  			
  			setQuantizationValues(_aabbMin, _aabbMax);
  
  			QuantizedNodeTriangleCallback callback = new QuantizedNodeTriangleCallback(quantizedLeafNodes, this);
  
  			triangles.internalProcessAllTriangles(callback, bvhAabbMin, bvhAabbMax);
  
  			
  			numLeafNodes = quantizedLeafNodes.size();
  
  			quantizedContiguousNodes.resize(2 * numLeafNodes);
  		}
  		else {
  			NodeTriangleCallback callback = new NodeTriangleCallback(leafNodes);
  
  			v3 aabbMin = new v3();
  			aabbMin.set(-1e30f, -1e30f, -1e30f);
  			v3 aabbMax = new v3();
  			aabbMax.set(1e30f, 1e30f, 1e30f);
  
  			triangles.internalProcessAllTriangles(callback, aabbMin, aabbMax);
  
  			
  			numLeafNodes = leafNodes.size();
  
  			
  			
  			MiscUtil.resize(contiguousNodes, 2 * numLeafNodes, OptimizedBvhNode.class);
  		}
  
  		curNodeIndex = 0;
  
  		buildTree(0, numLeafNodes);
  
  		
  		if (useQuantization && SubtreeHeaders.isEmpty()) {
  			BvhSubtreeInfo subtree = new BvhSubtreeInfo();
  			SubtreeHeaders.add(subtree);
  
  			subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, 0);
  			subtree.rootNodeIndex = 0;
  			subtree.subtreeSize = quantizedContiguousNodes.isLeafNode(0) ? 1 : quantizedContiguousNodes.getEscapeIndex(0);
  		}
  
  		
  		subtreeHeaderCount = SubtreeHeaders.size();
  
  		
  		quantizedLeafNodes.clear();
  		leafNodes.clearFast();
  	}
  
  	public void refit(StridingMeshInterface meshInterface) {
  		if (useQuantization) {
  			
  			v3 aabbMin = new v3(), aabbMax = new v3();
  			meshInterface.calculateAabbBruteForce(aabbMin, aabbMax);
  
  			setQuantizationValues(aabbMin, aabbMax);
  
  			updateBvhNodes(meshInterface, 0, curNodeIndex, 0);
  
  			
  
  			int i;
  			for (i = 0; i < SubtreeHeaders.size(); i++) {
  				
  				BvhSubtreeInfo subtree = SubtreeHeaders.get(i);
  				subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, subtree.rootNodeIndex);
  			}
  
  		}
  		else {
  			
  			build(meshInterface, false, null, null);
  		}
  	}
  
  	public static void refitPartial(StridingMeshInterface meshInterface, v3 aabbMin, v3 aabbMax) {
  		throw new UnsupportedOperationException();
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  	}
  
  	private void updateBvhNodes(StridingMeshInterface meshInterface, int firstNode, int endNode, int index) {
  		assert (useQuantization);
  
  		int curNodeSubPart = -1;
  
  		v3[] triangleVerts/*[3]*/ = { new v3(), new v3(), new v3() };
  		v3 aabbMin = new v3(), aabbMax = new v3();
  		v3 meshScaling = meshInterface.getScaling(new v3());
  
  		VertexData data = null;
  
  		for (int i = endNode - 1; i >= firstNode; i--) {
  			QuantizedBvhNodes curNodes = quantizedContiguousNodes;
  			int curNodeId = i;
  
  			if (curNodes.isLeafNode(curNodeId)) {
  				
  				int nodeSubPart = curNodes.getPartId(curNodeId);
  				int nodeTriangleIndex = curNodes.getTriangleIndex(curNodeId);
  				if (nodeSubPart != curNodeSubPart) {
  					if (curNodeSubPart >= 0) {
  						meshInterface.unLockReadOnlyVertexBase(curNodeSubPart);
  					}
  					data = meshInterface.getLockedReadOnlyVertexIndexBase(nodeSubPart);
  				}
  				
  
  				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 {
  				
  
  				
  				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);
  		}
  	}
  
  	private void buildTree(int startIndex, int endIndex) {
  		
  		if (DEBUG_TREE_BUILDING) {
  			gStackDepth++;
  			if (gStackDepth > gMaxStackDepth) {
  				gMaxStackDepth = gStackDepth;
  			}
  		}
  		
  
  		int splitAxis, splitIndex, i;
  