贝塞尔曲线 实现路径设定曲线行走

作者:雨辰 发布于:2017-2-9 14:37 Thursday 分类:Unity3D

using UnityEngine;
using System.Collections;
using Pathfinding;

public class BezierMover : MonoBehaviour
{
    public Transform[] points;
    public float tangentLengths = 5;
    public float speed = 1;

    float time = 0;
    void LateUpdate()
    {
        Move();
    }

    Vector3 Plot(float t)
    {
        Vector3 inTang, outTang;
        int c = points.Length;
        int pt = Mathf.FloorToInt(t);
        inTang = ((points[(pt + 1) % c].position - points[(pt + 0) % c].position).normalized - (points[(pt - 1 + c) % c].position - points[(pt + 0) % c].position).normalized).normalized;
        outTang = ((points[(pt + 2) % c].position - points[(pt + 1) % c].position).normalized - (points[(pt - 0 + c) % c].position - points[(pt + 1) % c].position).normalized).normalized;

        Debug.DrawLine(points[pt % c].position, points[pt % c].position + inTang * tangentLengths, Color.red);
        Debug.DrawLine(points[(pt + 1) % c].position - outTang * tangentLengths, points[(pt + 1) % c].position, Color.green);

        return AstarMath.CubicBezier(points[pt % c].position, points[pt % c].position + inTang * tangentLengths, points[(pt + 1) % c].position - outTang * tangentLengths, points[(pt + 1) % c].position, t - pt);
    }

    // Update is called once per frame
    void Move()
    {
        float mn = time;
        float mx = time + 1;
        while (mx - mn > 0.0001f)
        {
            float mid = (mn + mx) / 2;
            Vector3 p = Plot(mid);
            if ((p - transform.position).sqrMagnitude > (speed * Time.deltaTime) * (speed * Time.deltaTime))
            {
                mx = mid;
            }
            else
            {
                mn = mid;
            }
        }

        time = (mn + mx) / 2;
        Vector3 p1 = Plot(time);
        Vector3 p2 = Plot(time + 0.001f);
        transform.position = p1;
        transform.rotation = Quaternion.LookRotation(p2 - p1);
    }

    public void OnDrawGizmos()
    {
        if (points.Length >= 3)
        {
            for (int i = 0; i < points.Length; i++) if (points[i] == null) return;
            for (int pt = 0; pt < points.Length; pt++)
            {
                int c = points.Length;
                Vector3 inTang = ((points[(pt + 1) % c].position - points[pt + 0].position).normalized - (points[(pt - 1 + c) % c].position - points[pt + 0].position).normalized).normalized;
                Vector3 outTang = ((points[(pt + 2) % c].position - points[(pt + 1) % c].position).normalized - (points[(pt - 0 + c) % c].position - points[(pt + 1) % c].position).normalized).normalized;
                Vector3 pp = points[pt].position;
                for (int i = 1; i <= 100; i++)
                {
                    Vector3 p = AstarMath.CubicBezier(points[pt].position, points[pt].position + inTang * tangentLengths, points[(pt + 1) % c].position - outTang * tangentLengths, points[(pt + 1) % c].position, i / 100.0f);
                    Gizmos.DrawLine(pp, p);
                    pp = p;
                }
            }
        }
    }

}

 

---------------------------------------------------------------AstarMath----------------------------------------- 

using UnityEngine;
using Pathfinding;
using System;
using System.Collections.Generic;

namespace Pathfinding {

