pickresult.java

JAVA3D矩陈的相关类

	    if (vec0.length() > 0.0) break;
	}
        
	for (j=i; j<coordinates.length-1; j++) {
	    vec1.x = coordinates[j+1].x - coordinates[j].x;
	    vec1.y = coordinates[j+1].y - coordinates[j].y;
	    vec1.z = coordinates[j+1].z - coordinates[j].z;
	    if (vec1.length() > 0.0) break;
	}
      
	if (j == (coordinates.length-1)) {
	    // System.out.println("(1) Degenerated polygon.");
	    return false;  // Degenerated polygon.
	}

	/*
	  for (i=0; i<coordinates.length; i++) 
	  System.out.println("coordinates P" + i + " " + coordinates[i]);
	  for (i=0; i<coord2.length; i++) 
	  System.out.println("coord2 P" + i + " " + coord2[i]);
	  */
      
	pNrm.cross(vec0,vec1);
      
	nLenSq = pNrm.lengthSquared(); 
	if ( nLenSq == 0.0) {
	    // System.out.println("(2) Degenerated polygon.");
	    return false;  // Degenerated polygon.
	}

	pa.x = coordinates[0].x - center.x;
	pa.y = coordinates[0].y - center.y;
	pa.z = coordinates[0].z - center.z;

	pNrmDotPa = pNrm.dot(pa);
      
	pqLen = Math.sqrt(pNrmDotPa * pNrmDotPa/ nLenSq);
      
	if (pqLen > radius)
	    return false;

	tq = pNrmDotPa / nLenSq;

	q.x = center.x + tq * pNrm.x;
	q.y = center.y + tq * pNrm.y;
	q.z = center.z + tq * pNrm.z;

	// PolyPnt2D Test.
	return pointIntersectPolygon2D( pNrm, coordinates, q);
    }

    static boolean intersectBoundingPolytope (Point3d coordinates[], 
					      BoundingPolytope polytope) {
      
	boolean debug = false;    
    
	// this is a multiplier to the halfplane distance coefficients
	double distanceSign = -1.0;
	// Variable needed for intersection.
	Point4d tP4d = new Point4d();

	Vector4d[] planes = new Vector4d [polytope.getNumPlanes()];

	for(int i=0; i<planes.length; i++)
	    planes[i]=new Vector4d();

	polytope.getPlanes (planes);

	if (coordinates.length == 2) {
	    // we'll handle line separately.
	    throw new java.lang.RuntimeException ("TODO: must make polytope.intersect(coordinates[0], coordinates[1], tP4d) public!");
	    // TODO: must make this public !!!
	    //      return polytope.intersect(coordinates[0], coordinates[1], tP4d );
	}

	// It is a triangle or a quad.
      
	// first test to see if any of the coordinates are all inside of the
	// intersection polytope's half planes
	// essentially do a matrix multiply of the constraintMatrix K*3 with
	// the input coordinates 3*1 = K*1 vector

	if (debug) { 
	    System.out.println("The value of the input vertices are: ");
	    for (int i=0; i < coordinates.length; i++) {
		System.out.println("The " +i+ " th vertex is: " + coordinates[i]);
	    }
      
	    System.out.println("The value of the input bounding Polytope's planes =");
	    for (int i=0; i < planes.length; i++) {
		System.out.println("The " +i+ " th plane is: " + planes[i]);
	    }
      
	}
    
	// the direction for the intersection cost function
	double centers[] = new double[4];
	centers[0] = 0.8; centers[1] = 0.9; centers[2] = 1.1; centers[3] = 1.2;
      
	boolean intersection = true;
	boolean PreTest = false;
    
	if (PreTest) {
	    // for each coordinate, test it with each half plane
	    for (int i=0; i < coordinates.length; i++) {
		for (int j=0; j < planes.length; j++) {
		    if ((planes[j].x * coordinates[i].x +
			 planes[j].y * coordinates[i].y +
			 planes[j].z*coordinates[i].z) <= 
			(distanceSign)*planes[j].w){
			// the point satisfies this particular hyperplane
			intersection = true;
		    } else {
			// the point fails this hyper plane try with a new hyper plane
			intersection = false;
			break;
		    }
		}
		if (intersection) {
		    // a point was found to be completely inside the bounding hull
		    return true;
		}
	    }
	}  // end of pretest
    
	// at this point all points are outside of the bounding hull
	// build the problem tableau for the linear program
    
	int numberCols = planes.length + 2 + coordinates.length + 1;
	int numberRows = 1 + coordinates.length;
    
	double problemTableau[][] = new double[numberRows][numberCols];
    
	// compute -Mtrans = -A*P
	for ( int i = 0; i < planes.length; i++) {
	    for ( int j=0; j < coordinates.length;  j++) {
		problemTableau[j][i] = (-1.0)* (planes[i].x*coordinates[j].x+
						planes[i].y*coordinates[j].y+
						planes[i].z*coordinates[j].z);
	    }
	}
    
	// add the other rows
	for (int i = 0; i < coordinates.length; i++) {
	    problemTableau[i][planes.length] = -1.0;
	    problemTableau[i][planes.length + 1] =  1.0;
      
	    for (int j=0; j < coordinates.length; j++) {
		if ( i==j ) {
		    problemTableau[i][j + planes.length + 2] = 1.0;
		} else {
		    problemTableau[i][j + planes.length + 2] = 0.0;
		}
	
		// place the last column elements the Ci's
		problemTableau[i][numberCols - 1] = centers[i];
	    }
	}
    
	// place the final rows value
	for (int j = 0; j < planes.length; j++) {
	    problemTableau[numberRows - 1][j] = 
		(distanceSign)*planes[j].w;
	}
	problemTableau[numberRows - 1][planes.length] =  1.0;
	problemTableau[numberRows - 1][planes.length+1] = -1.0;
	for (int j = 0; j < coordinates.length; j++) {
	    problemTableau[numberRows - 1][planes.length+2+j] = 0.0;
	}
    
	if (debug) {
	    System.out.println("The value of the problem tableau is: " );
	    for (int i=0; i < problemTableau.length; i++) {
		for (int j=0; j < problemTableau[0].length; j++) {
		    System.out.print(problemTableau[i][j] + "  ");
		}
		System.out.println();
	    }
	}
    
	double distance = generalStandardSimplexSolver(problemTableau, 
						       Float.NEGATIVE_INFINITY);
	if (debug) {
	    System.out.println("The value returned by the general standard simplex = " +
			       distance);
	}
	if (distance == Float.POSITIVE_INFINITY) {
	    return false;
	} 
	return true;
    }


    // optimized version using arrays of doubles, but using the standard simplex
    // method to solve the LP tableau.  This version has not been optimized to
    // work with a particular size input tableau and is much slower than some
    // of the other variants...supposedly
    static double generalStandardSimplexSolver(double problemTableau[][], 
					       double stopingValue) {
	boolean debug = false;
	int numRow = problemTableau.length;
	int numCol = problemTableau[0].length;
	boolean optimal = false;
	int i, pivotRowIndex, pivotColIndex;
	double maxElement, element, endElement, ratio, prevRatio;
	int count = 0;
	double multiplier;
    
	if (debug) {
	    System.out.println("The number of rows is : " + numRow);
	    System.out.println("The number of columns is : " + numCol);
	}
    
	// until the optimal solution is found continue to do
	// iterations of the simplex method
	while(!optimal) {

	    if (debug) {
		System.out.println("input problem tableau is:");
		for (int k=0; k < numRow; k++) {
		    for (int j=0; j < numCol; j++) {
			System.out.println("kth, jth value is:" +k+" "+j+" : " +
					   problemTableau[k][j]);
		    }
		}
	    }
      
	    // test to see if the current solution is optimal
	    // check all bottom row elements except the right most one and
	    // if all positive or zero its optimal
	    for (i = 0, maxElement = 0, pivotColIndex = -1; i < numCol - 1; i++) {
		// a bottom row element
		element = problemTableau[numRow - 1][i];
		if ( element < maxElement) {
		    maxElement = element;
		    pivotColIndex = i;
		}
	    }
      
	    // if there is no negative non-zero element then we
	    // have found an optimal solution (the last row of the tableau)
	    if (pivotColIndex == -1) {
		// found an optimal solution
		//System.out.println("Found an optimal solution");
		optimal = true;
	    }
      
	    //System.out.println("The value of maxElement is:" + maxElement);
      
	    if (!optimal) {
		// Case when the solution is not optimal but not known to be
		// either unbounded or infeasable
	
		// from the above we have found the maximum negative element in
		// bottom row, we have also found the column for this value
		// the pivotColIndex represents this
	
		// initialize the values for the algorithm, -1 for pivotRowIndex
		// indicates no solution
	
		prevRatio = Float.POSITIVE_INFINITY;
		ratio = 0.0;
		pivotRowIndex = -1;
	
		// note if all of the elements in the pivot column are zero or
		// negative the problem is unbounded.
		for (i = 0; i < numRow - 1; i++) {
		    element = problemTableau[i][pivotColIndex]; // r value
		    endElement = problemTableau[i][numCol-1]; // s value

		    // pivot according to the rule that we want to choose the row
		    // with smallest s/r ratio see third case
		    // currently we ignore valuse of r==0 (case 1) and cases where the
		    // ratio is negative, i.e. either r or s are negative (case 2)
		    if (element == 0) {
			if (debug) {
			    System.out.println("Division by zero has occurred");
			    System.out.println("Within the linear program solver");
			    System.out.println("Ignoring the zero as a potential pivot");
			}
		    } else if ( (element < 0.0) || (endElement < 0.0) ){
			if (debug) {
			    System.out.println("Ignoring cases where element is negative");
			    System.out.println("The value of element is: " + element);
			    System.out.println("The value of end Element is: " + endElement);
			}
		    } else {
			ratio = endElement/element;  // should be s/r
			if (debug) {
			    System.out.println("The value of element is: " + element);
			    System.out.println("The value of endElement is: " + endElement);
			    System.out.println("The value of ratio is: " + ratio);
			    System.out.println("The value of prevRatio is: " + prevRatio);
			    System.out.println("Value of ratio <= prevRatio is :" + 
					       (ratio <= prevRatio));
			}
			if (ratio <= prevRatio) {
			    if (debug) {
				System.out.println("updating prevRatio with ratio");
			    }
			    prevRatio = ratio;
			    pivotRowIndex = i;
			}
		    }
		}
	
		// if the pivotRowIndex is still -1 then we know the pivotColumn
		// has no viable pivot points and the solution is unbounded or
		// infeasable (all pivot elements were either zero or negative or
		// the right most value was negative (the later shouldn't happen?)
		if (pivotRowIndex == -1) {
		    if (debug) {
			System.out.println("UNABLE TO FIND SOLUTION");
			System.out.println("The system is infeasable or unbounded");
		    }
		    return(Float.POSITIVE_INFINITY);
		}
	
		// we now have the pivot row and col all that remains is
		// to divide through by this value and subtract the appropriate
		// multiple of the pivot row from all other rows to obtain
		// a tableau which has a column of all zeros and one 1 in the
		// intersection of pivot row and col
	
		// divide through by the pivot value
		double pivotValue = problemTableau[pivotRowIndex][pivotColIndex];
	
		if (debug) {
		    System.out.println("The value of row index is: " + pivotRowIndex);
		    System.out.println("The value of col index is: " + pivotColIndex);
		    System.out.println("The value of pivotValue is: " + pivotValue);
		}
		// divide through by s on the pivot row to obtain a 1 in pivot col
		for (i = 0; i < numCol; i++) {
		    problemTableau[pivotRowIndex][i] =
			problemTableau[pivotRowIndex][i] / pivotValue;
		}
	
		// subtract appropriate multiple of pivot row from all other rows
		// to zero out all rows except the final row and the pivot row
		for (i = 0; i < numRow; i++) {
		    if (i != pivotRowIndex) {
			multiplier = problemTableau[i][pivotColIndex];
			for (int j=0; j < numCol; j++) {
			    problemTableau[i][j] = problemTableau[i][j] -
				multiplier * problemTableau[pivotRowIndex][j];
			}
		    }
		}
	    }
	    // case when the element is optimal
	}
	return(problemTableau[numRow - 1][numCol - 1]);
    }

    static boolean edgeIntersectSphere (BoundingSphere sphere, Point3d start, 
					Point3d end) { 

	double abLenSq, acLenSq, apLenSq, abDotAp, radiusSq;
	Vector3d ab = new Vector3d();
	Vector3d ap = new Vector3d();

	Point3d center = new Point3d();
	sphere.getCenter (center);
	double radius = sphere.getRadius ();
      
	ab.x = end.x - start.x;
	ab.y = end.y - start.y;
	ab.z = end.z - start.z;
      
	ap.x = center.x - start.x;
	ap.y = center.y - start.y;
	ap.z = center.z - start.z;
      
	abDotAp = ab.dot(ap);
      
	if (abDotAp < 0.0)
	    return false; // line segment points away from sphere.

	abLenSq = ab.lengthSquared();
	acLenSq = abDotAp * abDotAp / abLenSq;

	if (acLenSq < abLenSq)
	    return false; // C doesn't lies between end points of edge.

	radiusSq = radius * radius;
	apLenSq = ap.lengthSquared();
     
	if ((apLenSq - acLenSq) <= radiusSq)
	    return true;      

	return false;
    }


    static double det2D(Point2d a, Point2d b, Point2d p) {
	return (((p).x - (a).x) * ((a).y - (b).y) + 
		((a).y - (p).y) * ((a).x - (b).x));
    }

    // Assume coord is CCW.
    static boolean pointIntersectPolygon2D(Vector3d normal, Point3d[] coord, 
					   Point3d point) {

	double  absNrmX, absNrmY, absNrmZ;
	Point2d coord2D[] = new Point2d[coord.length];
	Point2d pnt = new Point2d();

	int i, j, axis;
      
	// Project 3d points onto 2d plane.
	// Note : Area of polygon is not preserve in this projection, but
	// it doesn't matter here. 
    
	// Find the axis of projection.
	absNrmX = Math.abs(normal.x);
	absNrmY = Math.abs(normal.y);
	absNrmZ = Math.abs(normal.z);
      
	if (absNrmX > absNrmY)
	    axis = 0;
	else 
	    axis = 1;
      
	if (axis == 0) {
	    if (absNrmX < absNrmZ)
		axis = 2;
	}    
	else if