Piecewise linear curve approximation implementation of Hamann and Chen (1994).

PiecewiseLinearCurveApproximation.cs

using UnityEngine;
using System;
using System.Linq;
using System.Collections.Generic;
using MathNet.Numerics.Interpolation;
using MathNet.Numerics.RootFinding;
using MoreEnumerable = MoreLinq.MoreEnumerable;

public static class PiecewiseLinearCurveApproximation {

  /** Returns a f(t) where t \in [0, 1]. */
  // public static Func<float, float> Parameterize(IEnumerable<Vector2> _points) {
  //   var points = _points.ToList();
  //   var domain = points
  //     .Zip(points.Skip(1),
  //          (pi_1, pi) => pi_1.x - pi.x)
  //     .ToList();
  //   var chord = domain.Sum();
  //   return t => {
  //     float x_t = t * chord;
  //     float x = 0f;
  //     for (int i = 0; i < points.Count; i++) {
  //       float x_next = x + domain[i]
  //       if (x <= x_t && x_t <= 
  //     }
  //   };

  // }

  /**
     $ asciiTeX '\int_a^b f(t)~dt = C'
       _
      /  b
      |    f(t)~dt  =  C
     _/  a

     Finds b.
   */
  static double SolveIntegralBound(LinearSpline spline, double a, double bMin, double bMax, double C) {
    var F_a = spline.Integrate(a);
    Func<double, double> g = t => F_a - spline.Integrate(t) + C;
    // Find the root of g(t).
    // What's my differential dg?
    Func<double, double> dg = t => - spline.Interpolate(t); // Not differentiate, right?
    double b = RobustNewtonRaphson.FindRoot(g, dg, bMin, bMax, 0.001d);
    return b;
  }

  /**
     Assumes the points (x_i, y_i) are in ascending order by x_i.
   */
  public static IEnumerable<Vector2> SelectPoints(int m, IEnumerable<Vector2> points) {
    var xs = points.ToList();
    // Return the first point.
    yield return xs[0];
    var ks = Curvature(xs).ToList();
    var xbars_ks = xs
      .Zip(ks,
           Tuple.Create)
      // Throw out all the cases where the curvature equals 0.
      .Where(xk => ! Mathf.Approximately(xk.Item2, 0f))
      .ToList();
    var xbars = xbars_ks.Select(xk => xk.Item1).ToList();
    var ki = xbars_ks.Select(xk => xk.Item2).ToList();

    var si = xbars
      .Zip(xbars.Skip(1),
           (xi, xi_1) => (xi_1 - xi).magnitude)
      // .Prepend(0f)
      .ToList();
    var ss = MoreEnumerable.Scan(si, 0f, (a_, b_) => a_ + b_)
      // .Skip(1)
      .ToList();

    // Debug.Log($"counts ki {ki.Count} si {si.Count} ss {ss.Count}");
    // float S = si.Sum();
    // Debug.Log("si " + string.Join(", ", si));
    // Debug.Log("ki " + string.Join(", ", ki));
    // Debug.Log("xbars " + string.Join(", ", xbars));

    int n = ki.Count;
    // Sum up the total weighted curvature.
    float K = 0f;
    K += ki[0] * si[0] / 2f;
    for (int i = 1; i < n - 1; i++)
      K += ki[i] * (si[i - 1] + si[i]) / 2f;
    K += ki[n - 1] * si[n - 2] / 2f;

    // LinearSpline spline = LinearSpline.Interpolate(new [] { 0.0, 1.0, 2.0 },
    //                                                new [] { 3.0, 8.0, 1.0 });
    LinearSpline spline = LinearSpline.Interpolate(ss.Select(y => (double) y),
                                                   ki.Select(x => (double) x));
    // LinearSpline spline = LinearSpline.Interpolate(ki.Select(x => (double) x),
    //                                                xbars.Select(y => (double) y));
    var S_2 = ss.Last();
    // var K_2 = spline.Integrate(0.0, S_2);
    // Debug.Log($"K {K}, K_2 {K_2}, S {S}, S_2 {S_2}");

    // Find the intermediate points.
    int l = 0;
    float Sl = 0f;
    float bMin = 0f;
    float bMax = S_2;
    for (int j = 1; j < m - 1; j++) {
      float tj = (float) SolveIntegralBound(spline, 0f, bMin, bMax, j * K / m);
      // float actual = (float) spline.Integrate(0f, tj);
      // Debug.Log($"C {j * K / m}, tj found {tj}, C_1 {actual}, error {Math.Abs(j * K / m - actual)} ");
      // float tj = j * K / m;
      for (; l < n - 2; l++) {
        // Sl += si[l];
        // float Sl_1 = Sl + si[l + 1];
        Sl = ss[l];
        float Sl_1 = ss[l + 1];
        if (tj > Sl && tj < Sl_1) {
          if (Mathf.Abs(tj - Sl) <= Mathf.Abs(tj - Sl_1)) {
            bMin = tj;
            yield return xbars[l + 1];
            goto inner;
          } else {

            bMin = tj;
            // a = ss[l + 1];
            yield return xbars[l + 2];
            goto inner;
          }
        }
      }
    inner:
      ;
    }
    // Return the last point.
    yield return xs[xs.Count - 1];
  }

