/*
    * Copyright (C) 2013 AMIS research group, Faculty of Mathematics and Physics, Charles University in Prague, Czech Republic
    *
    * This program is free software: you can redistribute it and/or modify
    * it under the terms of the GNU General Public License as published by
    * the Free Software Foundation, either version 3 of the License, or
    * (at your option) any later version.
    *
    * This program is distributed in the hope that it will be useful,
   * but WITHOUT ANY WARRANTY; without even the implied warranty of
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   * GNU General Public License for more details.
   *
   * You should have received a copy of the GNU General Public License
   * along with this program.  If not, see <http://www.gnu.org/licenses/>.
   */
  package cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh;
  
  import java.io.BufferedReader;
  import java.io.File;
  import java.io.FileNotFoundException;
  import java.io.FileReader;
  import java.io.IOException;
  import java.io.ObjectInputStream;
  import java.io.Serializable;
  import java.util.ArrayList;
  import java.util.List;
  import java.util.Random;
  import java.util.logging.Logger;
  
  import javax.vecmath.Vector2d;
  import javax.vecmath.Vector3d;
  
  import math.geom2d.Point2D;
  import math.geom2d.line.StraightLine2D;
  import math.geom3d.Point3D;
  import math.geom3d.Vector3D;
  import math.geom3d.line.StraightLine3D;
  import math.geom3d.plane.Plane3D;
  import cz.cuni.amis.pogamut.base.utils.logging.LogCategory;
  import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
  import cz.cuni.amis.pogamut.ut2004.agent.module.sensomotoric.BSPRayInfoContainer;
  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.AutoTraceRay;
  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.AutoTraceRayMessage;
  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Self;
  import cz.cuni.amis.utils.ExceptionToString;
  
  /**
   * Class containing complete data with the geometry of the environment
   * It is useful for steering.
   * It is part of NavMesh class.
   * 
   * @author Jakub Tomek
   */
  public class LevelGeometry implements Serializable {
  	
  	/**
  	 * Auto-generated.
  	 */
  	private static final long serialVersionUID = -8146957816661176041L;
  
  	public static class RaycastResult {
  		public Location from;
  		public Location to;
  		public Location rayVector;
  		public Location hitLocation;
  		public Vector3d hitNormal;
  		public boolean hit;
  		public double hitDistance;
  		public List<LevelGeometryBSPNode> travelledNodes = new ArrayList<LevelGeometryBSPNode>();
  		public List<Location> rayPoints = new ArrayList<Location>();
  		/**
  		 * Index into {@link LevelGeometry#triangles}.
  		 */
  		public int hitTriangle;
  		
  		public RaycastResult(Location from, Location to) {
  			super();
  			this.from = from;
  			this.to = to;
  			this.hit = false;
  			rayVector = to.sub(from);
  		}		
  	}
      
  	private transient Logger log; 
  	
  	private transient boolean loaded = false;
  	
  	public static Random random = new Random();
  	
      public ArrayList<double[]> verts = new ArrayList<double[]>();
      public ArrayList<int[]>  triangles = new ArrayList<int[]>();
      private LevelGeometryBSPNode bspTree;
      private LevelGeometryBSPNode biggestLeafInTree;
      public int leafCount = 0;
      
      // boundries of the space
      public double minX = Double.POSITIVE_INFINITY;
     public double maxX = Double.NEGATIVE_INFINITY;
     public double minY = Double.POSITIVE_INFINITY;
     public double maxY = Double.NEGATIVE_INFINITY;
     public double minZ = Double.POSITIVE_INFINITY;
     public double maxZ = Double.NEGATIVE_INFINITY;
 
     /**
      * Constructor creates the object from a file defined by map name and path which is in constants
      * @param mapName 
      */
     public LevelGeometry(Logger log) {
         this.log = log;
         if (this.log == null) {
         	this.log = new LogCategory("LevelGeometry");
         }
     }
     
     public boolean isLoaded() {
     	return loaded;
     }
     
     public LevelGeometryBSPNode getBSPRoot() {
     	return bspTree;
     }
     
     public void clear() {
     	loaded = false;
     	
     	verts = new ArrayList<double[]>();
         triangles = new ArrayList<int[]>();
         bspTree = null;
         biggestLeafInTree = null;
         leafCount = 0;
         
         minX = Double.POSITIVE_INFINITY;
         maxX = Double.NEGATIVE_INFINITY;
         minY = Double.POSITIVE_INFINITY;
         maxY = Double.NEGATIVE_INFINITY;
         minZ = Double.POSITIVE_INFINITY;
         maxZ = Double.NEGATIVE_INFINITY; 
     }
     
     public boolean load(String mapName) throws FileNotFoundException, IOException, Exception { 
     	if (loaded) {
     		clear();
     	}
     	
         // vertices are indexed from 1. Put something to 0
         verts.add(new double[0]);
         
         double scale;
         double[] centre;
         String fileName, line;
         File file;
         BufferedReader br;
         
         // read scale
         fileName = NavMeshConstants.pureLevelGeometryReadDir + "\\" + mapName + ".scale";
         file = new File(fileName);
         if (!file.exists()) {
         	log.warning("LevelGeometry .scale file does not exist at: " + file.getAbsolutePath());
         	return false;
         }
         br = new BufferedReader(new FileReader(file));
         line = br.readLine();
         scale = Double.parseDouble(line);
         
         // read centre
         fileName = NavMeshConstants.pureLevelGeometryReadDir + "\\" + mapName + ".centre";
         file = new File(fileName);
         if (!file.exists()) {
         	log.warning("LevelGeometry .centre file does not exist at: " + file.getAbsolutePath());
         	return false;
         }
         br = new BufferedReader(new FileReader(file));
         line = br.readLine();
         String[] sc = line.split("[ \\t]");
         centre = new double[3];
         for(int i=0; i<3; i++) {
             centre[i] = Double.parseDouble(sc[i]);
         }
         
         // read all vertices and triangles from file
         fileName = NavMeshConstants.pureLevelGeometryReadDir + "\\" + mapName + ".obj";
         file = new File(fileName);
         if (!file.exists()) {
         	log.warning("LevelGeometry .obj file does not exist at: " + file.getAbsolutePath());
         	return false;
         }
         br = new BufferedReader(new FileReader(file));     
         while((line = br.readLine()) != null) {
             String[] words = line.split("[ \\t]");
             if(words[0].equals("v")) {
                 double[] v = new double[3];
                 v[0] = Double.parseDouble(words[1])*scale + centre[0];
                 v[1] = Double.parseDouble(words[3])*scale + centre[1];
                 v[2] = Double.parseDouble(words[2])*scale + centre[2];
                 verts.add(v);
                 
                 // check the boundries
                 if(v[0] < minX) minX = v[0];
                 if(v[0] > maxX) maxX = v[0];
                 if(v[1] < minY) minY = v[1];
                 if(v[1] > maxY) maxY = v[1];
                 if(v[2] < minZ) minZ = v[2];
                 if(v[2] > maxZ) maxZ = v[2];                
                 
             }
             if(words[0].equals("f")) {
                 int[] t = new int[3];
                 t[0] = Integer.parseInt(words[1]);
                 t[1] = Integer.parseInt(words[2]);
                 t[2] = Integer.parseInt(words[3]);
                 triangles.add(t);
             }
         }
         
         // create a BSP tree structure
         resetBSPTree();
         
         loaded = true;
         
         return true;
     }
     
 	private void readObject(ObjectInputStream input) throws ClassNotFoundException, IOException {
     	input.defaultReadObject();
     	log = new LogCategory("LevelGeometry");
     	loaded = true;
     }
     
 //    public void draw(UT2004Server server, Location color) {
 //        for(int tId = 0; tId < triangles.size(); tId++) {
 //            DrawStayingDebugLines d = new DrawStayingDebugLines();
 //            String lines = this.triangleToDebugString(tId);
 //            d.setVectors(lines);
 //            d.setColor(new Location(255, 255, 255));
 //            d.setClearAll(tId==0);                
 //            server.getAct().act(d); 
 //        }
 //    }
 //
 //    public void drawOneTriangle(UT2004Server server, int tId, Location color) {
 //        DrawStayingDebugLines d = new DrawStayingDebugLines();
 //        String lines = this.triangleToDebugString(tId);
 //        d.setVectors(lines);
 //        d.setColor(color);
 //        d.setClearAll(false);                
 //        server.getAct().act(d);                           
 //    }
 //
 //    private String triangleToDebugString(int tId) {
 //        StringBuilder result = new StringBuilder("");      
 //        // projdeme vsechny polygony a vykreslime caru vzdy z vrcholu n do n+1
 //        int[] t = (int[]) triangles.get(tId);
 //        for(int j = 0; j<t.length; j++) {
 //            if(result.length()>0) result.append(";");
 //            // ziskame vrcholy v1 a v2 mezi kterymi vykreslime caru
 //            double[] v1,v2;
 //            v1 = (double[]) verts.get(t[j]);
 //            if(j==t.length-1) v2 = (double[]) verts.get(t[0]);
 //            else v2 = (double[]) verts.get(t[j+1]);
 //            // pridani cary
 //            result.append(v1[0]+","+v1[1]+","+v1[2]+";"+v2[0]+","+v2[1]+","+v2[2]);
 //        }     
 //        return result.toString(); 
 //    }
 
     /**
      * Gets a new BSPTree for this geometry
      */
     private void resetBSPTree() throws Exception {
         log.info("Creating BSP tree...");
         bspTree = new LevelGeometryBSPNode(this, null, minX, maxX, minY, maxY, minZ, maxZ);
         for(int i = 0; i<triangles.size(); i++) bspTree.triangles.add(i);
         biggestLeafInTree = null;
         bspTree.build();
         log.info("BSP tree is done building.");
         log.info("Biggest leaf has " + biggestLeafInTree.triangles.size() + " triangles.");
         log.info("BSP tree has " +leafCount+ " leaves");
         bspTree.cleanInnerNodes();      
     }
 
     /**
      * Sets the biggest leag in tree
      * @param node 
      */
     void setBiggestLeafInTree(LevelGeometryBSPNode node) {
         biggestLeafInTree = node;
     }
 
     int getNumberOfTrianglesInBiggestLeaf() {
         if(biggestLeafInTree==null) return 0;
         else return biggestLeafInTree.triangles.size();
     }
 
     /**
      * Returns the node of BSP leaf which contains this location 
      * Walk througt BSP tree compating the coordinates
      * @param from
      * @return 
      */
     private LevelGeometryBSPNode getBSPLeaf(Location location) {
         double x = location.x;
         double y = location.y;
         double z = location.z;
         
         LevelGeometryBSPNode node = this.bspTree;
         //check if we are in
         if(x>node.maxX || x<node.minX) return null;
         if(y>node.maxY || y<node.minY) return null;
         if(z>node.maxZ || z<node.minZ) return null;
         
         // we want a leaf!
         while(node.left!=null) {
             // in which half are we?
             switch(node.sepDim) {
                 case 0:
                     if(x<node.sepVal) node = node.left;
                     else node = node.right;
                     break;
                 case 1:
                     if(y<node.sepVal) node = node.left;
                     else node = node.right;                    
                     break;
                 case 2:
                     if(z<node.sepVal) node = node.left;
                     else node = node.right;                    
                     break;
                 default: throw new UnsupportedOperationException("This should not happen!");
             }
         }
         return node;
     }
     
     /**
      * The most important method on this class.
      * It computes the information about collision of the given ray and this geometry
      * @param self
      * @param rayInfo
      * @return 
      */
     public AutoTraceRay getAutoTraceRayMessage(Self self, BSPRayInfoContainer rayInfo) {
 
         // lets update the direction vector according to agent rotation  
         Vector3d direction = rayInfo.direction;
         direction.normalize();
         
         // angle up/down
         double upDownAngle = Math.asin(direction.getZ()/1);
         
         // we transform the ray vector to polar coordinates 
         // -32768 - 32767 total is 65536
         double UTFullAngle = 65536;
         double UTHalfAngle = UTFullAngle/2;
         double UTQuarterAngle = UTHalfAngle/2;        
         // yaw:  
         double rayYaw;
         double x = rayInfo.direction.x;
         double y = rayInfo.direction.y;
         
         if(x==0 && y==0) {
             direction = new Vector3d(0,0,1);
         } else {
             rayYaw = NavMeshConstants.transform2DVectorToRotation(new Vector2d(x,y));
             // now we have the input yaw
             // let's add adent's rotation to get the final rotation
             double Yaw = rayYaw + self.getRotation().getYaw();
             Vector2d vector2d = NavMeshConstants.transformRotationTo2DVector(Yaw);
             direction.x = vector2d.x;
             direction.y = vector2d.y;
             // now we have the direction on x,y.
             // we must add back the z direction
             direction.normalize();
             direction.z = Math.tan(upDownAngle);           
         }
         
         
         direction.normalize();
         // last thing with direction: stretch it to the proper length
         direction.x *= rayInfo.length;
         direction.y *= rayInfo.length;
         direction.z *= rayInfo.length;   
         // The direction is now Done. The agent's rotation is added
 
 
         
         // now get the from and to locations
         
         Location from = self.getLocation(); 
         Location to = new Location(from.x + direction.x, from.y + direction.y, from.z + direction.z); 
 
         // call the recursive function
         return getAutoTraceRayMessage(from, to, rayInfo);
     }
     
     /**
      * Recursive function for getting ray collision
      * @param from
      * @param to
      * @param rayInfo
      * @return 
      */
     private AutoTraceRay getAutoTraceRayMessage(Location from, Location to, BSPRayInfoContainer rayInfo) {
         // the actual 3D line
         StraightLine3D ray =  new StraightLine3D(from.asPoint3D(), to.asPoint3D());              
         
         RaycastResult raycast = raycast(from, to);
         
         if (raycast.hit) {
         	return 
         			new AutoTraceRayMessage(
         					rayInfo.unrealId,
         					from,
         					to,
         					false,
         					rayInfo.floorCorrection,
         					raycast.hit,
         					raycast.hitNormal,
         					raycast.hitLocation,
         					false,
         					null
         			); 
           
           } else {
               // otherwise there is no intersection at all
               return 
             		  new AutoTraceRayMessage(
             				  rayInfo.unrealId,
             				  from,
             				  to,
             				  false,
             				  rayInfo.floorCorrection,
             				  false,
             				  null,
             				  null,
             				  false,
             				  null
                       ); 
         }
     }
     
     /**
      * Recursive function for getting ray collision
      * @param from
      * @param to
      * @param rayInfo
      * @return 
      */
     public RaycastResult raycast(Location from, Location to) {
     	RaycastResult result = new RaycastResult(from, to);
     	try {
     		raycastInner(from, to, result);
     	} catch (Exception e) {
     		log.severe(ExceptionToString.process("Failed to raycast: " + from + " -> " + to, e));
     	}
     	if (result.hit) {
     		result.hitDistance = result.hitLocation.getDistance(from.getLocation());
     	}
     	return result;
     }
     
     private boolean isValidNumber(Double number) {
     	if (number == null) return false;
     	if (number == Double.POSITIVE_INFINITY) return false;
     	if (number == Double.NEGATIVE_INFINITY) return false;
     	if (number == Double.NaN) return false;
     	return true;
     }
     
     private boolean isValidPoint(Point3D point) {
     	return isValidNumber(point.getX()) && isValidNumber(point.getY()) && isValidNumber(point.getZ());
     }
     
     private void raycastInner(Location from, Location to, RaycastResult result) {
         // the actual 3D line
         StraightLine3D ray =  new StraightLine3D(from.asPoint3D(), to.asPoint3D());              
                 
         // information abou ray's collision with triangle
         boolean collisionFound = false;
         double collisionDistance = Double.POSITIVE_INFINITY;
         Location hitLocation = null;
         Vector3d normalVector = null;
         
         // now start examining hitting the walls
         // let's get the BSP leaf
         LevelGeometryBSPNode leaf = this.getBSPLeaf(from);
         
         if (leaf == null) {
         	// we're pretty far out of the geometry
         	// TODO: not ideal...
         	return;
         }
         
         result.travelledNodes.add(leaf);
         
         int triangleHit = -1;
         
         // now let's examine the ray's collisions with triangles
         for(Integer tId : leaf.triangles) {
             int[] triangle = this.triangles.get(tId);
             double[] v1 = this.verts.get(triangle[0]);
             double[] v2 = this.verts.get(triangle[1]);
             double[] v3 = this.verts.get(triangle[2]);
             Point3D p1 = new Point3D(v1[0],v1[1],v1[2]);
             Vector3D vector1 = new Vector3D(v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]);
             Vector3D vector2 = new Vector3D(v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2]);            
             Plane3D plane = new Plane3D(p1,vector1,vector2);
             Point3D intersection = plane.getLineIntersection(ray);
             
             // is intersection inside triangle?
             boolean collision = isValidPoint(intersection) && this.isPointInTriangle(intersection, tId);
             if(!collision) continue;
             
             // is the point on the HalfLine FROM->TO?
             double zero = intersection.getDistance(from.asPoint3D()) + intersection.getDistance(to.asPoint3D()) - from.getDistance(to);
             collision = Math.abs(zero) < 0.001;
             if(!collision) continue;
 
             // we remember the closest collision
             double distance = intersection.getDistance(from.asPoint3D());
             if(distance < collisionDistance) {
                 collisionFound = true;
                 collisionDistance = distance;
                 hitLocation = new Location(intersection.getX(),intersection.getY(),intersection.getZ());
                 Vector3D normalVector3D = plane.getNormalVector();
                 normalVector = new Vector3d(normalVector3D.getX(),normalVector3D.getY(),normalVector3D.getZ());
                 triangleHit = tId;
             }
         }
         
         // if we found a collision, we collect and return the information about it
         if(collisionFound) {
         	result.hitLocation = hitLocation;
         	result.hitNormal = normalVector;
         	result.hit = true;
         	result.hitTriangle = triangleHit;
             return;
         }
         
         // we did not find a collision. Does the ray continue to different node?
         boolean out = false;
         
         if(to.x > leaf.maxX) {
             out = true;
         }
         if(to.x < leaf.minX) {
             out = true;
         }         
         if(to.y > leaf.maxY) {
             out = true;
         }
         if(to.y < leaf.minY) {
             out = true;
         }
         if(to.z > leaf.maxZ) {
             out = true;
         }
         if(to.z < leaf.minZ) {
             out = true;
         }
         
         if (!out) {
         	// NO COLLISION AT ALL
         	return;
         }
         
         // if we are out, we must continue to the next node
         // we must find the ray's intersections with the nodee's borders. 
         // we will take 3 intersection with planes bordering the node on x,y,z and take the clocest one
         
         collisionDistance = Double.MAX_VALUE;
         Point3D collisionPoint = null; 
         double distance;
         
         // 1. x axis
         if(to.x > from.x) {
             Point3D p1 = new Point3D(leaf.maxX,0,0);
             Vector3D vector1 = new Vector3D(0, 1, 0);
             Vector3D vector2 = new Vector3D(0, 0, 1);
             Plane3D plane = new Plane3D(p1,vector1,vector2);
             Point3D intersection = plane.getLineIntersection(ray);
             if (isValidPoint(intersection)) {
             	distance = intersection.getDistance(from.asPoint3D());
 	            // is it the closest one sofar?
 	            if(distance < collisionDistance) {
 	                collisionPoint = intersection;
 	                collisionDistance = distance;
 	            }
             }
         } else {
             Point3D p1 = new Point3D(leaf.minX,0,0);
             Vector3D vector1 = new Vector3D(0, 1, 0);
             Vector3D vector2 = new Vector3D(0, 0, 1);
             Plane3D plane = new Plane3D(p1,vector1,vector2);
             Point3D intersection = plane.getLineIntersection(ray);
             if (isValidPoint(intersection)) {
             	distance = intersection.getDistance(from.asPoint3D());
 	            // is it the closest one sofar?
 	            if(distance < collisionDistance) {
 	                collisionPoint = intersection;
 	                collisionDistance = distance;
 	            }
             }
         }
         
         // 2. y axis
         if(to.y > from.y) {
             Point3D p1 = new Point3D(0, leaf.maxY,0);
             Vector3D vector1 = new Vector3D(1, 0, 0);
             Vector3D vector2 = new Vector3D(0, 0, 1);
             Plane3D plane = new Plane3D(p1,vector1,vector2);
             Point3D intersection = plane.getLineIntersection(ray);
             if (isValidPoint(intersection)) {
             	distance = intersection.getDistance(from.asPoint3D());
 	            // is it the closest one sofar?
 	            if(distance < collisionDistance) {
 	                collisionPoint = intersection;
 	                collisionDistance = distance;
 	            }
             }
         } else {
             Point3D p1 = new Point3D(0,leaf.minY,0);
             Vector3D vector1 = new Vector3D(1, 0, 0);
             Vector3D vector2 = new Vector3D(0, 0, 1);
             Plane3D plane = new Plane3D(p1,vector1,vector2);
             Point3D intersection = plane.getLineIntersection(ray);
             distance = intersection.getDistance(from.asPoint3D());
             // is it the closest one sofar?
             if(distance < collisionDistance) {
                 collisionPoint = intersection;
                 collisionDistance = distance;
             }
         }
         
         // 3. z axis
         if(to.z > from.z) {
             Point3D p1 = new Point3D(0,0,leaf.maxZ);
             Vector3D vector1 = new Vector3D(1, 0, 0);
             Vector3D vector2 = new Vector3D(0, 1, 0);
             Plane3D plane = new Plane3D(p1,vector1,vector2);
             Point3D intersection = plane.getLineIntersection(ray);
             if (isValidPoint(intersection)) {
 	            distance = intersection.getDistance(from.asPoint3D());
 	            // is it the closest one sofar?
 	            if(distance < collisionDistance) {
 	                collisionPoint = intersection;
 	                collisionDistance = distance;
 	            }
             }
         } else {
             Point3D p1 = new Point3D(0,0,leaf.minZ);
             Vector3D vector1 = new Vector3D(1, 0, 0);
             Vector3D vector2 = new Vector3D(0, 1, 0);
             Plane3D plane = new Plane3D(p1,vector1,vector2);
             Point3D intersection = plane.getLineIntersection(ray);
             if (isValidPoint(intersection)) {
 	            distance = intersection.getDistance(from.asPoint3D());
 	            // is it the closest one sofar?
 	            if(distance < collisionDistance) {
 	                collisionPoint = intersection;
 	                collisionDistance = distance;
 	            }
             }
         }
         
         if (collisionPoint == null) {
         	// ???
         	return;
         }
         
         // now we have the collision point at the border of the node.
         // we will move it a little further, to push it into the node
         Vector3d direction = to.sub(from).getNormalized().asVector3d();
         Location newPoint = new Location(collisionPoint.getX()+direction.x,collisionPoint.getY()+direction.y,collisionPoint.getZ()+direction.z);
         
         if (from.getDistance(to) < from.getDistance(newPoint)) {
         	// NO COLLISION
         	return;
         }
         
         // and get the answer recursively...
         result.rayPoints.add(newPoint);
         raycastInner(newPoint, to, result);        
     }
 
 //    /***
 //     * returns true is point is  inside 3D triangle, assuming it is on the same 2D plane in 3D
 //     * @param intersection
 //     * @param tId
 //     * @return 
 //     */
 //    private boolean isPointInsideTriangle(Point3D point3D, Integer tId) {
 //       boolean result = true;
 //       int[] triangle = this.triangles.get(tId);
 //            
 //       // let's see if the point is inside in 2D projection
 //       Point2D point2D = new Point2D(point3D.getX(), point3D.getY());
 //       double rightSide = 0.0;
 //       double[] v1, v2;
 //       for(int i = 0; i < triangle.length; i++) {
 //           v1 = this.verts.get(triangle[i]);
 //           if(i < triangle.length -1) v2 = this.verts.get(triangle[i+1]);
 //           else  v2 = this.verts.get(triangle[0]);
 //           Point2D p1 = new Point2D(v1[0], v1[1]);
 //           Point2D p2 = new Point2D(v2[0], v2[1]);
 //           StraightLine2D line = new StraightLine2D(p1, p2);
 //           double dist = line.getSignedDistance(point2D);
 //           
 //           if(rightSide == 0.0) {
 //               rightSide = Math.signum(dist);
 //           }
 //           else {
 //               if(rightSide * dist < 0) {
 //                   result = false;
 //                   break;
 //               }
 //           }
 //       }
 //       return result;      
 //    }
     
     public boolean isPointInTriangle(Point3D point3D, Integer tId) {
     	int[] triangle = triangles.get(tId);
     	
     	Location p  = new Location(point3D); 
     	Location a = new Location(verts.get(triangle[0]));
     	Location b = new Location(verts.get(triangle[1]));
     	Location c = new Location(verts.get(triangle[2]));
 
     	Location v0 = b.sub(a);
     	Location v1 = c.sub(a);
     	Location v2 = p.sub(a);
     	
     	double d00 = v0.dot(v0);
     	double d01 = v0.dot(v1);
     	double d11 = v1.dot(v1);
     	double d20 = v2.dot(v0);
     	double d21 = v2.dot(v1);
     	double denom = d00 * d11 - d01 *d01;
     	
     	if (Math.abs(denom) < 0.0000001) return false; // stupid triangle, ignore...
     	
     	double v = (d11 * d20 - d01 * d21) / denom;
         double w = (d00 * d21 - d01 * d20) / denom;
         double u = 1 - v - w;
         
         boolean inside = 0 < u && u < 1 && 0 < v && v < 1 && 0 < w && w < 1;
     	
     	if (inside) {
     		return true;
     	} else {
     		return false;
     	}
     }
 
 	public double[][] getTriangle(int tId) {
 		int[] triangle = triangles.get(tId);
 		return new double[][]{ verts.get(triangle[0]), verts.get(triangle[1]), verts.get(triangle[2]) };
 	}
     
 }