org.scijava.java3d.BoundingPolytope Maven / Gradle / Ivy

Go to download
/*
 * Copyright 1997-2008 Sun Microsystems, Inc.  All Rights Reserved.
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * This code is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License version 2 only, as
 * published by the Free Software Foundation.  Sun designates this
 * particular file as subject to the "Classpath" exception as provided
 * by Sun in the LICENSE file that accompanied this code.
 *
 * This code is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * version 2 for more details (a copy is included in the LICENSE file that
 * accompanied this code).
 *
 * You should have received a copy of the GNU General Public License version
 * 2 along with this work; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
 * CA 95054 USA or visit www.sun.com if you need additional information or
 * have any questions.
 *
 */

package org.scijava.java3d;

import org.scijava.vecmath.Point3d;
import org.scijava.vecmath.Point4d;
import org.scijava.vecmath.Vector3d;
import org.scijava.vecmath.Vector4d;

/**
 * A BoundingPolytope defines a polyhedral bounding region using the
 * intersection of four or more half spaces.  The region defined by a
 * BoundingPolytope is always convex and must be closed.
 * 
 * Each plane in the BoundingPolytope specifies a half-space defined
 * by the equation:
 * 

 * Ax + By + Cz + D <= 0
 * 
 * where A, B, C, D are the parameters that specify the plane.  The
 * parameters are passed in the x, y, z, and w fields, respectively,
 * of a Vector4d object.  The intersection of the set of half-spaces
 * corresponding to the planes in this BoundingPolytope defines the
 * bounding region.
 */

public class BoundingPolytope extends Bounds {

    /**
     * An array of bounding planes.
     */
    Vector4d[]    planes;
    double[]      mag;        // magnitude of plane vector
    double[]      pDotN;      // point on plane dotted with normal
    Point3d[]     verts;      // vertices of polytope
    int           nVerts;     // number of verts in polytope
    Point3d       centroid = new Point3d();   // centroid of polytope

    Point3d boxVerts[];
    boolean allocBoxVerts = false;

