boundingcapsule.java

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

/*
 * Copyright (c) 2003-2009 jMonkeyEngine
 * 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.
 *
 * * Neither the name of 'jMonkeyEngine' nor the names of its contributors
 *   may be used to endorse or promote products derived from this software
 *   without specific prior written permission.
 *
 * 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 OWNER 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 com.jme.bounding;

import java.nio.FloatBuffer;

import com.jme.intersection.IntersectionRecord;
import com.jme.math.FastMath;
import com.jme.math.Line;
import com.jme.math.LineSegment;
import com.jme.math.Plane;
import com.jme.math.Quaternion;
import com.jme.math.Ray;
import com.jme.math.Triangle;
import com.jme.math.Vector3f;
import com.jme.math.Plane.Side;
import com.jme.scene.TriMesh;
import com.jme.util.geom.BufferUtils;

public class BoundingCapsule extends BoundingVolume {
    private static final long serialVersionUID = 1L;

    private transient LineSegment ls;

    private float radius;

    private static Vector3f compVec1 = new Vector3f();
    private static Vector3f compVec2 = new Vector3f();
    private static Vector3f compVec3 = new Vector3f();
    private static Vector3f compVec4 = new Vector3f();
    private static Vector3f diff = new Vector3f();
    private static Line compLine1 = new Line();

    public BoundingCapsule() {
        ls = new LineSegment();
    }

    public BoundingCapsule(Vector3f center, LineSegment ls, float radius) {
        this.center = center;
        this.ls = ls;
        this.radius = radius;
    }

    @Override
    public BoundingVolume clone(BoundingVolume store) {
        if (store != null && store.getType() == Type.Capsule) {
            BoundingCapsule capsule = (BoundingCapsule) store;
            capsule.getCenter().set(center);
            capsule.setRadius(radius);
            capsule.getLineSegment().set(ls);
            capsule.checkPlane = checkPlane;
            return capsule;
        }

        BoundingCapsule capsule = new BoundingCapsule(
                (Vector3f) center.clone(), (LineSegment) ls.clone(), radius);
        return capsule;
    }

    @Override
    public void computeFromPoints(FloatBuffer points) {
        leastSquaresFit(points);
    }

    /**
     * Least-Squares Fit will fit the points by a line using the least-squares
     * algorithm. Let the line be A + tW, with W unit length and A the average
     * of the data points. This line contains the capsule line segment. Compute
     * the radius to be the maximum distance from the data points to the line.
     * 
     * @param points
     */
    private void leastSquaresFit(FloatBuffer points) {
        if (points == null) {
            return;
        }

        points.rewind();
        if (points.remaining() <= 2) { // we need at least a 3 float vector
            return;
        }

        if (ls == null) {
            ls = new LineSegment();
        }

        int length = points.remaining() / 3;

        compLine1.orthogonalLineFit(points);

        float maxRadiusSquared = 0f;
        for (int i = 0; i < length; i++) {
            BufferUtils.populateFromBuffer(compVec1, points, i);
            float radiusSquared = compLine1.distanceSquared(compVec1);
            if (radiusSquared > maxRadiusSquared) {
                maxRadiusSquared = radiusSquared;
            }
        }

        Vector3f u = new Vector3f();
        Vector3f v = new Vector3f();
        Vector3f w = new Vector3f(compLine1.getDirection()).normalizeLocal();

        Vector3f.generateComplementBasis(u, v, w);
        float min = Float.POSITIVE_INFINITY;
        float max = Float.NEGATIVE_INFINITY;

        for (int i = 0; i < length; i++) {
            BufferUtils.populateFromBuffer(compVec1, points, i);
            compVec1.subtractLocal(compLine1.getOrigin());
            float uDot = u.dot(compVec1);
            float vDot = v.dot(compVec1);
            float wDot = w.dot(compVec1);
            float discrimenator = maxRadiusSquared
                    - (uDot * uDot + vDot * vDot);
            float radical = FastMath.sqrt(FastMath.abs(discrimenator));

            float testValue = wDot + radical;
            if (testValue < min) {
                min = testValue;
            }

            testValue = wDot - radical;
            if (testValue > max) {
                max = testValue;
            }
        }

        radius = FastMath.sqrt(maxRadiusSquared);
        ls.getOrigin().set(
                (compLine1.getOrigin().addLocal((compLine1.getDirection().mult(
                        ((min + max) * 0.5f), compVec1)))));
        center = ls.getOrigin();
        ls.getDirection().set(compLine1.getDirection());

        if (max > min) {
            // container is a capsule
            ls.setExtent((0.5f) * (max - min));
        } else {
            // container is a sphere
            ls.setExtent(0);
        }
    }

    @Override
    public void computeFromTris(int[] triIndex, TriMesh mesh, int start,
            int end) {
        // TODO Auto-generated method stub

    }

    @Override
    public void computeFromTris(Triangle[] tris, int start, int end) {
        // TODO Auto-generated method stub

    }

    @Override
    public boolean contains(Vector3f point) {
        return ls.distanceSquared(point) <= radius * radius;
    }

    public boolean contains(BoundingSphere sphere) {
        return contains(sphere.getRadius(), sphere.getCenter());
    }

    public boolean contains(float sphereRadius, Vector3f sphereCenter) {
        float diff = radius - sphereRadius;
        if (diff >= 0.0) {
            return ls.distanceSquared(sphereCenter) <= diff * diff;
        } else {
            return false;
        }
    }

    public boolean contains(BoundingCapsule capsule) {
        return contains(capsule.getRadius(), capsule.getLineSegment()
                .getNegativeEnd(compVec1), capsule.getLineSegment()
                .getPositiveEnd(compVec2));
    }

    public boolean contains(float radius, Vector3f start, Vector3f end) {
        return contains(radius, start) && contains(radius, end);
    }

    @Override
    public float distanceToEdge(Vector3f point) {
        // return lineSegment.distance(point) - radius;
        return 0;
    }

    @Override
    public Type getType() {
        return Type.Capsule;
    }

    @Override
    public boolean intersects(BoundingVolume bv) {
        if (bv == null) {
            return false;
        }

        return bv.intersectsCapsule(this);
    }

    @Override
    public boolean intersects(Ray ray) {
        if (!Vector3f.isValidVector(center)) return false;

        float squareDistance = ls.distanceSquared(ray);
        return squareDistance <= radius * radius;
    }

    @Override
    public boolean intersectsBoundingBox(BoundingBox bb) {
        if (!Vector3f.isValidVector(center) || !Vector3f.isValidVector(bb.center)) return false;

        // TODO Auto-generated method stub
        return false;
    }

    @Override
    public boolean intersectsCapsule(BoundingCapsule bc) {
        if (!Vector3f.isValidVector(center) || !Vector3f.isValidVector(bc.center)) return false;

        float distance = ls.distance(bc.getLineSegment());
        float radiusSum = radius + bc.getRadius();
        return distance <= radiusSum;
    }

    @Override
    public boolean intersectsOrientedBoundingBox(OrientedBoundingBox obb) {
        if (!Vector3f.isValidVector(center) || !Vector3f.isValidVector(obb.center)) return false;

        // TODO Auto-generated method stub
        return false;
    }

    @Override
    public boolean intersectsSphere(BoundingSphere bs) {
        if (!Vector3f.isValidVector(center) || !Vector3f.isValidVector(bs.center)) return false;

        // float distanceSqr = lineSegment.distanceSquared(bs.getCenter());
        // return distanceSqr <= radius * bs.getRadius();
        return false;
    }

    @Override
    public IntersectionRecord intersectsWhere(Ray ray) {
        // TODO Auto-generated method stub
        return new IntersectionRecord();
    }

    @Override
    public BoundingVolume merge(BoundingVolume volume) {

        switch (volume.getType()) {
            case Capsule:
                return mergeCapsule((BoundingCapsule) volume,
                        new BoundingCapsule());
            case Sphere:
                return mergeSphere((BoundingSphere) volume,
                        new BoundingCapsule());
            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, new BoundingCapsule());
            }
            case OBB: {
                OrientedBoundingBox box = (OrientedBoundingBox) volume;
                Vector3f temp_center = box.center;
                BoundingSphere rVal = new BoundingSphere();
                rVal.setCenter(temp_center);
                rVal.setRadius(0);
                rVal.mergeLocal(box);
                return mergeSphere(rVal, new BoundingCapsule());
            }
            default:
                return null;
        }
    }

    public BoundingCapsule mergeSphere(BoundingSphere sphere,
            BoundingCapsule merged) {
        if (contains(sphere)) {
            return this;
        }

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

        // axis center is average of input axis centers

        compLine1.getOrigin().set(
                ls.getOrigin().add(sphere.getCenter(), compVec1).mult(0.5f,
                        compVec1));

        compLine1.getDirection().set(ls.getDirection());