public abstract class DebugDraw {

	public static final int e_shapeBit				= 0x0001; ///< draw shapes
	public static final int e_jointBit				= 0x0002; ///< draw joint connections
	public static final int e_aabbBit				= 0x0004; ///< draw core (TOI) shapes
	public static final int e_pairBit				= 0x0008; ///< draw axis aligned bounding boxes
	public static final int e_centerOfMassBit		= 0x0010; ///< draw center of mass frame
	public static final int e_dynamicTreeBit		= 0x0020; ///< draw dynamic tree.
	
	protected int m_drawFlags;
	protected final IViewportTransform viewportTransform;

	public DebugDraw(IViewportTransform viewport) {
		m_drawFlags = 0;
		viewportTransform = viewport;
	}

	public void setFlags(int flags) {
		m_drawFlags = flags;
	}

	public int getFlags() {
		return m_drawFlags;
	}

	public void appendFlags(int flags) {
		m_drawFlags |= flags;
	}

	public void clearFlags(int flags) {
		m_drawFlags &= ~flags;
	}

	/**
	 * Draw a closed polygon provided in CCW order.  This implementation
	 * uses {@link #drawSegment(Vec2, Vec2, Color3f)} to draw each side of the
	 * polygon.
	 * @param vertices
	 * @param vertexCount
	 * @param color
	 */
	public void drawPolygon(Vec2[] vertices, int vertexCount, Color3f color){
		if(vertexCount == 1){
			drawSegment(vertices[0], vertices[0], color);
			return;
		}
		
		for(int i=0; i<vertexCount-1; i+=1){
			drawSegment(vertices[i], vertices[i+1], color);
		}
		
		if(vertexCount > 2){
			drawSegment(vertices[vertexCount-1], vertices[0], color);
		}
	}
	
	public abstract void drawPoint(Vec2 argPoint, float argRadiusOnScreen, Color3f argColor);

	/**
	 * Draw a solid closed polygon provided in CCW order.
	 * @param vertices
	 * @param vertexCount
	 * @param color
	 */
	public abstract void drawSolidPolygon(Vec2[] vertices, int vertexCount, Color3f color);

	/**
	 * Draw a circle.
	 * @param center
	 * @param radius
	 * @param color
	 */
	public abstract void drawCircle(Vec2 center, float radius, Color3f color);
	
	/**
	 * Draw a solid circle.
	 * @param center
	 * @param radius
	 * @param axis
	 * @param color
	 */
	public abstract void drawSolidCircle(Vec2 center, float radius, Vec2 axis, Color3f color);
	
	/**
	 * Draw a line segment.
	 * @param p1
	 * @param p2
	 * @param color
	 */
	public abstract void drawSegment(Vec2 p1, Vec2 p2, Color3f color);

	/**
	 * Draw a transform.  Choose your own length scale
	 * @param xf
	 */
	public abstract void drawTransform(Transform xf);

	/**
	 * Draw a string.
	 * @param x
	 * @param y
	 * @param s
	 * @param color
	 */
	public abstract void drawString(float x, float y, String s, Color3f color);
	
	public void drawString(Vec2 pos, String s, Color3f color) {
	  drawString(pos.x, pos.y, s, color);
	}
		
	public IViewportTransform getViewportTranform(){
		return viewportTransform;
	}
	
	/**
	 * @param x
	 * @param y
	 * @param scale
	 * @see IViewportTransform#setCamera(float, float, float)
	 */
	public void setCamera(float x, float y, float scale){
		viewportTransform.setCamera(x,y,scale);
	}
	
	
	/**
	 * @param argScreen
	 * @param argWorld
	 * @see org.jbox2d.common.IViewportTransform#getScreenToWorld(org.jbox2d.common.Vec2, org.jbox2d.common.Vec2)
	 */
	public void getScreenToWorldToOut(Vec2 argScreen, Vec2 argWorld) {
		viewportTransform.getScreenToWorld(argScreen, argWorld);
	}

	/**
	 * @param argWorld
	 * @param argScreen
	 * @see org.jbox2d.common.IViewportTransform#getWorldToScreen(org.jbox2d.common.Vec2, org.jbox2d.common.Vec2)
	 */
	public void getWorldToScreenToOut(Vec2 argWorld, Vec2 argScreen) {
		viewportTransform.getWorldToScreen(argWorld, argScreen);
	}
	
	/**
	 * Takes the world coordinates and puts the corresponding screen
	 * coordinates in argScreen.
	 * @param worldX
	 * @param worldY
	 * @param argScreen
	 */
	public void getWorldToScreenToOut(float worldX, float worldY, Vec2 argScreen){
		argScreen.set(worldX,worldY);
		viewportTransform.getWorldToScreen(argScreen, argScreen);
	}
	
	/**
	 * takes the world coordinate (argWorld) and returns
	 * the screen coordinates.
	 * @param argWorld
	 */
	public Vec2 getWorldToScreen(Vec2 argWorld){
		Vec2 screen = new Vec2();
		viewportTransform.getWorldToScreen( argWorld, screen);
		return screen;
	}
	
	/**
	 * Takes the world coordinates and returns the screen
	 * coordinates.
	 * @param worldX
	 * @param worldY
	 */
	public Vec2 getWorldToScreen(float worldX, float worldY){
		Vec2 argScreen = new Vec2(worldX, worldY);
		viewportTransform.getWorldToScreen( argScreen, argScreen);
		return argScreen;
	}
	
	/**
	 * takes the screen coordinates and puts the corresponding 
	 * world coordinates in argWorld.
	 * @param screenX
	 * @param screenY
	 * @param argWorld
	 */
	public void getScreenToWorldToOut(float screenX, float screenY, Vec2 argWorld){
		argWorld.set(screenX,screenY);
		viewportTransform.getScreenToWorld(argWorld, argWorld);
	}
	
	/**
	 * takes the screen coordinates (argScreen) and returns
	 * the world coordinates
	 * @param argScreen
	 */
	public Vec2 getScreenToWorld(Vec2 argScreen){
		Vec2 world = new Vec2();
		viewportTransform.getScreenToWorld(argScreen, world);
		return world;
	}
	
