pbox2d 0.6.0-java → 0.8.0-java

data/src/org/jbox2d/collision/broadphase/DynamicTree.java

@@ -0,0 +1,883 @@
+/*******************************************************************************
+ * Copyright (c) 2013, Daniel Murphy
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *  * Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ ******************************************************************************/
+package org.jbox2d.collision.broadphase;
+
+import org.jbox2d.callbacks.DebugDraw;
+import org.jbox2d.callbacks.TreeCallback;
+import org.jbox2d.callbacks.TreeRayCastCallback;
+import org.jbox2d.collision.AABB;
+import org.jbox2d.collision.RayCastInput;
+import org.jbox2d.common.Color3f;
+import org.jbox2d.common.MathUtils;
+import org.jbox2d.common.Settings;
+import org.jbox2d.common.Vec2;
+
+/**
+ * A dynamic tree arranges data in a binary tree to accelerate queries such as volume queries and
+ * ray casts. Leafs are proxies with an AABB. In the tree we expand the proxy AABB by _fatAABBFactor
+ * so that the proxy AABB is bigger than the client object. This allows the client object to move by
+ * small amounts without triggering a tree update.
+ * 
+ * @author daniel
+ */
+public class DynamicTree implements BroadPhaseStrategy {
+  public static final int MAX_STACK_SIZE = 64;
+  public static final int NULL_NODE = -1;
+
+  private DynamicTreeNode m_root;
+  private DynamicTreeNode[] m_nodes;
+  private int m_nodeCount;
+  private int m_nodeCapacity;
+
+  private int m_freeList;
+
+  private final Vec2[] drawVecs = new Vec2[4];
+  private DynamicTreeNode[] nodeStack = new DynamicTreeNode[20];
+  private int nodeStackIndex = 0;
+
+  public DynamicTree() {
+    m_root = null;
+    m_nodeCount = 0;
+    m_nodeCapacity = 16;
+    m_nodes = new DynamicTreeNode[16];
+
+    // Build a linked list for the free list.
+    for (int i = m_nodeCapacity - 1; i >= 0; i--) {
+      m_nodes[i] = new DynamicTreeNode(i);
+      m_nodes[i].parent = (i == m_nodeCapacity - 1) ? null : m_nodes[i + 1];
+      m_nodes[i].height = -1;
+    }
+    m_freeList = 0;
+
+    for (int i = 0; i < drawVecs.length; i++) {
+      drawVecs[i] = new Vec2();
+    }
+  }
+
+  @Override
+  public final int createProxy(final AABB aabb, Object userData) {
+    assert(aabb.isValid());
+    final DynamicTreeNode node = allocateNode();
+    int proxyId = node.id;
+    // Fatten the aabb
+    final AABB nodeAABB = node.aabb;
+    nodeAABB.lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension;
+    nodeAABB.lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension;
+    nodeAABB.upperBound.x = aabb.upperBound.x + Settings.aabbExtension;
+    nodeAABB.upperBound.y = aabb.upperBound.y + Settings.aabbExtension;
+    node.userData = userData;
+
+    insertLeaf(proxyId);
+
+    return proxyId;
+  }
+
+  @Override
+  public final void destroyProxy(int proxyId) {
+    assert (0 <= proxyId && proxyId < m_nodeCapacity);
+    DynamicTreeNode node = m_nodes[proxyId];
+    assert (node.child1 == null);
+
+    removeLeaf(node);
+    freeNode(node);
+  }
+
+  @Override
+  public final boolean moveProxy(int proxyId, final AABB aabb, Vec2 displacement) {
+    assert(aabb.isValid());
+    assert (0 <= proxyId && proxyId < m_nodeCapacity);
+    final DynamicTreeNode node = m_nodes[proxyId];
+    assert (node.child1 == null);
+
+    final AABB nodeAABB = node.aabb;
+    // if (nodeAABB.contains(aabb)) {
+    if (nodeAABB.lowerBound.x <= aabb.lowerBound.x && nodeAABB.lowerBound.y <= aabb.lowerBound.y
+        && aabb.upperBound.x <= nodeAABB.upperBound.x && aabb.upperBound.y <= nodeAABB.upperBound.y) {
+      return false;
+    }
+
+    removeLeaf(node);
+
+    // Extend AABB
+    final Vec2 lowerBound = nodeAABB.lowerBound;
+    final Vec2 upperBound = nodeAABB.upperBound;
+    lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension;
+    lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension;
+    upperBound.x = aabb.upperBound.x + Settings.aabbExtension;
+    upperBound.y = aabb.upperBound.y + Settings.aabbExtension;
+
+    // Predict AABB displacement.
+    final float dx = displacement.x * Settings.aabbMultiplier;
+    final float dy = displacement.y * Settings.aabbMultiplier;
+    if (dx < 0.0f) {
+      lowerBound.x += dx;
+    } else {
+      upperBound.x += dx;
+    }
+
+    if (dy < 0.0f) {
+      lowerBound.y += dy;
+    } else {
+      upperBound.y += dy;
+    }
+
+    insertLeaf(proxyId);
+    return true;
+  }
+
+  @Override
+  public final Object getUserData(int proxyId) {
+    assert (0 <= proxyId && proxyId < m_nodeCapacity);
+    return m_nodes[proxyId].userData;
+  }
+
+  @Override
+  public final AABB getFatAABB(int proxyId) {
+    assert (0 <= proxyId && proxyId < m_nodeCapacity);
+    return m_nodes[proxyId].aabb;
+  }
+
+  @Override
+  public final void query(TreeCallback callback, AABB aabb) {
+    assert(aabb.isValid());
+    nodeStackIndex = 0;
+    nodeStack[nodeStackIndex++] = m_root;
+
+    while (nodeStackIndex > 0) {
+      DynamicTreeNode node = nodeStack[--nodeStackIndex];
+      if (node == null) {
+        continue;
+      }
+
+      if (AABB.testOverlap(node.aabb, aabb)) {
+        if (node.child1 == null) {
+          boolean proceed = callback.treeCallback(node.id);
+          if (!proceed) {
+            return;
+          }
+        } else {
+          if (nodeStack.length - nodeStackIndex - 2 <= 0) {
+            DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.length * 2];
+            System.arraycopy(nodeStack, 0, newBuffer, 0, nodeStack.length);
+            nodeStack = newBuffer;
+          }
+          nodeStack[nodeStackIndex++] = node.child1;
+          nodeStack[nodeStackIndex++] = node.child2;
+        }
+      }
+    }
+  }
+
+  private final Vec2 r = new Vec2();
+  private final AABB aabb = new AABB();
+  private final RayCastInput subInput = new RayCastInput();
+
+  @Override
+  public void raycast(TreeRayCastCallback callback, RayCastInput input) {
+    final Vec2 p1 = input.p1;
+    final Vec2 p2 = input.p2;
+    float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
+    float vx, vy;
+    float rx, ry;
+    float absVx, absVy;
+    float cx, cy;
+    float hx, hy;
+    float tempx, tempy;
+    r.x = p2x - p1x;
+    r.y = p2y - p1y;
+    assert ((r.x * r.x + r.y * r.y) > 0f);
+    r.normalize();
+    rx = r.x;
+    ry = r.y;
+
+    // v is perpendicular to the segment.
+    vx = -1f * ry;
+    vy = 1f * rx;
+    absVx = MathUtils.abs(vx);
+    absVy = MathUtils.abs(vy);
+
+    // Separating axis for segment (Gino, p80).
+    // |dot(v, p1 - c)| > dot(|v|, h)
+
+    float maxFraction = input.maxFraction;
+
+    // Build a bounding box for the segment.
+    final AABB segAABB = aabb;
+    // Vec2 t = p1 + maxFraction * (p2 - p1);
+    // before inline
+    // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
+    // Vec2.minToOut(p1, temp, segAABB.lowerBound);
+    // Vec2.maxToOut(p1, temp, segAABB.upperBound);
+    tempx = (p2x - p1x) * maxFraction + p1x;
+    tempy = (p2y - p1y) * maxFraction + p1y;
+    segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
+    segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
+    segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
+    segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
+    // end inline
+
+    nodeStackIndex = 0;
+    nodeStack[nodeStackIndex++] = m_root;
+    while (nodeStackIndex > 0) {
+      final DynamicTreeNode node = nodeStack[--nodeStackIndex];
+      if (node == null) {
+        continue;
+      }
+
+      final AABB nodeAABB = node.aabb;
+      if (!AABB.testOverlap(nodeAABB, segAABB)) {
+        continue;
+      }
+
+      // Separating axis for segment (Gino, p80).
+      // |dot(v, p1 - c)| > dot(|v|, h)
+      // node.aabb.getCenterToOut(c);
+      // node.aabb.getExtentsToOut(h);
+      cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5f;
+      cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5f;
+      hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5f;
+      hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5f;
+      tempx = p1x - cx;
+      tempy = p1y - cy;
+      float separation = MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
+      if (separation > 0.0f) {
+        continue;
+      }
+
+      if (node.child1 == null) {
+        subInput.p1.x = p1x;
+        subInput.p1.y = p1y;
+        subInput.p2.x = p2x;
+        subInput.p2.y = p2y;
+        subInput.maxFraction = maxFraction;
+
+        float value = callback.raycastCallback(subInput, node.id);
+
+        if (value == 0.0f) {
+          // The client has terminated the ray cast.
+          return;
+        }
+
+        if (value > 0.0f) {
+          // Update segment bounding box.
+          maxFraction = value;
+          // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
+          // Vec2.minToOut(p1, temp, segAABB.lowerBound);
+          // Vec2.maxToOut(p1, temp, segAABB.upperBound);
+          tempx = (p2x - p1x) * maxFraction + p1x;
+          tempy = (p2y - p1y) * maxFraction + p1y;
+          segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
+          segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
+          segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
+          segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
+        }
+      } else {
+        if (nodeStack.length - nodeStackIndex - 2 <= 0) {
+          DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.length * 2];
+          System.arraycopy(nodeStack, 0, newBuffer, 0, nodeStack.length);
+          nodeStack = newBuffer;
+        }
+        nodeStack[nodeStackIndex++] = node.child1;
+        nodeStack[nodeStackIndex++] = node.child2;
+      }
+    }
+  }
+
+  @Override
+  public final int computeHeight() {
+    return computeHeight(m_root);
+  }
+
+  private final int computeHeight(DynamicTreeNode node) {
+    assert (0 <= node.id && node.id < m_nodeCapacity);
+
+    if (node.child1 == null) {
+      return 0;
+    }
+    int height1 = computeHeight(node.child1);
+    int height2 = computeHeight(node.child2);
+    return 1 + MathUtils.max(height1, height2);
+  }
+
+  /**
+   * Validate this tree. For testing.
+   */
+  public void validate() {
+    validateStructure(m_root);
+    validateMetrics(m_root);
+
+    int freeCount = 0;
+    DynamicTreeNode freeNode = m_freeList != NULL_NODE ? m_nodes[m_freeList] : null;
+    while (freeNode != null) {
+      assert (0 <= freeNode.id && freeNode.id < m_nodeCapacity);
+      assert (freeNode == m_nodes[freeNode.id]);
+      freeNode = freeNode.parent;
+      ++freeCount;
+    }
+
+    assert (getHeight() == computeHeight());
+
+    assert (m_nodeCount + freeCount == m_nodeCapacity);
+  }
+
+  @Override
+  public int getHeight() {
+    if (m_root == null) {
+      return 0;
+    }
+    return m_root.height;
+  }
+
+  @Override
+  public int getMaxBalance() {
+    int maxBalance = 0;
+    for (int i = 0; i < m_nodeCapacity; ++i) {
+      final DynamicTreeNode node = m_nodes[i];
+      if (node.height <= 1) {
+        continue;
+      }
+
+      assert (node.child1 == null == false);
+
+      DynamicTreeNode child1 = node.child1;
+      DynamicTreeNode child2 = node.child2;
+      int balance = MathUtils.abs(child2.height - child1.height);
+      maxBalance = MathUtils.max(maxBalance, balance);
+    }
+
+    return maxBalance;
+  }
+
+  @Override
+  public float getAreaRatio() {
+    if (m_root == null) {
+      return 0.0f;
+    }
+
+    final DynamicTreeNode root = m_root;
+    float rootArea = root.aabb.getPerimeter();
+
+    float totalArea = 0.0f;
+    for (int i = 0; i < m_nodeCapacity; ++i) {
+      final DynamicTreeNode node = m_nodes[i];
+      if (node.height < 0) {
+        // Free node in pool
+        continue;
+      }
+
+      totalArea += node.aabb.getPerimeter();
+    }
+
+    return totalArea / rootArea;
+  }
+
+  /**
+   * Build an optimal tree. Very expensive. For testing.
+   */
+  public void rebuildBottomUp() {
+    int[] nodes = new int[m_nodeCount];
+    int count = 0;
+
+    // Build array of leaves. Free the rest.
+    for (int i = 0; i < m_nodeCapacity; ++i) {
+      if (m_nodes[i].height < 0) {
+        // free node in pool
+        continue;
+      }
+
+      DynamicTreeNode node = m_nodes[i];
+      if (node.child1 == null) {
+        node.parent = null;
+        nodes[count] = i;
+        ++count;
+      } else {
+        freeNode(node);
+      }
+    }
+
+    AABB b = new AABB();
+    while (count > 1) {
+      float minCost = Float.MAX_VALUE;
+      int iMin = -1, jMin = -1;
+      for (int i = 0; i < count; ++i) {
+        AABB aabbi = m_nodes[nodes[i]].aabb;
+
+        for (int j = i + 1; j < count; ++j) {
+          AABB aabbj = m_nodes[nodes[j]].aabb;
+          b.combine(aabbi, aabbj);
+          float cost = b.getPerimeter();
+          if (cost < minCost) {
+            iMin = i;
+            jMin = j;
+            minCost = cost;
+          }
+        }
+      }
+
+      int index1 = nodes[iMin];
+      int index2 = nodes[jMin];
+      DynamicTreeNode child1 = m_nodes[index1];
+      DynamicTreeNode child2 = m_nodes[index2];
+
+      DynamicTreeNode parent = allocateNode();
+      parent.child1 = child1;
+      parent.child2 = child2;
+      parent.height = 1 + MathUtils.max(child1.height, child2.height);
+      parent.aabb.combine(child1.aabb, child2.aabb);
+      parent.parent = null;
+
+      child1.parent = parent;
+      child2.parent = parent;
+
+      nodes[jMin] = nodes[count - 1];
+      nodes[iMin] = parent.id;
+      --count;
+    }
+
+    m_root = m_nodes[nodes[0]];
+
+    validate();
+  }
+
+  private final DynamicTreeNode allocateNode() {
+    if (m_freeList == NULL_NODE) {
+      assert (m_nodeCount == m_nodeCapacity);
+
+      DynamicTreeNode[] old = m_nodes;
+      m_nodeCapacity *= 2;
+      m_nodes = new DynamicTreeNode[m_nodeCapacity];
+      System.arraycopy(old, 0, m_nodes, 0, old.length);
+
+      // Build a linked list for the free list.
+      for (int i = m_nodeCapacity - 1; i >= m_nodeCount; i--) {
+        m_nodes[i] = new DynamicTreeNode(i);
+        m_nodes[i].parent = (i == m_nodeCapacity - 1) ? null : m_nodes[i + 1];
+        m_nodes[i].height = -1;
+      }
+      m_freeList = m_nodeCount;
+    }
+    int nodeId = m_freeList;
+    final DynamicTreeNode treeNode = m_nodes[nodeId];
+    m_freeList = treeNode.parent != null ? treeNode.parent.id : NULL_NODE;
+
+    treeNode.parent = null;
+    treeNode.child1 = null;
+    treeNode.child2 = null;
+    treeNode.height = 0;
+    treeNode.userData = null;
+    ++m_nodeCount;
+    return treeNode;
+  }
+
+  /**
+   * returns a node to the pool
+   */
+  private final void freeNode(DynamicTreeNode node) {
+    assert (node != null);
+    assert (0 < m_nodeCount);
+    node.parent = m_freeList != NULL_NODE ? m_nodes[m_freeList] : null;
+    node.height = -1;
+    m_freeList = node.id;
+    m_nodeCount--;
+  }
+
+  private final AABB combinedAABB = new AABB();
+
+  private final void insertLeaf(int leaf_index) {
+    DynamicTreeNode leaf = m_nodes[leaf_index];
+    if (m_root == null) {
+      m_root = leaf;
+      m_root.parent = null;
+      return;
+    }
+
+    // find the best sibling
+    AABB leafAABB = leaf.aabb;
+    DynamicTreeNode index = m_root;
+    while (index.child1 != null) {
+      final DynamicTreeNode node = index;
+      DynamicTreeNode child1 = node.child1;
+      DynamicTreeNode child2 = node.child2;
+
+      float area = node.aabb.getPerimeter();
+
+      combinedAABB.combine(node.aabb, leafAABB);
+      float combinedArea = combinedAABB.getPerimeter();
+
+      // Cost of creating a new parent for this node and the new leaf
+      float cost = 2.0f * combinedArea;
+
+      // Minimum cost of pushing the leaf further down the tree
+      float inheritanceCost = 2.0f * (combinedArea - area);
+
+      // Cost of descending into child1
+      float cost1;
+      if (child1.child1 == null) {
+        combinedAABB.combine(leafAABB, child1.aabb);
+        cost1 = combinedAABB.getPerimeter() + inheritanceCost;
+      } else {
+        combinedAABB.combine(leafAABB, child1.aabb);
+        float oldArea = child1.aabb.getPerimeter();
+        float newArea = combinedAABB.getPerimeter();
+        cost1 = (newArea - oldArea) + inheritanceCost;
+      }
+
+      // Cost of descending into child2
+      float cost2;
+      if (child2.child1 == null) {
+        combinedAABB.combine(leafAABB, child2.aabb);
+        cost2 = combinedAABB.getPerimeter() + inheritanceCost;
+      } else {
+        combinedAABB.combine(leafAABB, child2.aabb);
+        float oldArea = child2.aabb.getPerimeter();
+        float newArea = combinedAABB.getPerimeter();
+        cost2 = newArea - oldArea + inheritanceCost;
+      }
+
+      // Descend according to the minimum cost.
+      if (cost < cost1 && cost < cost2) {
+        break;
+      }
+
+      // Descend
+      if (cost1 < cost2) {
+        index = child1;
+      } else {
+        index = child2;
+      }
+    }
+
+    DynamicTreeNode sibling = index;
+    DynamicTreeNode oldParent = m_nodes[sibling.id].parent;
+    final DynamicTreeNode newParent = allocateNode();
+    newParent.parent = oldParent;
+    newParent.userData = null;
+    newParent.aabb.combine(leafAABB, sibling.aabb);
+    newParent.height = sibling.height + 1;
+
+    if (oldParent != null) {
+      // The sibling was not the root.
+      if (oldParent.child1 == sibling) {
+        oldParent.child1 = newParent;
+      } else {
+        oldParent.child2 = newParent;
+      }
+
+      newParent.child1 = sibling;
+      newParent.child2 = leaf;
+      sibling.parent = newParent;
+      leaf.parent = newParent;
+    } else {
+      // The sibling was the root.
+      newParent.child1 = sibling;
+      newParent.child2 = leaf;
+      sibling.parent = newParent;
+      leaf.parent = newParent;
+      m_root = newParent;
+    }
+
+    // Walk back up the tree fixing heights and AABBs
+    index = leaf.parent;
+    while (index != null) {
+      index = balance(index);
+
+      DynamicTreeNode child1 = index.child1;
+      DynamicTreeNode child2 = index.child2;
+
+      assert (child1 != null);
+      assert (child2 != null);
+
+      index.height = 1 + MathUtils.max(child1.height, child2.height);
+      index.aabb.combine(child1.aabb, child2.aabb);
+
+      index = index.parent;
+    }
+    // validate();
+  }
+
+  private final void removeLeaf(DynamicTreeNode leaf) {
+    if (leaf == m_root) {
+      m_root = null;
+      return;
+    }
+
+    DynamicTreeNode parent = leaf.parent;
+    DynamicTreeNode grandParent = parent.parent;
+    DynamicTreeNode sibling;
+    if (parent.child1 == leaf) {
+      sibling = parent.child2;
+    } else {
+      sibling = parent.child1;
+    }
+
+    if (grandParent != null) {
+      // Destroy parent and connect sibling to grandParent.
+      if (grandParent.child1 == parent) {
+        grandParent.child1 = sibling;
+      } else {
+        grandParent.child2 = sibling;
+      }
+      sibling.parent = grandParent;
+      freeNode(parent);
+
+      // Adjust ancestor bounds.
+      DynamicTreeNode index = grandParent;
+      while (index != null) {
+        index = balance(index);
+
+        DynamicTreeNode child1 = index.child1;
+        DynamicTreeNode child2 = index.child2;
+
+        index.aabb.combine(child1.aabb, child2.aabb);
+        index.height = 1 + MathUtils.max(child1.height, child2.height);
+
+        index = index.parent;
+      }
+    } else {
+      m_root = sibling;
+      sibling.parent = null;
+      freeNode(parent);
+    }
+
+    // validate();
+  }
+
+  // Perform a left or right rotation if node A is imbalanced.
+  // Returns the new root index.
+  private DynamicTreeNode balance(DynamicTreeNode iA) {
+    assert (iA != null);
+
+    DynamicTreeNode A = iA;
+    if (A.child1 == null || A.height < 2) {
+      return iA;
+    }
+
+    DynamicTreeNode iB = A.child1;
+    DynamicTreeNode iC = A.child2;
+    assert (0 <= iB.id && iB.id < m_nodeCapacity);
+    assert (0 <= iC.id && iC.id < m_nodeCapacity);
+
+    DynamicTreeNode B = iB;
+    DynamicTreeNode C = iC;
+
+    int balance = C.height - B.height;
+
+    // Rotate C up
+    if (balance > 1) {
+      DynamicTreeNode iF = C.child1;
+      DynamicTreeNode iG = C.child2;
+      DynamicTreeNode F = iF;
+      DynamicTreeNode G = iG;
+      assert (F != null);
+      assert (G != null);
+      assert (0 <= iF.id && iF.id < m_nodeCapacity);
+      assert (0 <= iG.id && iG.id < m_nodeCapacity);
+
+      // Swap A and C
+      C.child1 = iA;
+      C.parent = A.parent;
+      A.parent = iC;
+
+      // A's old parent should point to C
+      if (C.parent != null) {
+        if (C.parent.child1 == iA) {
+          C.parent.child1 = iC;
+        } else {
+          assert (C.parent.child2 == iA);
+          C.parent.child2 = iC;
+        }
+      } else {
+        m_root = iC;
+      }
+
+      // Rotate
+      if (F.height > G.height) {
+        C.child2 = iF;
+        A.child2 = iG;
+        G.parent = iA;
+        A.aabb.combine(B.aabb, G.aabb);
+        C.aabb.combine(A.aabb, F.aabb);
+
+        A.height = 1 + MathUtils.max(B.height, G.height);
+        C.height = 1 + MathUtils.max(A.height, F.height);
+      } else {
+        C.child2 = iG;
+        A.child2 = iF;
+        F.parent = iA;
+        A.aabb.combine(B.aabb, F.aabb);
+        C.aabb.combine(A.aabb, G.aabb);
+
+        A.height = 1 + MathUtils.max(B.height, F.height);
+        C.height = 1 + MathUtils.max(A.height, G.height);
+      }
+
+      return iC;
+    }
+
+    // Rotate B up
+    if (balance < -1) {
+      DynamicTreeNode iD = B.child1;
+      DynamicTreeNode iE = B.child2;
+      DynamicTreeNode D = iD;
+      DynamicTreeNode E = iE;
+      assert (0 <= iD.id && iD.id < m_nodeCapacity);
+      assert (0 <= iE.id && iE.id < m_nodeCapacity);
+
+      // Swap A and B
+      B.child1 = iA;
+      B.parent = A.parent;
+      A.parent = iB;
+
+      // A's old parent should point to B
+      if (B.parent != null) {
+        if (B.parent.child1 == iA) {
+          B.parent.child1 = iB;
+        } else {
+          assert (B.parent.child2 == iA);
+          B.parent.child2 = iB;
+        }
+      } else {
+        m_root = iB;
+      }
+
+      // Rotate
+      if (D.height > E.height) {
+        B.child2 = iD;
+        A.child1 = iE;
+        E.parent = iA;
+        A.aabb.combine(C.aabb, E.aabb);
+        B.aabb.combine(A.aabb, D.aabb);
+
+        A.height = 1 + MathUtils.max(C.height, E.height);
+        B.height = 1 + MathUtils.max(A.height, D.height);
+      } else {
+        B.child2 = iE;
+        A.child1 = iD;
+        D.parent = iA;
+        A.aabb.combine(C.aabb, D.aabb);
+        B.aabb.combine(A.aabb, E.aabb);
+
+        A.height = 1 + MathUtils.max(C.height, D.height);
+        B.height = 1 + MathUtils.max(A.height, E.height);
+      }
+
+      return iB;
+    }
+
+    return iA;
+  }
+
+  private void validateStructure(DynamicTreeNode node) {
+    if (node == null) {
+      return;
+    }
+    assert (node == m_nodes[node.id]);
+
+    if (node == m_root) {
+      assert (node.parent == null);
+    }
+
+    DynamicTreeNode child1 = node.child1;
+    DynamicTreeNode child2 = node.child2;
+
+    if (node.child1 == null) {
+      assert (child1 == null);
+      assert (child2 == null);
+      assert (node.height == 0);
+      return;
+    }
+
+    assert (child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity);
+    assert (child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity);
+
+    assert (child1.parent == node);
+    assert (child2.parent == node);
+
+    validateStructure(child1);
+    validateStructure(child2);
+  }
+
+  private void validateMetrics(DynamicTreeNode node) {
+    if (node == null) {
+      return;
+    }
+
+    DynamicTreeNode child1 = node.child1;
+    DynamicTreeNode child2 = node.child2;
+
+    if (node.child1 == null) {
+      assert (child1 == null);
+      assert (child2 == null);
+      assert (node.height == 0);
+      return;
+    }
+
+    assert (child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity);
+    assert (child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity);
+
+    int height1 = child1.height;
+    int height2 = child2.height;
+    int height;
+    height = 1 + MathUtils.max(height1, height2);
+    assert (node.height == height);
+
+    AABB aabb = new AABB();
+    aabb.combine(child1.aabb, child2.aabb);
+
+    assert (aabb.lowerBound.equals(node.aabb.lowerBound));
+    assert (aabb.upperBound.equals(node.aabb.upperBound));
+
+    validateMetrics(child1);
+    validateMetrics(child2);
+  }
+
+  @Override
+  public void drawTree(DebugDraw argDraw) {
+    if (m_root == null) {
+      return;
+    }
+    int height = computeHeight();
+    drawTree(argDraw, m_root, 0, height);
+  }
+
+  private final Color3f color = new Color3f();
+  private final Vec2 textVec = new Vec2();
+
+  public void drawTree(DebugDraw argDraw, DynamicTreeNode node, int spot, int height) {
+    node.aabb.getVertices(drawVecs);
+
+    color.set(1, (height - spot) * 1f / height, (height - spot) * 1f / height);
+    argDraw.drawPolygon(drawVecs, 4, color);
+
+    argDraw.getViewportTranform().getWorldToScreen(node.aabb.upperBound, textVec);
+    argDraw.drawString(textVec.x, textVec.y, node.id + "-" + (spot + 1) + "/" + height, color);
+
+    if (node.child1 != null) {
+      drawTree(argDraw, node.child1, spot + 1, height);
+    }
+    if (node.child2 != null) {
+      drawTree(argDraw, node.child2, spot + 1, height);
+    }
+  }
+}