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 }