	/**
	 * takes the screen coordinates and returns the
	 * world coordinates.
	 * @param screenX
	 * @param screenY
	 */
	public Vec2 getScreenToWorld(float screenX, float screenY){
		Vec2 screen = new Vec2(screenX, screenY);
		viewportTransform.getScreenToWorld( screen, screen);
		return screen;
	}
} public class AABB {
  /** Bottom left vertex of bounding box. */
  public final Vec2 lowerBound;
  /** Top right vertex of bounding box. */
  public final Vec2 upperBound;

  /**
   * Creates the default object, with vertices at 0,0 and 0,0.
   */
  public AABB() {
    lowerBound = new Vec2();
    upperBound = new Vec2();
  }

  /**
   * Copies from the given object
   * 
   * @param copy the object to copy from
   */
  public AABB(final AABB copy) {
    this(copy.lowerBound, copy.upperBound);
  }

  /**
   * Creates an AABB object using the given bounding vertices.
   * 
   * @param lowerVertex the bottom left vertex of the bounding box
   * @param maxVertex the top right vertex of the bounding box
   */
  public AABB(final Vec2 lowerVertex, final Vec2 upperVertex) {
    this.lowerBound = lowerVertex.clone(); // clone to be safe
    this.upperBound = upperVertex.clone();
  }

  /**
   * Sets this object from the given object
   * 
   * @param aabb the object to copy from
   */
  public final void set(final AABB aabb) {
    Vec2 v = aabb.lowerBound;
    lowerBound.x = v.x;
    lowerBound.y = v.y;
    Vec2 v1 = aabb.upperBound;
    upperBound.x = v1.x;
    upperBound.y = v1.y;
  }

  /** Verify that the bounds are sorted */
  public final boolean isValid() {
    final float dx = upperBound.x - lowerBound.x;
    if (dx < 0f) {
      return false;
    }
    final float dy = upperBound.y - lowerBound.y;
    if (dy < 0) {
      return false;
    }
    return lowerBound.isValid() && upperBound.isValid();
  }

  /**
   * Get the center of the AABB
   * 
   * @return
   */
  public final Vec2 getCenter() {
    final Vec2 center = new Vec2(lowerBound);
    center.addLocal(upperBound);
    center.mulLocal(.5f);
    return center;
  }

  public final void getCenterToOut(final Vec2 out) {
    out.x = (lowerBound.x + upperBound.x) * .5f;
    out.y = (lowerBound.y + upperBound.y) * .5f;
  }

  /**
   * Get the extents of the AABB (half-widths).
   * 
   * @return
   */
  public final Vec2 getExtents() {
    final Vec2 center = new Vec2(upperBound);
    center.subLocal(lowerBound);
    center.mulLocal(.5f);
    return center;
  }

  public final void getExtentsToOut(final Vec2 out) {
    out.x = (upperBound.x - lowerBound.x) * .5f;
    out.y = (upperBound.y - lowerBound.y) * .5f; // thanks FDN1
  }

  public final void getVertices(Vec2[] argRay) {
    argRay[0].set(lowerBound);
    argRay[1].set(lowerBound);
    argRay[1].x += upperBound.x - lowerBound.x;
    argRay[2].set(upperBound);
    argRay[3].set(upperBound);
    argRay[3].x -= upperBound.x - lowerBound.x;
  }

  /**
   * Combine two AABBs into this one.
   * 
   * @param aabb1
   * @param aab
   */
  public final void combine(final AABB aabb1, final AABB aab) {
    lowerBound.x = aabb1.lowerBound.x < aab.lowerBound.x ? aabb1.lowerBound.x : aab.lowerBound.x;
    lowerBound.y = aabb1.lowerBound.y < aab.lowerBound.y ? aabb1.lowerBound.y : aab.lowerBound.y;
    upperBound.x = aabb1.upperBound.x > aab.upperBound.x ? aabb1.upperBound.x : aab.upperBound.x;
    upperBound.y = aabb1.upperBound.y > aab.upperBound.y ? aabb1.upperBound.y : aab.upperBound.y;
  }

  /**
   * Gets the perimeter length
   * 
   * @return
   */
  public final float getPerimeter() {
    return 2.0f * (upperBound.x - lowerBound.x + upperBound.y - lowerBound.y);
  }

  /**
   * Combines another aabb with this one
   * 
   * @param aabb
   */
  public final void combine(final AABB aabb) {
    lowerBound.x = lowerBound.x < aabb.lowerBound.x ? lowerBound.x : aabb.lowerBound.x;
    lowerBound.y = lowerBound.y < aabb.lowerBound.y ? lowerBound.y : aabb.lowerBound.y;
    upperBound.x = upperBound.x > aabb.upperBound.x ? upperBound.x : aabb.upperBound.x;
    upperBound.y = upperBound.y > aabb.upperBound.y ? upperBound.y : aabb.upperBound.y;
  }

  /**
   * Does this aabb contain the provided AABB.
   * 
   * @return
   */
  public final boolean contains(final AABB aabb) {
    /*
     * boolean result = true; result = result && lowerBound.x <= aabb.lowerBound.x; result = result
     * && lowerBound.y <= aabb.lowerBound.y; result = result && aabb.upperBound.x <= upperBound.x;
     * result = result && aabb.upperBound.y <= upperBound.y; return result;
     */
    // djm: faster putting all of them together, as if one is false we leave the logic
    // early
    return lowerBound.x > aabb.lowerBound.x && lowerBound.y > aabb.lowerBound.y
        && aabb.upperBound.x > upperBound.x && aabb.upperBound.y > upperBound.y;
  }

  /**
   * @deprecated please use {@link #raycast(RayCastOutput, RayCastInput, IWorldPool)} for better
   *             performance
   * @param output
   * @param input
   * @return
   */
  public final boolean raycast(final RayCastOutput output, final RayCastInput input) {
    return raycast(output, input, new DefaultWorldPool(4, 4));
  }

  /**
   * From Real-time Collision Detection, p179.
   * 
   * @param output
   * @param input
   */
  public final boolean raycast(final RayCastOutput output, final RayCastInput input,
      IWorldPool argPool) {
    float tmin = -Float.MAX_VALUE;
    float tmax = Float.MAX_VALUE;

    final Vec2 p = argPool.popVec2();
    final Vec2 d = argPool.popVec2();
    final Vec2 absD = argPool.popVec2();
    final Vec2 normal = argPool.popVec2();

    p.set(input.p1);
    d.set(input.p2).subLocal(input.p1);
    Vec2.absToOut(d, absD);

    // x then y
    if (absD.x < Settings.EPSILON) {
      // Parallel.
      if (p.x < lowerBound.x || upperBound.x < p.x) {
        argPool.pushVec2(4);
        return false;
      }
    } else {
      final float inv_d = 1.0f / d.x;
      float t1 = (lowerBound.x - p.x) * inv_d;
      float t2 = (upperBound.x - p.x) * inv_d;

      // Sign of the normal vector.
      float s = -1.0f;

      if (t1 > t2) {
        final float temp = t1;
        t1 = t2;
        t2 = temp;
        s = 1.0f;
      }

      // Push the min up
      if (t1 > tmin) {
        normal.setZero();
        normal.x = s;
        tmin = t1;
      }

      // Pull the max down
      tmax = MathUtils.min(tmax, t2);

      if (tmin > tmax) {
        argPool.pushVec2(4);
        return false;
      }
    }

    if (absD.y < Settings.EPSILON) {
      // Parallel.
      if (p.y < lowerBound.y || upperBound.y < p.y) {
        argPool.pushVec2(4);
        return false;
      }
    } else {
      final float inv_d = 1.0f / d.y;
      float t1 = (lowerBound.y - p.y) * inv_d;
      float t2 = (upperBound.y - p.y) * inv_d;

      // Sign of the normal vector.
      float s = -1.0f;

      if (t1 > t2) {
        final float temp = t1;
        t1 = t2;
        t2 = temp;
        s = 1.0f;
      }

      // Push the min up
      if (t1 > tmin) {
        normal.setZero();
        normal.y = s;
        tmin = t1;
      }

      // Pull the max down
      tmax = MathUtils.min(tmax, t2);

      if (tmin > tmax) {
        argPool.pushVec2(4);
        return false;
      }
    }

    // Does the ray start inside the box?
    // Does the ray intersect beyond the max fraction?
    if (tmin < 0.0f || input.maxFraction < tmin) {
      argPool.pushVec2(4);
      return false;
    }

    // Intersection.
    output.fraction = tmin;
    output.normal.x = normal.x;
    output.normal.y = normal.y;
    argPool.pushVec2(4);
    return true;
  }

  public static final boolean testOverlap(final AABB a, final AABB b) {
    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 String toString() {
    final String s = "AABB[" + lowerBound + " . " + upperBound + "]";
    return s;
  }
} public class RayCastOutput{
	public final Vec2 normal;
	public float fraction;

	public RayCastOutput(){
		normal = new Vec2();
		fraction = 0;
	}

	public void set(final RayCastOutput rco){
		normal.set(rco.normal);
		fraction = rco.fraction;
	}
} public class BroadPhase implements TreeCallback {

  public static final int NULL_PROXY = -1;

  private final DynamicTree 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 BroadPhase() {
    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 = new DynamicTree();
    m_queryProxyId = NULL_PROXY;
  }

  /**
   * Create a proxy with an initial AABB. Pairs are not reported until updatePairs is called.
   * 
   * @param aabb
   * @param userData
   * @return
   */
  public final int createProxy(final AABB aabb, Object userData) {
    int proxyId = m_tree.createProxy(aabb, userData);
    ++m_proxyCount;
    bufferMove(proxyId);
    return proxyId;
  }

  /**
   * Destroy a proxy. It is up to the client to remove any pairs.
   * 
   * @param proxyId
   */
  public final void destroyProxy(int proxyId) {
    unbufferMove(proxyId);
    --m_proxyCount;
    m_tree.destroyProxy(proxyId);
  }

  /**
   * Call MoveProxy as many times as you like, then when you are done call UpdatePairs to finalized
   * the proxy pairs (for your time step).
   */
  public final void moveProxy(int proxyId, final AABB aabb, final Vec2 displacement) {
    boolean buffer = m_tree.moveProxy(proxyId, aabb, displacement);
    if (buffer) {
      bufferMove(proxyId);
    }
  }

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

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

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

  public boolean testOverlap(int proxyIdA, int proxyIdB) {
    // return AABB.testOverlap(proxyA.aabb, proxyB.aabb);
    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;
  }

  /**
   * Get the number of proxies.
   * 
   * @return
   */
  public final int getProxyCount() {
    return m_proxyCount;
  }

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

  /**
   * Update the pairs. This results in pair callbacks. This can only add pairs.
   * 
   * @param callback
   */
  public final void updatePairs(PairCallback callback) {
    // log.debug("beginning to update pairs");
    // 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;
        }
        // log.debug("skipping duplicate");
        ++i;
      }
    }

    // Try to keep the tree balanced.
    // m_tree.rebalance(Settings.TREE_REBALANCE_STEPS);
  }

  /**
   * Query an AABB for overlapping proxies. The callback class is called for each proxy that
   * overlaps the supplied AABB.
   * 
   * @param callback
   * @param aabb
   */
  public final void query(final TreeCallback callback, final AABB aabb) {
    m_tree.query(callback, aabb);
  }

  /**
   * 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.
   */
  public final void raycast(final TreeRayCastCallback callback, final RayCastInput input) {
    m_tree.raycast(callback, input);
  }

  /**
   * Get the height of the embedded tree.
   * 
   * @return
   */
  public final int getTreeHeight() {
    return m_tree.computeHeight();
  }

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

  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;
      }
    }
  }

  // private final PairStack pairStack = new PairStack();
  /**
   * 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) {
      // log.debug("It was us...");
      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) {
      // log.debug("new proxy is first");
      m_pairBuffer[m_pairCount].proxyIdA = proxyId;
      m_pairBuffer[m_pairCount].proxyIdB = m_queryProxyId;
    } else {
      // log.debug("new proxy is second");
      m_pairBuffer[m_pairCount].proxyIdA = m_queryProxyId;
      m_pairBuffer[m_pairCount].proxyIdB = proxyId;
    }

    ++m_pairCount;
    return true;
  }
} public class ChainShape extends Shape {

  public Vec2[] m_vertices;
  public int m_count;
  public final Vec2 m_prevVertex = new Vec2(), m_nextVertex = new Vec2();
  public boolean m_hasPrevVertex = false, m_hasNextVertex = false;

  private final EdgeShape pool0 = new EdgeShape();

  public ChainShape() {
    super(ShapeType.CHAIN);
    m_vertices = null;
    m_radius = Settings.polygonRadius;
    m_count = 0;
  }

  @Override
  public int getChildCount() {
    return m_count - 1;
  }

  /**
   * Get a child edge.
   */
  public void getChildEdge(EdgeShape edge, int index) {
    assert (0 <= index && index < m_count - 1);
    edge.m_radius = m_radius;
    
    final Vec2 v0 = m_vertices[index + 0];
    final Vec2 v1 = m_vertices[index + 1];
    edge.m_vertex1.x = v0.x;
    edge.m_vertex1.y = v0.y;
    edge.m_vertex2.x = v1.x;
    edge.m_vertex2.y = v1.y;

    if (index > 0) {
      Vec2 v = m_vertices[index - 1];
      edge.m_vertex0.x = v.x;
      edge.m_vertex0.y = v.y;
      edge.m_hasVertex0 = true;
    } else {
      edge.m_vertex0.x = m_prevVertex.x;
      edge.m_vertex0.y = m_prevVertex.y;
      edge.m_hasVertex0 = m_hasPrevVertex;
    }

    if (index < m_count - 2) {
      Vec2 v = m_vertices[index + 2];
      edge.m_vertex3.x = v.x;
      edge.m_vertex3.y = v.y;
      edge.m_hasVertex3 = true;
    } else {
      edge.m_vertex3.x = m_nextVertex.x;
      edge.m_vertex3.y = m_nextVertex.y;
      edge.m_hasVertex3 = m_hasNextVertex;
    }
  }

  @Override
  public boolean testPoint(Transform xf, Vec2 p) {
    return false;
  }

  @Override
  public boolean raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex) {
    assert (childIndex < m_count);

    final EdgeShape edgeShape = pool0;

    int i1 = childIndex;
    int i2 = childIndex + 1;
    if (i2 == m_count) {
      i2 = 0;
    }
    Vec2 v = m_vertices[i1];
    edgeShape.m_vertex1.x = v.x;
    edgeShape.m_vertex1.y = v.y;
    Vec2 v1 = m_vertices[i2];
    edgeShape.m_vertex2.x = v1.x;
    edgeShape.m_vertex2.y = v1.y;

    return edgeShape.raycast(output, input, xf, 0);
  }

  @Override
  public void computeAABB(AABB aabb, Transform xf, int childIndex) {
    assert (childIndex < m_count);
    final Vec2 lower = aabb.lowerBound;
    final Vec2 upper = aabb.upperBound;
    
    int i1 = childIndex;
    int i2 = childIndex + 1;
    if (i2 == m_count) {
      i2 = 0;
    }

    final Vec2 vi1 = m_vertices[i1];
    final Vec2 vi2 = m_vertices[i2];
    final Rot xfq = xf.q;
    final Vec2 xfp = xf.p;
    float v1x = (xfq.c * vi1.x - xfq.s * vi1.y) + xfp.x;
    float v1y = (xfq.s * vi1.x + xfq.c * vi1.y) + xfp.y;
    float v2x = (xfq.c * vi2.x - xfq.s * vi2.y) + xfp.x;
    float v2y = (xfq.s * vi2.x + xfq.c * vi2.y) + xfp.y;

    lower.x = v1x < v2x ? v1x : v2x;
    lower.y = v1y < v2y ? v1y : v2y;
    upper.x = v1x > v2x ? v1x : v2x;
    upper.y = v1y > v2y ? v1y : v2y;
  }

  @Override
  public void computeMass(MassData massData, float density) {
    massData.mass = 0.0f;
    massData.center.setZero();
    massData.I = 0.0f;
  }

  @Override
  public Shape clone() {
    ChainShape clone = new ChainShape();
    clone.createChain(m_vertices, m_count);
    clone.m_prevVertex.set(m_prevVertex);
    clone.m_nextVertex.set(m_nextVertex);
    clone.m_hasPrevVertex = m_hasPrevVertex;
    clone.m_hasNextVertex = m_hasNextVertex;
    return clone;
  }

  /**
   * Create a loop. This automatically adjusts connectivity.
   * 
   * @param vertices an array of vertices, these are copied
   * @param count the vertex count
   */
  public void createLoop(final Vec2[] vertices, int count) {
    assert (m_vertices == null && m_count == 0);
    assert (count >= 3);
    m_count = count + 1;
    m_vertices = new Vec2[m_count];
    for (int i = 1; i < count; i++) {
      Vec2 v1 = vertices[i - 1];
      Vec2 v2 = vertices[i];
      // If the code crashes here, it means your vertices are too close together.
      if (MathUtils.distanceSquared(v1, v2) < Settings.linearSlop * Settings.linearSlop) {
        throw new RuntimeException("Vertices of chain shape are too close together");
      }
    }
    for (int i = 0; i < count; i++) {
      m_vertices[i] = new Vec2(vertices[i]);
    }
    m_vertices[count] = new Vec2(m_vertices[0]);
    m_prevVertex.set(m_vertices[m_count - 2]);
    m_nextVertex.set(m_vertices[1]);
    m_hasPrevVertex = true;
    m_hasNextVertex = true;
  }

  /**
   * Create a chain with isolated end vertices.
   * 
   * @param vertices an array of vertices, these are copied
   * @param count the vertex count
   */
  public void createChain(final Vec2 vertices[], int count) {
    assert (m_vertices == null && m_count == 0);
    assert (count >= 2);
    m_count = count;
    m_vertices = new Vec2[m_count];
    for (int i = 1; i < m_count; i++) {
      Vec2 v1 = vertices[i - 1];
      Vec2 v2 = vertices[i];
      // If the code crashes here, it means your vertices are too close together.
      if (MathUtils.distanceSquared(v1, v2) < Settings.linearSlop * Settings.linearSlop) {
        throw new RuntimeException("Vertices of chain shape are too close together");
      }
    }
    for (int i = 0; i < m_count; i++) {
      m_vertices[i] = new Vec2(vertices[i]);
    }
    m_hasPrevVertex = false;
    m_hasNextVertex = false;
  }

  /**
   * Establish connectivity to a vertex that precedes the first vertex. Don't call this for loops.
   * 
   * @param prevVertex
   */
  public void setPrevVertex(final Vec2 prevVertex) {
    m_prevVertex.set(prevVertex);
    m_hasPrevVertex = true;
  }

  /**
   * Establish connectivity to a vertex that follows the last vertex. Don't call this for loops.
   * 
   * @param nextVertex
   */
  public void setNextVertex(final Vec2 nextVertex) {
    m_nextVertex.set(nextVertex);
    m_hasNextVertex = true;
  }
} public class MathUtils {
  public static final float PI = (float) Math.PI;
  public static final float TWOPI = (float) (Math.PI * 2);
  public static final float INV_PI = 1f / PI;
  public static final float HALF_PI = PI / 2;
  public static final float QUARTER_PI = PI / 4;
  public static final float THREE_HALVES_PI = TWOPI - HALF_PI;

  /**
   * Degrees to radians conversion factor
   */
  public static final float DEG2RAD = PI / 180;

  /**
   * Radians to degrees conversion factor
   */
  public static final float RAD2DEG = 180 / PI;

  private static final float SHIFT23 = 1 << 23;
  private static final float INV_SHIFT23 = 1.0f / SHIFT23;

  public static final float[] sinLUT = new float[Settings.SINCOS_LUT_LENGTH];
  public static final float[] cosLUT = new float[Settings.SINCOS_LUT_LENGTH];

  static {
    for (int i = 0; i < Settings.SINCOS_LUT_LENGTH; i++) {
      sinLUT[i] = (float) Math.sin(i * Settings.SINCOS_LUT_PRECISION);
      cosLUT[i] = (float) Math.cos(i * Settings.SINCOS_LUT_PRECISION);
    }
  }

  public static final float sin(float x) {
    if (Settings.SINCOS_LUT_ENABLED) {
      return sinLUT(x);
    } else {
      return (float) StrictMath.sin(x);
    }
  }

  public static final float sinLUT(float x) {
    x %= TWOPI;

    if (x < 0) {
      x += TWOPI;
    }

    if (Settings.SINCOS_LUT_LERP) {

      x /= Settings.SINCOS_LUT_PRECISION;

      final int index = (int) x;

      if (index != 0) {
        x %= index;
      }

      // the next index is 0
      if (index == Settings.SINCOS_LUT_LENGTH - 1) {
        return ((1 - x) * sinLUT[index] + x * sinLUT[0]);
      } else {
        return ((1 - x) * sinLUT[index] + x * sinLUT[index + 1]);
      }

    } else {
      return sinLUT[MathUtils.round(x / Settings.SINCOS_LUT_PRECISION) % Settings.SINCOS_LUT_LENGTH];
    }
  }

  public static final float cos(float x) {
    if (Settings.SINCOS_LUT_ENABLED) {
      x %= TWOPI;

      while (x < 0) {
        x += TWOPI;
      }

      if (Settings.SINCOS_LUT_LERP) {

        x /= Settings.SINCOS_LUT_PRECISION;

        final int index = (int) x;

        if (index != 0) {
          x %= index;
        }

        // the next index is 0
        if (index == Settings.SINCOS_LUT_LENGTH - 1) {
          return ((1 - x) * cosLUT[index] + x * cosLUT[0]);
        } else {
          return ((1 - x) * cosLUT[index] + x * cosLUT[index + 1]);
        }

      } else {
        return cosLUT[MathUtils.round(x / Settings.SINCOS_LUT_PRECISION)
            % Settings.SINCOS_LUT_LENGTH];
      }

    } else {
      return (float) StrictMath.cos(x);
    }
  }

  public static final float abs(final float x) {
    if (Settings.FAST_MATH) {
      return x > 0 ? x : -x;
    } else {
      return Math.abs(x);
    }
  }

  public static final int abs(int x) {
    int y = x >> 31;
    return (x ^ y) - y;
  }

  public static final int floor(final float x) {
    if (Settings.FAST_MATH) {
      int y = (int) x;
      if (x < 0 && x != y) {
        return y - 1;
      }
      return y;
    } else {
      return (int) Math.floor(x);
    }
  }

  public static final int ceil(final float x) {
    if (Settings.FAST_MATH) {
      int y = (int) x;
      if (x > 0 && x != y) {
        return y + 1;
      }
      return y;
    } else {
      return (int) Math.ceil(x);
    }
  }

  public static final int round(final float x) {
    if (Settings.FAST_MATH) {
      return floor(x + .5f);
    } else {
      return StrictMath.round(x);
    }
  }

  /**
   * Rounds up the value to the nearest higher power^2 value.
   * 
   * @param x
   * @return power^2 value
   */
  public static final int ceilPowerOf2(int x) {
    int pow2 = 1;
    while (pow2 < x) {
      pow2 <<= 1;
    }
    return pow2;
  }

  public final static float max(final float a, final float b) {
    return a > b ? a : b;
  }

  public final static int max(final int a, final int b) {
    return a > b ? a : b;
  }

  public final static float min(final float a, final float b) {
    return a < b ? a : b;
  }

  public final static int min(final int a, final int b) {
    return a < b ? a : b;
  }

  public final static float map(final float val, final float fromMin, final float fromMax,
      final float toMin, final float toMax) {
    final float mult = (val - fromMin) / (fromMax - fromMin);
    final float res = toMin + mult * (toMax - toMin);
    return res;
  }

  /** Returns the closest value to 'a' that is in between 'low' and 'high' */
  public final static float clamp(final float a, final float low, final float high) {
    return max(low, min(a, high));
  }

  public final static Vec2 clamp(final Vec2 a, final Vec2 low, final Vec2 high) {
    final Vec2 min = new Vec2();
    min.x = a.x < high.x ? a.x : high.x;
    min.y = a.y < high.y ? a.y : high.y;
    min.x = low.x > min.x ? low.x : min.x;
    min.y = low.y > min.y ? low.y : min.y;
    return min;
  }

  public final static void clampToOut(final Vec2 a, final Vec2 low, final Vec2 high, final Vec2 dest) {
    dest.x = a.x < high.x ? a.x : high.x;
    dest.y = a.y < high.y ? a.y : high.y;
    dest.x = low.x > dest.x ? low.x : dest.x;
    dest.y = low.y > dest.y ? low.y : dest.y;
  }

  /**
   * Next Largest Power of 2: Given a binary integer value x, the next largest power of 2 can be
   * computed by a SWAR algorithm that recursively "folds" the upper bits into the lower bits. This
   * process yields a bit vector with the same most significant 1 as x, but all 1's below it. Adding
   * 1 to that value yields the next largest power of 2.
   */
  public final static int nextPowerOfTwo(int x) {
    x |= x >> 1;
    x |= x >> 2;
    x |= x >> 4;
    x |= x >> 8;
    x |= x >> 16;
    return x + 1;
  }

  public final static boolean isPowerOfTwo(final int x) {
    return x > 0 && (x & x - 1) == 0;
  }

  public static final float fastPow(float a, float b) {
    float x = Float.floatToRawIntBits(a);
    x *= INV_SHIFT23;
    x -= 127;
    float y = x - (x >= 0 ? (int) x : (int) x - 1);
    b *= x + (y - y * y) * 0.346607f;
    y = b - (b >= 0 ? (int) b : (int) b - 1);
    y = (y - y * y) * 0.33971f;
    return Float.intBitsToFloat((int) ((b + 127 - y) * SHIFT23));
  }

  public static final float atan2(final float y, final float x) {
    if (Settings.FAST_MATH) {
      return fastAtan2(y, x);
    } else {
      return (float) StrictMath.atan2(y, x);
    }
  }

  public static final float fastAtan2(float y, float x) {
    if (x == 0.0f) {
      if (y > 0.0f) return HALF_PI;
      if (y == 0.0f) return 0.0f;
      return -HALF_PI;
    }
    float atan;
    final float z = y / x;
    if (abs(z) < 1.0f) {
      atan = z / (1.0f + 0.28f * z * z);
      if (x < 0.0f) {
        if (y < 0.0f) return atan - PI;
        return atan + PI;
      }
    } else {
      atan = HALF_PI - z / (z * z + 0.28f);
      if (y < 0.0f) return atan - PI;
    }
    return atan;
  }

  public static final float reduceAngle(float theta) {
    theta %= TWOPI;
    if (abs(theta) > PI) {
      theta = theta - TWOPI;
    }
    if (abs(theta) > HALF_PI) {
      theta = PI - theta;
    }
    return theta;
  }

  public static final float randomFloat(float argLow, float argHigh) {
    return (float) Math.random() * (argHigh - argLow) + argLow;
  }

  public static final float randomFloat(Random r, float argLow, float argHigh) {
    return r.nextFloat() * (argHigh - argLow) + argLow;
  }

  public static final float sqrt(float x) {
    return (float) StrictMath.sqrt(x);
  }

  public final static float distanceSquared(Vec2 v1, Vec2 v2) {
    float dx = (v1.x - v2.x);
    float dy = (v1.y - v2.y);
    return dx * dx + dy * dy;
  }

  public final static float distance(Vec2 v1, Vec2 v2) {
    return sqrt(distanceSquared(v1, v2));
  }
} public class Settings {

  /** A "close to zero" float epsilon value for use */
  public static final float EPSILON = 1.1920928955078125E-7f;

  /** Pi. */
  public static final float PI = (float) Math.PI;

  // JBox2D specific settings
  /**
   * needs to be final, or will slow down math methods
   */
  public static final boolean FAST_MATH = true;
  public static final int CONTACT_STACK_INIT_SIZE = 10;
  public static final boolean SINCOS_LUT_ENABLED = true;
  /**
   * smaller the precision, the larger the table. If a small table is used (eg, precision is .006 or
   * greater), make sure you set the table to lerp it's results. Accuracy chart is in the MathUtils
   * source. Or, run the tests yourself in {@link SinCosTest}.</br> </br> Good lerp precision
   * values:
   * <ul>
   * <li>.0092</li>
   * <li>.008201</li>
   * <li>.005904</li>
   * <li>.005204</li>
   * <li>.004305</li>
   * <li>.002807</li>
   * <li>.001508</li>
   * <li>9.32500E-4</li>
   * <li>7.48000E-4</li>
   * <li>8.47000E-4</li>
   * <li>.0005095</li>
   * <li>.0001098</li>
   * <li>9.50499E-5</li>
   * <li>6.08500E-5</li>
   * <li>3.07000E-5</li>
   * <li>1.53999E-5</li>
   * </ul>
   */
  public static final float SINCOS_LUT_PRECISION = .00011f;
  public static final int SINCOS_LUT_LENGTH = (int) Math.ceil(Math.PI * 2 / SINCOS_LUT_PRECISION);
  /**
   * Use if the table's precision is large (eg .006 or greater). Although it is more expensive, it
   * greatly increases accuracy. Look in the MathUtils source for some test results on the accuracy
   * and speed of lerp vs non lerp. Or, run the tests yourself in {@link SinCosTest}.
   */
  public static final boolean SINCOS_LUT_LERP = false;


  // Collision

  /**
   * The maximum number of contact points between two convex shapes.
   */
  public static final int maxManifoldPoints = 2;

  /**
   * The maximum number of vertices on a convex polygon.
   */
  public static final int maxPolygonVertices = 8;

  /**
   * This is used to fatten AABBs in the dynamic tree. This allows proxies to move by a small amount
   * without triggering a tree adjustment. This is in meters.
   */
  public static final float aabbExtension = 0.1f;

  /**
   * This is used to fatten AABBs in the dynamic tree. This is used to predict the future position
   * based on the current displacement. This is a dimensionless multiplier.
   */
  public static final float aabbMultiplier = 2.0f;

  /**
   * A small length used as a collision and constraint tolerance. Usually it is chosen to be
   * numerically significant, but visually insignificant.
   */
  public static final float linearSlop = 0.005f;

  /**
   * A small angle used as a collision and constraint tolerance. Usually it is chosen to be
   * numerically significant, but visually insignificant.
   */
  public static final float angularSlop = (2.0f / 180.0f * PI);

  /**
   * The radius of the polygon/edge shape skin. This should not be modified. Making this smaller
   * means polygons will have and insufficient for continuous collision. Making it larger may create
   * artifacts for vertex collision.
   */
  public static final float polygonRadius = (2.0f * linearSlop);

  /** Maximum number of sub-steps per contact in continuous physics simulation. */
  public static final int maxSubSteps = 8;

  // Dynamics

  /**
   * Maximum number of contacts to be handled to solve a TOI island.
   */
  public static final int maxTOIContacts = 32;

  /**
   * A velocity threshold for elastic collisions. Any collision with a relative linear velocity
   * below this threshold will be treated as inelastic.
   */
  public static final float velocityThreshold = 1.0f;

  /**
   * The maximum linear position correction used when solving constraints. This helps to prevent
   * overshoot.
   */
  public static final float maxLinearCorrection = 0.2f;

  /**
   * The maximum angular position correction used when solving constraints. This helps to prevent
   * overshoot.
   */
  public static final float maxAngularCorrection = (8.0f / 180.0f * PI);

  /**
   * The maximum linear velocity of a body. This limit is very large and is used to prevent
   * numerical problems. You shouldn't need to adjust this.
   */
  public static final float maxTranslation = 2.0f;
  public static final float maxTranslationSquared = (maxTranslation * maxTranslation);

  /**
   * The maximum angular velocity of a body. This limit is very large and is used to prevent
   * numerical problems. You shouldn't need to adjust this.
   */
  public static final float maxRotation = (0.5f * PI);
  public static float maxRotationSquared = (maxRotation * maxRotation);

  /**
   * This scale factor controls how fast overlap is resolved. Ideally this would be 1 so that
   * overlap is removed in one time step. However using values close to 1 often lead to overshoot.
   */
  public static final float baumgarte = 0.2f;
  public static final float toiBaugarte = 0.75f;


  // Sleep

  /**
   * The time that a body must be still before it will go to sleep.
   */
  public static final float timeToSleep = 0.5f;

  /**
   * A body cannot sleep if its linear velocity is above this tolerance.
   */
  public static final float linearSleepTolerance = 0.01f;

  /**
   * A body cannot sleep if its angular velocity is above this tolerance.
   */
  public static final float angularSleepTolerance = (2.0f / 180.0f * PI);

  /**
   * Friction mixing law. Feel free to customize this. TODO djm: add customization
   * 
   * @param friction1
   * @param friction2
   * @return
   */
  public static final float mixFriction(float friction1, float friction2) {
    return MathUtils.sqrt(friction1 * friction2);
  }

  /**
   * Restitution mixing law. Feel free to customize this. TODO djm: add customization
   * 
   * @param restitution1
   * @param restitution2
   * @return
   */
  public static final float mixRestitution(float restitution1, float restitution2) {
    return restitution1 > restitution2 ? restitution1 : restitution2;
  }
} public class Vec2 implements Serializable {
  private static final long serialVersionUID = 1L;

  public float x, y;

  public Vec2() {
    this(0, 0);
  }

  public Vec2(float x, float y) {
    this.x = x;
    this.y = y;
  }

  public Vec2(Vec2 toCopy) {
    this(toCopy.x, toCopy.y);
  }

  /** Zero out this vector. */
  public final void setZero() {
    x = 0.0f;
    y = 0.0f;
  }

  /** Set the vector component-wise. */
  public final Vec2 set(float x, float y) {
    this.x = x;
    this.y = y;
    return this;
  }

  /** Set this vector to another vector. */
  public final Vec2 set(Vec2 v) {
    this.x = v.x;
    this.y = v.y;
    return this;
  }

  /** Return the sum of this vector and another; does not alter either one. */
  public final Vec2 add(Vec2 v) {
    return new Vec2(x + v.x, y + v.y);
  }



  /** Return the difference of this vector and another; does not alter either one. */
  public final Vec2 sub(Vec2 v) {
    return new Vec2(x - v.x, y - v.y);
  }

  /** Return this vector multiplied by a scalar; does not alter this vector. */
  public final Vec2 mul(float a) {
    return new Vec2(x * a, y * a);
  }

  /** Return the negation of this vector; does not alter this vector. */
  public final Vec2 negate() {
    return new Vec2(-x, -y);
  }

  /** Flip the vector and return it - alters this vector. */
  public final Vec2 negateLocal() {
    x = -x;
    y = -y;
    return this;
  }

  /** Add another vector to this one and returns result - alters this vector. */
  public final Vec2 addLocal(Vec2 v) {
    x += v.x;
    y += v.y;
    return this;
  }

  /** Adds values to this vector and returns result - alters this vector. */
  public final Vec2 addLocal(float x, float y) {
    this.x += x;
    this.y += y;
    return this;
  }

  /** Subtract another vector from this one and return result - alters this vector. */
  public final Vec2 subLocal(Vec2 v) {
    x -= v.x;
    y -= v.y;
    return this;
  }

  /** Multiply this vector by a number and return result - alters this vector. */
  public final Vec2 mulLocal(float a) {
    x *= a;
    y *= a;
    return this;
  }

  /** Get the skew vector such that dot(skew_vec, other) == cross(vec, other) */
  public final Vec2 skew() {
    return new Vec2(-y, x);
  }

  /** Get the skew vector such that dot(skew_vec, other) == cross(vec, other) */
  public final void skew(Vec2 out) {
    out.x = -y;
    out.y = x;
  }

  /** Return the length of this vector. */
  public final float length() {
    return MathUtils.sqrt(x * x + y * y);
  }

  /** Return the squared length of this vector. */
  public final float lengthSquared() {
    return (x * x + y * y);
  }

  /** Normalize this vector and return the length before normalization. Alters this vector. */
  public final float normalize() {
    float length = length();
    if (length < Settings.EPSILON) {
      return 0f;
    }

    float invLength = 1.0f / length;
    x *= invLength;
    y *= invLength;
    return length;
  }

  /** True if the vector represents a pair of valid, non-infinite floating point numbers. */
  public final boolean isValid() {
    return !Float.isNaN(x) && !Float.isInfinite(x) && !Float.isNaN(y) && !Float.isInfinite(y);
  }

  /** Return a new vector that has positive components. */
  public final Vec2 abs() {
    return new Vec2(MathUtils.abs(x), MathUtils.abs(y));
  }

  public final void absLocal() {
    x = MathUtils.abs(x);
    y = MathUtils.abs(y);
  }

  @Override
  /** Return a copy of this vector. */
  public final Vec2 clone() {
    return new Vec2(x, y);
  }

  @Override
  public final String toString() {
    return "(" + x + "," + y + ")";
  }

  /*
   * Static
   */

  public final static Vec2 abs(Vec2 a) {
    return new Vec2(MathUtils.abs(a.x), MathUtils.abs(a.y));
  }

  public final static void absToOut(Vec2 a, Vec2 out) {
    out.x = MathUtils.abs(a.x);
    out.y = MathUtils.abs(a.y);
  }

  public final static float dot(Vec2 a, Vec2 b) {
    return a.x * b.x + a.y * b.y;
  }

  public final static float cross(Vec2 a, Vec2 b) {
    return a.x * b.y - a.y * b.x;
  }

  public final static Vec2 cross(Vec2 a, float s) {
    return new Vec2(s * a.y, -s * a.x);
  }

  public final static void crossToOut(Vec2 a, float s, Vec2 out) {
    final float tempy = -s * a.x;
    out.x = s * a.y;
    out.y = tempy;
  }

  public final static void crossToOutUnsafe(Vec2 a, float s, Vec2 out) {
    assert (out != a);
    out.x = s * a.y;
    out.y = -s * a.x;
  }

  public final static Vec2 cross(float s, Vec2 a) {
    return new Vec2(-s * a.y, s * a.x);
  }

  public final static void crossToOut(float s, Vec2 a, Vec2 out) {
    final float tempY = s * a.x;
    out.x = -s * a.y;
    out.y = tempY;
  }

  public final static void crossToOutUnsafe(float s, Vec2 a, Vec2 out) {
    assert (out != a);
    out.x = -s * a.y;
    out.y = s * a.x;
  }

  public final static void negateToOut(Vec2 a, Vec2 out) {
    out.x = -a.x;
    out.y = -a.y;
  }

  public final static Vec2 min(Vec2 a, Vec2 b) {
    return new Vec2(a.x < b.x ? a.x : b.x, a.y < b.y ? a.y : b.y);
  }

  public final static Vec2 max(Vec2 a, Vec2 b) {
    return new Vec2(a.x > b.x ? a.x : b.x, a.y > b.y ? a.y : b.y);
  }

  public final static void minToOut(Vec2 a, Vec2 b, Vec2 out) {
    out.x = a.x < b.x ? a.x : b.x;
    out.y = a.y < b.y ? a.y : b.y;
  }

  public final static void maxToOut(Vec2 a, Vec2 b, Vec2 out) {
    out.x = a.x > b.x ? a.x : b.x;
    out.y = a.y > b.y ? a.y : b.y;
  }

  /**
   * @see java.lang.Object#hashCode()
   */
  @Override
  public int hashCode() { // automatically generated by Eclipse
    final int prime = 31;
    int result = 1;
    result = prime * result + Float.floatToIntBits(x);
    result = prime * result + Float.floatToIntBits(y);
    return result;
  }

  /**
   * @see java.lang.Object#equals(java.lang.Object)
   */
  @Override
  public boolean equals(Object obj) { // automatically generated by Eclipse
    if (this == obj) return true;
    if (obj == null) return false;
    if (getClass() != obj.getClass()) return false;
    Vec2 other = (Vec2) obj;
    if (Float.floatToIntBits(x) != Float.floatToIntBits(other.x)) return false;
    if (Float.floatToIntBits(y) != Float.floatToIntBits(other.y)) return false;
    return true;
  }
} public class PulleyJoint extends Joint {

  public static final float MIN_PULLEY_LENGTH = 2.0f;

  private final Vec2 m_groundAnchorA = new Vec2();
  private final Vec2 m_groundAnchorB = new Vec2();
  private float m_lengthA;
  private float m_lengthB;

  // Solver shared
  private final Vec2 m_localAnchorA = new Vec2();
  private final Vec2 m_localAnchorB = new Vec2();
  private float m_constant;
  private float m_ratio;
  private float m_impulse;

  // Solver temp
  private int m_indexA;
  private int m_indexB;
  private final Vec2 m_uA = new Vec2();
  private final Vec2 m_uB = new Vec2();
  private final Vec2 m_rA = new Vec2();
  private final Vec2 m_rB = new Vec2();
  private final Vec2 m_localCenterA = new Vec2();
  private final Vec2 m_localCenterB = new Vec2();
  private float m_invMassA;
  private float m_invMassB;
  private float m_invIA;
  private float m_invIB;
  private float m_mass;

  protected PulleyJoint(IWorldPool argWorldPool, PulleyJointDef def) {
    super(argWorldPool, def);
    m_groundAnchorA.set(def.groundAnchorA);
    m_groundAnchorB.set(def.groundAnchorB);
    m_localAnchorA.set(def.localAnchorA);
    m_localAnchorB.set(def.localAnchorB);

    assert (def.ratio != 0.0f);
    m_ratio = def.ratio;

    m_lengthA = def.lengthA;
    m_lengthB = def.lengthB;

    m_constant = def.lengthA + m_ratio * def.lengthB;
    m_impulse = 0.0f;
  }

  public float getLengthA() {
    return m_lengthA;
  }

  public float getLengthB() {
    return m_lengthB;
  }

  public float getCurrentLengthA() {
    final Vec2 p = pool.popVec2();
    m_bodyA.getWorldPointToOut(m_localAnchorA, p);
    p.subLocal(m_groundAnchorA);
    float length = p.length();
    pool.pushVec2(1);
    return length;
  }

  public float getCurrentLengthB() {
    final Vec2 p = pool.popVec2();
    m_bodyB.getWorldPointToOut(m_localAnchorB, p);
    p.subLocal(m_groundAnchorB);
    float length = p.length();
    pool.pushVec2(1);
    return length;
  }


  public Vec2 getLocalAnchorA() {
    return m_localAnchorA;
  }

  public Vec2 getLocalAnchorB() {
    return m_localAnchorB;
  }


  @Override
  public void getAnchorA(Vec2 argOut) {
    m_bodyA.getWorldPointToOut(m_localAnchorA, argOut);
  }

  @Override
  public void getAnchorB(Vec2 argOut) {
    m_bodyB.getWorldPointToOut(m_localAnchorB, argOut);
  }

  @Override
  public void getReactionForce(float inv_dt, Vec2 argOut) {
    argOut.set(m_uB).mulLocal(m_impulse).mulLocal(inv_dt);
  }

  @Override
  public float getReactionTorque(float inv_dt) {
    return 0f;
  }

  public Vec2 getGroundAnchorA() {
    return m_groundAnchorA;
  }

  public Vec2 getGroundAnchorB() {
    return m_groundAnchorB;
  }

  public float getLength1() {
    final Vec2 p = pool.popVec2();
    m_bodyA.getWorldPointToOut(m_localAnchorA, p);
    p.subLocal(m_groundAnchorA);

    float len = p.length();
    pool.pushVec2(1);
    return len;
  }

  public float getLength2() {
    final Vec2 p = pool.popVec2();
    m_bodyB.getWorldPointToOut(m_localAnchorB, p);
    p.subLocal(m_groundAnchorB);

    float len = p.length();
    pool.pushVec2(1);
    return len;
  }

  public float getRatio() {
    return m_ratio;
  }

  @Override
  public void initVelocityConstraints(final SolverData data) {
    m_indexA = m_bodyA.m_islandIndex;
    m_indexB = m_bodyB.m_islandIndex;
    m_localCenterA.set(m_bodyA.m_sweep.localCenter);
    m_localCenterB.set(m_bodyB.m_sweep.localCenter);
    m_invMassA = m_bodyA.m_invMass;
    m_invMassB = m_bodyB.m_invMass;
    m_invIA = m_bodyA.m_invI;
    m_invIB = m_bodyB.m_invI;

    Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;

    Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;

    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 temp = pool.popVec2();

    qA.set(aA);
    qB.set(aB);

    // Compute the effective masses.
    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);

    m_uA.set(cA).addLocal(m_rA).subLocal(m_groundAnchorA);
    m_uB.set(cB).addLocal(m_rB).subLocal(m_groundAnchorB);

    float lengthA = m_uA.length();
    float lengthB = m_uB.length();

    if (lengthA > 10f * Settings.linearSlop) {
      m_uA.mulLocal(1.0f / lengthA);
    } else {
      m_uA.setZero();
    }

    if (lengthB > 10f * Settings.linearSlop) {
      m_uB.mulLocal(1.0f / lengthB);
    } else {
      m_uB.setZero();
    }

    // Compute effective mass.
    float ruA = Vec2.cross(m_rA, m_uA);
    float ruB = Vec2.cross(m_rB, m_uB);

    float mA = m_invMassA + m_invIA * ruA * ruA;
    float mB = m_invMassB + m_invIB * ruB * ruB;

    m_mass = mA + m_ratio * m_ratio * mB;

    if (m_mass > 0.0f) {
      m_mass = 1.0f / m_mass;
    }

    if (data.step.warmStarting) {

      // Scale impulses to support variable time steps.
      m_impulse *= data.step.dtRatio;

      // Warm starting.
      final Vec2 PA = pool.popVec2();
      final Vec2 PB = pool.popVec2();

      PA.set(m_uA).mulLocal(-m_impulse);
      PB.set(m_uB).mulLocal(-m_ratio * m_impulse);

      vA.x += m_invMassA * PA.x;
      vA.y += m_invMassA * PA.y;
      wA += m_invIA * Vec2.cross(m_rA, PA);
      vB.x += m_invMassB * PB.x;
      vB.y += m_invMassB * PB.y;
      wB += m_invIB * Vec2.cross(m_rB, PB);

      pool.pushVec2(2);
    } else {
      m_impulse = 0.0f;
    }
    data.velocities[m_indexA].v.set(vA);
    data.velocities[m_indexA].w = wA;
    data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;

    pool.pushVec2(1);
    pool.pushRot(2);
  }

  @Override
  public void solveVelocityConstraints(final SolverData data) {
    Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;

    final Vec2 vpA = pool.popVec2();
    final Vec2 vpB = pool.popVec2();
    final Vec2 PA = pool.popVec2();
    final Vec2 PB = pool.popVec2();

    Vec2.crossToOutUnsafe(wA, m_rA, vpA);
    vpA.addLocal(vA);
    Vec2.crossToOutUnsafe(wB, m_rB, vpB);
    vpB.addLocal(vB);

    float Cdot = -Vec2.dot(m_uA, vpA) - m_ratio * Vec2.dot(m_uB, vpB);
    float impulse = -m_mass * Cdot;
    m_impulse += impulse;

    PA.set(m_uA).mulLocal(-impulse);
    PB.set(m_uB).mulLocal(-m_ratio * impulse);
    vA.x += m_invMassA * PA.x;
    vA.y += m_invMassA * PA.y;
    wA += m_invIA * Vec2.cross(m_rA, PA);
    vB.x += m_invMassB * PB.x;
    vB.y += m_invMassB * PB.y;
    wB += m_invIB * Vec2.cross(m_rB, PB);

    data.velocities[m_indexA].v.set(vA);
    data.velocities[m_indexA].w = wA;
    data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;

    pool.pushVec2(4);
  }

  @Override
  public boolean solvePositionConstraints(final SolverData data) {
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();
    final Vec2 uA = pool.popVec2();
    final Vec2 uB = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    final Vec2 PA = pool.popVec2();
    final Vec2 PB = pool.popVec2();

    Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;

    qA.set(aA);
    qB.set(aB);

    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);

    uA.set(cA).addLocal(rA).subLocal(m_groundAnchorA);
    uB.set(cB).addLocal(rB).subLocal(m_groundAnchorB);

    float lengthA = uA.length();
    float lengthB = uB.length();

    if (lengthA > 10.0f * Settings.linearSlop) {
      uA.mulLocal(1.0f / lengthA);
    } else {
      uA.setZero();
    }

    if (lengthB > 10.0f * Settings.linearSlop) {
      uB.mulLocal(1.0f / lengthB);
    } else {
      uB.setZero();
    }

    // Compute effective mass.
    float ruA = Vec2.cross(rA, uA);
    float ruB = Vec2.cross(rB, uB);

    float mA = m_invMassA + m_invIA * ruA * ruA;
    float mB = m_invMassB + m_invIB * ruB * ruB;

    float mass = mA + m_ratio * m_ratio * mB;

    if (mass > 0.0f) {
      mass = 1.0f / mass;
    }

    float C = m_constant - lengthA - m_ratio * lengthB;
    float linearError = MathUtils.abs(C);

    float impulse = -mass * C;

    PA.set(uA).mulLocal(-impulse);
    PB.set(uB).mulLocal(-m_ratio * impulse);

    cA.x += m_invMassA * PA.x;
    cA.y += m_invMassA * PA.y;
    aA += m_invIA * Vec2.cross(rA, PA);
    cB.x += m_invMassB * PB.x;
    cB.y += m_invMassB * PB.y;
    aB += m_invIB * Vec2.cross(rB, PB);

    data.positions[m_indexA].c.set(cA);
    data.positions[m_indexA].a = aA;
    data.positions[m_indexB].c.set(cB);
    data.positions[m_indexB].a = aB;

    pool.pushRot(2);
    pool.pushVec2(7);

    return linearError < Settings.linearSlop;
  }
} public class DefaultWorldPool implements IWorldPool {

  private final OrderedStack<Vec2> vecs;
  private final OrderedStack<Vec3> vec3s;
  private final OrderedStack<Mat22> mats;
  private final OrderedStack<Mat33> mat33s;
  private final OrderedStack<AABB> aabbs;
  private final OrderedStack<Rot> rots;

  private final HashMap<Integer, float[]> afloats = new HashMap<Integer, float[]>();
  private final HashMap<Integer, int[]> aints = new HashMap<Integer, int[]>();
  private final HashMap<Integer, Vec2[]> avecs = new HashMap<Integer, Vec2[]>();

  private final Class<?>[] classes = new Class<?>[] {IWorldPool.class};
  private final Object[] args = new Object[] {this};

  private final MutableStack<Contact, PolygonContact> pcstack =
      new MutableStack<Contact, PolygonContact>(PolygonContact.class,
          Settings.CONTACT_STACK_INIT_SIZE, classes, args);

  private final MutableStack<Contact, CircleContact> ccstack =
      new MutableStack<Contact, CircleContact>(CircleContact.class,
          Settings.CONTACT_STACK_INIT_SIZE, classes, args);

  private final MutableStack<Contact, PolygonAndCircleContact> cpstack =
      new MutableStack<Contact, PolygonAndCircleContact>(PolygonAndCircleContact.class,
          Settings.CONTACT_STACK_INIT_SIZE, classes, args);

  private final MutableStack<Contact, EdgeAndCircleContact> ecstack =
      new MutableStack<Contact, EdgeAndCircleContact>(EdgeAndCircleContact.class,
          Settings.CONTACT_STACK_INIT_SIZE, classes, args);

  private final MutableStack<Contact, EdgeAndPolygonContact> epstack =
      new MutableStack<Contact, EdgeAndPolygonContact>(EdgeAndPolygonContact.class,
          Settings.CONTACT_STACK_INIT_SIZE, classes, args);

  private final MutableStack<Contact, ChainAndCircleContact> chcstack =
      new MutableStack<Contact, ChainAndCircleContact>(ChainAndCircleContact.class,
          Settings.CONTACT_STACK_INIT_SIZE, classes, args);

  private final MutableStack<Contact, ChainAndPolygonContact> chpstack =
      new MutableStack<Contact, ChainAndPolygonContact>(ChainAndPolygonContact.class,
          Settings.CONTACT_STACK_INIT_SIZE, classes, args);

  private final Collision collision;
  private final TimeOfImpact toi;
  private final Distance dist;

  public DefaultWorldPool(int argSize, int argContainerSize) {
    vecs = new OrderedStack<Vec2>(Vec2.class, argSize, argContainerSize);
    vec3s = new OrderedStack<Vec3>(Vec3.class, argSize, argContainerSize);
    mats = new OrderedStack<Mat22>(Mat22.class, argSize, argContainerSize);
    aabbs = new OrderedStack<AABB>(AABB.class, argSize, argContainerSize);
    rots = new OrderedStack<Rot>(Rot.class, argSize, argContainerSize);
    mat33s = new OrderedStack<Mat33>(Mat33.class, argSize, argContainerSize);

    dist = new Distance();
    collision = new Collision(this);
    toi = new TimeOfImpact(this);
  }

  public final IDynamicStack<Contact> getPolyContactStack() {
    return pcstack;
  }

  public final IDynamicStack<Contact> getCircleContactStack() {
    return ccstack;
  }

  public final IDynamicStack<Contact> getPolyCircleContactStack() {
    return cpstack;
  }

  @Override
  public IDynamicStack<Contact> getEdgeCircleContactStack() {
    return ecstack;
  }

  @Override
  public IDynamicStack<Contact> getEdgePolyContactStack() {
    return epstack;
  }

  @Override
  public IDynamicStack<Contact> getChainCircleContactStack() {
    return chcstack;
  }

  @Override
  public IDynamicStack<Contact> getChainPolyContactStack() {
    return chpstack;
  }

  public final Vec2 popVec2() {
    return vecs.pop();
  }

  public final Vec2[] popVec2(int argNum) {
    return vecs.pop(argNum);
  }

  public final void pushVec2(int argNum) {
    vecs.push(argNum);
  }

  public final Vec3 popVec3() {
    return vec3s.pop();
  }

  public final Vec3[] popVec3(int argNum) {
    return vec3s.pop(argNum);
  }

  public final void pushVec3(int argNum) {
    vec3s.push(argNum);
  }

  public final Mat22 popMat22() {
    return mats.pop();
  }

  public final Mat22[] popMat22(int argNum) {
    return mats.pop(argNum);
  }

  public final void pushMat22(int argNum) {
    mats.push(argNum);
  }

  public final Mat33 popMat33() {
    return mat33s.pop();
  }

  public final void pushMat33(int argNum) {
    mat33s.push(argNum);
  }

  public final AABB popAABB() {
    return aabbs.pop();
  }

  public final AABB[] popAABB(int argNum) {
    return aabbs.pop(argNum);
  }

  public final void pushAABB(int argNum) {
    aabbs.push(argNum);
  }

  public final Rot popRot() {
    return rots.pop();
  }

  public final void pushRot(int num) {
    rots.push(num);
  }

  public final Collision getCollision() {
    return collision;
  }

  public final TimeOfImpact getTimeOfImpact() {
    return toi;
  }

  public final Distance getDistance() {
    return dist;
  }

  public final float[] getFloatArray(int argLength) {
    if (!afloats.containsKey(argLength)) {
      afloats.put(argLength, new float[argLength]);
    }

    assert (afloats.get(argLength).length == argLength) : "Array not built with correct length";
    return afloats.get(argLength);
  }

  public final int[] getIntArray(int argLength) {
    if (!aints.containsKey(argLength)) {
      aints.put(argLength, new int[argLength]);
    }

    assert (aints.get(argLength).length == argLength) : "Array not built with correct length";
    return aints.get(argLength);
  }

  public final Vec2[] getVec2Array(int argLength) {
    if (!avecs.containsKey(argLength)) {
      Vec2[] ray = new Vec2[argLength];
      for (int i = 0; i < argLength; i++) {
        ray[i] = new Vec2();
      }
      avecs.put(argLength, ray);
    }

    assert (avecs.get(argLength).length == argLength) : "Array not built with correct length";
    return avecs.get(argLength);
  }
}
import org.jbox2d.callbacks.ContactFilter;
import org.jbox2d.callbacks.ContactListener;
import org.jbox2d.callbacks.DebugDraw;
import org.jbox2d.callbacks.DestructionListener;
import org.jbox2d.callbacks.QueryCallback;
import org.jbox2d.callbacks.RayCastCallback;
import org.jbox2d.callbacks.TreeCallback;
import org.jbox2d.callbacks.TreeRayCastCallback;
import org.jbox2d.collision.AABB;
import org.jbox2d.collision.RayCastInput;
import org.jbox2d.collision.RayCastOutput;
import org.jbox2d.collision.TimeOfImpact.TOIInput;
import org.jbox2d.collision.TimeOfImpact.TOIOutput;
import org.jbox2d.collision.TimeOfImpact.TOIOutputState;
import org.jbox2d.collision.broadphase.BroadPhase;
import org.jbox2d.collision.shapes.ChainShape;
import org.jbox2d.collision.shapes.CircleShape;
import org.jbox2d.collision.shapes.EdgeShape;
import org.jbox2d.collision.shapes.PolygonShape;
import org.jbox2d.collision.shapes.ShapeType;
import org.jbox2d.common.Color3f;
import org.jbox2d.common.MathUtils;
import org.jbox2d.common.Settings;
import org.jbox2d.common.Sweep;
import org.jbox2d.common.Timer;
import org.jbox2d.common.Transform;
import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.contacts.Contact;
import org.jbox2d.dynamics.contacts.ContactEdge;
import org.jbox2d.dynamics.contacts.ContactRegister;
import org.jbox2d.dynamics.joints.Joint;
import org.jbox2d.dynamics.joints.JointDef;
import org.jbox2d.dynamics.joints.JointEdge;
import org.jbox2d.dynamics.joints.PulleyJoint;
import org.jbox2d.pooling.IDynamicStack;
import org.jbox2d.pooling.IWorldPool;
import org.jbox2d.pooling.arrays.Vec2Array;
import org.jbox2d.pooling.normal.DefaultWorldPool;
        Vec2 s2 = pulley.getGroundAnchorB();
        m_debugDraw.drawSegment(s1, p1, color);
        m_debugDraw.drawSegment(s2, p2, color);
        m_debugDraw.drawSegment(s1, s2, color);
      }
        break;
      case CONSTANT_VOLUME:
      case MOUSE:
        // don't draw this
        break;
      default:
        m_debugDraw.drawSegment(x1, p1, color);
        m_debugDraw.drawSegment(p1, p2, color);
        m_debugDraw.drawSegment(x2, p2, color);
    }
    pool.pushVec2(2);
  }

  // NOTE this corresponds to the liquid test, so the debugdraw can draw
  // the liquid particles correctly. They should be the same.
  private static Integer LIQUID_INT = new Integer(1234598372);
  private float liquidLength = .12f;
  private float averageLinearVel = -1;
  private final Vec2 liquidOffset = new Vec2();
  private final Vec2 circCenterMoved = new Vec2();
  private final Color3f liquidColor = new Color3f(.4f, .4f, 1f);

  private final Vec2 center = new Vec2();
  private final Vec2 axis = new Vec2();
  private final Vec2 v1 = new Vec2();
  private final Vec2 v2 = new Vec2();
  private final Vec2Array tlvertices = new Vec2Array();

  private void drawShape(Fixture fixture, Transform xf, Color3f color) {
    switch (fixture.getType()) {
      case CIRCLE: {
        CircleShape circle = (CircleShape) fixture.getShape();

        // Vec2 center = Mul(xf, circle.m_p);
        Transform.mulToOutUnsafe(xf, circle.m_p, center);
        float radius = circle.m_radius;
        xf.q.getXAxis(axis);

        if (fixture.getUserData() != null && fixture.getUserData().equals(LIQUID_INT)) {
          Body b = fixture.getBody();
          liquidOffset.set(b.m_linearVelocity);
          float linVelLength = b.m_linearVelocity.length();
          if (averageLinearVel == -1) {
            averageLinearVel = linVelLength;
          } else {
            averageLinearVel = .98f * averageLinearVel + .02f * linVelLength;
          }
          liquidOffset.mulLocal(liquidLength / averageLinearVel / 2);
          circCenterMoved.set(center).addLocal(liquidOffset);
          center.subLocal(liquidOffset);
          m_debugDraw.drawSegment(center, circCenterMoved, liquidColor);
          return;
        }

        m_debugDraw.drawSolidCircle(center, radius, axis, color);
      }
        break;

      case POLYGON: {
        PolygonShape poly = (PolygonShape) fixture.getShape();
        int vertexCount = poly.m_count;
        assert (vertexCount <= Settings.maxPolygonVertices);
        Vec2[] vertices = tlvertices.get(Settings.maxPolygonVertices);

        for (int i = 0; i < vertexCount; ++i) {
          // vertices[i] = Mul(xf, poly.m_vertices[i]);
          Transform.mulToOutUnsafe(xf, poly.m_vertices[i], vertices[i]);
        }

        m_debugDraw.drawSolidPolygon(vertices, vertexCount, color);
      }
        break;
      case EDGE: {
        EdgeShape edge = (EdgeShape) fixture.getShape();
        Transform.mulToOutUnsafe(xf, edge.m_vertex1, v1);
        Transform.mulToOutUnsafe(xf, edge.m_vertex2, v2);
        m_debugDraw.drawSegment(v1, v2, color);
      }
        break;

      case CHAIN: {
        ChainShape chain = (ChainShape) fixture.getShape();
        int count = chain.m_count;
        Vec2[] vertices = chain.m_vertices;

        Transform.mulToOutUnsafe(xf, vertices[0], v1);
        for (int i = 1; i < count; ++i) {
          Transform.mulToOutUnsafe(xf, vertices[i], v2);
          m_debugDraw.drawSegment(v1, v2, color);
          m_debugDraw.drawCircle(v1, 0.05f, color);
          v1.set(v2);
        }
      }
        break;
      default:
        break;
    }
  }
}


class WorldQueryWrapper implements TreeCallback {
  public boolean treeCallback(int nodeId) {
    FixtureProxy proxy = (FixtureProxy) broadPhase.getUserData(nodeId);
    return callback.reportFixture(proxy.fixture);
  }

  BroadPhase broadPhase;
  QueryCallback callback;
};


class WorldRayCastWrapper implements TreeRayCastCallback {

  // djm pooling
  private final RayCastOutput output = new RayCastOutput();