/// <summary>
/// Modifies a curve in the spline. It must connect with the previous and next curves (if applicable). This requires that the
/// arc length table be recalculated for that curve and all curves after it -- for example, if you update the first curve in the
/// spline, each curve after that would need to be recalculated (could avoid this by caching the lengths on a per-curve basis if you're
/// doing this often, but since the typical case is only updating the last curve, and the entire array needs to be visited anyway, it
/// wouldn't save much).
/// </summary>
/// <param name="index">Index of the curve to update in <see cref="Curves"/>.</param>
/// <param name="curve">The new curve with which to replace it.</param>
public void Update(int index, CubicBezier curve)
{
if (index < 0)
{
throw new IndexOutOfRangeException("Negative index");
}
if (index >= _curves.Count)
{
throw new IndexOutOfRangeException("Curve index " + index + " is out of range (there are " + _curves.Count + " curves in the spline)");
}
if (index > 0 && !VectorHelper.EqualsOrClose(_curves[index - 1].p3, curve.p0))
{
throw new InvalidOperationException("The updated curve at index " + index + " does not connect with the previous curve at index " + (index - 1));
}
if (index < _curves.Count - 1 && !VectorHelper.EqualsOrClose(_curves[index + 1].p0, curve.p3))
{
throw new InvalidOperationException("The updated curve at index " + index + " does not connect with the next curve at index " + (index + 1));
}
_curves[index] = curve;
for (int i = index; i < _curves.Count; i++)
{
UpdateArcLengths(i);
}
}
public static unsafe void GetLeftTangent(int last, Vector2 *pts, int ptsCount, float *arclen, int arclenCount, out Vector2 retval)
{
float totalLen = arclen[arclenCount - 1];
Vector2 p0 = pts[0];
Vector2 tanL = VectorHelper.Normalize(pts[1] - p0);
Vector2 total = tanL;
float weightTotal = 1;
last = Math.Min(CurveFitBase.END_TANGENT_N_PTS, last - 1);
for (int i = 2; i <= last; i++)
{
float ti = 1 - (arclen[i] / totalLen);
float weight = ti * ti * ti;
Vector2 v = VectorHelper.Normalize(pts[i] - p0);
total += v * weight;
weightTotal += weight;
}
// if the vectors add up to zero (ie going opposite directions), there's no way to normalize them
if (VectorHelper.Length(total) > CurveFitBase.EPSILON)
{
tanL = VectorHelper.Normalize(total / weightTotal);
}
retval = tanL;
}
/// <summary>
/// Generates a bezier curve for the segment using a least-squares approximation. for the derivation of this and why it works,
/// see http://read.pudn.com/downloads141/ebook/610086/Graphics_Gems_I.pdf page 626 and beyond. tl;dr: math.
/// </summary>
protected CubicBezier GenerateBezier(int first, int last, VECTOR tanL, VECTOR tanR)
{
#if OPTIMIZED_GENERATEBEZIERSIMD
using var _ = GenerateBezierMarker.Auto();
unsafe
{
using (_pts.ViewAsNativeArray(out var pts))
using (_u.ViewAsNativeArray(out var u))
{
OptimizedHelpers.GenerateBezier(first, last, tanL, tanR, (VECTOR *)pts.GetUnsafePtr(), (FLOAT *)u.GetUnsafePtr(), out var bezier);
return(bezier);
}
}
#elif OPTIMIZED_GENERATEBEZIER
using var _ = GenerateBezierMarker.Auto();
unsafe
{
using (_pts.ViewAsNativeArray(out var pts))
using (_u.ViewAsNativeArray(out var u))
{
OptimizedHelpers.OptimizedGenerateBezier(first, last, tanL, tanR, (VECTOR *)pts.GetUnsafePtr(), (FLOAT *)u.GetUnsafePtr(), out var bezier);
return(bezier);
}
}
#else
using var _ = GenerateBezierMarker.Auto();
List <VECTOR> pts = _pts;
List <FLOAT> u = _u;
int nPts = last - first + 1;
VECTOR p0 = pts[first], p3 = pts[last]; // first and last points of curve are actual points on data
FLOAT c00 = 0, c01 = 0, c11 = 0, x0 = 0, x1 = 0; // matrix members -- both C[0,1] and C[1,0] are the same, stored in c01
for (int i = 1; i < nPts; i++)
{
// Calculate cubic bezier multipliers
FLOAT t = u[i];
FLOAT ti = 1 - t;
FLOAT t0 = ti * ti * ti;
FLOAT t1 = 3 * ti * ti * t;
FLOAT t2 = 3 * ti * t * t;
FLOAT t3 = t * t * t;
// For X matrix; moving this up here since profiling shows it's better up here (maybe a0/a1 not in registers vs only v not in regs)
VECTOR s = (p0 * t0) + (p0 * t1) + (p3 * t2) + (p3 * t3); // NOTE: this would be Q(t) if p1=p0 and p2=p3
VECTOR v = pts[first + i] - s;
// C matrix
VECTOR a0 = tanL * t1;
VECTOR a1 = tanR * t2;
c00 += VectorHelper.Dot(a0, a0);
c01 += VectorHelper.Dot(a0, a1);
c11 += VectorHelper.Dot(a1, a1);
// X matrix
x0 += VectorHelper.Dot(a0, v);
x1 += VectorHelper.Dot(a1, v);
}
// determinents of X and C matrices
FLOAT det_C0_C1 = c00 * c11 - c01 * c01;
FLOAT det_C0_X = c00 * x1 - c01 * x0;
FLOAT det_X_C1 = x0 * c11 - x1 * c01;
FLOAT alphaL = det_X_C1 / det_C0_C1;
FLOAT alphaR = det_C0_X / det_C0_C1;
// if alpha is negative, zero, or very small (or we can't trust it since C matrix is small), fall back to Wu/Barsky heuristic
FLOAT linDist = VectorHelper.Distance(p0, p3);
FLOAT epsilon2 = EPSILON * linDist;
if (Math.Abs(det_C0_C1) < EPSILON || alphaL < epsilon2 || alphaR < epsilon2)
{
FLOAT alpha = linDist / 3;
VECTOR p1 = (tanL * alpha) + p0;
VECTOR p2 = (tanR * alpha) + p3;
return(new CubicBezier(p0, p1, p2, p3));
}
else
{
VECTOR p1 = (tanL * alphaL) + p0;
VECTOR p2 = (tanR * alphaR) + p3;
return(new CubicBezier(p0, p1, p2, p3));
}
#endif
}
/// <summary>
/// Attempts to find a slightly better parameterization for u on the given curve.
/// </summary>
protected void Reparameterize(int first, int last, CubicBezier curve)
{
#if OPTIMIZED_REPARAMETERIZE
using var _ = ReparameterizeMarker.Auto();
unsafe
{
using (_pts.ViewAsNativeArray(out var pts))
using (_u.ViewAsNativeArray(out var u))
OptimizedHelpers.Reparameterize(first, last, curve, (VECTOR *)pts.GetUnsafePtr(), (FLOAT *)u.GetUnsafePtr());
}
#else
using var _ = ReparameterizeMarker.Auto();
List <VECTOR> pts = _pts;
List <FLOAT> u = _u;
int nPts = last - first;
for (int i = 1; i < nPts; i++)
{
VECTOR p = pts[first + i];
FLOAT t = u[i];
FLOAT ti = 1 - t;
// Control vertices for Q'
VECTOR qp0 = (curve.p1 - curve.p0) * 3;
VECTOR qp1 = (curve.p2 - curve.p1) * 3;
VECTOR qp2 = (curve.p3 - curve.p2) * 3;
// Control vertices for Q''
VECTOR qpp0 = (qp1 - qp0) * 2;
VECTOR qpp1 = (qp2 - qp1) * 2;
// Evaluate Q(t), Q'(t), and Q''(t)
VECTOR p0 = curve.Sample(t);
VECTOR p1 = ((ti * ti) * qp0) + ((2 * ti * t) * qp1) + ((t * t) * qp2);
VECTOR p2 = (ti * qpp0) + (t * qpp1);
// these are the actual fitting calculations using http://en.wikipedia.org/wiki/Newton%27s_method
// We can't just use .X and .Y because Unity uses lower-case "x" and "y".
FLOAT num = ((VectorHelper.GetX(p0) - VectorHelper.GetX(p)) * VectorHelper.GetX(p1)) + ((VectorHelper.GetY(p0) - VectorHelper.GetY(p)) * VectorHelper.GetY(p1));
FLOAT den = (VectorHelper.GetX(p1) * VectorHelper.GetX(p1)) + (VectorHelper.GetY(p1) * VectorHelper.GetY(p1)) + ((VectorHelper.GetX(p0) - VectorHelper.GetX(p)) * VectorHelper.GetX(p2)) + ((VectorHelper.GetY(p0) - VectorHelper.GetY(p)) * VectorHelper.GetY(p2));
FLOAT newU = t - num / den;
if (Math.Abs(den) > EPSILON && newU >= 0 && newU <= 1)
{
u[i] = newU;
}
}
#endif
}
/// <summary>
/// Gets the tangent at a given point in the curve.
/// </summary>
protected VECTOR GetCenterTangent(int first, int last, int split)
{
using var _ = GetCenterTangentMarker.Auto();
List <VECTOR> pts = _pts;
List <FLOAT> arclen = _arclen;
// because we want to maintain C1 continuity on the spline, the tangents on either side must be inverses of one another
Debug.Assert(first < split && split < last);
FLOAT splitLen = arclen[split];
VECTOR pSplit = pts[split];
// left side
FLOAT firstLen = arclen[first];
FLOAT partLen = splitLen - firstLen;
VECTOR total = default(VECTOR);
FLOAT weightTotal = 0;
for (int i = Math.Max(first, split - MID_TANGENT_N_PTS); i < split; i++)
{
FLOAT t = (arclen[i] - firstLen) / partLen;
FLOAT weight = t * t * t;
VECTOR v = VectorHelper.Normalize(pts[i] - pSplit);
total += v * weight;
weightTotal += weight;
}
VECTOR tanL = VectorHelper.Length(total) > EPSILON && weightTotal > EPSILON?
VectorHelper.Normalize(total / weightTotal) :
VectorHelper.Normalize(pts[split - 1] - pSplit);
// right side
partLen = arclen[last] - splitLen;
int rMax = Math.Min(last, split + MID_TANGENT_N_PTS);
total = default(VECTOR);
weightTotal = 0;
for (int i = split + 1; i <= rMax; i++)
{
FLOAT ti = 1 - ((arclen[i] - splitLen) / partLen);
FLOAT weight = ti * ti * ti;
VECTOR v = VectorHelper.Normalize(pSplit - pts[i]);
total += v * weight;
weightTotal += weight;
}
VECTOR tanR = VectorHelper.Length(total) > EPSILON && weightTotal > EPSILON?
VectorHelper.Normalize(total / weightTotal) :
VectorHelper.Normalize(pSplit - pts[split + 1]);
// The reason we separate this into two halves is because we want the right and left tangents to be weighted
// equally no matter the weights of the individual parts of them, so that one of the curves doesn't get screwed
// for the pleasure of the other half
total = tanL + tanR;
// Since the points are never coincident, the vector between any two of them will be normalizable, however this can happen in some really
// odd cases when the points are going directly opposite directions (therefore the tangent is undefined)
if (VectorHelper.LengthSquared(total) < EPSILON)
{
// try one last time using only the three points at the center, otherwise just use one of the sides
tanL = VectorHelper.Normalize(pts[split - 1] - pSplit);
tanR = VectorHelper.Normalize(pSplit - pts[split + 1]);
total = tanL + tanR;
return(VectorHelper.LengthSquared(total) < EPSILON ? tanL : VectorHelper.Normalize(total / 2));
}
else
{
return(VectorHelper.Normalize(total / 2));
}
}
public VECTOR Tangent(FLOAT t)
{
return(VectorHelper.Normalize(Derivative(t)));
}
private AddPointResult AddInternal(VECTOR np)
{
using var _ = AddInternalMarker.Auto();
List <VECTOR> pts = _pts;
int last = pts.Count;
Debug.Assert(last != 0); // should always have one point at least
_pts.Add(np);
_arclen.Add(_totalLength = _totalLength + _linDist);
if (last == 1)
{
// This is the second point
Debug.Assert(_result.Count == 0);
VECTOR p0 = pts[0];
VECTOR tanL = VectorHelper.Normalize(np - p0);
VECTOR tanR = -tanL;
_tanL = tanL;
FLOAT alpha = _linDist / 3;
VECTOR p1 = (tanL * alpha) + p0;
VECTOR p2 = (tanR * alpha) + np;
_result.Add(new CubicBezier(p0, p1, p2, np));
return(new AddPointResult(0, true));
}
else
{
int lastCurve = _result.Count - 1;
int first = _first;
// If we're on the first curve, we're free to improve the left tangent
VECTOR tanL = lastCurve == 0 ? GetLeftTangent(last) : _tanL;
// We can always do the end tangent
VECTOR tanR = GetRightTangent(first);
// Try fitting with the new point
int split;
CubicBezier curve;
if (FitCurve(first, last, tanL, tanR, out curve, out split))
{
_result[lastCurve] = curve;
return(new AddPointResult(lastCurve, false));
}
else
{
// Need to split
// first, get mid tangent
VECTOR tanM1 = GetCenterTangent(first, last, split);
VECTOR tanM2 = -tanM1;
// PERHAPS do a full fitRecursive here since its our last chance?
// our left tangent might be based on points outside the new curve (this is possible for mid tangents too
// but since we need to maintain C1 continuity, it's too late to do anything about it)
if (first == 0 && split < END_TANGENT_N_PTS)
{
tanL = GetLeftTangent(split);
}
// do a final pass on the first half of the curve
int unused;
FitCurve(first, split, tanL, tanM1, out curve, out unused);
_result[lastCurve] = curve;
// perpare to fit the second half
FitCurve(split, last, tanM2, tanR, out curve, out unused);
_result.Add(curve);
_first = split;
_tanL = tanM2;
return(new AddPointResult(lastCurve, true));
}
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)] // originally this method wasn't be inlined
#endif
private static FLOAT PerpendicularDistance(VECTOR a, VECTOR b, FLOAT abDist, FLOAT aCrossB, VECTOR p)
{
// a profile with the test data showed that originally this was eating up ~44% of the runtime. So, this went through
// several iterations of optimization and staring at the disassembly. I tried different methods of using cross
// products, doing the computation with larger vector types, etc... this is the best I could do in ~45 minutes
// running on 3 hours of sleep, which is all scalar math, but RyuJIT puts it into XMM registers and does
// ADDSS/SUBSS/MULSS/DIVSS because that's what it likes to do whenever it sees a vector in a function.
FLOAT area = Math.Abs(aCrossB +
VectorHelper.GetX(b) * VectorHelper.GetY(p) + VectorHelper.GetX(p) * VectorHelper.GetY(a) -
VectorHelper.GetX(p) * VectorHelper.GetY(b) - VectorHelper.GetX(a) * VectorHelper.GetY(p));
FLOAT height = area / abDist;
return(height);
}
private static void RdpRecursive(List <VECTOR> pts, FLOAT error, int first, int last, List <int> keepIndex)
{
int nPts = last - first + 1;
if (nPts < 3)
{
return;
}
VECTOR a = pts[first];
VECTOR b = pts[last];
FLOAT abDist = VectorHelper.Distance(a, b);
FLOAT aCrossB = VectorHelper.GetX(a) * VectorHelper.GetY(b) - VectorHelper.GetX(b) * VectorHelper.GetY(a);
FLOAT maxDist = error;
int split = 0;
for (int i = first + 1; i < last - 1; i++)
{
VECTOR p = pts[i];
FLOAT pDist = PerpendicularDistance(a, b, abDist, aCrossB, p);
if (pDist > maxDist)
{
maxDist = pDist;
split = i;
}
}
if (split != 0)
{
keepIndex.Add(split);
RdpRecursive(pts, error, first, split, keepIndex);
RdpRecursive(pts, error, split, last, keepIndex);
}
}
