/**

 * File:      CollisionDetectionTerrainReader.java

 * Author:    M. L. Smith

 * Date:      5 May 99

 *

 * Description:

 *  Reads a file named "FortIrwinTerrain.wrl" (multi-gen file) and searches for the Indexed

 *  Face Set(s) to extract all of the (x,y,z) coordinate data and coordinate index data.

 *  Stores all Indexed Face Set(s) data in a single array and coordinate index data in

 *  another array.  Rotates and translates the coordinate data into VRML coordinates.

 *  After converting into VRML, it converts VRML into PDU coordinates.  Then we create the

 *  final array holding all coordinate data based on the coordinate index data.  Calculates

 *  unit normal for each facet.

 *

 * Assumptions:

 *  - assumes input file is called FortIrwinTerrain.wrl

 *  - assumes Indexed Face Set(s) are triangle surfaces

 *  - if number of coordinate indices is not multiple of 3 (i.e., triangle surface),

 *    disregards leftover coordinate indices

 *  - user sets the translation values and rotation coordinates/angle in definitions

 *    below to match the input files' values

**/



package demo.helicopter;



import java.util.*;

import java.io.*;



public class CollisionDetectionTerrainReader {


   /**
    * @param BufferedReader in - Used as a buffer for data input
    */
   private BufferedReader in;


   /**
    *  @param StreamTokenizer st - Used to get the next token after white space
    */
   private StreamTokenizer st;


   /**
    * @param static final String filename1 - The filename to load the terrain data from. <p>
    * "FortIrwinTerrain.wrl" in this case.
    */
   private static final String fileName1 = "FortIrwinTerrain.wrl";

   /**
    * @param static final String fileName2 - backup path look for fileName1
    */
   private static final String fileName2 = "/vrtp/demo/helicopter/FortIrwinTerrain.wrl";

   /**
    * @param static final String fileName3 - backup path look for fileName1
    */
   private static final String fileName3 = "c:/vrtp/demo/helicopter/FortIrwinTerrain.wrl";

   /**
    * @param static final String fileName4 - backup path look for fileName1
    */
   private static final String fileName4 = "d:/vrtp/demo/helicopter/FortIrwinTerrain.wrl";


   /**
    * @param String s
    */
   private String s;


   /**
    * @param static final String INDEXEDFACESETMARKER - annotates type of vrml node <p>
    * we are reading the data from
    */
   private static final String INDEXEDFACESETMARKER = "IndexedFaceSet";


   /**
    * @param static final String COORDMARKER - used for the VRML parsing to get at the elvevation <p>
    *  data.
    * @param static final String COORDINDEXMARKER - also used for VRML file parsing to get at the <p>
    * elevation data.
    */


   private static final String COORDMARKER = "Coordinate",

                               COORDINDEXMARKER = "coordIndex";


   /**
    * @param int numPoints - the number of points in the file
    */
   private int numPoints;

   /**
    * @param numUniquePoints - the number of unique points in the file.
    */
   private int numUniquePoints;

   /**
    * @param int numFacets - number of triangles
    */
   private int numFacets;                     // no. of triangles


   /**
    * @param int numCoordsThisSet - the number of coorinates in the current set.
    */
   private int numCoordsThisSet = 0;          // no. of coord in current set


   /**
    * @param int numCoordsPreviousSets - the number of coordinates in previously <p>
    * processed indexed face sets
    */
   private int numCoordsPreviousSets = 0;

   /**
    * @param coordFlag - boolean flag
    */
   private boolean coordFlag = false;


   /**
    * @param coordIndexFlag - another boolean flag
    */
   private boolean coordIndexFlag = false;


   /**
    * @param double [] xValsArray - the x vertices for the terrain
    */
   public double[] xValsArray;

   /**
    * @param double [] yValsArray - the y vertices for the terrain
    */
   public double[] yValsArray;


   /**
    * @param double [] zValsArray - the z vertices for the terrain
    */
   public double[] zValsArray;


   /**
    * @param int [] xCoordIndexArray - the index for X coordinates
    */
   public int[] xCoordIndexArray;

   /**
    * @param int [] yCoordIndexArray - the index for Y coordinates
    */
   public int[] yCoordIndexArray;


   /**
    * @param int [] zCoordIndexArray - the index for Z coordinates
    */
   public int[] zCoordIndexArray;


   /**
    * @param double[] xFacetNormal - Normal for X coordinates
    */
   public double[] xFacetNormal;


   /**
    * @param double[] yFacetNormal - Normal for Y coordinates
    */
   public double[] yFacetNormal;


   /**
    * @param double[] zFacetNormal - Normal for Z coordinates
    */
   public double[] zFacetNormal;



   /**
    * @param double rotateX - vector to rotate in x plane, must match the terrain file.
    */
   private double rotateX = -0.000176679;


