public bool Step(out OrientedPoint orientedPoint)
{
float alpha = Distance / curve.Length;
float speedLow = speedInitial + alpha * speedFinal;
float increment = speedLow * Time.deltaTime;
distance += increment;
// Recompute index
while (index + 1 < curve.Count)
{
if (Distance < curve.distances[index + 1])
{
break;
}
index++;
}
// Return appropriately
if (index + 1 < curve.Count)
{
OrientedPoint a = curve[index];
OrientedPoint b = curve[index + 1];
float t = (Distance - curve.distances[index]) / curve.deltas[index + 1];
// Compue the oriented point
Vector3 position = Vector3.Lerp(a.position, b.position, t);
Quaternion rotation = Quaternion.Slerp(a.rotation, b.rotation, t);
orientedPoint = new OrientedPoint(position, rotation);
return(true);
}
orientedPoint = default;
return(false);
}
/// <summary>
/// Performs Bezier interpolation in R3 with the specified point count.
/// </summary>
/// <param name="controlPointA">The first control point to use.</param>
/// <param name="controlPointB">The second control point to use.</param>
/// <param name="pointCount">The desired point count.</param>
/// <returns>The computed Bezier curve.</returns>
public static Curve Evaluate(ControlPoint controlPointA, ControlPoint controlPointB, int pointCount, bool use2DMode = false)
{
// Precompute t-parameter increment
float t = 0.0f;
float increment = 1.0f / (pointCount - 1);
// Helper variables
Vector3 a = controlPointA.position;
Vector3 b = controlPointA.ForwardHandler;
Vector3 c = controlPointB.BackHandler;
Vector3 d = controlPointB.position;
CalculusHelper helper = new CalculusHelper(a, b, c, d);
// Compute each oriented point
List <OrientedPoint> orientedPoints = new List <OrientedPoint>(pointCount);
for (int i = 0; i < pointCount; i++, t += increment)
{
// Optimization variables
float opt = 1.0f - t;
float optSqr = opt * opt;
float tSqr = t * t;
// Compute point vectors
Vector3 position =
optSqr * opt * a
+ 3.0f * optSqr * t * b
+ 3.0f * opt * tSqr * c
+ tSqr * t * d;
Vector3 tangent = helper.CalculateTangent(t);
Vector3 normal = helper.CalculateNormal(t);
Vector3 up;
if (use2DMode)
{
up = Vector2.Perpendicular(tangent);
}
else
{
up = Vector3.Lerp(controlPointA.normal, controlPointB.normal, t);
}
Quaternion rotation = Quaternion.LookRotation(tangent, up);
// Calculate curvature
Vector3 radius = helper.CalculateRadius(t);
// Add the oriented point to the list
OrientedPoint orientedPoint = new OrientedPoint(position, rotation, radius.magnitude);
orientedPoint.radius = radius;
orientedPoints.Add(orientedPoint);
//Debug.DrawRay(position, tangent, Color.cyan);
//Debug.DrawRay(position, normal, Color.magenta);
//Debug.DrawRay(position, orientedPoint.radius, Color.yellow);
//Debug.DrawRay(position, Vector3.Project(orientedPoint.radius, orientedPoint.TransformDirection(Vector3.right)), Color.red);
//Debug.DrawRay(position, Vector3.Project(orientedPoint.radius, orientedPoint.TransformDirection(Vector3.up)), Color.green);
//Debug.DrawRay(position, Vector3.Project(orientedPoint.radius, orientedPoint.TransformDirection(Vector3.forward)), Color.blue);
}
return(new Curve(orientedPoints));
}