bounds.java

JAVA3D矩陈的相关类

	firstPoint = true;
	firstInside = false;

	Vector4d pln;

	while( !converged ) {
	    if (debug) {
		System.err.println("start: p="+" "+p.x+" "+p.y+" "+p.z);
	    }

	    // test the current point against the planes, for each
	    // plane that is violated, add it's contribution to the
	    // penalty function
	    inside = true;
	    aa=0.0; bb=0.0; cc=0.0; 
	    ab=0.0; ac=0.0; bc=0.0; ad=0.0; bd=0.0; cd=0.0;
	    for(i = 0; i < planes.length; i++){
		pln = planes[i];
		dist = (p.x*pln.x + p.y*pln.y +
			p.z*pln.z + pln.w ) ;
		// if point is outside or within EPSILON of the boundary, add 
		// the plane to the penalty matrix.  We do this even if the 
		// point is already inside the polytope to prevent numerical 
		// instablity in cases where the point is just outside the 
		// boundary of several planes of the polytope
		if (dist > -EPSILON ){
		    aa = aa + pln.x * pln.x;
		    bb = bb + pln.y * pln.y;
		    cc = cc + pln.z * pln.z;
		    ab = ab + pln.x * pln.y;
		    ac = ac + pln.x * pln.z;
		    bc = bc + pln.y * pln.z;
		    ad = ad + pln.x * pln.w;
		    bd = bd + pln.y * pln.w;
		    cd = cd + pln.z * pln.w;
		}
		// If the point is inside if dist is <= EPSILON 
		if (dist > EPSILON ){
		    inside = false;
		    if (debug) {
			System.err.println("point outside plane["+i+"]=("+
			    pln.x+ ","+pln.y+",\n\t"+pln.z+
			    ","+ pln.w+")\ndist = " + dist);
		    }
		}
	    }
	    // see if we are done
	    if (inside) {
		if (debug) {
		    System.err.println("p is inside");
		}
		if (firstPoint) {
		    firstInside = true;
		} 
		new_point.set(p);
		converged = true;
	    } else { // solve for a closer point
		firstPoint = false;

		// this is the upper right corner of H, which is all we
		// need to do the decomposition since the matrix is symetric
		h11 = 1.0 + aa * w;
		h12 =       ab * w;
		h13 =       ac * w;
		h22 = 1.0 + bb * w;
		h23 =       bc * w;
		h33 = 1.0 + cc * w;

		if (debug) {
		    System.err.println(" hessin= ");
		    System.err.println(h11+" "+h12+" "+h13);
		    System.err.println("     "+h22+" "+h23);
		    System.err.println("           "+h33);
		}

		// these are the constant terms
		b1 = g.x - w * ad;
		b2 = g.y - w * bd;
		b3 = g.z - w * cd;

		if (debug) {
		    System.err.println(" b1,b2,b3 = "+b1+" "+b2+" " +b3);
		}

		// solve, d1, d2, d3 actually 1/dx, which is more useful
		d1 = 1/h11;
		l12 = d1 * h12;
		l13 = d1 * h13;
		s = h22-l12*h12;
		d2 = 1/s;
		t = h23-h12*l13;
		l23 = d2 * t;
		d3 = 1/(h33 - h13*l13 - t*l23);

		if (debug) {
		    System.err.println(" l12,l13,l23 "+l12+" "+l13+" "+l23);
		    System.err.println(" d1,d2,d3 "+ d1+" "+d2+" "+d3);
		}

		// we have L and D, now solve for y
		y1 = d1 * b1;
		y2 = d2 * (b2 - h12*y1);
		y3 = d3 * (b3 - h13*y1 - t*y2);

		if (debug) {
		    System.err.println(" y1,y2,y3 = "+y1+" "+y2+" "+y3);
		}

		// we have y, solve for n
		n.z = y3;
		n.y = (y2 - l23*n.z);
		n.x = (y1 - l13*n.z - l12*n.y);

		if (debug) {
		    System.err.println("new point = " + n.x+" " + n.y+" " + 
			n.z);
		    test_point(planes, n);

		    if (delta == null) delta = new Vector3d();
		    delta.sub(n, p);
		    delta.normalize();
		    System.err.println("p->n direction: " + delta);

		    // check using the the javax.vecmath routine
		    hMatrix.m00 = h11;
		    hMatrix.m01 = h12;
		    hMatrix.m02 = h13;
		    hMatrix.m10 = h12; // h21 = h12
		    hMatrix.m11 = h22;
		    hMatrix.m12 = h23;
		    hMatrix.m20 = h13; // h31 = h13
		    hMatrix.m21 = h23; // h32 = h22
		    hMatrix.m22 = h33;
		    hMatrix.invert();
		    Point3d check = new Point3d(b1, b2, b3);
		    hMatrix.transform(check);

		    System.err.println("check point = " + check.x+" " + 
			check.y+" " + check.z);
		}

		// see if we have converged yet
		dist = (p.x-n.x)*(p.x-n.x) + (p.y-n.y)*(p.y-n.y) + 
		    (p.z-n.z)*(p.z-n.z);

		if (debug) {
		    System.err.println("p->n distance =" + dist );
		}

		if( dist < EPSILON) { // close enough
		    converged = true;
		    new_point.set(n);
		} else {
		    p.set(n);
		    count++;
		    if(count > 4 ){ // watch for cycling between two minimums
			new_point.set(n);
			converged = true;
		    }
		}
	    }
	}
	if (debug) {
	    System.err.println("returning pnt ("+new_point.x+" "+
		    new_point.y+" "+new_point.z+")");

	    if(firstInside) System.err.println("input point inside polytope ");
	}
	return firstInside;
    }

    boolean intersect_ptope_sphere( BoundingPolytope polyTope, 
				BoundingSphere sphere) {
	Point3d p = new Point3d();
	boolean inside;


	if (debug) {
	    System.err.println("ptope_sphere intersect sphere ="+sphere);
	}
	inside = closest_point( sphere.center, polyTope.planes, p );
	if (debug) {
	    System.err.println("ptope sphere intersect point ="+p);
	}
	if (!inside){
	    // if distance between polytope and sphere center is greater than 
	    // radius then no intersection
	    if (p.distanceSquared( sphere.center) > 
					sphere.radius*sphere.radius){
		if (debug) {
		    System.err.println("ptope_sphere returns false");
		}
		return false;
	    } else {
		if (debug) {
		    System.err.println("ptope_sphere returns true");
		}
		return true;
	    }
	} else {
	    if (debug) {
		System.err.println("ptope_sphere returns true");
	    }
	    return true;
	}
    }

    boolean intersect_ptope_abox( BoundingPolytope polyTope, BoundingBox box) {
         Vector4d planes[] = new Vector4d[6];

	if (debug) {
	    System.err.println("ptope_abox, box = " + box);
	}
	planes[0] = new Vector4d( -1.0, 0.0, 0.0, box.lower.x);
	planes[1] = new Vector4d(  1.0, 0.0, 0.0,-box.upper.x);
	planes[2] = new Vector4d(  0.0,-1.0, 0.0, box.lower.y);
	planes[3] = new Vector4d(  0.0, 1.0, 0.0,-box.upper.y);
	planes[4] = new Vector4d(  0.0, 0.0,-1.0, box.lower.z);
	planes[5] = new Vector4d(  0.0, 0.0, 1.0,-box.upper.z);


	BoundingPolytope pbox = new BoundingPolytope( planes);
 
	boolean result = intersect_ptope_ptope( polyTope, pbox );
	if (debug) {
	    System.err.println("ptope_abox returns " + result);
	}
	return(result);
    }   


    boolean intersect_ptope_ptope( BoundingPolytope poly1, 
					BoundingPolytope poly2) {
	boolean intersect;
	Point3d p = new Point3d();
	Point3d g = new Point3d();
	Point3d gnew = new Point3d();
	Point3d pnew = new Point3d();

	intersect = false;

	p.x = 0.0;
	p.y = 0.0;
	p.z = 0.0;

	//  start from an arbitrary point on poly1
	closest_point( p, poly1.planes, g);
	
	// get the closest points on each polytope
	if (debug) {
	    System.err.println("ptope_ptope: first g = "+g); 
	}
	intersect = closest_point( g, poly2.planes, p);

	if (intersect) {
	    return true;
	}

	if (debug) {
	    System.err.println("first p = "+p+"\n"); 
	}
	 intersect = closest_point( p, poly1.planes, gnew);
	if (debug) {
	    System.err.println("gnew = "+gnew+" intersect="+intersect); 
	}

	// loop until the closest points on the two polytopes are not changing

	double prevDist = p.distanceSquared(g);
	double dist;

	while( !intersect ) {

	    dist = p.distanceSquared(gnew);

	    if (dist < prevDist) {
		g.set(gnew);
		intersect = closest_point( g, poly2.planes, pnew );
		if (debug) {
		    System.err.println("pnew = "+pnew+" intersect="+intersect); 
		}
	    } else {
		g.set(gnew);
		break;
	    }
	    prevDist = dist;
	    dist =  pnew.distanceSquared(g);

	    if (dist < prevDist) {
		p.set(pnew);
		if( !intersect ) { 
		    intersect = closest_point( p, poly1.planes, gnew );
		    if (debug) {
			System.err.println("gnew = "+gnew+" intersect="+
			    intersect); 
		    }
		}
	    } else {
		p.set(pnew);
		break;
	    }
	    prevDist = dist;
	}

	if (debug) {
	    System.err.println("gnew="+" "+gnew.x+" "+gnew.y+" "+gnew.z);
	    System.err.println("pnew="+" "+pnew.x+" "+pnew.y+" "+pnew.z);
	}
	return intersect;
    }


    synchronized void setWithLock(Bounds b) {
	this.set(b);
    }

    synchronized void getWithLock(Bounds b) {
	b.set(this);
    }

    // Return one of Pick Bounds type define in PickShape
    abstract int getPickType();
}