orientedboundingbox.java

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

        // pmin[i] and a maximum projected value pmax[i]. The corresponding end
        // points on the axes are C+pmin[i]*D[i] and C+pmax[i]*D[i]. The point C
        // is not necessarily the midpoint for any of the intervals. The actual
        // box center will be adjusted from C to a point C' that is the midpoint
        // of each interval,
        // C' = C + sum_{i=0}^1 0.5*(pmin[i]+pmax[i])*D[i]
        // The box extents are
        // e[i] = 0.5*(pmax[i]-pmin[i])

        int i;
        float fDot;
        Vector3f kDiff = _compVect4;
        Vector3f kMin = _compVect5;
        Vector3f kMax = _compVect6;
        kMin.zero();
        kMax.zero();

        if (!rkBox0.correctCorners)
            rkBox0.computeCorners();
        for (i = 0; i < 8; i++) {
            rkBox0.vectorStore[i].subtract(kBoxCenter, kDiff);

            fDot = kDiff.dot(newXaxis);
            if (fDot > kMax.x)
                kMax.x = fDot;
            else if (fDot < kMin.x)
                kMin.x = fDot;

            fDot = kDiff.dot(newYaxis);
            if (fDot > kMax.y)
                kMax.y = fDot;
            else if (fDot < kMin.y)
                kMin.y = fDot;

            fDot = kDiff.dot(newZaxis);
            if (fDot > kMax.z)
                kMax.z = fDot;
            else if (fDot < kMin.z)
                kMin.z = fDot;

        }

        if (!rkBox1.correctCorners)
            rkBox1.computeCorners();
        for (i = 0; i < 8; i++) {
            rkBox1.vectorStore[i].subtract(kBoxCenter, kDiff);

            fDot = kDiff.dot(newXaxis);
            if (fDot > kMax.x)
                kMax.x = fDot;
            else if (fDot < kMin.x)
                kMin.x = fDot;

            fDot = kDiff.dot(newYaxis);
            if (fDot > kMax.y)
                kMax.y = fDot;
            else if (fDot < kMin.y)
                kMin.y = fDot;

            fDot = kDiff.dot(newZaxis);
            if (fDot > kMax.z)
                kMax.z = fDot;
            else if (fDot < kMin.z)
                kMin.z = fDot;
        }

        this.xAxis.set(newXaxis);
        this.yAxis.set(newYaxis);
        this.zAxis.set(newZaxis);

        this.extent.x = .5f * (kMax.x - kMin.x);
        kBoxCenter.addLocal(this.xAxis.mult(.5f * (kMax.x + kMin.x), tempVe));

        this.extent.y = .5f * (kMax.y - kMin.y);
        kBoxCenter.addLocal(this.yAxis.mult(.5f * (kMax.y + kMin.y), tempVe));

        this.extent.z = .5f * (kMax.z - kMin.z);
        kBoxCenter.addLocal(this.zAxis.mult(.5f * (kMax.z + kMin.z), tempVe));

        this.center.set(kBoxCenter);

        this.correctCorners = false;
        return this;
    }

    public BoundingVolume clone(BoundingVolume store) {
        OrientedBoundingBox toReturn;
        if (store instanceof OrientedBoundingBox) {
            toReturn = (OrientedBoundingBox) store;
        } else {
            toReturn = new OrientedBoundingBox();
        }
        toReturn.extent.set(extent);
        toReturn.xAxis.set(xAxis);
        toReturn.yAxis.set(yAxis);
        toReturn.zAxis.set(zAxis);
        toReturn.center.set(center);
        toReturn.checkPlane = checkPlane;
        for (int x = vectorStore.length; --x >= 0; )
            toReturn.vectorStore[x].set(vectorStore[x]);
        toReturn.correctCorners = this.correctCorners;
        return toReturn;
    }

    /**
     * Sets the vectorStore information to the 8 corners of the box.
     */
    public void computeCorners() {
        Vector3f akEAxis0 = xAxis.mult(extent.x, _compVect1);
        Vector3f akEAxis1 = yAxis.mult(extent.y, _compVect2);
        Vector3f akEAxis2 = zAxis.mult(extent.z, _compVect3);
        
        vectorStore[0].set(center).subtractLocal(akEAxis0).subtractLocal(akEAxis1).subtractLocal(akEAxis2);
        vectorStore[1].set(center).addLocal(akEAxis0).subtractLocal(akEAxis1).subtractLocal(akEAxis2);
        vectorStore[2].set(center).addLocal(akEAxis0).addLocal(akEAxis1).subtractLocal(akEAxis2);
        vectorStore[3].set(center).subtractLocal(akEAxis0).addLocal(akEAxis1).subtractLocal(akEAxis2);
        vectorStore[4].set(center).subtractLocal(akEAxis0).subtractLocal(akEAxis1).addLocal(akEAxis2);
        vectorStore[5].set(center).addLocal(akEAxis0).subtractLocal(akEAxis1).addLocal(akEAxis2);
        vectorStore[6].set(center).addLocal(akEAxis0).addLocal(akEAxis1).addLocal(akEAxis2);
        vectorStore[7].set(center).subtractLocal(akEAxis0).addLocal(akEAxis1).addLocal(akEAxis2);
        correctCorners = true;
    }
    
    public void computeFromTris(int[] indices, TriMesh mesh, int start, int end) {
        if (end - start <= 0) {
            return;
        }
        Vector3f[] verts = new Vector3f[3];
        Vector3f min = _compVect1.set(new Vector3f(Float.POSITIVE_INFINITY, Float.POSITIVE_INFINITY, Float.POSITIVE_INFINITY));
        Vector3f max = _compVect2.set(new Vector3f(Float.NEGATIVE_INFINITY, Float.NEGATIVE_INFINITY, Float.NEGATIVE_INFINITY));
        Vector3f point;
        for (int i = start; i < end; i++) {
        	mesh.getTriangle(indices[i], verts);
            point = verts[0];
            if (point.x < min.x)
                min.x = point.x;
            else if (point.x > max.x)
                max.x = point.x;
            if (point.y < min.y)
                min.y = point.y;
            else if (point.y > max.y)
                max.y = point.y;
            if (point.z < min.z)
                min.z = point.z;
            else if (point.z > max.z)
                max.z = point.z;

            point = verts[1];
            if (point.x < min.x)
                min.x = point.x;
            else if (point.x > max.x)
                max.x = point.x;
            if (point.y < min.y)
                min.y = point.y;
            else if (point.y > max.y)
                max.y = point.y;
            if (point.z < min.z)
                min.z = point.z;
            else if (point.z > max.z)
                max.z = point.z;

            point = verts[2];
            if (point.x < min.x)
                min.x = point.x;
            else if (point.x > max.x)
                max.x = point.x;

            if (point.y < min.y)
                min.y = point.y;
            else if (point.y > max.y)
                max.y = point.y;

            if (point.z < min.z)
                min.z = point.z;
            else if (point.z > max.z)
                max.z = point.z;
        }

        center.set(min.addLocal(max));
        center.multLocal(0.5f);

        extent.set(max.x - center.x, max.y - center.y, max.z - center.z);

        xAxis.set(1, 0, 0);
        yAxis.set(0, 1, 0);
        zAxis.set(0, 0, 1);
        
        correctCorners = false;
    }

    public void computeFromTris(Triangle[] tris, int start, int end) {
        if (end - start <= 0) {
            return;
        }

        Vector3f min = _compVect1.set(tris[start].get(0));
        Vector3f max = _compVect2.set(min);
        Vector3f point;
        for (int i = start; i < end; i++) {

            point = tris[i].get(0);
            if (point.x < min.x)
                min.x = point.x;
            else if (point.x > max.x)
                max.x = point.x;
            if (point.y < min.y)
                min.y = point.y;
            else if (point.y > max.y)
                max.y = point.y;
            if (point.z < min.z)
                min.z = point.z;
            else if (point.z > max.z)
                max.z = point.z;

            point = tris[i].get(1);
            if (point.x < min.x)
                min.x = point.x;
            else if (point.x > max.x)
                max.x = point.x;
            if (point.y < min.y)
                min.y = point.y;
            else if (point.y > max.y)
                max.y = point.y;
            if (point.z < min.z)
                min.z = point.z;
            else if (point.z > max.z)
                max.z = point.z;

            point = tris[i].get(2);
            if (point.x < min.x)
                min.x = point.x;
            else if (point.x > max.x)
                max.x = point.x;

            if (point.y < min.y)
                min.y = point.y;
            else if (point.y > max.y)
                max.y = point.y;

            if (point.z < min.z)
                min.z = point.z;
            else if (point.z > max.z)
                max.z = point.z;
        }

        center.set(min.addLocal(max));
        center.multLocal(0.5f);

        extent.set(max.x - center.x, max.y - center.y, max.z - center.z);

        xAxis.set(1, 0, 0);
        yAxis.set(0, 1, 0);
        zAxis.set(0, 0, 1);
        
        correctCorners = false;
    }

    public boolean intersection(OrientedBoundingBox box1) {
        // Cutoff for cosine of angles between box axes. This is used to catch
        // the cases when at least one pair of axes are parallel. If this
        // happens,
        // there is no need to test for separation along the Cross(A[i],B[j])
        // directions.
        OrientedBoundingBox box0 = this;
        float cutoff = 0.999999f;
        boolean parallelPairExists = false;
        int i;

        // convenience variables
        Vector3f akA[] = new Vector3f[] { box0.xAxis, box0.yAxis, box0.zAxis };
        Vector3f[] akB = new Vector3f[] { box1.xAxis, box1.yAxis, box1.zAxis };
        Vector3f afEA = box0.extent;
        Vector3f afEB = box1.extent;

        // compute difference of box centers, D = C1-C0
        Vector3f kD = box1.center.subtract(box0.center, _compVect1);

        float[][] aafC = { fWdU, fAWdU, fDdU };

        float[][] aafAbsC = { fADdU, fAWxDdU, tempFa };

        float[] afAD = tempFb;
        float fR0, fR1, fR; // interval radii and distance between centers
        float fR01; // = R0 + R1

        // axis C0+t*A0
        for (i = 0; i < 3; i++) {
            aafC[0][i] = akA[0].dot(akB[i]);
            aafAbsC[0][i] = FastMath.abs(aafC[0][i]);
            if (aafAbsC[0][i] > cutoff) {
                parallelPairExists = true;
            }
        }
        afAD[0] = akA[0].dot(kD);
        fR = FastMath.abs(afAD[0]);
        fR1 = afEB.x * aafAbsC[0][0] + afEB.y * aafAbsC[0][1] + afEB.z
                * aafAbsC[0][2];
        fR01 = afEA.x + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A1
        for (i = 0; i < 3; i++) {
            aafC[1][i] = akA[1].dot(akB[i]);
            aafAbsC[1][i] = FastMath.abs(aafC[1][i]);
            if (aafAbsC[1][i] > cutoff) {
                parallelPairExists = true;
            }
        }
        afAD[1] = akA[1].dot(kD);
        fR = FastMath.abs(afAD[1]);
        fR1 = afEB.x * aafAbsC[1][0] + afEB.y * aafAbsC[1][1] + afEB.z
                * aafAbsC[1][2];
        fR01 = afEA.y + fR1;
        if (fR > fR01) {
            return false;
        }

        // axis C0+t*A2
        for (i = 0; i < 3; i++) {
            aafC[2][i] = akA[2].dot(akB[i]);
            aafAbsC[2][i] = FastMath.abs(aafC[2][i]);
            if (aafAbsC[2][i] > cutoff) {
                parallelPairExists = true;
            }
        }
        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];