orientedboundingbox.java

java 3d game jme 工程开发源代码

        afAD[2] = akA[2].dot(kD);
        fR = FastMath.abs(afAD[2]);
        fR1 = afEB.x * aafAbsC[2][0] + afEB.y * aafAbsC[2][1] + afEB.z
                * aafAbsC[2][2];
        fR01 = afEA.z + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*B0
        fR = FastMath.abs(akB[0].dot(kD));
        fR0 = afEA.x * aafAbsC[0][0] + afEA.y * aafAbsC[1][0] + afEA.z
                * aafAbsC[2][0];
        fR01 = fR0 + afEB.x;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*B1
        fR = FastMath.abs(akB[1].dot(kD));
        fR0 = afEA.x * aafAbsC[0][1] + afEA.y * aafAbsC[1][1] + afEA.z
                * aafAbsC[2][1];
        fR01 = fR0 + afEB.y;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*B2
        fR = FastMath.abs(akB[2].dot(kD));
        fR0 = afEA.x * aafAbsC[0][2] + afEA.y * aafAbsC[1][2] + afEA.z
                * aafAbsC[2][2];
        fR01 = fR0 + afEB.z;
        if (fR > fR01) {
            return false;
        }

        // At least one pair of box axes was parallel, so the separation is
        // effectively in 2D where checking the "edge" normals is sufficient for
        // the separation of the boxes.
        if (parallelPairExists) {
            return true;
        }

        // axis C0+t*A0xB0
        fR = FastMath.abs(afAD[2] * aafC[1][0] - afAD[1] * aafC[2][0]);
        fR0 = afEA.y * aafAbsC[2][0] + afEA.z * aafAbsC[1][0];
        fR1 = afEB.y * aafAbsC[0][2] + afEB.z * aafAbsC[0][1];
        fR01 = fR0 + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A0xB1
        fR = FastMath.abs(afAD[2] * aafC[1][1] - afAD[1] * aafC[2][1]);
        fR0 = afEA.y * aafAbsC[2][1] + afEA.z * aafAbsC[1][1];
        fR1 = afEB.x * aafAbsC[0][2] + afEB.z * aafAbsC[0][0];
        fR01 = fR0 + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A0xB2
        fR = FastMath.abs(afAD[2] * aafC[1][2] - afAD[1] * aafC[2][2]);
        fR0 = afEA.y * aafAbsC[2][2] + afEA.z * aafAbsC[1][2];
        fR1 = afEB.x * aafAbsC[0][1] + afEB.y * aafAbsC[0][0];
        fR01 = fR0 + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A1xB0
        fR = FastMath.abs(afAD[0] * aafC[2][0] - afAD[2] * aafC[0][0]);
        fR0 = afEA.x * aafAbsC[2][0] + afEA.z * aafAbsC[0][0];
        fR1 = afEB.y * aafAbsC[1][2] + afEB.z * aafAbsC[1][1];
        fR01 = fR0 + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A1xB1
        fR = FastMath.abs(afAD[0] * aafC[2][1] - afAD[2] * aafC[0][1]);
        fR0 = afEA.x * aafAbsC[2][1] + afEA.z * aafAbsC[0][1];
        fR1 = afEB.x * aafAbsC[1][2] + afEB.z * aafAbsC[1][0];
        fR01 = fR0 + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A1xB2
        fR = FastMath.abs(afAD[0] * aafC[2][2] - afAD[2] * aafC[0][2]);
        fR0 = afEA.x * aafAbsC[2][2] + afEA.z * aafAbsC[0][2];
        fR1 = afEB.x * aafAbsC[1][1] + afEB.y * aafAbsC[1][0];
        fR01 = fR0 + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A2xB0
        fR = FastMath.abs(afAD[1] * aafC[0][0] - afAD[0] * aafC[1][0]);
        fR0 = afEA.x * aafAbsC[1][0] + afEA.y * aafAbsC[0][0];
        fR1 = afEB.y * aafAbsC[2][2] + afEB.z * aafAbsC[2][1];
        fR01 = fR0 + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A2xB1
        fR = FastMath.abs(afAD[1] * aafC[0][1] - afAD[0] * aafC[1][1]);
        fR0 = afEA.x * aafAbsC[1][1] + afEA.y * aafAbsC[0][1];
        fR1 = afEB.x * aafAbsC[2][2] + afEB.z * aafAbsC[2][0];
        fR01 = fR0 + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A2xB2
        fR = FastMath.abs(afAD[1] * aafC[0][2] - afAD[0] * aafC[1][2]);
        fR0 = afEA.x * aafAbsC[1][2] + afEA.y * aafAbsC[0][2];
        fR1 = afEB.x * aafAbsC[2][1] + afEB.y * aafAbsC[2][0];
        fR01 = fR0 + fR1;
        if (fR > fR01) {
            return false;
        }

        return true;
    }
    
    public boolean intersectsCapsule(BoundingCapsule bc) {
    	return bc.intersectsOrientedBoundingBox(this);
    }

    /*
     * (non-Javadoc)
     * 
     * @see com.jme.bounding.BoundingVolume#intersects(com.jme.math.Ray)
     */
    public boolean intersects(Ray ray) {
        if (!Vector3f.isValidVector(center)) return false;

        float rhs;
        Vector3f diff = ray.origin.subtract(getCenter(_compVect2), _compVect1);

        fWdU[0] = ray.getDirection().dot(xAxis);
        fAWdU[0] = FastMath.abs(fWdU[0]);
        fDdU[0] = diff.dot(xAxis);
        fADdU[0] = FastMath.abs(fDdU[0]);
        if (fADdU[0] > extent.x && fDdU[0] * fWdU[0] >= 0.0) {
            return false;
        }

        fWdU[1] = ray.getDirection().dot(yAxis);
        fAWdU[1] = FastMath.abs(fWdU[1]);
        fDdU[1] = diff.dot(yAxis);
        fADdU[1] = FastMath.abs(fDdU[1]);
        if (fADdU[1] > extent.y && fDdU[1] * fWdU[1] >= 0.0) {
            return false;
        }

        fWdU[2] = ray.getDirection().dot(zAxis);
        fAWdU[2] = FastMath.abs(fWdU[2]);
        fDdU[2] = diff.dot(zAxis);
        fADdU[2] = FastMath.abs(fDdU[2]);
        if (fADdU[2] > extent.z && fDdU[2] * fWdU[2] >= 0.0) {
            return false;
        }

        Vector3f wCrossD = ray.getDirection().cross(diff, _compVect2);

        fAWxDdU[0] = FastMath.abs(wCrossD.dot(xAxis));
        rhs = extent.y * fAWdU[2] + extent.z * fAWdU[1];
        if (fAWxDdU[0] > rhs) {
            return false;
        }

        fAWxDdU[1] = FastMath.abs(wCrossD.dot(yAxis));
        rhs = extent.x * fAWdU[2] + extent.z * fAWdU[0];
        if (fAWxDdU[1] > rhs) {
            return false;
        }

        fAWxDdU[2] = FastMath.abs(wCrossD.dot(zAxis));
        rhs = extent.x * fAWdU[1] + extent.y * fAWdU[0];
        if (fAWxDdU[2] > rhs) {
            return false;

        }

        return true;
    }

    /**
     * @see com.jme.bounding.BoundingVolume#intersectsWhere(com.jme.math.Ray)
     */
    public IntersectionRecord intersectsWhere(Ray ray) {
        Vector3f diff = _compVect1.set(ray.origin).subtractLocal(center);
        // convert ray to box coordinates
        Vector3f direction = _compVect2.set(ray.direction.x, ray.direction.y,
                ray.direction.z);
        float[] t = { 0f, Float.POSITIVE_INFINITY };
        
        float saveT0 = t[0], saveT1 = t[1];
        boolean notEntirelyClipped = clip(+direction.x, -diff.x - extent.x, t)
                && clip(-direction.x, +diff.x - extent.x, t)
                && clip(+direction.y, -diff.y - extent.y, t)
                && clip(-direction.y, +diff.y - extent.y, t)
                && clip(+direction.z, -diff.z - extent.z, t)
                && clip(-direction.z, +diff.z - extent.z, t);
        
        if (notEntirelyClipped && (t[0] != saveT0 || t[1] != saveT1)) {
            if (t[1] > t[0]) {
                float[] distances = t;
                Vector3f[] points = new Vector3f[] { 
                        new Vector3f(ray.direction).multLocal(distances[0]).addLocal(ray.origin),
                        new Vector3f(ray.direction).multLocal(distances[1]).addLocal(ray.origin)
                        };
                IntersectionRecord record = new IntersectionRecord(distances, points);
                return record;
            }
                
            float[] distances = new float[] { t[0] };
            Vector3f[] points = new Vector3f[] { 
                    new Vector3f(ray.direction).multLocal(distances[0]).addLocal(ray.origin),
                    };
            IntersectionRecord record = new IntersectionRecord(distances, points);
            return record;            
        } 
            
        return new IntersectionRecord();        

    }

    /**
     * <code>clip</code> determines if a line segment intersects the current
     * test plane.
     * 
     * @param denom
     *            the denominator of the line segment.
     * @param numer
     *            the numerator of the line segment.
     * @param t
     *            test values of the plane.
     * @return true if the line segment intersects the plane, false otherwise.
     */
    private boolean clip(float denom, float numer, float[] t) {
        // Return value is 'true' if line segment intersects the current test
        // plane. Otherwise 'false' is returned in which case the line segment
        // is entirely clipped.
        if (denom > 0.0f) {
            if (numer > denom * t[1])
                return false;
            if (numer > denom * t[0])
                t[0] = numer / denom;
            return true;
        } else if (denom < 0.0f) {
            if (numer > denom * t[0])
                return false;
            if (numer > denom * t[1])
                t[1] = numer / denom;
            return true;
        } else {
            return numer <= 0.0;
        }
    }

    public void setXAxis(Vector3f axis) {
        xAxis.set(axis);
        correctCorners = false;
    }

    public void setYAxis(Vector3f axis) {
        yAxis.set(axis);
        correctCorners = false;
    }

    public void setZAxis(Vector3f axis) {
        zAxis.set(axis);
        correctCorners = false;
    }

    public void setExtent(Vector3f ext) {
        extent.set(ext);
        correctCorners = false;
    }

    public Vector3f getXAxis() {
        return xAxis;
    }

    public Vector3f getYAxis() {
        return yAxis;
    }

    public Vector3f getZAxis() {
        return zAxis;
    }

    public Vector3f getExtent() {
        return extent;
    }

    @Override
    public boolean contains(Vector3f point) {
        _compVect1.set(point).subtractLocal(center);
        float coeff = _compVect1.dot(xAxis);
        if (FastMath.abs(coeff) > extent.x) return false;
        
        coeff = _compVect1.dot(yAxis);
        if (FastMath.abs(coeff) > extent.y) return false;
        
        coeff = _compVect1.dot(zAxis);
        if (FastMath.abs(coeff) > extent.z) return false;
        
        return true;
    }
    
    @Override
    public float distanceToEdge(Vector3f point) {
        // compute coordinates of point in box coordinate system
        Vector3f diff = point.subtract(center);
        Vector3f closest = new Vector3f(diff.dot(xAxis), diff.dot(yAxis), diff
                .dot(zAxis));

        // project test point onto box
        float sqrDistance = 0.0f;
        float delta;

        if (closest.x < -extent.x) {
            delta = closest.x + extent.x;
            sqrDistance += delta * delta;
            closest.x = -extent.x;
        } else if (closest.x > extent.x) {
            delta = closest.x - extent.x;
            sqrDistance += delta * delta;
            closest.x = extent.x;
        }

        if (closest.y < -extent.y) {
            delta = closest.y + extent.y;
            sqrDistance += delta * delta;
            closest.y = -extent.y;
        } else if (closest.y > extent.y) {
            delta = closest.y - extent.y;
            sqrDistance += delta * delta;
            closest.y = extent.y;
        }

        if (closest.z < -extent.z) {
            delta = closest.z + extent.z;
            sqrDistance += delta * delta;
            closest.z = -extent.z;
        } else if (closest.z > extent.z) {
            delta = closest.z - extent.z;
            sqrDistance += delta * delta;
            closest.z = extent.z;
        }

        return FastMath.sqrt(sqrDistance);
    }

    public void write(JMEExporter e) throws IOException {
        super.write(e);
        OutputCapsule capsule = e.getCapsule(this);
        capsule.write(xAxis, "xAxis", Vector3f.UNIT_X);
        capsule.write(yAxis, "yAxis", Vector3f.UNIT_Y);
        capsule.write(zAxis, "zAxis", Vector3f.UNIT_Z);
        capsule.write(extent, "extent", Vector3f.ZERO);
    }

    public void read(JMEImporter e) throws IOException {
        super.read(e);
        InputCapsule capsule = e.getCapsule(this);
        xAxis.set((Vector3f) capsule.readSavable("xAxis", Vector3f.UNIT_X.clone()));
        yAxis.set((Vector3f) capsule.readSavable("yAxis", Vector3f.UNIT_Y.clone()));
        zAxis.set((Vector3f) capsule.readSavable("zAxis", Vector3f.UNIT_Z.clone()));
        extent.set((Vector3f) capsule.readSavable("extent", Vector3f.ZERO.clone()));
        correctCorners = false;
    }

    @Override
    public float getVolume() {
        return (8*extent.x*extent.y*extent.z);
    }
}