   /**
    * @param double rotateY - vector to rotate in y plane, must match the terrain file.
    */
   private double rotateY =  0.703891;


   /**
    * @param double rotateZ - vector to rotate in z plane, must match the terrain file.
    */
   private double rotateZ =  0.710308;


   /**
    * @param double rotateAngle - the angle to rotate the plane in radians
    */
   private double rotateAngle = 3.14126;


   /**
    * @param double translateX - the x tranlsation, also hardcoded to match input file
    */
   private double translateX = 28067.7 ;

   /**
    * @param double translateY - the y tranlsation, also hardcoded to match input file
    */
   private double translateY = 60.0;


   /**
    * @param double translateZ - the z tranlsation, also hardcoded to match input file
    */
   private double translateZ = -38031.4;


   /**
    * @param Vector data - coordinate data
    */
   public Vector data;                        // coordinate data


   /**
    * @param Vector index - the index data
    */
   public Vector index;                       // index data


   /**
    * parseFile() - the parsfile for reading the terrain
    *
    * @param - none
    *
    * @return - void
    */
   public void parseFile() {


      //Get a new data vector object
      data = new Vector(10000);

      //Get a new index vector Object
      index = new Vector(10000);


      //Read the data in, try various names and local path lookups
      try {

	   in = new BufferedReader(new FileReader(new File(fileName1)));

           System.out.println("found " + fileName1 + ", loading...");

      }

      catch (IOException e1) {



        try {

	   in = new BufferedReader(new FileReader(new File(fileName2)));

           System.out.println("found " + fileName2 + ", loading...");

        }

        catch (IOException e2) {

           try {

	   	in = new BufferedReader(new FileReader(new File(fileName3)));

           	System.out.println("found " + fileName3 + ", loading...");

           }

           catch (IOException e3) {

        	try {

	   		in = new BufferedReader(new FileReader(new File(fileName4)));

           		System.out.println("found " + fileName3 + ", loading...");

           	}

           	catch (IOException e4) {

         		System.out.println("Failure to open input file: " + fileName1);

	        	System.out.println("Failure to open input file: " + fileName2);

           		System.out.println("Failure to open input file: " + fileName3);

           		System.out.println("Failure to open input file: " + fileName4);

           		java.lang.System.exit (-1);

           	}

	   }

	}

      }



      st = new StreamTokenizer(in);



      try {



         while (st.nextToken() != StreamTokenizer.TT_EOF) {

            switch(st.ttype) {

	         case StreamTokenizer.TT_WORD:

   	            s = st.sval;

                  if (s.equals(INDEXEDFACESETMARKER)) {

		         createArrayData(st);

                  } // endif

		      break;

               default:

		      //s = String.valueOf((char)st.ttype);

            } // end switch

         } // endwhile



	}

	catch (IOException ioe) {}



      try {

         in.close();

      }

      catch(IOException e) {

         System.out.println("Failed to close: " + in);

      }



//    System.out.println("Terrain File Coord Index Size= " + index.size() );



      numPoints = index.size();                  // total no. of vertices

      xValsArray = new double[numPoints];        // allocate arrays to hold

      yValsArray = new double[numPoints];        //  (x,y,z) values

      zValsArray = new double[numPoints];

      numUniquePoints = numCoordsPreviousSets / 3;



//      System.out.println("No. of unique (x,y,z) coords= " + numUniquePoints);

      int ix = 0;


       //Loop through the input storing in
      for (Enumeration e = data.elements(); e.hasMoreElements(); ix++) {

         double[] val = (double[]) e.nextElement();

         xValsArray[ix] = val[0];

         val = (double[]) e.nextElement();

         yValsArray[ix] = val[0];

         val = (double[]) e.nextElement();

         zValsArray[ix] = val[0];

      } //next



      transform();

      createFullArray();

      calcNormals();



   } // end parseFile





   /**
    * createArrayData() - creates the array data from the data from the input terrain file
    *
    * @param StreamTokenizer st - Used to get next valid piece of data, uses whitespace to <p>
    * separate
    *
    * @return - void
    */

   public void createArrayData(StreamTokenizer st) {


    /**
     * @param doneWithFaceSet - boolean variable used to indicate complete with the data <p>
     * we want from the stream for the terrain file.
     */
	boolean doneWithFaceSet = false;


      //Get the info til we're done
      try {



         while (st.nextToken() != StreamTokenizer.TT_EOF && !doneWithFaceSet) {

	      switch(st.ttype) {

	         case StreamTokenizer.TT_EOL:

		      s = new String("EOL");

		      break;

	         case StreamTokenizer.TT_WORD:

		      s = st.sval;

		      if (s.equals(COORDMARKER)) {

                     coordFlag = true;

		         extractArrayValues();       // get vertex data

                  }

			else if (s.equals(COORDINDEXMARKER)) {

                     coordIndexFlag = true;

		         extractArrayValues();       // get index data

		      } // endif

	            break;

	         default:

		      s = String.valueOf((char)st.ttype);

            } // end switch

         } // endwhile



      }

	catch (IOException ioe) {}

	st.pushBack();



   } // end createArrayData





