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);
}
}
/// <summary>
/// Creates a list of equally spaced points that lie on the path described by straight line segments between
/// adjacent points in the source list.
/// </summary>
/// <param name="src">Source list of points.</param>
/// <param name="md">Distance between points on the new path.</param>
/// <returns>List of equally-spaced points on the path.</returns>
public static List <VECTOR> Linearize(List <VECTOR> src, FLOAT md)
{
if (src == null)
{
throw new ArgumentNullException("src");
}
if (md <= VectorHelper.EPSILON)
{
throw new InvalidOperationException("md " + md + " is be less than epislon " + EPSILON);
}
List <VECTOR> dst = new List <VECTOR>();
if (src.Count > 0)
{
VECTOR pp = src[0];
dst.Add(pp);
FLOAT cd = 0;
for (int ip = 1; ip < src.Count; ip++)
{
VECTOR p0 = src[ip - 1];
VECTOR p1 = src[ip];
FLOAT td = VectorHelper.Distance(p0, p1);
if (cd + td > md)
{
FLOAT pd = md - cd;
dst.Add(VectorHelper.Lerp(p0, p1, pd / td));
FLOAT rd = td - pd;
while (rd > md)
{
rd -= md;
VECTOR np = VectorHelper.Lerp(p0, p1, (td - rd) / td);
if (!VectorHelper.EqualsOrClose(np, pp))
{
dst.Add(np);
pp = np;
}
}
cd = rd;
}
else
{
cd += td;
}
}
// last point
VECTOR lp = src[src.Count - 1];
if (!VectorHelper.EqualsOrClose(pp, lp))
{
dst.Add(lp);
}
}
return(dst);
}
/// <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>
/// Builds the arc length array using the points array. Assumes _pts has points and _arclen is empty.
/// </summary>
protected void InitializeArcLengths()
{
List <VECTOR> pts = _pts;
List <FLOAT> arclen = _arclen;
int count = pts.Count;
Debug.Assert(arclen.Count == 0);
arclen.Add(0);
FLOAT clen = 0;
VECTOR pp = pts[0];
for (int i = 1; i < count; i++)
{
VECTOR np = pts[i];
clen += VectorHelper.Distance(pp, np);
arclen.Add(clen);
pp = np;
}
}
/// <summary>
/// Updates the internal arc length array for a curve. Expects the list to contain enough elements.
/// </summary>
private void UpdateArcLengths(int iCurve)
{
CubicBezier curve = _curves[iCurve];
int nSamples = _samplesPerCurve;
List <FLOAT> arclen = _arclen;
FLOAT clen = iCurve > 0 ? arclen[iCurve * nSamples - 1] : 0;
VECTOR pp = curve.p0;
Debug.Assert(arclen.Count >= ((iCurve + 1) * nSamples));
for (int iPoint = 0; iPoint < nSamples; iPoint++)
{
int idx = (iCurve * nSamples) + iPoint;
FLOAT t = (iPoint + 1) / (FLOAT)nSamples;
VECTOR np = curve.Sample(t);
FLOAT d = VectorHelper.Distance(np, pp);
clen += d;
arclen[idx] = clen;
pp = np;
}
}
/// <summary>
/// Adds a data point to the curve builder. This doesn't always result in the generated curve changing immediately.
/// </summary>
/// <param name="p">The data point to add.</param>
/// <returns><see cref="AddPointResult"/> for info about this.</returns>
public AddPointResult AddPoint(VECTOR p)
{
using var _ = AddPointMarker.Auto();
VECTOR prev = _prev;
List <VECTOR> pts = _pts;
int count = pts.Count;
if (count != 0)
{
FLOAT td = VectorHelper.Distance(prev, p);
FLOAT md = _linDist;
if (td > md)
{
int first = int.MaxValue;
bool add = false;
FLOAT rd = td - md;
// OPTIMIZE if we're adding many points at once, we could do them in a batch
VECTOR dir = VectorHelper.Normalize(p - prev);
using var marker = AddPointDoWhileLoopMarker.Auto();
do
{
VECTOR np = prev + dir * md;
AddPointResult res = AddInternal(np);
first = Math.Min(first, res.FirstChangedIndex);
add |= res.WasAdded;
prev = np;
rd -= md;
} while(rd > md);
_prev = prev;
return(new AddPointResult(first, add));
}
return(AddPointResult.NO_CHANGE);
}
else
{
_prev = p;
_pts.Add(p);
_arclen.Add(0);
return(AddPointResult.NO_CHANGE); // no curves were actually added yet
}
}
/// <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
}