    /**
     * Constructs a BoundingPolytope using the specified planes.
     * @param planes a set of planes defining the polytope.
     * @exception IllegalArgumentException if the length of the
     * specified array of planes is less than 4.
     */
    public BoundingPolytope(Vector4d[] planes) {
	if (planes.length < 4) {
	    throw new IllegalArgumentException(J3dI18N.getString("BoundingPolytope11"));
	}

	boundId = BOUNDING_POLYTOPE;
	int i;
	double invMag;
	this.planes = new Vector4d[planes.length];
	mag = new double[planes.length];
	pDotN  = new double[planes.length];

	for(i=0;i
     * planes[0] : x <= 1 (1,0,0,-1)

     * planes[1] : -x <= 1 (-1,0,0,-1)

     * planes[2] : y <= 1 (0,1,0,-1)

     * planes[3] : -y <= 1 (0,-1,0,-1)

     * planes[4] : z <= 1 (0,0,1,-1)

     * planes[5] : -z <= 1 (0,0,-1,-1)

     * 
     */
    public BoundingPolytope() {
	boundId = BOUNDING_POLYTOPE;
	planes = new Vector4d[6];
	mag = new double[planes.length];
	pDotN  = new double[planes.length];

	planes[0] = new Vector4d( 1.0, 0.0, 0.0, -1.0 );
	planes[1] = new Vector4d(-1.0, 0.0, 0.0, -1.0 );
	planes[2] = new Vector4d( 0.0, 1.0, 0.0, -1.0 );
	planes[3] = new Vector4d( 0.0,-1.0, 0.0, -1.0 );
	planes[4] = new Vector4d( 0.0, 0.0, 1.0, -1.0 );
	planes[5] = new Vector4d( 0.0, 0.0,-1.0, -1.0 );
	mag[0] = 1.0;
	mag[1] = 1.0;
	mag[2] = 1.0;
	mag[3] = 1.0;
	mag[4] = 1.0;
	mag[5] = 1.0;

	computeAllVerts(); // XXXX: lazy evaluate
    }


    /**
     * Constructs a BoundingPolytope from the specified bounds object.
     * The new polytope will circumscribe the region specified by the
     * input bounds.
     * @param boundsObject the bounds object from which this polytope
     * is constructed.
     */
    public BoundingPolytope(Bounds boundsObject ) {
	int i;

	boundId = BOUNDING_POLYTOPE;

	if( boundsObject == null )  {
	    boundsIsEmpty = true;
	    boundsIsInfinite = false;
	    initEmptyPolytope();
	    computeAllVerts(); // XXXX: lazy evaluate
	    return;
	}

	boundsIsEmpty = boundsObject.boundsIsEmpty;
	boundsIsInfinite = boundsObject.boundsIsInfinite;

	if( boundsObject.boundId == BOUNDING_SPHERE ) {
	    BoundingSphere sphere = (BoundingSphere)boundsObject;
	    planes = new Vector4d[6];
	    mag = new double[planes.length];
	    pDotN  = new double[planes.length];

	    planes[0] = new Vector4d( 1.0, 0.0, 0.0, -(sphere.center.x+sphere.radius) );
	    planes[1] = new Vector4d(-1.0, 0.0, 0.0,   sphere.center.x-sphere.radius );
	    planes[2] = new Vector4d( 0.0, 1.0, 0.0, -(sphere.center.y+sphere.radius) );
	    planes[3] = new Vector4d( 0.0,-1.0, 0.0,   sphere.center.y-sphere.radius );
	    planes[4] = new Vector4d( 0.0, 0.0, 1.0, -(sphere.center.z+sphere.radius) );
	    planes[5] = new Vector4d( 0.0, 0.0,-1.0,   sphere.center.z-sphere.radius );
	    mag[0] = 1.0;
	    mag[1] = 1.0;
	    mag[2] = 1.0;
	    mag[3] = 1.0;
	    mag[4] = 1.0;
	    mag[5] = 1.0;
	    computeAllVerts(); // XXXX: lazy evaluate

	} else if( boundsObject.boundId == BOUNDING_BOX ){
	    BoundingBox box = (BoundingBox)boundsObject;
	    planes = new Vector4d[6];
	    pDotN  = new double[planes.length];
	    mag = new double[planes.length];

	    planes[0] = new Vector4d( 1.0, 0.0, 0.0, -box.upper.x );
	    planes[1] = new Vector4d(-1.0, 0.0, 0.0,  box.lower.x );
	    planes[2] = new Vector4d( 0.0, 1.0, 0.0, -box.upper.y );
	    planes[3] = new Vector4d( 0.0,-1.0, 0.0,  box.lower.y );
	    planes[4] = new Vector4d( 0.0, 0.0, 1.0, -box.upper.z );
	    planes[5] = new Vector4d( 0.0, 0.0,-1.0,  box.lower.z );
	    mag[0] = 1.0;
	    mag[1] = 1.0;
	    mag[2] = 1.0;
	    mag[3] = 1.0;
	    mag[4] = 1.0;
	    mag[5] = 1.0;
	    computeAllVerts(); // XXXX: lazy evaluate

	} else if( boundsObject.boundId == BOUNDING_POLYTOPE ) {
	    BoundingPolytope polytope = (BoundingPolytope)boundsObject;
	    planes = new Vector4d[polytope.planes.length];
	    mag = new double[planes.length];
	    pDotN  = new double[planes.length];
	    nVerts = polytope.nVerts;
	    verts  = new Point3d[nVerts];
	    for(i=0;i= boundsObjects.length ) { // all bounds objects were empty
	    boundsIsEmpty = true;
	    boundsIsInfinite = false;
	    initEmptyPolytope();
	    computeAllVerts(); // XXXX: lazy evaluate
	    return;
	}

	boundsIsEmpty = boundsObjects[i].boundsIsEmpty;
	boundsIsInfinite = boundsObjects[i].boundsIsInfinite;

	if( boundsObjects[i].boundId == BOUNDING_SPHERE ) {
	    BoundingSphere sphere = (BoundingSphere)boundsObjects[i];
	    planes = new Vector4d[6];
	    mag    = new double[planes.length];
	    pDotN  = new double[planes.length];

	    planes[0] = new Vector4d( 1.0, 0.0, 0.0, -(sphere.center.x+sphere.radius) );
	    planes[1] = new Vector4d(-1.0, 0.0, 0.0,   sphere.center.x-sphere.radius );
	    planes[2] = new Vector4d( 0.0, 1.0, 0.0, -(sphere.center.y+sphere.radius) );
	    planes[3] = new Vector4d( 0.0,-1.0, 0.0,   sphere.center.y-sphere.radius );
	    planes[4] = new Vector4d( 0.0, 0.0, 1.0, -(sphere.center.z+sphere.radius) );
	    planes[5] = new Vector4d( 0.0, 0.0,-1.0,   sphere.center.z-sphere.radius );
	    mag[0] = 1.0;
	    mag[1] = 1.0;
	    mag[2] = 1.0;
	    mag[3] = 1.0;
	    mag[4] = 1.0;
	    mag[5] = 1.0;

	    computeAllVerts(); // XXXX: lazy evaluate
	} else if( boundsObjects[i].boundId == BOUNDING_BOX ){
	    BoundingBox box = (BoundingBox)boundsObjects[i];
	    planes = new Vector4d[6];
	    mag    = new double[planes.length];
	    pDotN  = new double[planes.length];

	    planes[0] = new Vector4d( 1.0, 0.0, 0.0, -box.upper.x );
	    planes[1] = new Vector4d(-1.0, 0.0, 0.0,  box.lower.x );
	    planes[2] = new Vector4d( 0.0, 1.0, 0.0, -box.upper.y );
	    planes[3] = new Vector4d( 0.0,-1.0, 0.0,  box.lower.y );
	    planes[4] = new Vector4d( 0.0, 0.0, 1.0, -box.upper.z );
	    planes[5] = new Vector4d( 0.0, 0.0,-1.0,  box.lower.z );
	    mag[0] = 1.0;
	    mag[1] = 1.0;
	    mag[2] = 1.0;
	    mag[3] = 1.0;
	    mag[4] = 1.0;
	    mag[5] = 1.0;

	    computeAllVerts(); // XXXX: lazy evaluate
	} else if( boundsObjects[i].boundId == BOUNDING_POLYTOPE ) {
	    BoundingPolytope polytope = (BoundingPolytope)boundsObjects[i];
	    planes = new Vector4d[polytope.planes.length];
	    mag    = new double[planes.length];
	    pDotN  = new double[planes.length];
	    nVerts = polytope.nVerts;
	    verts  = new Point3d[nVerts];
	    for(i=0;i 0.0) planes[i].w = -newD;
		if( (newD = ux + ly + uz ) + planes[i].w > 0.0) planes[i].w = -newD;
		if( (newD = ux + ly + lz ) + planes[i].w > 0.0) planes[i].w = -newD;

		if( (newD = lx + uy + uz ) + planes[i].w > 0.0) planes[i].w = -newD;
		if( (newD = lx + uy + lz ) + planes[i].w > 0.0) planes[i].w = -newD;
		if( (newD = lx + ly + uz ) + planes[i].w > 0.0) planes[i].w = -newD;
		if( (newD = lx + ly + lz ) + planes[i].w > 0.0) planes[i].w = -newD;
	    }

	    boundsIsEmpty = boundsObject.boundsIsEmpty;
	    boundsIsInfinite = boundsObject.boundsIsInfinite;
	    computeAllVerts(); // XXXX: lazy evaluate

	} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
	    BoundingPolytope polytope = (BoundingPolytope)boundsObject;
	    if( planes.length != polytope.planes.length) {
	        planes = new Vector4d[polytope.planes.length];
                for(k=0;kbounds object is
     * equal to this BoundingPolytope object.  They are equal if the
     * specified bounds object is an instance of
     * BoundingPolytope and all of the data
     * members of bounds are equal to the corresponding
     * data members in this BoundingPolytope.
     * @param bounds the object with which the comparison is made.
     * @return true if this BoundingPolytope is equal to bounds;
     * otherwise false
     *
     * @since Java 3D 1.2
     */
    @Override
    public boolean equals(Object bounds) {
	try {
	    BoundingPolytope polytope = (BoundingPolytope)bounds;
	    if (planes.length != polytope.planes.length)
		return false;
	    for (int i = 0; i < planes.length; i++)
		if (!planes[i].equals(polytope.planes[i]))
		    return false;

	    return true;
	}
	catch (NullPointerException e) {
	    return false;
	}
        catch (ClassCastException e) {
	    return false;
	}
    }


    /**
     * Returns a hash code value for this BoundingPolytope object
     * based on the data values in this object.  Two different
     * BoundingPolytope objects with identical data values (i.e.,
     * BoundingPolytope.equals returns true) will return the same hash
     * code value.  Two BoundingPolytope objects with different data
     * members may return the same hash code value, although this is
     * not likely.
     * @return a hash code value for this BoundingPolytope object.
     *
     * @since Java 3D 1.2
     */
    @Override
    public int hashCode() {
	long bits = 1L;

	for (int i = 0; i < planes.length; i++) {
		bits = J3dHash.mixDoubleBits(bits, planes[i].x);
		bits = J3dHash.mixDoubleBits(bits, planes[i].y);
		bits = J3dHash.mixDoubleBits(bits, planes[i].z);
		bits = J3dHash.mixDoubleBits(bits, planes[i].w);
	}

	return J3dHash.finish(bits);
    }


    /**
     * Combines this bounding polytope with a bounding object so that the
     * resulting bounding polytope encloses the original bounding polytope and the
     * given bounds object.
     * @param boundsObject another bounds object
     */
    @Override
    public void combine(Bounds boundsObject) {
        BoundingSphere sphere;

	if((boundsObject == null) || (boundsObject.boundsIsEmpty)
	   || (boundsIsInfinite))
	    return;


	if((boundsIsEmpty) || (boundsObject.boundsIsInfinite)) {
	    this.set(boundsObject);
	    return;
	}

	boundsIsEmpty = boundsObject.boundsIsEmpty;
	boundsIsInfinite = boundsObject.boundsIsInfinite;

	if( boundsObject.boundId == BOUNDING_SPHERE ) {
            sphere = (BoundingSphere)boundsObject;
 	    int i;
	    double dis;
	    for(i = 0; i < planes.length; i++){
	        dis = sphere.radius+ sphere.center.x*planes[i].x +
		    sphere.center.y*planes[i].y + sphere.center.z *
		    planes[i].z + planes[i].w;
	        if( dis > 0.0 ) {
		    planes[i].w += -dis;
                }
            }
	} else if( boundsObject  instanceof BoundingBox){
	    BoundingBox b = (BoundingBox)boundsObject;
	    if( !allocBoxVerts){
		boxVerts = new Point3d[8];
		for(int j=0;j<8;j++)boxVerts[j] = new Point3d();
		allocBoxVerts = true;
	    }
	    boxVerts[0].set(b.lower.x, b.lower.y, b.lower.z );
	    boxVerts[1].set(b.lower.x, b.upper.y, b.lower.z );
	    boxVerts[2].set(b.upper.x, b.lower.y, b.lower.z );
	    boxVerts[3].set(b.upper.x, b.upper.y, b.lower.z );
	    boxVerts[4].set(b.lower.x, b.lower.y, b.upper.z );
	    boxVerts[5].set(b.lower.x, b.upper.y, b.upper.z );
	    boxVerts[6].set(b.upper.x, b.lower.y, b.upper.z );
	    boxVerts[7].set(b.upper.x, b.upper.y, b.upper.z );
	    this.combine(boxVerts);

	} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
	    BoundingPolytope polytope = (BoundingPolytope)boundsObject;
	    this.combine(polytope.verts);
	}   else {
	    throw new IllegalArgumentException(J3dI18N.getString("BoundingPolytope3"));
	}

	computeAllVerts();
    }

    /**
     * Combines this bounding polytope with an array of bounding objects so that the
     * resulting bounding polytope encloses the original bounding polytope and the
     * given array of bounds object.
     * @param boundsObjects an array of bounds objects
     */
    @Override
    public void combine(Bounds[] boundsObjects) {
        int i=0;
	double dis;

	if( (boundsObjects == null) || (boundsObjects.length <= 0)
	    || (boundsIsInfinite))
	    return;

	// find first non empty bounds object
	while( (i= boundsObjects.length)
	    return;   // no non empty bounds so do not modify current bounds

	if(boundsIsEmpty)
	    this.set(boundsObjects[i++]);

	if(boundsIsInfinite)
	    return;

	for(;i 0.0 ) {
			planes[j].w += -dis;
		    }
		}
	    } else if( boundsObjects[i].boundId == BOUNDING_BOX){
		BoundingBox b = (BoundingBox)boundsObjects[i];
		if( !allocBoxVerts){
		    boxVerts = new Point3d[8];
		    for(int j=0;j<8;j++)boxVerts[j] = new Point3d();
		    allocBoxVerts = true;
		}
		boxVerts[0].set(b.lower.x, b.lower.y, b.lower.z );
		boxVerts[1].set(b.lower.x, b.upper.y, b.lower.z );
		boxVerts[2].set(b.upper.x, b.lower.y, b.lower.z );
		boxVerts[3].set(b.upper.x, b.upper.y, b.lower.z );
		boxVerts[4].set(b.lower.x, b.lower.y, b.upper.z );
		boxVerts[5].set(b.lower.x, b.upper.y, b.upper.z );
		boxVerts[6].set(b.upper.x, b.lower.y, b.upper.z );
		boxVerts[7].set(b.upper.x, b.upper.y, b.upper.z );
		this.combine(boxVerts);

	    } else if(boundsObjects[i] instanceof BoundingPolytope) {
		BoundingPolytope polytope = (BoundingPolytope)boundsObjects[i];
		this.combine(polytope.verts);

	    } else {
		throw new IllegalArgumentException(J3dI18N.getString("BoundingPolytope4"));
	    }

	    computeAllVerts();
	}
    }

    /**
     * Combines this bounding polytope with a point.
     * @param point a 3d point in space
     */
    @Override
    public void combine(Point3d point) {
	int i;
	double dis;

	if(boundsIsInfinite) {
	    return;
	}

	if( boundsIsEmpty ){
	    planes = new Vector4d[6];
	    mag = new double[planes.length];
	    pDotN  = new double[planes.length];
	    nVerts = 1;
	    verts = new Point3d[nVerts];
	    verts[0] = new Point3d( point.x, point.y, point.z);

	    for(i=0;i 0.0 ) {
		    planes[i].w += -dis;
		}
	    }
	    computeAllVerts();
	}
    }

    /**
     * Combines this bounding polytope with an array of points.
     * @param points an array of 3d points in space
     */
    @Override
    public void combine(Point3d[] points) {
	int i,j;
	double dis;

	if( boundsIsInfinite) {
	    return;
	}

	if( boundsIsEmpty ){
	    planes = new Vector4d[6];
	    mag = new double[planes.length];
	    pDotN  = new double[planes.length];
	    nVerts = points.length;
	    verts = new Point3d[nVerts];
	    verts[0] = new Point3d( points[0].x, points[0].y, points[0].z);

	    for(i=0;i 0.0 ) {
		    planes[i].w += -dis;
		}
	    }
	}

	computeAllVerts();
    }