  /**
   * extractArrayValues() - processes each token and puts in vector if its a vertex
   *
   * @param - none
   *
   * @return boolean if success
   */

   public boolean extractArrayValues() {


    /**
     * @param boolean endOfArray - we've reached the end of the array
     */
    boolean endOfArray = false;

    /**
     * @param arrayNotEmpty - the array is still not empty
     */
	boolean arrayNotEmpty = false;


      //Read through the array and get the coordinate indices
      try {



         while (st.nextToken() != StreamTokenizer.TT_EOF && !endOfArray) {

	      switch(st.ttype) {



	         case StreamTokenizer.TT_EOL:

		      s = new String("EOL");

		      break;



               case StreamTokenizer.TT_NUMBER:

                  if (coordFlag) {

                     double[] values = new double[1];

                     values[0] = st.nval;

                     data.add(values);

                     numCoordsThisSet++;

                  }

                  else if (coordIndexFlag) {

                     if (st.nval != -1) {

                        int[] values = new int[1];

                        values[0] = (int)st.nval + (numCoordsPreviousSets / 3);

                        index.add(values);

                     } //endif

                  } //endif



		      arrayNotEmpty = true;

                  break;



		   default:

		      if ((char)st.ttype == ']') {

		         endOfArray = true;

                     if (coordFlag) {

                        coordFlag = false;

                     }

                     else if (coordIndexFlag) {

                        numCoordsPreviousSets += numCoordsThisSet;

                        numCoordsThisSet = 0;

                        coordIndexFlag = false;

                     } // endif

			   st.pushBack();

		      } // endif

	      } // end switch

	   }  // endwhile



	}

	catch (IOException ioe) {}



      return arrayNotEmpty;



   } // end extractArrayValues





  /**
   * transform ()- rotate and translate coordinates into VRML;
   *             for rotate about axis v vector where u = normal(v)
   *                S = Matrix[(0,-u3,u2) (u3,0,-u1) (-u2,u1,0)]
   *                R (rotation matrix) = u*u + cos(alpha)*(Identity
   *                  -u*u) + sin(alpha)*S
   *             then translate VRML into PDU coordinates
   *
   * @param - none
   *
   * @return void
   */

