/// <summary>
/// Gives the approximate closest point to point p. dt controls how accurate it will be.
/// </summary>
/// <param name="p">Point we are getting closest point on curve to</param>
/// <param name="bc">Bezier Component</param>
/// <param name="dt">delta controls number of iterations taken
/// and is therefore important to understand efficiency is reduced
/// as dt gets smaller...
/// </param>
/// <returns>The closesst point along curve to p</returns>
public static Vector3 ClosestPoint(Vector3 p, BezierComponent bc, float dt = 0.01f)
{
if (dt < 0.000001f)
{
return(Vector3.positiveInfinity);
}
int C = bc.controlPoints.Length;
int N = (C - C % 4) / 4;
Vector3 ccp = Evaluate(0, bc);
float csd = (p - ccp).sqrMagnitude;
float t = dt;
Vector3 cp;
float d;
while (t < N)
{
cp = Evaluate(t, bc);
d = (p - cp).sqrMagnitude;
if (d < csd)
{
ccp = cp;
csd = d;
}
t += dt;
}
return(ccp);
}
public static void DrawGizmoForCurve(BezierComponent bc, GizmoType gizmoType)
{
BezierCPComponent[] CPS = bc.controlPoints;
if (CPS == null || CPS.Length == 0)
{
return;
}
int N = CurveCount(bc);
// Draw curve
Vector3 cp;
Vector3 pp = BezierSystem.Evaluate(0, bc);
Gizmos.color = bc.curveColor;
for (float t = bc.drawDelta; t < N; t += bc.drawDelta)
{
cp = BezierSystem.Evaluate(t, bc);
Gizmos.DrawLine(pp, cp);
pp = cp;
}
cp = BezierSystem.Evaluate(N - 0.0002f, bc);
Gizmos.DrawLine(pp, cp);
// Draw CP lines
Gizmos.color = bc.tangentColor;
for (int i = 0; i < N; i++)
{
int s = 4 * i;
Gizmos.DrawLine(CPS[s].transform.position, CPS[s + 1].transform.position);
Gizmos.DrawLine(CPS[s + 2].transform.position, CPS[s + 3].transform.position);
}
}
/// <summary>
/// Gives the next param after t such that the the Length along curve
/// going from t to retured value is approximately arc length length.
/// i.e for getting a uniform speed along curve.
/// </summary>
/// <param name="t">param</param>
/// <param name="length">length to travel along curve</param>
/// <param name="crv">BezierComponent to evaluate</param>
/// <returns>new uniform length parameter</returns>
public static float UniformSpeedParam(float t, float length, BezierComponent crv)
{
Vector3 p = Evaluate(t, crv);
Vector3 tangent = TangentVector(t, crv);
t += length / tangent.magnitude;
return(t);
}
/// <summary>
/// Gives the position of the curve in World Space at parameter t
/// </summary>
/// <param name="t">
/// parameter. Domain -> [0, N)
/// where N is the number of curves appended together
/// to give the full curve. The BezierCPComponent[]
/// length must be of form: 4*i where is a natural number...
/// </param>
/// <param name="bc">The BezierComponent to evaluate</param>
/// <returns>point on curve at param t</returns>
public static Vector3 Evaluate(float t, BezierComponent bc)
{
int startIdx = -1;
t = Param(t, bc, out startIdx);
float p = 1 - t;
BezierCPComponent[] cps = bc.controlPoints;
Vector3 p0 = cps[startIdx].transform.position;
Vector3 p1 = cps[startIdx + 1].transform.position;
Vector3 p2 = cps[startIdx + 2].transform.position;
Vector3 p3 = cps[startIdx + 3].transform.position;
return((p0 * p * p * p) + (3 * p1 * t * p * p) + (3 * p2 * p * t * t) + (p3 * t * t * t));
}
/// <summary>
/// Transforms the parameter to be between [0,1]
/// and sets the startIdx so that we can index into
/// BezierCurve array and get parameter
/// </summary>
/// <param name="t">param from [0, CurveCount(BezierComponent) )</param>
/// <param name="bc">BezierComponent to eval</param>
/// <param name="startIdx">the start index into the BezierComponent.controlPoints array...</param>
/// <returns>returns normalized parameter between [0,1]</returns>
public static float Param(float t, BezierComponent bc, out int startIdx)
{
int p = (int)Mathf.Floor(t);
int N = CurveCount(bc);
if (p >= N)
{
startIdx = -1;
return(-1);
}
else
{
startIdx = p * 4;
}
return(t - p);
}
/// <summary>
/// Better to call this when evaluating the arclength for the entire curve...
/// </summary>
/// <param name="bezierData">BezierComponent data to evaluate</param>
/// <returns>Arc Length</returns>
public static float FastArcLength(BezierComponent bc)
{
float L = 0.0f;
int N = (bc.controlPoints.Length - bc.controlPoints.Length % 4) / 4;
float arclength = 0f;
for (int i = 0; i < N; i++)
{
Vector3 A = bc.controlPoints[4 * i].transform.position;
Vector3 B = bc.controlPoints[4 * i + 1].transform.position;
Vector3 C = bc.controlPoints[4 * i + 2].transform.position;
Vector3 D = bc.controlPoints[4 * i + 3].transform.position;
L = 0;
ArcLengthUtil(A, B, C, D, 5, ref L);
arclength += L;
}
return(arclength);
}
/// <summary>
/// Gives the Tangent vector to the curve
/// </summary>
/// <param name="t">param</param>
/// <param name="bc">BezierComponent to evaluate</param>
/// <returns></returns>
public static Vector3 TangentVector(float t, BezierComponent bc)
{
int startIdx = 0;
t = Param(t, bc, out startIdx);
BezierCPComponent[] cps = bc.controlPoints;
Vector3 p0 = cps[startIdx].transform.position;
Vector3 p1 = cps[startIdx + 1].transform.position;
Vector3 p2 = cps[startIdx + 2].transform.position;
Vector3 p3 = cps[startIdx + 3].transform.position;
Vector3 A = -3 * p0 + 9 * p1 - 9 * p2 + 3 * p3;
Vector3 B = 6 * p0 + -12 * p1 + 6 * p2;
Vector3 C = -3 * p0 + 3 * p1;
Vector3 tangent = (t * t * A) + (t * B) + C;
return(tangent);
}
/// <summary>
/// Returns the Arc Length of the curve.
/// Currently, its mostly a bruteforce approach and therefore
/// the paramter dt is important for efficiency. If you want
/// to evaulate the entire curve, use FastArcLength instead.
/// </summary>
/// <param name="t0">starting param</param>
/// <param name="t1">end param</param>
/// <param name="bc">BezierComponent to eval.</param>
/// <param name="dt">delta param. This controls the steps algo takes.
/// Its important to understand the lower the number, the more accurate
/// but the more expensive it becomes.
/// </param>
/// <returns>The Arclength of curve between t0 and t1</returns>
public static float ArcLength(float t0, float t1, BezierComponent bc, float dt = 0.01f)
{
float length = 0;
float t = t0 + dt;
Vector3 pp = BezierSystem.Evaluate(t0, bc);
Vector3 cp;
while (t < t1)
{
cp = BezierSystem.Evaluate(t, bc);
length += (cp - pp).magnitude;
pp = cp;
t += dt;
}
cp = BezierSystem.Evaluate(t1, bc);
length += (cp - pp).magnitude;
return(length);
}
/// <summary>
/// Gives the current count of curves.
/// Entire curve is made up of individual
/// cubic bezier curves.
/// </summary>
/// <param name="bc">BezierComponent to eval</param>
/// <returns>Curve count</returns>
public static int CurveCount(BezierComponent bc)
{
return((bc.controlPoints.Length - bc.controlPoints.Length % 4) / 4);
}