boundingcapsule.java

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

        compLine1.getDirection().normalizeLocal();

        // Cylinder with axis 'compLine1' must contain the spheres centered at
        // the end points of the input capsules.
        ls.getPositiveEnd(compVec1);
        float maxRadius = compLine1.distance(compVec1) + radius;

        ls.getNegativeEnd(compVec2);
        float temp = compLine1.distance(compVec2) + radius;
        if (temp > maxRadius) {
            maxRadius = temp;
        }

        temp = compLine1.distance(sphere.getCenter()) + sphere.getRadius();
        if (temp > maxRadius) {
            maxRadius = temp;
        }

        // process sphere <PosEnd0,r0>
        float radiusDiff = maxRadius - sphere.getRadius();
        float radiusDiffSqr = radiusDiff * radiusDiff;
        diff = compLine1.getOrigin().subtract(compVec1, diff);
        float lengthMinusDiff = diff.lengthSquared() - radiusDiffSqr;
        float dotDirection = diff.dot(compLine1.getDirection());
        float discr = dotDirection * dotDirection - lengthMinusDiff;
        float root = FastMath.sqrt(FastMath.abs(discr));
        float tPositive = -dotDirection - root;
        float tNegative = -dotDirection + root;

        // process sphere <NegEnd0,r0>
        diff = compLine1.getOrigin().subtract(compVec2, diff);
        lengthMinusDiff = diff.lengthSquared() - radiusDiffSqr;
        dotDirection = diff.dot(compLine1.getDirection());
        discr = dotDirection * dotDirection - lengthMinusDiff;
        root = FastMath.sqrt(FastMath.abs(discr));
        temp = -dotDirection - root;
        if (temp > tPositive) {
            tPositive = temp;
        }
        temp = -dotDirection + root;
        if (temp < tNegative) {
            tNegative = temp;
        }

        // process sphere <PosEnd1,r1>
        radiusDiff = maxRadius - sphere.getRadius();
        radiusDiffSqr = radiusDiff * radiusDiff;
        diff = compLine1.getOrigin().subtract(sphere.getCenter(), diff);
        lengthMinusDiff = diff.lengthSquared() - radiusDiffSqr;
        dotDirection = diff.dot(compLine1.getDirection());
        discr = dotDirection * dotDirection - lengthMinusDiff;
        root = FastMath.sqrt(FastMath.abs(discr));
        temp = -dotDirection - root;
        if (temp > tPositive) {
            tPositive = temp;
        }
        temp = -dotDirection + root;
        if (temp < tNegative) {
            tNegative = temp;
        }

        merged.setRadius(maxRadius);
        merged.getLineSegment().getOrigin().set(
                compLine1.getOrigin().add(
                        compLine1.getDirection().mult(
                                ((float) 0.5) * (tPositive + tNegative))));
        merged.getLineSegment().getDirection().set(compLine1.getDirection());

        if (tPositive > tNegative) {
            // container is a capsule
            merged.getLineSegment().setExtent((0.5f) * (tPositive - tNegative));
        } else {
            // container is a sphere
            merged.getLineSegment().setExtent(0.0f);
        }

        return merged;
    }

    public BoundingCapsule mergeCapsule(BoundingCapsule capsule,
            BoundingCapsule merged) {

        if (contains(capsule)) {
            return this;
        }

        if (capsule.contains(this)) {
            return capsule;
        }

        // axis center is average of input axis centers

        compLine1.getOrigin().set(
                ls.getOrigin().add(capsule.getLineSegment().getOrigin(),
                        compVec1).mult(0.5f, compVec1));

        // axis unit direction is average of input axis unit directions
        if (ls.getDirection().dot(capsule.getLineSegment().getDirection()) >= 0.0f) {
            compLine1.getDirection().set(
                    ls.getDirection().add(
                            capsule.getLineSegment().getDirection(), compVec1));
        } else {
            compLine1.getDirection().set(
                    ls.getDirection().subtract(
                            capsule.getLineSegment().getDirection(), compVec1));
        }

        compLine1.getDirection().normalizeLocal();

        // Cylinder with axis 'compLine1' must contain the spheres centered at
        // the end points of the input capsules.
        ls.getPositiveEnd(compVec1);
        float maxRadius = compLine1.distance(compVec1) + radius;

        ls.getNegativeEnd(compVec2);
        float temp = compLine1.distance(compVec2) + radius;
        if (temp > maxRadius) {
            maxRadius = temp;
        }

        capsule.getLineSegment().getPositiveEnd(compVec3);
        temp = compLine1.distance(compVec3) + capsule.getRadius();
        if (temp > maxRadius) {
            maxRadius = temp;
        }

        capsule.getLineSegment().getNegativeEnd(compVec4);
        temp = compLine1.distance(compVec4) + capsule.getRadius();
        if (temp > maxRadius) {
            maxRadius = temp;
        }

        // process sphere <PosEnd0,r0>
        float radiusDiff = maxRadius - capsule.getRadius();
        float radiusDiffSqr = radiusDiff * radiusDiff;
        diff = compLine1.getOrigin().subtract(compVec1, diff);
        float lengthMinusDiff = diff.lengthSquared() - radiusDiffSqr;
        float dotDirection = diff.dot(compLine1.getDirection());
        float discr = dotDirection * dotDirection - lengthMinusDiff;
        float root = FastMath.sqrt(FastMath.abs(discr));
        float tPositive = -dotDirection - root;
        float tNegative = -dotDirection + root;

        // process sphere <NegEnd0,r0>
        diff = compLine1.getOrigin().subtract(compVec2, diff);
        lengthMinusDiff = diff.lengthSquared() - radiusDiffSqr;
        dotDirection = diff.dot(compLine1.getDirection());
        discr = dotDirection * dotDirection - lengthMinusDiff;
        root = FastMath.sqrt(FastMath.abs(discr));
        temp = -dotDirection - root;
        if (temp > tPositive) {
            tPositive = temp;
        }
        temp = -dotDirection + root;
        if (temp < tNegative) {
            tNegative = temp;
        }

        // process sphere <PosEnd1,r1>
        radiusDiff = maxRadius - capsule.getRadius();
        radiusDiffSqr = radiusDiff * radiusDiff;
        diff = compLine1.getOrigin().subtract(compVec3, diff);
        lengthMinusDiff = diff.lengthSquared() - radiusDiffSqr;
        dotDirection = diff.dot(compLine1.getDirection());
        discr = dotDirection * dotDirection - lengthMinusDiff;
        root = FastMath.sqrt(FastMath.abs(discr));
        temp = -dotDirection - root;
        if (temp > tPositive) {
            tPositive = temp;
        }
        temp = -dotDirection + root;
        if (temp < tNegative) {
            tNegative = temp;
        }

        // process sphere <NegEnd1,r1>
        diff = compLine1.getOrigin().subtract(compVec4, diff);
        lengthMinusDiff = diff.lengthSquared() - radiusDiffSqr;
        dotDirection = diff.dot(compLine1.getDirection());
        discr = dotDirection * dotDirection - lengthMinusDiff;
        root = FastMath.sqrt(FastMath.abs(discr));
        temp = -dotDirection - root;
        if (temp > tPositive) {
            tPositive = temp;
        }
        temp = -dotDirection + root;
        if (temp < tNegative) {
            tNegative = temp;
        }

        merged.setRadius(maxRadius);
        merged.getLineSegment().getOrigin().set(
                compLine1.getOrigin().add(
                        compLine1.getDirection().mult(
                                ((float) 0.5) * (tPositive + tNegative))));
        merged.getLineSegment().getDirection().set(compLine1.getDirection());

        if (tPositive > tNegative) {
            // container is a capsule
            merged.getLineSegment().setExtent((0.5f) * (tPositive - tNegative));
        } else {
            // container is a sphere
            merged.getLineSegment().setExtent(0.0f);
        }

        return merged;
    }

    @Override
    public BoundingVolume mergeLocal(BoundingVolume volume) {
        if (volume == null) {
            return this;
        }

        switch (volume.getType()) {

            case Capsule: {
                return mergeCapsule((BoundingCapsule) volume, this);
            }

            case Sphere: {
                return mergeSphere((BoundingSphere) volume, this);
            }

            case AABB: {
                BoundingBox box = (BoundingBox) volume;
                Vector3f radVect = new Vector3f(box.xExtent, box.yExtent,
                        box.zExtent);
                Vector3f temp_center = box.center;
                BoundingSphere rVal = new BoundingSphere();
                rVal.setCenter(temp_center);
                rVal.setRadius(radVect.length());
                return mergeSphere(rVal, this);
            }

            default:
                return this;
        }

    }

    @Override
    public BoundingVolume transform(Quaternion rotate, Vector3f translate,
            Vector3f scale, BoundingVolume store) {
        BoundingCapsule capsule;
        if (store == null || store.getType() != Type.Capsule) {
            capsule = new BoundingCapsule();
            capsule.setLineSegment(new LineSegment());
        } else {
            capsule = (BoundingCapsule) store;
        }

        center.mult(scale, capsule.getCenter());
        rotate.mult(capsule.getCenter(), capsule.getCenter());
        capsule.getCenter().addLocal(translate);

        ls.getOrigin().mult(scale, capsule.getLineSegment().getOrigin());
        rotate.mult(capsule.getLineSegment().getOrigin(), capsule
                .getLineSegment().getOrigin());
        capsule.getLineSegment().getOrigin().addLocal(translate);

        capsule.getLineSegment().getDirection().set(ls.getDirection());
        rotate.mult(capsule.getLineSegment().getDirection(), capsule
                .getLineSegment().getDirection());

        ls.getDirection().mult(scale, compVec1).multLocal(ls.getExtent());
        capsule.getLineSegment().setExtent(compVec1.length());

        capsule.setRadius(FastMath.abs(getMaxAxis(scale) * radius));

        return capsule;
    }

    private float getMaxAxis(Vector3f scale) {
        float x = FastMath.abs(scale.x);
        float y = FastMath.abs(scale.y);
        float z = FastMath.abs(scale.z);

        if (x >= y) {
            if (x >= z)
                return x;
            return z;
        }

        if (y >= z)
            return y;

        return z;
    }

    @Override
    public Side whichSide(Plane plane) {
        float distance = plane.pseudoDistance(ls.getNegativeEnd(compVec1));
        if (distance <= -radius) {
            distance = plane.pseudoDistance(ls.getPositiveEnd(compVec1));
            if (distance <= -radius) { return Side.NEGATIVE; }
            if (distance >=  radius) { return Side.POSITIVE; }
            return Side.NONE;
        } else if (distance >= radius) {
            return Side.POSITIVE;
        } else {
            return Side.NONE;
        }
    }

    public LineSegment getLineSegment() {
        return ls;
    }

    public void setLineSegment(LineSegment lineSegment) {
        this.ls = lineSegment;
    }

    public float getRadius() {
        return radius;
    }

    public void setRadius(float radius) {
        this.radius = radius;
    }

    @Override
    public float getVolume() {
        return (4 * FastMath.ONE_THIRD * FastMath.PI * radius * radius * radius)
                + (FastMath.PI * radius * radius *
                // FIXME: replace with 2 * line segment extents once that is
                // changed over.
                (getLineSegment().getOrigin().distance(getLineSegment()
                        .getDirection())));
    }
}