diff --git a/build_files/cmake/macros.cmake b/build_files/cmake/macros.cmake
index 2b7a43b7e969aae7b1692ffffd644f4bcbea9b97..85bc4008346065fc0540057abbbe0210703f344d 100644
--- a/build_files/cmake/macros.cmake
+++ b/build_files/cmake/macros.cmake
@@ -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
diff --git a/doc/doxygen/doxygen.extern.h b/doc/doxygen/doxygen.extern.h
index 6c6872ff53fafb6f27e58ba44f0319490f360419..27b53ad24ab079a20f305bf6f96aafdb83755363 100644
--- a/doc/doxygen/doxygen.extern.h
+++ b/doc/doxygen/doxygen.extern.h
@@ -11,6 +11,10 @@
*
*/
+/** \defgroup curve_fit Curve Fitting Library
+ * \ingroup extern
+ */
+
/** \defgroup bullet Bullet Physics Library
* \ingroup extern
* \see \ref bulletdoc
diff --git a/extern/CMakeLists.txt b/extern/CMakeLists.txt
index 1cce7dc24cc9aaf478a6ed8264dc9af6d785deed..58e0c68148b56262e45f65bcba5619c3e6d62a86 100644
--- a/extern/CMakeLists.txt
+++ b/extern/CMakeLists.txt
@@ -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
index 0000000000000000000000000000000000000000..6669971aa2d8584b24ee6738fd03116eccba23e2
--- /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 0000000000000000000000000000000000000000..67b0ed75b8ef031beb8b618bcf8f1d190c402f6f
--- /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 0000000000000000000000000000000000000000..c02581827749c4003bb8ae42a018199a6557b7e8
--- /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 0000000000000000000000000000000000000000..d623b51ed0b8f9b83b54885c554902197fbf84b2
--- /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 0000000000000000000000000000000000000000..17aa02be3e5b42a81b572ff900f4f33ed904d773
--- /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;
+}
+
+/** \} */