pbox2d 0.6.0-java → 0.8.0-java

data/src/org/jbox2d/collision/AABB.java

@@ -0,0 +1,338 @@
+/**
+ * *****************************************************************************
+ * 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;
+
+import org.jbox2d.common.MathUtils;
+import org.jbox2d.common.Settings;
+import org.jbox2d.common.Vec2;
+import org.jbox2d.pooling.IWorldPool;
+import org.jbox2d.pooling.normal.DefaultWorldPool;
+
+/**
+ * An axis-aligned bounding box.
+ */
+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 upperVertex
+     */
+    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
+     *
+     * @return
+     */
+    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.
+     *
+     * @param 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
+     * @param argPool
+     * @return
+     */
+    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;
+        }
+        return !(a.lowerBound.x - b.upperBound.x > 0.0f || a.lowerBound.y - b.upperBound.y > 0.0f);
+    }
+
+    @Override
+    public final String toString() {
+        final String s = "AABB[" + lowerBound + " . " + upperBound + "]";
+        return s;
+    }
+}