context: 
  using UnityEngine;
  using System.Collections;
  using Pathfinding;
  using System;
  using System.Collections.Generic;
  
  namespace Pathfinding {
  	
  	/** Matrix implementation.
  	 * \author Thanks to whoever wrote the Matrix class on the wiki, it has been a great help.
  	 * \ingroup utils
  	 */
  	public class Matrix
  	{
  		public float[] m;
  	
  		public Matrix()
  		{	 
  			LoadIdentity();
  		}	 
  	
  		// Loads this matrix with an identity matrix
  	
  		public void LoadIdentity()
  		{
  			m = new float[ 16 ];
  	
  			for( int x = 0 ; x < 16 ; ++x )
  			{
  				m[ x ] = 0;
  			}	 
  	
  			m [ 0 ] = 1;
  			m [ 5 ] = 1;
  			m [ 10 ] = 1;
  			m [ 15 ] = 1;
  		}
  	
  		// Returns a translation matrix along the XYZ axes
  	
  		public static Matrix Translate( float _X, float _Y, float _Z )
  		{
  			Matrix wk = new Matrix();
  	
  			wk.m [ 12 ] = _X;
  			wk.m [ 13 ] = _Y;
  			wk.m [ 14 ] = _Z;
  	
  			return wk;
  		}
  		
  		public Matrix translate( float _X, float _Y, float _Z )
  		{
  			m [ 12 ] = _X;
  			m [ 13 ] = _Y;
  			m [ 14 ] = _Z;
  			return this;
  		}
  	
  		// Returns a rotation matrix around the X axis
  	
  		public static Matrix RotateX( float _Degree )
  		{
  			Matrix wk = new Matrix();
  	
  			if( _Degree == 0 )
  			{
  				return wk;
  			}
  	
  			float C = Mathf.Cos( _Degree * Mathf.Deg2Rad );
  			float S = Mathf.Sin( _Degree * Mathf.Deg2Rad );
  	
  			wk.m [ 5 ] = C;
  			wk.m [ 6 ] = S;
  			wk.m [ 9 ] = -S;
  			wk.m [ 10 ] = C;
  	
  			return wk;
  		}
  	
  		// Returns a rotation matrix around the Y axis
  	
  		public static Matrix RotateY( float _Degree )
  		{
  			Matrix wk = new Matrix();
  	
  			if( _Degree == 0 )
  			{
  				return wk;
  			}
  	
  			float C = Mathf.Cos( _Degree * Mathf.Deg2Rad );
  			float S = Mathf.Sin( _Degree * Mathf.Deg2Rad );
  	
  			wk.m [ 0 ] = C;
  			wk.m [ 2 ] = -S;
  			wk.m [ 8 ] = S;
  			wk.m [ 10 ] = C;
  	
  			return wk;
  		}
  	
  		// Returns a rotation matrix around the Z axis
  	
  		public static Matrix RotateZ( float _Degree )
  		{
  			Matrix wk = new Matrix();
  	
  			if( _Degree == 0 )
  			{
  				return wk;
  			}
  	
  			float C = Mathf.Cos( _Degree * Mathf.Deg2Rad );
  			float S = Mathf.Sin( _Degree * Mathf.Deg2Rad );
  	
  			wk.m [ 0 ] = C;
  			wk.m [ 1 ] = S;
  			wk.m [ 4 ] = -S;
  			wk.m [ 5 ] = C;
  	
  			return wk;
  		}
  	
  		// Returns a scale matrix uniformly scaled on the XYZ axes
  	
  		public static Matrix Scale( float _In )
  		{
  			return Matrix.Scale( _In, _In, _In );
  		}
  	
  		// Returns a scale matrix scaled on the XYZ axes
  	
  		public static Matrix Scale( float _X, float _Y, float _Z )
  		{
  			Matrix wk = new Matrix();
  	
  			wk.m [ 0 ] = _X;
  			wk.m [ 5 ] = _Y;
  			wk.m [ 10 ] = _Z;
  	
  			return wk;
  		}
  	
  		// Transforms a vector with this matrix
  	
  		public Vector3 TransformVector( Vector3 _V )
  		{
  			Vector3 vtx = new Vector3(0,0,0);
  	
  			vtx.x = ( _V.x * m [ 0 ] ) + ( _V.y * m [ 4 ] ) + ( _V.z * m [ 8 ] ) + m [ 12 ];
  			vtx.y = ( _V.x * m [ 1 ] ) + ( _V.y * m [ 5 ] ) + ( _V.z * m [ 9 ] ) + m [ 13 ];
  			vtx.z = ( _V.x * m [ 2 ] ) + ( _V.y * m [ 6 ] ) + ( _V.z * m [ 10 ] ) + m [ 14 ];
  	
  			return vtx;
  		}
  	
  		// Overloaded operators
  	
  		public static Matrix operator *( Matrix _A, Matrix _B )
  		{
  			Matrix wk = new Matrix();
  	
  			wk.m [ 0 ] = _A.m [ 0 ] * _B.m [ 0 ] + _A.m [ 4 ] * _B.m [ 1 ] + _A.m [ 8 ] * _B.m [ 2 ] + _A.m [ 12 ] * _B.m [ 3 ];
  			wk.m [ 4 ] = _A.m [ 0 ] * _B.m [ 4 ] + _A.m [ 4 ] * _B.m [ 5 ] + _A.m [ 8 ] * _B.m [ 6 ] + _A.m [ 12 ] * _B.m [ 7 ];
  			wk.m [ 8 ] = _A.m [ 0 ] * _B.m [ 8 ] + _A.m [ 4 ] * _B.m [ 9 ] + _A.m [ 8 ] * _B.m [ 10 ] + _A.m [ 12 ] * _B.m [ 11 ];
  			wk.m [ 12 ] = _A.m [ 0 ] * _B.m [ 12 ] + _A.m [ 4 ] * _B.m [ 13 ] + _A.m [ 8 ] * _B.m [ 14 ] + _A.m [ 12 ] * _B.m [ 15 ];
  	
  			wk.m [ 1 ] = _A.m [ 1 ] * _B.m [ 0 ] + _A.m [ 5 ] * _B.m [ 1 ] + _A.m [ 9 ] * _B.m [ 2 ] + _A.m [ 13 ] * _B.m [ 3 ];
  			wk.m [ 5 ] = _A.m [ 1 ] * _B.m [ 4 ] + _A.m [ 5 ] * _B.m [ 5 ] + _A.m [ 9 ] * _B.m [ 6 ] + _A.m [ 13 ] * _B.m [ 7 ];
  			wk.m [ 9 ] = _A.m [ 1 ] * _B.m [ 8 ] + _A.m [ 5 ] * _B.m [ 9 ] + _A.m [ 9 ] * _B.m [ 10 ] + _A.m [ 13 ] * _B.m [ 11 ];
  			wk.m [ 13 ] = _A.m [ 1 ] * _B.m [ 12 ] + _A.m [ 5 ] * _B.m [ 13 ] + _A.m [ 9 ] * _B.m [ 14 ] + _A.m [ 13 ] * _B.m [ 15 ];
  	
  			wk.m [ 2 ] = _A.m [ 2 ] * _B.m [ 0 ] + _A.m [ 6 ] * _B.m [ 1 ] + _A.m [ 10 ] * _B.m [ 2 ] + _A.m [ 14 ] * _B.m [ 3 ];
  			wk.m [ 6 ] = _A.m [ 2 ] * _B.m [ 4 ] + _A.m [ 6 ] * _B.m [ 5 ] + _A.m [ 10 ] * _B.m [ 6 ] + _A.m [ 14 ] * _B.m [ 7 ];
  			wk.m [ 10 ] = _A.m [ 2 ] * _B.m [ 8 ] + _A.m [ 6 ] * _B.m [ 9 ] + _A.m [ 10 ] * _B.m [ 10 ] + _A.m [ 14 ] * _B.m [ 11 ];
  			wk.m [ 14 ] = _A.m [ 2 ] * _B.m [ 12 ] + _A.m [ 6 ] * _B.m [ 13 ] + _A.m [ 10 ] * _B.m [ 14 ] + _A.m [ 14 ] * _B.m [ 15 ];
  	
  			wk.m [ 3 ] = _A.m [ 3 ] * _B.m [ 0 ] + _A.m [ 7 ] * _B.m [ 1 ] + _A.m [ 11 ] * _B.m [ 2 ] + _A.m [ 15 ] * _B.m [ 3 ];
  			wk.m [ 7 ] = _A.m [ 3 ] * _B.m [ 4 ] + _A.m [ 7 ] * _B.m [ 5 ] + _A.m [ 11 ] * _B.m [ 6 ] + _A.m [ 15 ] * _B.m [ 7 ];
  			wk.m [ 11 ] = _A.m [ 3 ] * _B.m [ 8 ] + _A.m [ 7 ] * _B.m [ 9 ] + _A.m [ 11 ] * _B.m [ 10 ] + _A.m [ 15 ] * _B.m [ 11 ];
  			wk.m [ 15 ] = _A.m [ 3 ] * _B.m [ 12 ] + _A.m [ 7 ] * _B.m [ 13 ] + _A.m [ 11 ] * _B.m [ 14 ] + _A.m [ 15 ] * _B.m [ 15 ];
  	
  			return wk;
  		}
  	}
  	
  	/*class Arrays {
  		/*public static bool Contains (Node[] arr,Node target) {
  			for (int i=0;i<arr.Length;i++) {
  				if (arr[i] == target) {
  					return true;
  				}
  			}
  			return false;
  		}*
  	}*/
  	
  	/** Contains various spline functions.
  	 * \ingroup utils
  	 */
  	class AstarSplines {
  		public static Vector3 CatmullRom(Vector3 previous,Vector3 start, Vector3 end, Vector3 next, float elapsedTime) {
  			// References used:
  			// p.266 GemsV1
  			//
  			// tension is often set to 0.5 but you can use any reasonable value:
  			// http://www.cs.cmu.edu/~462/projects/assn2/assn2/catmullRom.pdf
  			//
  			// bias and tension controls:
  			// http://local.wasp.uwa.edu.au/~pbourke/miscellaneous/interpolation/
  		
  			float percentComplete = elapsedTime;
  			float percentCompleteSquared = percentComplete * percentComplete;
  			float percentCompleteCubed = percentCompleteSquared * percentComplete;
  			
  			/*return previous * (-0.5F*percentCompleteCubed +
  							 percentCompleteSquared -
  							 tension*percentComplete) +
  							 
  			start * ((2-tension) *percentCompleteCubed +
  					 (tension - 3)*percentCompleteSquared + 1.0F) +
  					 
  			end * ((tension - 2)*percentCompleteCubed +
  					 2.0F *percentCompleteSquared +
  					 0.5F*percentComplete) +
  					 
  			next * (0.5F*percentCompleteCubed -
  					tension*percentCompleteSquared);*/
  					
  			return 
  			previous * (-0.5F*percentCompleteCubed +
  							 percentCompleteSquared -
  							 0.5F*percentComplete) +
  							 
  			start * 
  				(1.5F*percentCompleteCubed +
  				-2.5F*percentCompleteSquared + 1.0F) +
  				
  			end * 
  				(-1.5F*percentCompleteCubed +
  				2.0F*percentCompleteSquared +
  				0.5F*percentComplete) +
  				
  			next * 
  				(0.5F*percentCompleteCubed -
  				0.5F*percentCompleteSquared);
  		}
  		
  		public static Vector3 CatmullRomOLD (Vector3 previous,Vector3 start, Vector3 end, Vector3 next, float elapsedTime) {
  			// References used:
  			// p.266 GemsV1
  			//
  			// tension is often set to 0.5 but you can use any reasonable value:
  			// http://www.cs.cmu.edu/~462/projects/assn2/assn2/catmullRom.pdf
  			//
  			// bias and tension controls:
  			// http://local.wasp.uwa.edu.au/~pbourke/miscellaneous/interpolation/
  		
  			float percentComplete = elapsedTime;
  			float percentCompleteSquared = percentComplete * percentComplete;
  			float percentCompleteCubed = percentCompleteSquared * percentComplete;
  		
  			return previous * (-0.5F*percentCompleteCubed +
  							 percentCompleteSquared -
  							 0.5F*percentComplete) +
  			start * (1.5F*percentCompleteCubed +
  					 -2.5F*percentCompleteSquared + 1.0F) +
  			end * (-1.5F*percentCompleteCubed +
  					 2.0F*percentCompleteSquared +
  					 0.5F*percentComplete) +
  			next * (0.5F*percentCompleteCubed -
  					0.5F*percentCompleteSquared);
  		}
  	}
  	
  	/** Utility functions for working with numbers, lines and vectors.
  	 * \ingroup utils
  	  * \see Polygon */
  	public class Mathfx {
  		
  		/** Returns a hash value for a integer vector. Code got from the internet */
  		public static int ComputeVertexHash (int x, int y, int z) {
  			uint h1 = 0x8da6b343; // Large multiplicative constants;
  			uint h2 = 0xd8163841; // here arbitrarily chosen primes
  			uint h3 = 0xcb1ab31f;
  			uint n = (uint)(h1 * x + h2 * y + h3 * z);
  			
  			return (int)(n & ((1<<30)-1));
  		}
  
  		/** Returns the closest point on the line. The line is treated as infinite.
  		 * \see NearestPointStrict
  		 */
  		public static Vector3 NearestPoint(Vector3 lineStart, Vector3 lineEnd, Vector3 point)
  	    {
  	        Vector3 lineDirection = Vector3.Normalize(lineEnd-lineStart);
  	        
  	        float closestPoint = Vector3.Dot((point-lineStart),lineDirection); //Vector3.Dot(lineDirection,lineDirection);
  	        return lineStart+(closestPoint*lineDirection);
  	    }
  	 
  	 	public static float NearestPointFactor (Vector3 lineStart, Vector3 lineEnd, Vector3 point)
  	    {
  	    	Vector3 lineDirection = lineEnd-lineStart;
  	    	float magn = lineDirection.magnitude;
  			lineDirection /= magn;
  	        
  	        float closestPoint = Vector3.Dot((point-lineStart),lineDirection); //Vector3.Dot(lineDirection,lineDirection);
  	        return closestPoint / magn;
  	    }
  	    
  		/** Returns the closest point on the line segment. The line is NOT treated as infinite.
  		 * \see NearestPoint
  		 */
  	    public static Vector3 NearestPointStrict(Vector3 lineStart, Vector3 lineEnd, Vector3 point)
  	    {
  	        Vector3 fullDirection = lineEnd-lineStart;
  	        Vector3 lineDirection = Vector3.Normalize(fullDirection);
  	        
  	        float closestPoint = Vector3.Dot((point-lineStart),lineDirection); //WASTE OF CPU POWER - This is always ONE -- Vector3.Dot(lineDirection,lineDirection);
  	        return lineStart+(Mathf.Clamp(closestPoint,0.0f,fullDirection.magnitude)*lineDirection);
  	    }
  	    
  		/** Returns the closest point on the line segment on the XZ plane. The line is NOT treated as infinite.
  		 * \see NearestPoint
  		 */
  	    public static Vector3 NearestPointStrictXZ (Vector3 lineStart, Vector3 lineEnd, Vector3 point)
  	    {
  			lineStart.y = point.y;
  			lineEnd.y = point.y;
  	        Vector3 fullDirection = lineEnd-lineStart;
  			Vector3 fullDirection2 = fullDirection;
  			fullDirection2.y = 0;
  	        Vector3 lineDirection = Vector3.Normalize(fullDirection2);
  	        //lineDirection.y = 0;
  			
  	        float closestPoint = Vector3.Dot((point-lineStart),lineDirection); //WASTE OF CPU POWER - This is always ONE -- Vector3.Dot(lineDirection,lineDirection);
  	        return lineStart+(Mathf.Clamp(closestPoint,0.0f,fullDirection2.magnitude)*lineDirection);
  	    }
  		
  	    /** Returns the approximate shortest distance between x,z and the line p-q. The line is considered infinite. This function is not entirely exact, but it is about twice as fast as DistancePointSegment2. */
  	    public static float DistancePointSegment (int x,int z, int px, int pz, int qx, int qz) {
  	    	
  	    	float pqx = (float)(qx - px);
  			float pqz = (float)(qz - pz);
  			float dx = (float)(x - px);
  			float dz = (float)(z - pz);
  			float d = pqx*pqx + pqz*pqz;
  			float t = pqx*dx + pqz*dz;
  			if (d > 0)
  				t /= d;
  			if (t < 0)
  				t = 0;
  			else if (t > 1)
  				t = 1;
  			
  			dx = px + t*pqx - x;
  			dz = pz + t*pqz - z;
  			
  			return dx*dx + dz*dz;
  	    }
  	    
  	    /*public static float DistancePointSegment2 (int x,int z, int px, int pz, int qx, int qz) {
  	    	
  	    	Vector3 p = new Vector3 (x,0,z);
  	    	
  	    	Vector3 p1 = new Vector3 (px,0,pz);
  	    	Vector3 p2 = new Vector3 (qx,0,qz);
  	    	
  	    	Vector3 nearest = NearestPoint (p1,p2,p);
  			
  			return (nearest-p).sqrMagnitude;
  	    }*/
  	    
  	    /** Returns the distance between x,z and the line p-q. The line is considered infinite. */
  	    public static float DistancePointSegment2 (int x,int z, int px, int pz, int qx, int qz) {
  	    	
  	    	Vector3 p = new Vector3 (x,0,z);
  	    	
  	    	Vector3 p1 = new Vector3 (px,0,pz);
  	    	Vector3 p2 = new Vector3 (qx,0,qz);
  	    	
  	    	return DistancePointSegment2 (p1,p2,p);
  	    }
  	   
  	    /** Returns the distance between c and the line a-b. The line is considered infinite. */
  	    public static float DistancePointSegment2 (Vector3 a, Vector3 b, Vector3 p) {
  	    	
  	    	float bax = b.x - a.x;
  	    	float baz = b.z - a.z;
  	    	
  	    	float area = Mathf.Abs (bax * (p.z - a.z) - (p.x - a.x) * baz);
  	    	
  	    	float d = bax*bax+baz*baz;
  	    	
  	    	if (d > 0) {
  	    		return area / Mathf.Sqrt (d);
  	    	}
  	    	
  	    	return (a-p).magnitude;
  	    }
  	    
  	    /** Returns the squared distance between c and the line a-b. The line is not considered infinite. */
  	    public static float DistancePointSegmentStrict (Vector3 a, Vector3 b, Vector3 p) {
  	    	
  	    	Vector3 nearest = NearestPointStrict (a,b,p);
  			return (nearest-p).sqrMagnitude;
  	    }
  	    
  		/** Returns a point on a hermite curve. Slow start and slow end, fast in the middle */
  		public static float Hermite(float start, float end, float value) {
  			
  			return Mathf.Lerp(start, end, value * value * (3.0f - 2.0f * value));
  		}
  		
  		/** Returns a point on a cubic bezier curve. \a t is clamped between 0 and 1 */
  		public static Vector3 CubicBezier (Vector3 p0,Vector3 p1,Vector3 p2,Vector3 p3, float t) {
  			t = Mathf.Clamp01 (t);
  			float t2 = 1-t;
  			return Mathf.Pow(t2,3) * p0 + 3 * Mathf.Pow(t2,2) * t * p1 + 3 * t2 * Mathf.Pow(t,2) * p2 + Mathf.Pow(t,3) * p3;
  		}
  		
  		/** Maps a value between startMin and startMax to be between 0 and 1 */
  		public static float MapTo (float startMin,float startMax, float value) {
  			value -= startMin;
  			value /= (startMax-startMin);
  			value = Mathf.Clamp01 (value);
  			return value;
  		}
  		
  		/** Maps a value (0...1) to be between targetMin and targetMax */
  		public static float MapToRange (float targetMin,float targetMax, float value) {
  			value *= (targetMax-targetMin);
  			value += targetMin;
  			return value;
  		}
  		
  		/** Maps a value between startMin and startMax to be between targetMin and targetMax */
  		public static float MapTo (float startMin,float startMax, float targetMin, float targetMax, float value) {
  			value -= startMin;
  			value /= (startMax-startMin);
  			value = Mathf.Clamp01 (value);
  			value *= (targetMax-targetMin);
  			value += targetMin;
  			return value;
  		}
  		
  		/** Returns a nicely formatted string for the number of bytes (kB, MB, GB etc). Uses decimal values, not binary ones
  		  * \see FormatBytesBinary */
  		public static string FormatBytes (int bytes) {
  			double sign = bytes >= 0 ? 1D : -1D;
  			bytes = bytes >= 0 ? bytes : -bytes;
  			
  			if (bytes < 1000) {
  				return (bytes*sign).ToString ()+" bytes";
  			} else if (bytes < 1000000) {
  				return ((bytes/1000D)*sign).ToString ("0.0") + " kb";
  			} else if (bytes < 1000000000) {
  				return ((bytes/1000000D)*sign).ToString ("0.0") +" mb";
  			} else {
  				return ((bytes/1000000000D)*sign).ToString ("0.0") +" gb";
  			}
  		}
  		
  		/** Returns a nicely formatted string for the number of bytes (KiB, MiB, GiB etc). Uses decimal names (KB, Mb - 1000) but calculates using binary values (KiB, MiB - 1024) */
  		public static string FormatBytesBinary (int bytes) {
  			double sign = bytes >= 0 ? 1D : -1D;
  			bytes = bytes >= 0 ? bytes : -bytes;
  			
  			if (bytes < 1024) {
  				return (bytes*sign).ToString ()+" bytes";
  			} else if (bytes < 1024) {
  				return ((bytes/1024D)*sign).ToString ("0.0") + " kb";
  			} else if (bytes < 1000000000) {
  				return ((bytes/(1024D*1024D))*sign).ToString ("0.0") +" mb";
  			} else {
  				return ((bytes/(1024D*1024D*1024D))*sign).ToString ("0.0") +" gb";
  			}
  		}
  		
  		/** Returns bit number \a b from int \a a. The bit number is zero based. Relevant \a b values are from 0 to 31\n
  		  * Equals to (a >> b) & 1 */
  		public static int Bit (int a, int b) {
  			return (a >> b) & 1;
  			//return (a & (1 << b)) >> b; //Original code, one extra shift operation required
  		}
  		
  		/** Returns a nice color from int \a i with alpha \a a. Got code from the open-source Recast project, works really good\n
  		  * Seems like there are only 64 possible colors from studying the code*/
  		public static Color IntToColor (int i, float a) {
  			int	r = Bit(i, 1) + Bit(i, 3) * 2 + 1;
  			int	g = Bit(i, 2) + Bit(i, 4) * 2 + 1;
  			int	b = Bit(i, 0) + Bit(i, 5) * 2 + 1;
  			return new Color (r*0.25F,g*0.25F,b*0.25F,a);
  		}
  	
  		/** Distance between two points on the XZ plane */
  		public static float MagnitudeXZ (Vector3 a, Vector3 b) {
  			Vector3 delta = a-b;
  			return (float)Math.Sqrt (delta.x*delta.x+delta.z*delta.z);
  		}
  		
  		/** Squared distance between two points on the XZ plane */
  		public static float SqrMagnitudeXZ (Vector3 a, Vector3 b) {
  			Vector3 delta = a-b;
  			return delta.x*delta.x+delta.z*delta.z;
  		}
  		
  		public static int Repeat (int i, int n) {
  			while (i >= n) {
  				i -= n;
  			}
  			return i;
  		}
  		
  		public static float Abs (float a) {
  			if (a < 0) {
  				return -a;
  			}
  			return a;
  		}
  	
  		public static int Abs (int a) {
  			if (a < 0) {
  				return -a;
  			}
  			return a;
  		}
  		
  		public static float Min (float a, float b) {
  			return a < b ? a : b;
  		}
  		
  		public static int Min (int a, int b) {
  
gt: 			return a < b ? a : b;