collision/broadphase/DefaultBroadPhaseBuffer.java

/*******************************************************************************
 * 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 java.util.Arrays;

import org.jbox2d.callbacks.DebugDraw;
import org.jbox2d.callbacks.PairCallback;
import org.jbox2d.callbacks.TreeCallback;
import org.jbox2d.callbacks.TreeRayCastCallback;
import org.jbox2d.collision.AABB;
import org.jbox2d.collision.RayCastInput;
import org.jbox2d.common.Vec2;

/**
 * The broad-phase is used for computing pairs and performing volume queries and ray casts. This
 * broad-phase does not persist pairs. Instead, this reports potentially new pairs. It is up to the
 * client to consume the new pairs and to track subsequent overlap.
 *
 * @author Daniel Murphy
 */
public class DefaultBroadPhaseBuffer implements TreeCallback, BroadPhase {

  private final BroadPhaseStrategy m_tree;

  private int m_proxyCount;

  private int[] m_moveBuffer;
  private int m_moveCapacity;
  private int m_moveCount;

  private Pair[] m_pairBuffer;
  private int m_pairCapacity;
  private int m_pairCount;

  private int m_queryProxyId;

  public DefaultBroadPhaseBuffer(BroadPhaseStrategy strategy) {
    m_proxyCount = 0;

    m_pairCapacity = 16;
    m_pairCount = 0;
    m_pairBuffer = new Pair[m_pairCapacity];
    for (int i = 0; i < m_pairCapacity; i++) {
      m_pairBuffer[i] = new Pair();
    }

    m_moveCapacity = 16;
    m_moveCount = 0;
    m_moveBuffer = new int[m_moveCapacity];

    m_tree = strategy;
    m_queryProxyId = NULL_PROXY;
  }

  @Override
  public final int createProxy(final AABB aabb, Object userData) {
    int proxyId = m_tree.createProxy(aabb, userData);
    ++m_proxyCount;
    bufferMove(proxyId);
    return proxyId;
  }

  @Override
  public final void destroyProxy(int proxyId) {
    unbufferMove(proxyId);
    --m_proxyCount;
    m_tree.destroyProxy(proxyId);
  }

  @Override
  public final void moveProxy(int proxyId, final AABB aabb, final Vec2 displacement) {
    boolean buffer = m_tree.moveProxy(proxyId, aabb, displacement);
    if (buffer) {
      bufferMove(proxyId);
    }
  }

  @Override
  public void touchProxy(int proxyId) {
    bufferMove(proxyId);
  }

  @Override
  public Object getUserData(int proxyId) {
    return m_tree.getUserData(proxyId);
  }

  @Override
  public AABB getFatAABB(int proxyId) {
    return m_tree.getFatAABB(proxyId);
  }

  @Override
  public boolean testOverlap(int proxyIdA, int proxyIdB) {
    // return AABB.testOverlap(proxyA.aabb, proxyB.aabb);
    // return m_tree.overlap(proxyIdA, proxyIdB);
    final AABB a = m_tree.getFatAABB(proxyIdA);
    final AABB b = m_tree.getFatAABB(proxyIdB);
    if (b.lowerBound.x - a.upperBound.x > 0.0f || b.lowerBound.y - a.upperBound.y > 0.0f) {
      return false;
    }

    if (a.lowerBound.x - b.upperBound.x > 0.0f || a.lowerBound.y - b.upperBound.y > 0.0f) {
      return false;
    }

    return true;
  }

  @Override
  public final int getProxyCount() {
    return m_proxyCount;
  }

  @Override
  public void drawTree(DebugDraw argDraw) {
    m_tree.drawTree(argDraw);
  }

  @Override
  public final void updatePairs(PairCallback callback) {
    // Reset pair buffer
    m_pairCount = 0;

    // Perform tree queries for all moving proxies.
    for (int i = 0; i < m_moveCount; ++i) {
      m_queryProxyId = m_moveBuffer[i];
      if (m_queryProxyId == NULL_PROXY) {
        continue;
      }

      // We have to query the tree with the fat AABB so that
      // we don't fail to create a pair that may touch later.
      final AABB fatAABB = m_tree.getFatAABB(m_queryProxyId);

      // Query tree, create pairs and add them pair buffer.
      // log.debug("quering aabb: "+m_queryProxy.aabb);
      m_tree.query(this, fatAABB);
    }
    // log.debug("Number of pairs found: "+m_pairCount);

    // Reset move buffer
    m_moveCount = 0;

    // Sort the pair buffer to expose duplicates.
    Arrays.sort(m_pairBuffer, 0, m_pairCount);

    // Send the pairs back to the client.
    int i = 0;
    while (i < m_pairCount) {
      Pair primaryPair = m_pairBuffer[i];
      Object userDataA = m_tree.getUserData(primaryPair.proxyIdA);
      Object userDataB = m_tree.getUserData(primaryPair.proxyIdB);

      // log.debug("returning pair: "+userDataA+", "+userDataB);
      callback.addPair(userDataA, userDataB);
      ++i;

      // Skip any duplicate pairs.
      while (i < m_pairCount) {
        Pair pair = m_pairBuffer[i];
        if (pair.proxyIdA != primaryPair.proxyIdA || pair.proxyIdB != primaryPair.proxyIdB) {
          break;
        }
        ++i;
      }
    }
  }

  @Override
  public final void query(final TreeCallback callback, final AABB aabb) {
    m_tree.query(callback, aabb);
  }

  @Override
  public final void raycast(final TreeRayCastCallback callback, final RayCastInput input) {
    m_tree.raycast(callback, input);
  }

  @Override
  public final int getTreeHeight() {
    return m_tree.getHeight();
  }

  @Override
  public int getTreeBalance() {
    return m_tree.getMaxBalance();
  }

  @Override
  public float getTreeQuality() {
    return m_tree.getAreaRatio();
  }

  protected final void bufferMove(int proxyId) {
    if (m_moveCount == m_moveCapacity) {
      int[] old = m_moveBuffer;
      m_moveCapacity *= 2;
      m_moveBuffer = new int[m_moveCapacity];
      System.arraycopy(old, 0, m_moveBuffer, 0, old.length);
    }

    m_moveBuffer[m_moveCount] = proxyId;
    ++m_moveCount;
  }

  protected final void unbufferMove(int proxyId) {
    for (int i = 0; i < m_moveCount; i++) {
      if (m_moveBuffer[i] == proxyId) {
        m_moveBuffer[i] = NULL_PROXY;
      }
    }
  }

  /**
   * This is called from DynamicTree::query when we are gathering pairs.
   */
  public final boolean treeCallback(int proxyId) {
    // A proxy cannot form a pair with itself.
    if (proxyId == m_queryProxyId) {
      return true;
    }

    // Grow the pair buffer as needed.
    if (m_pairCount == m_pairCapacity) {
      Pair[] oldBuffer = m_pairBuffer;
      m_pairCapacity *= 2;
      m_pairBuffer = new Pair[m_pairCapacity];
      System.arraycopy(oldBuffer, 0, m_pairBuffer, 0, oldBuffer.length);
      for (int i = oldBuffer.length; i < m_pairCapacity; i++) {
        m_pairBuffer[i] = new Pair();
      }
    }

    if (proxyId < m_queryProxyId) {
      m_pairBuffer[m_pairCount].proxyIdA = proxyId;
      m_pairBuffer[m_pairCount].proxyIdB = m_queryProxyId;
    } else {
      m_pairBuffer[m_pairCount].proxyIdA = m_queryProxyId;
      m_pairBuffer[m_pairCount].proxyIdB = proxyId;
    }

    ++m_pairCount;
    return true;
  }
}