    /**
     * Modifies the bounding polytope so that it bounds the volume
     * generated by transforming the given bounding object.
     * @param boundsObject the bounding object to be transformed
     * @param matrix a transformation matrix
     */
    @Override
    public void transform( Bounds boundsObject, Transform3D matrix) {

	if( boundsObject == null || boundsObject.boundsIsEmpty)  {
	    boundsIsEmpty = true;
	    boundsIsInfinite = false;
	    computeAllVerts();
	    return;
	}

	if(boundsObject.boundsIsInfinite) {
	    this.set(boundsObject);
	    return;
	}

	if( boundsObject.boundId == BOUNDING_SPHERE ) {
	    BoundingSphere sphere = new BoundingSphere(boundsObject);
	    sphere.transform(matrix);
	    this.set(sphere);
	} else if( boundsObject.boundId == BOUNDING_BOX){
	    BoundingBox box = new BoundingBox(boundsObject);
	    box.transform(matrix);
	    this.set(box);
	} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
	    BoundingPolytope polytope = new BoundingPolytope(boundsObject);
	    polytope.transform(matrix);
	    this.set(polytope);
	} else {
	    throw new IllegalArgumentException(J3dI18N.getString("BoundingPolytope5"));
	}
    }

    /**
     * Transforms this  bounding polytope by the given transformation matrix.
     * @param matrix a transformation matrix
     */
    @Override
    public void transform( Transform3D matrix) {

	if(boundsIsInfinite)
	    return;

	int i;
	double invMag;
	Transform3D invTrans = new Transform3D(matrix);

	invTrans.invert();
	invTrans.transpose();

	for(i = 0; i < planes.length; i++){
	    planes[i].x = planes[i].x * mag[i];
	    planes[i].y = planes[i].y * mag[i];
	    planes[i].z = planes[i].z * mag[i];
	    planes[i].w = planes[i].w * mag[i];
	    invTrans.transform( planes[i] );
	}

	for(i=0;i= 0.0) { // plane is behind origin

		    x = origin.x + dx*t;   // compute intersection point
		    y = origin.y + dy*t;
		    z = origin.z + dz*t;

		    if( pointInPolytope(x,y,z) ) {
			intersectPoint.x = x;
			intersectPoint.y = y;
			intersectPoint.z = z;
			return true;  // ray intersects a face of polytope
		    }
		}
	    }
	}

	return false;
    }

    /**
     * Test for intersection with a ray
     * @param origin is a the starting point of the ray
     * @param direction is the direction of the ray
     * @param position is a point defining the location  of the pick w= distance to pick
     * @return true or false indicating if an intersection occured
     */
    @Override
    boolean intersect(Point3d origin, Vector3d direction, Point4d position ) {
	double t,v0,vd,x,y,z,invMag;
	double dx, dy, dz;
	int i;

	if( boundsIsEmpty ) {
	    return false;
	}

	if( boundsIsInfinite ) {
	    position.x = origin.x;
	    position.y = origin.y;
	    position.z = origin.z;
	    position.w = 0.0;
	    return true;
	}

	invMag = 1.0/Math.sqrt(direction.x*direction.x + direction.y*
			       direction.y + direction.z*direction.z);
	dx = direction.x*invMag;
	dy = direction.y*invMag;
	dz = direction.z*invMag;

	for(i=0;i 0.0 )
		return false;

	}
	return true;

    }

    /**
     * Test for intersection with a segment
     * @param start is a point defining  the start of the line segment
     * @param end is a point defining the end of the line segment
     * @param position is a point defining the location  of the pick w= distance to pick
     * @return true or false indicating if an intersection occured
     */
    @Override
    boolean intersect( Point3d start, Point3d end, Point4d position ) {
	double t,v0,vd,x,y,z;
	int i;

	//System.err.println("line segment intersect : planes.length " + planes.length);

	if( boundsIsEmpty ) {
	    return false;
	}

	if( boundsIsInfinite ) {
	    position.x = start.x;
	    position.y = start.y;
	    position.z = start.z;
	    position.w = 0.0;
	    return true;
	}

	Point3d direction = new Point3d();

	direction.x = end.x - start.x;
	direction.y = end.y - start.y;
	direction.z = end.z - start.z;

	for(i=0;i= 0.0) { // plane is behind start

		    x = start.x + direction.x*t;   // compute intersection point
		    y = start.y + direction.y*t;
		    z = start.z + direction.z*t;
		    // System.err.println("t="+t+" point="+x+" "+y+" "+z);

		    if( pointInPolytope(x,y,z) ) {
			//                   if((t*t) > (end.x-start.x)*(end.x-start.x) +
			//                              (end.y-start.y)*(end.y-start.y) +
			//                              (end.z-start.z)*(end.z-start.z)) {
			if(t <= 1.0) {
			    position.x = x;
			    position.y = y;
			    position.z = z;
			    position.w = t;
			    return true;  // ray intersects a face of polytope
			}
		    }
		}
	    }
	}

	return false;

    }

    /**
     * Test for intersection with a ray.
     * @param origin the starting point of the ray
     * @param direction the direction of the ray
     * @return true or false indicating if an intersection occured
     */
    @Override
    public boolean intersect(Point3d origin, Vector3d direction ) {

	// compute intersection point of ray and each plane then test if point is in polytope

	double t,v0,vd,x,y,z;
	int i;

        if( boundsIsEmpty ) {
	    return false;
        }

	if( boundsIsInfinite ) {
	    return true;
	}

	for(i=0;i= 0.0) { // plane is behind origin

		    x = origin.x + direction.x*t;   // compute intersection point
		    y = origin.y + direction.y*t;
		    z = origin.z + direction.z*t;

		    if( pointInPolytope(x,y,z) ) {
			return true;  // ray intersects a face of polytope
		    } else {
			// System.err.println("point outside polytope");
		    }
		}
	    }
	}

	return false;

    }

    /**
     * Tests whether the bounding polytope is empty.  A bounding polytope is
     * empty if it is null (either by construction or as the result of
     * a null intersection) or if its volume is negative.  A bounding polytope
     * with a volume of zero is not empty.
     * @return true if the bounding polytope is empty;
     * otherwise, it returns false
     */
    @Override
    public boolean isEmpty() {
	// if nVerts > 0 after computeAllVerts(), that means
	// there is some intersection between 3 planes.
	return (boundsIsEmpty || (nVerts <= 0));
    }

    /**
     * Test for intersection with a point.
     * @param point a Point defining a position in 3-space
     * @return true or false indicating if an intersection occured
     */
    @Override
    public boolean intersect(Point3d point ) {

	int i;
        if( boundsIsEmpty ) {
	   return false;
        }
	if( boundsIsInfinite ) {
	  return true;
	}

	for(i = 0; i < this.planes.length; i++){
	    if(( point.x*this.planes[i].x +
		 point.y*this.planes[i].y +
		 point.z*this.planes[i].z + planes[i].w ) > 0.0 )
		return false;

	}
	return true;
    }


    /**
     * Test for intersection with another bounds object.
     * @param boundsObject another bounds object
     * @return true or false indicating if an intersection occured
     */
    @Override
    boolean intersect(Bounds boundsObject, Point4d position) {
	return intersect(boundsObject);
    }

    /**
     * Test for intersection with another bounds object.
     * @param boundsObject another bounds object
     * @return true or false indicating if an intersection occured
     */
    @Override
    public boolean intersect(Bounds boundsObject) {

	if( boundsObject == null ) {
	    return false;
	}

        if( boundsIsEmpty || boundsObject.boundsIsEmpty ) {
	    return false;
        }

	if( boundsIsInfinite || boundsObject.boundsIsInfinite ) {
	    return true;
	}

	if( boundsObject.boundId == BOUNDING_SPHERE ) {
	    return intersect_ptope_sphere( this, (BoundingSphere)boundsObject);
	} else if( boundsObject.boundId == BOUNDING_BOX){
	    return intersect_ptope_abox( this, (BoundingBox)boundsObject);
	} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
	    return intersect_ptope_ptope( this, (BoundingPolytope)boundsObject);
	} else {
	    throw new IllegalArgumentException(J3dI18N.getString("BoundingPolytope6"));
	}
    }

    /**
     * Test for intersection with another bounds object.
     * @param boundsObjects an array of bounding objects
     * @return true or false indicating if an intersection occured
     */
    @Override
    public boolean intersect(Bounds[] boundsObjects) {

	double distsq, radsq;
	BoundingSphere sphere;
	int i;
      	if( boundsObjects == null || boundsObjects.length <= 0  )  {
	    return false;
	}

	if( boundsIsEmpty ) {
	    return false;
	}

	for(i = 0; i < boundsObjects.length; i++){
	    if( boundsObjects[i] == null || boundsObjects[i].boundsIsEmpty) ;
	    else if( boundsIsInfinite || boundsObjects[i].boundsIsInfinite ) {
		return true; // We're done here.
	    }
	    if( boundsObjects[i].boundId == BOUNDING_SPHERE ) {
		sphere = (BoundingSphere)boundsObjects[i];
		radsq =  sphere.radius;
		radsq *= radsq;
		distsq = sphere.center.distanceSquared(sphere.center);
		if (distsq < radsq) {
		    return true;
		}
	    } else if(boundsObjects[i].boundId == BOUNDING_BOX){
		if( this.intersect(boundsObjects[i])) return true;
	    } else if(boundsObjects[i].boundId == BOUNDING_POLYTOPE) {
		if( this.intersect(boundsObjects[i])) return true;
	    } else {
		throw new IllegalArgumentException(J3dI18N.getString("BoundingPolytope7"));
	    }
	}

	return false;
    }
    /**
     * Test for intersection with another bounds object.
     * @param boundsObject another bounds object
     * @param newBoundPolytope the new bounding polytope, which is the intersection of
     *      the boundsObject and this BoundingPolytope
     * @return true or false indicating if an intersection occured
     */
    public boolean intersect(Bounds boundsObject, BoundingPolytope newBoundPolytope) {
	int i;

	if((boundsObject == null) || boundsIsEmpty || boundsObject.boundsIsEmpty ) {
	    newBoundPolytope.boundsIsEmpty = true;
	    newBoundPolytope.boundsIsInfinite = false;
	    newBoundPolytope.computeAllVerts();
	    return false;
	}
	if(boundsIsInfinite && (!boundsObject.boundsIsInfinite)) {
	    newBoundPolytope.set(boundsObject);
	    return true;
	}
	else if((!boundsIsInfinite) && boundsObject.boundsIsInfinite) {
	    newBoundPolytope.set(this);
	    return true;
	}
	else if(boundsIsInfinite && boundsObject.boundsIsInfinite) {
	    newBoundPolytope.set(this);
	    return true;
	}


	BoundingBox tbox = new BoundingBox(); // convert sphere to box

	if( boundsObject.boundId == BOUNDING_SPHERE ) {
	    BoundingSphere sphere = (BoundingSphere)boundsObject;
	    if( this.intersect( sphere)) {
		BoundingBox sbox = new BoundingBox( sphere ); // convert sphere to box
		BoundingBox pbox = new BoundingBox( this ); // convert polytope to box
		pbox.intersect(sbox, tbox);                        // insersect two boxes
		newBoundPolytope.set( tbox );
		return true;
	    }
	} else if( boundsObject.boundId == BOUNDING_BOX){
	    BoundingBox box = (BoundingBox)boundsObject;
	    if( this.intersect( box)) {
		BoundingBox pbox = new BoundingBox( this ); // convert polytope to box
		pbox.intersect(box, tbox);                        // insersect two boxes
		newBoundPolytope.set( tbox );
		return true;
	    }

	} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
	    BoundingPolytope polytope = (BoundingPolytope)boundsObject;
	    if( this.intersect( polytope)) {
		Vector4d newPlanes[] = new Vector4d[planes.length + polytope.planes.length];
		for(i=0;i= boundsObjects.length ) { // all bounds objects were empty
	    newBoundingPolytope.boundsIsEmpty = true;
	    newBoundingPolytope.boundsIsInfinite = false;
	    newBoundingPolytope.computeAllVerts();
	    return false;
	}

	boolean status = false;
	BoundingBox tbox = new BoundingBox(); // convert sphere to box

	for(i=0;i 0.0 ) { // check if sphere center in polytope
			    disToPlane = sphere.center.x*planes[j].x +
				sphere.center.y*planes[j].y +
				sphere.center.z*planes[j].z + planes[j].w;

				// check if distance from center to plane is larger than radius
			    if( disToPlane > sphere.radius ) inside = false;
			}
		    }
		    if( inside) { // contains the sphere
			if( !contains ){ // initialize smallest_distance for the first containment
			    index = i;
			    smallest_distance = dis;
			    contains = true;
			} else{
			    if( dis < smallest_distance){
				index = i;
				smallest_distance = dis;
			    }
			}
		    } else if (!contains) {
			if( dis < smallest_distance){
			    index = i;
			    smallest_distance = dis;
			}
		    }
		} else if( boundsObjects[i] instanceof BoundingBox){
		    BoundingBox box = (BoundingBox)boundsObjects[i];
		    cenX = (box.upper.x+box.lower.x)/2.0;
		    cenY = (box.upper.y+box.lower.y)/2.0;
		    cenZ = (box.upper.z+box.lower.z)/2.0;
		    dis = Math.sqrt( (centroid.x-cenX)*(centroid.x-cenX) +
				     (centroid.y-cenY)*(centroid.y-cenY) +
				     (centroid.z-cenZ)*(centroid.z-cenZ) );
		    inside = true;
		    if( !pointInPolytope( box.upper.x, box.upper.y, box.upper.z ) ) inside = false;
		    if( !pointInPolytope( box.upper.x, box.upper.y, box.lower.z ) ) inside = false;
		    if( !pointInPolytope( box.upper.x, box.lower.y, box.upper.z ) ) inside = false;
		    if( !pointInPolytope( box.upper.x, box.lower.y, box.lower.z ) ) inside = false;
		    if( !pointInPolytope( box.lower.x, box.upper.y, box.upper.z ) ) inside = false;
		    if( !pointInPolytope( box.lower.x, box.upper.y, box.lower.z ) ) inside = false;
		    if( !pointInPolytope( box.lower.x, box.lower.y, box.upper.z ) ) inside = false;
		    if( !pointInPolytope( box.lower.x, box.lower.y, box.lower.z ) ) inside = false;

		    if( inside )  { // contains box
			if( !contains ){ // initialize smallest_distance for the first containment
			    index = i;
			    smallest_distance = dis;
			    contains = true;
			} else{
			    if( dis < smallest_distance){
				index = i;
				smallest_distance = dis;
			    }
			}
		    } else if (!contains) {
			if( dis < smallest_distance){
			    index = i;
			    smallest_distance = dis;
			}
		    }

		} else if(boundsObjects[i] instanceof BoundingPolytope) {
		    BoundingPolytope polytope = (BoundingPolytope)boundsObjects[i];
		    dis = Math.sqrt( (centroid.x-polytope.centroid.x)*(centroid.x-polytope.centroid.x) +
				     (centroid.y-polytope.centroid.y)*(centroid.y-polytope.centroid.y) +
				     (centroid.z-polytope.centroid.z)*(centroid.z-polytope.centroid.z) );
		    inside = true;
		    for(j=0;j= verts.length) {
		Point3d newVerts[] = new Point3d[nVerts << 1];
		for(int i=0;i EPSILON ) {
		return false;
	    }

	}
	return true;
    }

    private void checkBoundsIsEmpty() {
	boundsIsEmpty = (planes.length < 4);
    }

    private void initEmptyPolytope() {
	planes = new Vector4d[6];
	pDotN  = new double[6];
	mag    = new double[6];
	verts  = new Point3d[planes.length*planes.length];
	nVerts = 0;

	planes[0] = new Vector4d( 1.0, 0.0, 0.0, -1.0 );
	planes[1] = new Vector4d(-1.0, 0.0, 0.0, -1.0 );
	planes[2] = new Vector4d( 0.0, 1.0, 0.0, -1.0 );
	planes[3] = new Vector4d( 0.0,-1.0, 0.0, -1.0 );
	planes[4] = new Vector4d( 0.0, 0.0, 1.0, -1.0 );
	planes[5] = new Vector4d( 0.0, 0.0,-1.0, -1.0 );
	mag[0] = 1.0;
	mag[1] = 1.0;
	mag[2] = 1.0;
	mag[3] = 1.0;
	mag[4] = 1.0;
	mag[5] = 1.0;

	checkBoundsIsEmpty();
    }

    @Override
    Point3d getCenter() {
	return centroid;
    }

@Override
public void getCenter(Point3d center) {
	center.set(centroid);
}

    /**
     * if the passed the "region" is same type as this object
     * then do a copy, otherwise clone the Bounds  and
     * return
     */
    @Override
    Bounds copy(Bounds r) {
	int i, k;

	if (r != null && this.boundId == r.boundId) {
	    BoundingPolytope region = (BoundingPolytope) r;
	    if( region.planes.length !=planes.length) {
		region.planes = new Vector4d[planes.length];

		for(k=0;k< region.planes.length;k++)
		    region.planes[k] = new Vector4d();

		region.mag    = new double[planes.length];
		region.pDotN  = new double[planes.length];
		region.verts  = new Point3d[nVerts];
		region.nVerts = nVerts;
		for(k=0;k