Add lib for n-dimensional cubic curve fitting
authorCampbell Barton <ideasman42@gmail.com>
Fri, 15 Apr 2016 08:02:17 +0000 (18:02 +1000)
committerCampbell Barton <ideasman42@gmail.com>
Fri, 15 Apr 2016 10:33:58 +0000 (20:33 +1000)
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.

build_files/cmake/macros.cmake
doc/doxygen/doxygen.extern.h
extern/CMakeLists.txt
extern/curve_fit_nd/CMakeLists.txt [new file with mode: 0644]
extern/curve_fit_nd/curve_fit_nd.h [new file with mode: 0644]
extern/curve_fit_nd/intern/curve_fit_corners_detect.c [new file with mode: 0644]
extern/curve_fit_nd/intern/curve_fit_cubic.c [new file with mode: 0644]
extern/curve_fit_nd/intern/curve_fit_inline.h [new file with mode: 0644]

index 2b7a43b..85bc400 100644 (file)
@@ -578,6 +578,7 @@ function(SETUP_BLENDER_SORTED_LIBS)
                ge_phys_bullet
                bf_intern_smoke
                extern_lzma
+               extern_curve_fit_nd
                ge_logic_ketsji
                extern_recastnavigation
                ge_logic
index 6c6872f..27b53ad 100644 (file)
  *
  */
 
+/** \defgroup curve_fit Curve Fitting Library
+ *  \ingroup extern
+ */
+
 /** \defgroup bullet Bullet Physics Library
  *  \ingroup extern
  *  \see \ref bulletdoc
index 1cce7dc..58e0c68 100644 (file)
@@ -23,6 +23,9 @@
 #
 # ***** END GPL LICENSE BLOCK *****
 
+# Libs that adhere to strict flags
+add_subdirectory(curve_fit_nd)
+
 # Otherwise we get warnings here that we cant fix in external projects
 remove_strict_flags()
 
diff --git a/extern/curve_fit_nd/CMakeLists.txt b/extern/curve_fit_nd/CMakeLists.txt
new file mode 100644 (file)
index 0000000..6669971
--- /dev/null
@@ -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 (file)
index 0000000..67b0ed7
--- /dev/null
@@ -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 (file)
index 0000000..c025818
--- /dev/null
@@ -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 (file)
index 0000000..d623b51
--- /dev/null
@@ -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 (file)
index 0000000..17aa02b
--- /dev/null
@@ -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;
+}
+
+/** \} */