	/** Contains various spline functions.
	 * \ingroup utils
	 */
	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);
		}

		[System.Obsolete("Use CatmullRom")]
		public static Vector3 CatmullRomOLD (Vector3 previous,Vector3 start, Vector3 end, Vector3 next, float elapsedTime) {
			return CatmullRom(previous, start, end, next, elapsedTime);
		}
	}

	/** Utility functions for working with numbers, lines and vectors.
	 * \ingroup utils
	  * \see Polygon */
	public static class AstarMath {

		/** Returns a hash value for a integer vector.
		 * \author Code got from the internet */
		public static int ComputeVertexHash (int x, int y, int z) {
			const uint h1 = 0x8da6b343; // Large multiplicative constants;
			const uint h2 = 0xd8163841; // here arbitrarily chosen primes
			const 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.Epsilon ? lineDirection/magn : Vector3.zero;

			float closestPoint = Vector3.Dot((point-lineStart),lineDirection); //Vector3.Dot(lineDirection,lineDirection);
			return closestPoint / magn;
		}

		/** 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 got by (end-start)*factor + start;
		 */
		public static float NearestPointFactor (Int3 lineStart, Int3 lineEnd, Int3 point)
		{
			Int3 lineDirection = lineEnd-lineStart;
			float magn = lineDirection.sqrMagnitude;

			float closestPoint = Int3.Dot((point-lineStart),lineDirection); //Vector3.Dot(lineDirection,lineDirection);
			if (magn != 0) closestPoint /= magn;

			return closestPoint;
			//return closestPoint / magn;
		}

		/** 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 got by (end-start)*factor + start;
		 */
		public static float NearestPointFactor (Int2 lineStart, Int2 lineEnd, Int2 point)
		{
			Int2 lineDirection = lineEnd-lineStart;
			double magn = lineDirection.sqrMagnitudeLong;

			double closestPoint = Int2.DotLong(point-lineStart,lineDirection); //Vector3.Dot(lineDirection,lineDirection);
			if (magn != 0) closestPoint /= magn;

			return (float)closestPoint;
			//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;
			float magn = fullDirection.magnitude;
			Vector3 lineDirection = magn > float.Epsilon ? fullDirection/magn : Vector3.zero;

			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,magn)*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;
			float magn = fullDirection2.magnitude;
			Vector3 lineDirection = magn > float.Epsilon ? fullDirection2/magn : Vector3.zero;
			//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 squared 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;
		}

		/** Returns the approximate shortest squared 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 (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;
		}

		/** 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) {

			var p = new Vector3 (x,0,z);

			var p1 = new Vector3 (px,0,pz);
			var 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 t2*t2*t2 * p0 + 3 * t2*t2 * t * p1 + 3 * t2 * t*t * p2 + t*t*t * 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)+" bytes";
			}
			if (bytes < 1000000) {
				return ((bytes/1000D)*sign).ToString ("0.0") + " kb";
			}
			if (bytes < 1000000000) {
				return ((bytes/1000000D)*sign).ToString ("0.0") +" mb";
			}
			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)+" bytes";
			}
			if (bytes < 1024*1024) {
				return ((bytes/1024D)*sign).ToString ("0.0") + " kb";
			}
			if (bytes < 1024*1024*1024) {
				return ((bytes/(1024D*1024D))*sign).ToString ("0.0") +" mb";
			}
			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
		 */
		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) {
			return a < 0 ? -a : a;
		}

		public static int Abs (int a) {
			return a < 0 ? -a : a;
		}

		public static float Min (float a, float b) {
			return a < b ? a : b;
		}

		public static int Min (int a, int b) {
			return a < b ? a : b;
		}

		public static uint Min (uint a, uint b) {
			return a < b ? a : b;
		}

		public static float Max (float a, float b) {
			return a > b ? a : b;
		}

		public static int Max (int a, int b) {
			return a > b ? a : b;
		}

		public static uint Max (uint a, uint b) {
			return a > b ? a : b;
		}

		public static ushort Max (ushort a, ushort b) {
			return a > b ? a : b;
		}

		public static float Sign (float a) {
			return a < 0 ? -1F : 1F;
		}

		public static int Sign (int a) {
			return a < 0 ? -1 : 1;
		}

		public static float Clamp (float a, float b, float c) {
			return a > c ? c : a < b ? b : a;
		}

		public static int Clamp (int a, int b, int c) {
			return a > c ? c : a < b ? b : a;
		}

		public static float Clamp01 (float a) {
			return a > 1 ? 1 : a < 0 ? 0 : a;
		}

		public static int Clamp01 (int a) {
			return a > 1 ? 1 : a < 0 ? 0 : a;
		}

		public static float Lerp (float a,float b, float t) {
			return a + (b-a)*(t > 1 ? 1 : t < 0 ? 0 : t);
		}

		public static int RoundToInt (float v) {
			return (int)(v+0.5F);
		}

		public static int RoundToInt (double v) {
			return (int)(v+0.5D);
		}

	}

	/** 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.
	  * \ingroup utils */
	public static class Polygon {

		/** 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 */
		public static long TriangleArea2 (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);

		}

		/** 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.
		 */
		public static float TriangleArea2 (Vector3 a, Vector3 b, Vector3 c) {
			return (b.x - a.x) * (c.z - a.z) - (c.x - a.x) * (b.z - a.z);
		}

		/** 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.
		 * This method can handle larger numbers than TriangleArea2(Int3)
		 */
		[System.Obsolete("Use TriangleArea2 instead to avoid confusion regarding the factor 2")]
		public static long TriangleArea (Int3 a, Int3 b, Int3 c) {
			return TriangleArea2(a, b, c);
		}

		/** 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.
		 * Idential to TriangleArea2(Vector3), kept for compability.
		 */
		[System.Obsolete("Use TriangleArea2 instead to avoid confusion regarding the factor 2")]
		public static float TriangleArea (Vector3 a, Vector3 b, Vector3 c) {
			return TriangleArea2(a, b, c);
		}

		/** Returns if the triangle \a ABC contains the point \a p in XZ space */
		public static bool ContainsPoint (Vector3 a, Vector3 b, Vector3 c, Vector3 p) {
			return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p);
		}

		/** Returns if the triangle \a ABC contains the point \a p */
		public static bool ContainsPoint (Int2 a, Int2 b, Int2 c, Int2 p) {
			return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p);
		}

		/** Returns if the triangle \a ABC contains the point \a p */
		public static bool ContainsPoint (Int3 a, Int3 b, Int3 c, Int3 p) {
			return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p);
		}

		/** Checks if \a p is inside the polygon.
		 * \author http://unifycommunity.com/wiki/index.php?title=PolyContainsPoint (Eric5h5)
		 */
		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;
		}

		/** Checks if \a p is inside the polygon (XZ space)
		 * \author http://unifycommunity.com/wiki/index.php?title=PolyContainsPoint (Eric5h5)
		 */
		public static bool ContainsPoint (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;
		}

		/** Returns if \a p lies on the left side of the line \a a - \a b. Uses XZ space.
		  * Does not return true if the points are colinear. */
		public static bool LeftNotColinear (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;
		}

		/** Returns if \a p lies on the left side of the line \a a - \a b. Uses XZ space. Also returns true if the points are colinear */
		public static bool Left (Vector3 a, Vector3 b, Vector3 p) {
			return (b.x - a.x) * (p.z - a.z) - (p.x - a.x) * (b.z - a.z) <= 0;
		}

		/** Returns if \a p lies on the left side of the line \a a - \a b. Also returns true if the points are colinear */
		public static bool Left (Vector2 a, Vector2 b, Vector2 p) {
			return (b.x - a.x) * (p.y - a.y) - (p.x - a.x) * (b.y - a.y) <= 0;
		}

		/** Returns if \a p lies on the left side of the line \a a - \a b. Uses XZ space. Also returns true if the points are colinear */
		public static bool Left (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;
		}

		/** Returns if \a p lies on the left side of the line \a a - \a b. Uses XZ space. */
		public static bool LeftNotColinear (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;
		}

		/** Returns if \a p lies on the left side of the line \a a - \a b. Also returns true if the points are colinear */
		public static bool Left (Int2 a, Int2 b, Int2 c) {
			return (long)(b.x - a.x) * (long)(c.y - a.y) - (long)(c.x - a.x) * (long)(b.y - a.y) <= 0;
		}

		/** 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) */
		public static bool IsClockwiseMargin (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;
		}

		/** Returns if the points a in a clockwise order */
		public static bool IsClockwise (Vector3 a, Vector3 b, Vector3 c) {
			return (b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z) < 0;
		}

		/** Returns if the points a in a clockwise order */
		public static bool IsClockwise (Int3 a, Int3 b, Int3 c) {
			return LeftNotColinear(a, b, c);
		}

		/** Returns true if the points a in a clockwise order or if they are colinear */
		public static bool IsClockwiseMargin (Int3 a, Int3 b, Int3 c) {
			return Left(a, b, c);
		}

		/** Returns true if the points a in a clockwise order or if they are colinear */
		public static bool IsClockwiseMargin (Int2 a, Int2 b, Int2 c) {
			return Left(a, b, c);
		}

		/** Returns if the points are colinear (lie on a straight line) */
		public static bool IsColinear (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;
		}

		/** Returns if the points are colinear (lie on a straight line) */
		public static bool IsColinearAlmost (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;
		}

		/** Returns if the points are colinear (lie on a straight line) */
		public static bool IsColinear (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 though, just that float.Epsilon was a bit too small.
			return v <= 0.0000001f && v >= -0.0000001f;
		}

		/** Returns if the line segment \a a2 - \a b2 intersects the infinite line \a a - \a b. a-b is infinite, a2-b2 is not infinite */
		public static bool IntersectsUnclamped (Vector3 a, Vector3 b, Vector3 a2, Vector3 b2) {
			return Left (a,b,a2) != Left (a,b,b2);
		}

		/** Returns if the line segment \a a2 - \a b2 intersects the line segment \a a - \a b.
		 * If only the endpoints coincide, the result is undefined (may be true or false).
		 */
		public static bool Intersects (Int2 a, Int2 b, Int2 a2, Int2 b2) {
			return Left (a,b,a2) != Left (a,b,b2) && Left (a2,b2,a) != Left (a2,b2,b);
		}

		/** Returns if the line segment \a a2 - \a b2 intersects the line segment \a a - \a b.
		 * If only the endpoints coincide, the result is undefined (may be true or false).
		 *
		 * \note XZ space
		 */
		public static bool Intersects (Int3 a, Int3 b, Int3 a2, Int3 b2) {
			return Left (a,b,a2) != Left (a,b,b2) && Left (a2,b2,a) != Left (a2,b2,b);
		}

		/** Returns if the two line segments intersects. The lines are NOT treated as infinite (just for clarification)
		  * \see IntersectionPoint
		  */
		public static bool Intersects (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;
		}

		/** Intersection point between two infinite lines.
		 * Lines are treated as infinite. If the lines are parallel 'start1' will be returned. Intersections are calculated on the XZ plane.
		 */
		public static Vector3 IntersectionPointOptimized (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;
		}

		/** Intersection point between two infinite lines.
		 * Lines are treated as infinite. If the lines are parallel 'start1' will be returned. Intersections are calculated on the XZ plane.
		 */
		public static Vector3 IntersectionPointOptimized (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;
		}

		/** 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
		 */
		public static bool IntersectionFactorRaySegment (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;
			}
			//factor1 = (float)nom/den;
			//factor2 = (float)nom2/den;
			return true;
		}

		/** Returns the intersection factors for line 1 and line 2. The intersection factors is a distance along the line \a start - \a end where the other line intersects it.\n
		 * \code intersectionPoint = start1 + factor1 * (end1-start1) \endcode
		 * \code intersectionPoint2 = start2 + factor2 * (end2-start2) \endcode
		 * Lines are treated as infinite.\n
		 * false is returned if the lines are parallel and true if they are not.
		 * Only the XZ coordinates are used.
		 */
		public static bool IntersectionFactor (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;
		}

		/** Returns the intersection factors for line 1 and line 2. The intersection factors is a distance along the line \a start - \a end where the other line intersects it.\n
		 * \code intersectionPoint = start1 + factor1 * (end1-start1) \endcode
		 * \code intersectionPoint2 = start2 + factor2 * (end2-start2) \endcode
		 * Lines are treated as infinite.\n
		 * false is returned if the lines are parallel and true if they are not.
		 * Only the XZ coordinates are used.
		 */
		public static bool IntersectionFactor (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;
		}

		/** Returns the intersection factor for line 1 with ray 2.
		 * The intersection factors is a factor distance along the line \a start - \a end where the other line intersects it.\n
		 * \code intersectionPoint = start1 + factor * (end1-start1) \endcode
		 * Lines are treated as infinite.\n
		 *
		 * 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. */
		public static float IntersectionFactorRay (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;
		}

		/** Returns the intersection factor for line 1 with line 2.
		 * The intersection factor is a distance along the line \a start1 - \a end1 where the line \a start2 - \a end2 intersects it.\n
		 * \code intersectionPoint = start1 + intersectionFactor * (end1-start1) \endcode.
		 * Lines are treated as infinite.\n
		 * -1 is returned if the lines are parallel (note that this is a valid return value if they are not parallel too) */
		public static float IntersectionFactor (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;
		}

		/** Returns the intersection point between the two lines. Lines are treated as infinite. \a start1 is returned if the lines are parallel */
		public static Vector3 IntersectionPoint (Vector3 start1, Vector3 end1, Vector3 start2, Vector3 end2) {
			bool s;
			return IntersectionPoint (start1,end1,start2,end2, out s);
		}

		/** Returns the intersection point between the two lines. Lines are treated as infinite. \a start1 is returned if the lines are parallel */
		public static Vector3 IntersectionPoint (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;
		}

		/** Returns the intersection point between the two lines. Lines are treated as infinite. \a start1 is returned if the lines are parallel */
		public static Vector2 IntersectionPoint (Vector2 start1, Vector2 end1, Vector2 start2, Vector2 end2) {
			bool s;
			return IntersectionPoint (start1,end1,start2,end2, out s);
		}

		/** Returns the intersection point between the two lines. Lines are treated as infinite. \a start1 is returned if the lines are parallel */
		public static Vector2 IntersectionPoint (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;
		}

		/** Returns the intersection point between the two line segments in XZ space.
		 * Lines are NOT treated as infinite. \a 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).
		 */
		public static Vector3 SegmentIntersectionPoint (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;
		}

		public static List<Vector3> hullCache = new List<Vector3>();

		/** 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 \a n is the number of points and \a h is the number of points on the hull,
		  * so it is in the worst case quadratic.
		  */
		public static Vector3[] ConvexHull (Vector3[] points) {

			if (points.Length == 0) return new Vector3[0];

			lock (hullCache) {
				List<Vector3> hull = hullCache;
				hull.Clear ();

	#if ASTARDEBUG
				for (int i=0;i<points.Length;i++) {
					Debug.DrawLine (points[i],points[(i+1)%points.Length],Color.blue);
				}
	#endif

				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 || !Left (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);

				return hull.ToArray ();
			}
		}

		/** 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
		 */
		public static bool LineIntersectsBounds (Bounds bounds, Vector3 a, Vector3 b) {
			// Put line 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 line 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;
		}

		/** Subdivides \a path and returns the new array with interpolated values.
		 * The returned array is \a path subdivided \a subdivisions times, the resulting points are interpolated using Mathf.SmoothStep.\n
		 * If \a subdivisions is less or equal to 0 (zero), the original array will be returned */
		public static Vector3[] Subdivide (Vector3[] path, int subdivisions) {

			subdivisions = subdivisions < 0 ? 0 : subdivisions;

			if (subdivisions == 0) {
				return path;
			}

			var path2 = new Vector3[(path.Length-1)*(int)Mathf.Pow (2,subdivisions)+1];

			int c = 0;
			for (int p=0;p<path.Length-1;p++) {
				float step = 1.0F/Mathf.Pow (2,subdivisions);

				for (float i=0;i<1.0F;i+=step) {
					path2[c] = Vector3.Lerp (path[p],path[p+1],Mathf.SmoothStep (0,1, i));
					c++;
				}
			}

			path2[c] = path[path.Length-1];
			return path2;
		}

		/** Returns the closest point on the triangle. The \a triangle array must have a length of at least 3.
		 * \see ClosesPointOnTriangle(Vector3,Vector3,Vector3,Vector3);
		 */
		public static Vector3 ClosestPointOnTriangle ( Vector3[] triangle, Vector3 point ) {
			return ClosestPointOnTriangle (triangle[0],triangle[1],triangle[2],point);
		}

		/** Returns the closest point on the triangle.
		 *
		 * \author Got code from the internet, changed a bit to work with the Unity API
		 *
		 */
		public static Vector3 ClosestPointOnTriangle(Vector3 tr0, Vector3 tr1, Vector3 tr2, Vector3 point)
		{
			Vector3 diff = tr0 - point;
			Vector3 edge0 = tr1 - tr0;
			Vector3 edge1 = tr2 - tr0;
			float a00 = edge0.sqrMagnitude;
			float a01 = Vector3.Dot(edge0, edge1);
			float a11 = edge1.sqrMagnitude;
			float b0 = Vector3.Dot(diff, edge0);
			float b1 = Vector3.Dot(diff, edge1);
			//float c = diff.sqrMagnitude;
			float det = a00 * a11 - a01 * a01;

			float s = a01 * b1 - a11 * b0;
			float t = a01 * b0 - a00 * b1;
			//float sqrDistance;

			if (s + t <= det)
			{
				if (s < 0f)
				{
					if (t < 0f)  // region 4
					{
						if (b0 < 0f)
						{
							t = 0f;
							if (-b0 >= a00)
							{
								s = 1f;
								//sqrDistance = a00 + (2f) * b0 + c;
							}
							else
							{
								s = -b0 / a00;
								//sqrDistance = b0 * s + c;
							}
						}
						else
						{
							s = 0f;
							if (b1 >= 0f)
							{
								t = 0f;
								//sqrDistance = c;
							}
							else if (-b1 >= a11)
							{
								t = 1f;
								//sqrDistance = a11 + (2f) * b1 + c;
							}
							else
							{
								t = -b1 / a11;
								//sqrDistance = b1 * t + c;
							}
						}
					}
					else  // region 3
					{
						s = 0f;
						if (b1 >= 0f)
						{
							t = 0f;
							//sqrDistance = c;
						}
						else if (-b1 >= a11)
						{
							t = 1f;
							//sqrDistance = a11 + (2f) * b1 + c;
						}
						else
						{
							t = -b1 / a11;
							// sqrDistance = b1 * t + c;
						}
					}
				}
				else if (t < 0f)  // region 5
				{
					t = 0f;
					if (b0 >= 0f)
					{
						s = 0f;
						// sqrDistance = c;
					}
					else if (-b0 >= a00)
					{
						s = 1f;
						//sqrDistance = a00 + (2f) * b0 + c;
					}
					else
					{
						s = -b0 / a00;
						//  sqrDistance = b0 * s + c;
					}
				}
				else  // region 0
				{
					// minimum at interior point
					float invDet = 1f / det;
					s *= invDet;
					t *= invDet;
					// sqrDistance = s * (a00 * s + a01 * t + (2f) * b0) + t * (a01 * s + a11 * t + (2f) * b1) + c;
				}
			}
			else
			{
				float tmp0, tmp1, numer, denom;

				if (s < 0f)  // region 2
				{
					tmp0 = a01 + b0;
					tmp1 = a11 + b1;
					if (tmp1 > tmp0)
					{
						numer = tmp1 - tmp0;
						denom = a00 - (2f) * a01 + a11;
						if (numer >= denom)
						{
							s = 1f;
							t = 0f;
							// sqrDistance = a00 + (2f) * b0 + c;
						}
						else
						{
							s = numer / denom;
							t = 1f - s;
							// sqrDistance = s * (a00 * s + a01 * t + (2f) * b0) + t * (a01 * s + a11 * t + (2f) * b1) + c;
						}
					}
					else
					{
						s = 0f;
						if (tmp1 <= 0f)
						{
							t = 1f;
							// sqrDistance = a11 + (2f) * b1 + c;
						}
						else if (b1 >= 0f)
						{
							t = 0f;
							//  sqrDistance = c;
						}
						else
						{
							t = -b1 / a11;
							//  sqrDistance = b1 * t + c;
						}
					}
				}
				else if (t < 0f)  // region 6
				{
					tmp0 = a01 + b1;
					tmp1 = a00 + b0;
					if (tmp1 > tmp0)
					{
						numer = tmp1 - tmp0;
						denom = a00 - (2f) * a01 + a11;
						if (numer >= denom)
						{
							t = 1f;
							s = 0f;
							// sqrDistance = a11 + (2f) * b1 + c;
						}
						else
						{
							t = numer / denom;
							s = 1f - t;
							// sqrDistance = s * (a00 * s + a01 * t + (2f) * b0) + t * (a01 * s + a11 * t + (2f) * b1) + c;
						}
					}
					else
					{
						t = 0f;
						if (tmp1 <= 0f)
						{
							s = 1f;
							//sqrDistance = a00 + (2f) * b0 + c;
						}
						else if (b0 >= 0f)
						{
							s = 0f;
							// sqrDistance = c;
						}
						else
						{
							s = -b0 / a00;
							//  sqrDistance = b0 * s + c;
						}
					}
				}
				else  // region 1
				{
					numer = a11 + b1 - a01 - b0;
					if (numer <= 0f)
					{
						s = 0f;
						t = 1f;
						//  sqrDistance = a11 + (2f) * b1 + c;
					}
					else
					{
						denom = a00 - (2f) * a01 + a11;
						if (numer >= denom)
						{
							s = 1f;
							t = 0f;
							//  sqrDistance = a00 + (2f) * b0 + c;
						}
						else
						{
							s = numer / denom;
							t = 1f - s;
							// sqrDistance = s * (a00 * s + a01 * t + (2f) * b0) + t * (a01 * s + a11 * t + (2f) * b1) + c;
						}
					}
				}
			}

			// Account for numerical round-off error.
			//  if (sqrDistance < 0f)
			//  {
			//       sqrDistance = 0f;
			//    }

			return tr0 + s * edge0 + t * edge1;
		}

		/** Get the 3D minimum distance between 2 segments
		* Input:  two 3D line segments S1 and S2
		* \returns the shortest squared distance between S1 and S2
		*/
		public static float DistanceSegmentSegment3D (Vector3 s1, Vector3 e1, Vector3 s2, Vector3 e2) {
			Vector3   u = e1 - s1;
			Vector3   v = e2 - s2;
			Vector3   w = s1 - s2;
			float    a = Vector3.Dot(u,u);         // always >= 0
			float    b = Vector3.Dot(u,v);
			float    c = Vector3.Dot(v,v);         // always >= 0
			float    d = Vector3.Dot(u,w);
			float    e = Vector3.Dot(v,w);
			float    D = a*c - b*b;        // always >= 0
			float    sc, sN, sD = D;       // sc = sN / sD, default sD = D >= 0
			float    tc, tN, tD = D;       // tc = tN / tD, default tD = D >= 0

			// compute the line parameters of the two closest points
			if (D < 0.000001f) { // 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 = c;
			}
			else {                 // get the closest points on the infinite lines
				sN = (b*e - c*d);
				tN = (a*e - b*d);
				if (sN < 0.0f) {        // sc < 0 => the s=0 edge is visible
					sN = 0.0f;
					tN = e;
					tD = c;
				}
				else if (sN > sD) {  // sc > 1  => the s=1 edge is visible
					sN = sD;
					tN = e + b;
					tD = c;
				}
			}

			if (tN < 0.0f) {            // tc < 0 => the t=0 edge is visible
				tN = 0.0f;
				// recompute sc for this edge
				if (-d < 0.0f)
					sN = 0.0f;
				else if (-d > a)
					sN = sD;
				else {
					sN = -d;
					sD = a;
				}
			}
			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) > a)
					sN = sD;
				else {
					sN = (-d +  b);
					sD = a;
				}
			}
			// finally do the division to get sc and tc
			sc = (Math.Abs(sN) < 0.000001f ? 0.0f : sN / sD);
			tc = (Math.Abs(tN) < 0.000001f ? 0.0f : tN / tD);

			// get the difference of the two closest points
			Vector3 dP = w + (sc * u) - (tc * v);  // =  S1(sc) - S2(tc)

			return dP.sqrMagnitude;   // return the closest distance
		}

	}
}

 

标签: Unity3D

发表评论:

雨辰 joyimp|@2011-2017 京ICP备16030765号