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.

5 files changed, 1924 insertions, 0 deletions

diff --git a/extern/curve_fit_nd/CMakeLists.txt b/extern/curve_fit_nd/CMakeLists.txt
new file mode 100644
index 00000000000..6669971aa2d
--- /dev/null
+++ b/extern/curve_fit_nd/CMakeLists.txt
@@ -0,0 +1,35 @@
+# ***** BEGIN GPL LICENSE BLOCK *****
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software Foundation,
+# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+#
+# ***** END GPL LICENSE BLOCK *****
+
+set(INC
+	.
+)
+
+set(INC_SYS
+
+)
+
+set(SRC
+	intern/curve_fit_cubic.c
+	intern/curve_fit_corners_detect.c
+
+	intern/curve_fit_inline.h
+	curve_fit_nd.h
+)
+
+blender_add_lib(extern_curve_fit_nd "${SRC}" "${INC}" "${INC_SYS}")
diff --git a/extern/curve_fit_nd/curve_fit_nd.h b/extern/curve_fit_nd/curve_fit_nd.h
new file mode 100644
index 00000000000..67b0ed75b8e
--- /dev/null
+++ b/extern/curve_fit_nd/curve_fit_nd.h
@@ -0,0 +1,125 @@
+/*
+ * 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.
+ */
+
+#ifndef __SPLINE_FIT__
+#define __SPLINE_FIT__
+
+/** \file curve_fit_nd.h
+ *  \ingroup curve_fit
+ */
+
+
+/* curve_fit_cubic.c */
+
+/**
+ * Takes a flat array of points and evalues that to calculate a bezier spline.
+ *
+ * \param points, points_len: The array of points to calculate a cubics from.
+ * \param dims: The number of dimensions for for each element in \a points.
+ * \param error_threshold: the error threshold to allow for,
+ * the curve will be within this distance from \a points.
+ * \param corners, corners_len: indices for points which will not have aligned tangents (optional).
+ * This can use the output of #curve_fit_corners_detect_db which has been included
+ * to evaluate a line to detect corner indices.
+ *
+ * \param r_cubic_array, r_cubic_array_len: Resulting array of tangents and knots, formatted as follows:
+ * ``r_cubic_array[r_cubic_array_len][3][dims]``,
+ * where each point has 0 and 2 for the tangents and the middle index 1 for the knot.
+ * The size of the *flat* array will be ``r_cubic_array_len * 3 * dims``.
+ * \param r_corner_index_array, r_corner_index_len: Corner indices in in \a r_cubic_array (optional).
+ * This allows you to access corners on the resulting curve.
+ *
+ * \returns zero on success, nonzero is reserved for error values.
+ */
+int curve_fit_cubic_from_points_db(
+        const double       *points,
+        const unsigned int  points_len,
+        const unsigned int  dims,
+        const double        error_threshold,
+        const unsigned int *corners,
+        unsigned int        corners_len,
+
+        double **r_cubic_array, unsigned int *r_cubic_array_len,
+        unsigned int **r_cubic_orig_index,
+        unsigned int **r_corner_index_array, unsigned int *r_corner_index_len);
+
+int curve_fit_cubic_from_points_fl(
+        const float        *points,
+        const unsigned int  points_len,
+        const unsigned int  dims,
+        const float         error_threshold,
+        const unsigned int *corners,
+        const unsigned int  corners_len,
+
+        float **r_cubic_array, unsigned int *r_cubic_array_len,
+        unsigned int **r_cubic_orig_index,
+        unsigned int **r_corners_index_array, unsigned int *r_corners_index_len);
+
+
+/* curve_fit_corners_detect.c */
+
+/**
+ * A helper function that takes a line and outputs its corner indices.
+ *
+ * \param points, points_len: Curve to evaluate.
+ * \param dims: The number of dimensions for for each element in \a points.
+ * \param radius_min: Corners on the curve between points below this radius are ignored.
+ * \param radius_max: Corners on the curve above this radius are ignored.
+ * \param samples_max: Prevent testing corners beyond this many points
+ * (prevents a large radius taking excessive time to compute).
+ * \param angle_threshold: Angles above this value are considered corners
+ * (higher value for fewer corners).
+ *
+ * \param r_corners, r_corners_len: Resulting array of corners.
+ *
+ * \returns zero on success, nonzero is reserved for error values.
+ */
+int curve_fit_corners_detect_db(
+        const double      *points,
+        const unsigned int points_len,
+        const unsigned int dims,
+        const double       radius_min,
+        const double       radius_max,
+        const unsigned int samples_max,
+        const double       angle_threshold,
+
+        unsigned int **r_corners,
+        unsigned int  *r_corners_len);
+
+int curve_fit_corners_detect_fl(
+        const float       *points,
+        const unsigned int points_len,
+        const unsigned int dims,
+        const float        radius_min,
+        const float        radius_max,
+        const unsigned int samples_max,
+        const float        angle_threshold,
+
+        unsigned int **r_corners,
+        unsigned int  *r_corners_len);
+
+#endif  /* __SPLINE_FIT__ */
diff --git a/extern/curve_fit_nd/intern/curve_fit_corners_detect.c b/extern/curve_fit_nd/intern/curve_fit_corners_detect.c
new file mode 100644
index 00000000000..c0258182774
--- /dev/null
+++ b/extern/curve_fit_nd/intern/curve_fit_corners_detect.c
@@ -0,0 +1,468 @@
+/*
+ * Copyright (c) 2016, Blender Foundation.
+ * 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_corners_detect.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"
+
+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
+
+/* -------------------------------------------------------------------- */
+
+/** \name Simple Vector Math Lib
+ * \{ */
+
+static double angle_vnvnvn_cos(
+        const double v0[], const double v1[], const double v2[],
+        const uint dims)
+{
+#ifdef USE_VLA
+	double dvec0[dims];
+	double dvec1[dims];
+#else
+	double *dvec0 = alloca(sizeof(double) * dims);
+	double *dvec1 = alloca(sizeof(double) * dims);
+#endif
+	normalize_vn_vnvn(dvec0, v0, v1, dims);
+	normalize_vn_vnvn(dvec1, v1, v2, dims);
+	double d = dot_vnvn(dvec0, dvec1, dims);
+	/* sanity check */
+	d = max(-1.0, min(1.0, d));
+	return d;
+}
+
+static double angle_vnvnvn(
+        const double v0[], const double v1[], const double v2[],
+        const uint dims)
+{
+	return acos(angle_vnvnvn_cos(v0, v1, v2, dims));
+}
+
+
+static bool isect_line_sphere_vn(
+        const double l1[],
+        const double l2[],
+        const double sp[],
+        const double r,
+        uint dims,
+
+        double r_p1[]
+#if 0   /* UNUSED */
+        double r_p2[]
+#endif
+        )
+{
+#ifdef USE_VLA
+	double ldir[dims];
+	double tvec[dims];
+#else
+	double *ldir = alloca(sizeof(double) * dims);
+	double *tvec = alloca(sizeof(double) * dims);
+#endif
+
+	sub_vn_vnvn(ldir, l2, l1, dims);
+
+	sub_vn_vnvn(tvec, l1, sp, dims);
+	const double a = len_squared_vn(ldir, dims);
+	const double b = 2.0 * dot_vnvn(ldir, tvec, dims);
+	const double c = len_squared_vn(sp, dims) + len_squared_vn(l1, dims) - (2.0 * dot_vnvn(sp, l1, dims)) - sq(r);
+
+	const double i = b * b - 4.0 * a * c;
+
+	if ((i < 0.0) || (a == 0.0)) {
+		return false;
+	}
+	else if (i == 0.0) {
+		/* one intersection */
+		const double mu = -b / (2.0 * a);
+		mul_vnvn_fl(r_p1, ldir, mu, dims);
+		iadd_vnvn(r_p1, l1, dims);
+		return true;
+	}
+	else if (i > 0.0) {
+		/* # avoid calc twice */
+		const double i_sqrt = sqrt(i);
+		double mu;
+
+		/* Note: when l1 is inside the sphere and l2 is outside.
+		 * the first intersection point will always be between the pair. */
+
+		/* first intersection */
+		mu = (-b + i_sqrt) / (2.0 * a);
+		mul_vnvn_fl(r_p1, ldir, mu, dims);
+		iadd_vnvn(r_p1, l1, dims);
+#if 0
+		/* second intersection */
+		mu = (-b - i_sqrt) / (2.0 * a);
+		mul_vnvn_fl(r_p2, ldir, mu, dims);
+		iadd_vnvn(r_p2, l1, dims);
+#endif
+		return true;
+	}
+	else {
+		return false;
+	}
+}
+
+/** \} */
+
+
+/* -------------------------------------------------------------------- */
+
+
+static bool point_corner_measure(
+        const double *points,
+        const uint    points_len,
+        const uint i,
+        const uint i_prev_init,
+        const uint i_next_init,
+        const double radius,
+        const uint samples_max,
+        const uint dims,
+
+        double r_p_prev[], uint *r_i_prev_next,
+        double r_p_next[], uint *r_i_next_prev)
+{
+	const double *p = &points[i * dims];
+	uint sample;
+
+
+	uint i_prev = i_prev_init;
+	uint i_prev_next = i_prev + 1;
+	sample = 0;
+	while (true) {
+		if ((i_prev == -1) || (sample++ > samples_max)) {
+			return false;
+		}
+		else if (len_squared_vnvn(p, &points[i_prev * dims], dims) < radius) {
+			i_prev -= 1;
+		}
+		else {
+			break;
+		}
+	}
+
+	uint i_next = i_next_init;
+	uint i_next_prev = i_next - 1;
+	sample = 0;
+	while (true) {
+		if ((i_next == points_len) || (sample++ > samples_max)) {
+			return false;
+		}
+		else if (len_squared_vnvn(p, &points[i_next * dims], dims) < radius) {
+			i_next += 1;
+		}
+		else {
+			break;
+		}
+	}
+
+	/* find points on the sphere */
+	if (!isect_line_sphere_vn(
+	        &points[i_prev * dims], &points[i_prev_next * dims], p, radius, dims,
+	        r_p_prev))
+	{
+		return false;
+	}
+
+	if (!isect_line_sphere_vn(
+	        &points[i_next * dims], &points[i_next_prev * dims], p, radius, dims,
+	        r_p_next))
+	{
+		return false;
+	}
+
+	*r_i_prev_next = i_prev_next;
+	*r_i_next_prev = i_next_prev;
+
+	return true;
+}
+
+
+static double point_corner_angle(
+        const double *points,
+        const uint    points_len,
+        const uint i,
+        const double radius_mid,
+        const double radius_max,
+        const double angle_threshold,
+        const double angle_threshold_cos,
+        /* prevent locking up when for example `radius_min` is very large
+         * (possibly larger then the curve).
+         * In this case we would end up checking every point from every other point,
+         * never reaching one that was outside the `radius_min`. */
+
+        /* prevent locking up when for e */
+        const uint samples_max,
+
+        const uint dims)
+{
+	assert(angle_threshold_cos == cos(angle_threshold));
+
+	if (i == 0 || i == points_len - 1) {
+		return 0.0;
+	}
+
+	const double *p = &points[i * dims];
+
+	/* initial test */
+	if (angle_vnvnvn_cos(&points[(i - 1) * dims], p, &points[(i + 1) * dims], dims) > angle_threshold_cos) {
+		return 0.0;
+	}
+
+#ifdef USE_VLA
+	double p_mid_prev[dims];
+	double p_mid_next[dims];
+#else
+	double *p_mid_prev = alloca(sizeof(double) * dims);
+	double *p_mid_next = alloca(sizeof(double) * dims);
+#endif
+
+	uint i_mid_prev_next, i_mid_next_prev;
+	if (point_corner_measure(
+	        points, points_len,
+	        i, i - 1, i + 1,
+	        radius_mid,
+	        samples_max,
+	        dims,
+
+	        p_mid_prev, &i_mid_prev_next,
+	        p_mid_next, &i_mid_next_prev))
+	{
+		const double angle_mid_cos = angle_vnvnvn_cos(p_mid_prev, p, p_mid_next, dims);
+
+		/* compare as cos and flip direction */
+
+		/* if (angle_mid > angle_threshold) { */
+		if (angle_mid_cos < angle_threshold_cos) {
+#ifdef USE_VLA
+			double p_max_prev[dims];
+			double p_max_next[dims];
+#else
+			double *p_max_prev = alloca(sizeof(double) * dims);
+			double *p_max_next = alloca(sizeof(double) * dims);
+#endif
+
+			uint i_max_prev_next, i_max_next_prev;
+			if (point_corner_measure(
+			        points, points_len,
+			        i, i - 1, i + 1,
+			        radius_max,
+			        samples_max,
+			        dims,
+
+			        p_max_prev, &i_max_prev_next,
+			        p_max_next, &i_max_next_prev))
+			{
+				const double angle_mid = acos(angle_mid_cos);
+				const double angle_max = angle_vnvnvn(p_max_prev, p, p_max_next, dims) / 2.0;
+				const double angle_diff = angle_mid - angle_max;
+				if (angle_diff > angle_threshold) {
+					return angle_diff;
+				}
+			}
+		}
+	}
+
+	return 0.0;
+}
+
+
+int curve_fit_corners_detect_db(
+        const double *points,
+        const uint    points_len,
+        const uint dims,
+        const double radius_min,  /* ignore values below this */
+        const double radius_max,  /* ignore values above this */
+        const uint samples_max,
+        const double angle_threshold,
+
+        uint **r_corners,
+        uint  *r_corners_len)
+{
+	const double angle_threshold_cos = cos(angle_threshold);
+	uint corners_len = 0;
+
+	/* Use the difference in angle between the mid-max radii
+	 * to detect the difference between a corner and a sharp turn. */
+	const double radius_mid = (radius_min + radius_max) / 2.0;
+
+	/* we could ignore first/last- but simple to keep aligned with the point array */
+	double *points_angle = malloc(sizeof(double) * points_len);
+	points_angle[0] = 0.0;
+
+	*r_corners = NULL;
+	*r_corners_len = 0;
+
+	for (uint i = 0; i < points_len; i++) {
+		points_angle[i] =  point_corner_angle(
+		        points, points_len, i,
+		        radius_mid, radius_max,
+		        angle_threshold, angle_threshold_cos,
+		        samples_max,
+		        dims);
+
+		if (points_angle[i] != 0.0) {
+			corners_len++;
+		}
+	}
+
+	if (corners_len == 0) {
+		free(points_angle);
+		return 0;
+	}
+
+	/* Clean angle limits!
+	 *
+	 * How this works:
+	 * - Find contiguous 'corners' (where the distance is less or equal to the error threshold).
+	 * - Keep track of the corner with the highest angle
+	 * - Clear every other angle (so they're ignored when setting corners). */
+	{
+		const double radius_min_sq = sq(radius_min);
+		uint i_span_start = 0;
+		while (i_span_start < points_len) {
+			uint i_span_end = i_span_start;
+			if (points_angle[i_span_start] != 0.0) {
+				uint i_next = i_span_start + 1;
+				uint i_best = i_span_start;
+				while (i_next < points_len) {
+					if ((points_angle[i_next] == 0.0) ||
+					   (len_squared_vnvn(
+					        &points[(i_next - 1) * dims],
+					        &points[i_next * dims], dims) > radius_min_sq))
+					{
+						break;
+					}
+					else {
+						if (points_angle[i_best] < points_angle[i_next]) {
+							i_best = i_next;
+						}
+						i_span_end = i_next;
+						i_next += 1;
+					}
+				}
+
+				if (i_span_start != i_span_end) {
+					uint i = i_span_start;
+					while (i <= i_span_end) {
+						if (i != i_best) {
+							/* we could use some other error code */
+							assert(points_angle[i] != 0.0);
+							points_angle[i] = 0.0;
+							corners_len--;
+						}
+						i += 1;
+					}
+				}
+			}
+			i_span_start = i_span_end + 1;
+		}
+	}
+	/* End angle limit cleaning! */
+
+	corners_len += 2;  /* first and last */
+	uint *corners = malloc(sizeof(uint) * corners_len);
+	uint i_corner = 0;
+	corners[i_corner++] = 0;
+	for (uint i = 0; i < points_len; i++) {
+		if (points_angle[i] != 0.0) {
+			corners[i_corner++] = i;
+		}
+	}
+	corners[i_corner++] = points_len - 1;
+	assert(i_corner == corners_len);
+
+	free(points_angle);
+
+	*r_corners = corners;
+	*r_corners_len = corners_len;
+
+	return 0;
+}
+
+int curve_fit_corners_detect_fl(
+        const float *points,
+        const uint   points_len,
+        const uint dims,
+        const float radius_min,  /* ignore values below this */
+        const float radius_max,  /* ignore values above this */
+        const uint samples_max,
+        const float angle_threshold,
+
+        uint **r_corners,
+        uint  *r_corners_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];
+	}
+
+	int result = curve_fit_corners_detect_db(
+	        points_db, points_len,
+	        dims,
+	        radius_min, radius_max,
+	        samples_max,
+	        angle_threshold,
+	        r_corners, r_corners_len);
+
+	free(points_db);
+
+	return result;
+}
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;
+}
+
+/** \} */
diff --git a/extern/curve_fit_nd/intern/curve_fit_inline.h b/extern/curve_fit_nd/intern/curve_fit_inline.h
new file mode 100644
index 00000000000..17aa02be3e5
--- /dev/null
+++ b/extern/curve_fit_nd/intern/curve_fit_inline.h
@@ -0,0 +1,262 @@
+/*
+ * Copyright (c) 2016, Blender Foundation.
+ * 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_inline.h
+ *  \ingroup curve_fit
+ */
+
+/** \name Simple Vector Math Lib
+ * \{ */
+
+#ifdef _MSC_VER
+#  define MINLINE static __forceinline
+#else
+#  define MINLINE static inline
+#endif
+
+MINLINE double sq(const double d)
+{
+	return d * d;
+}
+
+#ifndef _MSC_VER
+MINLINE double min(const double a, const double b)
+{
+	return b < a ? b : a;
+}
+
+MINLINE double max(const double a, const double b)
+{
+	return a < b ? b : a;
+}
+#endif
+
+MINLINE void zero_vn(
+		double v0[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v0[j] = 0.0;
+	}
+}
+
+MINLINE void flip_vn_vnvn(
+		double v_out[], const double v0[], const double v1[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v_out[j] = v0[j] + (v0[j] - v1[j]);
+	}
+}
+
+MINLINE void copy_vnvn(
+        double v0[], const double v1[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v0[j] = v1[j];
+	}
+}
+
+MINLINE double dot_vnvn(
+        const double v0[], const double v1[], const uint dims)
+{
+	double d = 0.0;
+	for (uint j = 0; j < dims; j++) {
+		d += v0[j] * v1[j];
+	}
+	return d;
+}
+
+MINLINE void add_vn_vnvn(
+        double v_out[], const double v0[], const double v1[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v_out[j] = v0[j] + v1[j];
+	}
+}
+
+MINLINE void sub_vn_vnvn(
+        double v_out[], const double v0[], const double v1[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v_out[j] = v0[j] - v1[j];
+	}
+}
+
+MINLINE void iadd_vnvn(
+        double v0[], const double v1[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v0[j] += v1[j];
+	}
+}
+
+MINLINE void isub_vnvn(
+        double v0[], const double v1[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v0[j] -= v1[j];
+	}
+}
+
+MINLINE void madd_vn_vnvn_fl(
+        double v_out[],
+        const double v0[], const double v1[],
+        const double f, const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v_out[j] = v0[j] + v1[j] * f;
+	}
+}
+
+MINLINE void msub_vn_vnvn_fl(
+        double v_out[],
+        const double v0[], const double v1[],
+        const double f, const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v_out[j] = v0[j] - v1[j] * f;
+	}
+}
+
+MINLINE void miadd_vn_vn_fl(
+        double v_out[], const double v0[], double f, const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v_out[j] += v0[j] * f;
+	}
+}
+
+#if 0
+MINLINE void misub_vn_vn_fl(
+        double v_out[], const double v0[], double f, const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v_out[j] -= v0[j] * f;
+	}
+}
+#endif
+
+MINLINE void mul_vnvn_fl(
+        double v_out[],
+        const double v0[], const double f, const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v_out[j] = v0[j] * f;
+	}
+}
+
+MINLINE void imul_vn_fl(double v0[], const double f, const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v0[j] *= f;
+	}
+}
+
+
+MINLINE double len_squared_vnvn(
+		const double v0[], const double v1[], const uint dims)
+{
+	double d = 0.0;
+	for (uint j = 0; j < dims; j++) {
+		d += sq(v0[j] - v1[j]);
+	}
+	return d;
+}
+
+MINLINE double len_squared_vn(
+        const double v0[], const uint dims)
+{
+	double d = 0.0;
+	for (uint j = 0; j < dims; j++) {
+		d += sq(v0[j]);
+	}
+	return d;
+}
+
+MINLINE double len_vnvn(
+        const double v0[], const double v1[], const uint dims)
+{
+	return sqrt(len_squared_vnvn(v0, v1, dims));
+}
+
+#if 0
+static double len_vn(
+		const double v0[], const uint dims)
+{
+	return sqrt(len_squared_vn(v0, dims));
+}
+
+MINLINE double normalize_vn(
+        double v0[], const uint dims)
+{
+	double d = len_squared_vn(v0, dims);
+	if (d != 0.0 && ((d = sqrt(d)) != 0.0)) {
+		imul_vn_fl(v0, 1.0 / d, dims);
+	}
+	return d;
+}
+#endif
+
+/* v_out = (v0 - v1).normalized() */
+MINLINE double normalize_vn_vnvn(
+        double v_out[],
+        const double v0[], const double v1[], const uint dims)
+{
+	double d = 0.0;
+	for (uint j = 0; j < dims; j++) {
+		double a = v0[j] - v1[j];
+		d += sq(a);
+		v_out[j] = a;
+	}
+	if (d != 0.0 && ((d = sqrt(d)) != 0.0)) {
+		imul_vn_fl(v_out, 1.0 / d, dims);
+	}
+	return d;
+}
+
+MINLINE bool is_almost_zero_ex(double val, double eps)
+{
+	return (-eps < val) && (val < eps);
+}
+
+MINLINE bool is_almost_zero(double val)
+{
+	return is_almost_zero_ex(val, 1e-8);
+}
+
+MINLINE bool equals_vnvn(
+		const double v0[], const double v1[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		if (v0[j] != v1[j]) {
+			return false;
+		}
+	}
+	return true;
+}
+
+/** \} */