planck-js/lib/collision/DynamicTree.js

2D physics engine for JavaScript/HTML5 game development

/*
 * Copyright (c) 2016      Ali Shakiba http://shakiba.me/planck.js
 * Copyright (c) 2006-2011 Erin Catto  http://www.box2d.org
 *
 * 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.
 */

var Settings = require('../Settings');
var Pool = require('../util/Pool');
var Vec2 = require('../common/Vec2');
var Math = require('../common/Math');
var AABB = require('./AABB');

module.exports = DynamicTree;

/**
 * A node in the dynamic tree. The client does not interact with this directly.
 * 
 * @prop {AABB} aabb Enlarged AABB
 * @prop {integer} height 0: leaf, -1: free node
 */
function TreeNode(id) {
  this.id = id;
  this.aabb = new AABB();
  this.userData = null;
  this.parent = null;
  this.child1 = null;
  this.child2 = null;
  this.height = -1;

  this.toString = function() {
    return this.id + ": " + this.userData;
  }
};

TreeNode.prototype.IsLeaf = function() {
  return this.child1 == null;
}
/**
 * A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. 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 `aabbExtension` 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.
 * 
 * Nodes are pooled and relocatable, so we use node indices rather than
 * pointers.
 */
function DynamicTree() {
  this.m_root = null;
  this.m_nodes = {}
  this.m_lastProxyId = 0;

  this.m_pool = new Pool({
    create : function() {
      return new TreeNode();
    }
  });
};

/**
 * Get proxy user data.
 * 
 * @return the proxy user data or 0 if the id is invalid.
 */
DynamicTree.prototype.GetUserData = function(id) {
  var node = this.m_nodes[id];
  Assert(!!node);
  return node.userData;
}

/**
 * Get the fat AABB for a node id.
 * 
 * @return the proxy user data or 0 if the id is invalid.
 */
DynamicTree.prototype.GetFatAABB = function(id) {
  var node = this.m_nodes[id];
  Assert(!!node);
  return node.aabb;
}

DynamicTree.prototype.AllocateNode = function() {
  var node = this.m_pool.allocate();
  node.id = ++this.m_lastProxyId;
  node.userData = null;
  node.parent = null;
  node.child1 = null;
  node.child2 = null;
  node.height = -1;
  this.m_nodes[node.id] = node;
  return node;
}

DynamicTree.prototype.FreeNode = function(node) {
  this.m_pool.release(node);
  node.height = -1;
  delete this.m_nodes[node.id];
}

/**
 * Create a proxy in the tree as a leaf node. We return the index of the node
 * instead of a pointer so that we can grow the node pool.
 * 
 * Create a proxy. Provide a tight fitting AABB and a userData pointer.
 */
DynamicTree.prototype.CreateProxy = function(aabb, userData) {
  Assert(AABB.IsValid(aabb))

  var node = this.AllocateNode()

  node.aabb.Set(aabb);

  // Fatten the aabb.
  AABB.Extend(node.aabb, Settings.aabbExtension);

  node.userData = userData;
  node.height = 0;

  this.InsertLeaf(node);

  return node.id;
}

/**
 * Destroy a proxy. This asserts if the id is invalid.
 */
DynamicTree.prototype.DestroyProxy = function(id) {
  var node = this.m_nodes[id];

  Assert(!!node);
  Assert(node.IsLeaf());

  this.RemoveLeaf(node);
  this.FreeNode(node);
}

/**
 * Move a proxy with a swepted AABB. If the proxy has moved outside of its
 * fattened AABB, then the proxy is removed from the tree and re-inserted.
 * Otherwise the function returns immediately.
 * 
 * @param id
 * @param aabb
 * @param {Vec2} d Displacement
 * 
 * @return true if the proxy was re-inserted.
 */
