boundingsphere.java

JAVA3D矩陈的相关类

	    l = b.lower.x-center.x;
	    if( u*u > l*l) 
                x = b.upper.x;
	    else
                x = b.lower.x;

	    u = b.upper.y-center.y;
	    l = b.lower.y-center.y;
	    if( u*u > l*l) 
                y = b.upper.y;
	    else
                y = b.lower.y;

	    u = b.upper.z-center.z;
	    l = b.lower.z-center.z;
	    if( u*u > l*l) 
                z = b.upper.z;
	    else
                z = b.lower.z;

	    dis = Math.sqrt( (x - center.x)*(x - center.x) +
			     (y - center.y)*(y - center.y) +
			     (z - center.z)*(z - center.z) );
 
	    if( dis > radius) {
                radius = (dis + radius)*.5;
                oldc_to_new_c = dis - radius;
                center.x = (radius*center.x + oldc_to_new_c*x)/dis;
                center.y = (radius*center.y + oldc_to_new_c*y)/dis;
                center.z = (radius*center.z + oldc_to_new_c*z)/dis;
                combinePoint( b.upper.x, b.upper.y, b.upper.z);
                combinePoint( b.upper.x, b.upper.y, b.lower.z);
                combinePoint( b.upper.x, b.lower.y, b.upper.z);
                combinePoint( b.upper.x, b.lower.y, b.lower.z);
                combinePoint( b.lower.x, b.upper.y, b.upper.z);
                combinePoint( b.lower.x, b.upper.y, b.lower.z);
                combinePoint( b.lower.x, b.lower.y, b.upper.z);
                combinePoint( b.lower.x, b.lower.y, b.lower.z);
	    }
	} else if( boundsObject.boundId == BOUNDING_SPHERE ) {
	    sphere = (BoundingSphere)boundsObject;
	    dis = Math.sqrt( (center.x - sphere.center.x)*
			     (center.x - sphere.center.x) +
			     (center.y - sphere.center.y)*
			     (center.y - sphere.center.y) +
			     (center.z - sphere.center.z)*
			     (center.z - sphere.center.z) );
	    if( radius > sphere.radius) {
		if( (dis+sphere.radius) > radius) {
		    d1 = .5*(radius-sphere.radius+dis);
		    t = d1/dis;
		    radius = d1+sphere.radius;
		    center.x = sphere.center.x + (center.x-sphere.center.x)*t;
		    center.y = sphere.center.y + (center.y-sphere.center.y)*t;
		    center.z = sphere.center.z + (center.z-sphere.center.z)*t;
		}  
	    }else {
		if( (dis+radius) <= sphere.radius) {
		    center.x = sphere.center.x;
		    center.y = sphere.center.y;
		    center.z = sphere.center.z;
		    radius = sphere.radius;
		}else {
		    d1 = .5*(sphere.radius-radius+dis);
		    t = d1/dis;
		    radius = d1+radius;
		    center.x = center.x + (sphere.center.x-center.x)*t;
		    center.y = center.y + (sphere.center.y-center.y)*t;
		    center.z = center.z + (sphere.center.z-center.z)*t;
		}
	    }    

	} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
	    BoundingPolytope polytope = (BoundingPolytope)boundsObject;
	    this.combine(polytope.verts);
	} else {
	    throw new IllegalArgumentException(J3dI18N.getString("BoundingSphere3"));
	}
	updateBoundsStates();
    }
    
    private void combinePoint( double x, double y, double z) {
	double dis,oldc_to_new_c;
	dis = Math.sqrt( (x - center.x)*(x - center.x) +
			 (y - center.y)*(y - center.y) +
			 (z - center.z)*(z - center.z) );
 
	if( dis > radius) {
	    radius = (dis + radius)*.5;
	    oldc_to_new_c = dis - radius;
	    center.x = (radius*center.x + oldc_to_new_c*x)/dis;
	    center.y = (radius*center.y + oldc_to_new_c*y)/dis;
	    center.z = (radius*center.z + oldc_to_new_c*z)/dis;
	}
    }

    /** 
     * Combines this bounding sphere with an array of bounding objects so that the
     * resulting bounding sphere encloses the original bounding sphere and the
     * given array of bounds object.
     * @param boundsObjects an array of bounds objects
     */
    public void combine(Bounds[] boundsObjects) {
	BoundingSphere sphere;
	BoundingBox b;
	BoundingPolytope polytope;
	double t,dis,d1,u,l,x,y,z,oldc_to_new_c;
	int i=0;
        
 
	if((boundsObjects == null) || (boundsObjects.length <= 0)
	   || (boundsIsInfinite))
	    return;
	
	// find first non empty bounds object
	while((i<boundsObjects.length) &&
	      ((boundsObjects[i] == null) || boundsObjects[i].boundsIsEmpty)) {
	    i++;
	}
	if( 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<boundsObjects.length;i++) {
	    if( boundsObjects[i] == null );  // do nothing
	    else if( boundsObjects[i].boundsIsEmpty); // do nothing
	    else if( boundsObjects[i].boundsIsInfinite ) {
		center.x = 0.0;
		center.y = 0.0;
		center.z = 0.0;
		radius = Double.POSITIVE_INFINITY;
		break;  // We're done.
	    } else if( boundsObjects[i].boundId == BOUNDING_BOX){ 
		b = (BoundingBox)boundsObjects[i]; 
 
		//       start with point furthest from sphere 
		u = b.upper.x-center.x;
		l = b.lower.x-center.x;
		if( u*u > l*l) 
		    x = b.upper.x;
		else
		    x = b.lower.x;
 
		u = b.upper.y-center.y;
		l = b.lower.y-center.y;
		if( u*u > l*l) 
		    y = b.upper.y;
		else
		    y = b.lower.y;

		u = b.upper.z-center.z;
		l = b.lower.z-center.z;
		if( u*u > l*l) 
		    z = b.upper.z;
		else
		    z = b.lower.z;
 
		dis = Math.sqrt( (x - center.x)*(x - center.x) +
				 (y - center.y)*(y - center.y) +
				 (z - center.z)*(z - center.z) );
 
		if( dis > radius) {
		    radius = (dis + radius)*.5;
		    oldc_to_new_c = dis - radius;
		    center.x = (radius*center.x + oldc_to_new_c*x)/dis;
		    center.y = (radius*center.y + oldc_to_new_c*y)/dis;
		    center.z = (radius*center.z + oldc_to_new_c*z)/dis;
		    combinePoint( b.upper.x, b.upper.y, b.upper.z);
		    combinePoint( b.upper.x, b.upper.y, b.lower.z);
		    combinePoint( b.upper.x, b.lower.y, b.upper.z);
		    combinePoint( b.upper.x, b.lower.y, b.lower.z);
		    combinePoint( b.lower.x, b.upper.y, b.upper.z);
		    combinePoint( b.lower.x, b.upper.y, b.lower.z);
		    combinePoint( b.lower.x, b.lower.y, b.upper.z);
		    combinePoint( b.lower.x, b.lower.y, b.lower.z);
		}
	    } else if( boundsObjects[i].boundId == BOUNDING_SPHERE ) {
		sphere = (BoundingSphere)boundsObjects[i];
		dis = Math.sqrt( (center.x - sphere.center.x)*
				 (center.x - sphere.center.x) +
				 (center.y - sphere.center.y)*
				 (center.y - sphere.center.y) +
				 (center.z - sphere.center.z)*
				 (center.z - sphere.center.z) );
		if( radius > sphere.radius) {
		    if( (dis+sphere.radius) > radius) { 
			d1 = .5*(radius-sphere.radius+dis);
			t = d1/dis;
			radius = d1+sphere.radius;
			center.x = sphere.center.x + (center.x-sphere.center.x)*t;
			center.y = sphere.center.y + (center.y-sphere.center.y)*t;
			center.z = sphere.center.z + (center.z-sphere.center.z)*t;
		    }   
		}else {
		    if( (dis+radius) <= sphere.radius) {
			center.x = sphere.center.x;
			center.y = sphere.center.y;
			center.z = sphere.center.z;
			radius = sphere.radius;
		    }else {
			d1 = .5*(sphere.radius-radius+dis);
			t = d1/dis;
			radius = d1+radius;
			center.x = center.x + (sphere.center.x-center.x)*t;
			center.y = center.y + (sphere.center.y-center.y)*t;
			center.z = center.z + (sphere.center.z-center.z)*t;
		    }
		}    
	    } else if(boundsObjects[i].boundId == BOUNDING_POLYTOPE) {
		polytope = (BoundingPolytope)boundsObjects[i]; 
		this.combine(polytope.verts); 
	    } else {
		throw new IllegalArgumentException(J3dI18N.getString("BoundingSphere4"));
	    }
	}

	updateBoundsStates();
    }

    /** 
     * Combines this bounding sphere with a point.
     * @param point a 3D point in space
     */
    public void combine(Point3d point) {
	double t,dis,oldc_to_new_c;

	if( boundsIsInfinite) {
	    return;
	}
	
	if( boundsIsEmpty) {
	    radius = 0.0;
	    center.x = point.x;
	    center.y = point.y;
	    center.z = point.z;
	} else {
	    dis = Math.sqrt( (point.x - center.x)*(point.x - center.x) +
			     (point.y - center.y)*(point.y - center.y) +
			     (point.z - center.z)*(point.z - center.z) );

	    if( dis > radius) {
	        radius = (dis + radius)*.5;
      	        oldc_to_new_c = dis - radius;
                center.x = (radius*center.x + oldc_to_new_c*point.x)/dis;
                center.y = (radius*center.y + oldc_to_new_c*point.y)/dis;
                center.z = (radius*center.z + oldc_to_new_c*point.z)/dis;
	    }
	}
	
	updateBoundsStates();
    }  
 
    /**
     * Combines this bounding sphere with an array of points.
     * @param points an array of 3D points in space
     */
    public void combine(Point3d[] points) {
	int i; 
	double dis,dis_sq,rad_sq,oldc_to_new_c;

	if( boundsIsInfinite) {
	    return;
	}

	if( boundsIsEmpty ) {
	    center.x = points[0].x;
	    center.y = points[0].y;
	    center.z = points[0].z;
	    radius = 0.0;
	}

	for(i=0;i<points.length;i++) {
	    rad_sq = radius * radius;
	    dis_sq =  (points[i].x - center.x)*(points[i].x - center.x) +
		(points[i].y - center.y)*(points[i].y - center.y) +
		(points[i].z - center.z)*(points[i].z - center.z);
	    
	    // change sphere so one side passes through the point and
	    // other passes through the old sphere
	    if( dis_sq > rad_sq) {
		dis = Math.sqrt( dis_sq);
		radius = (radius + dis)*.5;
		oldc_to_new_c = dis - radius;
                center.x = (radius*center.x + oldc_to_new_c*points[i].x)/dis;
                center.y = (radius*center.y + oldc_to_new_c*points[i].y)/dis;
                center.z = (radius*center.z + oldc_to_new_c*points[i].z)/dis;
	    }
	}
	
	updateBoundsStates();
    }
    

    /**
     * Modifies the bounding sphere 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
     */
    public void transform( Bounds boundsObject, Transform3D matrix) {
	double scale;

	if( boundsObject == null || boundsObject.boundsIsEmpty)  {
	    // Negative volume.
	    center.x = center.y = center.z = 0.0;
	    radius = -1.0;
	    updateBoundsStates();
	    return;
	}
	
	if(boundsObject.boundsIsInfinite) {
	    center.x = center.y = center.z = 0.0;
	    radius = Double.POSITIVE_INFINITY;
	    updateBoundsStates();
	    return;
	}
	
	if( boundsObject.boundId == BOUNDING_BOX){
	    if (tmpBox == null) {
                tmpBox = new BoundingBox( (BoundingBox)boundsObject);
	    } else {
                tmpBox.set((BoundingBox)boundsObject);
	    }
            tmpBox.transform(matrix);
            this.set(tmpBox);
	}else if( boundsObject.boundId == BOUNDING_SPHERE ) {
	    matrix.transform(((BoundingSphere)boundsObject).center, this.center);
	    // A very simple radius scale.
	    scale = matrix.getDistanceScale();
	    this.radius = ((BoundingSphere)boundsObject).radius * scale;
	    if (Double.isNaN(radius)) {
		// Negative volume.
		center.x = center.y = center.z = 0.0;
		radius = -1.0;
		updateBoundsStates();
		return;
	    }
	} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
	    if (tmpPolytope == null) {
            	tmpPolytope = new BoundingPolytope((BoundingPolytope)boundsObject);
	    } else {
            	tmpPolytope.set((BoundingPolytope)boundsObject);
	    }
            tmpPolytope.transform(matrix);
            this.set(tmpPolytope);
	} else {
	    throw new IllegalArgumentException(J3dI18N.getString("BoundingSphere5"));
	}
    }

    /** 
     * Transforms this bounding sphere by the given matrix.
     */
    public void transform( Transform3D trans) {
	double scale;

	if(boundsIsInfinite)
	    return;

	trans.transform(center);
	scale = trans.getDistanceScale();
	radius = radius * scale;
	if (Double.isNaN(radius)) {
	    // Negative volume.
	    center.x = center.y = center.z = 0.0;
	    radius = -1.0;
	    updateBoundsStates();
	    return;
	}
    }

    /** 
     * Test for intersection with a ray 
     * @param origin the starting point of the ray   
     * @param direction the direction of the ray
     * @param position3 a point defining the location of the pick w= distance to pick
     * @return true or false indicating if an intersection occured 
     */
    boolean intersect(Point3d origin, Vector3d direction, Point4d position ) {
 
	if( boundsIsEmpty ) {
	    return false;
	}

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

	double l2oc,rad2,tca,t2hc,mag,t,invMag;
	Vector3d dir = new Vector3d();  // normalized direction of ray
	Point3d oc  = new Point3d();  // vector from sphere center to ray origin

	oc.x = center.x - origin.x;
	oc.y = center.y - origin.y;
	oc.z = center.z - origin.z;
 
	l2oc = oc.x*oc.x + oc.y*oc.y + oc.z*oc.z; // center to origin squared
 
	rad2 = radius*radius;
	if( l2oc < rad2 ){
	    //      System.err.println("ray origin inside sphere" );
	    return true;   // ray origin inside sphere
	}
 
	invMag = 1.0/Math.sqrt(direction.x*direction.x +
			       direction.y*direction.y +
			       direction.z*direction.z);
	dir.x = direction.x*invMag;
	dir.y = direction.y*invMag;
	dir.z = direction.z*invMag;
	tca = oc.x*dir.x + oc.y*dir.y + oc.z*dir.z;
 
	if( tca <= 0.0 ) {
	    //      System.err.println("ray points away from sphere" );
	    return false;  // ray points away from sphere
	}
	
	t2hc = rad2 - l2oc + tca*tca;
	
	if( t2hc > 0.0 ){
	    t = tca - Math.sqrt(t2hc);
	    //      System.err.println("ray  hits sphere:"+this.toString()+" t="+t+" direction="+dir );
	    position.x = origin.x + dir.x*t;
	    position.y = origin.y + dir.y*t;
	    position.z = origin.z + dir.z*t;
	    position.w = t;
	    return true;   // ray hits sphere
	}else {
	    //      System.err.println("ray does not hit sphere" );
	    return false;
	}

    }