Syniobots AABB class

package bone008.syniobots.shared.bounds;

import bone008.syniobots.shared.util.Vector3d;

/**
 * An immutable axis-aligned bounding box that is represented by a minimum and maximum point.
 */
public class AABB {

    private static final Vector3d AXIS_X = new Vector3d(1, 0, 0);
    private static final Vector3d AXIS_Y = new Vector3d(0, 1, 0);
    private static final Vector3d AXIS_Z = new Vector3d(0, 0, 1);


    private final Vector3d min;
    private final Vector3d max;

    /**
     * Creates a new AABB.<br>
     * Note that the parameters are cloned before being stored.
     */
    public AABB(Vector3d min, Vector3d max) {
        this.min = min.clone();
        this.max = max.clone();
    }


    // roughly based on http://gamedev.stackexchange.com/questions/32807/collision-resolve-sphere-aabb-and-aabb-aabb
    /**
     * Tests for an intersection of two axis-aligned bounding boxes, respecting a spatial offset for both of them.
     *
     * @param box1 the first box
     * @param pos1 the bias to offset the first box
     * @param box2 the second box
     * @param pos2 the bias to offset the second box
     * @param mtvOut modified only if an intersection was detected; it then contains the minimum translation vector required to move box1 out of box2
     *
     * @return true if the boxes intersect, false otherwise
     */
    public static boolean testIntersection(AABB box1, Vector3d pos1, AABB box2, Vector3d pos2, Vector3d mtvOut) {
        // initial state
        Vector3d mtvAxis = new Vector3d();
        double[] mtvDistance = { Double.MAX_VALUE }; // wrap in single-element array to allow pass-by-reference

        if (!testAxis(AXIS_X, pos1.x, box1.min.x, box1.max.x, pos2.x, box2.min.x, box2.max.x, mtvAxis, mtvDistance))
            return false;
        if (!testAxis(AXIS_Y, pos1.y, box1.min.y, box1.max.y, pos2.y, box2.min.y, box2.max.y, mtvAxis, mtvDistance))
            return false;
        if (!testAxis(AXIS_Z, pos1.z, box1.min.z, box1.max.z, pos2.z, box2.min.z, box2.max.z, mtvAxis, mtvDistance))
            return false;

        if (mtvOut != null)
            mtvOut.copy(mtvAxis).multiply(mtvDistance[0] + 1); // small bias
        return true;
    }

    private static boolean testAxis(Vector3d axis, double bias1, double min1, double max1, double bias2, double min2, double max2, Vector3d mtvAxis, double[] mtvDistance) {
        double pen1 = (bias2 + max2) - (bias1 + min1); // "left"
        double pen2 = (bias1 + max1) - (bias2 + min2); // "right"

        if (pen1 < 0 || pen2 < 0)
            return false;

        double overlap = (pen1 < pen2 ? pen1 : -pen2);

        if (overlap * overlap < mtvDistance[0] * mtvDistance[0]) {
            mtvDistance[0] = overlap;
            mtvAxis.copy(axis);
        }

        return true;
    }

}