DynamicTree.prototype.MoveProxy = function(id, aabb, d) {
  Assert(AABB.IsValid(aabb));
  Assert(!d || Vec2.IsValid(d));

  var node = this.m_nodes[id];

  Assert(!!node);
  Assert(node.IsLeaf());

  if (node.aabb.Contains(aabb)) {
    return false;
  }

  this.RemoveLeaf(node);

  node.aabb.Set(aabb)

  // Extend AABB.
  aabb = node.aabb;
  AABB.Extend(aabb, Settings.aabbExtension);

  // Predict AABB displacement.
  // var d = Vec2.Mul(Settings.aabbMultiplier, displacement);

  if (d.x < 0.0) {
    aabb.lowerBound.x += d.x * Settings.aabbMultiplier;
  } else {
    aabb.upperBound.x += d.x * Settings.aabbMultiplier;
  }

  if (d.y < 0.0) {
    aabb.lowerBound.y += d.y * Settings.aabbMultiplier;
  } else {
    aabb.upperBound.y += d.y * Settings.aabbMultiplier;
  }

  this.InsertLeaf(node);

  return true;
}

DynamicTree.prototype.InsertLeaf = function(leaf) {
  Assert(AABB.IsValid(leaf.aabb));

  if (this.m_root == null) {
    this.m_root = leaf;
    this.m_root.parent = null;
    return;
  }

  // Find the best sibling for this node
  var leafAABB = leaf.aabb;
  var index = this.m_root;
  while (index.IsLeaf() == false) {
    var child1 = index.child1;
    var child2 = index.child2;

    var area = index.aabb.GetPerimeter();

    var combinedAABB = new AABB();
    combinedAABB.Combine(index.aabb, leafAABB);
    var combinedArea = combinedAABB.GetPerimeter();

    // Cost of creating a new parent for this node and the new leaf
    var cost = 2.0 * combinedArea;

    // Minimum cost of pushing the leaf further down the tree
    var inheritanceCost = 2.0 * (combinedArea - area);

    // Cost of descending into child1
    var cost1;
    if (child1.IsLeaf()) {
      var aabb = new AABB();
      aabb.Combine(leafAABB, child1.aabb);
      cost1 = aabb.GetPerimeter() + inheritanceCost;
    } else {
      var aabb = new AABB();
      aabb.Combine(leafAABB, child1.aabb);
      var oldArea = child1.aabb.GetPerimeter();
      var newArea = aabb.GetPerimeter();
      cost1 = (newArea - oldArea) + inheritanceCost;
    }

    // Cost of descending into child2
    var cost2;
    if (child2.IsLeaf()) {
      var aabb = new AABB();
      aabb.Combine(leafAABB, child2.aabb);
      cost2 = aabb.GetPerimeter() + inheritanceCost;
    } else {
      var aabb = new AABB();
      aabb.Combine(leafAABB, child2.aabb);
      var oldArea = child2.aabb.GetPerimeter();
      var newArea = aabb.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;
    }
  }

  var sibling = index;

  // Create a new parent.
  var oldParent = sibling.parent;
  var newParent = this.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;
    this.m_root = newParent;
  }

  // Walk back up the tree fixing heights and AABBs
  index = leaf.parent;
  while (index != null) {
    index = this.Balance(index);

    var child1 = index.child1;
    var child2 = index.child2;

    Assert(child1 != null);
    Assert(child2 != null);

    index.height = 1 + Math.max(child1.height, child2.height);
    index.aabb.Combine(child1.aabb, child2.aabb);

    index = index.parent;
  }

  // Validate();
}

DynamicTree.prototype.RemoveLeaf = function(leaf) {
  if (leaf == this.m_root) {
    this.m_root = null;
    return;
  }

  var parent = leaf.parent;
  var grandParent = parent.parent;
  var 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;
    this.FreeNode(parent);

    // Adjust ancestor bounds.
    var index = grandParent;
    while (index != null) {
      index = this.Balance(index);

      var child1 = index.child1;
      var child2 = index.child2;

      index.aabb.Combine(child1.aabb, child2.aabb);
      index.height = 1 + Math.max(child1.height, child2.height);

      index = index.parent;
    }
  } else {
    this.m_root = sibling;
    sibling.parent = null;
    this.FreeNode(parent);
  }

  // Validate();
}

/**
 * Perform a left or right rotation if node A is imbalanced. Returns the new
 * root index.
 */
