Add lib for n-dimensional cubic curve fitting

This will be used for calculating bezier curves from freehand drawing,
may be used for other areas too.

Original code from GraphicsGems, 1990 (FitCurve.c),
with updates from OpenToonz, under 3 clause BSD license.
with own minor modifications for integration with Blender:
- support adding extra custom-data.
- improved handle clamping.

1 files changed, 1034 insertions, 0 deletions

diff --git a/extern/curve_fit_nd/intern/curve_fit_cubic.c b/extern/curve_fit_nd/intern/curve_fit_cubic.c
new file mode 100644
index 00000000000..d623b51ed0b
--- /dev/null
+++ b/extern/curve_fit_nd/intern/curve_fit_cubic.c
@@ -0,0 +1,1034 @@
+/*
+ * Copyright (c) 2016, DWANGO Co., Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the <organization> nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \file curve_fit_cubic.c
+ *  \ingroup curve_fit
+ */
+
+#include <math.h>
+#include <float.h>
+#include <stdbool.h>
+#include <assert.h>
+
+#include <string.h>
+#include <stdlib.h>
+
+#include "../curve_fit_nd.h"
+
+/* avoid re-calculating lengths multiple times */
+#define USE_LENGTH_CACHE
+
+/* store the indices in the cubic data so we can return the original indices,
+ * useful when the caller has data assosiated with the curve. */
+#define USE_ORIG_INDEX_DATA
+
+typedef unsigned int uint;
+
+#include "curve_fit_inline.h"
+
+#ifdef _MSC_VER
+#  define alloca(size) _alloca(size)
+#endif
+
+#if !defined(_MSC_VER)
+#  define USE_VLA
+#endif
+
+#ifdef USE_VLA
+#  ifdef __GNUC__
+#    pragma GCC diagnostic ignored "-Wvla"
+#  endif
+#else
+#  ifdef __GNUC__
+#    pragma GCC diagnostic error "-Wvla"
+#  endif
+#endif
+
+#define SWAP(type, a, b)  {    \
+	type sw_ap;                \
+	sw_ap = (a);               \
+	(a) = (b);                 \
+	(b) = sw_ap;               \
+} (void)0
+
+
+/* -------------------------------------------------------------------- */
+
+/** \name Cubic Type & Functions
+ * \{ */
+
+typedef struct Cubic {
+	/* single linked lists */
+	struct Cubic *next;
+#ifdef USE_ORIG_INDEX_DATA
+	uint orig_span;
+#endif
+	/* 0: point_0, 1: handle_0, 2: handle_1, 3: point_1,
+	 * each one is offset by 'dims' */
+	double pt_data[0];
+} Cubic;
+
+#define CUBIC_PT(cubic, index, dims) \
+	(&(cubic)->pt_data[(index) * (dims)])
+
+#define CUBIC_VARS(c, dims, _p0, _p1, _p2, _p3) \
+	double \
+	*_p0 = (c)->pt_data, \
+	*_p1 = _p0 + (dims), \
+	*_p2 = _p1 + (dims), \
+	*_p3 = _p2 + (dims); ((void)0)
+#define CUBIC_VARS_CONST(c, dims, _p0, _p1, _p2, _p3) \
+	const double \
+	*_p0 = (c)->pt_data, \
+	*_p1 = _p0 + (dims), \
+	*_p2 = _p1 + (dims), \
+	*_p3 = _p2 + (dims); ((void)0)
+
+
+static Cubic *cubic_alloc(const uint dims)
+{
+	return malloc(sizeof(Cubic) + (sizeof(double) * 4 * dims));
+}
+
+static void cubic_init(
+        Cubic *cubic,
+        const double p0[], const double p1[], const double p2[], const double p3[],
+        const uint dims)
+{
+	copy_vnvn(CUBIC_PT(cubic, 0, dims), p0, dims);
+	copy_vnvn(CUBIC_PT(cubic, 1, dims), p1, dims);
+	copy_vnvn(CUBIC_PT(cubic, 2, dims), p2, dims);
+	copy_vnvn(CUBIC_PT(cubic, 3, dims), p3, dims);
+}
+
+static void cubic_free(Cubic *cubic)
+{
+	free(cubic);
+}
+
+/** \} */
+
+
+/* -------------------------------------------------------------------- */
+
+/** \name CubicList Type & Functions
+ * \{ */
+
+typedef struct CubicList {
+	struct Cubic *items;
+	uint          len;
+	uint          dims;
+} CubicList;
+
+static void cubic_list_prepend(CubicList *clist, Cubic *cubic)
+{
+	cubic->next = clist->items;
+	clist->items = cubic;
+	clist->len++;
+}
+
+static double *cubic_list_as_array(
+        const CubicList *clist
+#ifdef USE_ORIG_INDEX_DATA
+        ,
+        const uint index_last,
+        uint *r_orig_index
+#endif
+        )
+{
+	const uint dims = clist->dims;
+	const uint array_flat_len = (clist->len + 1) * 3 * dims;
+
+	double *array = malloc(sizeof(double) * array_flat_len);
+	const double *handle_prev = &((Cubic *)clist->items)->pt_data[dims];
+
+#ifdef USE_ORIG_INDEX_DATA
+	uint orig_index_value = index_last;
+	uint orig_index_index = clist->len;
+	bool use_orig_index = (r_orig_index != NULL);
+#endif
+
+	/* fill the array backwards */
+	const size_t array_chunk = 3 * dims;
+	double *array_iter = array + array_flat_len;
+	for (Cubic *citer = clist->items; citer; citer = citer->next) {
+		array_iter -= array_chunk;
+		memcpy(array_iter, &citer->pt_data[2 * dims], sizeof(double) * 2 * dims);
+		memcpy(&array_iter[2 * dims], &handle_prev[dims], sizeof(double) * dims);
+		handle_prev = citer->pt_data;
+
+#ifdef USE_ORIG_INDEX_DATA
+		if (use_orig_index) {
+			r_orig_index[orig_index_index--] = orig_index_value;
+			orig_index_value -= citer->orig_span;
+		}
+#endif
+	}
+
+#ifdef USE_ORIG_INDEX_DATA
+	if (use_orig_index) {
+		assert(orig_index_index == 0);
+		assert(orig_index_value == 0 || index_last == 0);
+		r_orig_index[orig_index_index] = index_last ? orig_index_value : 0;
+
+	}
+#endif
+
+	/* flip tangent for first and last (we could leave at zero, but set to something useful) */
+
+	/* first */
+	array_iter -= array_chunk;
+	memcpy(&array_iter[dims], handle_prev, sizeof(double) * 2 * dims);
+	flip_vn_vnvn(&array_iter[0 * dims], &array_iter[1 * dims], &array_iter[2 * dims], dims);
+	assert(array == array_iter);
+
+	/* last */
+	array_iter += array_flat_len - (3 * dims);
+	flip_vn_vnvn(&array_iter[2 * dims], &array_iter[1 * dims], &array_iter[0 * dims], dims);
+
+	return array;
+}
+
+static void cubic_list_clear(CubicList *clist)
+{
+	Cubic *cubic_next;
+	for (Cubic *citer = clist->items; citer; citer = cubic_next) {
+		cubic_next = citer->next;
+		cubic_free(citer);
+	}
+	clist->items = NULL;
+	clist->len  = 0;
+}
+
+/** \} */
+
+
+/* -------------------------------------------------------------------- */
+
+/** \name Cubic Evaluation
+ * \{ */
+
+static void cubic_evaluate(
+        const Cubic *cubic, const double t, const uint dims,
+        double r_v[])
+{
+	CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
+	const double s = 1.0 - t;
+
+	for (uint j = 0; j < dims; j++) {
+		const double p01 = (p0[j] * s) + (p1[j] * t);
+		const double p12 = (p1[j] * s) + (p2[j] * t);
+		const double p23 = (p2[j] * s) + (p3[j] * t);
+		r_v[j] = ((((p01 * s) + (p12 * t))) * s) +
+		         ((((p12 * s) + (p23 * t))) * t);
+	}
+}
+
+static void cubic_calc_point(
+        const Cubic *cubic, const double t, const uint dims,
+        double r_v[])
+{
+	CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
+	const double s = 1.0 - t;
+	for (uint j = 0; j < dims; j++) {
+		r_v[j] = p0[j] * s * s * s +
+		         3.0 * t * s * (s * p1[j] + t * p2[j]) + t * t * t * p3[j];
+	}
+}
+
+static void cubic_calc_speed(
+        const Cubic *cubic, const double t, const uint dims,
+        double r_v[])
+{
+	CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
+	const double s = 1.0 - t;
+	for (uint j = 0; j < dims; j++) {
+		r_v[j] =  3.0 * ((p1[j] - p0[j]) * s * s + 2.0 *
+		                 (p2[j] - p0[j]) * s * t +
+		                 (p3[j] - p2[j]) * t * t);
+	}
+}
+
+static void cubic_calc_acceleration(
+        const Cubic *cubic, const double t, const uint dims,
+        double r_v[])
+{
+	CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
+    const double s = 1.0 - t;
+	for (uint j = 0; j < dims; j++) {
+		r_v[j] = 6.0 * ((p2[j] - 2.0 * p1[j] + p0[j]) * s +
+		                (p3[j] - 2.0 * p2[j] + p1[j]) * t);
+	}
+}
+
+/**
+ * Returns a 'measure' of the maximal discrepancy of the points specified
+ * by points_offset from the corresponding cubic(u[]) points.
+ */
+static void cubic_calc_error(
+        const Cubic *cubic,
+        const double *points_offset,
+        const uint points_offset_len,
+        const double *u,
+        const uint dims,
+
+        double *r_error_sq_max,
+        uint *r_error_index)
+{
+	double error_sq_max = 0.0;
+	uint   error_index = 0;
+
+	const double *pt_real = points_offset + dims;
+#ifdef USE_VLA
+	double        pt_eval[dims];
+#else
+	double       *pt_eval = alloca(sizeof(double) * dims);
+#endif
+
+	for (uint i = 1; i < points_offset_len - 1; i++, pt_real += dims) {
+		cubic_evaluate(cubic, u[i], dims, pt_eval);
+
+		const double err_sq = len_squared_vnvn(pt_real, pt_eval, dims);
+		if (err_sq >= error_sq_max) {
+			error_sq_max = err_sq;
+			error_index = i;
+		}
+	}
+
+	*r_error_sq_max   = error_sq_max;
+	*r_error_index = error_index;
+}
+
+/**
+ * Bezier multipliers
+ */
+
+static double B1(double u)
+{
+	double tmp = 1.0 - u;
+	return 3.0 * u * tmp * tmp;
+}
+
+static double B2(double u)
+{
+	return 3.0 * u * u * (1.0 - u);
+}
+
+static double B0plusB1(double u)
+{
+    double tmp = 1.0 - u;
+    return tmp * tmp * (1.0 + 2.0 * u);
+}
+
+static double B2plusB3(double u)
+{
+    return u * u * (3.0 - 2.0 * u);
+}
+
+static void points_calc_center_weighted(
+        const double *points_offset,
+        const uint    points_offset_len,
+        const uint    dims,
+
+        double r_center[])
+{
+	/*
+	 * Calculate a center that compensates for point spacing.
+	 */
+
+	const double *pt_prev = &points_offset[(points_offset_len - 2) * dims];
+	const double *pt_curr = pt_prev + dims;
+	const double *pt_next = points_offset;
+
+	double w_prev = len_vnvn(pt_prev, pt_curr, dims);
+
+	zero_vn(r_center, dims);
+	double w_tot = 0.0;
+
+	for (uint i_next = 0; i_next < points_offset_len; i_next++) {
+		const double w_next = len_vnvn(pt_curr, pt_next, dims);
+		const double w = w_prev + w_next;
+		w_tot += w;
+
+		miadd_vn_vn_fl(r_center, pt_curr, w, dims);
+
+		w_prev = w_next;
+
+		pt_prev = pt_curr;
+		pt_curr = pt_next;
+		pt_next += dims;
+	}
+
+	if (w_tot != 0.0) {
+		imul_vn_fl(r_center, 1.0 / w_tot, dims);
+	}
+}
+
+/**
+ * Use least-squares method to find Bezier control points for region.
+ */
+static void cubic_from_points(
+        const double *points_offset,
+        const uint    points_offset_len,
+        const double *u_prime,
+        const double  tan_l[],
+        const double  tan_r[],
+        const uint dims,
+
+        Cubic *r_cubic)
+{
+
+	const double *p0 = &points_offset[0];
+	const double *p3 = &points_offset[(points_offset_len - 1) * dims];
+
+	/* Point Pairs */
+	double alpha_l, alpha_r;
+#ifdef USE_VLA
+	double a[2][dims];
+	double tmp[dims];
+#else
+	double *a[2] = {
+	    alloca(sizeof(double) * dims),
+	    alloca(sizeof(double) * dims),
+	};
+	double *tmp = alloca(sizeof(double) * dims);
+#endif
+
+	{
+		double x[2] = {0.0}, c[2][2] = {{0.0}};
+		const double *pt = points_offset;
+
+		for (uint i = 0; i < points_offset_len; i++, pt += dims) {
+			mul_vnvn_fl(a[0], tan_l, B1(u_prime[i]), dims);
+			mul_vnvn_fl(a[1], tan_r, B2(u_prime[i]), dims);
+
+			c[0][0] += dot_vnvn(a[0], a[0], dims);
+			c[0][1] += dot_vnvn(a[0], a[1], dims);
+			c[1][1] += dot_vnvn(a[1], a[1], dims);
+
+			c[1][0] = c[0][1];
+
+			{
+				const double b0_plus_b1 = B0plusB1(u_prime[i]);
+				const double b2_plus_b3 = B2plusB3(u_prime[i]);
+				for (uint j = 0; j < dims; j++) {
+					tmp[j] = (pt[j] - (p0[j] * b0_plus_b1)) + (p3[j] * b2_plus_b3);
+				}
+
+				x[0] += dot_vnvn(a[0], tmp, dims);
+				x[1] += dot_vnvn(a[1], tmp, dims);
+			}
+		}
+
+		double det_C0_C1 = c[0][0] * c[1][1] - c[0][1] * c[1][0];
+		double det_C_0X  = x[1]    * c[0][0] - x[0]    * c[0][1];
+		double det_X_C1  = x[0]    * c[1][1] - x[1]    * c[0][1];
+
+		if (is_almost_zero(det_C0_C1)) {
+			det_C0_C1 = c[0][0] * c[1][1] * 10e-12;
+		}
+
+		/* may still divide-by-zero, check below will catch nan values */
+		alpha_l = det_X_C1 / det_C0_C1;
+		alpha_r = det_C_0X / det_C0_C1;
+	}
+
+	/*
+	 * The problem that the stupid values for alpha dare not put
+	 * only when we realize that the sign and wrong,
+	 * but even if the values are too high.
+	 * But how do you evaluate it?
+	 *
+	 * Meanwhile, we should ensure that these values are sometimes
+	 * so only problems absurd of approximation and not for bugs in the code.
+	 */
+
+	/* flip check to catch nan values */
+	if (!(alpha_l >= 0.0) ||
+	    !(alpha_r >= 0.0))
+	{
+		alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
+	}
+
+	double *p1 = CUBIC_PT(r_cubic, 1, dims);
+	double *p2 = CUBIC_PT(r_cubic, 2, dims);
+
+	copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims);
+	copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims);
+
+#ifdef USE_ORIG_INDEX_DATA
+	r_cubic->orig_span = (points_offset_len - 1);
+#endif
+
+	/* p1 = p0 - (tan_l * alpha_l);
+	 * p2 = p3 + (tan_r * alpha_r);
+	 */
+	msub_vn_vnvn_fl(p1, p0, tan_l, alpha_l, dims);
+	madd_vn_vnvn_fl(p2, p3, tan_r, alpha_r, dims);
+
+	/* ------------------------------------
+	 * Clamping (we could make it optional)
+	 */
+#ifdef USE_VLA
+	double center[dims];
+#else
+	double *center = alloca(sizeof(double) * dims);
+#endif
+	points_calc_center_weighted(points_offset, points_offset_len, dims, center);
+
+	const double clamp_scale = 3.0;  /* clamp to 3x */
+	double dist_sq_max = 0.0;
+
+	{
+		const double *pt = points_offset;
+		for (uint i = 0; i < points_offset_len; i++, pt += dims) {
+#if 0
+			double dist_sq_test = sq(len_vnvn(center, pt, dims) * clamp_scale);
+#else
+			/* do inline */
+			double dist_sq_test = 0.0;
+			for (uint j = 0; j < dims; j++) {
+				dist_sq_test += sq((pt[j] - center[j]) * clamp_scale);
+			}
+#endif
+			dist_sq_max = max(dist_sq_max, dist_sq_test);
+		}
+	}
+
+	double p1_dist_sq = len_squared_vnvn(center, p1, dims);
+	double p2_dist_sq = len_squared_vnvn(center, p2, dims);
+
+	if (p1_dist_sq > dist_sq_max ||
+	    p2_dist_sq > dist_sq_max)
+	{
+
+		alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
+
+		/*
+		 * p1 = p0 - (tan_l * alpha_l);
+		 * p2 = p3 + (tan_r * alpha_r);
+		 */
+		for (uint j = 0; j < dims; j++) {
+			p1[j] = p0[j] - (tan_l[j] * alpha_l);
+			p2[j] = p3[j] + (tan_r[j] * alpha_r);
+		}
+
+		p1_dist_sq = len_squared_vnvn(center, p1, dims);
+		p2_dist_sq = len_squared_vnvn(center, p2, dims);
+	}
+
+	/* clamp within the 3x radius */
+	if (p1_dist_sq > dist_sq_max) {
+		isub_vnvn(p1, center, dims);
+		imul_vn_fl(p1, sqrt(dist_sq_max) / sqrt(p1_dist_sq), dims);
+		iadd_vnvn(p1, center, dims);
+	}
+	if (p2_dist_sq > dist_sq_max) {
+		isub_vnvn(p2, center, dims);
+		imul_vn_fl(p2, sqrt(dist_sq_max) / sqrt(p2_dist_sq), dims);
+		iadd_vnvn(p2, center, dims);
+	}
+	/* end clamping */
+}
+
+#ifdef USE_LENGTH_CACHE
+static void points_calc_coord_length_cache(
+        const double *points_offset,
+        const uint    points_offset_len,
+        const uint    dims,
+
+        double     *r_points_length_cache)
+{
+	const double *pt_prev = points_offset;
+	const double *pt = pt_prev + dims;
+	r_points_length_cache[0] = 0.0;
+	for (uint i = 1; i < points_offset_len; i++) {
+		r_points_length_cache[i] = len_vnvn(pt, pt_prev, dims);
+		pt_prev = pt;
+		pt += dims;
+	}
+}
+#endif  /* USE_LENGTH_CACHE */
+
+
+static void points_calc_coord_length(
+        const double *points_offset,
+        const uint    points_offset_len,
+        const uint    dims,
+#ifdef USE_LENGTH_CACHE
+        const double *points_length_cache,
+#endif
+        double *r_u)
+{
+	const double *pt_prev = points_offset;
+	const double *pt = pt_prev + dims;
+	r_u[0] = 0.0;
+	for (uint i = 1, i_prev = 0; i < points_offset_len; i++) {
+		double length;
+
+#ifdef USE_LENGTH_CACHE
+		length = points_length_cache[i];
+
+		assert(len_vnvn(pt, pt_prev, dims) == points_length_cache[i]);
+#else
+		length = len_vnvn(pt, pt_prev, dims);
+#endif
+
+		r_u[i] = r_u[i_prev] + length;
+		i_prev = i;
+		pt_prev = pt;
+		pt += dims;
+	}
+	assert(!is_almost_zero(r_u[points_offset_len - 1]));
+	const double w = r_u[points_offset_len - 1];
+	for (uint i = 0; i < points_offset_len; i++) {
+		r_u[i] /= w;
+	}
+}
+
+/**
+ * Use Newton-Raphson iteration to find better root.
+ *
+ * \param cubic: Current fitted curve.
+ * \param p: Point to test against.
+ * \param u: Parameter value for \a p.
+ *
+ * \note Return value may be `nan` caller must check for this.
+ */
+static double cubic_find_root(
+		const Cubic *cubic,
+		const double p[],
+		const double u,
+		const uint dims)
+{
+	/* Newton-Raphson Method. */
+	/* all vectors */
+#ifdef USE_VLA
+	double q0_u[dims];
+	double q1_u[dims];
+	double q2_u[dims];
+#else
+	double *q0_u = alloca(sizeof(double) * dims);
+	double *q1_u = alloca(sizeof(double) * dims);
+	double *q2_u = alloca(sizeof(double) * dims);
+#endif
+
+	cubic_calc_point(cubic, u, dims, q0_u);
+	cubic_calc_speed(cubic, u, dims, q1_u);
+	cubic_calc_acceleration(cubic, u, dims, q2_u);
+
+	/* may divide-by-zero, caller must check for that case */
+	/* u - ((q0_u - p) * q1_u) / (q1_u.length_squared() + (q0_u - p) * q2_u) */
+	isub_vnvn(q0_u, p, dims);
+	return u - dot_vnvn(q0_u, q1_u, dims) /
+	       (len_squared_vn(q1_u, dims) + dot_vnvn(q0_u, q2_u, dims));
+}
+
+static int compare_double_fn(const void *a_, const void *b_)
+{
+	const double *a = a_;
+	const double *b = b_;
+	if      (*a > *b) return  1;
+	else if (*a < *b) return -1;
+	else              return  0;
+}
+
+/**
+ * Given set of points and their parameterization, try to find a better parameterization.
+ */
+static bool cubic_reparameterize(
+        const Cubic *cubic,
+        const double *points_offset,
+        const uint    points_offset_len,
+        const double *u,
+        const uint    dims,
+
+        double       *r_u_prime)
+{
+	/*
+	 * Recalculate the values of u[] based on the Newton Raphson method
+	 */
+
+	const double *pt = points_offset;
+	for (uint i = 0; i < points_offset_len; i++, pt += dims) {
+		r_u_prime[i] = cubic_find_root(cubic, pt, u[i], dims);
+		if (!isfinite(r_u_prime[i])) {
+			return false;
+		}
+	}
+
+	qsort(r_u_prime, points_offset_len, sizeof(double), compare_double_fn);
+
+	if ((r_u_prime[0] < 0.0) ||
+	    (r_u_prime[points_offset_len - 1] > 1.0))
+	{
+		return false;
+	}
+
+	assert(r_u_prime[0] >= 0.0);
+	assert(r_u_prime[points_offset_len - 1] <= 1.0);
+	return true;
+}
+
+
+static void fit_cubic_to_points(
+        const double *points_offset,
+        const uint    points_offset_len,
+#ifdef USE_LENGTH_CACHE
+        const double *points_length_cache,
+#endif
+        const double  tan_l[],
+        const double  tan_r[],
+        const double  error_threshold,
+        const uint    dims,
+        /* fill in the list */
+        CubicList *clist)
+{
+	const uint iteration_max = 4;
+	const double error_sq = sq(error_threshold);
+
+	Cubic *cubic;
+
+	if (points_offset_len == 2) {
+		cubic = cubic_alloc(dims);
+		CUBIC_VARS(cubic, dims, p0, p1, p2, p3);
+
+		copy_vnvn(p0, &points_offset[0 * dims], dims);
+		copy_vnvn(p3, &points_offset[1 * dims], dims);
+
+		const double dist = len_vnvn(p0, p3, dims) / 3.0;
+		msub_vn_vnvn_fl(p1, p0, tan_l, dist, dims);
+		madd_vn_vnvn_fl(p2, p3, tan_r, dist, dims);
+
+#ifdef USE_ORIG_INDEX_DATA
+		cubic->orig_span = 1;
+#endif
+
+		cubic_list_prepend(clist, cubic);
+		return;
+	}
+
+	double *u = malloc(sizeof(double) * points_offset_len);
+	points_calc_coord_length(
+	        points_offset, points_offset_len, dims,
+#ifdef USE_LENGTH_CACHE
+	        points_length_cache,
+#endif
+	        u);
+
+	cubic = cubic_alloc(dims);
+
+	double error_sq_max;
+	uint split_index;
+
+	/* Parameterize points, and attempt to fit curve */
+	cubic_from_points(
+	        points_offset, points_offset_len, u, tan_l, tan_r, dims, cubic);
+
+	/* Find max deviation of points to fitted curve */
+	cubic_calc_error(
+	        cubic, points_offset, points_offset_len, u, dims,
+	        &error_sq_max, &split_index);
+
+	if (error_sq_max < error_sq) {
+		free(u);
+		cubic_list_prepend(clist, cubic);
+		return;
+	}
+	else {
+		/* If error not too large, try some reparameterization and iteration */
+		double *u_prime = malloc(sizeof(double) * points_offset_len);
+		for (uint iter = 0; iter < iteration_max; iter++) {
+			if (!cubic_reparameterize(
+			        cubic, points_offset, points_offset_len, u, dims, u_prime))
+			{
+				break;
+			}
+
+			cubic_from_points(
+			        points_offset, points_offset_len, u_prime,
+			        tan_l, tan_r, dims, cubic);
+			cubic_calc_error(
+			        cubic, points_offset, points_offset_len, u_prime, dims,
+			        &error_sq_max, &split_index);
+
+			if (error_sq_max < error_sq) {
+				free(u_prime);
+				free(u);
+				cubic_list_prepend(clist, cubic);
+				return;
+			}
+
+			SWAP(double *, u, u_prime);
+		}
+		free(u_prime);
+	}
+
+	free(u);
+	cubic_free(cubic);
+
+
+	/* Fitting failed -- split at max error point and fit recursively */
+
+	/* Check splinePoint is not an endpoint?
+	 *
+	 * This assert happens sometimes...
+	 * Look into it but disable for now. Campbell! */
+
+	// assert(split_index > 1)
+#ifdef USE_VLA
+	double tan_center[dims];
+#else
+	double *tan_center = alloca(sizeof(double) * dims);
+#endif
+
+	const double *pt_a = &points_offset[(split_index - 1) * dims];
+	const double *pt_b = &points_offset[(split_index + 1) * dims];
+
+	assert(split_index < points_offset_len);
+	if (equals_vnvn(pt_a, pt_b, dims)) {
+		pt_a += dims;
+	}
+
+	/* tan_center = (pt_a - pt_b).normalized() */
+	normalize_vn_vnvn(tan_center, pt_a, pt_b, dims);
+
+	fit_cubic_to_points(
+	        points_offset, split_index + 1,
+#ifdef USE_LENGTH_CACHE
+	        points_length_cache,
+#endif
+	        tan_l, tan_center, error_threshold, dims, clist);
+	fit_cubic_to_points(
+	        &points_offset[split_index * dims], points_offset_len - split_index,
+#ifdef USE_LENGTH_CACHE
+	        points_length_cache + split_index,
+#endif
+	        tan_center, tan_r, error_threshold, dims, clist);
+
+}
+
+/** \} */
+
+
+/* -------------------------------------------------------------------- */
+
+/** \name External API for Curve-Fitting
+ * \{ */
+
+/**
+ * Main function:
+ *
+ * Take an array of 3d points.
+ * return the cubic splines
+ */
+int curve_fit_cubic_from_points_db(
+        const double *points,
+        const uint    points_len,
+        const uint    dims,
+        const double  error_threshold,
+        const uint   *corners,
+        uint          corners_len,
+
+        double **r_cubic_array, uint *r_cubic_array_len,
+        uint **r_cubic_orig_index,
+        uint **r_corner_index_array, uint *r_corner_index_len)
+{
+	uint corners_buf[2];
+	if (corners == NULL) {
+		assert(corners_len == 0);
+		corners_buf[0] = 0;
+		corners_buf[1] = points_len - 1;
+		corners = corners_buf;
+		corners_len = 2;
+	}
+
+	CubicList clist = {0};
+	clist.dims = dims;
+
+#ifdef USE_VLA
+	double tan_l[dims];
+	double tan_r[dims];
+#else
+	double *tan_l = alloca(sizeof(double) * dims);
+	double *tan_r = alloca(sizeof(double) * dims);
+#endif
+
+#ifdef USE_LENGTH_CACHE
+	double *points_length_cache = NULL;
+	uint    points_length_cache_len_alloc = 0;
+#endif
+
+	uint *corner_index_array = NULL;
+	uint  corner_index = 0;
+	if (r_corner_index_array && (corners != corners_buf)) {
+		corner_index_array = malloc(sizeof(uint) * corners_len);
+		corner_index_array[corner_index++] = corners[0];
+	}
+
+	for (uint i = 1; i < corners_len; i++) {
+		const uint points_offset_len = corners[i] - corners[i - 1] + 1;
+		const uint first_point = corners[i - 1];
+
+		assert(points_offset_len >= 1);
+		if (points_offset_len > 1) {
+			const double *pt_l = &points[first_point * dims];
+			const double *pt_r = &points[(first_point + points_offset_len - 1) * dims];
+			const double *pt_l_next = pt_l + dims;
+			const double *pt_r_prev = pt_r - dims;
+
+			/* tan_l = (pt_l - pt_l_next).normalized()
+			 * tan_r = (pt_r_prev - pt_r).normalized()
+			 */
+			normalize_vn_vnvn(tan_l, pt_l, pt_l_next, dims);
+			normalize_vn_vnvn(tan_r, pt_r_prev, pt_r, dims);
+
+#ifdef USE_LENGTH_CACHE
+			if (points_length_cache_len_alloc < points_offset_len) {
+				if (points_length_cache) {
+					free(points_length_cache);
+				}
+				points_length_cache = malloc(sizeof(double) * points_offset_len);
+			}
+			points_calc_coord_length_cache(
+			        &points[first_point * dims], points_offset_len, dims,
+			        points_length_cache);
+#endif
+
+			fit_cubic_to_points(
+			        &points[first_point * dims], points_offset_len,
+#ifdef USE_LENGTH_CACHE
+			        points_length_cache,
+#endif
+			        tan_l, tan_r, error_threshold, dims, &clist);
+		}
+		else if (points_len == 1) {
+			assert(points_offset_len == 1);
+			assert(corners_len == 2);
+			assert(corners[0] == 0);
+			assert(corners[1] == 0);
+			const double *pt = &points[0];
+			Cubic *cubic = cubic_alloc(dims);
+			cubic_init(cubic, pt, pt, pt, pt, dims);
+			cubic_list_prepend(&clist, cubic);
+		}
+
+		if (corner_index_array) {
+			corner_index_array[corner_index++] = clist.len;
+		}
+	}
+
+#ifdef USE_LENGTH_CACHE
+	if (points_length_cache) {
+		free(points_length_cache);
+	}
+#endif
+
+#ifdef USE_ORIG_INDEX_DATA
+	uint *cubic_orig_index = NULL;
+	if (r_cubic_orig_index) {
+		cubic_orig_index = malloc(sizeof(uint) * (clist.len + 1));
+	}
+#else
+	*r_cubic_orig_index = NULL;
+#endif
+
+	/* allocate a contiguous array and free the linked list */
+	*r_cubic_array = cubic_list_as_array(
+	        &clist
+#ifdef USE_ORIG_INDEX_DATA
+	        , corners[corners_len - 1], cubic_orig_index
+#endif
+	        );
+	*r_cubic_array_len = clist.len + 1;
+
+	cubic_list_clear(&clist);
+
+#ifdef USE_ORIG_INDEX_DATA
+	if (cubic_orig_index) {
+		*r_cubic_orig_index = cubic_orig_index;
+	}
+#endif
+
+	if (corner_index_array) {
+		assert(corner_index == corners_len);
+		*r_corner_index_array = corner_index_array;
+		*r_corner_index_len = corner_index;
+	}
+
+	return 0;
+}
+
+/**
+ * A version of #curve_fit_cubic_from_points_db to handle floats
+ */
+int curve_fit_cubic_from_points_fl(
+        const float  *points,
+        const uint    points_len,
+        const uint    dims,
+        const float   error_threshold,
+        const uint   *corners,
+        const uint    corners_len,
+
+        float **r_cubic_array, uint *r_cubic_array_len,
+        uint **r_cubic_orig_index,
+        uint **r_corner_index_array, uint *r_corner_index_len)
+{
+	const uint points_flat_len = points_len * dims;
+	double *points_db = malloc(sizeof(double) * points_flat_len);
+
+	for (uint i = 0; i < points_flat_len; i++) {
+		points_db[i] = (double)points[i];
+	}
+
+	double *cubic_array_db = NULL;
+	float  *cubic_array_fl = NULL;
+	uint    cubic_array_len = 0;
+
+	int result = curve_fit_cubic_from_points_db(
+	        points_db, points_len, dims, error_threshold, corners, corners_len,
+	        &cubic_array_db, &cubic_array_len,
+	        r_cubic_orig_index,
+	        r_corner_index_array, r_corner_index_len);
+	free(points_db);
+
+	if (!result) {
+		uint cubic_array_flat_len = cubic_array_len * 3 * dims;
+		cubic_array_fl = malloc(sizeof(float) * cubic_array_flat_len);
+		for (uint i = 0; i < cubic_array_flat_len; i++) {
+			cubic_array_fl[i] = (float)cubic_array_db[i];
+		}
+		free(cubic_array_db);
+	}
+
+	*r_cubic_array = cubic_array_fl;
+	*r_cubic_array_len = cubic_array_len;
+
+	return result;
+}
+
+/** \} */