   public void transform() {


      /**
       * @param double v1 - x rotation
       */
      double v1 = rotateX;

      /**
       * @param double v2 - y rotation
       */
      double v2 = rotateY;

      /**
       * @param double v3 - z rotation
       */
      double v3 = rotateZ;

      double magV = Math.sqrt(v1*v1 + v2*v2 + v3*v3);



      double u1 = 0.0;

      double u2 = 0.0;

      double u3 = 0.0;



      if (magV != 0.0) {

         u1 = v1 / magV;

         u2 = v2 / magV;

         u3 = v3 / magV;

      } //endif



      double s11 = 0.0;

      double s12 = -1.0 * u3;

      double s13 = u2;

      double s21 = u3;

      double s22 = 0.0;

      double s23 = -1.0 * u1;

      double s31 = -1.0 * u2;

      double s32 = u1;

      double s33 = 0.0;



      double uu11 = u1 * u1;

      double uu12 = u1 * u2;

      double uu13 = u1 * u3;

      double uu21 = uu12;

      double uu22 = u2 * u2;

      double uu23 = u2 * u3;

      double uu31 = uu13;

      double uu32 = uu23;

      double uu33 = u3 * u3;


      /**
       * @param double cosAlpha - the cos of the rotation angle
       */
      double cosAlpha = Math.cos(rotateAngle);


      /**
       * @param double sinAlpha - the sin of the rotation angle
       */
      double sinAlpha = Math.sin(rotateAngle);



      double r11 = uu11 + cosAlpha*(1.0-uu11);

      double r12 = uu12 - cosAlpha*uu12       + sinAlpha*s12;

      double r13 = uu13 - cosAlpha*uu13       + sinAlpha*s13;

      double r21 = uu21 - cosAlpha*uu21       + sinAlpha*s21;

      double r22 = uu22 + cosAlpha*(1.0-uu22);

      double r23 = uu23 - cosAlpha*uu23       + sinAlpha*s23;

      double r31 = uu31 - cosAlpha*uu31       + sinAlpha*s31;

      double r32 = uu32 - cosAlpha*uu32       + sinAlpha*s32;

      double r33 = uu33 + cosAlpha*(1.0-uu33);



//   System.out.println("row1 matrix= " + r11 + " " + r12 + " " + r13);

//   System.out.println("row2 matrix= " + r21 + " " + r22 + " " + r23);

//   System.out.println("row3 matrix= " + r31 + " " + r32 + " " + r33);



     double maxX = -999999.0;

     double maxY = -999999.0;

     double maxZ = -999999.0;

     double minX = 999999.0;

     double minY = 999999.0;

     double minZ = 999999.0;



     double maxXR = -999999.0;

     double maxYR = -999999.0;

     double maxZR = -999999.0;

     double minXR = 999999.0;

     double minYR = 999999.0;

     double minZR = 999999.0;



     double maxXT = -999999.0;

     double maxYT = -999999.0;

     double maxZT = -999999.0;

     double minXT = 999999.0;

     double minYT = 999999.0;

     double minZT = 999999.0;



	// Calculate rotated vertices

      for (int ix = 0; ix < numUniquePoints; ix++) {

         if (maxX < xValsArray[ix]) maxX = xValsArray[ix];

         if (maxY < yValsArray[ix]) maxY = yValsArray[ix];

         if (maxZ < zValsArray[ix]) maxZ = zValsArray[ix];



         if (minX > xValsArray[ix]) minX = xValsArray[ix];

         if (minY > yValsArray[ix]) minY = yValsArray[ix];

         if (minZ > zValsArray[ix]) minZ = zValsArray[ix];

        //calculate the rotations

         double rotatedX = xValsArray[ix]*r11 + yValsArray[ix]*r12 + zValsArray[ix]*r13;

         double rotatedY = xValsArray[ix]*r21 + yValsArray[ix]*r22 + zValsArray[ix]*r23;

         double rotatedZ = xValsArray[ix]*r31 + yValsArray[ix]*r32 + zValsArray[ix]*r33;



         xValsArray[ix] = rotatedX;

         yValsArray[ix] = rotatedY;

         zValsArray[ix] = rotatedZ;



         if (maxXR < xValsArray[ix]) maxXR = xValsArray[ix];

         if (maxYR < yValsArray[ix]) maxYR = yValsArray[ix];

         if (maxZR < zValsArray[ix]) maxZR = zValsArray[ix];



         if (minXR > xValsArray[ix]) minXR = xValsArray[ix];

         if (minYR > yValsArray[ix]) minYR = yValsArray[ix];

         if (minZR > zValsArray[ix]) minZR = zValsArray[ix];



//         System.out.println("Rotated x,y,z= " + xValsArray[ix] +

//                 " " + yValsArray[ix] + " " + zValsArray[ix]);



         xValsArray[ix] += translateX;

         yValsArray[ix] += translateY;

         zValsArray[ix] += translateZ;



         if (maxXT < xValsArray[ix]) maxXT = xValsArray[ix];

         if (maxYT < yValsArray[ix]) maxYT = yValsArray[ix];

         if (maxZT < zValsArray[ix]) maxZT = zValsArray[ix];



         if (minXT > xValsArray[ix]) minXT = xValsArray[ix];

         if (minYT > yValsArray[ix]) minYT = yValsArray[ix];

         if (minZT > zValsArray[ix]) minZT = zValsArray[ix];



//       System.out.println("Translated x,y,z= " + xValsArray[ix] +

//                   " " + yValsArray[ix] + " " + zValsArray[ix]);



         // convert VRML coordinates into PDU coordinates

         double tempCoord = zValsArray[ix];

         zValsArray[ix] = -1.0 * yValsArray[ix];

         yValsArray[ix] = tempCoord;



//         System.out.println("PDU (x,y,z)= " + xValsArray[ix] +

//             " " + yValsArray[ix] + " " + zValsArray[ix]);

//         System.out.println();



      } //next



//   System.out.println("Max (x,y,z) prior= " + maxX + " " + maxY + " " + maxZ);

//   System.out.println("Min (x,y,z) prior= " + minX + " " + minY + " " + minZ);

//   System.out.println("Max (x,y,z) R but T " + maxXR + " " + maxYR + " " + maxZR);

//   System.out.println("Min (x,y,z) R but T " + minXR + " " + minYR + " " + minZR);

//   System.out.println("Max (x,y,z) after= " + maxXT + " " + maxYT + " " + maxZT);

//   System.out.println("Min (x,y,z) after= " + minXT + " " + minYT + " " + minZT);



   } // end transform





   /**
    * createFullArray() - create complete array of all coordinate data
    *
    *                   using index information
    * @param - none
    *
    * @return void
   */