DynamicTree.prototype.Balance = function(iA) {
  Assert(iA != null);

  var A = iA;
  if (A.IsLeaf() || A.height < 2) {
    return iA;
  }

  var B = A.child1;
  var C = A.child2;

  var balance = C.height - B.height;

  // Rotate C up
  if (balance > 1) {
    var F = C.child1;
    var G = C.child2;

    // Swap A and C
    C.child1 = A;
    C.parent = A.parent;
    A.parent = C;

    // A's old parent should point to C
    if (C.parent != null) {
      if (C.parent.child1 == iA) {
        C.parent.child1 = C;
      } else {
        C.parent.child2 = C;
      }
    } else {
      this.m_root = C;
    }

    // Rotate
    if (F.height > G.height) {
      C.child2 = F;
      A.child2 = G;
      G.parent = A;
      A.aabb.Combine(B.aabb, G.aabb);
      C.aabb.Combine(A.aabb, F.aabb);

      A.height = 1 + Math.max(B.height, G.height);
      C.height = 1 + Math.max(A.height, F.height);
    } else {
      C.child2 = G;
      A.child2 = F;
      F.parent = A;
      A.aabb.Combine(B.aabb, F.aabb);
      C.aabb.Combine(A.aabb, G.aabb);

      A.height = 1 + Math.max(B.height, F.height);
      C.height = 1 + Math.max(A.height, G.height);
    }

    return C;
  }

  // Rotate B up
  if (balance < -1) {
    var D = B.child1;
    var E = B.child2;

    // Swap A and B
    B.child1 = A;
    B.parent = A.parent;
    A.parent = B;

    // A's old parent should point to B
    if (B.parent != null) {
      if (B.parent.child1 == A) {
        B.parent.child1 = B;
      } else {
        B.parent.child2 = B;
      }
    } else {
      this.m_root = B;
    }

    // Rotate
    if (D.height > E.height) {
      B.child2 = D;
      A.child1 = E;
      E.parent = A;
      A.aabb.Combine(C.aabb, E.aabb);
      B.aabb.Combine(A.aabb, D.aabb);

      A.height = 1 + Math.max(C.height, E.height);
      B.height = 1 + Math.max(A.height, D.height);
    } else {
      B.child2 = E;
      A.child1 = D;
      D.parent = A;
      A.aabb.Combine(C.aabb, D.aabb);
      B.aabb.Combine(A.aabb, E.aabb);

      A.height = 1 + Math.max(C.height, D.height);
      B.height = 1 + Math.max(A.height, E.height);
    }

    return B;
  }

  return A;
}

/**
 * Compute the height of the binary tree in O(N) time. Should not be called
 * often.
 */
DynamicTree.prototype.GetHeight = function() {
  if (this.m_root == null) {
    return 0;
  }

  return this.m_root.height;
}

/**
 * Get the ratio of the sum of the node areas to the root area.
 */
DynamicTree.prototype.GetAreaRatio = function() {
  if (this.m_root == null) {
    return 0.0;
  }

  var root = this.m_root;
  var rootArea = root.aabb.GetPerimeter();

  var totalArea = 0.0;
  var node, it = iteratorPool.allocate().preorder();
  while (node = it.next()) {
    if (node.height < 0) {
      // Free node in pool
      continue;
    }

    totalArea += node.aabb.GetPerimeter();
  }

  iteratorPool.release(it);

  return totalArea / rootArea;
}

/**
 * Compute the height of a sub-tree.
 */
DynamicTree.prototype.ComputeHeight = function(id) {
  var node;
  if (typeof id !== 'undefined') {
    node = this.m_nodes[id];
  } else {
    node = this.m_root;
  }

  // Assert(0 <= id && id < this.m_nodeCapacity);

  if (node.IsLeaf()) {
    return 0;
  }

  var height1 = ComputeHeight(node.child1);
  var height2 = ComputeHeight(node.child2);
  return 1 + Math.max(height1, height2);
}

DynamicTree.prototype.ValidateStructure = function(node) {
  if (node == null) {
    return;
  }

  if (node == this.m_root) {
    Assert(node.parent == null);
  }

  var child1 = node.child1;
  var child2 = node.child2;

  if (node.IsLeaf()) {
    Assert(child1 == null);
    Assert(child2 == null);
    Assert(node.height == 0);
    return;
  }

  // Assert(0 <= child1 && child1 < this.m_nodeCapacity);
  // Assert(0 <= child2 && child2 < this.m_nodeCapacity);

  Assert(child1.parent == node);
  Assert(child2.parent == node);

  this.ValidateStructure(child1);
  this.ValidateStructure(child2);
}

