using System;
using System.Collections.Generic;
using System.Linq;
using Unity.Collections;
using Unity.Mathematics;
namespace UnityEngine.Splines
{
/// <summary>
/// A collection of methods for extracting information about <see cref="Spline"/> types.
/// </summary>
/// <remarks>
/// `SplineUtility` methods do not consider Transform values except where explicitly requested. To perform operations in world space, you can use the <see cref="SplineContainer"/> evaluate methods or build a <see cref="NativeSpline"/> with a constructor that accepts a matrix and evaluate that spline.
/// </remarks>
public static class SplineUtility
{
const int k_SubdivisionCountMin = 6;
const int k_SubdivisionCountMax = 1024;
/// <summary>
/// The default tension value used for <see cref="TangentMode.AutoSmooth"/> knots.
/// Use with <see cref="Spline.SetTangentMode(UnityEngine.Splines.TangentMode)"/> and
/// <see cref="Spline.SetAutoSmoothTension(int,float)"/> to control the curvature of the spline at control
/// points.
/// </summary>
public const float DefaultTension = 1 / 3f;
/// <summary>
/// The tension value for a Catmull-Rom type spline.
/// Use with <see cref="Spline.SetTangentMode(UnityEngine.Splines.TangentMode)"/> and
/// <see cref="Spline.SetAutoSmoothTension(int,float)"/> to control the curvature of the spline at control
/// points.
/// </summary>
//todo Deprecate in 3.0.
public const float CatmullRomTension = 1 / 2f;
/// <summary>
/// The minimum resolution allowable when unrolling a curve to hit test while picking (selecting a spline with a cursor).
///
/// Pick resolution is used when determining how many segments are required to unroll a curve. Unrolling is the
/// process of calculating a series of line segments to approximate a curve. Some functions in SplineUtility
/// allow you to specify a resolution. Lower resolution means fewer segments, while higher resolutions result
/// in more segments. Use lower resolutions where performance is critical and accuracy is not paramount. Use
/// higher resolution where a fine degree of accuracy is necessary and performance is less important.
/// </summary>
public const int PickResolutionMin = 2;
/// <summary>
/// The default resolution used when unrolling a curve to hit test while picking (selecting a spline with a cursor).
///
/// Pick resolution is used when determining how many segments are required to unroll a curve. Unrolling is the
/// process of calculating a series of line segments to approximate a curve. Some functions in SplineUtility
/// allow you to specify a resolution. Lower resolution means fewer segments, while higher resolutions result
/// in more segments. Use lower resolutions where performance is critical and accuracy is not paramount. Use
/// higher resolution where a fine degree of accuracy is necessary and performance is less important.
/// </summary>
public const int PickResolutionDefault = 4;
/// <summary>
/// The maximum resolution allowed when unrolling a curve to hit test while picking (selecting a spline with a cursor).
///
/// Pick resolution is used when determining how many segments are required to unroll a curve. Unrolling is the
/// process of calculating a series of line segments to approximate a curve. Some functions in SplineUtility
/// allow you to specify a resolution. Lower resolution means fewer segments, while higher resolutions result
/// in more segments. Use lower resolutions where performance is critical and accuracy is not paramount. Use
/// higher resolution where a fine degree of accuracy is necessary and performance is less important.
/// </summary>
public const int PickResolutionMax = 64;
/// <summary>
/// The default resolution used when unrolling a curve to draw a preview in the Scene View.
///
/// Pick resolution is used when determining how many segments are required to unroll a curve. Unrolling is the
/// process of calculating a series of line segments to approximate a curve. Some functions in SplineUtility
/// allow you to specify a resolution. Lower resolution means fewer segments, while higher resolutions result
/// in more segments. Use lower resolutions where performance is critical and accuracy is not paramount. Use
/// higher resolution where a fine degree of accuracy is necessary and performance is less important.
/// </summary>
public const int DrawResolutionDefault = 10;
/// <summary>
/// Compute interpolated position, direction and upDirection at ratio t. Calling this method to get the
/// 3 vectors is faster than calling independently EvaluatePosition, EvaluateDirection and EvaluateUpVector
/// for the same time t as it reduces some redundant computation.
/// </summary>
/// <param name="spline">The spline to interpolate.</param>
/// <param name="t">A value between 0 and 1 representing the ratio along the curve.</param>
/// <param name="position">Output variable for the float3 position at t.</param>
/// <param name="tangent">Output variable for the float3 tangent at t.</param>
/// <param name="upVector">Output variable for the float3 up direction at t.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>True if successful.</returns>
public static bool Evaluate<T>(this T spline,
float t,
out float3 position,
out float3 tangent,
out float3 upVector
) where T : ISpline
{
if (spline.Count < 1)
{
position = float3.zero;
tangent = new float3(0, 0, 1);
upVector = new float3(0, 1, 0);
return false;
}
var curveIndex = SplineToCurveT(spline, t, out var curveT);
var curve = spline.GetCurve(curveIndex);
position = CurveUtility.EvaluatePosition(curve, curveT);
tangent = CurveUtility.EvaluateTangent(curve, curveT);
upVector = spline.GetCurveUpVector(curveIndex, curveT);
return true;
}
/// <summary>
/// Computes the interpolated position for NURBS defined by order, controlPoints, and knotVector at ratio t.
/// </summary>
/// <param name="t">The value between knotVector[0] and knotVector[-1] that represents the ratio along the curve.</param>
/// <param name="controlPoints">The control points for the NURBS.</param>
/// <param name="knotVector">The knot vector for the NURBS. There must be at least order + controlPoints.Length - 1 knots.</param>
/// <param name="order">The order of the curve. For example, 4 for a cubic curve or 3 for quadratic.</param>
/// <param name="position">The output variable for the float3 position at t.</param>
/// <returns>True if successful.</returns>
public static bool EvaluateNurbs(
float t,
List<float3> controlPoints,
List<double> knotVector,
int order,
out float3 position
)
{
position = float3.zero;
if (knotVector.Count < controlPoints.Count + order - 1 || controlPoints.Count < order || t < 0 || 1 < t)
{
return false;
}
knotVector = new List<double>(knotVector);
var originalFirstKnot = knotVector[0];
var fullKnotSpan = knotVector[knotVector.Count - 1] - knotVector[0];
//normalize knots
if (knotVector[0] != 0 || knotVector[knotVector.Count - 1] != 1)
{
for (int i = 0; i < knotVector.Count; ++i)
{
knotVector[i] = (knotVector[i] - originalFirstKnot) / fullKnotSpan;
}
}
var span = order;
while (span < controlPoints.Count && knotVector[span] <= t)
{
span++;
}
span--;
var basis = SplineUtility.GetNurbsBasisFunctions(order, t, knotVector, span);
for (int i = 0; i < order; ++i)
{
position += basis[i] * controlPoints[span - order + 1 + i];
}
return true;
}
static float[] GetNurbsBasisFunctions(int degree, float t, List<double> knotVector, int span)
{
//Constructs the Basis functions at t for the nurbs curve.
//The nurbs basis function form can be found at this link under the section
//"Construction of the basis functions": https://en.wikipedia.org/wiki/Non-uniform_rational_B-spline
//This is an iterative way of computing the same thing.
var left = new float[degree];
var right = new float[degree];
var N = new float[degree];
for (int j = 0; j < degree; ++j)
{
left[j] = 0f;
right[j] = 0f;
N[j] = 1f;
}
for (int j = 1; j < degree; ++j)
{
left[j] = (float)(t - knotVector[span + 1 - j]);
right[j] = (float)(knotVector[span + j] - t);
var saved = 0f;
for (int k = 0; k < j; k++)
{
float temp = N[k] / (right[k + 1] + left[j - k]);
N[k] = saved + right[k + 1] * temp;
saved = left[j - k] * temp;
}
N[j] = saved;
}
return N;
}
/// <summary>
/// Return an interpolated position at ratio t.
/// </summary>
/// <param name="spline">The spline to interpolate.</param>
/// <param name="t">A value between 0 and 1 representing the ratio along the curve.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>A position on the spline.</returns>
public static float3 EvaluatePosition<T>(this T spline, float t) where T : ISpline
{
if (spline.Count < 1)
return float.PositiveInfinity;
var curve = spline.GetCurve(SplineToCurveT(spline, t, out var curveT));
return CurveUtility.EvaluatePosition(curve, curveT);
}
/// <summary>
/// Return an interpolated direction at ratio t.
/// </summary>
/// <param name="spline">The spline to interpolate.</param>
/// <param name="t">A value between 0 and 1 representing the ratio along the curve.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>A direction on the spline.</returns>
public static float3 EvaluateTangent<T>(this T spline, float t) where T : ISpline
{
if (spline.Count < 1)
return float.PositiveInfinity;
var curve = spline.GetCurve(SplineToCurveT(spline, t, out var curveT));
return CurveUtility.EvaluateTangent(curve, curveT);
}
/// <summary>
/// Evaluate the normal (up) vector of a spline.
/// </summary>
/// <param name="spline">The spline to evaluate.</param>
/// <param name="t">A value between 0 and 1 representing a ratio of the curve.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>An up vector</returns>
public static float3 EvaluateUpVector<T>(this T spline, float t) where T : ISpline
{
if (spline.Count < 1)
return float3.zero;
var curveIndex = SplineToCurveT(spline, t, out var curveT);
return spline.GetCurveUpVector(curveIndex, curveT);
}
/// <summary>
/// Calculate the normal (up) vector of a spline. This is a more accurate but more expensive operation
/// than <see cref="EvaluateUpVector{T}"/>.
/// </summary>
/// <param name="spline">The <see cref="NativeSpline"/> to evaluate.</param>
/// <param name="t">A value between 0 and 1 representing a ratio of the curve.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>An up vector</returns>
public static float3 CalculateUpVector<T>(this T spline, float t) where T : ISpline
{
if (spline.Count < 1)
return float3.zero;
var curveIndex = SplineToCurveT(spline, t, out var curveT);
return spline.CalculateUpVector(curveIndex, curveT);
}
internal static float3 CalculateUpVector<T>(this T spline, int curveIndex, float curveT) where T : ISpline
{
if (spline.Count < 1)
return float3.zero;
var curve = spline.GetCurve(curveIndex);
var curveStartRotation = spline[curveIndex].Rotation;
var curveStartUp = math.rotate(curveStartRotation, math.up());
if (curveT == 0f)
return curveStartUp;
var endKnotIndex = spline.NextIndex(curveIndex);
var curveEndRotation = spline[endKnotIndex].Rotation;
var curveEndUp = math.rotate(curveEndRotation, math.up());
if (curveT == 1f)
return curveEndUp;
var up = CurveUtility.EvaluateUpVector(curve, curveT, curveStartUp, curveEndUp);
return up;
}
internal static void EvaluateUpVectorsForCurve<T>(this T spline, int curveIndex, NativeArray<float3> upVectors)
where T : ISpline
{
if (spline.Count < 1 || !upVectors.IsCreated)
return;
var curveStartRotation = spline[curveIndex].Rotation;
var curveStartUp = math.rotate(curveStartRotation, math.up());
var endKnotIndex = spline.NextIndex(curveIndex);
var curveEndRotation = spline[endKnotIndex].Rotation;
var curveEndUp = math.rotate(curveEndRotation, math.up());
CurveUtility.EvaluateUpVectors(spline.GetCurve(curveIndex), curveStartUp, curveEndUp, upVectors);
}
internal static void EvaluateUpVectorsForCurve<T>(this T spline, int curveIndex, float3[] upVectors) where T : ISpline
{
if (upVectors == null)
return;
var upVectorsNative = new NativeArray<float3>(upVectors, Allocator.Temp);
EvaluateUpVectorsForCurve(spline, curveIndex, upVectorsNative);
upVectorsNative.CopyTo(upVectors);
}
/// <summary>
/// Return an interpolated acceleration at ratio t.
/// </summary>
/// <param name="spline">The spline to interpolate.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <param name="t">A value between 0 and 1 representing the ratio along the curve.</param>
/// <returns>An acceleration on the spline.</returns>
public static float3 EvaluateAcceleration<T>(this T spline, float t) where T : ISpline
{
if (spline.Count < 1)
return float3.zero;
var curve = spline.GetCurve(SplineToCurveT(spline, t, out var curveT));
return CurveUtility.EvaluateAcceleration(curve, curveT);
}
/// <summary>
/// Return an interpolated curvature at ratio t.
/// </summary>
/// <param name="spline">The spline to interpolate.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <param name="t">A value between 0 and 1 representing the ratio along the curve.</param>
/// <returns>A curvature on the spline.</returns>
public static float EvaluateCurvature<T>(this T spline, float t) where T : ISpline
{
if (spline.Count < 1)
return 0f;
var curveIndex = SplineToCurveT(spline, t, out var curveT);
var curve = spline.GetCurve(curveIndex);
return CurveUtility.EvaluateCurvature(curve, curveT);
}
/// <summary>
/// Return the curvature center at ratio t. The curvature center represents the center of the circle
/// that is tangent to the curve at t. This circle is in the plane defined by the curve velocity (tangent)
/// and the curve acceleration at that point.
/// </summary>
/// <param name="spline">The spline to interpolate.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <param name="t">A value between 0 and 1 representing the ratio along the curve.</param>
/// <returns>A point representing the curvature center associated to the position at t on the spline.</returns>
public static float3 EvaluateCurvatureCenter<T>(this T spline, float t) where T : ISpline
{
if (spline.Count < 1)
return 0f;
var curveIndex = SplineToCurveT(spline, t, out var curveT);
var curve = spline.GetCurve(curveIndex);
var curvature = CurveUtility.EvaluateCurvature(curve, curveT);
if (curvature != 0)
{
var radius = 1f / curvature;
var position = CurveUtility.EvaluatePosition(curve, curveT);
var velocity = CurveUtility.EvaluateTangent(curve, curveT);
var acceleration = CurveUtility.EvaluateAcceleration(curve, curveT);
var curvatureUp = math.normalize(math.cross(acceleration, velocity));
var curvatureRight = math.normalize(math.cross(velocity, curvatureUp));
return position + radius * curvatureRight;
}
return float3.zero;
}
/// <summary>
/// Given a normalized interpolation (t) for a spline, calculate the curve index and curve-relative
/// normalized interpolation.
/// </summary>
/// <param name="spline">The target spline.</param>
/// <param name="splineT">A normalized spline interpolation value to be converted into curve space.</param>
/// <param name="curveT">A normalized curve interpolation value.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>The curve index.</returns>
public static int SplineToCurveT<T>(this T spline, float splineT, out float curveT) where T : ISpline
{
return SplineToCurveT(spline, splineT, out curveT, true);
}
static int SplineToCurveT<T>(this T spline, float splineT, out float curveT, bool useLUT) where T : ISpline
{
var knotCount = spline.Count;
if (knotCount <= 1)
{
curveT = 0f;
return 0;
}
splineT = math.clamp(splineT, 0, 1);
var tLength = splineT * spline.GetLength();
var start = 0f;
var closed = spline.Closed;
for (int i = 0, c = closed ? knotCount : knotCount - 1; i < c; i++)
{
var index = i % knotCount;
var curveLength = spline.GetCurveLength(index);
if (tLength <= (start + curveLength))
{
curveT = useLUT ?
spline.GetCurveInterpolation(index, tLength - start) :
(tLength - start) / curveLength;
return index;
}
start += curveLength;
}
curveT = 1f;
return closed ? knotCount - 1 : knotCount - 2;
}
/// <summary>
/// Given an interpolation value for a curve, calculate the relative normalized spline interpolation.
/// </summary>
/// <param name="spline">The target spline.</param>
/// <param name="curve">A curve index and normalized interpolation. The curve index is represented by the
/// integer part of the float, and interpolation is the fractional part. This is the format used by
/// <see cref="PathIndexUnit.Knot"/>.
/// </param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>An interpolation value relative to normalized Spline length (0 to 1).</returns>
/// <seealso cref="SplineToCurveT{T}"/>
public static float CurveToSplineT<T>(this T spline, float curve) where T : ISpline
{
// Clamp negative curve index to 0
if (spline.Count <= 1 || curve < 0f)
return 0f;
// Clamp postive curve index beyond last knot to 1
if (curve >= (spline.Closed ? spline.Count : spline.Count - 1))
return 1f;
var curveIndex = (int)math.floor(curve);
float t = 0f;
for (int i = 0; i < curveIndex; i++)
t += spline.GetCurveLength(i);
t += spline.GetCurveLength(curveIndex) * math.frac(curve);
return t / spline.GetLength();
}
/// <summary>
/// Calculate the length of a spline when transformed by a matrix.
/// </summary>
/// <param name="spline">The spline whose length is to be calculated.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <param name="transform">The transformation matrix to apply to the spline.</param>
/// <returns>The length of the transformed spline.</returns>
public static float CalculateLength<T>(this T spline, float4x4 transform) where T : ISpline
{
using var nativeSpline = new NativeSpline(spline, transform);
return nativeSpline.GetLength();
}
/// <summary>
/// Calculates the number of curves in a spline.
/// </summary>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <param name="spline">The spline for which to calculate the number of curves.</param>
/// <returns>The number of curves in a spline.</returns>
public static int GetCurveCount<T>(this T spline) where T : ISpline
{
return math.max(0, spline.Count - (spline.Closed ? 0 : 1));
}
/// <summary>
/// Calculate the bounding box of a Spline.
/// </summary>
/// <param name="spline">The spline for which to calculate bounds.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>The bounds of a spline.</returns>
public static Bounds GetBounds<T>(this T spline) where T : ISpline
{
return GetBounds(spline, float4x4.identity);
}
/// <summary>
/// Creates a bounding box for a spline.
/// </summary>
/// <param name="spline">The spline to calculate bounds for.</param>
/// <param name="transform">The matrix to transform the spline's elements with.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>The bounds of a spline.</returns>
public static Bounds GetBounds<T>(this T spline, float4x4 transform) where T : ISpline
{
if (spline.Count < 1)
return default;
var knot = spline[0];
Bounds bounds = new Bounds(math.transform(transform, knot.Position), Vector3.zero);
// Only encapsulate first tangentIn if the spline is closed - otherwise it's not contributing to the spline's shape.
if (spline.Closed)
bounds.Encapsulate(math.transform(transform, knot.Position + math.rotate(knot.Rotation, knot.TangentIn)));
bounds.Encapsulate(math.transform(transform, knot.Position + math.rotate(knot.Rotation, knot.TangentOut)));
for (int i = 1, c = spline.Count; i < c; ++i)
{
knot = spline[i];
bounds.Encapsulate(math.transform(transform, knot.Position));
bounds.Encapsulate(math.transform(transform, knot.Position + math.rotate(knot.Rotation, knot.TangentIn)));
// Encapsulate last tangentOut if the spline is closed - otherwise it's not contributing to the spline's shape.
if (spline.Closed || (!spline.Closed && i < c - 1))
bounds.Encapsulate(math.transform(transform, knot.Position + math.rotate(knot.Rotation, knot.TangentOut)));
}
return bounds;
}
/// <summary>
/// Gets the number of segments for a specified spline length and resolution.
/// </summary>
/// <param name="length">The length of the spline to consider.</param>
/// <param name="resolution">The value used to calculate the number of segments for a length. This is calculated
/// as max(MIN_SEGMENTS, min(MAX_SEGMENTS, sqrt(length) * resolution)).
/// </param>
/// <returns>
/// The number of segments for a length and resolution.
/// </returns>
[Obsolete("Use " + nameof(GetSubdivisionCount) + " instead.", false)]
public static int GetSegmentCount(float length, int resolution) => GetSubdivisionCount(length, resolution);
/// <summary>
/// Gets the number of subdivisions for a spline length and resolution.
/// </summary>
/// <param name="length">The length of the spline to consider.</param>
/// <param name="resolution">The resolution to consider. Higher resolutions result in more
/// precise representations. However, higher resolutions have higher performance requirements.
/// </param>
/// <returns>
/// The number of subdivisions as calculated for given length and resolution.
/// </returns>
public static int GetSubdivisionCount(float length, int resolution)
{
return (int)math.max(k_SubdivisionCountMin, math.min(k_SubdivisionCountMax, math.sqrt(length) * resolution));
}
struct Segment
{
public float start, length;
public Segment(float start, float length)
{
this.start = start;
this.length = length;
}
}
static Segment GetNearestPoint<T>(T spline,
float3 ro, float3 rd,
Segment range,
out float distance, out float3 nearest, out float time,
int segments) where T : ISpline
{
distance = float.PositiveInfinity;
nearest = float.PositiveInfinity;
time = float.PositiveInfinity;
Segment segment = new Segment(-1f, 0f);
float t0 = range.start;
float3 a = EvaluatePosition(spline, t0);
for (int i = 1; i < segments; i++)
{
float t1 = range.start + (range.length * (i / (segments - 1f)));
float3 b = EvaluatePosition(spline, t1);
var (rayPoint, linePoint) = SplineMath.RayLineNearestPoint(ro, rd, a, b, out _, out var lineParam);
float dsqr = math.lengthsq(linePoint - rayPoint);
if (dsqr < distance)
{
segment.start = t0;
segment.length = t1 - t0;
time = segment.start + segment.length * lineParam;
distance = dsqr;
nearest = linePoint;
}
t0 = t1;
a = b;
}
distance = math.sqrt(distance);
return segment;
}
static Segment GetNearestPoint<T>(T spline,
float3 point,
Segment range,
out float distance, out float3 nearest, out float time,
int segments) where T : ISpline
{
distance = float.PositiveInfinity;
nearest = float.PositiveInfinity;
time = float.PositiveInfinity;
Segment segment = new Segment(-1f, 0f);
float t0 = range.start;
float3 a = EvaluatePosition(spline, t0);
for (int i = 1; i < segments; i++)
{
float t1 = range.start + (range.length * (i / (segments - 1f)));
float3 b = EvaluatePosition(spline, t1);
var p = SplineMath.PointLineNearestPoint(point, a, b, out var lineParam);
float dsqr = math.distancesq(p, point);
if (dsqr < distance)
{
segment.start = t0;
segment.length = t1 - t0;
time = segment.start + segment.length * lineParam;
distance = dsqr;
nearest = p;
}
t0 = t1;
a = b;
}
distance = math.sqrt(distance);
return segment;
}
/// <summary>
/// Calculate the point on a spline nearest to a ray.
/// </summary>
/// <param name="spline">The input spline to search for nearest point.</param>
/// <param name="ray">The input ray to search against.</param>
/// <param name="nearest">The point on a spline nearest to the input ray. The accuracy of this value is
/// affected by the <paramref name="resolution"/>.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <param name="t">The normalized time value to the nearest point.</param>
/// <param name="resolution">Affects how many segments to split a spline into when calculating the nearest point.
/// Higher values mean smaller and more segments, which increases accuracy at the cost of processing time.
/// The minimum resolution is defined by <see cref="PickResolutionMin"/>, and the maximum is defined by
/// <see cref="PickResolutionMax"/>.
/// In most cases, the default resolution is appropriate. Use with <paramref name="iterations"/> to fine tune
/// point accuracy.
/// </param>
/// <param name="iterations">
/// The nearest point is calculated by finding the nearest point on the entire length
/// of the spline using <paramref name="resolution"/> to divide into equally spaced line segments. Successive
/// iterations will then subdivide further the nearest segment, producing more accurate results. In most cases,
/// the default value is sufficient, but if extreme accuracy is required this value can be increased to a
/// maximum of <see cref="PickResolutionMax"/>.
/// </param>
/// <returns>The distance from ray to nearest point.</returns>
public static float GetNearestPoint<T>(T spline,
Ray ray,
out float3 nearest,
out float t,
int resolution = PickResolutionDefault,
int iterations = 2) where T : ISpline
{
float distance = float.PositiveInfinity;
nearest = float.PositiveInfinity;
float3 ro = ray.origin, rd = ray.direction;
Segment segment = new Segment(0f, 1f);
t = 0f;
int res = math.min(math.max(PickResolutionMin, resolution), PickResolutionMax);
for (int i = 0, c = math.min(10, iterations); i < c; i++)
{
int segments = GetSubdivisionCount(spline.GetLength() * segment.length, res);
segment = GetNearestPoint(spline, ro, rd, segment, out distance, out nearest, out t, segments);
}
return distance;
}
/// <summary>
/// Calculate the point on a spline nearest to a point.
/// </summary>
/// <param name="spline">The input spline to search for nearest point.</param>
/// <param name="point">The input point to compare.</param>
/// <param name="nearest">The point on a spline nearest to the input point. The accuracy of this value is
/// affected by the <paramref name="resolution"/>.</param>
/// <param name="t">The normalized interpolation ratio corresponding to the nearest point.</param>
/// <param name="resolution">Affects how many segments to split a spline into when calculating the nearest point.
/// Higher values mean smaller and more segments, which increases accuracy at the cost of processing time.
/// The minimum resolution is defined by <see cref="PickResolutionMin"/>, and the maximum is defined by
/// <see cref="PickResolutionMax"/>.
/// In most cases, the default resolution is appropriate. Use with <paramref name="iterations"/> to fine tune
/// point accuracy.
/// </param>
/// <param name="iterations">
/// The nearest point is calculated by finding the nearest point on the entire length
/// of the spline using <paramref name="resolution"/> to divide into equally spaced line segments. Successive
/// iterations will then subdivide further the nearest segment, producing more accurate results. In most cases,
/// the default value is sufficient, but if extreme accuracy is required this value can be increased to a
/// maximum of <see cref="PickResolutionMax"/>.
/// </param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>The distance from input point to nearest point on spline.</returns>
public static float GetNearestPoint<T>(T spline,
float3 point,
out float3 nearest,
out float t,
int resolution = PickResolutionDefault,
int iterations = 2) where T : ISpline
{
float distance = float.PositiveInfinity;
nearest = float.PositiveInfinity;
Segment segment = new Segment(0f, 1f);
t = 0f;
int res = math.min(math.max(PickResolutionMin, resolution), PickResolutionMax);
for (int i = 0, c = math.min(10, iterations); i < c; i++)
{
int segments = GetSubdivisionCount(spline.GetLength() * segment.length, res);
segment = GetNearestPoint(spline, point, segment, out distance, out nearest, out t, segments);
}
return distance;
}
/// <summary>
/// Given a Spline and interpolation ratio, calculate the 3d point at a linear distance from point at spline.EvaluatePosition(t).
/// Returns the corresponding time associated to this 3d position on the Spline.
/// </summary>
/// <param name="spline">The Spline on which to compute the point.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <param name="fromT">The Spline interpolation ratio `t` (normalized) from which the next position need to be computed.</param>
/// <param name="relativeDistance">
/// The relative distance at which the new point should be placed. A negative value will compute a point at a
/// `resultPointTime` previous to `fromT` (backward search).
/// </param>
/// <param name="resultPointT">The normalized interpolation ratio of the resulting point.</param>
/// <returns>The 3d point from the spline located at a linear distance from the point at t.</returns>
public static float3 GetPointAtLinearDistance<T>(this T spline,
float fromT,
float relativeDistance,
out float resultPointT) where T : ISpline
{
const float epsilon = 0.001f;
if (fromT < 0)
{
resultPointT = 0f;
return spline.EvaluatePosition(0f);
}
var length = spline.GetLength();
var lengthAtT = fromT * length;
float currentLength = lengthAtT;
if (currentLength + relativeDistance >= length) //relativeDistance >= 0 -> Forward search
{
resultPointT = 1f;
return spline.EvaluatePosition(1f);
}
else if (currentLength + relativeDistance <= 0) //relativeDistance < 0 -> Forward search
{
resultPointT = 0f;
return spline.EvaluatePosition(0f);
}
var currentPos = spline.EvaluatePosition(fromT);
resultPointT = fromT;
var forwardSearch = relativeDistance >= 0;
var residual = math.abs(relativeDistance);
float linearDistance = 0;
float3 point = currentPos;
while (residual > epsilon && (forwardSearch ? resultPointT < 1f : resultPointT > 0))
{
currentLength += forwardSearch ? residual : -residual;
resultPointT = currentLength / length;
if (resultPointT > 1f) //forward search
resultPointT = 1f;
else if (resultPointT < 0f) //backward search
resultPointT = 0f;
point = spline.EvaluatePosition(resultPointT);
linearDistance = math.distance(currentPos, point);
residual = math.abs(relativeDistance) - linearDistance;
}
return point;
}
/// <summary>
/// Given a normalized interpolation ratio, calculate the associated interpolation value in another <see cref="PathIndexUnit"/> regarding a specific spline.
/// </summary>
/// <param name="spline">The Spline to use for the conversion.</param>
/// <param name="t">Normalized interpolation ratio (0 to 1).</param>
/// <param name="targetPathUnit">The <see cref="PathIndexUnit"/> to which `t` should be converted.</param>
/// <typeparam name="T">A type implementing <see cref="ISpline"/>.</typeparam>
/// <returns>The interpolation value converted to targetPathUnit.</returns>
public static float ConvertIndexUnit<T>(this T spline, float t, PathIndexUnit targetPathUnit)
where T : ISpline
{
if (targetPathUnit == PathIndexUnit.Normalized)
return WrapInterpolation(t, spline.Closed);
return ConvertNormalizedIndexUnit(spline, t, targetPathUnit);
}
/// <summary>
/// Given an interpolation value using one of the various <see cref="PathIndexUnit"/> types, calculate the associated interpolation value in another <see cref="PathIndexUnit"/> regarding a specific spline.
/// </summary>
/// <param name="spline">The spline to use for the conversion.</param>
/// <param name="value">Interpolation value in the original <see cref="PathIndexUnit"/> `fromPathUnit`.</param>
/// <param name="fromPathUnit">The <see cref="PathIndexUnit"/> for the original interpolation value type.</param>
/// <param name="targetPathUnit">The <see cref="PathIndexUnit"/> to which `value` should be converted.</param>
/// <typeparam name="T">A type implementing <see cref="ISpline"/>.</typeparam>
/// <returns>The interpolation value converted to targetPathUnit.</returns>
public static float ConvertIndexUnit<T>(this T spline, float value, PathIndexUnit fromPathUnit, PathIndexUnit targetPathUnit)
where T : ISpline
{
if (fromPathUnit == targetPathUnit)
{
if (targetPathUnit == PathIndexUnit.Normalized)
value = WrapInterpolation(value, spline.Closed);
return value;
}
return ConvertNormalizedIndexUnit(spline, GetNormalizedInterpolation(spline, value, fromPathUnit), targetPathUnit);
}
static float ConvertNormalizedIndexUnit<T>(T spline, float t, PathIndexUnit targetPathUnit) where T : ISpline
{
switch (targetPathUnit)
{
case PathIndexUnit.Knot:
//LUT SHOULD NOT be used here as PathIndexUnit.KnotIndex is linear regarding the distance
//(and thus not be interpreted using the LUT and the interpolated T)
int splineIndex = spline.SplineToCurveT(t, out float curveTime, false);
return splineIndex + curveTime;
case PathIndexUnit.Distance:
return t * spline.GetLength();
default:
return t;
}
}
static float WrapInterpolation(float t, bool closed)
{
if (!closed)
return math.clamp(t, 0f, 1f);
return t % 1f == 0f ? math.clamp(t, 0f, 1f) : t - math.floor(t);
}
/// <summary>
/// Given an interpolation value in any PathIndexUnit type, calculate the normalized interpolation ratio value
/// relative to a <see cref="Spline"/>.
/// </summary>
/// <param name="spline">The Spline to use for the conversion, this is necessary to compute Normalized and Distance PathIndexUnits.</param>
/// <param name="t">The `t` value to normalize in the original PathIndexUnit.</param>
/// <param name="originalPathUnit">The PathIndexUnit from the original `t`.</param>
/// <typeparam name="T">A type implementing ISpline.</typeparam>
/// <returns>The normalized interpolation ratio (0 to 1).</returns>
public static float GetNormalizedInterpolation<T>(T spline, float t, PathIndexUnit originalPathUnit) where T : ISpline
{
switch (originalPathUnit)
{
case PathIndexUnit.Knot:
return WrapInterpolation(CurveToSplineT(spline, t), spline.Closed);
case PathIndexUnit.Distance:
var length = spline.GetLength();
return WrapInterpolation(length > 0 ? t / length : 0f, spline.Closed);
default:
return WrapInterpolation(t, spline.Closed);
}
}
/// <summary>
/// Gets the index of a knot that precedes a spline index. This method uses the <see cref="Spline.Count"/>
/// and <see cref="Spline.Closed"/> properties to ensure that it returns the correct index of the knot.
/// </summary>
/// <param name="spline">The spline to consider.</param>
/// <param name="index">The current index to consider.</param>
/// <typeparam name="T">A type that implements ISpline.</typeparam>
/// <returns>Returns a knot index that precedes the `index` on the considered spline.</returns>
public static int PreviousIndex<T>(this T spline, int index) where T : ISpline
=> PreviousIndex(index, spline.Count, spline.Closed);
/// <summary>
/// Gets the index of a knot that follows a spline index. This method uses the <see cref="Spline.Count"/> and
/// <see cref="Spline.Closed"/> properties to ensure that it returns the correct index of the knot.
/// </summary>
/// <param name="spline">The spline to consider.</param>
/// <param name="index">The current index to consider.</param>
/// <typeparam name="T">A type that implements ISpline.</typeparam>
/// <returns>The knot index after `index` on the considered spline.</returns>
public static int NextIndex<T>(this T spline, int index) where T : ISpline
=> NextIndex(index, spline.Count, spline.Closed);
/// <summary>
/// Gets the <see cref="BezierKnot"/> before spline[index]. This method uses the <see cref="Spline.Count"/>
/// and <see cref="Spline.Closed"/> properties to ensure that it returns the correct knot.
/// </summary>
/// <param name="spline">The spline to consider.</param>
/// <param name="index">The current index to consider.</param>
/// <typeparam name="T">A type that implements ISpline.</typeparam>
/// <returns>The knot before the knot at spline[index].</returns>
public static BezierKnot Previous<T>(this T spline, int index) where T : ISpline
=> spline[PreviousIndex(spline, index)];
/// <summary>
/// Gets the <see cref="BezierKnot"/> after spline[index]. This method uses the <see cref="Spline.Count"/>
/// and <see cref="Spline.Closed"/> properties to ensure that it returns the correct knot.
/// </summary>
/// <param name="spline">The spline to consider.</param>
/// <param name="index">The current index to consider.</param>
/// <typeparam name="T">A type that implements ISpline.</typeparam>
/// <returns>The knot after the knot at spline[index].</returns>
public static BezierKnot Next<T>(this T spline, int index) where T : ISpline
=> spline[NextIndex(spline, index)];
internal static int PreviousIndex(int index, int count, bool wrap)
{
return wrap ? (index + (count - 1)) % count : math.max(index - 1, 0);
}
internal static int NextIndex(int index, int count, bool wrap)
{
return wrap ? (index + 1) % count : math.min(index + 1, count - 1);
}
internal static float3 GetExplicitLinearTangent(float3 point, float3 to)
{
return (to - point) / 3.0f;
}
// Typically a linear tangent is stored as a zero length vector. When a tangent is user controlled, we need to
// extrude out the tangent to something a user can grab rather than leaving the tangent on top of the knot.
internal static float3 GetExplicitLinearTangent(BezierKnot from, BezierKnot to)
{
var tin = to.Position - from.Position;
return math.mul(math.inverse(from.Rotation), tin * .33f);
}
/// <summary>
/// Calculates a tangent from the previous and next knot positions.
/// </summary>
/// <param name="previous">The position of the previous <see cref="BezierKnot"/>.</param>
/// <param name="next">The position of the next <see cref="BezierKnot"/>.</param>
/// <returns>Returns a tangent calculated from the previous and next knot positions.</returns>
// todo Deprecate in 3.0 - this is not a correct uniform catmull rom parameterization.
public static float3 GetCatmullRomTangent(float3 previous, float3 next) =>
GetAutoSmoothTangent(previous, next, CatmullRomTension);
/// <summary>
/// Calculates a tangent from the previous and next knot positions.
/// </summary>
/// <param name="previous">The position of the previous <see cref="BezierKnot"/>.</param>
/// <param name="next">The position of the next <see cref="BezierKnot"/>.</param>
/// <param name="tension">Set the length of the tangent vectors.</param>
/// <returns>Returns a tangent calculated from the previous and next knot positions.</returns>
public static float3 GetAutoSmoothTangent(float3 previous, float3 next, float tension = DefaultTension)
{
if (next.Equals(previous))
return 0f;
return (next - previous) / math.sqrt(math.length(next - previous)) * tension;
}
/// <summary>
/// Gets a tangent from the previous, current, and next knot positions.
/// </summary>
/// <param name="previous">The position of the previous <see cref="BezierKnot"/>.</param>
/// <param name="current">The position of the current <see cref="BezierKnot"/>.</param>
/// <param name="next">The position of the next <see cref="BezierKnot"/>.</param>
/// <param name="tension">The length of the tangent vectors.</param>
/// <returns>Returns a tangent calculated from the previous, current, and next knot positions.</returns>
public static float3 GetAutoSmoothTangent(float3 previous, float3 current, float3 next, float tension = DefaultTension)
{
var d1 = math.length(current - previous);
var d2 = math.length(next - current);
if (d1 == 0f) // If we're only working with 2 valid points, then calculate roughly Uniform parametrization tangent and scale it down so we can atleast build rotation.
return (next - current) * 0.1f;
else if (d2 == 0f)
return (current - previous) * 0.1f;
// Calculations below are based on (pp. 5-6): http://www.cemyuksel.com/research/catmullrom_param/catmullrom_cad.pdf
// Catmull-Rom parameterization: a = 0 - Uniform, a = 0.5 - Centripetal, a = 1.0 - Chordal.
var a = tension;
var twoA = 2f * tension;
var d1PowA = math.pow(d1, a);
var d1Pow2A = math.pow(d1, twoA);
var d2PowA = math.pow(d2, a);
var d2Pow2A = math.pow(d2, twoA);
return (d1Pow2A * next - d2Pow2A * previous + (d2Pow2A - d1Pow2A) * current) / (3f * d1PowA * (d1PowA + d2PowA));
}
static float3 GetUniformAutoSmoothTangent(float3 previous, float3 next, float tension)
{
return (next - previous) * tension;
}
/// <summary>
/// Gets a <see cref="BezierKnot"/> with its tangents and rotation calculated using the previous and next knot positions.
/// </summary>
/// <param name="position">The position of the knot.</param>
/// <param name="previous">The knot that immediately precedes the requested knot.</param>
/// <param name="next">The knot that immediately follows the requested knot.</param>
/// <returns>A <see cref="BezierKnot"/> with tangent and rotation values calculated from the previous and next knot positions.</returns>
public static BezierKnot GetAutoSmoothKnot(float3 position, float3 previous, float3 next) =>
GetAutoSmoothKnot(position, previous, next, math.up());
/// <summary>
/// Gets a <see cref="BezierKnot"/> with its tangents and rotation calculated using the previous and next knot positions.
/// </summary>
/// <param name="position">The position of the knot.</param>
/// <param name="previous">The knot that immediately precedes the requested knot.</param>
/// <param name="next">The knot that immediately follows the requested knot.</param>
/// <param name="normal">The normal vector of the knot.</param>
/// <returns>A <see cref="BezierKnot"/> with tangent and rotation values calculated from the previous and next knot positions.</returns>
public static BezierKnot GetAutoSmoothKnot(float3 position, float3 previous, float3 next, float3 normal) =>
GetAutoSmoothKnot(position, previous, next, normal, CatmullRomTension);
/// <summary>
/// Gets a <see cref="BezierKnot"/> with its tangents and rotation calculated using the previous and next knot positions.
/// </summary>
/// <param name="position">The position of the knot.</param>
/// <param name="previous">The knot that immediately precedes the requested knot.</param>
/// <param name="next">The knot that immediately follows the requested knot.</param>
/// <param name="normal">The normal vector of the knot.</param>
/// <param name="tension">Set the length of the tangent vectors.</param>
/// <returns>A <see cref="BezierKnot"/> with tangent and rotation values calculated from the previous and next knot positions.</returns>
public static BezierKnot GetAutoSmoothKnot(float3 position, float3 previous, float3 next, float3 normal, float tension = DefaultTension)
{
var tanIn = GetAutoSmoothTangent(next, position, previous, tension);
var tanOut = GetAutoSmoothTangent(previous, position, next, tension);
var dirIn = new float3(0, 0, math.length(tanIn));
var dirOut = new float3(0, 0, math.length(tanOut));
var dirRot = tanOut;
if (dirIn.z == 0f)
dirIn.z = dirOut.z;
if (dirOut.z == 0f)
{
dirOut.z = dirIn.z;
dirRot = -tanIn;
}
return new BezierKnot(position, -dirIn, dirOut, GetKnotRotation(dirRot, normal));
}
internal static quaternion GetKnotRotation(float3 tangent, float3 normal)
{
if (math.lengthsq(tangent) == 0f)
tangent = math.rotate(Quaternion.FromToRotation(math.up(), normal), math.forward());
float3 up = Mathf.Approximately(math.abs(math.dot(math.normalizesafe(tangent), math.normalizesafe(normal))), 1f)
? math.cross(math.normalizesafe(tangent), math.right())
: Vector3.ProjectOnPlane(normal, tangent).normalized;
return quaternion.LookRotationSafe(math.normalizesafe(tangent), up);
}
/// <summary>
/// Reset a transform position to a position while keeping knot positions in the same place. This modifies both
/// knot positions and transform position.
/// </summary>
/// <param name="container">The target spline.</param>
/// <param name="position">The point in world space to move the pivot to.</param>
public static void SetPivot(SplineContainer container, Vector3 position)
{
var transform = container.transform;
var delta = position - transform.position;
transform.position = position;
var spline = container.Spline;
for (int i = 0, c = spline.Count; i < c; i++)
spline[i] = spline[i] - delta;
}
/// <summary>
/// Computes a <see cref="Spline"/> to approximate the curve formed by the list of provided points
/// within the given error threshold.
/// </summary>
/// <param name="points">The list of <see cref="float3"/> points that define the curve to approximate.</param>
/// <param name="errorThreshold">The error threshold to use. Represents the largest distance between any of the
/// provided points and the curve.</param>
/// <param name="closed">Whether to close the <see cref="Spline"/>.</param>
/// <param name="spline">The output <see cref="Spline"/> fitted to the points, or an empty spline if could not be fitted.</param>
/// <returns>Whether a curve could be fitted according to the provided error threshold.</returns>
public static bool FitSplineToPoints(List<float3> points, float errorThreshold, bool closed,
out Spline spline)
{
/*
Implementation based on:
"Algorithm for Automatically Fitting Digitized Curves"
by Philip J. Schneider
"Graphics Gems", Academic Press, 1990
*/
const float epsilon = 0.001f;
if (points == null || points.Count < 2)
{
Debug.LogError("FitSplineToPoints failed: The provided points list is either null, empty or contains only one point.");
spline = new Spline();
return false;
}
var pointsCopy = new List<float3>(points);
if (closed)
{
// As the base algorithm does not have a notion of closed splines, we add 2-3 extra/extension points to the input list
// to help the algorithm smooth out tangents at the connecting/closing point. They are removed from the resulting
// spline at the end of the processing.
// 1) If the first and last points do not match, we duplicate the first and add it to the end of the curve
if (math.length(pointsCopy[0] - pointsCopy[^1]) > epsilon)
pointsCopy.Add(pointsCopy[0]);
// 2) We then do a forward loop/revolution of one point by duplicating the second knot and attaching it to the end
pointsCopy.Add(pointsCopy[1]);
// 3) What was initially the last point, we duplicate and insert at front - backward loop/revolution of one point
pointsCopy.Insert(0, pointsCopy[^3]);
}
var leftTangent = math.normalize(pointsCopy[1] - pointsCopy[0]);
var rightTangent = math.normalize(pointsCopy[^2] - pointsCopy[^1]);
// Do recursive fitting
if (FitSplineToPointsStepInternal(pointsCopy, 0, pointsCopy.Count - 1, leftTangent, rightTangent, errorThreshold, closed, closed, out spline))
{
// For closed spline, clean up/merge knots that were added due to the appended extension points
if (closed && spline.Count > 2)
{
// Remove 2 of the 3 extension knots of closed splines.
spline.RemoveAt(0);
spline.RemoveAt(spline.Count - 1);
// Merge the overlaping front/end knots.
var firstKnot = spline[0];
firstKnot.TangentIn = spline[^1].TangentOut;
spline[0] = firstKnot;
spline.RemoveAt(spline.Count - 1);
spline.Closed = true;
}
// At this point we have the final shape of the spline - tangents can now be baked into rotation.
var up = float3.zero;
for (int i = 0; i < spline.Count; ++i)
{
var knot = spline[i];
var knotDir = knot.TangentOut;
if (math.lengthsq(knotDir) == 0f)
knotDir = spline.EvaluateTangent(GetNormalizedInterpolation(spline, i, PathIndexUnit.Knot));
var knotDirNorm = math.normalizesafe(knotDir);
if (i == 0)
up = CalculatePreferredNormalForDirection(knotDirNorm);
else
up = CurveUtility.EvaluateUpVector(spline.GetCurve(i - 1), 1f, up, float3.zero, false);
var tangentMode = TangentMode.Broken;
if (math.dot(math.normalizesafe(knot.TangentIn), math.normalizesafe(knot.TangentOut)) <= -1f + epsilon)
{
if (math.abs(math.length(knot.TangentIn) - math.length(knot.TangentOut)) <= epsilon)
tangentMode = TangentMode.Mirrored;
else
tangentMode = TangentMode.Continuous;
}
knot.TangentIn = new float3(0f, 0f, -math.length(knot.TangentIn));
knot.TangentOut = new float3(0f, 0f, math.length(knot.TangentOut));
knot.Rotation = quaternion.LookRotationSafe(knotDirNorm, up);
spline[i] = knot;
if (tangentMode != TangentMode.Broken)
spline.SetTangentModeNoNotify(i, tangentMode);
}
return true;
}
return false;
}
private static bool FitSplineToPointsStepInternal(IReadOnlyList<float3> allPoints, int rangeStart, int rangeEnd, float3 leftTangent, float3 rightTangent, float errorThreshold, bool closed, bool splineClosed,
out Spline spline)
{
const int maxIterations = 4;
spline = new Spline();
var curRangePointCount = rangeEnd - rangeStart + 1;
if (curRangePointCount < 2)
return false;
if (curRangePointCount == 2)
{
var difference = allPoints[rangeStart] - allPoints[rangeEnd];
//we use 1/3 of the distance between each of the points as described in the graphics gems paper.
var diffLength = math.length(difference) / 3f;
var leftTanScaled = leftTangent * diffLength;
var rightTanScaled = rightTangent * diffLength;
var firstKnot = new BezierKnot(allPoints[rangeStart], -leftTanScaled, leftTanScaled, Quaternion.identity);
var secondKnot = new BezierKnot(allPoints[rangeStart + 1], rightTanScaled, -rightTanScaled, Quaternion.identity);
spline = new Spline();
spline.Add(firstKnot, TangentMode.Broken);
spline.Add(secondKnot, TangentMode.Broken);
spline.Closed = closed;
return true;
}
// Find corresponding t values for each point so we can compute errors (chord-length parameterization)
var correspondingTValues = new float[curRangePointCount];
var firstPassTValues = new float[curRangePointCount];
firstPassTValues[0] = 0f;
var cumulativeChordLengths = new float[curRangePointCount - 1];
var chordLengthSum = 0f;
for (int i = 1; i < curRangePointCount; i++)
{
var chord = allPoints[rangeStart + i] - allPoints[rangeStart + i - 1];
chordLengthSum += math.length(chord);
cumulativeChordLengths[i - 1] = chordLengthSum;
}
for (int i = 0; i < cumulativeChordLengths.Length; i++)
{
firstPassTValues[i + 1] = cumulativeChordLengths[i] / chordLengthSum;
}
correspondingTValues = firstPassTValues;
spline = GenerateSplineFromTValues(allPoints, rangeStart, rangeEnd, closed, correspondingTValues, leftTangent, rightTangent);
var tPositions = new float3[curRangePointCount];
for (int i = 0; i < correspondingTValues.Length; i++)
{
float t = correspondingTValues[i];
float3 position = spline.EvaluatePosition(t);
tPositions[i] = position;
}
var errorResult = ComputeMaxError(allPoints, rangeStart, rangeEnd, tPositions, errorThreshold, splineClosed);
var errorBeforeReparameterization = errorResult.maxError;
var splitPoint = errorResult.maxErrorIndex;
if (errorBeforeReparameterization < errorThreshold)
return true;
// If error is small enough, try reparameterizing and fitting again
else if (errorBeforeReparameterization < 4 * errorThreshold)
{
var numIterations = 0;
while (numIterations < maxIterations)
{
var curveKnots = new BezierKnot[spline.Count];
int i = 0;
foreach (BezierKnot bezierKnot in spline.Knots)
{
curveKnots[i] = bezierKnot;
++i;
}
var P0 = curveKnots[0].Position;
var P1 = curveKnots[0].Position + curveKnots[0].TangentOut;
var P2 = curveKnots[1].Position + curveKnots[1].TangentIn;
var P3 = curveKnots[1].Position;
var P = new float3[] { P0, P1, P2, P3 };
var q1 = new float3[3];
for (int j = 0; j <= 2; ++j)
{
q1[j] = (P[j + 1] - P[j]) * 3f;
}
var q2 = new float3[2];
for (int j = 0; j <= 1; ++j)
{
q2[j] = (q1[j + 1] - q1[j]) * 2f;
}
for (int k = rangeStart; k < curRangePointCount-1; k++)
{
var t = correspondingTValues[k];
var q = Bernstein(t, P, 3);
var q1_t = Bernstein(t, q1, 2);
var q2_t = Bernstein(t, q2, 1);
var numerator = math.dot(q - tPositions[k], q1_t);
var denominator = math.dot(q1_t, q1_t) + math.dot(q - tPositions[k], q2_t);
if (denominator != 0)
correspondingTValues[k] -= (numerator / denominator);
}
spline = GenerateSplineFromTValues(allPoints, rangeStart, rangeEnd, closed, correspondingTValues, leftTangent, rightTangent);
for (int k = 0; k < tPositions.Length; k++)
{
var t = correspondingTValues[k];
var position = spline.EvaluatePosition(t);
tPositions[k] = position;
}
errorResult = ComputeMaxError(allPoints, rangeStart, rangeEnd, tPositions, errorThreshold, splineClosed);
var errorAfterReparameterization = errorResult.maxError;
splitPoint = errorResult.maxErrorIndex;
if (errorAfterReparameterization < errorThreshold)
{
return true;
}
numIterations++;
}
}
// Still not good enough. Try splitting at point of max error.
if (curRangePointCount == 3)
splitPoint = rangeStart + 1;
else
{
if (splitPoint == 0)
splitPoint++;
else if (splitPoint == curRangePointCount - 1)
splitPoint--;
}
var splitTangent = CalculateCenterTangent(allPoints[splitPoint - 1], allPoints[splitPoint], allPoints[splitPoint + 1]);
if (!FitSplineToPointsStepInternal(allPoints, rangeStart, splitPoint, leftTangent, splitTangent, errorThreshold, false, splineClosed, out var firstSpline) ||
!FitSplineToPointsStepInternal(allPoints, splitPoint, rangeEnd, -splitTangent, rightTangent, errorThreshold, false, splineClosed, out var secondSpline))
{
spline = new Spline();
return false;
}
var splitPointIdx = firstSpline.Count - 1;
var splitPointTangentIn = firstSpline[^1].TangentIn;
firstSpline.RemoveAt(splitPointIdx);
firstSpline.Add(secondSpline);
spline = firstSpline;
var splitKnot = spline[splitPointIdx];
splitKnot.TangentIn = splitPointTangentIn;
spline[splitPointIdx] = splitKnot;
return true;
}
static float3 CalculatePreferredNormalForDirection(float3 splineDirection)
{
var dirNorm = math.normalizesafe(splineDirection);
float3 absDotRUF;
absDotRUF.x = math.abs(math.dot(dirNorm, math.right()));
absDotRUF.y = math.abs(math.dot(dirNorm, math.up()));
absDotRUF.z = math.abs(math.dot(dirNorm, math.forward()));
float3 right;
// Pick the right vector based on a world axis that the spline tangent is most orthogonal to
if (absDotRUF.x < absDotRUF.y && absDotRUF.x < absDotRUF.z)
right = math.normalizesafe(math.cross(math.right(), dirNorm));
else if (absDotRUF.y < absDotRUF.x && absDotRUF.y < absDotRUF.z)
right = math.normalizesafe(math.cross(math.up(), dirNorm));
else
right = math.normalizesafe(math.cross(math.forward(), dirNorm));
return math.normalizesafe(math.cross(dirNorm, right));
}
static float3 CalculateCenterTangent(float3 prevPoint, float3 centerPoint, float3 nextPoint)
{
var v1 = prevPoint - centerPoint;
var v2 = centerPoint - nextPoint;
var centerTangent = math.normalize((v1 + v2) / 2.0f);
return centerTangent;
}
//A helper method for FitSplineToPoints that computes bernstein basis functions
static float3 Bernstein(float t, float3[] bezier, int degree)
{
var copy = new float3[bezier.Length];
bezier.CopyTo(copy, 0);
for (int i = 1; i <= degree; ++i)
{
for (int j = 0; j <= degree - i; ++j)
{
copy[j] = (1 - t) * copy[j] + t * copy[j + 1];
}
}
return copy[0];
}
//A helper method for FitSplineToPoints that generates a spline from a provided array of
// interpolation values.
static Spline GenerateSplineFromTValues(IReadOnlyList<float3> allPoints, int rangeStart, int rangeEnd, bool closed, float[] tValues, float3 leftTangent, float3 rightTangent)
{
var curRangePointCount = rangeEnd - rangeStart + 1;
var aLeft = new float3[curRangePointCount];
var aRight = new float3[curRangePointCount];
for (int i = 0; i < curRangePointCount; ++i)
{
var t = tValues[i];
aLeft[i] = leftTangent * 3f * t * (1f - t) * (1f - t);
aRight[i] = rightTangent * 3f * t * t * (1f - t);
}
var c = new float[2, 2];
float x1 = 0f, x2 = 0f;
for (int i = 0; i < curRangePointCount; ++i)
{
var t = tValues[i];
c[0, 0] += math.dot(aLeft[i], aLeft[i]);
c[0, 1] += math.dot(aLeft[i], aRight[i]);
c[1, 0] = c[0, 1];
c[1, 1] += math.dot(aRight[i], aRight[i]);
var tmp = allPoints[rangeStart + i] - (
allPoints[rangeStart] * (1f - t) * (1f - t) * (1f - t)
+ allPoints[rangeStart] * 3f * t * (1f - t) * (1f - t)
+ allPoints[rangeEnd] * 3f * t * t * (1f - t)
+ allPoints[rangeEnd] * t * t * t
);
x1 += math.dot(aLeft[i], tmp);
x2 += math.dot(aRight[i], tmp);
}
var c0_c1_determinant = c[0, 0] * c[1, 1] - c[1, 0] * c[0, 1];
var c0_x_determinant = c[0, 0] * x2 - c[1, 0] * x1;
var x_c1_determinant = x1 * c[1, 1] - x2 * c[0, 1];
//now we compute the coefficients for our tangents
var alpha1 = c0_c1_determinant == 0 ? 0 : x_c1_determinant / c0_c1_determinant;
var alpha2 = c0_c1_determinant == 0 ? 0 : c0_x_determinant / c0_c1_determinant;
// alpha1 and alpha2 can be zero and in that case the original algorithm just falls back to the value
// of 1/3 distance between the range's first and last points. Omitting this can introduce
// zero tangent curves that break the resulting spline's continuity around certain knots.
var segLength = math.length(allPoints[rangeEnd] - allPoints[rangeStart]);
var epsilon = 1.0e-6f * segLength;
if (alpha1 < epsilon || alpha2 < epsilon)
alpha1 = alpha2 = segLength / 3f;
var first = new BezierKnot(allPoints[rangeStart], leftTangent * -alpha1, leftTangent * alpha1, quaternion.identity);
var second = new BezierKnot(allPoints[rangeEnd], rightTangent * alpha2, rightTangent * -alpha2, quaternion.identity);
var spline = new Spline();
// Use broken tangent modes for now as we'll pick the correct mode once the spline curvature is finalized.
spline.Add(first, TangentMode.Broken);
spline.Add(second, TangentMode.Broken);
spline.Closed = closed;
return spline;
}
// Helper function for FitSplineToPoints that computes the maximum least squares distance error
// and the index of the point for which the error is maximum.
static (float maxError, int maxErrorIndex) ComputeMaxError(IReadOnlyList<float3> allPoints, int rangeStart, int rangeEnd, float3[] positions, float errorThreshold, bool splineClosed)
{
var rangeCount = rangeEnd - rangeStart + 1;
// For closed spline, we force-split the fit curve at the extra/extension points that we add for closed spline points at the beginning.
// This gives the first and last knots additional adjacent knots that help the algorithm to smoothly unify the spline
// at the closing point (essentially treating closed splines as open splines that "loop" one extra knot forward and two extra knots backward).
// This also ensures that for all of the extension points that were added, there will be matching temporary spline knots
// that we can safely remove/merge at the end of the process.
if (splineClosed && rangeCount > 2)
{
var secondKnotIdx = 1;
if (rangeStart < secondKnotIdx && rangeEnd > secondKnotIdx)
return (errorThreshold, secondKnotIdx);
var knotBeforeLastIdx = allPoints.Count - 2;
if (rangeStart < knotBeforeLastIdx && rangeEnd > knotBeforeLastIdx)
return (errorThreshold, knotBeforeLastIdx);
}
// Use least squares difference to evaluate error.
var maxError = 0f;
var splitPoint = 0;
for (int i = 0; i < rangeCount; ++i)
{
var globalIndex = rangeStart + i;
var difference = allPoints[globalIndex] - positions[i];
var error = math.length(difference);
if (error > maxError)
{
maxError = error;
splitPoint = globalIndex;
}
}
return (maxError, splitPoint);
}
/// <summary>
/// Creates a new spline and adds it to the <see cref="ISplineContainer"/>.
/// </summary>
/// <param name="container">The target container.</param>
/// <typeparam name="T">A type that implements <see cref="ISplineContainer"/>.</typeparam>
/// <returns>Returns the spline that was created and added to the container.</returns>
public static Spline AddSpline<T>(this T container) where T : ISplineContainer
{
var spline = new Spline();
AddSpline(container, spline);
return spline;
}
/// <summary>
/// Add a new <see cref="Spline"/> to the <see cref="ISplineContainer"/>.
/// </summary>
/// <param name="container">The target container.</param>
/// <param name="spline">The spline to append to this container.</param>
/// <typeparam name="T">A type that implements <see cref="ISplineContainer"/>.</typeparam>
public static void AddSpline<T>(this T container, Spline spline) where T : ISplineContainer
{
var splines = new List<Spline>(container.Splines);
splines.Add(spline);
container.Splines = splines;
}
/// <summary>
/// Removes a spline from a <see cref="ISplineContainer"/>.
/// </summary>
/// <param name="container">The target container.</param>
/// <param name="splineIndex">The index of the spline to remove from the SplineContainer.</param>
/// <typeparam name="T">A type that implements <see cref="ISplineContainer"/>.</typeparam>
/// <returns>Returns true if the spline was removed from the container.</returns>
public static bool RemoveSplineAt<T>(this T container, int splineIndex) where T : ISplineContainer
{
if (splineIndex < 0 || splineIndex >= container.Splines.Count)
return false;
var splines = new List<Spline>(container.Splines);
splines.RemoveAt(splineIndex);
container.KnotLinkCollection.SplineRemoved(splineIndex);
container.Splines = splines;
return true;
}
/// <summary>
/// Removes a spline from a <see cref="ISplineContainer"/>.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="spline">The spline to remove from the SplineContainer.</param>
/// <typeparam name="T">A type that implements <see cref="ISplineContainer"/>.</typeparam>
/// <returns>Returns true if the spline was removed from the container.</returns>
public static bool RemoveSpline<T>(this T container, Spline spline) where T : ISplineContainer
{
var splines = new List<Spline>(container.Splines);
var index = splines.IndexOf(spline);
if (index < 0)
return false;
splines.RemoveAt(index);
container.KnotLinkCollection.SplineRemoved(index);
container.Splines = splines;
return true;
}
/// <summary>
/// Reorders a spline in a <see cref="ISplineContainer"/>.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="previousSplineIndex">The previous index of the spline to reorder in the SplineContainer.</param>
/// <param name="newSplineIndex">The new index of the spline to reorder in the SplineContainer.</param>
/// <typeparam name="T">A type that implements <see cref="ISplineContainer"/>.</typeparam>
/// <returns>Returns true if the spline was reordered in the container.</returns>
public static bool ReorderSpline<T>(this T container, int previousSplineIndex, int newSplineIndex) where T : ISplineContainer
{
if (previousSplineIndex < 0 || previousSplineIndex >= container.Splines.Count ||
newSplineIndex < 0 || newSplineIndex >= container.Splines.Count)
return false;
var splines = new List<Spline>(container.Splines);
var spline = splines[previousSplineIndex];
splines.RemoveAt(previousSplineIndex);
splines.Insert(newSplineIndex, spline);
container.KnotLinkCollection.SplineIndexChanged(previousSplineIndex, newSplineIndex);
container.Splines = splines;
return true;
}
internal static bool IsIndexValid<T>(T container, SplineKnotIndex index) where T : ISplineContainer
{
return index.Knot >= 0 && index.Knot < container.Splines[index.Spline].Count &&
index.Spline < container.Splines.Count && index.Knot < container.Splines[index.Spline].Count;
}
/// <summary>
/// Sets the position of all knots linked to the knot at `index` in an <see cref="ISplineContainer"/> to the same position.
/// </summary>
/// <param name="container">The target container.</param>
/// <param name="index">The `SplineKnotIndex` of the knot to use to synchronize the positions.</param>
/// <typeparam name="T">A type that implements <see cref="ISplineContainer"/>.</typeparam>
public static void SetLinkedKnotPosition<T>(this T container, SplineKnotIndex index) where T : ISplineContainer
{
if (!container.KnotLinkCollection.TryGetKnotLinks(index, out var knots))
return;
var splines = container.Splines;
var position = splines[index.Spline][index.Knot].Position;
foreach (var i in knots)
{
if (!IsIndexValid(container, i))
return;
var knot = splines[i.Spline][i.Knot];
var originalPosition = knot.Position;
knot.Position = position;
splines[i.Spline].SetKnotNoNotify(i.Knot, knot);
// If the knot has been moved, notifies a knotModified event
if (!Mathf.Approximately(Vector3.Distance(position, originalPosition), 0f))
splines[i.Spline].SetDirty(SplineModification.KnotModified, i.Knot);
}
}
/// <summary>
/// Links two knots in an <see cref="ISplineContainer"/>. The two knots can be on different splines, but both must be in the referenced SplineContainer.
/// If these knots are linked to other knots, all existing links are kept and updated.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="knotA">The first knot to link.</param>
/// <typeparam name="T">A type that implements <see cref="ISplineContainer"/>.</typeparam>
/// <param name="knotB">The second knot to link.</param>
public static void LinkKnots<T>(this T container, SplineKnotIndex knotA, SplineKnotIndex knotB) where T : ISplineContainer
{
bool similarPositions = Mathf.Approximately(math.length(container.Splines[knotA.Spline][knotA.Knot].Position - container.Splines[knotB.Spline][knotB.Knot].Position), 0f);
var knotsToNotify = similarPositions ? null : container.KnotLinkCollection.GetKnotLinks(knotB);
container.KnotLinkCollection.Link(knotA, knotB);
if (knotsToNotify != null)
{
foreach (var ski in knotsToNotify)
container.Splines[ski.Spline].SetDirty(SplineModification.KnotModified, ski.Knot);
}
}
/// <summary>
/// Unlinks several knots from an <see cref="ISplineContainer"/>. A knot in `knots` disconnects from other knots it was linked to.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="knots">The knot to unlink.</param>
/// <typeparam name="T">A type implementing <see cref="ISplineContainer"/>.</typeparam>
public static void UnlinkKnots<T>(this T container, IReadOnlyList<SplineKnotIndex> knots) where T : ISplineContainer
{
foreach (var knot in knots)
container.KnotLinkCollection.Unlink(knot);
}
/// <summary>
/// Checks if two knots from an <see cref="ISplineContainer"/> are linked together.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="knotA">The first knot to check.</param>
/// <param name="knotB">The second knot to check against.</param>
/// <returns>Returns <c>true</c> if <paramref name="knotA"/> and <paramref name="knotB"/> are linked; otherwise, <c>false</c>.</returns>
public static bool AreKnotLinked(this ISplineContainer container, SplineKnotIndex knotA, SplineKnotIndex knotB)
{
if (!container.KnotLinkCollection.TryGetKnotLinks(knotA, out var linkedKnots))
return false;
for (int i = 0; i < linkedKnots.Count; ++i)
if (linkedKnots[i] == knotB)
return true;
return false;
}
/// <summary>
/// Copies knot links between two splines of the same <see cref="ISplineContainer"/>.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="srcSplineIndex">The index of the source spline to copy from.</param>
/// <param name="destSplineIndex">The index of the destination spline to copy to.</param>
/// <typeparam name="T">A type implementing <see cref="ISplineContainer"/>.</typeparam>
/// <remarks>
/// The knot links will only be copied if both of the spline indices are valid and both splines have the same amount of knots.
/// </remarks>
public static void CopyKnotLinks<T>(this T container, int srcSplineIndex, int destSplineIndex) where T : ISplineContainer
{
if ((srcSplineIndex < 0 || srcSplineIndex >= container.Splines.Count) ||
(destSplineIndex < 0 || destSplineIndex >= container.Splines.Count))
return;
var srcSpline = container.Splines[srcSplineIndex];
var dstSpline = container.Splines[destSplineIndex];
if (srcSpline.Count == 0 || srcSpline.Count != dstSpline.Count)
return;
for (int i = 0, c = srcSpline.Count; i < c; ++i)
{
if (container.KnotLinkCollection.TryGetKnotLinks(new SplineKnotIndex(srcSplineIndex, i), out _))
container.KnotLinkCollection.Link(new SplineKnotIndex(srcSplineIndex, i), new SplineKnotIndex(destSplineIndex, i));
}
}
/// <summary>
/// Removes redundant points in a poly line to form a similar shape with fewer points.
/// </summary>
/// <param name="line">The poly line to act on.</param>
/// <param name="epsilon">The maximum distance from the reduced poly line shape for a point to be discarded.</param>
/// <typeparam name="T">The collection type. Usually this will be list or array of float3.</typeparam>
/// <returns>Returns a new list with a poly line matching the shape of the original line with fewer points.</returns>
public static List<float3> ReducePoints<T>(T line, float epsilon = .15f) where T : IList<float3>
{
var ret = new List<float3>();
var rdp = new RamerDouglasPeucker<T>(line);
rdp.Reduce(ret, epsilon);
return ret;
}
/// <summary>
/// Removes redundant points in a poly line to form a similar shape with fewer points.
/// </summary>
/// <param name="line">The poly line to act on.</param>
/// <param name="results">A pre-allocated list to be filled with the new reduced line points.</param>
/// <param name="epsilon">The maximum distance from the reduced poly line shape for a point to be discarded.</param>
/// <typeparam name="T">The collection type. Usually this will be list or array of float3.</typeparam>
public static void ReducePoints<T>(T line, List<float3> results, float epsilon = .15f) where T : IList<float3>
{
var rdp = new RamerDouglasPeucker<T>(line);
rdp.Reduce(results, epsilon);
}
internal static bool AreTangentsModifiable(TangentMode mode)
{
return mode == TangentMode.Broken || mode == TangentMode.Continuous || mode == TangentMode.Mirrored;
}
/// <summary>
/// Reverses the flow direction of a spline.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="splineIndex">The index of the spline to reverse.</param>
public static void ReverseFlow(this ISplineContainer container, int splineIndex)
{
ReverseFlow(new SplineInfo(container, splineIndex));
}
/// <summary>
/// Reverses the flow direction of a spline.
/// </summary>
/// <param name="splineInfo">The spline to reverse.</param>
public static void ReverseFlow(SplineInfo splineInfo)
{
var spline = splineInfo.Spline;
var knots = splineInfo.Spline.ToArray();
var tangentModes = new TangentMode[spline.Count];
for (int i = 0; i < tangentModes.Length; ++i)
tangentModes[i] = spline.GetTangentMode(i);
var splineLinks = new List<List<SplineKnotIndex>>();
// GetAll LinkedKnots on the spline
for(int previousKnotIndex = 0; previousKnotIndex < spline.Count; ++previousKnotIndex)
{
var knot = new SplineKnotIndex(splineInfo.Index, previousKnotIndex);
var collection = splineInfo.Container.KnotLinkCollection.GetKnotLinks(knot).ToList();
splineLinks.Add(collection);
}
//Unlink all knots in the spline
foreach(var linkedKnots in splineLinks)
splineInfo.Container.UnlinkKnots(linkedKnots);
// Reverse order and tangents
for (int previousKnotIndex = 0; previousKnotIndex < spline.Count; ++previousKnotIndex)
{
var knot = knots[previousKnotIndex];
var worldKnot = knot.Transform(splineInfo.LocalToWorld);
var tangentIn = worldKnot.TangentIn;
var tangentOut = worldKnot.TangentOut;
var reverseRotation = quaternion.AxisAngle(math.mul(knot.Rotation, math.up()), math.radians(180));
reverseRotation = math.normalizesafe(reverseRotation);
// Reverse the tangents to keep the same shape while reversing the order
knot.Rotation = math.mul(reverseRotation, knot.Rotation);
if(tangentModes[previousKnotIndex] is TangentMode.Broken)
{
var localRot = quaternion.AxisAngle(math.up(), math.radians(180));
knot.TangentIn = math.rotate(localRot, tangentOut);
knot.TangentOut = math.rotate(localRot, tangentIn);
}
else if(tangentModes[previousKnotIndex] is TangentMode.Continuous)
{
knot.TangentIn = -tangentOut;
knot.TangentOut = -tangentIn;
}
var newKnotIndex = spline.Count - 1 - previousKnotIndex;
spline.SetTangentMode(newKnotIndex, tangentModes[previousKnotIndex]);
spline[newKnotIndex] = knot;
}
//Redo all links
foreach (var linkedKnots in splineLinks)
{
if(linkedKnots.Count == 1)
continue;
var originalKnot = linkedKnots[0];
originalKnot = new SplineKnotIndex(originalKnot.Spline,
originalKnot.Spline.Equals(splineInfo.Index) ? spline.Count - 1 - originalKnot.Knot : originalKnot.Knot);
for(int i = 1; i < linkedKnots.Count; ++i)
{
var knotInfo = linkedKnots[i];
if (knotInfo.Spline.Equals(splineInfo.Index))
linkedKnots[i] = new SplineKnotIndex(splineInfo.Index, spline.Count - 1 - knotInfo.Knot);
splineInfo.Container.LinkKnots(originalKnot, linkedKnots[i]);
}
}
}
/// <summary>
/// Reverses the flow direction of a spline. Should only be used with Splines that aren't inside any container.
/// </summary>
/// <param name="spline">The spline to reverse.</param>
public static void ReverseFlow(Spline spline)
{
var knots = spline.ToArray();
var tangentModes = new TangentMode[spline.Count];
for (int i = 0; i < tangentModes.Length; ++i)
tangentModes[i] = spline.GetTangentMode(i);
// Reverse order and tangents
for (int previousKnotIndex = 0; previousKnotIndex < spline.Count; ++previousKnotIndex)
{
var knot = knots[previousKnotIndex];
var tangentIn = knot.TangentIn;
var tangentOut = knot.TangentOut;
var reverseRotation = quaternion.AxisAngle(math.mul(knot.Rotation, math.up()), math.radians(180));
reverseRotation = math.normalizesafe(reverseRotation);
// Reverse the tangents to keep the same shape while reversing the order
knot.Rotation = math.mul(reverseRotation, knot.Rotation);
if (tangentModes[previousKnotIndex] is TangentMode.Broken)
{
var localRot = quaternion.AxisAngle(math.up(), math.radians(180));
knot.TangentIn = math.rotate(localRot, tangentOut);
knot.TangentOut = math.rotate(localRot, tangentIn);
}
else if (tangentModes[previousKnotIndex] is TangentMode.Continuous)
{
knot.TangentIn = -tangentOut;
knot.TangentOut = -tangentIn;
}
var newKnotIndex = spline.Count - 1 - previousKnotIndex;
spline.SetTangentMode(newKnotIndex, tangentModes[previousKnotIndex]);
spline[newKnotIndex] = knot;
}
}
/// <summary>
/// Joins two splines together at the specified knots. The two splines must belong to the same container and
/// the knots must be at an extremity of their respective splines.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="mainKnot">The first spline extremity to join.</param>
/// <param name="otherKnot">The second spline extremity to join.</param>
/// <returns> The `SplineKnotIndex` of the junction knot.</returns>
/// <remarks>
/// In case a spline needs to be reversed to join the two extremities, the mainKnot defines which spline will be kept.
/// Hence, the second one will be the reversed one.
/// </remarks>
/// <exception cref="ArgumentException">
/// An exception is thrown on impossible join request (out of bounds
/// parameters, knots on the same spline, non-extremity knots)
/// </exception>
public static SplineKnotIndex JoinSplinesOnKnots(this ISplineContainer container, SplineKnotIndex mainKnot, SplineKnotIndex otherKnot)
{
if (mainKnot.Spline == otherKnot.Spline)
{
Debug.LogError("Trying to join Knots already belonging to the same spline.", container as SplineContainer);
return new SplineKnotIndex();
}
if (mainKnot.Spline < 0 || mainKnot.Spline > container.Splines.Count)
{
Debug.LogError($"Spline index {mainKnot.Spline} does not exist for the current container.", container as SplineContainer);
return new SplineKnotIndex();
}
if(otherKnot.Spline < 0 || otherKnot.Spline > container.Splines.Count)
{
Debug.LogError($"Spline index {otherKnot.Spline} does not exist for the current container.", container as SplineContainer);
return new SplineKnotIndex();
}
if(mainKnot.Knot < 0 || mainKnot.Knot > container.Splines[mainKnot.Spline].Count)
{
Debug.LogError($"Knot index {mainKnot.Knot} does not exist for the current container for Spline[{mainKnot.Spline}].", container as SplineContainer);
return new SplineKnotIndex();
}
if(otherKnot.Knot < 0 || otherKnot.Knot > container.Splines[otherKnot.Spline].Count)
{
Debug.LogError($"Knot index {otherKnot.Knot} does not exist for the current container for Spline[{otherKnot.Spline}].", container as SplineContainer);
return new SplineKnotIndex();
}
if(mainKnot.Knot != 0 && mainKnot.Knot != container.Splines[mainKnot.Spline].Count - 1)
{
Debug.LogError($"Knot index {mainKnot.Knot} is not an extremity knot for the current container for Spline[{mainKnot.Spline}]." +
"Only extremity knots can be joined.", container as SplineContainer);
return new SplineKnotIndex();
}
if(otherKnot.Knot != 0 && otherKnot.Knot != container.Splines[otherKnot.Spline].Count - 1)
{
Debug.LogError($"Knot index {otherKnot.Knot} is not an extremity knot for the current container for Spline[{otherKnot.Spline}]." +
"Only extremity knots can be joined.", container as SplineContainer);
return new SplineKnotIndex();
}
var isActiveKnotAtStart = mainKnot.Knot == 0;
var isOtherKnotAtStart = otherKnot.Knot == 0;
//Reverse spline if needed, this is needed when the 2 knots are both starts or ends of their respective spline
if(isActiveKnotAtStart == isOtherKnotAtStart)
//We give more importance to the main knot, so we reverse the spline associated to otherKnot
container.ReverseFlow(otherKnot.Spline);
//Save Links
var links = new List<List<SplineKnotIndex>>();
//Save relevant data before joining the splines
var activeSplineIndex = mainKnot.Spline;
var activeSpline = container.Splines[activeSplineIndex];
var activeSplineCount = activeSpline.Count;
var otherSplineIndex = otherKnot.Spline;
var otherSpline = container.Splines[otherSplineIndex];
var otherSplineCount = otherSpline.Count;
// Get all LinkedKnots on the splines
for(int i = 0; i < activeSplineCount; ++i)
{
var knot = new SplineKnotIndex(mainKnot.Spline, i);
links.Add(container.KnotLinkCollection.GetKnotLinks(knot).ToList());
}
for(int i = 0; i < otherSplineCount; ++i)
{
var knot = new SplineKnotIndex(otherKnot.Spline, i);
links.Add(container.KnotLinkCollection.GetKnotLinks(knot).ToList());
}
//Unlink all knots in the spline
foreach(var linkedKnots in links)
container.UnlinkKnots(linkedKnots);
if(otherSplineCount > 1)
{
//Join Splines
if(isActiveKnotAtStart)
{
//All position from the other spline must be added before the knot A
//Don't copy the last knot of the other spline as this is the one to join
for(int i = otherSplineCount - 2; i >= 0 ; i--)
activeSpline.Insert(0,otherSpline[i], otherSpline.GetTangentMode(i));
}
else
{
//All position from the other spline must be added after the knot A
//Don't copy the first knot of the other spline as this is the one to join
for(int i = 1; i < otherSplineCount; i++)
activeSpline.Add(otherSpline[i], otherSpline.GetTangentMode(i));
}
}
container.RemoveSplineAt(otherSplineIndex);
var newActiveSplineIndex = otherSplineIndex > activeSplineIndex ? activeSplineIndex : mainKnot.Spline - 1;
//Restore links
foreach (var linkedKnots in links)
{
if(linkedKnots.Count == 1)
continue;
for(int i = 0; i < linkedKnots.Count; ++i)
{
var knotInfo = linkedKnots[i];
if(knotInfo.Spline == activeSplineIndex || knotInfo.Spline == otherSplineIndex)
{
var newIndex = knotInfo.Knot;
if(knotInfo.Spline == activeSplineIndex && isActiveKnotAtStart)
newIndex += otherSplineCount - 1;
if(knotInfo.Spline == otherSplineIndex && !isActiveKnotAtStart)
newIndex += activeSplineCount - 1;
linkedKnots[i] = new SplineKnotIndex(activeSplineIndex, newIndex);
}
else
{
if(knotInfo.Spline > otherSplineIndex)
linkedKnots[i] = new SplineKnotIndex(knotInfo.Spline - 1,knotInfo.Knot);
}
}
var originalKnot = linkedKnots[0];
for(int i = 1; i < linkedKnots.Count; ++i)
container.LinkKnots(originalKnot, linkedKnots[i]);
}
return new SplineKnotIndex(newActiveSplineIndex, isActiveKnotAtStart ? otherSplineCount - 1 : mainKnot.Knot);
}
internal static SplineKnotIndex DuplicateKnot(this ISplineContainer container, SplineKnotIndex originalKnot, int targetIndex)
{
var spline = container.Splines[originalKnot.Spline];
var knot = spline[originalKnot.Knot];
spline.Insert(targetIndex, knot);
spline.SetTangentMode(targetIndex, spline.GetTangentMode(originalKnot.Knot));
return new SplineKnotIndex(originalKnot.Spline, targetIndex);
}
/// <summary>
/// Duplicate a spline between 2 knots of a source spline.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="fromKnot">The start knot to use to duplicate the spline.</param>
/// <param name="toKnot">The end knot to use to duplicate the spline.</param>
/// <param name="newSplineIndex">The index of the new created spline in the container.</param>
/// <exception cref="ArgumentException">Thrown when the provided knots aren't valid or aren't on the same spline.</exception>
public static void DuplicateSpline(
this ISplineContainer container,
SplineKnotIndex fromKnot,
SplineKnotIndex toKnot,
out int newSplineIndex)
{
newSplineIndex = -1;
if (!(fromKnot.IsValid() && toKnot.IsValid()))
throw new ArgumentException("Duplicate failed: The 2 provided knots must be valid knots.");
if(fromKnot.Spline != toKnot.Spline)
throw new ArgumentException("Duplicate failed: The 2 provided knots must be on the same Spline.");
var duplicate = container.AddSpline();
//Copy knots to the new spline
int startIndex = Math.Min(fromKnot.Knot, toKnot.Knot);
int toIndex = Math.Max(fromKnot.Knot, toKnot.Knot);
var originalSplineIndex = fromKnot.Spline;
var originalSpline = container.Splines[originalSplineIndex];
newSplineIndex = container.Splines.Count - 1;
for (int i = startIndex; i <= toIndex; ++i)
{
duplicate.Add(originalSpline[i], originalSpline.GetTangentMode(i));
// If the old knot had any links we link both old and new knot.
// This will result in the new knot linking to what the old knot was linked to.
// The old knot being removed right after that takes care of cleaning the old knot from the link.
if (container.KnotLinkCollection.TryGetKnotLinks(new SplineKnotIndex(originalSplineIndex, i), out _))
container.KnotLinkCollection.Link(new SplineKnotIndex(originalSplineIndex, i), new SplineKnotIndex(newSplineIndex, i - startIndex));
}
}
/// <summary>
/// Splits a spline in a SplineContainer into two splines at a specified knot.
/// </summary>
/// <param name="container">The target SplineContainer.</param>
/// <param name="knotInfo">The SplineKnotIndex of the spline to split. </param>
/// <returns>The `SplineKnotIndex` of the first knot of the spline that was created from the split.</returns>
/// <exception cref="IndexOutOfRangeException">
/// An exception is thrown when the knot belongs to a spline not contained by
/// the provided container or when the knot index is out of range.
/// </exception>
public static SplineKnotIndex SplitSplineOnKnot(this ISplineContainer container, SplineKnotIndex knotInfo)
{
if (knotInfo.Spline < 0 || knotInfo.Spline > container.Splines.Count)
{
throw new IndexOutOfRangeException($"Spline index {knotInfo.Spline} does not exist for the current container.");
}
if (knotInfo.Knot < 0 || knotInfo.Knot > container.Splines[knotInfo.Spline].Count)
{
throw new IndexOutOfRangeException($"Knot index {knotInfo.Knot} does not exist for the current container for Spline[{knotInfo.Spline}].");
}
var originalSpline = container.Splines[knotInfo.Spline];
if (originalSpline.Closed)
{
// Unclose and add a knot to the end with the same data
originalSpline.Closed = false;
var firstKnot = new SplineKnotIndex(knotInfo.Spline, 0);
var lastKnot = container.DuplicateKnot(firstKnot, originalSpline.Count);
// If the knot was the first one of the spline nothing else needs to be done to split the knot
if (knotInfo.Knot == 0)
return firstKnot;
// If the knot wasn't the first one we also need need to link both ends of the spline to keep the same spline we had before
// Link knots is recording the changes to prefab instances so we don't need to add a call to RecordPrefabInstancePropertyModifications
container.LinkKnots(firstKnot, lastKnot);
}
// If not closed, split does nothing on the extremity of the spline
else if (knotInfo.Knot == 0 || knotInfo.Knot == originalSpline.Count - 1)
return knotInfo;
container.DuplicateSpline(knotInfo, new SplineKnotIndex(knotInfo.Spline, originalSpline.Count - 1),
out int newSplineIndex);
originalSpline.Resize(knotInfo.Knot + 1);
return new SplineKnotIndex(newSplineIndex, 0);
}
}
}