+ astar project

1 files changed, 1954 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/AstarMath.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/AstarMath.cs
new file mode 100644
index 0000000..696037f
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/AstarMath.cs
@@ -0,0 +1,1954 @@
+using UnityEngine;
+using System.Collections.Generic;
+using System;
+
+namespace Pathfinding {
+	using Pathfinding.Util;
+	using Unity.Mathematics;
+	using Unity.Burst;
+	using Pathfinding.Graphs.Navmesh;
+
+	/// <summary>Contains various spline functions.</summary>
+	public static 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 -
+							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);
+		}
+
+		/// <summary>Returns a point on a cubic bezier curve. t is clamped between 0 and 1</summary>
+		public static Vector3 CubicBezier (Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t) {
+			t = Mathf.Clamp01(t);
+			float t2 = 1-t;
+			return t2*t2*t2 * p0 + 3 * t2*t2 * t * p1 + 3 * t2 * t*t * p2 + t*t*t * p3;
+		}
+
+		/// <summary>Returns the derivative for a point on a cubic bezier curve. t is clamped between 0 and 1</summary>
+		public static Vector3 CubicBezierDerivative (Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t) {
+			t = Mathf.Clamp01(t);
+			float t2 = 1-t;
+			return 3*t2*t2*(p1-p0) + 6*t2*t*(p2 - p1) + 3*t*t*(p3 - p2);
+		}
+
+		/// <summary>Returns the second derivative for a point on a cubic bezier curve. t is clamped between 0 and 1</summary>
+		public static Vector3 CubicBezierSecondDerivative (Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t) {
+			t = Mathf.Clamp01(t);
+			float t2 = 1-t;
+			return 6*t2*(p2 - 2*p1 + p0) + 6*t*(p3 - 2*p2 + p1);
+		}
+	}
+
+	/// <summary>
+	/// Various vector math utility functions.
+	/// Version: A lot of functions in the Polygon class have been moved to this class
+	/// the names have changed slightly and everything now consistently assumes a left handed
+	/// coordinate system now instead of sometimes using a left handed one and sometimes
+	/// using a right handed one. This is why the 'Left' methods in the Polygon class redirect
+	/// to methods named 'Right'. The functionality is exactly the same.
+	///
+	/// Note the difference between segments and lines. Lines are infinitely
+	/// long but segments have only a finite length.
+	/// </summary>
+	public static class VectorMath {
+		/// <summary>
+		/// Complex number multiplication.
+		/// Returns: a * b
+		///
+		/// Used to rotate vectors in an efficient way.
+		///
+		/// See: https://en.wikipedia.org/wiki/Complex_number<see cref="Multiplication_and_division"/>
+		/// </summary>
+		public static Vector2 ComplexMultiply (Vector2 a, Vector2 b) {
+			return new Vector2(a.x * b.x - a.y * b.y, a.x * b.y + a.y * b.x);
+		}
+
+		/// <summary>
+		/// Complex number multiplication.
+		/// Returns: a * b
+		///
+		/// Used to rotate vectors in an efficient way.
+		///
+		/// See: https://en.wikipedia.org/wiki/Complex_number<see cref="Multiplication_and_division"/>
+		/// </summary>
+		public static float2 ComplexMultiply (float2 a, float2 b) {
+			return a.x*b + a.y*new float2(-b.y, b.x);
+		}
+
+		/// <summary>
+		/// Complex number multiplication.
+		/// Returns: a * conjugate(b)
+		///
+		/// Used to rotate vectors in an efficient way.
+		///
+		/// See: https://en.wikipedia.org/wiki/Complex_number<see cref="Multiplication_and_division"/>
+		/// See: https://en.wikipedia.org/wiki/Complex_conjugate
+		/// </summary>
+		public static float2 ComplexMultiplyConjugate (float2 a, float2 b) {
+			return new float2(a.x * b.x + a.y * b.y, a.y * b.x - a.x * b.y);
+		}
+
+		/// <summary>
+		/// Complex number multiplication.
+		/// Returns: a * conjugate(b)
+		///
+		/// Used to rotate vectors in an efficient way.
+		///
+		/// See: https://en.wikipedia.org/wiki/Complex_number<see cref="Multiplication_and_division"/>
+		/// See: https://en.wikipedia.org/wiki/Complex_conjugate
+		/// </summary>
+		public static Vector2 ComplexMultiplyConjugate (Vector2 a, Vector2 b) {
+			return new Vector2(a.x * b.x + a.y * b.y, a.y * b.x - a.x * b.y);
+		}
+
+		/// <summary>
+		/// Returns the closest point on the line.
+		/// The line is treated as infinite.
+		/// See: ClosestPointOnSegment
+		/// See: ClosestPointOnLineFactor
+		/// </summary>
+		public static Vector3 ClosestPointOnLine (Vector3 lineStart, Vector3 lineEnd, Vector3 point) {
+			Vector3 lineDirection = Vector3.Normalize(lineEnd - lineStart);
+			float dot = Vector3.Dot(point - lineStart, lineDirection);
+
+			return lineStart + (dot*lineDirection);
+		}
+
+		/// <summary>
+		/// Factor along the line which is closest to the point.
+		/// Returned value is in the range [0,1] if the point lies on the segment otherwise it just lies on the line.
+		/// The closest point can be calculated using (end-start)*factor + start.
+		///
+		/// See: ClosestPointOnLine
+		/// See: ClosestPointOnSegment
+		/// </summary>
+		public static float ClosestPointOnLineFactor (Vector3 lineStart, Vector3 lineEnd, Vector3 point) {
+			var dir = lineEnd - lineStart;
+			float sqrMagn = dir.sqrMagnitude;
+
+			if (sqrMagn <= 0.000001f) return 0;
+
+			return Vector3.Dot(point - lineStart, dir) / sqrMagn;
+		}
+
+		/// <summary>
+		/// Factor along the line which is closest to the point.
+		/// Returned value is in the range [0,1] if the point lies on the segment otherwise it just lies on the line.
+		/// The closest point can be calculated using (end-start)*factor + start
+		/// </summary>
+		public static float ClosestPointOnLineFactor (float3 lineStart, float3 lineEnd, float3 point) {
+			var lineDirection = lineEnd - lineStart;
+			var sqrMagn = math.dot(lineDirection, lineDirection);
+			return math.select(0, math.dot(point - lineStart, lineDirection) / sqrMagn, sqrMagn > 0.000001f);
+		}
+
+		/// <summary>
+		/// Factor along the line which is closest to the point.
+		/// Returned value is in the range [0,1] if the point lies on the segment otherwise it just lies on the line.
+		/// The closest point can be calculated using (end-start)*factor + start
+		/// </summary>
+		public static float ClosestPointOnLineFactor (Int3 lineStart, Int3 lineEnd, Int3 point) {
+			var lineDirection = lineEnd - lineStart;
+			float magn = lineDirection.sqrMagnitude;
+
+			float closestPoint = (float)Int3.DotLong(point - lineStart, lineDirection);
+
+			if (magn != 0) closestPoint /= magn;
+
+			return closestPoint;
+		}
+
+		/// <summary>
+		/// Factor of the nearest point on the segment.
+		/// Returned value is in the range [0,1] if the point lies on the segment otherwise it just lies on the line.
+		/// The closest point can be calculated using (end-start)*factor + start;
+		/// </summary>
+		public static float ClosestPointOnLineFactor (Int2 lineStart, Int2 lineEnd, Int2 point) {
+			var lineDirection = lineEnd - lineStart;
+			double magn = lineDirection.sqrMagnitudeLong;
+
+			double closestPoint = Int2.DotLong(point - lineStart, lineDirection);
+
+			if (magn != 0) closestPoint /= magn;
+
+			return (float)closestPoint;
+		}
+
+		/// <summary>
+		/// Returns the closest point on the segment.
+		/// The segment is NOT treated as infinite.
+		/// See: ClosestPointOnLine
+		/// See: ClosestPointOnSegmentXZ
+		/// </summary>
+		public static Vector3 ClosestPointOnSegment (Vector3 lineStart, Vector3 lineEnd, Vector3 point) {
+			var dir = lineEnd - lineStart;
+			float sqrMagn = dir.sqrMagnitude;
+
+			if (sqrMagn <= 0.000001) return lineStart;
+
+			float factor = Vector3.Dot(point - lineStart, dir) / sqrMagn;
+			return lineStart + Mathf.Clamp01(factor)*dir;
+		}
+
+		/// <summary>
+		/// Returns the closest point on the segment in the XZ plane.
+		/// The y coordinate of the result will be the same as the y coordinate of the point parameter.
+		///
+		/// The segment is NOT treated as infinite.
+		/// See: ClosestPointOnSegment
+		/// See: ClosestPointOnLine
+		/// </summary>
+		public static Vector3 ClosestPointOnSegmentXZ (Vector3 lineStart, Vector3 lineEnd, Vector3 point) {
+			lineStart.y = point.y;
+			lineEnd.y = point.y;
+			Vector3 fullDirection = lineEnd-lineStart;
+			Vector3 fullDirection2 = fullDirection;
+			fullDirection2.y = 0;
+			float magn = fullDirection2.magnitude;
+			Vector3 lineDirection = magn > float.Epsilon ? fullDirection2/magn : Vector3.zero;
+
+			float closestPoint = Vector3.Dot((point-lineStart), lineDirection);
+			return lineStart+(Mathf.Clamp(closestPoint, 0.0f, fullDirection2.magnitude)*lineDirection);
+		}
+
+		/// <summary>
+		/// Returns the approximate shortest squared distance between x,z and the segment p-q.
+		/// The segment is not considered infinite.
+		/// This function is not entirely exact, but it is about twice as fast as DistancePointSegment2.
+		/// TODO: Is this actually approximate? It looks exact.
+		/// </summary>
+		public static float SqrDistancePointSegmentApproximate (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;
+		}
+
+		/// <summary>
+		/// Returns the approximate shortest squared distance between x,z and the segment p-q.
+		/// The segment is not considered infinite.
+		/// This function is not entirely exact, but it is about twice as fast as DistancePointSegment2.
+		/// TODO: Is this actually approximate? It looks exact.
+		/// </summary>
+		public static float SqrDistancePointSegmentApproximate (Int3 a, Int3 b, Int3 p) {
+			float pqx = (float)(b.x - a.x);
+			float pqz = (float)(b.z - a.z);
+			float dx = (float)(p.x - a.x);
+			float dz = (float)(p.z - a.z);
+			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 = a.x + t*pqx - p.x;
+			dz = a.z + t*pqz - p.z;
+
+			return dx*dx + dz*dz;
+		}
+
+		/// <summary>
+		/// Returns the squared distance between p and the segment a-b.
+		/// The line is not considered infinite.
+		/// </summary>
+		public static float SqrDistancePointSegment (Vector3 a, Vector3 b, Vector3 p) {
+			var nearest = ClosestPointOnSegment(a, b, p);
+
+			return (nearest-p).sqrMagnitude;
+		}
+
+		/// <summary>
+		/// 3D minimum distance between 2 segments.
+		/// Input: two 3D line segments S1 and S2
+		/// Returns: the shortest squared distance between S1 and S2
+		/// </summary>
+		public static float SqrDistanceSegmentSegment (Vector3 s1, Vector3 e1, Vector3 s2, Vector3 e2) {
+			Vector3 dir1 = e1 - s1;
+			Vector3 dir2 = e2 - s2;
+			Vector3 startOffset = s1 - s2;
+			double dir1sq = Vector3.Dot(dir1, dir1);           // always >= 0
+			double b = Vector3.Dot(dir1, dir2);
+			double dir2sq = Vector3.Dot(dir2, dir2);           // always >= 0
+			double d = Vector3.Dot(dir1, startOffset);
+			double e = Vector3.Dot(dir2, startOffset);
+			double D = dir1sq*dir2sq - b*b;           // always >= 0
+			double sc, sN, sD = D;          // sc = sN / sD, default sD = D >= 0
+			double tc, tN, tD = D;          // tc = tN / tD, default tD = D >= 0
+
+			// compute the line parameters of the two closest points
+			// D is approximately |dir1|^2|dir2|^2*(1-cos^2 alpha), where alpha is the angle between the lines
+			if (D < 0.000001 * dir1sq*dir2sq) { // the lines are almost parallel
+				sN = 0.0f;         // force using point P0 on segment S1
+				sD = 1.0f;         // to prevent possible division by 0.0 later
+				tN = e;
+				tD = dir2sq;
+			} else {               // get the closest points on the infinite lines
+				sN = (b*e - dir2sq*d);
+				tN = (dir1sq*e - b*d);
+				if (sN < 0.0) {        // sc < 0 => the s=0 edge is visible
+					sN = 0.0;
+					tN = e;
+					tD = dir2sq;
+				} else if (sN > sD) { // sc > 1  => the s=1 edge is visible
+					sN = sD;
+					tN = e + b;
+					tD = dir2sq;
+				}
+			}
+
+			if (tN < 0.0) {            // tc < 0 => the t=0 edge is visible
+				tN = 0.0;
+				// recompute sc for this edge
+				if (-d < 0.0f)
+					sN = 0.0f;
+				else if (-d > dir1sq)
+					sN = sD;
+				else {
+					sN = -d;
+					sD = dir1sq;
+				}
+			} else if (tN > tD) {    // tc > 1  => the t=1 edge is visible
+				tN = tD;
+				// recompute sc for this edge
+				if ((-d + b) < 0.0f)
+					sN = 0;
+				else if ((-d + b) > dir1sq)
+					sN = sD;
+				else {
+					sN = (-d +  b);
+					sD = dir1sq;
+				}
+			}
+
+			// finally do the division to get sc and tc
+			sc = (Math.Abs(sN) < 0.00001f ? 0.0 : sN / sD);
+			tc = (Math.Abs(tN) < 0.00001f ? 0.0 : tN / tD);
+
+			// get the difference of the two closest points
+			Vector3 dP = startOffset + ((float)sc * dir1) - ((float)tc * dir2);  // =  S1(sc) - S2(tc)
+
+			return dP.sqrMagnitude;   // return the closest distance
+		}
+
+		/// <summary>
+		/// Determinant of the 2x2 matrix [c1, c2].
+		///
+		/// This is useful for many things, like calculating distances between lines and points.
+		///
+		/// Equivalent to Cross(new float3(c1, 0), new float 3(c2, 0)).z
+		/// </summary>
+		public static float Determinant (float2 c1, float2 c2) {
+			return c1.x*c2.y - c1.y*c2.x;
+		}
+
+		/// <summary>Squared distance between two points in the XZ plane</summary>
+		public static float SqrDistanceXZ (Vector3 a, Vector3 b) {
+			var delta = a-b;
+
+			return delta.x*delta.x+delta.z*delta.z;
+		}
+
+		/// <summary>
+		/// Signed area of a triangle in the XZ plane multiplied by 2.
+		/// This will be negative for clockwise triangles and positive for counter-clockwise ones
+		/// </summary>
+		public static long SignedTriangleAreaTimes2XZ (Int3 a, Int3 b, Int3 c) {
+			return (long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z);
+		}
+
+		/// <summary>
+		/// Signed area of a triangle in the XZ plane multiplied by 2.
+		/// This will be negative for clockwise triangles and positive for counter-clockwise ones.
+		/// </summary>
+		public static float SignedTriangleAreaTimes2XZ (Vector3 a, Vector3 b, Vector3 c) {
+			return (b.x - a.x) * (c.z - a.z) - (c.x - a.x) * (b.z - a.z);
+		}
+
+		/// <summary>
+		/// Returns if p lies on the right side of the line a - b.
+		/// Uses XZ space. Does not return true if the points are colinear.
+		/// </summary>
+		public static bool RightXZ (Vector3 a, Vector3 b, Vector3 p) {
+			return (b.x - a.x) * (p.z - a.z) - (p.x - a.x) * (b.z - a.z) < -float.Epsilon;
+		}
+
+		/// <summary>
+		/// Returns if p lies on the right side of the line a - b.
+		/// Uses XZ space. Does not return true if the points are colinear.
+		/// </summary>
+		public static bool RightXZ (Int3 a, Int3 b, Int3 p) {
+			return (long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) < 0;
+		}
+
+		/// <summary>
+		/// Returns which side of the line a - b that p lies on.
+		/// Uses XZ space.
+		/// </summary>
+		public static Side SideXZ (Int3 a, Int3 b, Int3 p) {
+			var s = (long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z);
+
+			return s > 0 ? Side.Left : (s < 0 ? Side.Right : Side.Colinear);
+		}
+
+		/// <summary>
+		/// Returns if p lies on the right side of the line a - b.
+		/// Also returns true if the points are colinear.
+		/// </summary>
+		public static bool RightOrColinear (Vector2 a, Vector2 b, Vector2 p) {
+			return (b.x - a.x) * (p.y - a.y) - (p.x - a.x) * (b.y - a.y) <= 0;
+		}
+
+		/// <summary>
+		/// Returns if p lies on the right side of the line a - b.
+		/// Also returns true if the points are colinear.
+		/// </summary>
+		public static bool RightOrColinear (Int2 a, Int2 b, Int2 p) {
+			return (long)(b.x - a.x) * (long)(p.y - a.y) - (long)(p.x - a.x) * (long)(b.y - a.y) <= 0;
+		}
+
+		/// <summary>
+		/// Returns if p lies on the left side of the line a - b.
+		/// Uses XZ space. Also returns true if the points are colinear.
+		/// </summary>
+		public static bool RightOrColinearXZ (Vector3 a, Vector3 b, Vector3 p) {
+			return (b.x - a.x) * (p.z - a.z) - (p.x - a.x) * (b.z - a.z) <= 0;
+		}
+
+		/// <summary>
+		/// Returns if p lies on the left side of the line a - b.
+		/// Uses XZ space. Also returns true if the points are colinear.
+		/// </summary>
+		public static bool RightOrColinearXZ (Int3 a, Int3 b, Int3 p) {
+			return (long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) <= 0;
+		}
+
+		/// <summary>
+		/// Returns if the points a in a clockwise order.
+		/// Will return true even if the points are colinear or very slightly counter-clockwise
+		/// (if the signed area of the triangle formed by the points has an area less than or equals to float.Epsilon)
+		/// </summary>
+		public static bool IsClockwiseMarginXZ (Vector3 a, Vector3 b, Vector3 c) {
+			return (b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z) <= float.Epsilon;
+		}
+
+		/// <summary>Returns if the points a in a clockwise order</summary>
+		public static bool IsClockwiseXZ (Vector3 a, Vector3 b, Vector3 c) {
+			return (b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z) < 0;
+		}
+
+		/// <summary>Returns if the points a in a clockwise order</summary>
+		public static bool IsClockwiseXZ (Int3 a, Int3 b, Int3 c) {
+			return RightXZ(a, b, c);
+		}
+
+		/// <summary>Returns true if the points a in a clockwise order or if they are colinear</summary>
+		public static bool IsClockwiseOrColinearXZ (Int3 a, Int3 b, Int3 c) {
+			return RightOrColinearXZ(a, b, c);
+		}
+
+		/// <summary>Returns true if the points a in a clockwise order or if they are colinear</summary>
+		public static bool IsClockwiseOrColinear (Int2 a, Int2 b, Int2 c) {
+			return RightOrColinear(a, b, c);
+		}
+
+		/// <summary>Returns if the points are colinear (lie on a straight line)</summary>
+		public static bool IsColinear (Vector3 a, Vector3 b, Vector3 c) {
+			var lhs = b - a;
+			var rhs = c - a;
+			// Take the cross product of lhs and rhs
+			// The magnitude of the cross product will be zero if the points a,b,c are colinear
+			float x = lhs.y * rhs.z - lhs.z * rhs.y;
+			float y = lhs.z * rhs.x - lhs.x * rhs.z;
+			float z = lhs.x * rhs.y - lhs.y * rhs.x;
+			float v = x*x + y*y + z*z;
+			float lengthsq = lhs.sqrMagnitude * rhs.sqrMagnitude;
+
+			// Epsilon not chosen with much thought, just that float.Epsilon was a bit too small.
+			return v <= math.sqrt(lengthsq) * 0.0001f || lengthsq == 0.0f;
+		}
+
+		/// <summary>Returns if the points are colinear (lie on a straight line)</summary>
+		public static bool IsColinear (Vector2 a, Vector2 b, Vector2 c) {
+			float v = (b.x-a.x)*(c.y-a.y)-(c.x-a.x)*(b.y-a.y);
+
+			// Epsilon not chosen with much thought, just that float.Epsilon was a bit too small.
+			return v <= 0.0001f && v >= -0.0001f;
+		}
+
+		/// <summary>Returns if the points are colinear (lie on a straight line)</summary>
+		public static bool IsColinearXZ (Int3 a, Int3 b, Int3 c) {
+			return (long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z) == 0;
+		}
+
+		/// <summary>Returns if the points are colinear (lie on a straight line)</summary>
+		public static bool IsColinearXZ (Vector3 a, Vector3 b, Vector3 c) {
+			float v = (b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z);
+
+			// Epsilon not chosen with much thought, just that float.Epsilon was a bit too small.
+			return v <= 0.0000001f && v >= -0.0000001f;
+		}
+
+		/// <summary>Returns if the points are colinear (lie on a straight line)</summary>
+		public static bool IsColinearAlmostXZ (Int3 a, Int3 b, Int3 c) {
+			long v = (long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z);
+
+			return v > -1 && v < 1;
+		}
+
+		/// <summary>
+		/// Returns if the line segment start2 - end2 intersects the line segment start1 - end1.
+		/// If only the endpoints coincide, the result is undefined (may be true or false).
+		/// </summary>
+		public static bool SegmentsIntersect (Int2 start1, Int2 end1, Int2 start2, Int2 end2) {
+			return RightOrColinear(start1, end1, start2) != RightOrColinear(start1, end1, end2) && RightOrColinear(start2, end2, start1) != RightOrColinear(start2, end2, end1);
+		}
+
+		/// <summary>
+		/// Returns if the line segment start2 - end2 intersects the line segment start1 - end1.
+		/// If only the endpoints coincide, the result is undefined (may be true or false).
+		///
+		/// Note: XZ space
+		/// </summary>
+		public static bool SegmentsIntersectXZ (Int3 start1, Int3 end1, Int3 start2, Int3 end2) {
+			return RightOrColinearXZ(start1, end1, start2) != RightOrColinearXZ(start1, end1, end2) && RightOrColinearXZ(start2, end2, start1) != RightOrColinearXZ(start2, end2, end1);
+		}
+
+		/// <summary>
+		/// Returns if the two line segments intersects. The lines are NOT treated as infinite (just for clarification)
+		/// See: IntersectionPoint
+		/// </summary>
+		public static bool SegmentsIntersectXZ (Vector3 start1, Vector3 end1, Vector3 start2, Vector3 end2) {
+			Vector3 dir1 = end1-start1;
+			Vector3 dir2 = end2-start2;
+
+			float den = dir2.z*dir1.x - dir2.x * dir1.z;
+
+			if (den == 0) {
+				return false;
+			}
+
+			float nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+			float nom2 = dir1.x*(start1.z-start2.z) - dir1.z * (start1.x - start2.x);
+			float u = nom/den;
+			float u2 = nom2/den;
+
+			if (u < 0F || u > 1F || u2 < 0F || u2 > 1F) {
+				return false;
+			}
+
+			return true;
+		}
+
+		/// <summary>
+		/// Calculates the intersection points between a "capsule" (segment expanded by a radius), and a line.
+		///
+		/// Returns: (t1, t2), the intersection points on the form lineStart + lineDir*t. Where t2 >= t1. If t2 < t1 then there are no intersections.
+		/// </summary>
+		/// <param name="capsuleStart">Center of the capsule's first circle</param>
+		/// <param name="capsuleDir">Main axis of the capsule. Must be normalized.</param>
+		/// <param name="capsuleLength">Distance betwen the capsule's circle centers.</param>
+		/// <param name="lineStart">A point on the line</param>
+		/// <param name="lineDir">The (normalized) direction of the line.</param>
+		/// <param name="radius">The radius of the circle.</param>
+		public static float2 CapsuleLineIntersectionFactors (float2 capsuleStart, float2 capsuleDir, float capsuleLength, float2 lineStart, float2 lineDir, float radius) {
+			var cosAlpha = math.dot(capsuleDir, lineDir);
+			var sinAlpha = math.sqrt(1.0f - cosAlpha*cosAlpha);
+			var tmin = float.PositiveInfinity;
+			var tmax = float.NegativeInfinity;
+
+			if (LineCircleIntersectionFactors(lineStart - capsuleStart, lineDir, radius, out float t11, out float t12)) {
+				tmin = math.min(tmin, t11);
+				tmax = math.max(tmax, t12);
+			}
+			if (LineCircleIntersectionFactors(lineStart - (capsuleStart + capsuleDir*capsuleLength), lineDir, radius, out float t21, out float t22)) {
+				tmin = math.min(tmin, t21);
+				tmax = math.max(tmax, t22);
+			}
+
+			if (LineLineIntersectionFactor(capsuleStart, capsuleDir, lineStart, lineDir, out float ucenter)) {
+				var normal = new float2(-capsuleDir.y, capsuleDir.x);
+				var offset = radius * cosAlpha / sinAlpha;
+				var side = math.sign(capsuleDir.y*lineDir.x - capsuleDir.x*lineDir.y);
+				var ustraight1 = ucenter + offset*side;
+				var ustraight2 = ucenter - offset*side;
+				if (ustraight1 >= 0 && ustraight1 <= capsuleLength) {
+					var p = capsuleStart + capsuleDir * ustraight1 - normal * radius;
+					var tstraight1 = math.dot(p - lineStart, lineDir);
+					tmin = math.min(tmin, tstraight1);
+					tmax = math.max(tmax, tstraight1);
+				}
+				if (ustraight2 >= 0 && ustraight2 <= capsuleLength) {
+					var p = capsuleStart + capsuleDir * ustraight2 + normal * radius;
+					var tstraight2 = math.dot(p - lineStart, lineDir);
+					tmin = math.min(tmin, tstraight2);
+					tmax = math.max(tmax, tstraight2);
+				}
+			} else {
+				// Parallel, or almost parallel.
+				// In this case we can just rely on the circle intersection checks.
+			}
+
+			return new float2(tmin, tmax);
+		}
+
+		/// <summary>
+		/// Calculates the point start1 + dir1*t where the two infinite lines intersect.
+		/// Returns false if the lines are close to parallel.
+		/// </summary>
+		public static bool LineLineIntersectionFactor (float2 start1, float2 dir1, float2 start2, float2 dir2, out float t) {
+			float den = dir2.y*dir1.x - dir2.x * dir1.y;
+
+			if (math.abs(den) < 0.0001f) {
+				t = 0;
+				return false;
+			}
+
+			float nom = dir2.x*(start1.y-start2.y) - dir2.y*(start1.x-start2.x);
+			t = nom/den;
+			return true;
+		}
+
+		/// <summary>
+		/// Calculates the point start1 + dir1*factor1 == start2 + dir2*factor2 where the two infinite lines intersect.
+		/// Returns false if the lines are close to parallel.
+		/// </summary>
+		public static bool LineLineIntersectionFactors (float2 start1, float2 dir1, float2 start2, float2 dir2, out float factor1, out float factor2) {
+			float den = dir2.y*dir1.x - dir2.x * dir1.y;
+
+			if (math.abs(den) < 0.0001f) {
+				factor1 = factor2 = 0;
+				return false;
+			}
+
+			float nom1 = dir2.x*(start1.y-start2.y) - dir2.y*(start1.x-start2.x);
+			float nom2 = dir1.x*(start1.y-start2.y) - dir1.y*(start1.x - start2.x);
+			factor1 = nom1/den;
+			factor2 = nom2/den;
+			return true;
+		}
+
+		/// <summary>
+		/// Intersection point between two infinite lines.
+		/// Note that start points and directions are taken as parameters instead of start and end points.
+		/// Lines are treated as infinite. If the lines are parallel 'start1' will be returned.
+		/// Intersections are calculated on the XZ plane.
+		///
+		/// See: LineIntersectionPointXZ
+		/// </summary>
+		public static Vector3 LineDirIntersectionPointXZ (Vector3 start1, Vector3 dir1, Vector3 start2, Vector3 dir2) {
+			float den = dir2.z*dir1.x - dir2.x * dir1.z;
+
+			if (den == 0) {
+				return start1;
+			}
+
+			float nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+			float u = nom/den;
+
+			return start1 + dir1*u;
+		}
+
+		/// <summary>
+		/// Intersection point between two infinite lines.
+		/// Note that start points and directions are taken as parameters instead of start and end points.
+		/// Lines are treated as infinite. If the lines are parallel 'start1' will be returned.
+		/// Intersections are calculated on the XZ plane.
+		///
+		/// See: LineIntersectionPointXZ
+		/// </summary>
+		public static Vector3 LineDirIntersectionPointXZ (Vector3 start1, Vector3 dir1, Vector3 start2, Vector3 dir2, out bool intersects) {
+			float den = dir2.z*dir1.x - dir2.x * dir1.z;
+
+			if (den == 0) {
+				intersects = false;
+				return start1;
+			}
+
+			float nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+			float u = nom/den;
+
+			intersects = true;
+			return start1 + dir1*u;
+		}
+
+		/// <summary>
+		/// Returns if the ray (start1, end1) intersects the segment (start2, end2).
+		/// false is returned if the lines are parallel.
+		/// Only the XZ coordinates are used.
+		/// TODO: Double check that this actually works
+		/// </summary>
+		public static bool RaySegmentIntersectXZ (Int3 start1, Int3 end1, Int3 start2, Int3 end2) {
+			Int3 dir1 = end1-start1;
+			Int3 dir2 = end2-start2;
+
+			long den = dir2.z*dir1.x - dir2.x * dir1.z;
+
+			if (den == 0) {
+				return false;
+			}
+
+			long nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+			long nom2 = dir1.x*(start1.z-start2.z) - dir1.z * (start1.x - start2.x);
+
+			//factor1 < 0
+			// If both have the same sign, then nom/den < 0 and thus the segment cuts the ray before the ray starts
+			if (!(nom < 0 ^ den < 0)) {
+				return false;
+			}
+
+			//factor2 < 0
+			if (!(nom2 < 0 ^ den < 0)) {
+				return false;
+			}
+
+			if ((den >= 0 && nom2 > den) || (den < 0 && nom2 <= den)) {
+				return false;
+			}
+
+			return true;
+		}
+
+		/// <summary>
+		/// Returns the intersection factors for line 1 and line 2. The intersection factors is a distance along the line start - end where the other line intersects it.
+		/// <code> intersectionPoint = start1 + factor1 * (end1-start1) </code>
+		/// <code> intersectionPoint2 = start2 + factor2 * (end2-start2) </code>
+		/// Lines are treated as infinite.
+		/// false is returned if the lines are parallel and true if they are not.
+		/// Only the XZ coordinates are used.
+		/// </summary>
+		public static bool LineIntersectionFactorXZ (Int3 start1, Int3 end1, Int3 start2, Int3 end2, out float factor1, out float factor2) {
+			Int3 dir1 = end1-start1;
+			Int3 dir2 = end2-start2;
+
+			long den = dir2.z*dir1.x - dir2.x * dir1.z;
+
+			if (den == 0) {
+				factor1 = 0;
+				factor2 = 0;
+				return false;
+			}
+
+			long nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+			long nom2 = dir1.x*(start1.z-start2.z) - dir1.z * (start1.x - start2.x);
+
+			factor1 = (float)nom/den;
+			factor2 = (float)nom2/den;
+
+			return true;
+		}
+
+		/// <summary>
+		/// Returns the intersection factors for line 1 and line 2. The intersection factors is a distance along the line start - end where the other line intersects it.
+		/// <code> intersectionPoint = start1 + factor1 * (end1-start1) </code>
+		/// <code> intersectionPoint2 = start2 + factor2 * (end2-start2) </code>
+		/// Lines are treated as infinite.
+		/// false is returned if the lines are parallel and true if they are not.
+		/// Only the XZ coordinates are used.
+		/// </summary>
+		public static bool LineIntersectionFactorXZ (Vector3 start1, Vector3 end1, Vector3 start2, Vector3 end2, out float factor1, out float factor2) {
+			Vector3 dir1 = end1-start1;
+			Vector3 dir2 = end2-start2;
+
+			float den = dir2.z*dir1.x - dir2.x * dir1.z;
+
+			if (den <= 0.00001f && den >= -0.00001f) {
+				factor1 = 0;
+				factor2 = 0;
+				return false;
+			}
+
+			float nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+			float nom2 = dir1.x*(start1.z-start2.z) - dir1.z * (start1.x - start2.x);
+
+			float u = nom/den;
+			float u2 = nom2/den;
+
+			factor1 = u;
+			factor2 = u2;
+
+			return true;
+		}
+
+		/// <summary>
+		/// Returns the intersection factor for line 1 with ray 2.
+		/// The intersection factors is a factor distance along the line start - end where the other line intersects it.
+		/// <code> intersectionPoint = start1 + factor * (end1-start1) </code>
+		/// Lines are treated as infinite.
+		///
+		/// The second "line" is treated as a ray, meaning only matches on start2 or forwards towards end2 (and beyond) will be returned
+		/// If the point lies on the wrong side of the ray start, Nan will be returned.
+		///
+		/// NaN is returned if the lines are parallel.
+		/// </summary>
+		public static float LineRayIntersectionFactorXZ (Int3 start1, Int3 end1, Int3 start2, Int3 end2) {
+			Int3 dir1 = end1-start1;
+			Int3 dir2 = end2-start2;
+
+			int den = dir2.z*dir1.x - dir2.x * dir1.z;
+
+			if (den == 0) {
+				return float.NaN;
+			}
+
+			int nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+			int nom2 = dir1.x*(start1.z-start2.z) - dir1.z * (start1.x - start2.x);
+
+			if ((float)nom2/den < 0) {
+				return float.NaN;
+			}
+			return (float)nom/den;
+		}
+
+		/// <summary>
+		/// Returns the intersection factor for line 1 with line 2.
+		/// The intersection factor is a distance along the line start1 - end1 where the line start2 - end2 intersects it.
+		/// <code> intersectionPoint = start1 + intersectionFactor * (end1-start1) </code>.
+		/// Lines are treated as infinite.
+		/// -1 is returned if the lines are parallel (note that this is a valid return value if they are not parallel too)
+		/// </summary>
+		public static float LineIntersectionFactorXZ (Vector3 start1, Vector3 end1, Vector3 start2, Vector3 end2) {
+			Vector3 dir1 = end1-start1;
+			Vector3 dir2 = end2-start2;
+
+			float den = dir2.z*dir1.x - dir2.x * dir1.z;
+
+			if (den == 0) {
+				return -1;
+			}
+
+			float nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+			float u = nom/den;
+
+			return u;
+		}
+
+		/// <summary>Returns the intersection point between the two lines. Lines are treated as infinite. start1 is returned if the lines are parallel</summary>
+		public static Vector3 LineIntersectionPointXZ (Vector3 start1, Vector3 end1, Vector3 start2, Vector3 end2) {
+			bool s;
+
+			return LineIntersectionPointXZ(start1, end1, start2, end2, out s);
+		}
+
+		/// <summary>Returns the intersection point between the two lines. Lines are treated as infinite. start1 is returned if the lines are parallel</summary>
+		public static Vector3 LineIntersectionPointXZ (Vector3 start1, Vector3 end1, Vector3 start2, Vector3 end2, out bool intersects) {
+			Vector3 dir1 = end1-start1;
+			Vector3 dir2 = end2-start2;
+
+			float den = dir2.z*dir1.x - dir2.x * dir1.z;
+
+			if (den == 0) {
+				intersects = false;
+				return start1;
+			}
+
+			float nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+
+			float u = nom/den;
+
+			intersects = true;
+			return start1 + dir1*u;
+		}
+
+		/// <summary>Returns the intersection point between the two lines. Lines are treated as infinite. start1 is returned if the lines are parallel</summary>
+		public static Vector2 LineIntersectionPoint (Vector2 start1, Vector2 end1, Vector2 start2, Vector2 end2) {
+			bool s;
+
+			return LineIntersectionPoint(start1, end1, start2, end2, out s);
+		}
+
+		/// <summary>Returns the intersection point between the two lines. Lines are treated as infinite. start1 is returned if the lines are parallel</summary>
+		public static Vector2 LineIntersectionPoint (Vector2 start1, Vector2 end1, Vector2 start2, Vector2 end2, out bool intersects) {
+			Vector2 dir1 = end1-start1;
+			Vector2 dir2 = end2-start2;
+
+			float den = dir2.y*dir1.x - dir2.x * dir1.y;
+
+			if (den == 0) {
+				intersects = false;
+				return start1;
+			}
+
+			float nom = dir2.x*(start1.y-start2.y)- dir2.y*(start1.x-start2.x);
+
+			float u = nom/den;
+
+			intersects = true;
+			return start1 + dir1*u;
+		}
+
+		/// <summary>
+		/// Returns the intersection point between the two line segments in XZ space.
+		/// Lines are NOT treated as infinite. start1 is returned if the line segments do not intersect
+		/// The point will be returned along the line [start1, end1] (this matters only for the y coordinate).
+		/// </summary>
+		public static Vector3 SegmentIntersectionPointXZ (Vector3 start1, Vector3 end1, Vector3 start2, Vector3 end2, out bool intersects) {
+			Vector3 dir1 = end1-start1;
+			Vector3 dir2 = end2-start2;
+
+			float den = dir2.z * dir1.x - dir2.x * dir1.z;
+
+			if (den == 0) {
+				intersects = false;
+				return start1;
+			}
+
+			float nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x);
+			float nom2 = dir1.x*(start1.z-start2.z) - dir1.z*(start1.x-start2.x);
+			float u = nom/den;
+			float u2 = nom2/den;
+
+			if (u < 0F || u > 1F || u2 < 0F || u2 > 1F) {
+				intersects = false;
+				return start1;
+			}
+
+			intersects = true;
+			return start1 + dir1*u;
+		}
+
+		/// <summary>
+		/// Does the line segment intersect the bounding box.
+		/// The line is NOT treated as infinite.
+		/// \author Slightly modified code from http://www.3dkingdoms.com/weekly/weekly.php?a=21
+		/// </summary>
+		public static bool SegmentIntersectsBounds (Bounds bounds, Vector3 a, Vector3 b) {
+			// Put segment in box space
+			a -= bounds.center;
+			b -= bounds.center;
+
+			// Get line midpoint and extent
+			var LMid = (a + b) * 0.5F;
+			var L = (a - LMid);
+			var LExt = new Vector3(Math.Abs(L.x), Math.Abs(L.y), Math.Abs(L.z));
+
+			Vector3 extent = bounds.extents;
+
+			// Use Separating Axis Test
+			// Separation vector from box center to segment center is LMid, since the line is in box space
+			if (Math.Abs(LMid.x) > extent.x + LExt.x) return false;
+			if (Math.Abs(LMid.y) > extent.y + LExt.y) return false;
+			if (Math.Abs(LMid.z) > extent.z + LExt.z) return false;
+			// Crossproducts of line and each axis
+			if (Math.Abs(LMid.y * L.z - LMid.z * L.y) > (extent.y * LExt.z + extent.z * LExt.y)) return false;
+			if (Math.Abs(LMid.x * L.z - LMid.z * L.x) > (extent.x * LExt.z + extent.z * LExt.x)) return false;
+			if (Math.Abs(LMid.x * L.y - LMid.y * L.x) > (extent.x * LExt.y + extent.y * LExt.x)) return false;
+			// No separating axis, the line intersects
+			return true;
+		}
+
+		/// <summary>
+		/// Calculates the two intersection points (point + direction*t) on the line where it intersects with a circle at the origin.
+		///
+		/// t1 will always be less than or equal to t2 if there are intersections.
+		///
+		/// Returns false if there are no intersections.
+		/// </summary>
+		/// <param name="point">A point on the line</param>
+		/// <param name="direction">The normalized direction of the line</param>
+		/// <param name="radius">The radius of the circle at the origin.</param>
+		/// <param name="t1">The first intersection (if any).</param>
+		/// <param name="t2">The second intersection (if any).</param>
+		public static bool LineCircleIntersectionFactors (float2 point, float2 direction, float radius, out float t1, out float t2) {
+			// Distance from the closest point on the line (from the origin) to line.point
+			float dot = math.dot(point, direction);
+			// Squared distance from the origin to the closest point on the line
+			float distanceToLine = math.lengthsq(point) - dot*dot;
+			// Calculate the intersection of the line with the circle.
+			// This is the squared length of half the chord that intersects the circle.
+			float discriminant = radius*radius - distanceToLine;
+
+			if (discriminant < 0.0f) {
+				// The line is completely outside the circle
+				t1 = float.PositiveInfinity;
+				t2 = float.NegativeInfinity;
+				return false;
+			}
+
+			var sqrtDiscriminant = math.sqrt(discriminant);
+			t1 = -dot - sqrtDiscriminant;
+			t2 = -dot + sqrtDiscriminant;
+			return true;
+		}
+
+		/// <summary>
+		/// Calculates the two intersection points (lerp(point1, point2, t)) on the segment where it intersects with a circle at the origin.
+		///
+		/// t1 will always be less than or equal to t2 if there are intersections.
+		///
+		/// Returns false if there are no intersections.
+		/// </summary>
+		/// <param name="point1">Start of the segment</param>
+		/// <param name="point2">End of the segment</param>
+		/// <param name="radiusSq">The squared radius of the circle at the origin.</param>
+		/// <param name="t1">The first intersection (if any). Between 0 and 1.</param>
+		/// <param name="t2">The second intersection (if any). Between 0 and 1.</param>
+		public static bool SegmentCircleIntersectionFactors (float2 point1, float2 point2, float radiusSq, out float t1, out float t2) {
+			// Distance from the closest point on the line (from the origin) to line.point
+			var dir = point2 - point1;
+			var dirSq = math.lengthsq(dir);
+			float dot = math.dot(point1, dir) / dirSq;
+			// Proportional to the squared distance from the origin to the closest point on the line
+			float distanceToLine = math.lengthsq(point1) / dirSq - dot*dot;
+			float discriminant = radiusSq/dirSq - distanceToLine;
+
+			if (discriminant < 0.0f) {
+				// The line is completely outside the circle
+				t1 = float.PositiveInfinity;
+				t2 = float.NegativeInfinity;
+				return false;
+			}
+
+			var sqrtDiscriminant = math.sqrt(discriminant);
+			t1 = -dot - sqrtDiscriminant;
+			t2 = -dot + sqrtDiscriminant;
+			t1 = math.max(0, t1);
+			t2 = math.min(1, t2);
+
+			if (t1 >= 1 || t2 <= 0) return false;
+			return true;
+		}
+
+		/// <summary>
+		/// Intersection of a line and a circle.
+		/// Returns the greatest t such that segmentStart+t*(segmentEnd-segmentStart) lies on the circle.
+		///
+		/// In case the line does not intersect with the circle, the closest point on the line
+		/// to the circle will be returned.
+		///
+		/// Note: Works for line and sphere in 3D space as well.
+		///
+		/// See: http://mathworld.wolfram.com/Circle-LineIntersection.html
+		/// See: https://en.wikipedia.org/wiki/Intersection_(Euclidean_geometry)<see cref="A_line_and_a_circle"/>
+		/// </summary>
+		public static float LineCircleIntersectionFactor (Vector3 circleCenter, Vector3 linePoint1, Vector3 linePoint2, float radius) {
+			float segmentLength;
+			var normalizedDirection = Normalize(linePoint2 - linePoint1, out segmentLength);
+			var dirToStart = linePoint1 - circleCenter;
+
+			var dot = Vector3.Dot(dirToStart, normalizedDirection);
+			var discriminant = dot * dot - (dirToStart.sqrMagnitude - radius*radius);
+
+			if (discriminant < 0) {
+				// No intersection, pick closest point on segment
+				discriminant = 0;
+			}
+
+			var t = -dot + Mathf.Sqrt(discriminant);
+			// Note: the default value of 1 is important for the PathInterpolator.MoveToCircleIntersection2D
+			// method to work properly. Maybe find some better abstraction where this default value is more obvious.
+			return segmentLength > 0.00001f ? t / segmentLength : 1f;
+		}
+
+		/// <summary>
+		/// True if the matrix will reverse orientations of faces.
+		///
+		/// Scaling by a negative value along an odd number of axes will reverse
+		/// the orientation of e.g faces on a mesh. This must be counter adjusted
+		/// by for example the recast rasterization system to be able to handle
+		/// meshes with negative scales properly.
+		///
+		/// We can find out if they are flipped by finding out how the signed
+		/// volume of a unit cube is transformed when applying the matrix
+		///
+		/// If the (signed) volume turns out to be negative
+		/// that also means that the orientation of it has been reversed.
+		///
+		/// See: https://en.wikipedia.org/wiki/Normal_(geometry)
+		/// See: https://en.wikipedia.org/wiki/Parallelepiped
+		/// </summary>
+		public static bool ReversesFaceOrientations (Matrix4x4 matrix) {
+			var dX = matrix.MultiplyVector(new Vector3(1, 0, 0));
+			var dY = matrix.MultiplyVector(new Vector3(0, 1, 0));
+			var dZ = matrix.MultiplyVector(new Vector3(0, 0, 1));
+
+			// Calculate the signed volume of the parallelepiped
+			var volume = Vector3.Dot(Vector3.Cross(dX, dY), dZ);
+
+			return volume < 0;
+		}
+
+		/// <summary>
+		/// Normalize vector and also return the magnitude.
+		/// This is more efficient than calculating the magnitude and normalizing separately
+		/// </summary>
+		public static Vector3 Normalize (Vector3 v, out float magnitude) {
+			magnitude = v.magnitude;
+			// This is the same constant that Unity uses
+			if (magnitude > 1E-05f) {
+				return v / magnitude;
+			} else {
+				return Vector3.zero;
+			}
+		}
+
+		/// <summary>
+		/// Normalize vector and also return the magnitude.
+		/// This is more efficient than calculating the magnitude and normalizing separately
+		/// </summary>
+		public static Vector2 Normalize (Vector2 v, out float magnitude) {
+			magnitude = v.magnitude;
+			// This is the same constant that Unity uses
+			if (magnitude > 1E-05f) {
+				return v / magnitude;
+			} else {
+				return Vector2.zero;
+			}
+		}
+
+		/* Clamp magnitude along the X and Z axes.
+		 * The y component will not be changed.
+		 */
+		public static Vector3 ClampMagnitudeXZ (Vector3 v, float maxMagnitude) {
+			float squaredMagnitudeXZ = v.x*v.x + v.z*v.z;
+
+			if (squaredMagnitudeXZ > maxMagnitude*maxMagnitude && maxMagnitude > 0) {
+				var factor = maxMagnitude / Mathf.Sqrt(squaredMagnitudeXZ);
+				v.x *= factor;
+				v.z *= factor;
+			}
+			return v;
+		}
+
+		/* Magnitude in the XZ plane */
+		public static float MagnitudeXZ (Vector3 v) {
+			return Mathf.Sqrt(v.x*v.x + v.z*v.z);
+		}
+
+		/// <summary>
+		/// Number of radians that this quaternion rotates around its axis of rotation.
+		/// Will be in the range [-PI, PI].
+		///
+		/// Note: A quaternion of q and -q represent the same rotation, but their axis of rotation point in opposite directions, so the angle will be different.
+		/// </summary>
+		public static float QuaternionAngle (quaternion rot) {
+			return 2 * math.atan2(math.length(rot.value.xyz), rot.value.w);
+		}
+	}
+
+	/// <summary>
+	/// Utility functions for working with numbers and strings.
+	///
+	/// See: Polygon
+	/// See: VectorMath
+	/// </summary>
+	public static class AstarMath {
+		static Unity.Mathematics.Random GlobalRandom = Unity.Mathematics.Random.CreateFromIndex(0);
+		static object GlobalRandomLock = new object();
+
+		public static float ThreadSafeRandomFloat () {
+			lock (GlobalRandomLock) {
+				return GlobalRandom.NextFloat();
+			}
+		}
+
+		public static float2 ThreadSafeRandomFloat2 () {
+			lock (GlobalRandomLock) {
+				return GlobalRandom.NextFloat2();
+			}
+		}
+
+		/// <summary>Converts a non-negative float to a long, saturating at long.MaxValue if the value is too large</summary>
+		public static long SaturatingConvertFloatToLong(float v) => v > (float)long.MaxValue ? long.MaxValue : (long)v;
+
+		/// <summary>Maps a value between startMin and startMax to be between targetMin and targetMax</summary>
+		public static float MapTo (float startMin, float startMax, float targetMin, float targetMax, float value) {
+			return Mathf.Lerp(targetMin, targetMax, Mathf.InverseLerp(startMin, startMax, value));
+		}
+
+		/// <summary>
+		/// Returns bit number b from int a. The bit number is zero based. Relevant b values are from 0 to 31.
+		/// Equals to (a >> b) & 1
+		/// </summary>
+		static int Bit (int a, int b) {
+			return (a >> b) & 1;
+		}
+
+		/// <summary>
+		/// Returns a nice color from int i with alpha a. Got code from the open-source Recast project, works really well.
+		/// Seems like there are only 64 possible colors from studying the code
+		/// </summary>
+		public static Color IntToColor (int i, float a) {
+			int r = Bit(i, 2) + Bit(i, 3) * 2 + 1;
+			int g = Bit(i, 1) + 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);
+		}
+
+		/// <summary>
+		/// Converts an HSV color to an RGB color.
+		/// According to the algorithm described at http://en.wikipedia.org/wiki/HSL_and_HSV
+		///
+		/// @author Wikipedia
+		/// @return the RGB representation of the color.
+		/// </summary>
+		public static Color HSVToRGB (float h, float s, float v) {
+			float r = 0, g = 0, b = 0;
+
+			float Chroma = s * v;
+			float Hdash = h / 60.0f;
+			float X = Chroma * (1.0f - System.Math.Abs((Hdash % 2.0f) - 1.0f));
+
+			if (Hdash < 1.0f) {
+				r = Chroma;
+				g = X;
+			} else if (Hdash < 2.0f) {
+				r = X;
+				g = Chroma;
+			} else if (Hdash < 3.0f) {
+				g = Chroma;
+				b = X;
+			} else if (Hdash < 4.0f) {
+				g = X;
+				b = Chroma;
+			} else if (Hdash < 5.0f) {
+				r = X;
+				b = Chroma;
+			} else if (Hdash < 6.0f) {
+				r = Chroma;
+				b = X;
+			}
+
+			float Min = v - Chroma;
+
+			r += Min;
+			g += Min;
+			b += Min;
+
+			return new Color(r, g, b);
+		}
+
+		/// <summary>
+		/// Calculates the shortest difference between two given angles given in radians.
+		///
+		/// The return value will be between -pi/2 and +pi/2.
+		/// </summary>
+		public static float DeltaAngle (float angle1, float angle2) {
+			float diff = (angle2 - angle1 + math.PI) % (2*math.PI) - math.PI;
+			return math.select(diff, diff + 2*math.PI, diff < -math.PI);
+		}
+	}
+
+	/// <summary>
+	/// Utility functions for working with polygons, lines, and other vector math.
+	/// All functions which accepts Vector3s but work in 2D space uses the XZ space if nothing else is said.
+	///
+	/// Version: A lot of functions in this class have been moved to the VectorMath class
+	/// the names have changed slightly and everything now consistently assumes a left handed
+	/// coordinate system now instead of sometimes using a left handed one and sometimes
+	/// using a right handed one. This is why the 'Left' methods redirect to methods
+	/// named 'Right'. The functionality is exactly the same.
+	/// </summary>
+	[BurstCompile]
+	public static class Polygon {
+		/// <summary>
+		/// Returns if the triangle ABC contains the point p in XZ space.
+		/// The triangle vertices are assumed to be laid out in clockwise order.
+		/// </summary>
+		public static bool ContainsPointXZ (Vector3 a, Vector3 b, Vector3 c, Vector3 p) {
+			return VectorMath.IsClockwiseMarginXZ(a, b, p) && VectorMath.IsClockwiseMarginXZ(b, c, p) && VectorMath.IsClockwiseMarginXZ(c, a, p);
+		}
+
+		/// <summary>
+		/// Returns if the triangle ABC contains the point p.
+		/// The triangle vertices are assumed to be laid out in clockwise order.
+		/// </summary>
+		public static bool ContainsPointXZ (Int3 a, Int3 b, Int3 c, Int3 p) {
+			return VectorMath.IsClockwiseOrColinearXZ(a, b, p) && VectorMath.IsClockwiseOrColinearXZ(b, c, p) && VectorMath.IsClockwiseOrColinearXZ(c, a, p);
+		}
+
+		/// <summary>
+		/// Returns if the triangle ABC contains the point p.
+		/// The triangle vertices are assumed to be laid out in clockwise order.
+		/// </summary>
+		public static bool ContainsPoint (Int2 a, Int2 b, Int2 c, Int2 p) {
+			return VectorMath.IsClockwiseOrColinear(a, b, p) && VectorMath.IsClockwiseOrColinear(b, c, p) && VectorMath.IsClockwiseOrColinear(c, a, p);
+		}
+
+		/// <summary>
+		/// Checks if p is inside the polygon.
+		/// \author http://unifycommunity.com/wiki/index.php?title=PolyContainsPoint (Eric5h5)
+		/// </summary>
+		public static bool ContainsPoint (Vector2[] polyPoints, Vector2 p) {
+			int j = polyPoints.Length-1;
+			bool inside = false;
+
+			for (int i = 0; i < polyPoints.Length; j = i++) {
+				if (((polyPoints[i].y <= p.y && p.y < polyPoints[j].y) || (polyPoints[j].y <= p.y && p.y < polyPoints[i].y)) &&
+					(p.x < (polyPoints[j].x - polyPoints[i].x) * (p.y - polyPoints[i].y) / (polyPoints[j].y - polyPoints[i].y) + polyPoints[i].x))
+					inside = !inside;
+			}
+			return inside;
+		}
+
+		/// <summary>
+		/// Checks if p is inside the polygon (XZ space).
+		/// \author http://unifycommunity.com/wiki/index.php?title=PolyContainsPoint (Eric5h5)
+		/// </summary>
+		public static bool ContainsPointXZ (Vector3[] polyPoints, Vector3 p) {
+			int j = polyPoints.Length-1;
+			bool inside = false;
+
+			for (int i = 0; i < polyPoints.Length; j = i++) {
+				if (((polyPoints[i].z <= p.z && p.z < polyPoints[j].z) || (polyPoints[j].z <= p.z && p.z < polyPoints[i].z)) &&
+					(p.x < (polyPoints[j].x - polyPoints[i].x) * (p.z - polyPoints[i].z) / (polyPoints[j].z - polyPoints[i].z) + polyPoints[i].x))
+					inside = !inside;
+			}
+			return inside;
+		}
+
+		/// <summary>
+		/// Returns if the triangle contains the point p when projected on the movement plane.
+		/// The triangle vertices may be clockwise or counter-clockwise.
+		///
+		/// This method is numerically robust, as in, if the point is contained in exactly one of two adjacent triangles, then this
+		/// function will return true for at least one of them (both if the point is exactly on the edge between them).
+		/// If it was less numerically robust, it could conceivably return false for both of them if the point was on the edge between them, which would be bad.
+		/// </summary>
+		[BurstCompile]
+		public static bool ContainsPoint (ref int3 aWorld, ref int3 bWorld, ref int3 cWorld, ref int3 pWorld, ref NativeMovementPlane movementPlane) {
+			// Extract the coordinate axes of the movement plane
+			var m = new float3x3(movementPlane.rotation.value);
+			var m2D = math.transpose(new float3x2(m.c0, m.c2));
+			return ContainsPoint(ref aWorld, ref bWorld, ref cWorld, ref pWorld, in m2D);
+		}
+
+		/// <summary>
+		/// Returns if the triangle contains the point p when projected on a plane using the given projection.
+		/// The triangle vertices may be clockwise or counter-clockwise.
+		///
+		/// This method is numerically robust, as in, if the point is contained in exactly one of two adjacent triangles, then this
+		/// function will return true for at least one of them (both if the point is exactly on the edge between them).
+		/// If it was less numerically robust, it could conceivably return false for both of them if the point was on the edge between them, which would be bad.
+		/// </summary>
+		public static bool ContainsPoint (ref int3 aWorld, ref int3 bWorld, ref int3 cWorld, ref int3 pWorld, in float2x3 planeProjection) {
+			const int QUANTIZATION = 1024;
+			var m = new int2x3(planeProjection * QUANTIZATION);
+			// Project all the points onto the movement plane using SIMD
+			var xs = new int4(aWorld.x, bWorld.x, cWorld.x, pWorld.x);
+			var ys = new int4(aWorld.y, bWorld.y, cWorld.y, pWorld.y);
+			var zs = new int4(aWorld.z, bWorld.z, cWorld.z, pWorld.z);
+			// Subtract the first point from all the other points
+			// This ensures that large coordinates will not overflow due to using 32 bits here.
+			// Since we multiply all coordinates by QUANTIZATION, and Int3 coordinates are already multiplied by 1000,
+			// coordinates would otherwise be liable to start overflowing at unity world coordinates above around 2000.
+			// TODO: We could still get bad results if pWorld is very far away from the triangle (about 4000 units).
+			xs -= xs.x;
+			ys -= ys.x;
+			zs -= zs.x;
+			// Projected X and Y coordinates
+			var px = (xs * m.c0.x + ys * m.c1.x + zs * m.c2.x) / QUANTIZATION;
+			var py = (xs * m.c0.y + ys * m.c1.y + zs * m.c2.y) / QUANTIZATION;
+
+			// Do 3 cross products to check if the point is inside the triangle
+			var v1 = px.yzx - px.xyz;
+			var v2 = py.www - py.xyz;
+			var v3 = px.www - px.xyz;
+			var v4 = py.yzx - py.xyz;
+			long check1 = (long)v1.x * (long)v2.x - (long)v3.x * (long)v4.x;
+			long check2 = (long)v1.y * (long)v2.y - (long)v3.y * (long)v4.y;
+			long check3 = (long)v1.z * (long)v2.z - (long)v3.z * (long)v4.z;
+			// Allow for both clockwise and counter-clockwise triangle layouts.
+			// This can be important sometimes on spherical worlds where the "upside-down" triangles
+			// will be seen as having the reverse winding order when projected onto a plane.
+			// We take care to include points right on the edge of the triangle.
+			return (check1 >= 0 & check2 >= 0 & check3 >= 0) | (check1 <= 0 & check2 <= 0 & check3 <= 0);
+
+			// Note: It might be tempting to try to use SIMD-like code for this. But the following requires a lot more instructions, as it turns out.
+			// return math.all(new bool3(check1 >= 0, check2 >= 0, check3 >= 0)) || math.all(new bool3(check1 <= 0, check2 <= 0, check3 <= 0));
+		}
+
+		/// <summary>
+		/// Sample Y coordinate of the triangle (p1, p2, p3) at the point p in XZ space.
+		/// The y coordinate of p is ignored.
+		///
+		/// Returns: The interpolated y coordinate unless the triangle is degenerate in which case a DivisionByZeroException will be thrown
+		///
+		/// See: https://en.wikipedia.org/wiki/Barycentric_coordinate_system
+		/// </summary>
+		public static int SampleYCoordinateInTriangle (Int3 p1, Int3 p2, Int3 p3, Int3 p) {
+			double det = ((double)(p2.z - p3.z)) * (p1.x - p3.x) + ((double)(p3.x - p2.x)) * (p1.z - p3.z);
+
+			double lambda1 = ((((double)(p2.z - p3.z)) * (p.x - p3.x) + ((double)(p3.x - p2.x)) * (p.z - p3.z)) / det);
+			double lambda2 = ((((double)(p3.z - p1.z)) * (p.x - p3.x) + ((double)(p1.x - p3.x)) * (p.z - p3.z)) / det);
+
+			return (int)Math.Round(lambda1 * p1.y + lambda2 * p2.y + (1 - lambda1 - lambda2) * p3.y);
+		}
+
+		/// <summary>
+		/// Calculates convex hull in XZ space for the points.
+		/// Implemented using the very simple Gift Wrapping Algorithm
+		/// which has a complexity of O(nh) where n is the number of points and h is the number of points on the hull,
+		/// so it is in the worst case quadratic.
+		/// </summary>
+		public static Vector3[] ConvexHullXZ (Vector3[] points) {
+			if (points.Length == 0) return new Vector3[0];
+
+			var hull = Pathfinding.Util.ListPool<Vector3>.Claim();
+
+			int pointOnHull = 0;
+			for (int i = 1; i < points.Length; i++) if (points[i].x < points[pointOnHull].x) pointOnHull = i;
+
+			int startpoint = pointOnHull;
+			int counter = 0;
+
+			do {
+				hull.Add(points[pointOnHull]);
+				int endpoint = 0;
+				for (int i = 0; i < points.Length; i++) if (endpoint == pointOnHull || !VectorMath.RightOrColinearXZ(points[pointOnHull], points[endpoint], points[i])) endpoint = i;
+
+				pointOnHull = endpoint;
+
+				counter++;
+				if (counter > 10000) {
+					Debug.LogWarning("Infinite Loop in Convex Hull Calculation");
+					break;
+				}
+			} while (pointOnHull != startpoint);
+
+			var result = hull.ToArray();
+
+			// Return to pool
+			Pathfinding.Util.ListPool<Vector3>.Release(hull);
+			return result;
+		}
+
+		/// <summary>
+		/// Closest point on the triangle abc to the point p.
+		/// See: 'Real Time Collision Detection' by Christer Ericson, chapter 5.1, page 141
+		/// </summary>
+		public static Vector2 ClosestPointOnTriangle (Vector2 a, Vector2 b, Vector2 c, Vector2 p) {
+			// Check if p is in vertex region outside A
+			var ab = b - a;
+			var ac = c - a;
+			var ap = p - a;
+
+			var d1 = Vector2.Dot(ab, ap);
+			var d2 = Vector2.Dot(ac, ap);
+
+			// Barycentric coordinates (1,0,0)
+			if (d1 <= 0 && d2 <= 0) {
+				return a;
+			}
+
+			// Check if p is in vertex region outside B
+			var bp = p - b;
+			var d3 = Vector2.Dot(ab, bp);
+			var d4 = Vector2.Dot(ac, bp);
+
+			// Barycentric coordinates (0,1,0)
+			if (d3 >= 0 && d4 <= d3) {
+				return b;
+			}
+
+			// Check if p is in edge region outside AB, if so return a projection of p onto AB
+			if (d1 >= 0 && d3 <= 0) {
+				var vc = d1 * d4 - d3 * d2;
+				if (vc <= 0) {
+					// Barycentric coordinates (1-v, v, 0)
+					var v = d1 / (d1 - d3);
+					return a + ab*v;
+				}
+			}
+
+			// Check if p is in vertex region outside C
+			var cp = p - c;
+			var d5 = Vector2.Dot(ab, cp);
+			var d6 = Vector2.Dot(ac, cp);
+
+			// Barycentric coordinates (0,0,1)
+			if (d6 >= 0 && d5 <= d6) {
+				return c;
+			}
+
+			// Check if p is in edge region of AC, if so return a projection of p onto AC
+			if (d2 >= 0 && d6 <= 0) {
+				var vb = d5 * d2 - d1 * d6;
+				if (vb <= 0) {
+					// Barycentric coordinates (1-v, 0, v)
+					var v = d2 / (d2 - d6);
+					return a + ac*v;
+				}
+			}
+
+			// Check if p is in edge region of BC, if so return projection of p onto BC
+			if ((d4 - d3) >= 0 && (d5 - d6) >= 0) {
+				var va = d3 * d6 - d5 * d4;
+				if (va <= 0) {
+					var v = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+					return b + (c - b) * v;
+				}
+			}
+
+			return p;
+		}
+
+		/// <summary>
+		/// Closest point on the triangle abc to the point p when seen from above.
+		/// See: 'Real Time Collision Detection' by Christer Ericson, chapter 5.1, page 141
+		/// </summary>
+		public static Vector3 ClosestPointOnTriangleXZ (Vector3 a, Vector3 b, Vector3 c, Vector3 p) {
+			// Check if p is in vertex region outside A
+			var ab = new Vector2(b.x - a.x, b.z - a.z);
+			var ac = new Vector2(c.x - a.x, c.z - a.z);
+			var ap = new Vector2(p.x - a.x, p.z - a.z);
+
+			var d1 = Vector2.Dot(ab, ap);
+			var d2 = Vector2.Dot(ac, ap);
+
+			// Barycentric coordinates (1,0,0)
+			if (d1 <= 0 && d2 <= 0) {
+				return a;
+			}
+
+			// Check if p is in vertex region outside B
+			var bp = new Vector2(p.x - b.x, p.z - b.z);
+			var d3 = Vector2.Dot(ab, bp);
+			var d4 = Vector2.Dot(ac, bp);
+
+			// Barycentric coordinates (0,1,0)
+			if (d3 >= 0 && d4 <= d3) {
+				return b;
+			}
+
+			// Check if p is in edge region outside AB, if so return a projection of p onto AB
+			var vc = d1 * d4 - d3 * d2;
+			if (d1 >= 0 && d3 <= 0 && vc <= 0) {
+				// Barycentric coordinates (1-v, v, 0)
+				var v = d1 / (d1 - d3);
+				return (1-v)*a + v*b;
+			}
+
+			// Check if p is in vertex region outside C
+			var cp = new Vector2(p.x - c.x, p.z - c.z);
+			var d5 = Vector2.Dot(ab, cp);
+			var d6 = Vector2.Dot(ac, cp);
+
+			// Barycentric coordinates (0,0,1)
+			if (d6 >= 0 && d5 <= d6) {
+				return c;
+			}
+
+			// Check if p is in edge region of AC, if so return a projection of p onto AC
+			var vb = d5 * d2 - d1 * d6;
+			if (d2 >= 0 && d6 <= 0 && vb <= 0) {
+				// Barycentric coordinates (1-v, 0, v)
+				var v = d2 / (d2 - d6);
+				return (1-v)*a + v*c;
+			}
+
+			// Check if p is in edge region of BC, if so return projection of p onto BC
+			var va = d3 * d6 - d5 * d4;
+			if ((d4 - d3) >= 0 && (d5 - d6) >= 0 && va <= 0) {
+				var v = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+				return b + (c - b) * v;
+			} else {
+				// P is inside the face region. Compute the point using its barycentric coordinates (u, v, w)
+				// Note that the x and z coordinates will be exactly the same as P's x and z coordinates
+				var denom = 1f / (va + vb + vc);
+				var v = vb * denom;
+				var w = vc * denom;
+
+				return new Vector3(p.x, (1 - v - w)*a.y + v*b.y + w*c.y, p.z);
+			}
+		}
+
+		/// <summary>
+		/// Closest point on the triangle abc to the point p.
+		/// See: 'Real Time Collision Detection' by Christer Ericson, chapter 5.1, page 141
+		/// </summary>
+		public static float3 ClosestPointOnTriangle (float3 a, float3 b, float3 c, float3 p) {
+			ClosestPointOnTriangleByRef(in a, in b, in c, in p, out var output);
+			return output;
+		}
+
+		/// <summary>
+		/// Closest point on the triangle abc to the point p.
+		///
+		/// Takes arguments by reference to be able to be burst-compiled.
+		///
+		/// See: 'Real Time Collision Detection' by Christer Ericson, chapter 5.1, page 141
+		///
+		/// Returns: True if the point is inside the triangle, false otherwise, after the point has been projected on the plane that the triangle is in.
+		/// </summary>
+		[BurstCompile]
+		public static bool ClosestPointOnTriangleByRef (in float3 a, in float3 b, in float3 c, in float3 p, [NoAlias] out float3 output) {
+			// Check if p is in vertex region outside A
+			var ab = b - a;
+			var ac = c - a;
+			var ap = p - a;
+
+			var d1 = math.dot(ab, ap);
+			var d2 = math.dot(ac, ap);
+
+			// Barycentric coordinates (1,0,0)
+			if (d1 <= 0 && d2 <= 0) {
+				output = a;
+				return false;
+			}
+
+			// Check if p is in vertex region outside B
+			var bp = p - b;
+			var d3 = math.dot(ab, bp);
+			var d4 = math.dot(ac, bp);
+
+			// Barycentric coordinates (0,1,0)
+			if (d3 >= 0 && d4 <= d3) {
+				output = b;
+				return false;
+			}
+
+			// Check if p is in edge region outside AB, if so return a projection of p onto AB
+			var vc = d1 * d4 - d3 * d2;
+			if (d1 >= 0 && d3 <= 0 && vc <= 0) {
+				// Barycentric coordinates (1-v, v, 0)
+				var v = d1 / (d1 - d3);
+				output = a + ab * v;
+				return false;
+			}
+
+			// Check if p is in vertex region outside C
+			var cp = p - c;
+			var d5 = math.dot(ab, cp);
+			var d6 = math.dot(ac, cp);
+
+			// Barycentric coordinates (0,0,1)
+			if (d6 >= 0 && d5 <= d6) {
+				output = c;
+				return false;
+			}
+
+			// Check if p is in edge region of AC, if so return a projection of p onto AC
+			var vb = d5 * d2 - d1 * d6;
+			if (d2 >= 0 && d6 <= 0 && vb <= 0) {
+				// Barycentric coordinates (1-v, 0, v)
+				var v = d2 / (d2 - d6);
+				output = a + ac * v;
+				return false;
+			}
+
+			// Check if p is in edge region of BC, if so return projection of p onto BC
+			var va = d3 * d6 - d5 * d4;
+			if ((d4 - d3) >= 0 && (d5 - d6) >= 0 && va <= 0) {
+				var v = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+				output = b + (c - b) * v;
+				return false;
+			} else {
+				// P is inside the face region. Compute the point using its barycentric coordinates (u, v, w)
+				var denom = 1f / (va + vb + vc);
+				var v = vb * denom;
+				var w = vc * denom;
+
+				// This is equal to: u*a + v*b + w*c, u = va*denom = 1 - v - w;
+				output = a + ab * v + ac * w;
+				return true;
+			}
+		}
+
+		/// <summary>
+		/// Closest point on the triangle abc to the point p as barycentric coordinates.
+		///
+		/// See: 'Real Time Collision Detection' by Christer Ericson, chapter 5.1, page 141
+		/// </summary>
+		public static float3 ClosestPointOnTriangleBarycentric (float2 a, float2 b, float2 c, float2 p) {
+			// Check if p is in vertex region outside A
+			var ab = b - a;
+			var ac = c - a;
+			var ap = p - a;
+
+			var d1 = math.dot(ab, ap);
+			var d2 = math.dot(ac, ap);
+
+			// Barycentric coordinates (1,0,0)
+			if (d1 <= 0 && d2 <= 0) {
+				return new float3(1, 0, 0);
+			}
+
+			// Check if p is in vertex region outside B
+			var bp = p - b;
+			var d3 = math.dot(ab, bp);
+			var d4 = math.dot(ac, bp);
+
+			// Barycentric coordinates (0,1,0)
+			if (d3 >= 0 && d4 <= d3) {
+				return new float3(0, 1, 0);
+			}
+
+			// Check if p is in edge region outside AB, if so return a projection of p onto AB
+			var vc = d1 * d4 - d3 * d2;
+			if (d1 >= 0 && d3 <= 0 && vc <= 0) {
+				// Barycentric coordinates (1-v, v, 0)
+				var v = d1 / (d1 - d3);
+				return new float3(1-v, v, 0);
+			}
+
+			// Check if p is in vertex region outside C
+			var cp = p - c;
+			var d5 = math.dot(ab, cp);
+			var d6 = math.dot(ac, cp);
+
+			// Barycentric coordinates (0,0,1)
+			if (d6 >= 0 && d5 <= d6) {
+				return new float3(0, 0, 1);
+			}
+
+			// Check if p is in edge region of AC, if so return a projection of p onto AC
+			var vb = d5 * d2 - d1 * d6;
+			if (d2 >= 0 && d6 <= 0 && vb <= 0) {
+				// Barycentric coordinates (1-v, 0, v)
+				var v = d2 / (d2 - d6);
+				return new float3(1 - v, 0, v);
+			}
+
+			// Check if p is in edge region of BC, if so return projection of p onto BC
+			var va = d3 * d6 - d5 * d4;
+			if ((d4 - d3) >= 0 && (d5 - d6) >= 0 && va <= 0) {
+				var v = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+				return new float3(0, 1 - v, v);
+			} else {
+				// P is inside the face region. Compute the point using its barycentric coordinates (u, v, w)
+				var denom = 1f / (va + vb + vc);
+				var v = vb * denom;
+				var w = vc * denom;
+				return new float3(1 - v - w, v, w);
+
+				// This is equal to: u*a + v*b + w*c, u = va*denom = 1 - v - w;
+				// return a + ab * v + ac * w;
+			}
+		}
+
+		/// <summary>
+		/// Closest point on a triangle when one axis is scaled.
+		///
+		/// Project the triangle onto the plane defined by the projection axis.
+		/// Then find the closest point on the triangle in the plane.
+		/// Calculate the distance to the closest point in the plane, call that D1.
+		/// Convert the closest point into 3D space, and calculate the distance to the
+		/// query point along the plane's normal, call that D2.
+		/// The final cost for a given point is D1 + D2 * distanceScaleAlongProjectionDirection.
+		///
+		/// This will form a diamond shape of equivalent cost points around the query point (x).
+		/// The ratio of the width of this diamond to the height is equal to distanceScaleAlongProjectionDirection.
+		///
+		///     ^
+		///    / \
+		///   /   \
+		///  /  x  \
+		///  \	  /
+		///   \   /
+		///    \ /
+		///     v
+		///
+		/// See: <see cref="DistanceMetric.ClosestAsSeenFromAboveSoft(Vector3)"/>
+		/// </summary>
+		/// <param name="vi1">First vertex of the triangle, in graph space.</param>
+		/// <param name="vi2">Second vertex of the triangle, in graph space.</param>
+		/// <param name="vi3">Third vertex of the triangle, in graph space.</param>
+		/// <param name="projection">Projection parameters that are for example constructed from a movement plane.</param>
+		/// <param name="point">Point to find the closest point to.</param>
+		/// <param name="closest">Closest point on the triangle to the point.</param>
+		/// <param name="sqrDist">Squared cost from the point to the closest point on the triangle.</param>
+		/// <param name="distAlongProjection">Distance from the point to the closest point on the triangle along the projection axis.</param>
+		[BurstCompile]
+		public static void ClosestPointOnTriangleProjected (ref Int3 vi1, ref Int3 vi2, ref Int3 vi3, ref BBTree.ProjectionParams projection, ref float3 point, [NoAlias] out float3 closest, [NoAlias] out float sqrDist, [NoAlias] out float distAlongProjection) {
+			var v1 = (float3)vi1;
+			var v2 = (float3)vi2;
+			var v3 = (float3)vi3;
+			var v1proj = math.mul(projection.planeProjection, v1);
+			var v2proj = math.mul(projection.planeProjection, v2);
+			var v3proj = math.mul(projection.planeProjection, v3);
+			// TODO: Can be cached
+			var pointProj = math.mul(projection.planeProjection, point);
+			var closestBarycentric = ClosestPointOnTriangleBarycentric(v1proj, v2proj, v3proj, pointProj);
+			closest = v1*closestBarycentric.x + v2*closestBarycentric.y + v3*closestBarycentric.z;
+			var closestProj = v1proj*closestBarycentric.x + v2proj*closestBarycentric.y + v3proj*closestBarycentric.z;
+			distAlongProjection = math.abs(math.dot(closest - point, projection.projectionAxis));
+			var distInPlane = math.length(closestProj - pointProj);
+			if (distInPlane < 0.01f) {
+				// If we are very close to being inside the triangle,
+				// check if we are actually inside the triangle using a more numerically robust method.
+				// If we are, set the in-plane-distance to 0.
+				// This is particularly important if distanceScaleAlongProjectionAxis is zero,
+				// as otherwise tie breaking may not work due to numerical issues.
+				var ci1 = (int3)vi1;
+				var ci2 = (int3)vi2;
+				var ci3 = (int3)vi3;
+				// wow, ugly
+				var pi = (int3)(Int3)(Vector3)point;
+				if (ContainsPoint(ref ci1, ref ci2, ref ci3, ref pi, in projection.planeProjection)) {
+					distInPlane = 0;
+				}
+			}
+			var dist = distInPlane + distAlongProjection*projection.distanceScaleAlongProjectionAxis;
+			sqrDist = dist*dist;
+		}
+
+		/// <summary>Cached dictionary to avoid excessive allocations</summary>
+		static readonly Dictionary<Int3, int> cached_Int3_int_dict = new Dictionary<Int3, int>();
+
+		/// <summary>
+		/// Compress the mesh by removing duplicate vertices.
+		///
+		/// Vertices that differ by only 1 along the y coordinate will also be merged together.
+		/// Warning: This function is not threadsafe. It uses some cached structures to reduce allocations.
+		/// </summary>
+		/// <param name="vertices">Vertices of the input mesh</param>
+		/// <param name="triangles">Triangles of the input mesh</param>
+		/// <param name="tags">Tags of the input mesh. One for each triangle.</param>
+		/// <param name="outVertices">Vertices of the output mesh.</param>
+		/// <param name="outTriangles">Triangles of the output mesh.</param>
+		/// <param name="outTags">Tags of the output mesh. One for each triangle.</param>
+		public static void CompressMesh (List<Int3> vertices, List<int> triangles, List<uint> tags, out Int3[] outVertices, out int[] outTriangles, out uint[] outTags) {
+			Dictionary<Int3, int> firstVerts = cached_Int3_int_dict;
+
+			firstVerts.Clear();
+
+			// Use cached array to reduce memory allocations
+			int[] compressedPointers = ArrayPool<int>.Claim(vertices.Count);
+
+			// Map positions to the first index they were encountered at
+			int count = 0;
+			for (int i = 0; i < vertices.Count; i++) {
+				// Check if the vertex position has already been added
+				// Also check one position up and one down because rounding errors can cause vertices
+				// that should end up in the same position to be offset 1 unit from each other
+				// TODO: Check along X and Z axes as well?
+				int ind;
+				if (!firstVerts.TryGetValue(vertices[i], out ind) && !firstVerts.TryGetValue(vertices[i] + new Int3(0, 1, 0), out ind) && !firstVerts.TryGetValue(vertices[i] + new Int3(0, -1, 0), out ind)) {
+					firstVerts.Add(vertices[i], count);
+					compressedPointers[i] = count;
+					vertices[count] = vertices[i];
+					count++;
+				} else {
+					compressedPointers[i] = ind;
+				}
+			}
+
+			// Create the triangle array or reuse the existing buffer
+			outTriangles = new int[triangles.Count];
+
+			// Remap the triangles to the new compressed indices
+			for (int i = 0; i < outTriangles.Length; i++) {
+				outTriangles[i] = compressedPointers[triangles[i]];
+			}
+
+			// Create the vertex array or reuse the existing buffer
+			outVertices = new Int3[count];
+
+			for (int i = 0; i < count; i++)
+				outVertices[i] = vertices[i];
+
+			ArrayPool<int>.Release(ref compressedPointers);
+
+			outTags = tags.ToArray();
+		}
+
+		/// <summary>
+		/// Given a set of edges between vertices, follows those edges and returns them as chains and cycles.
+		///
+		/// [Open online documentation to see images]
+		/// </summary>
+		/// <param name="outline">outline[a] = b if there is an edge from a to b.</param>
+		/// <param name="hasInEdge">hasInEdge should contain b if outline[a] = b for any key a.</param>
+		/// <param name="results">Will be called once for each contour with the contour as a parameter as well as a boolean indicating if the contour is a cycle or a chain (see image).</param>
+		public static void TraceContours (Dictionary<int, int> outline, HashSet<int> hasInEdge, System.Action<List<int>, bool> results) {
+			// Iterate through chains of the navmesh outline.
+			// I.e segments of the outline that are not loops
+			// we need to start these at the beginning of the chain.
+			// Then iterate over all the loops of the outline.
+			// Since they are loops, we can start at any point.
+			var obstacleVertices = ListPool<int>.Claim();
+			var outlineKeys = ListPool<int>.Claim();
+
+			outlineKeys.AddRange(outline.Keys);
+			for (int k = 0; k <= 1; k++) {
+				bool cycles = k == 1;
+				for (int i = 0; i < outlineKeys.Count; i++) {
+					var startIndex = outlineKeys[i];
+
+					// Chains (not cycles) need to start at the start of the chain
+					// Cycles can start at any point
+					if (!cycles && hasInEdge.Contains(startIndex)) {
+						continue;
+					}
+
+					var index = startIndex;
+					obstacleVertices.Clear();
+					obstacleVertices.Add(index);
+
+					while (outline.ContainsKey(index)) {
+						var next = outline[index];
+						outline.Remove(index);
+
+						obstacleVertices.Add(next);
+
+						// We traversed a full cycle
+						if (next == startIndex) break;
+
+						index = next;
+					}
+
+					if (obstacleVertices.Count > 1) {
+						results(obstacleVertices, cycles);
+					}
+				}
+			}
+
+			ListPool<int>.Release(ref outlineKeys);
+			ListPool<int>.Release(ref obstacleVertices);
+		}
+
+		/// <summary>Divides each segment in the list into subSegments segments and fills the result list with the new points</summary>
+		public static void Subdivide (List<Vector3> points, List<Vector3> result, int subSegments) {
+			for (int i = 0; i < points.Count-1; i++)
+				for (int j = 0; j < subSegments; j++)
+					result.Add(Vector3.Lerp(points[i], points[i+1], j / (float)subSegments));
+
+			result.Add(points[points.Count-1]);
+		}
+	}
+}