boundingpolytope.java

JAVA3D矩陈的相关类

	return false;
    }
    
    /**
     * Test for intersection with an array of  bounds objects.
     * @param boundsObjects an array of bounds objects
     * @param newBoundingPolytope 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[] boundsObjects, BoundingPolytope newBoundingPolytope) {
	
	if( boundsObjects == null || boundsObjects.length <= 0 || boundsIsEmpty ) {
	    newBoundingPolytope.boundsIsEmpty = true;
	    newBoundingPolytope.boundsIsInfinite = false;
	    newBoundingPolytope.computeAllVerts();
	    return false;
	} 

	int i=0;
	// find first non null bounds object
	while( boundsObjects[i] == null && i < boundsObjects.length) {
	    i++;
	}
	
	if( 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<boundsObjects.length;i++) {
	    if( boundsObjects[i] == null || boundsObjects[i].boundsIsEmpty) ;
	    else if(  boundsObjects[i].boundId == BOUNDING_SPHERE ) {
		BoundingSphere sphere = (BoundingSphere)boundsObjects[i];
		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
		    if ( status ) {
			newBoundingPolytope.combine( tbox );                       
		    } else {
			newBoundingPolytope.set( tbox );                       
			status = true;
		    }
		}
	    } else if( boundsObjects[i].boundId == BOUNDING_BOX){
		BoundingBox box = (BoundingBox)boundsObjects[i];
		if( this.intersect( box) ){
		    BoundingBox pbox = new BoundingBox( this ); // convert polytope to box 
		    pbox.intersect(box,tbox);                        // insersect two boxes
		    if ( status ) {
			newBoundingPolytope.combine( tbox );                       
		    } else {
			newBoundingPolytope.set( tbox );                       
			status = true;
		    }
		} else {
		}

	    } else if(boundsObjects[i].boundId == BOUNDING_POLYTOPE) {
		BoundingPolytope polytope = (BoundingPolytope)boundsObjects[i];
		if( this.intersect( polytope)) {
		    Vector4d newPlanes[] = new Vector4d[planes.length + polytope.planes.length];
		    for(i=0;i<planes.length;i++) {
			newPlanes[i] = new Vector4d(planes[i]);
		    }
		    for(i=0;i<polytope.planes.length;i++) {
			newPlanes[planes.length + i] = new Vector4d(polytope.planes[i]);
		    }
		    BoundingPolytope newPtope= new BoundingPolytope( newPlanes );
		    if ( status ) {
			newBoundingPolytope.combine( newPtope );
		    } else {
			newBoundingPolytope.set( newPtope );
			status = true;
		    }
		}
	    } else {
		throw new IllegalArgumentException(J3dI18N.getString("BoundingPolytope8"));
	    }
	    
	    if(newBoundingPolytope.boundsIsInfinite)
		break; // We're done. 
	    
	}

	if( status == false ) {
	    newBoundingPolytope.boundsIsEmpty = true;
	    newBoundingPolytope.boundsIsInfinite = false;
	    newBoundingPolytope.computeAllVerts();
	}
	return status;
 
    }   
    /** 
     * Finds closest bounding object that intersects this bounding polytope.
     * @param boundsObjects is an array of  bounds objects 
     * @return closest bounding object 
     */
    public Bounds closestIntersection( Bounds[] boundsObjects) {

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

        if( boundsIsEmpty ) {
	    return null;
        }

	double dis,disToPlane;
	boolean contains = false;
	boolean inside;
	double smallest_distance = Double.MAX_VALUE; 
	int i,j,index=0;
	double cenX = 0.0, cenY = 0.0, cenZ = 0.0;

	for(i = 0; i < boundsObjects.length; i++){
	    if( boundsObjects[i] == null );

	    else if( this.intersect( boundsObjects[i])) {
		if( boundsObjects[i] instanceof BoundingSphere ) {
		    BoundingSphere sphere = (BoundingSphere)boundsObjects[i];
		    dis = Math.sqrt( (centroid.x-sphere.center.x)*(centroid.x-sphere.center.x) +
				     (centroid.y-sphere.center.y)*(centroid.y-sphere.center.y) +
				     (centroid.z-sphere.center.z)*(centroid.z-sphere.center.z) );
		    inside = true;
		    for(j=0;j<planes.length;j++) {
			if( ( sphere.center.x*planes[j].x +
			      sphere.center.y*planes[j].y +
			      sphere.center.z*planes[j].z + planes[i].w ) > 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<polytope.nVerts;j++) {
			if ( !pointInPolytope( polytope.verts[j].x, polytope.verts[j].y, polytope.verts[j].z ) ) 
			    inside = false;
		    }
		    if( inside ) { 
			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 {
		    throw new IllegalArgumentException(J3dI18N.getString("BoundingPolytope10"));
		}
	    }
	}

	return boundsObjects[index];
    }

    /** 
     * Returns a string representation of this class
     */
    public String toString() {
	int i;
	
	String description = new String("BoundingPolytope:\n Num Planes ="+planes.length);
	for(i = 0; i < planes.length; i++){
	    description = description+"\n"+mag[i]*planes[i].x+" "+
		mag[i]*planes[i].y+" "+mag[i]*planes[i].z+" "+mag[i]*planes[i].w;
	}
	
	return description;
    }

    private void computeVertex( int a, int b, int c ) {
	double det,x,y,z;

	det = planes[a].x*planes[b].y*planes[c].z + planes[a].y*planes[b].z*planes[c].x +
	    planes[a].z*planes[b].x*planes[c].y - planes[a].z*planes[b].y*planes[c].x -
	    planes[a].y*planes[b].x*planes[c].z - planes[a].x*planes[b].z*planes[c].y; 

	// System.err.println("\n det="+det);
	if( det*det < EPSILON ){
	    // System.err.println("parallel planes="+a+" "+b+" "+c);
	    return; // two planes are parallel
	}
	
	det = 1.0/det;
	
	x = (planes[b].y*planes[c].z - planes[b].z*planes[c].y) * pDotN[a];
	y = (planes[b].z*planes[c].x - planes[b].x*planes[c].z) * pDotN[a];
	z = (planes[b].x*planes[c].y - planes[b].y*planes[c].x) * pDotN[a];
	
	x += (planes[c].y*planes[a].z - planes[c].z*planes[a].y) * pDotN[b];
	y += (planes[c].z*planes[a].x - planes[c].x*planes[a].z) * pDotN[b];
	z += (planes[c].x*planes[a].y - planes[c].y*planes[a].x) * pDotN[b];
	
	x += (planes[a].y*planes[b].z - planes[a].z*planes[b].y) * pDotN[c];
	y += (planes[a].z*planes[b].x - planes[a].x*planes[b].z) * pDotN[c];
	z += (planes[a].x*planes[b].y - planes[a].y*planes[b].x) * pDotN[c];
	
	x = x*det;
	y = y*det;
	z = z*det;
	
	if (pointInPolytope( x, y, z ) ) {
	    if (nVerts >= verts.length) {
		Point3d newVerts[] = new Point3d[nVerts << 1];
		for(int i=0;i<nVerts;i++) {
		    newVerts[i] = verts[i];
		}
		verts = newVerts; 
	    }
	    verts[nVerts++] = new Point3d( x,y,z);
	}
    }


    private void computeAllVerts() {
	int i,a,b,c;
	double x,y,z;
	
	nVerts = 0;
	
	if( boundsIsEmpty) {
	    verts = null;
	    return;
	}
	
	verts = new Point3d[planes.length*planes.length];	

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

	for(a=0;a<planes.length-2;a++) {
	    for(b=a+1;b<planes.length-1;b++) {
		for(c=b+1;c<planes.length;c++) {
		    computeVertex(a,b,c);
		}
	    }
	}
	// XXXX: correctly compute centroid
	
	x=y=z=0.0; 
	Point3d newVerts[] = new Point3d[nVerts];

	for(i=0;i<nVerts;i++) {
	    x += verts[i].x;
	    y += verts[i].y;
	    z += verts[i].z;
	    // copy the verts into an array of the correct size
	    newVerts[i] = verts[i];
	}
	
	this.verts = newVerts; // copy the verts into an array of the correct size
	
	centroid.x = x/nVerts;
	centroid.y = y/nVerts;
	centroid.z = z/nVerts;
	
	checkBoundsIsEmpty();
	
    }

    private boolean pointInPolytope( double x, double y, double z ){

	for (int i = 0; i < planes.length; i++){
	    if(( x*planes[i].x +
		 y*planes[i].y +
		 z*planes[i].z + planes[i].w ) > 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();
    }

    Point3d getCenter() {
	return centroid;
    }

    /**
     * if the passed the "region" is same type as this object
     * then do a copy, otherwise clone the Bounds  and
     * return
     */
    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<nVerts;k++)
		    region.verts[k] = new Point3d(verts[k]);
	    }
	    
             
	    for(i=0;i<planes.length;i++) {
		region.planes[i].x = planes[i].x;
		region.planes[i].y = planes[i].y;
		region.planes[i].z = planes[i].z;
		region.planes[i].w = planes[i].w;
		region.mag[i] = mag[i];
	    }

	    region.boundsIsEmpty = boundsIsEmpty;
	    region.boundsIsInfinite = boundsIsInfinite;
	    return region;
	}
	else {
	    return (Bounds) this.clone();
	}
    }

    int getPickType() {
	return PickShape.PICKBOUNDINGPOLYTOPE;
    }
}