DynamicTree.prototype.ValidateMetrics = function(node) {
  if (node == null) {
    return;
  }

  var child1 = node.child1;
  var child2 = node.child2;

  if (node.IsLeaf()) {
    Assert(child1 == null);
    Assert(child2 == null);
    Assert(node.height == 0);
    return;
  }

  // Assert(0 <= child1 && child1 < this.m_nodeCapacity);
  // Assert(0 <= child2 && child2 < this.m_nodeCapacity);

  var height1 = this.m_nodes[child1].height;
  var height2 = this.m_nodes[child2].height;
  var height = 1 + Math.max(height1, height2);
  Assert(node.height == height);

  var aabb = new AABB();
  aabb.Combine(child1.aabb, child2.aabb);

  Assert(AABB.Equals(aabb, node.aabb));

  this.ValidateMetrics(child1);
  this.ValidateMetrics(child2);
}

// Validate this tree. For testing.
DynamicTree.prototype.Validate = function() {
  ValidateStructure(this.m_root);
  ValidateMetrics(this.m_root);

  Assert(this.GetHeight() == this.ComputeHeight());
}

/**
 * Get the maximum balance of an node in the tree. The balance is the difference
 * in height of the two children of a node.
 */
DynamicTree.prototype.GetMaxBalance = function() {
  var maxBalance = 0;
  var node, it = iteratorPool.allocate().preorder();
  while (node = it.next()) {
    if (node.height <= 1) {
      continue;
    }

    Assert(node.IsLeaf() == false);

    var balance = Abs(node.child2.height - node.child1.height);
    maxBalance = Math.max(maxBalance, balance);
  }
  iteratorPool.release(it);

  return maxBalance;
}

/**
 * Build an optimal tree. Very expensive. For testing.
 */
DynamicTree.prototype.RebuildBottomUp = function() {
  var nodes = [];
  var count = 0;

  // Build array of leaves. Free the rest.
  var node, it = iteratorPool.allocate().preorder();
  while (node = it.next()) {
    if (node.height < 0) {
      // free node in pool
      continue;
    }

    if (node.IsLeaf()) {
      node.parent = null;
      nodes[count] = node;
      ++count;
    } else {
      this.FreeNode(node);
    }
  }
  iteratorPool.release(it);

  while (count > 1) {
    var minCost = Infinity;
    var iMin = -1, jMin = -1;
    for (var i = 0; i < count; ++i) {
      var aabbi = nodes[i].aabb;
      for (var j = i + 1; j < count; ++j) {
        var aabbj = nodes[j].aabb;
        var b = new AABB();
        b.Combine(aabbi, aabbj);
        var cost = b.GetPerimeter();
        if (cost < minCost) {
          iMin = i;
          jMin = j;
          minCost = cost;
        }
      }
    }

    var child1 = nodes[iMin];
    var child2 = nodes[jMin];

    var parent = this.AllocateNode();
    parent.child1 = child1;
    parent.child2 = child2;
    parent.height = 1 + Math.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;
    --count;
  }

  this.m_root = nodes[0];

  this.Validate();
}

/**
 * Shift the world origin. Useful for large worlds. The shift formula is:
 * position -= newOrigin
 * 
 * @param newOrigin The new origin with respect to the old origin
 */
DynamicTree.prototype.ShiftOrigin = function(newOrigin) {
  // Build array of leaves. Free the rest.
  var node, it = iteratorPool.allocate().preorder();
  while (node = it.next()) {
    var aabb = node.aabb;
    aabb.lowerBound.x -= newOrigin.x;
    aabb.lowerBound.y -= newOrigin.y;
    aabb.lowerBound.x -= newOrigin.x;
    aabb.lowerBound.y -= newOrigin.y;
  }
  iteratorPool.release(it);
}

/**
 * @function {DynamicTree~QueryCallback}
 * 
 * @param id Node id.
 */

/**
 * Query an AABB for overlapping proxies. The callback class is called for each
 * proxy that overlaps the supplied AABB.
 * 
 * @param {DynamicTree~QueryCallback} callback.QueryCallback
 */
