context: using UnityEngine;
  using System.Collections.Generic;
  using Pathfinding.Serialization.JsonFx;
  using Pathfinding.Serialization;
  using Pathfinding;
  
  namespace Pathfinding {
  	public interface INavmesh {
  		void GetNodes(GraphNodeDelegateCancelable del);
  	}
  	
  	[System.Serializable]
  	[JsonOptIn]
  	/** Generates graphs based on navmeshes.
  \ingroup graphs
  Navmeshes are meshes where each polygon define a walkable area.
  These are great because the AI can get so much more information on how it can walk.
  Polygons instead of points mean that the funnel smoother can produce really nice looking paths and the graphs are also really fast to search
  and have a low memory footprint because of their smaller size to describe the same area (compared to grid graphs).
  \see Pathfinding.RecastGraph
  
  \shadowimage{navmeshgraph_graph.png}
  \shadowimage{navmeshgraph_inspector.png}
  
  	 */
  	public class NavMeshGraph : NavGraph, INavmesh, IUpdatableGraph, IFunnelGraph, INavmeshHolder
  	,IRaycastableGraph 
  	{
  		/** Mesh to construct navmesh from */
  		[JsonMember]
  		public Mesh sourceMesh;
  
  		/** Offset in world space */
  		[JsonMember]
  		public Vector3 offset;
  
  		/** Rotation in degrees */
  		[JsonMember]
  		public Vector3 rotation;
  
  		/** Scale of the graph */
  		[JsonMember]
  		public float scale = 1;
  		
  		/** More accurate nearest node queries.
  		 * When on, looks for the closest point on every triangle instead of if point is inside the node triangle in XZ space.
  		 * This is slower, but a lot better if your mesh contains overlaps (e.g bridges over other areas of the mesh).
  		 * Note that for maximum effect the Full Get Nearest Node Search setting should be toggled in A* Inspector Settings.
  		 */
  		[JsonMember]
  		public bool accurateNearestNode = true;
  		
  		public TriangleMeshNode[] nodes;
  		
  		public TriangleMeshNode[] TriNodes {
  			get { return nodes; }
  		}
  		
  		public override void GetNodes (GraphNodeDelegateCancelable del) {
  			if (nodes == null) return;
  			for (int i=0;i<nodes.Length && del (nodes[i]);i++) {}
  		}
  		
  		public override void OnDestroy () {
  			
  			base.OnDestroy ();
  			
  			// Cleanup
  			TriangleMeshNode.SetNavmeshHolder (active.astarData.GetGraphIndex(this), null);
  		}
  		
  		public Int3 GetVertex (int index) {
  			return vertices[index];
  		}
  		
  		public int GetVertexArrayIndex (int index) {
  			return index;
  		}
  		
  		public void GetTileCoordinates (int tileIndex, out int x, out int z) {
  			//Tiles not supported
  			x = z = 0;
  		}
  		
  		/** Bounding Box Tree. Enables really fast lookups of nodes. \astarpro */
  		BBTree _bbTree;
  		public BBTree bbTree {
  			get { return _bbTree; }
  			set { _bbTree = value;}
  		}
  		
  		[System.NonSerialized]
  		Int3[] _vertices;
  		
  		public Int3[] vertices {
  			get {
  				return _vertices;
  			}
  			set {
  				_vertices = value;
  			}
  		}
  		
  		[System.NonSerialized]
  		Vector3[] originalVertices;
  		
  		[System.NonSerialized]
  		public int[] triangles;
  		
  		public void GenerateMatrix () {
  #if !PhotonImplementation
  			SetMatrix (Matrix4x4.TRS (offset,Quaternion.Euler (rotation),new Vector3 (scale,scale,scale)));
  #else
  			SetMatrix (Matrix4x4.TRS (offset,rotation,new Vector3 (scale,scale,scale)));
  #endif
  			
  		}
  		
  		/** Transforms the nodes using newMatrix from their initial positions.
  		 * The "oldMatrix" variable can be left out in this function call (only for this graph generator)
  		 * since the information can be taken from other saved data, which gives better precision.
  		 */
  		public override void RelocateNodes (Matrix4x4 oldMatrix, Matrix4x4 newMatrix) {
  			
  			if (vertices == null || vertices.Length == 0 || originalVertices == null || originalVertices.Length != vertices.Length) {
  				return;
  			}
  			
  			for (int i=0;i<_vertices.Length;i++) {
  				_vertices[i] = (Int3)newMatrix.MultiplyPoint3x4 (originalVertices[i]);
  			}
  			
  			for (int i=0;i<nodes.Length;i++) {
  				var node = nodes[i];
  				node.UpdatePositionFromVertices();
  				
  				if (node.connections != null) {
  					for (int q=0;q<node.connections.Length;q++) {
  						node.connectionCosts[q] = (uint)(node.position-node.connections[q].position).costMagnitude;
  					}
  				}
  			}
  			
  			SetMatrix (newMatrix);
  
  			RebuildBBTree (this);
  		}
  	
  		public static NNInfo GetNearest (NavMeshGraph graph, GraphNode[] nodes, Vector3 position, NNConstraint constraint, bool accurateNearestNode) {
  			if (nodes == null || nodes.Length == 0) {
  				Debug.LogError ("NavGraph hasn't been generated yet or does not contain any nodes");
  				return new NNInfo ();
  			}
  			
  			if (constraint == null) constraint = NNConstraint.None;
  			
  			
  			Int3[] vertices = graph.vertices;
  			
  			//Query BBTree
  			
  			if (graph.bbTree == null) {
  				/** \todo Change method to require a navgraph */
  				return GetNearestForce (graph, graph, position, constraint, accurateNearestNode);
  			}
  			
  			//Searches in radiuses of 0.05 - 0.2 - 0.45 ... 1.28 times the average of the width and depth of the bbTree
  			float w = (graph.bbTree.Size.width + graph.bbTree.Size.height)*0.5F*0.02F;
  			
  			NNInfo query = graph.bbTree.QueryCircle (position,w,constraint);//graph.bbTree.Query (position,constraint);
  			
  			if (query.node == null) {
  				
  				for (int i=1;i<=8;i++) {
  					query = graph.bbTree.QueryCircle (position, i*i*w, constraint);
  					
  					if (query.node != null || (i-1)*(i-1)*w > AstarPath.active.maxNearestNodeDistance*2) { // *2 for a margin
  						break;
  					}
  				}
  			}
  			
  			if (query.node != null) {
  				query.clampedPosition = ClosestPointOnNode (query.node as TriangleMeshNode,vertices,position);
  			}
  			
  			if (query.constrainedNode != null) {
  				if (constraint.constrainDistance && ((Vector3)query.constrainedNode.position - position).sqrMagnitude > AstarPath.active.maxNearestNodeDistanceSqr) {
  					query.constrainedNode = null;
  				} else {
  					query.constClampedPosition = ClosestPointOnNode (query.constrainedNode as TriangleMeshNode, vertices, position);
  				}
  			}
  			
  			return query;	
  		}
  		
  		public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {
  			return GetNearest (this, nodes,position, constraint, accurateNearestNode);
  		}
  		
  		/** This performs a linear search through all polygons returning the closest one.
  		 * This is usually only called in the Free version of the A* Pathfinding Project since the Pro one supports BBTrees and will do another query
  		 */
  		public override NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) {
  			
  			return GetNearestForce (this, this,position,constraint, accurateNearestNode);
  			//Debug.LogWarning ("This function shouldn't be called since constrained nodes are sent back in the GetNearest call");
  			
  			//return new NNInfo ();
  		}
  		
  		/** This performs a linear search through all polygons returning the closest one */
  		public static NNInfo GetNearestForce (NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) {
  			NNInfo nn = GetNearestForceBoth (graph, navmesh,position,constraint,accurateNearestNode);
  			nn.node = nn.constrainedNode;
  			nn.clampedPosition = nn.constClampedPosition;
  			return nn;
  		}
  		
  		/** This performs a linear search through all polygons returning the closest one.
  		  * This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node
  		  * complying with the NNConstraint.
  		  * \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool)
  		  */
  		public static NNInfo GetNearestForceBoth (NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) {
  			var pos = (Int3)position;
  			
  			float minDist = -1;
  			GraphNode minNode = null;
  			
  			float minConstDist = -1;
  			GraphNode minConstNode = null;
  			
  			float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
  			
  			GraphNodeDelegateCancelable del = delegate (GraphNode _node) {
  				var node = _node as TriangleMeshNode;
  				
  				if (accurateNearestNode) {
  					
  					Vector3 closest = node.ClosestPointOnNode (position);
  					float dist = ((Vector3)pos-closest).sqrMagnitude;
  					
  					if (minNode == null || dist < minDist) {
  						minDist = dist;
  						minNode = node;
  					}
  					
  					if (dist < maxDistSqr && constraint.Suitable (node)) {
  						if (minConstNode == null || dist < minConstDist) {
  							minConstDist = dist;
  							minConstNode = node;
  						}
  					}
  					
  				} else {
  					
  					if (!node.ContainsPoint ((Int3)position)) {
  						
  						float dist = (node.position-pos).sqrMagnitude;
  						if (minNode == null || dist < minDist) {
  							minDist = dist;
  							minNode = node;
  						}
  						
  						if (dist < maxDistSqr && constraint.Suitable (node)) {
  							if (minConstNode == null || dist < minConstDist) {
  								minConstDist = dist;
  								minConstNode = node;
  							}
  						}
  						
  					} else {
  						int dist = AstarMath.Abs (node.position.y-pos.y);
  						
  						if (minNode == null || dist < minDist) {
  							minDist = dist;
  							minNode = node;
  						}
  						
  						if (dist < maxDistSqr && constraint.Suitable (node)) {
  							if (minConstNode == null || dist < minConstDist) {
  								minConstDist = dist;
  								minConstNode = node;
  							}
  						}
  					}
  				}
  				return true;
  			};
  			
  			graph.GetNodes (del);
  			
  			var nninfo = new NNInfo (minNode);
  			
  			//Find the point closest to the nearest triangle
  				
  			if (nninfo.node != null) {
  				var node = nninfo.node as TriangleMeshNode;//minNode2 as MeshNode;
  				
  				Vector3 clP = node.ClosestPointOnNode (position);
  				
  				nninfo.clampedPosition = clP;
  			}
  			
  			nninfo.constrainedNode = minConstNode;
  			if (nninfo.constrainedNode != null) {
  				var node = nninfo.constrainedNode as TriangleMeshNode;//minNode2 as MeshNode;
  				
  				Vector3 clP = node.ClosestPointOnNode (position);
  				
  				nninfo.constClampedPosition = clP;
  			}
  			
  			return nninfo;
  		}
  		
  		public void BuildFunnelCorridor (List<GraphNode> path, int startIndex, int endIndex, List<Vector3> left, List<Vector3> right) {
  			BuildFunnelCorridor (this,path,startIndex,endIndex,left,right);
  		}
  		
  		public static void BuildFunnelCorridor (INavmesh graph, List<GraphNode> path, int startIndex, int endIndex, List<Vector3> left, List<Vector3> right) {
  			
  			if (graph == null) {
  				Debug.LogError ("Couldn't cast graph to the appropriate type (graph isn't a Navmesh type graph, it doesn't implement the INavmesh interface)");
  				return;
  			}
  			
  			for (int i=startIndex;i<endIndex;i++) {
  				//Find the connection between the nodes
  				
  				var n1 = path[i] as TriangleMeshNode;
  
gt: 				var n2 = path[i+1] as TriangleMeshNode;