  // http://graphics.cs.ucdavis.edu/~hamann/HamannChen1994a.pdf
  public static IEnumerable<float> Curvature(IEnumerable<Vector2> points) {
    var xs = points.ToList();
    for (int i = 1; i < xs.Count - 1; i++) {
      Vector2 d1 = (xs[i - 1] - xs[i]).normalized;
      Vector2 d2 = (xs[i + 1] - xs[i]).normalized;
      Vector2 b1, b2;
      b2 = (d1 + d2).normalized;
      if (! Mathf.Approximately(d1.magnitude, 0f)
          && ! Mathf.Approximately(d2.magnitude, 0f)
          && ! Mathf.Approximately(b2.magnitude, 0f)) {
        // if they're not collinear...
        // b2 = (d1 + d2).normalized;
        b1 = new Vector2(b2.y, -b2.x);
      } else {
        // if they're collinear...
        b1 = (xs[i + 1] - xs[i]).normalized;
        b2 = new Vector2(-b1.y, b1.x);
      }
      float alpha = Vector2.Dot(d1, b1);
      float beta  = Vector2.Dot(d1, b2);

      float gamma = Vector2.Dot(d2, b1);
      float delta = Vector2.Dot(d2, b2);

      Vector2 soln = LinearSolve(new Vector2(alpha, gamma),
                                 new Vector2(alpha * alpha, gamma * gamma),
                                 new Vector2(beta, delta));
      float a1 = soln.x;
      float a2 = soln.y;

      if (i == 1) {
        // Curvature estimate for first value.
        float e = (a1 + 2f * a2 * alpha);
        float k0 = 2f * a2 / Mathf.Pow(1f + e * e, 3f / 2f);
        yield return k0;
      }
      float ki = 2f * a2 / Mathf.Pow(1f + a1 * a1, 3f / 2f);
      if (float.NaN.Equals(ki)) {
        Debug.Log($"Oops. d1 {d1} d2 {d2} alpha {alpha} beta {beta} gamma {gamma}");
      }
      yield return ki;

      if (i == xs.Count - 2) {
        // Curvature estimate for last value.
        float e = (a1 + 2f * a2 * beta);
        float kn = 2f * a2 / Mathf.Pow(1f + e * e, 3f / 2f);
        yield return kn;
      }
    }
  }

  /**
     Solve a small system of equations.

     [ a0 b0 ] [ x ] = [ c0 ]
     [ a1 b1 ] [ y ]   [ c1 ]

     Return (x, y).
   */
  public static Vector2 LinearSolve(Vector2 a, Vector2 b, Vector2 c) {
    Vector3 r1 = new Vector3(a.x, b.x, c.x);
    Vector3 r2 = new Vector3(a.y, b.y, c.y);
    r2 = r2 - (r1 * r2.x/r1.x);
    float y = r2.z / r2.y;
    float x = (r1.z - r1.y * y) / r1.x;
    return new Vector2(x, y);
  }
}