DynamicTree.prototype.Query = function(callback, aabb) {
  var stack = stackPool.allocate();

  stack.push(this.m_root);
  while (stack.length > 0) {
    var node = stack.pop();
    if (node == null) {
      continue;
    }

    if (AABB.TestOverlap(node.aabb, aabb)) {
      if (node.IsLeaf()) {
        var proceed = callback.QueryCallback(node.id);
        if (proceed == false) {
          return;
        }
      } else {
        stack.push(node.child1);
        stack.push(node.child2);
      }
    }
  }

  stackPool.release(stack);
}

/**
 * Ray-cast against the proxies in the tree. This relies on the callback to
 * perform a exact ray-cast in the case were the proxy contains a shape. The
 * callback also performs the any collision filtering. This has performance
 * roughly equal to k * log(n), where k is the number of collisions and n is the
 * number of proxies in the tree.
 * 
 * @param input The ray-cast input data. The ray extends from p1 to p1 +
 *          maxFraction * (p2 - p1).
 * @param callback A callback class that is called for each proxy that is hit by
 *          the ray.
 */
DynamicTree.prototype.RayCast = function(callback, input) { // TODO GC
  var p1 = input.p1;
  var p2 = input.p2;
  var r = Vec2.Sub(p2, p1);
  Assert(r.LengthSquared() > 0.0);
  r.Normalize();

  // v is perpendicular to the segment.
  var v = Vec2.Cross(1.0, r);
  var abs_v = Vec2.Abs(v);

  // Separating axis for segment (Gino, p80).
  // |dot(v, p1 - c)| > dot(|v|, h)

  var maxFraction = input.maxFraction;

  // Build a bounding box for the segment.
  var segmentAABB = new AABB();
  var t = Vec2.WAdd((1 - maxFraction), p1, maxFraction, p2);
  segmentAABB.CombinePoints(p1, t);

  var stack = stackPool.allocate();
  var subInput = inputPool.allocate();

  stack.push(this.m_root);
  while (stack.length > 0) {
    var node = stack.pop();
    if (node == null) {
      continue;
    }

    if (AABB.TestOverlap(node.aabb, segmentAABB) == false) {
      continue;
    }

    // Separating axis for segment (Gino, p80).
    // |dot(v, p1 - c)| > dot(|v|, h)
    var c = node.aabb.GetCenter();
    var h = node.aabb.GetExtents();
    var separation = Math.abs(Vec2.Dot(v, Vec2.Sub(p1, c)))
        - Vec2.Dot(abs_v, h);
    if (separation > 0.0) {
      continue;
    }

    if (node.IsLeaf()) {
      Vec2.Clone(input.p1, subInput.p1);
      Vec2.Clone(input.p2, subInput.p2);
      subInput.maxFraction = maxFraction;

      var value = callback.RayCastCallback(subInput, node.id);

      if (value == 0.0) {
        // The client has terminated the ray cast.
        return;
      }

      if (value > 0.0) {
        // Update segment bounding box.
        maxFraction = value;
        t = Vec2.WAdd((1 - maxFraction), p1, maxFraction, p2);
        segmentAABB.CombinePoints(p1, t);
      }
    } else {
      stack.push(node.child1);
      stack.push(node.child2);
    }
  }

  stackPool.release(stack);
  inputPool.release(subInput);
}

var inputPool = new Pool({
  create : function() {
    return new RayCastInput();
  },
  release : function(stack) {
  }
});

var stackPool = new Pool({
  create : function() {
    return [];
  },
  release : function(stack) {
    stack.length = 0;
  }
});

var iteratorPool = new Pool({
  create : function() {
    return new Iterator();
  },
  release : function(iterator) {
    iterator.close();
  }
});

function Iterator() {
  var parents = [];
  var states = [];
  return {
    preorder : function(root) {
      parents.length = 0;
      parents.push(root);
      states.length = 0;
      states.push(0);
      return this;
    },
    next : function() {
      while (parents.length > 0) {
        var i = parents.length - 1;
        var node = parents[i];
        if (states[i] === 0) {
          states[i] = 1;
          return node;
        }
        if (states[i] === 1) {
          states[i] = 2;
          if (node.child1) {
            parents.push(node.child1);
            states.push(1);
            return node.child1;
          }
        }
        if (states[i] === 2) {
          states[i] = 3;
          if (node.child2) {
            parents.push(node.child2);
            states.push(1);
            return node.child2;
          }
        }
        parents.pop();
        states.pop();
      }
    },
    close : function() {
      parents.length = 0;
    }
  };
}