/// <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>
/// Computes the maximum squared distance from a point to the curve using the current parameterization.
/// </summary>
protected FLOAT FindMaxSquaredError(int first, int last, CubicBezier curve, out int split)
{
#if OPTIMIZED_FINDMAXSQUAREDERROR
using var _ = FindMaxSquaredErrorMarker.Auto();
unsafe
{
using (_pts.ViewAsNativeArray(out var pts))
using (_u.ViewAsNativeArray(out var u))
{
OptimizedHelpers.FindMaxSquaredError(first, last, curve, out split, (VECTOR *)pts.GetUnsafePtr(), (FLOAT *)u.GetUnsafePtr(), out var max);
return(max);
}
}
#else
using var _ = FindMaxSquaredErrorMarker.Auto();
List <VECTOR> pts = _pts;
List <FLOAT> u = _u;
int s = (last - first + 1) / 2;
int nPts = last - first + 1;
FLOAT max = 0;
for (int i = 1; i < nPts; i++)
{
VECTOR v0 = pts[first + i];
VECTOR v1 = curve.Sample(u[i]);
FLOAT d = VectorHelper.DistanceSquared(v0, v1);
if (d > max)
{
max = d;
s = i;
}
}
// split at point of maximum error
split = s + first;
if (split <= first)
{
split = first + 1;
}
if (split >= last)
{
split = last - 1;
}
return(max);
#endif
}
public static void RunWholeTest(List <Vector2> list, uint seed)
{
var timings = new Action[]
{
//() =>
//{
// TestUtility.Time($"BMCurves {seed}: {list.Count}",
// () =>
// {
// var s = new BMCurves.CurveBuilder(DistanceBetweenPoints, CurveSplitError);
// for (int i = 0; i < list.Count; i++)
// s.AddPoint(list[i]);
// });
//},
() =>
{
TestUtility.Time($"OPCurves {seed}: {list.Count}",
() =>
{
using var total = OPTotal.Auto();
var s = new OPCurves.CurveBuilder(DistanceBetweenPoints, CurveSplitError);
using var addPoints = OPAddPoints.Auto();
for (int i = 0; i < list.Count; i++)
{
s.AddPoint(list[i]);
}
});
},
//() => {TestUtility.Time($"IEnumerableFilter ({list.Count})", () => {FilterBetween.IEnumerableFilter(list, greaterThan, lessThan); });},
};
#if RANDOM_SHUFFLE_TESTS
TestUtility.RandomShuffle(timings);
#endif
foreach (var timing in timings)
{
timing();
}
Debug.Log($"{new StackFrame(1).GetMethod().Name} finished");
}
/// <summary>
/// Gets the tangent for the start of the cure.
/// </summary>
public VECTOR GetLeftTangent(int last)
{
#if OPTIMIZED_GETLEFTTANGENT
using var _ = GetLeftTangentMarker.Auto();
unsafe
{
using (_pts.ViewAsNativeArray(out var pts))
using (_arclen.ViewAsNativeArray(out var arclen))
{
OptimizedHelpers.GetLeftTangent(last, (VECTOR *)pts.GetUnsafePtr(), pts.Length, (FLOAT *)arclen.GetUnsafePtr(), arclen.Length, out var tanL);
return(tanL);
}
}
#else
using var _ = GetLeftTangentMarker.Auto();
List <VECTOR> pts = _pts;
List <FLOAT> arclen = _arclen;
FLOAT totalLen = arclen[arclen.Count - 1];
VECTOR p0 = pts[0];
VECTOR tanL = VectorHelper.Normalize(pts[1] - p0);
VECTOR total = tanL;
FLOAT weightTotal = 1;
last = Math.Min(END_TANGENT_N_PTS, last - 1);
for (int i = 2; i <= last; i++)
{
FLOAT ti = 1 - (arclen[i] / totalLen);
FLOAT weight = ti * ti * ti;
VECTOR 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) > EPSILON)
{
tanL = VectorHelper.Normalize(total / weightTotal);
}
return(tanL);
#endif
}
/// <summary>
/// Gets the tangent for the the end of the curve.
/// </summary>
public VECTOR GetRightTangent(int first)
{
#if OPTIMIZED_GETRIGHTTANGENT
using var _ = GetRightTangentMarker.Auto();
unsafe
{
using (_pts.ViewAsNativeArray(out var pts))
using (_arclen.ViewAsNativeArray(out var arclen))
{
OptimizedHelpers.GetRightTangent(first, (VECTOR *)pts.GetUnsafePtr(), pts.Length, (FLOAT *)arclen.GetUnsafePtr(), arclen.Length, out var tanR);
return(tanR);
}
}
#else
using var _ = GetRightTangentMarker.Auto();
List <VECTOR> pts = _pts;
List <FLOAT> arclen = _arclen;
FLOAT totalLen = arclen[arclen.Count - 1];
VECTOR p3 = pts[pts.Count - 1];
VECTOR tanR = VectorHelper.Normalize(pts[pts.Count - 2] - p3);
VECTOR total = tanR;
FLOAT weightTotal = 1;
first = Math.Max(pts.Count - (END_TANGENT_N_PTS + 1), first + 1);
for (int i = pts.Count - 3; i >= first; i--)
{
FLOAT t = arclen[i] / totalLen;
FLOAT weight = t * t * t;
VECTOR v = VectorHelper.Normalize(pts[i] - p3);
total += v * weight;
weightTotal += weight;
}
if (VectorHelper.Length(total) > EPSILON)
{
tanR = VectorHelper.Normalize(total / weightTotal);
}
return(tanR);
#endif
}
static StackObject *Auto_3(ILIntepreter __intp, StackObject *__esp, IList <object> __mStack, CLRMethod __method, bool isNewObj)
{
CSHotFix.Runtime.Enviorment.AppDomain __domain = __intp.AppDomain;
StackObject *ptr_of_this_method;
StackObject *__ret = ILIntepreter.Minus(__esp, 1);
ptr_of_this_method = ILIntepreter.Minus(__esp, 1);
ptr_of_this_method = ILIntepreter.GetObjectAndResolveReference(ptr_of_this_method);
Unity.Profiling.ProfilerMarker instance_of_this_method = (Unity.Profiling.ProfilerMarker) typeof(Unity.Profiling.ProfilerMarker).CheckCLRTypes(StackObject.ToObject(ptr_of_this_method, __domain, __mStack));
var result_of_this_method = instance_of_this_method.Auto();
ptr_of_this_method = ILIntepreter.Minus(__esp, 1);
WriteBackInstance(__domain, ptr_of_this_method, __mStack, ref instance_of_this_method);
__intp.Free(ptr_of_this_method);
return(ILIntepreter.PushObject(__ret, __mStack, result_of_this_method));
}
/// <summary>
/// Initializes the first (last - first) elements of u with scaled arc lengths.
/// </summary>
protected void ArcLengthParamaterize(int first, int last)
{
using var _ = ArcLengthParamaterizeMarker.Auto();
List <FLOAT> arclen = _arclen;
List <FLOAT> u = _u;
u.Clear();
FLOAT diff = arclen[last] - arclen[first];
FLOAT start = arclen[first];
int nPts = last - first;
u.Add(0);
for (int i = 1; i < nPts; i++)
{
u.Add((arclen[first + i] - start) / diff);
}
u.Add(1);
}
/// <summary>
/// Tries to fit single Bezier curve to the points in [first ... last]. Destroys anything in <see cref="_u"/> in the process.
/// Assumes there are at least two points to fit.
/// </summary>
/// <param name="first">Index of first point to consider.</param>
/// <param name="last">Index of last point to consider (inclusive).</param>
/// <param name="tanL">Tangent at teh start of the curve ("left").</param>
/// <param name="tanR">Tangent on the end of the curve ("right").</param>
/// <param name="curve">The fitted curve.</param>
/// <param name="split">Point at which to split if this method returns false.</param>
/// <returns>true if the fit was within error tolerence, false if the curve should be split. Even if this returns false, curve will contain
/// a curve that somewhat fits the points; it's just outside error tolerance.</returns>
protected bool FitCurve(int first, int last, VECTOR tanL, VECTOR tanR, out CubicBezier curve, out int split)
{
using var _ = FitCurveMarker.Auto();
List <VECTOR> pts = _pts;
int nPts = last - first + 1;
if (nPts < 2)
{
throw new InvalidOperationException("INTERNAL ERROR: Should always have at least 2 points here");
}
else if (nPts == 2)
{
// if we only have 2 points left, estimate the curve using Wu/Barsky
VECTOR p0 = pts[first];
VECTOR p3 = pts[last];
FLOAT alpha = VectorHelper.Distance(p0, p3) / 3;
VECTOR p1 = (tanL * alpha) + p0;
VECTOR p2 = (tanR * alpha) + p3;
curve = new CubicBezier(p0, p1, p2, p3);
split = 0;
return(true);
}
else
{
split = 0;
ArcLengthParamaterize(first, last); // initially start u with a simple chord-length paramaterization
curve = default(CubicBezier);
for (int i = 0; i < MAX_ITERS + 1; i++)
{
if (i != 0)
{
Reparameterize(first, last, curve); // use newton's method to find better parameters (except on first run, since we don't have a curve yet)
}
curve = GenerateBezier(first, last, tanL, tanR); // generate the curve itself
FLOAT error = FindMaxSquaredError(first, last, curve, out split); // calculate error and get split point (point of max error)
if (error < _squaredError)
{
return(true); // if we're within error tolerance, awesome!
}
}
return(false);
}
}
/// <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>
/// 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));
}
}