pbox2d 0.6.0-java → 0.8.0-java

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

@@ -0,0 +1,873 @@
+/*******************************************************************************
+ * 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.BufferUtils;
+import org.jbox2d.common.Color3f;
+import org.jbox2d.common.MathUtils;
+import org.jbox2d.common.Settings;
+import org.jbox2d.common.Vec2;
+
+public class DynamicTreeFlatNodes implements BroadPhaseStrategy {
+  public static final int MAX_STACK_SIZE = 64;
+  public static final int NULL_NODE = -1;
+  public static final int INITIAL_BUFFER_LENGTH = 16;
+
+  public int m_root;
+  public AABB[] m_aabb;
+  public Object[] m_userData;
+  protected int[] m_parent;
+  protected int[] m_child1;
+  protected int[] m_child2;
+  protected int[] m_height;
+
+  private int m_nodeCount;
+  private int m_nodeCapacity;
+
+  private int m_freeList;
+
+  private final Vec2[] drawVecs = new Vec2[4];
+
+  public DynamicTreeFlatNodes() {
+    m_root = NULL_NODE;
+    m_nodeCount = 0;
+    m_nodeCapacity = 16;
+    expandBuffers(0, m_nodeCapacity);
+
+    for (int i = 0; i < drawVecs.length; i++) {
+      drawVecs[i] = new Vec2();
+    }
+  }
+
+  private void expandBuffers(int oldSize, int newSize) {
+    m_aabb = BufferUtils.reallocateBuffer(AABB.class, m_aabb, oldSize, newSize);
+    m_userData = BufferUtils.reallocateBuffer(Object.class, m_userData, oldSize, newSize);
+    m_parent = BufferUtils.reallocateBuffer(m_parent, oldSize, newSize);
+    m_child1 = BufferUtils.reallocateBuffer(m_child1, oldSize, newSize);
+    m_child2 = BufferUtils.reallocateBuffer(m_child2, oldSize, newSize);
+    m_height = BufferUtils.reallocateBuffer(m_height, oldSize, newSize);
+
+    // Build a linked list for the free list.
+    for (int i = oldSize; i < newSize; i++) {
+      m_aabb[i] = new AABB();
+      m_parent[i] = (i == newSize - 1) ? NULL_NODE : i + 1;
+      m_height[i] = -1;
+      m_child1[i] = -1;
+      m_child2[i] = -1;
+    }
+    m_freeList = oldSize;
+  }
+
+  @Override
+  public final int createProxy(final AABB aabb, Object userData) {
+    final int node = allocateNode();
+    // Fatten the aabb
+    final AABB nodeAABB = m_aabb[node];
+    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;
+    m_userData[node] = userData;
+
+    insertLeaf(node);
+
+    return node;
+  }
+
+  @Override
+  public final void destroyProxy(int proxyId) {
+    assert (0 <= proxyId && proxyId < m_nodeCapacity);
+    assert (m_child1[proxyId] == NULL_NODE);
+
+    removeLeaf(proxyId);
+    freeNode(proxyId);
+  }
+
+  @Override
+  public final boolean moveProxy(int proxyId, final AABB aabb, Vec2 displacement) {
+    assert (0 <= proxyId && proxyId < m_nodeCapacity);
+    final int node = proxyId;
+    assert (m_child1[node] == NULL_NODE);
+
+    final AABB nodeAABB = m_aabb[node];
+    // 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_nodeCount);
+    return m_userData[proxyId];
+  }
+
+  @Override
+  public final AABB getFatAABB(int proxyId) {
+    assert (0 <= proxyId && proxyId < m_nodeCount);
+    return m_aabb[proxyId];
+  }
+
+  private int[] nodeStack = new int[20];
+  private int nodeStackIndex;
+
+  @Override
+  public final void query(TreeCallback callback, AABB aabb) {
+    nodeStackIndex = 0;
+    nodeStack[nodeStackIndex++] = m_root;
+
+    while (nodeStackIndex > 0) {
+      int node = nodeStack[--nodeStackIndex];
+      if (node == NULL_NODE) {
+        continue;
+      }
+
+      if (AABB.testOverlap(m_aabb[node], aabb)) {
+        int child1 = m_child1[node];
+        if (child1 == NULL_NODE) {
+          boolean proceed = callback.treeCallback(node);
+          if (!proceed) {
+            return;
+          }
+        } else {
+          if (nodeStack.length - nodeStackIndex - 2 <= 0) {
+            nodeStack =
+                BufferUtils.reallocateBuffer(nodeStack, nodeStack.length, nodeStack.length * 2);
+          }
+          nodeStack[nodeStackIndex++] = child1;
+          nodeStack[nodeStackIndex++] = m_child2[node];
+        }
+      }
+    }
+  }
+
+  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) {
+      int node = nodeStack[--nodeStackIndex] = m_root;
+      if (node == NULL_NODE) {
+        continue;
+      }
+
+      final AABB nodeAABB = m_aabb[node];
+      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;
+      }
+
+      int child1 = m_child1[node];
+      if (child1 == NULL_NODE) {
+        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);
+
+        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 {
+        nodeStack[nodeStackIndex++] = child1;
+        nodeStack[nodeStackIndex++] = m_child2[node];
+      }
+    }
+  }
+
+  @Override
+  public final int computeHeight() {
+    return computeHeight(m_root);
+  }
+
+  private final int computeHeight(int node) {
+    assert (0 <= node && node < m_nodeCapacity);
+
+    if (m_child1[node] == NULL_NODE) {
+      return 0;
+    }
+    int height1 = computeHeight(m_child1[node]);
+    int height2 = computeHeight(m_child2[node]);
+    return 1 + MathUtils.max(height1, height2);
+  }
+
+  /**
+   * Validate this tree. For testing.
+   */
+  public void validate() {
+    validateStructure(m_root);
+    validateMetrics(m_root);
+
+    int freeCount = 0;
+    int freeNode = m_freeList;
+    while (freeNode != NULL_NODE) {
+      assert (0 <= freeNode && freeNode < m_nodeCapacity);
+      freeNode = m_parent[freeNode];
+      ++freeCount;
+    }
+
+    assert (getHeight() == computeHeight());
+    assert (m_nodeCount + freeCount == m_nodeCapacity);
+  }
+
+  @Override
+  public int getHeight() {
+    if (m_root == NULL_NODE) {
+      return 0;
+    }
+    return m_height[m_root];
+  }
+
+  @Override
+  public int getMaxBalance() {
+    int maxBalance = 0;
+    for (int i = 0; i < m_nodeCapacity; ++i) {
+      if (m_height[i] <= 1) {
+        continue;
+      }
+
+      assert (m_child1[i] != NULL_NODE);
+
+      int child1 = m_child1[i];
+      int child2 = m_child2[i];
+      int balance = MathUtils.abs(m_height[child2] - m_height[child1]);
+      maxBalance = MathUtils.max(maxBalance, balance);
+    }
+
+    return maxBalance;
+  }
+
+  @Override
+  public float getAreaRatio() {
+    if (m_root == NULL_NODE) {
+      return 0.0f;
+    }
+
+    final int root = m_root;
+    float rootArea = m_aabb[root].getPerimeter();
+
+    float totalArea = 0.0f;
+    for (int i = 0; i < m_nodeCapacity; ++i) {
+      if (m_height[i] < 0) {
+        // Free node in pool
+        continue;
+      }
+
+      totalArea += m_aabb[i].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.isLeaf()) {
+  // 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 int allocateNode() {
+    if (m_freeList == NULL_NODE) {
+      assert (m_nodeCount == m_nodeCapacity);
+      m_nodeCapacity *= 2;
+      expandBuffers(m_nodeCount, m_nodeCapacity);
+    }
+    assert (m_freeList != NULL_NODE);
+    int node = m_freeList;
+    m_freeList = m_parent[node];
+    m_parent[node] = NULL_NODE;
+    m_child1[node] = NULL_NODE;
+    m_height[node] = 0;
+    ++m_nodeCount;
+    return node;
+  }
+
+  /**
+   * returns a node to the pool
+   */
+  private final void freeNode(int node) {
+    assert (node != NULL_NODE);
+    assert (0 < m_nodeCount);
+    m_parent[node] = m_freeList != NULL_NODE ? m_freeList : NULL_NODE;
+    m_height[node] = -1;
+    m_freeList = node;
+    m_nodeCount--;
+  }
+
+  private final AABB combinedAABB = new AABB();
+
+  private final void insertLeaf(int leaf) {
+    if (m_root == NULL_NODE) {
+      m_root = leaf;
+      m_parent[m_root] = NULL_NODE;
+      return;
+    }
+
+    // find the best sibling
+    AABB leafAABB = m_aabb[leaf];
+    int index = m_root;
+    while (m_child1[index] != NULL_NODE) {
+      final int node = index;
+      int child1 = m_child1[node];
+      int child2 = m_child2[node];
+      final AABB nodeAABB = m_aabb[node];
+      float area = nodeAABB.getPerimeter();
+
+      combinedAABB.combine(nodeAABB, 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;
+      AABB child1AABB = m_aabb[child1];
+      if (m_child1[child1] == NULL_NODE) {
+        combinedAABB.combine(leafAABB, child1AABB);
+        cost1 = combinedAABB.getPerimeter() + inheritanceCost;
+      } else {
+        combinedAABB.combine(leafAABB, child1AABB);
+        float oldArea = child1AABB.getPerimeter();
+        float newArea = combinedAABB.getPerimeter();
+        cost1 = (newArea - oldArea) + inheritanceCost;
+      }
+
+      // Cost of descending into child2
+      float cost2;
+      AABB child2AABB = m_aabb[child2];
+      if (m_child1[child2] == NULL_NODE) {
+        combinedAABB.combine(leafAABB, child2AABB);
+        cost2 = combinedAABB.getPerimeter() + inheritanceCost;
+      } else {
+        combinedAABB.combine(leafAABB, child2AABB);
+        float oldArea = child2AABB.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;
+      }
+    }
+
+    int sibling = index;
+    int oldParent = m_parent[sibling];
+    final int newParent = allocateNode();
+    m_parent[newParent] = oldParent;
+    m_userData[newParent] = null;
+    m_aabb[newParent].combine(leafAABB, m_aabb[sibling]);
+    m_height[newParent] = m_height[sibling] + 1;
+
+    if (oldParent != NULL_NODE) {
+      // The sibling was not the root.
+      if (m_child1[oldParent] == sibling) {
+        m_child1[oldParent] = newParent;
+      } else {
+        m_child2[oldParent] = newParent;
+      }
+
+      m_child1[newParent] = sibling;
+      m_child2[newParent] = leaf;
+      m_parent[sibling] = newParent;
+      m_parent[leaf] = newParent;
+    } else {
+      // The sibling was the root.
+      m_child1[newParent] = sibling;
+      m_child2[newParent] = leaf;
+      m_parent[sibling] = newParent;
+      m_parent[leaf] = newParent;
+      m_root = newParent;
+    }
+
+    // Walk back up the tree fixing heights and AABBs
+    index = m_parent[leaf];
+    while (index != NULL_NODE) {
+      index = balance(index);
+
+      int child1 = m_child1[index];
+      int child2 = m_child2[index];
+
+      assert (child1 != NULL_NODE);
+      assert (child2 != NULL_NODE);
+
+      m_height[index] = 1 + MathUtils.max(m_height[child1], m_height[child2]);
+      m_aabb[index].combine(m_aabb[child1], m_aabb[child2]);
+
+      index = m_parent[index];
+    }
+    // validate();
+  }
+
+  private final void removeLeaf(int leaf) {
+    if (leaf == m_root) {
+      m_root = NULL_NODE;
+      return;
+    }
+
+    int parent = m_parent[leaf];
+    int grandParent = m_parent[parent];
+    int parentChild1 = m_child1[parent];
+    int parentChild2 = m_child2[parent];
+    int sibling;
+    if (parentChild1 == leaf) {
+      sibling = parentChild2;
+    } else {
+      sibling = parentChild1;
+    }
+
+    if (grandParent != NULL_NODE) {
+      // Destroy parent and connect sibling to grandParent.
+      if (m_child1[grandParent] == parent) {
+        m_child1[grandParent] = sibling;
+      } else {
+        m_child2[grandParent] = sibling;
+      }
+      m_parent[sibling] = grandParent;
+      freeNode(parent);
+
+      // Adjust ancestor bounds.
+      int index = grandParent;
+      while (index != NULL_NODE) {
+        index = balance(index);
+
+        int child1 = m_child1[index];
+        int child2 = m_child2[index];
+
+        m_aabb[index].combine(m_aabb[child1], m_aabb[child2]);
+        m_height[index] = 1 + MathUtils.max(m_height[child1], m_height[child2]);
+
+        index = m_parent[index];
+      }
+    } else {
+      m_root = sibling;
+      m_parent[sibling] = NULL_NODE;
+      freeNode(parent);
+    }
+
+    // validate();
+  }
+
+  // Perform a left or right rotation if node A is imbalanced.
+  // Returns the new root index.
+  private int balance(int iA) {
+    assert (iA != NULL_NODE);
+
+    int A = iA;
+    if (m_child1[A] == NULL_NODE || m_height[A] < 2) {
+      return iA;
+    }
+
+    int iB = m_child1[A];
+    int iC = m_child2[A];
+    assert (0 <= iB && iB < m_nodeCapacity);
+    assert (0 <= iC && iC < m_nodeCapacity);
+
+    int B = iB;
+    int C = iC;
+
+    int balance = m_height[C] - m_height[B];
+
+    // Rotate C up
+    if (balance > 1) {
+      int iF = m_child1[C];
+      int iG = m_child2[C];
+      int F = iF;
+      int G = iG;
+      // assert (F != null);
+      // assert (G != null);
+      assert (0 <= iF && iF < m_nodeCapacity);
+      assert (0 <= iG && iG < m_nodeCapacity);
+
+      // Swap A and C
+      m_child1[C] = iA;
+      int cParent = m_parent[C] = m_parent[A];
+      m_parent[A] = iC;
+
+      // A's old parent should point to C
+      if (cParent != NULL_NODE) {
+        if (m_child1[cParent] == iA) {
+          m_child1[cParent] = iC;
+        } else {
+          assert (m_child2[cParent] == iA);
+          m_child2[cParent] = iC;
+        }
+      } else {
+        m_root = iC;
+      }
+
+      // Rotate
+      if (m_height[F] > m_height[G]) {
+        m_child2[C] = iF;
+        m_child2[A] = iG;
+        m_parent[G] = iA;
+        m_aabb[A].combine(m_aabb[B], m_aabb[G]);
+        m_aabb[C].combine(m_aabb[A], m_aabb[F]);
+
+        m_height[A] = 1 + MathUtils.max(m_height[B], m_height[G]);
+        m_height[C] = 1 + MathUtils.max(m_height[A], m_height[F]);
+      } else {
+        m_child2[C] = iG;
+        m_child2[A] = iF;
+        m_parent[F] = iA;
+        m_aabb[A].combine(m_aabb[B], m_aabb[F]);
+        m_aabb[C].combine(m_aabb[A], m_aabb[G]);
+
+        m_height[A] = 1 + MathUtils.max(m_height[B], m_height[F]);
+        m_height[C] = 1 + MathUtils.max(m_height[A], m_height[G]);
+      }
+
+      return iC;
+    }
+
+    // Rotate B up
+    if (balance < -1) {
+      int iD = m_child1[B];
+      int iE = m_child2[B];
+      int D = iD;
+      int E = iE;
+      assert (0 <= iD && iD < m_nodeCapacity);
+      assert (0 <= iE && iE < m_nodeCapacity);
+
+      // Swap A and B
+      m_child1[B] = iA;
+      int Bparent = m_parent[B] = m_parent[A];
+      m_parent[A] = iB;
+
+      // A's old parent should point to B
+      if (Bparent != NULL_NODE) {
+        if (m_child1[Bparent] == iA) {
+          m_child1[Bparent] = iB;
+        } else {
+          assert (m_child2[Bparent] == iA);
+          m_child2[Bparent] = iB;
+        }
+      } else {
+        m_root = iB;
+      }
+
+      // Rotate
+      if (m_height[D] > m_height[E]) {
+        m_child2[B] = iD;
+        m_child1[A] = iE;
+        m_parent[E] = iA;
+        m_aabb[A].combine(m_aabb[C], m_aabb[E]);
+        m_aabb[B].combine(m_aabb[A], m_aabb[D]);
+
+        m_height[A] = 1 + MathUtils.max(m_height[C], m_height[E]);
+        m_height[B] = 1 + MathUtils.max(m_height[A], m_height[D]);
+      } else {
+        m_child2[B] = iE;
+        m_child1[A] = iD;
+        m_parent[D] = iA;
+        m_aabb[A].combine(m_aabb[C], m_aabb[D]);
+        m_aabb[B].combine(m_aabb[A], m_aabb[E]);
+
+        m_height[A] = 1 + MathUtils.max(m_height[C], m_height[D]);
+        m_height[B] = 1 + MathUtils.max(m_height[A], m_height[E]);
+      }
+
+      return iB;
+    }
+
+    return iA;
+  }
+
+  private void validateStructure(int node) {
+    if (node == NULL_NODE) {
+      return;
+    }
+
+    if (node == m_root) {
+      assert (m_parent[node] == NULL_NODE);
+    }
+
+    int child1 = m_child1[node];
+    int child2 = m_child2[node];
+
+    if (child1 == NULL_NODE) {
+      assert (child1 == NULL_NODE);
+      assert (child2 == NULL_NODE);
+      assert (m_height[node] == 0);
+      return;
+    }
+
+    assert (child1 != NULL_NODE && 0 <= child1 && child1 < m_nodeCapacity);
+    assert (child2 != NULL_NODE && 0 <= child2 && child2 < m_nodeCapacity);
+
+    assert (m_parent[child1] == node);
+    assert (m_parent[child2] == node);
+
+    validateStructure(child1);
+    validateStructure(child2);
+  }
+
+  private void validateMetrics(int node) {
+    if (node == NULL_NODE) {
+      return;
+    }
+
+    int child1 = m_child1[node];
+    int child2 = m_child2[node];
+
+    if (child1 == NULL_NODE) {
+      assert (child1 == NULL_NODE);
+      assert (child2 == NULL_NODE);
+      assert (m_height[node] == 0);
+      return;
+    }
+
+    assert (child1 != NULL_NODE && 0 <= child1 && child1 < m_nodeCapacity);
+    assert (child2 != child1 && 0 <= child2 && child2 < m_nodeCapacity);
+
+    int height1 = m_height[child1];
+    int height2 = m_height[child2];
+    int height;
+    height = 1 + MathUtils.max(height1, height2);
+    assert (m_height[node] == height);
+
+    AABB aabb = new AABB();
+    aabb.combine(m_aabb[child1], m_aabb[child2]);
+
+    assert (aabb.lowerBound.equals(m_aabb[node].lowerBound));
+    assert (aabb.upperBound.equals(m_aabb[node].upperBound));
+
+    validateMetrics(child1);
+    validateMetrics(child2);
+  }
+
+  @Override
+  public void drawTree(DebugDraw argDraw) {
+    if (m_root == NULL_NODE) {
+      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, int node, int spot, int height) {
+    AABB a = m_aabb[node];
+    a.getVertices(drawVecs);
+
+    color.set(1, (height - spot) * 1f / height, (height - spot) * 1f / height);
+    argDraw.drawPolygon(drawVecs, 4, color);
+
+    argDraw.getViewportTranform().getWorldToScreen(a.upperBound, textVec);
+    argDraw.drawString(textVec.x, textVec.y, node + "-" + (spot + 1) + "/" + height, color);
+
+    int c1 = m_child1[node];
+    int c2 = m_child2[node];
+    if (c1 != NULL_NODE) {
+      drawTree(argDraw, c1, spot + 1, height);
+    }
+    if (c2 != NULL_NODE) {
+      drawTree(argDraw, c2, spot + 1, height);
+    }
+  }
+}