   public void createFullArray() {



      int ix = 0;

      data.removeAllElements();                  // empty coordinate data vector



      for (Enumeration e = index.elements(); e.hasMoreElements();) {

         int[] coordIndex = (int[]) e.nextElement();

//         System.out.println("Insert index= " + coordIndex[0]);

         double[] valX = new double[1];

         valX[0] = xValsArray[coordIndex[0]];    // get index and place

         data.add(valX);

         double[] valY = new double[1];

         valY[0] = yValsArray[coordIndex[0]];    // (x,y,z) coord in vector

         data.add(valY);

         double[] valZ = new double[1];

         valZ[0] = zValsArray[coordIndex[0]];    // in proper order

         data.add(valZ);

      } //next



      for (Enumeration e = data.elements(); e.hasMoreElements(); ix++) {

         double[] values = (double[]) e.nextElement();

         xValsArray[ix] = values[0];

         values = (double[]) e.nextElement();

         yValsArray[ix] = values[0];

         values = (double[]) e.nextElement();

         zValsArray[ix] = values[0];

//       System.out.println("(x,y,z)= " +  xValsArray[ix] + " " + yValsArray[ix] +

//                " " + zValsArray[ix]);

      } //next



      data.removeAllElements();                  // free up vector memory

      index.removeAllElements();



   } // end createFullArray





   /**
    * calcNormals() - calculates the normals from the information from the terrain file <p>
    * being used.
    *
    * @param - none
    *
    * @return - void
    */

   public void calcNormals() {



      numFacets = numPoints / 3;

      xFacetNormal = new double[numFacets];

      yFacetNormal = new double[numFacets];

      zFacetNormal = new double[numFacets];

      int w = 0;



	// Calculate normals for each triangle surface

      // cross product: (v3 - v2) X (v1 - v2)

      for (int currFacet = 0; currFacet < numFacets; w=w+3, currFacet++) {

         double XV3MinusV2 = xValsArray[w+2] - xValsArray[w+1];

         double YV3MinusV2 = yValsArray[w+2] - yValsArray[w+1];

         double ZV3MinusV2 = zValsArray[w+2] - zValsArray[w+1];

         double XV1MinusV2 = xValsArray[w] - xValsArray[w+1];

         double YV1MinusV2 = yValsArray[w] - yValsArray[w+1];

         double ZV1MinusV2 = zValsArray[w] - zValsArray[w+1];



         double xNormal    = YV3MinusV2 * ZV1MinusV2 - YV1MinusV2 * ZV3MinusV2;

         double yNormal    = XV1MinusV2 * ZV3MinusV2 - XV3MinusV2 * ZV1MinusV2;

         double zNormal    = XV3MinusV2 * YV1MinusV2 - XV1MinusV2 * YV3MinusV2;



	   double mag = xNormal * xNormal + yNormal * yNormal + zNormal * zNormal;



         if (mag != 0.0) {

            mag = 1.0 / Math.sqrt(mag);

		xFacetNormal[currFacet] = xNormal * mag;

		yFacetNormal[currFacet] = yNormal * mag;

		zFacetNormal[currFacet] = zNormal * mag;

         }

         else {

            xFacetNormal[currFacet] = yFacetNormal[currFacet] = 0.0;

            zFacetNormal[currFacet] = -1.0;

	   } //endif



//   if (currFacet < 45) {

//         System.out.println("Vertices a, b, c; FacetNormal=");

//         System.out.println("a: " +  xValsArray[w] +

//                 " " + yValsArray[w] + " " + zValsArray[w]);

//         System.out.println("b: " +  xValsArray[w+1] +

//                 " " + yValsArray[w+1] + " " + zValsArray[w+1]);

//         System.out.println("c: " +  xValsArray[w+2] +

//                 " " + yValsArray[w+2] + " " + zValsArray[w+2]);

//         System.out.println("Normal: " + xFacetNormal[currFacet] +

//            " " + yFacetNormal[currFacet] + " " + zFacetNormal[currFacet]);

//         System.out.println();

// } //endif

      } //next



   } // end calcNormals


   /**
    *
    * getNormalHeight - return normal vector of current location in
    *
    *                   array indices 0-2 and height in position 3
    *
    */

   public void getNormalHeight(double xCoord, double yCoord, double []normalHeight) {



//      boolean onFlatTerrainFlag = false;

//      if (Math.abs(xCoord) <= 5000 && Math.abs(yCoord) <= 5000) {

//         onFlatTerrainFlag = true;

//      } // endif



      normalHeight[0] = 0.0;          // normal initially vertical, 0 height

      normalHeight[1] = 0.0;

      normalHeight[2] = -1.0;

      normalHeight[3] = 0.0;

      int ix = 0;                     // index number of vertices

      int jx = 0;                     // index number of facet normals

      int startIx;

      int endIx;

      boolean found = false;



     // to speed up processing, find indexed face set of current position

     // process indexed face set #12

     if (xCoord < 5519 && xCoord > -13088 && yCoord < 9548 && yCoord > -8131) {

//        System.out.println("Position in face set 12");

        startIx = 6586;

        endIx   = 7258;

     }

     // process indexed face set #1

     else if (xCoord < 28067 && xCoord > -17471 && yCoord < 17535 && yCoord > -38048) {

//        System.out.println("Position in face set 1");

        startIx = 0;

        endIx   = 823;

     }

     // process indexed face set #2

     else if (xCoord < -8184 && xCoord > -17467 && yCoord < 10828 && yCoord > -10671) {

//        System.out.println("Position in face set 2");

        startIx = 824;

        endIx   = 1172;

     }

     // process indexed face set #3

     else if (xCoord < 28052 && xCoord > 14649 && yCoord < 8802 && yCoord > -8568) {

//        System.out.println("Position in face set 3");

        startIx = 1173;

        endIx   = 1464;

     }

     // process indexed face set #4

     else if (xCoord < 28059 && xCoord > 13469 && yCoord < -6576 && yCoord > -23835) {

//        System.out.println("Position in face set 4");

        startIx = 1465;

        endIx   = 1651;

     }

     // process indexed face set #5

     else if (xCoord < 3453 && xCoord > -14065 && yCoord < -6528 && yCoord > -23104) {

//        System.out.println("Position in face set 5");

        startIx = 1652;

        endIx   = 2583;

     }

     // process indexed face set #6

     else if (xCoord < 15979 && xCoord > 1970 && yCoord < -22020 && yCoord > -38040) {

//        System.out.println("Position in face set 6");

        startIx = 2584;

        endIx   = 3526;

     }

     // process indexed face set #7

     else if (xCoord < 16101 && xCoord > 2113 && yCoord < -6336 && yCoord > -24362) {

//        System.out.println("Position in face set 7");

        startIx = 3527;

        endIx   = 4549;

     }

     // process indexed face set #8

     else if (xCoord < -7363 && xCoord > -17451 && yCoord < -22062 && yCoord > -38048) {

//        System.out.println("Position in face set 8");

        startIx = 4550;

        endIx   = 4695;

     }

     // process indexed face set #9

     else if (xCoord < 16262 && xCoord > 2911 && yCoord < 17533 && yCoord > 7708) {

//        System.out.println("Position in face set 9");

        startIx = 4696;

        endIx   = 5672;

     }

     // process indexed face set #10

     else if (xCoord < 28045 && xCoord > 14970 && yCoord < 17535 && yCoord > 7593) {

//        System.out.println("Position in face set 10");

        startIx = 5673;

        endIx   = 6017;

     }

     // process indexed face set #11

     else if (xCoord < 28067 && xCoord > 14968 && yCoord < -21821 && yCoord > -38031) {

//        System.out.println("Position in face set 11");

        startIx = 6018;

        endIx   = 6585;

     }

     // process indexed face set #13

     else if (xCoord < 4096 && xCoord > -10187 && yCoord < -21900 && yCoord > -38046) {

//        System.out.println("Position in face set 13");

        startIx = 7259;

        endIx   = 7603;

     }

     // process indexed face set #14

     else if (xCoord < -7669 && xCoord > -17458 && yCoord < -7985 && yCoord > -23234) {

//        System.out.println("Position in face set 14");

        startIx = 7604;

        endIx   = 7716;

     }

     // process indexed face set #15

     else if (xCoord < -9128 && xCoord > -17471 && yCoord < 17514 && yCoord > 6865) {

//        System.out.println("Position in face set 15");

        startIx = 7717;

        endIx   = 7911;

     }

     // process indexed face set #16

     else if (xCoord < 3923 && xCoord > -10110 && yCoord < 17518 && yCoord > 6741) {

//        System.out.println("Position in face set 16");

        startIx = 7912;

        endIx   = 7990;

     }

     // process all boundaries between indexed face sets

     else {

//        System.out.println("Position on border of face sets");

        startIx = 0;

        endIx   = 7990;

     } //endif



      jx = startIx;                             // facet to start search

      ix = 3 * jx;                              // vertex to start search



      while (! found && jx <= endIx) {



         // no chance to be in this triangle?

         if ((xCoord < xValsArray[ix] && xCoord < xValsArray[ix+1] && xCoord < xValsArray[ix+2]) ||

             (xCoord > xValsArray[ix] && xCoord > xValsArray[ix+1] && xCoord > xValsArray[ix+2]) ||

             (yCoord < yValsArray[ix] && yCoord < yValsArray[ix+1] && yCoord < yValsArray[ix+2]) ||

             (yCoord > yValsArray[ix] && yCoord > yValsArray[ix+1] && yCoord > yValsArray[ix+2])) {

            // not in this triangle

         }

         // ow let's check the triangle

         else {

            double [] P1 = new double[2];

            P1[0] = xValsArray[ix];

            P1[1] = yValsArray[ix];

            double [] P2 = new double[2];

            P2[0] = xValsArray[ix+1];

            P2[1] = yValsArray[ix+1];

            double [] P3 = new double[2];

            P3[0] = xValsArray[ix+2];

            P3[1] = yValsArray[ix+2];



            double [] P1minusP3 = new double[2];

            P1minusP3[0] = P1[0]-P3[0];

            P1minusP3[1] = P1[1]-P3[1];

            double magP1minusP3Squared = P1minusP3[0]*P1minusP3[0] +

                                      P1minusP3[1]*P1minusP3[1];



            double [] P2minusP1 = new double[2];

            P2minusP1[0] = P2[0]-P1[0];

            P2minusP1[1] = P2[1]-P1[1];

            double magP2minusP1Squared = P2minusP1[0]*P2minusP1[0] +

                                      P2minusP1[1]*P2minusP1[1];



            double [] P3minusP2 = new double[2];

            P3minusP2[0] = P3[0]-P2[0];

            P3minusP2[1] = P3[1]-P2[1];

            double magP3minusP2Squared = P3minusP2[0]*P3minusP2[0] +

                                      P3minusP2[1]*P3minusP2[1];



            // find each angle w/in triangle: theta = acos((AxB)/|A||B|); if

            //  angle between triangle side + position > angle between sides,

            //  then (x,y) position is not inside this triangle

            //* calc each triangle angle

            double dotProdP1 = P2minusP1[0]*P1minusP3[0] + P2minusP1[1]*P1minusP3[1];

            double magP1 = Math.sqrt(magP2minusP1Squared * magP1minusP3Squared);

            double thetaP1 = Math.PI - Math.acos(dotProdP1 / magP1);

//          System.out.println("theta P1= " + thetaP1);



            double dotProdP2 = P3minusP2[0]*P2minusP1[0] + P3minusP2[1]*P2minusP1[1];

            double magP2 = Math.sqrt(magP3minusP2Squared * magP2minusP1Squared);

            double thetaP2 = Math.PI - Math.acos(dotProdP2 / magP2);

//          System.out.println("theta P2= " + thetaP2);



            double dotProdP3 = P1minusP3[0]*P3minusP2[0] + P1minusP3[1]*P3minusP2[1];

            double magP3 = Math.sqrt(magP1minusP3Squared * magP3minusP2Squared);

            double thetaP3 = Math.PI - Math.acos(dotProdP3 / magP3);

//          System.out.println("theta P3= " + thetaP3);



            double [] xyMinusP1 = new double[2];

            xyMinusP1[0] = xCoord-P1[0];

            xyMinusP1[1] = yCoord-P1[1];

            double magxyMinusP1Squared = xyMinusP1[0]*xyMinusP1[0] +

                                      xyMinusP1[1]*xyMinusP1[1];

            double [] xyMinusP2 = new double[2];

            xyMinusP2[0] = xCoord-P2[0];

            xyMinusP2[1] = yCoord-P2[1];

            double magxyMinusP2Squared = xyMinusP2[0]*xyMinusP2[0] +

                                      xyMinusP2[1]*xyMinusP2[1];

            double [] xyMinusP3 = new double[2];

            xyMinusP3[0] = xCoord-P3[0];

            xyMinusP3[1] = yCoord-P3[1];

            double magxyMinusP3Squared = xyMinusP3[0]*xyMinusP3[0] +

                                      xyMinusP3[1]*xyMinusP3[1];



            //* calc each angle between triangle side and position

            double thetaP1xy = 0.0;

            double thetaP2xy = 0.0;

            double thetaP3xy = 0.0;



            double dotProdP1xy = xyMinusP1[0]*P1minusP3[0] + xyMinusP1[1]*P1minusP3[1];

            double magP1xy = Math.sqrt(magxyMinusP1Squared * magP1minusP3Squared);

            if (magP1xy != 0.0) {

               thetaP1xy = Math.PI - Math.acos(dotProdP1xy / magP1xy);

//             System.out.println("theta P1 xy= " + thetaP1xy);



            }

            else {                                   // posit collocated w/vertex

               found = true;

//             System.out.println("Found Match at vertex ix= " + ix);

               normalHeight[0] = xFacetNormal[jx];

               normalHeight[1] = yFacetNormal[jx];

               normalHeight[2] = zFacetNormal[jx];

               normalHeight[3] = zValsArray[ix];

            } //endif



            double dotProdP2xy = xyMinusP2[0]*P2minusP1[0] + xyMinusP2[1]*P2minusP1[1];

            double magP2xy = Math.sqrt(magxyMinusP2Squared * magP2minusP1Squared);

            if (magP2xy != 0.0) {

               thetaP2xy = Math.PI - Math.acos(dotProdP2xy / magP2xy);

//             System.out.println("theta P2 xy= " + thetaP2xy);

            }

            else {                                   // posit collocated wvertex

               found = true;

//             System.out.println("Found Match at vertex ix= " + ix+1);

               normalHeight[0] = xFacetNormal[jx];

               normalHeight[1] = yFacetNormal[jx];

               normalHeight[2] = zFacetNormal[jx];

               normalHeight[3] = zValsArray[ix+1];

            } //endif



            double dotProdP3xy = xyMinusP3[0]*P3minusP2[0] + xyMinusP3[1]*P3minusP2[1];

            double magP3xy = Math.sqrt(magxyMinusP3Squared * magP3minusP2Squared);

            if (magP3xy != 0.0) {

               thetaP3xy = Math.PI - Math.acos(dotProdP3xy / magP3xy);

//             System.out.println("theta P3 xy= " + thetaP3xy);

            }

            else {                                   // posit collocated w/vertex

               found = true;

//             System.out.println("Found Match at vertex ix= " + ix+2);

               normalHeight[0] = xFacetNormal[jx];

               normalHeight[1] = yFacetNormal[jx];

               normalHeight[2] = zFacetNormal[jx];

               normalHeight[3] = zValsArray[ix+2];

            } //endif



            if (!found && (thetaP1 >= thetaP1xy) && (thetaP2 >= thetaP2xy) &&

                       (thetaP3 >= thetaP3xy)) {

               found = true;

//             System.out.println("Found Facet Match at jx= " + jx);

//             System.out.println("** Coordinates= " + xValsArray[ix] +

//                   " " + yValsArray[ix] + " " + zValsArray[ix]);

//             System.out.println(xValsArray[ix+1] + " " + yValsArray[ix+1] + " " +

//                   zValsArray[ix+1]);

//             System.out.println(xValsArray[ix+2] +

//                   " " + yValsArray[ix+2] + " " + zValsArray[ix+2]);

               normalHeight[0] = xFacetNormal[jx];

               normalHeight[1] = yFacetNormal[jx];

               normalHeight[2] = zFacetNormal[jx];



               // calc height using equation of plane and point (D = Ax + By + Cz where

               //  N (normal vector) = Ai + Bj + Ck)

               double D = normalHeight[0]*xValsArray[ix] + normalHeight[1]*yValsArray[ix] +

                       normalHeight[2]*zValsArray[ix];

               if (normalHeight[2] != 0.0) {

                  normalHeight[3] = (D - normalHeight[0]*xCoord  -

                                 normalHeight[1]*yCoord) / normalHeight[2];

//                System.out.println("Height at a: " + zValsArray[ix]);

//                System.out.println("Height at b: " + zValsArray[ix+1]);

//                System.out.println("Height at c: " + zValsArray[ix+2]);

               }

               else {

                  normalHeight[3] = 0.0;

               } //endif



            } //endif



         } //endif



         ix += 3;

         jx++;



         // if not found, then search whole set of arrays

         if (jx > endIx && !found) {

//          System.out.println("Coord not found in facet; check whole file");

            endIx   = 7990;

            jx = 0;

            ix = 0;

          } //endif



      } // endwhile



//      if (onFlatTerrainFlag && normalHeight[3] > 0.0) {   // check if triangle is

//         normalHeight[3] = 0.0;                           //  under flat terrain

//         normalHeight[0] = 0.0;                           // set height to zero

//         normalHeight[1] = 0.0;                           //  and normal vertical

//         normalHeight[2] = -1.0;

//      } //endif



      if (!found) {

//         System.out.println("Position (x,y)= " + xCoord + " " + yCoord +

//            " not found in terrain file.");

         normalHeight[3] = 0.0;                           //  under flat terrain

         normalHeight[0] = 0.0;                           // set height to zero

         normalHeight[1] = 0.0;                           //  and normal vertical

         normalHeight[2] = -1.0;

      } //endif



   } // end getNormalHeight





  /**
   * Main routine for CollisionDetectionTerrainReader clss
   */

   public static void main(String[] args) {

      CollisionDetectionTerrainReader v = new CollisionDetectionTerrainReader();

      v.parseFile();



      // get normal to current terrain position

      double [] normalHeight = new double [4];

      double xVal = 6434.0;

      double yVal = -3720.0;



      v.getNormalHeight(xVal, yVal, normalHeight);

//      System.out.println("Returned normal: " + normalHeight[0] + " " +

//         normalHeight[1] + " " + normalHeight[2]);

//      System.out.println("Returned height: " + normalHeight[3]);



      xVal = 6105;

      yVal = -3750;



      v.getNormalHeight(xVal, yVal, normalHeight);

//      System.out.println("Returned normal: " + normalHeight[0] + " " +

//         normalHeight[1] + " " + normalHeight[2]);

//      System.out.println("Returned height: " + normalHeight[3]);



      xVal = 6105;

      yVal = -4200;



      v.getNormalHeight(xVal, yVal, normalHeight);

//      System.out.println("Returned normal: " + normalHeight[0] + " " +

//         normalHeight[1] + " " + normalHeight[2]);

//      System.out.println("Returned height: " + normalHeight[3]);



   } //end main



} // end CollisionDetectionTerrainReader