diff --git a/build_files/cmake/macros.cmake b/build_files/cmake/macros.cmake index 2b7a43b7e96..85bc4008346 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 6c6872ff53f..27b53ad24ab 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 1cce7dc24cc..58e0c68148b 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 00000000000..6669971aa2d --- /dev/null +++ b/extern/curve_fit_nd/CMakeLists.txt @@ -0,0 +1,35 @@ +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +# +# ***** END GPL LICENSE BLOCK ***** + +set(INC + . +) + +set(INC_SYS + +) + +set(SRC + intern/curve_fit_cubic.c + intern/curve_fit_corners_detect.c + + intern/curve_fit_inline.h + curve_fit_nd.h +) + +blender_add_lib(extern_curve_fit_nd "${SRC}" "${INC}" "${INC_SYS}") diff --git a/extern/curve_fit_nd/curve_fit_nd.h b/extern/curve_fit_nd/curve_fit_nd.h new file mode 100644 index 00000000000..67b0ed75b8e --- /dev/null +++ b/extern/curve_fit_nd/curve_fit_nd.h @@ -0,0 +1,125 @@ +/* + * Copyright (c) 2016, DWANGO Co., Ltd. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the 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 BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef __SPLINE_FIT__ +#define __SPLINE_FIT__ + +/** \file curve_fit_nd.h + * \ingroup curve_fit + */ + + +/* curve_fit_cubic.c */ + +/** + * Takes a flat array of points and evalues that to calculate a bezier spline. + * + * \param points, points_len: The array of points to calculate a cubics from. + * \param dims: The number of dimensions for for each element in \a points. + * \param error_threshold: the error threshold to allow for, + * the curve will be within this distance from \a points. + * \param corners, corners_len: indices for points which will not have aligned tangents (optional). + * This can use the output of #curve_fit_corners_detect_db which has been included + * to evaluate a line to detect corner indices. + * + * \param r_cubic_array, r_cubic_array_len: Resulting array of tangents and knots, formatted as follows: + * ``r_cubic_array[r_cubic_array_len][3][dims]``, + * where each point has 0 and 2 for the tangents and the middle index 1 for the knot. + * The size of the *flat* array will be ``r_cubic_array_len * 3 * dims``. + * \param r_corner_index_array, r_corner_index_len: Corner indices in in \a r_cubic_array (optional). + * This allows you to access corners on the resulting curve. + * + * \returns zero on success, nonzero is reserved for error values. + */ +int curve_fit_cubic_from_points_db( + const double *points, + const unsigned int points_len, + const unsigned int dims, + const double error_threshold, + const unsigned int *corners, + unsigned int corners_len, + + double **r_cubic_array, unsigned int *r_cubic_array_len, + unsigned int **r_cubic_orig_index, + unsigned int **r_corner_index_array, unsigned int *r_corner_index_len); + +int curve_fit_cubic_from_points_fl( + const float *points, + const unsigned int points_len, + const unsigned int dims, + const float error_threshold, + const unsigned int *corners, + const unsigned int corners_len, + + float **r_cubic_array, unsigned int *r_cubic_array_len, + unsigned int **r_cubic_orig_index, + unsigned int **r_corners_index_array, unsigned int *r_corners_index_len); + + +/* curve_fit_corners_detect.c */ + +/** + * A helper function that takes a line and outputs its corner indices. + * + * \param points, points_len: Curve to evaluate. + * \param dims: The number of dimensions for for each element in \a points. + * \param radius_min: Corners on the curve between points below this radius are ignored. + * \param radius_max: Corners on the curve above this radius are ignored. + * \param samples_max: Prevent testing corners beyond this many points + * (prevents a large radius taking excessive time to compute). + * \param angle_threshold: Angles above this value are considered corners + * (higher value for fewer corners). + * + * \param r_corners, r_corners_len: Resulting array of corners. + * + * \returns zero on success, nonzero is reserved for error values. + */ +int curve_fit_corners_detect_db( + const double *points, + const unsigned int points_len, + const unsigned int dims, + const double radius_min, + const double radius_max, + const unsigned int samples_max, + const double angle_threshold, + + unsigned int **r_corners, + unsigned int *r_corners_len); + +int curve_fit_corners_detect_fl( + const float *points, + const unsigned int points_len, + const unsigned int dims, + const float radius_min, + const float radius_max, + const unsigned int samples_max, + const float angle_threshold, + + unsigned int **r_corners, + unsigned int *r_corners_len); + +#endif /* __SPLINE_FIT__ */ diff --git a/extern/curve_fit_nd/intern/curve_fit_corners_detect.c b/extern/curve_fit_nd/intern/curve_fit_corners_detect.c new file mode 100644 index 00000000000..c0258182774 --- /dev/null +++ b/extern/curve_fit_nd/intern/curve_fit_corners_detect.c @@ -0,0 +1,468 @@ +/* + * Copyright (c) 2016, Blender Foundation. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the 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 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 +#include +#include +#include + +#include +#include + +#include "../curve_fit_nd.h" + +typedef unsigned int uint; + +#include "curve_fit_inline.h" + +#ifdef _MSC_VER +# define alloca(size) _alloca(size) +#endif + +#if !defined(_MSC_VER) +# define USE_VLA +#endif + +#ifdef USE_VLA +# ifdef __GNUC__ +# pragma GCC diagnostic ignored "-Wvla" +# endif +#else +# ifdef __GNUC__ +# pragma GCC diagnostic error "-Wvla" +# endif +#endif + +/* -------------------------------------------------------------------- */ + +/** \name Simple Vector Math Lib + * \{ */ + +static double angle_vnvnvn_cos( + const double v0[], const double v1[], const double v2[], + const uint dims) +{ +#ifdef USE_VLA + double dvec0[dims]; + double dvec1[dims]; +#else + double *dvec0 = alloca(sizeof(double) * dims); + double *dvec1 = alloca(sizeof(double) * dims); +#endif + normalize_vn_vnvn(dvec0, v0, v1, dims); + normalize_vn_vnvn(dvec1, v1, v2, dims); + double d = dot_vnvn(dvec0, dvec1, dims); + /* sanity check */ + d = max(-1.0, min(1.0, d)); + return d; +} + +static double angle_vnvnvn( + const double v0[], const double v1[], const double v2[], + const uint dims) +{ + return acos(angle_vnvnvn_cos(v0, v1, v2, dims)); +} + + +static bool isect_line_sphere_vn( + const double l1[], + const double l2[], + const double sp[], + const double r, + uint dims, + + double r_p1[] +#if 0 /* UNUSED */ + double r_p2[] +#endif + ) +{ +#ifdef USE_VLA + double ldir[dims]; + double tvec[dims]; +#else + double *ldir = alloca(sizeof(double) * dims); + double *tvec = alloca(sizeof(double) * dims); +#endif + + sub_vn_vnvn(ldir, l2, l1, dims); + + sub_vn_vnvn(tvec, l1, sp, dims); + const double a = len_squared_vn(ldir, dims); + const double b = 2.0 * dot_vnvn(ldir, tvec, dims); + const double c = len_squared_vn(sp, dims) + len_squared_vn(l1, dims) - (2.0 * dot_vnvn(sp, l1, dims)) - sq(r); + + const double i = b * b - 4.0 * a * c; + + if ((i < 0.0) || (a == 0.0)) { + return false; + } + else if (i == 0.0) { + /* one intersection */ + const double mu = -b / (2.0 * a); + mul_vnvn_fl(r_p1, ldir, mu, dims); + iadd_vnvn(r_p1, l1, dims); + return true; + } + else if (i > 0.0) { + /* # avoid calc twice */ + const double i_sqrt = sqrt(i); + double mu; + + /* Note: when l1 is inside the sphere and l2 is outside. + * the first intersection point will always be between the pair. */ + + /* first intersection */ + mu = (-b + i_sqrt) / (2.0 * a); + mul_vnvn_fl(r_p1, ldir, mu, dims); + iadd_vnvn(r_p1, l1, dims); +#if 0 + /* second intersection */ + mu = (-b - i_sqrt) / (2.0 * a); + mul_vnvn_fl(r_p2, ldir, mu, dims); + iadd_vnvn(r_p2, l1, dims); +#endif + return true; + } + else { + return false; + } +} + +/** \} */ + + +/* -------------------------------------------------------------------- */ + + +static bool point_corner_measure( + const double *points, + const uint points_len, + const uint i, + const uint i_prev_init, + const uint i_next_init, + const double radius, + const uint samples_max, + const uint dims, + + double r_p_prev[], uint *r_i_prev_next, + double r_p_next[], uint *r_i_next_prev) +{ + const double *p = &points[i * dims]; + uint sample; + + + uint i_prev = i_prev_init; + uint i_prev_next = i_prev + 1; + sample = 0; + while (true) { + if ((i_prev == -1) || (sample++ > samples_max)) { + return false; + } + else if (len_squared_vnvn(p, &points[i_prev * dims], dims) < radius) { + i_prev -= 1; + } + else { + break; + } + } + + uint i_next = i_next_init; + uint i_next_prev = i_next - 1; + sample = 0; + while (true) { + if ((i_next == points_len) || (sample++ > samples_max)) { + return false; + } + else if (len_squared_vnvn(p, &points[i_next * dims], dims) < radius) { + i_next += 1; + } + else { + break; + } + } + + /* find points on the sphere */ + if (!isect_line_sphere_vn( + &points[i_prev * dims], &points[i_prev_next * dims], p, radius, dims, + r_p_prev)) + { + return false; + } + + if (!isect_line_sphere_vn( + &points[i_next * dims], &points[i_next_prev * dims], p, radius, dims, + r_p_next)) + { + return false; + } + + *r_i_prev_next = i_prev_next; + *r_i_next_prev = i_next_prev; + + return true; +} + + +static double point_corner_angle( + const double *points, + const uint points_len, + const uint i, + const double radius_mid, + const double radius_max, + const double angle_threshold, + const double angle_threshold_cos, + /* prevent locking up when for example `radius_min` is very large + * (possibly larger then the curve). + * In this case we would end up checking every point from every other point, + * never reaching one that was outside the `radius_min`. */ + + /* prevent locking up when for e */ + const uint samples_max, + + const uint dims) +{ + assert(angle_threshold_cos == cos(angle_threshold)); + + if (i == 0 || i == points_len - 1) { + return 0.0; + } + + const double *p = &points[i * dims]; + + /* initial test */ + if (angle_vnvnvn_cos(&points[(i - 1) * dims], p, &points[(i + 1) * dims], dims) > angle_threshold_cos) { + return 0.0; + } + +#ifdef USE_VLA + double p_mid_prev[dims]; + double p_mid_next[dims]; +#else + double *p_mid_prev = alloca(sizeof(double) * dims); + double *p_mid_next = alloca(sizeof(double) * dims); +#endif + + uint i_mid_prev_next, i_mid_next_prev; + if (point_corner_measure( + points, points_len, + i, i - 1, i + 1, + radius_mid, + samples_max, + dims, + + p_mid_prev, &i_mid_prev_next, + p_mid_next, &i_mid_next_prev)) + { + const double angle_mid_cos = angle_vnvnvn_cos(p_mid_prev, p, p_mid_next, dims); + + /* compare as cos and flip direction */ + + /* if (angle_mid > angle_threshold) { */ + if (angle_mid_cos < angle_threshold_cos) { +#ifdef USE_VLA + double p_max_prev[dims]; + double p_max_next[dims]; +#else + double *p_max_prev = alloca(sizeof(double) * dims); + double *p_max_next = alloca(sizeof(double) * dims); +#endif + + uint i_max_prev_next, i_max_next_prev; + if (point_corner_measure( + points, points_len, + i, i - 1, i + 1, + radius_max, + samples_max, + dims, + + p_max_prev, &i_max_prev_next, + p_max_next, &i_max_next_prev)) + { + const double angle_mid = acos(angle_mid_cos); + const double angle_max = angle_vnvnvn(p_max_prev, p, p_max_next, dims) / 2.0; + const double angle_diff = angle_mid - angle_max; + if (angle_diff > angle_threshold) { + return angle_diff; + } + } + } + } + + return 0.0; +} + + +int curve_fit_corners_detect_db( + const double *points, + const uint points_len, + const uint dims, + const double radius_min, /* ignore values below this */ + const double radius_max, /* ignore values above this */ + const uint samples_max, + const double angle_threshold, + + uint **r_corners, + uint *r_corners_len) +{ + const double angle_threshold_cos = cos(angle_threshold); + uint corners_len = 0; + + /* Use the difference in angle between the mid-max radii + * to detect the difference between a corner and a sharp turn. */ + const double radius_mid = (radius_min + radius_max) / 2.0; + + /* we could ignore first/last- but simple to keep aligned with the point array */ + double *points_angle = malloc(sizeof(double) * points_len); + points_angle[0] = 0.0; + + *r_corners = NULL; + *r_corners_len = 0; + + for (uint i = 0; i < points_len; i++) { + points_angle[i] = point_corner_angle( + points, points_len, i, + radius_mid, radius_max, + angle_threshold, angle_threshold_cos, + samples_max, + dims); + + if (points_angle[i] != 0.0) { + corners_len++; + } + } + + if (corners_len == 0) { + free(points_angle); + return 0; + } + + /* Clean angle limits! + * + * How this works: + * - Find contiguous 'corners' (where the distance is less or equal to the error threshold). + * - Keep track of the corner with the highest angle + * - Clear every other angle (so they're ignored when setting corners). */ + { + const double radius_min_sq = sq(radius_min); + uint i_span_start = 0; + while (i_span_start < points_len) { + uint i_span_end = i_span_start; + if (points_angle[i_span_start] != 0.0) { + uint i_next = i_span_start + 1; + uint i_best = i_span_start; + while (i_next < points_len) { + if ((points_angle[i_next] == 0.0) || + (len_squared_vnvn( + &points[(i_next - 1) * dims], + &points[i_next * dims], dims) > radius_min_sq)) + { + break; + } + else { + if (points_angle[i_best] < points_angle[i_next]) { + i_best = i_next; + } + i_span_end = i_next; + i_next += 1; + } + } + + if (i_span_start != i_span_end) { + uint i = i_span_start; + while (i <= i_span_end) { + if (i != i_best) { + /* we could use some other error code */ + assert(points_angle[i] != 0.0); + points_angle[i] = 0.0; + corners_len--; + } + i += 1; + } + } + } + i_span_start = i_span_end + 1; + } + } + /* End angle limit cleaning! */ + + corners_len += 2; /* first and last */ + uint *corners = malloc(sizeof(uint) * corners_len); + uint i_corner = 0; + corners[i_corner++] = 0; + for (uint i = 0; i < points_len; i++) { + if (points_angle[i] != 0.0) { + corners[i_corner++] = i; + } + } + corners[i_corner++] = points_len - 1; + assert(i_corner == corners_len); + + free(points_angle); + + *r_corners = corners; + *r_corners_len = corners_len; + + return 0; +} + +int curve_fit_corners_detect_fl( + const float *points, + const uint points_len, + const uint dims, + const float radius_min, /* ignore values below this */ + const float radius_max, /* ignore values above this */ + const uint samples_max, + const float angle_threshold, + + uint **r_corners, + uint *r_corners_len) +{ + const uint points_flat_len = points_len * dims; + double *points_db = malloc(sizeof(double) * points_flat_len); + + for (uint i = 0; i < points_flat_len; i++) { + points_db[i] = (double)points[i]; + } + + int result = curve_fit_corners_detect_db( + points_db, points_len, + dims, + radius_min, radius_max, + samples_max, + angle_threshold, + r_corners, r_corners_len); + + free(points_db); + + return result; +} diff --git a/extern/curve_fit_nd/intern/curve_fit_cubic.c b/extern/curve_fit_nd/intern/curve_fit_cubic.c new file mode 100644 index 00000000000..d623b51ed0b --- /dev/null +++ b/extern/curve_fit_nd/intern/curve_fit_cubic.c @@ -0,0 +1,1034 @@ +/* + * Copyright (c) 2016, DWANGO Co., Ltd. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the 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 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 +#include +#include +#include + +#include +#include + +#include "../curve_fit_nd.h" + +/* avoid re-calculating lengths multiple times */ +#define USE_LENGTH_CACHE + +/* store the indices in the cubic data so we can return the original indices, + * useful when the caller has data assosiated with the curve. */ +#define USE_ORIG_INDEX_DATA + +typedef unsigned int uint; + +#include "curve_fit_inline.h" + +#ifdef _MSC_VER +# define alloca(size) _alloca(size) +#endif + +#if !defined(_MSC_VER) +# define USE_VLA +#endif + +#ifdef USE_VLA +# ifdef __GNUC__ +# pragma GCC diagnostic ignored "-Wvla" +# endif +#else +# ifdef __GNUC__ +# pragma GCC diagnostic error "-Wvla" +# endif +#endif + +#define SWAP(type, a, b) { \ + type sw_ap; \ + sw_ap = (a); \ + (a) = (b); \ + (b) = sw_ap; \ +} (void)0 + + +/* -------------------------------------------------------------------- */ + +/** \name Cubic Type & Functions + * \{ */ + +typedef struct Cubic { + /* single linked lists */ + struct Cubic *next; +#ifdef USE_ORIG_INDEX_DATA + uint orig_span; +#endif + /* 0: point_0, 1: handle_0, 2: handle_1, 3: point_1, + * each one is offset by 'dims' */ + double pt_data[0]; +} Cubic; + +#define CUBIC_PT(cubic, index, dims) \ + (&(cubic)->pt_data[(index) * (dims)]) + +#define CUBIC_VARS(c, dims, _p0, _p1, _p2, _p3) \ + double \ + *_p0 = (c)->pt_data, \ + *_p1 = _p0 + (dims), \ + *_p2 = _p1 + (dims), \ + *_p3 = _p2 + (dims); ((void)0) +#define CUBIC_VARS_CONST(c, dims, _p0, _p1, _p2, _p3) \ + const double \ + *_p0 = (c)->pt_data, \ + *_p1 = _p0 + (dims), \ + *_p2 = _p1 + (dims), \ + *_p3 = _p2 + (dims); ((void)0) + + +static Cubic *cubic_alloc(const uint dims) +{ + return malloc(sizeof(Cubic) + (sizeof(double) * 4 * dims)); +} + +static void cubic_init( + Cubic *cubic, + const double p0[], const double p1[], const double p2[], const double p3[], + const uint dims) +{ + copy_vnvn(CUBIC_PT(cubic, 0, dims), p0, dims); + copy_vnvn(CUBIC_PT(cubic, 1, dims), p1, dims); + copy_vnvn(CUBIC_PT(cubic, 2, dims), p2, dims); + copy_vnvn(CUBIC_PT(cubic, 3, dims), p3, dims); +} + +static void cubic_free(Cubic *cubic) +{ + free(cubic); +} + +/** \} */ + + +/* -------------------------------------------------------------------- */ + +/** \name CubicList Type & Functions + * \{ */ + +typedef struct CubicList { + struct Cubic *items; + uint len; + uint dims; +} CubicList; + +static void cubic_list_prepend(CubicList *clist, Cubic *cubic) +{ + cubic->next = clist->items; + clist->items = cubic; + clist->len++; +} + +static double *cubic_list_as_array( + const CubicList *clist +#ifdef USE_ORIG_INDEX_DATA + , + const uint index_last, + uint *r_orig_index +#endif + ) +{ + const uint dims = clist->dims; + const uint array_flat_len = (clist->len + 1) * 3 * dims; + + double *array = malloc(sizeof(double) * array_flat_len); + const double *handle_prev = &((Cubic *)clist->items)->pt_data[dims]; + +#ifdef USE_ORIG_INDEX_DATA + uint orig_index_value = index_last; + uint orig_index_index = clist->len; + bool use_orig_index = (r_orig_index != NULL); +#endif + + /* fill the array backwards */ + const size_t array_chunk = 3 * dims; + double *array_iter = array + array_flat_len; + for (Cubic *citer = clist->items; citer; citer = citer->next) { + array_iter -= array_chunk; + memcpy(array_iter, &citer->pt_data[2 * dims], sizeof(double) * 2 * dims); + memcpy(&array_iter[2 * dims], &handle_prev[dims], sizeof(double) * dims); + handle_prev = citer->pt_data; + +#ifdef USE_ORIG_INDEX_DATA + if (use_orig_index) { + r_orig_index[orig_index_index--] = orig_index_value; + orig_index_value -= citer->orig_span; + } +#endif + } + +#ifdef USE_ORIG_INDEX_DATA + if (use_orig_index) { + assert(orig_index_index == 0); + assert(orig_index_value == 0 || index_last == 0); + r_orig_index[orig_index_index] = index_last ? orig_index_value : 0; + + } +#endif + + /* flip tangent for first and last (we could leave at zero, but set to something useful) */ + + /* first */ + array_iter -= array_chunk; + memcpy(&array_iter[dims], handle_prev, sizeof(double) * 2 * dims); + flip_vn_vnvn(&array_iter[0 * dims], &array_iter[1 * dims], &array_iter[2 * dims], dims); + assert(array == array_iter); + + /* last */ + array_iter += array_flat_len - (3 * dims); + flip_vn_vnvn(&array_iter[2 * dims], &array_iter[1 * dims], &array_iter[0 * dims], dims); + + return array; +} + +static void cubic_list_clear(CubicList *clist) +{ + Cubic *cubic_next; + for (Cubic *citer = clist->items; citer; citer = cubic_next) { + cubic_next = citer->next; + cubic_free(citer); + } + clist->items = NULL; + clist->len = 0; +} + +/** \} */ + + +/* -------------------------------------------------------------------- */ + +/** \name Cubic Evaluation + * \{ */ + +static void cubic_evaluate( + const Cubic *cubic, const double t, const uint dims, + double r_v[]) +{ + CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3); + const double s = 1.0 - t; + + for (uint j = 0; j < dims; j++) { + const double p01 = (p0[j] * s) + (p1[j] * t); + const double p12 = (p1[j] * s) + (p2[j] * t); + const double p23 = (p2[j] * s) + (p3[j] * t); + r_v[j] = ((((p01 * s) + (p12 * t))) * s) + + ((((p12 * s) + (p23 * t))) * t); + } +} + +static void cubic_calc_point( + const Cubic *cubic, const double t, const uint dims, + double r_v[]) +{ + CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3); + const double s = 1.0 - t; + for (uint j = 0; j < dims; j++) { + r_v[j] = p0[j] * s * s * s + + 3.0 * t * s * (s * p1[j] + t * p2[j]) + t * t * t * p3[j]; + } +} + +static void cubic_calc_speed( + const Cubic *cubic, const double t, const uint dims, + double r_v[]) +{ + CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3); + const double s = 1.0 - t; + for (uint j = 0; j < dims; j++) { + r_v[j] = 3.0 * ((p1[j] - p0[j]) * s * s + 2.0 * + (p2[j] - p0[j]) * s * t + + (p3[j] - p2[j]) * t * t); + } +} + +static void cubic_calc_acceleration( + const Cubic *cubic, const double t, const uint dims, + double r_v[]) +{ + CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3); + const double s = 1.0 - t; + for (uint j = 0; j < dims; j++) { + r_v[j] = 6.0 * ((p2[j] - 2.0 * p1[j] + p0[j]) * s + + (p3[j] - 2.0 * p2[j] + p1[j]) * t); + } +} + +/** + * Returns a 'measure' of the maximal discrepancy of the points specified + * by points_offset from the corresponding cubic(u[]) points. + */ +static void cubic_calc_error( + const Cubic *cubic, + const double *points_offset, + const uint points_offset_len, + const double *u, + const uint dims, + + double *r_error_sq_max, + uint *r_error_index) +{ + double error_sq_max = 0.0; + uint error_index = 0; + + const double *pt_real = points_offset + dims; +#ifdef USE_VLA + double pt_eval[dims]; +#else + double *pt_eval = alloca(sizeof(double) * dims); +#endif + + for (uint i = 1; i < points_offset_len - 1; i++, pt_real += dims) { + cubic_evaluate(cubic, u[i], dims, pt_eval); + + const double err_sq = len_squared_vnvn(pt_real, pt_eval, dims); + if (err_sq >= error_sq_max) { + error_sq_max = err_sq; + error_index = i; + } + } + + *r_error_sq_max = error_sq_max; + *r_error_index = error_index; +} + +/** + * Bezier multipliers + */ + +static double B1(double u) +{ + double tmp = 1.0 - u; + return 3.0 * u * tmp * tmp; +} + +static double B2(double u) +{ + return 3.0 * u * u * (1.0 - u); +} + +static double B0plusB1(double u) +{ + double tmp = 1.0 - u; + return tmp * tmp * (1.0 + 2.0 * u); +} + +static double B2plusB3(double u) +{ + return u * u * (3.0 - 2.0 * u); +} + +static void points_calc_center_weighted( + const double *points_offset, + const uint points_offset_len, + const uint dims, + + double r_center[]) +{ + /* + * Calculate a center that compensates for point spacing. + */ + + const double *pt_prev = &points_offset[(points_offset_len - 2) * dims]; + const double *pt_curr = pt_prev + dims; + const double *pt_next = points_offset; + + double w_prev = len_vnvn(pt_prev, pt_curr, dims); + + zero_vn(r_center, dims); + double w_tot = 0.0; + + for (uint i_next = 0; i_next < points_offset_len; i_next++) { + const double w_next = len_vnvn(pt_curr, pt_next, dims); + const double w = w_prev + w_next; + w_tot += w; + + miadd_vn_vn_fl(r_center, pt_curr, w, dims); + + w_prev = w_next; + + pt_prev = pt_curr; + pt_curr = pt_next; + pt_next += dims; + } + + if (w_tot != 0.0) { + imul_vn_fl(r_center, 1.0 / w_tot, dims); + } +} + +/** + * Use least-squares method to find Bezier control points for region. + */ +static void cubic_from_points( + const double *points_offset, + const uint points_offset_len, + const double *u_prime, + const double tan_l[], + const double tan_r[], + const uint dims, + + Cubic *r_cubic) +{ + + const double *p0 = &points_offset[0]; + const double *p3 = &points_offset[(points_offset_len - 1) * dims]; + + /* Point Pairs */ + double alpha_l, alpha_r; +#ifdef USE_VLA + double a[2][dims]; + double tmp[dims]; +#else + double *a[2] = { + alloca(sizeof(double) * dims), + alloca(sizeof(double) * dims), + }; + double *tmp = alloca(sizeof(double) * dims); +#endif + + { + double x[2] = {0.0}, c[2][2] = {{0.0}}; + const double *pt = points_offset; + + for (uint i = 0; i < points_offset_len; i++, pt += dims) { + mul_vnvn_fl(a[0], tan_l, B1(u_prime[i]), dims); + mul_vnvn_fl(a[1], tan_r, B2(u_prime[i]), dims); + + c[0][0] += dot_vnvn(a[0], a[0], dims); + c[0][1] += dot_vnvn(a[0], a[1], dims); + c[1][1] += dot_vnvn(a[1], a[1], dims); + + c[1][0] = c[0][1]; + + { + const double b0_plus_b1 = B0plusB1(u_prime[i]); + const double b2_plus_b3 = B2plusB3(u_prime[i]); + for (uint j = 0; j < dims; j++) { + tmp[j] = (pt[j] - (p0[j] * b0_plus_b1)) + (p3[j] * b2_plus_b3); + } + + x[0] += dot_vnvn(a[0], tmp, dims); + x[1] += dot_vnvn(a[1], tmp, dims); + } + } + + double det_C0_C1 = c[0][0] * c[1][1] - c[0][1] * c[1][0]; + double det_C_0X = x[1] * c[0][0] - x[0] * c[0][1]; + double det_X_C1 = x[0] * c[1][1] - x[1] * c[0][1]; + + if (is_almost_zero(det_C0_C1)) { + det_C0_C1 = c[0][0] * c[1][1] * 10e-12; + } + + /* may still divide-by-zero, check below will catch nan values */ + alpha_l = det_X_C1 / det_C0_C1; + alpha_r = det_C_0X / det_C0_C1; + } + + /* + * The problem that the stupid values for alpha dare not put + * only when we realize that the sign and wrong, + * but even if the values are too high. + * But how do you evaluate it? + * + * Meanwhile, we should ensure that these values are sometimes + * so only problems absurd of approximation and not for bugs in the code. + */ + + /* flip check to catch nan values */ + if (!(alpha_l >= 0.0) || + !(alpha_r >= 0.0)) + { + alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0; + } + + double *p1 = CUBIC_PT(r_cubic, 1, dims); + double *p2 = CUBIC_PT(r_cubic, 2, dims); + + copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims); + copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims); + +#ifdef USE_ORIG_INDEX_DATA + r_cubic->orig_span = (points_offset_len - 1); +#endif + + /* p1 = p0 - (tan_l * alpha_l); + * p2 = p3 + (tan_r * alpha_r); + */ + msub_vn_vnvn_fl(p1, p0, tan_l, alpha_l, dims); + madd_vn_vnvn_fl(p2, p3, tan_r, alpha_r, dims); + + /* ------------------------------------ + * Clamping (we could make it optional) + */ +#ifdef USE_VLA + double center[dims]; +#else + double *center = alloca(sizeof(double) * dims); +#endif + points_calc_center_weighted(points_offset, points_offset_len, dims, center); + + const double clamp_scale = 3.0; /* clamp to 3x */ + double dist_sq_max = 0.0; + + { + const double *pt = points_offset; + for (uint i = 0; i < points_offset_len; i++, pt += dims) { +#if 0 + double dist_sq_test = sq(len_vnvn(center, pt, dims) * clamp_scale); +#else + /* do inline */ + double dist_sq_test = 0.0; + for (uint j = 0; j < dims; j++) { + dist_sq_test += sq((pt[j] - center[j]) * clamp_scale); + } +#endif + dist_sq_max = max(dist_sq_max, dist_sq_test); + } + } + + double p1_dist_sq = len_squared_vnvn(center, p1, dims); + double p2_dist_sq = len_squared_vnvn(center, p2, dims); + + if (p1_dist_sq > dist_sq_max || + p2_dist_sq > dist_sq_max) + { + + alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0; + + /* + * p1 = p0 - (tan_l * alpha_l); + * p2 = p3 + (tan_r * alpha_r); + */ + for (uint j = 0; j < dims; j++) { + p1[j] = p0[j] - (tan_l[j] * alpha_l); + p2[j] = p3[j] + (tan_r[j] * alpha_r); + } + + p1_dist_sq = len_squared_vnvn(center, p1, dims); + p2_dist_sq = len_squared_vnvn(center, p2, dims); + } + + /* clamp within the 3x radius */ + if (p1_dist_sq > dist_sq_max) { + isub_vnvn(p1, center, dims); + imul_vn_fl(p1, sqrt(dist_sq_max) / sqrt(p1_dist_sq), dims); + iadd_vnvn(p1, center, dims); + } + if (p2_dist_sq > dist_sq_max) { + isub_vnvn(p2, center, dims); + imul_vn_fl(p2, sqrt(dist_sq_max) / sqrt(p2_dist_sq), dims); + iadd_vnvn(p2, center, dims); + } + /* end clamping */ +} + +#ifdef USE_LENGTH_CACHE +static void points_calc_coord_length_cache( + const double *points_offset, + const uint points_offset_len, + const uint dims, + + double *r_points_length_cache) +{ + const double *pt_prev = points_offset; + const double *pt = pt_prev + dims; + r_points_length_cache[0] = 0.0; + for (uint i = 1; i < points_offset_len; i++) { + r_points_length_cache[i] = len_vnvn(pt, pt_prev, dims); + pt_prev = pt; + pt += dims; + } +} +#endif /* USE_LENGTH_CACHE */ + + +static void points_calc_coord_length( + const double *points_offset, + const uint points_offset_len, + const uint dims, +#ifdef USE_LENGTH_CACHE + const double *points_length_cache, +#endif + double *r_u) +{ + const double *pt_prev = points_offset; + const double *pt = pt_prev + dims; + r_u[0] = 0.0; + for (uint i = 1, i_prev = 0; i < points_offset_len; i++) { + double length; + +#ifdef USE_LENGTH_CACHE + length = points_length_cache[i]; + + assert(len_vnvn(pt, pt_prev, dims) == points_length_cache[i]); +#else + length = len_vnvn(pt, pt_prev, dims); +#endif + + r_u[i] = r_u[i_prev] + length; + i_prev = i; + pt_prev = pt; + pt += dims; + } + assert(!is_almost_zero(r_u[points_offset_len - 1])); + const double w = r_u[points_offset_len - 1]; + for (uint i = 0; i < points_offset_len; i++) { + r_u[i] /= w; + } +} + +/** + * Use Newton-Raphson iteration to find better root. + * + * \param cubic: Current fitted curve. + * \param p: Point to test against. + * \param u: Parameter value for \a p. + * + * \note Return value may be `nan` caller must check for this. + */ +static double cubic_find_root( + const Cubic *cubic, + const double p[], + const double u, + const uint dims) +{ + /* Newton-Raphson Method. */ + /* all vectors */ +#ifdef USE_VLA + double q0_u[dims]; + double q1_u[dims]; + double q2_u[dims]; +#else + double *q0_u = alloca(sizeof(double) * dims); + double *q1_u = alloca(sizeof(double) * dims); + double *q2_u = alloca(sizeof(double) * dims); +#endif + + cubic_calc_point(cubic, u, dims, q0_u); + cubic_calc_speed(cubic, u, dims, q1_u); + cubic_calc_acceleration(cubic, u, dims, q2_u); + + /* may divide-by-zero, caller must check for that case */ + /* u - ((q0_u - p) * q1_u) / (q1_u.length_squared() + (q0_u - p) * q2_u) */ + isub_vnvn(q0_u, p, dims); + return u - dot_vnvn(q0_u, q1_u, dims) / + (len_squared_vn(q1_u, dims) + dot_vnvn(q0_u, q2_u, dims)); +} + +static int compare_double_fn(const void *a_, const void *b_) +{ + const double *a = a_; + const double *b = b_; + if (*a > *b) return 1; + else if (*a < *b) return -1; + else return 0; +} + +/** + * Given set of points and their parameterization, try to find a better parameterization. + */ +static bool cubic_reparameterize( + const Cubic *cubic, + const double *points_offset, + const uint points_offset_len, + const double *u, + const uint dims, + + double *r_u_prime) +{ + /* + * Recalculate the values of u[] based on the Newton Raphson method + */ + + const double *pt = points_offset; + for (uint i = 0; i < points_offset_len; i++, pt += dims) { + r_u_prime[i] = cubic_find_root(cubic, pt, u[i], dims); + if (!isfinite(r_u_prime[i])) { + return false; + } + } + + qsort(r_u_prime, points_offset_len, sizeof(double), compare_double_fn); + + if ((r_u_prime[0] < 0.0) || + (r_u_prime[points_offset_len - 1] > 1.0)) + { + return false; + } + + assert(r_u_prime[0] >= 0.0); + assert(r_u_prime[points_offset_len - 1] <= 1.0); + return true; +} + + +static void fit_cubic_to_points( + const double *points_offset, + const uint points_offset_len, +#ifdef USE_LENGTH_CACHE + const double *points_length_cache, +#endif + const double tan_l[], + const double tan_r[], + const double error_threshold, + const uint dims, + /* fill in the list */ + CubicList *clist) +{ + const uint iteration_max = 4; + const double error_sq = sq(error_threshold); + + Cubic *cubic; + + if (points_offset_len == 2) { + cubic = cubic_alloc(dims); + CUBIC_VARS(cubic, dims, p0, p1, p2, p3); + + copy_vnvn(p0, &points_offset[0 * dims], dims); + copy_vnvn(p3, &points_offset[1 * dims], dims); + + const double dist = len_vnvn(p0, p3, dims) / 3.0; + msub_vn_vnvn_fl(p1, p0, tan_l, dist, dims); + madd_vn_vnvn_fl(p2, p3, tan_r, dist, dims); + +#ifdef USE_ORIG_INDEX_DATA + cubic->orig_span = 1; +#endif + + cubic_list_prepend(clist, cubic); + return; + } + + double *u = malloc(sizeof(double) * points_offset_len); + points_calc_coord_length( + points_offset, points_offset_len, dims, +#ifdef USE_LENGTH_CACHE + points_length_cache, +#endif + u); + + cubic = cubic_alloc(dims); + + double error_sq_max; + uint split_index; + + /* Parameterize points, and attempt to fit curve */ + cubic_from_points( + points_offset, points_offset_len, u, tan_l, tan_r, dims, cubic); + + /* Find max deviation of points to fitted curve */ + cubic_calc_error( + cubic, points_offset, points_offset_len, u, dims, + &error_sq_max, &split_index); + + if (error_sq_max < error_sq) { + free(u); + cubic_list_prepend(clist, cubic); + return; + } + else { + /* If error not too large, try some reparameterization and iteration */ + double *u_prime = malloc(sizeof(double) * points_offset_len); + for (uint iter = 0; iter < iteration_max; iter++) { + if (!cubic_reparameterize( + cubic, points_offset, points_offset_len, u, dims, u_prime)) + { + break; + } + + cubic_from_points( + points_offset, points_offset_len, u_prime, + tan_l, tan_r, dims, cubic); + cubic_calc_error( + cubic, points_offset, points_offset_len, u_prime, dims, + &error_sq_max, &split_index); + + if (error_sq_max < error_sq) { + free(u_prime); + free(u); + cubic_list_prepend(clist, cubic); + return; + } + + SWAP(double *, u, u_prime); + } + free(u_prime); + } + + free(u); + cubic_free(cubic); + + + /* Fitting failed -- split at max error point and fit recursively */ + + /* Check splinePoint is not an endpoint? + * + * This assert happens sometimes... + * Look into it but disable for now. Campbell! */ + + // assert(split_index > 1) +#ifdef USE_VLA + double tan_center[dims]; +#else + double *tan_center = alloca(sizeof(double) * dims); +#endif + + const double *pt_a = &points_offset[(split_index - 1) * dims]; + const double *pt_b = &points_offset[(split_index + 1) * dims]; + + assert(split_index < points_offset_len); + if (equals_vnvn(pt_a, pt_b, dims)) { + pt_a += dims; + } + + /* tan_center = (pt_a - pt_b).normalized() */ + normalize_vn_vnvn(tan_center, pt_a, pt_b, dims); + + fit_cubic_to_points( + points_offset, split_index + 1, +#ifdef USE_LENGTH_CACHE + points_length_cache, +#endif + tan_l, tan_center, error_threshold, dims, clist); + fit_cubic_to_points( + &points_offset[split_index * dims], points_offset_len - split_index, +#ifdef USE_LENGTH_CACHE + points_length_cache + split_index, +#endif + tan_center, tan_r, error_threshold, dims, clist); + +} + +/** \} */ + + +/* -------------------------------------------------------------------- */ + +/** \name External API for Curve-Fitting + * \{ */ + +/** + * Main function: + * + * Take an array of 3d points. + * return the cubic splines + */ +int curve_fit_cubic_from_points_db( + const double *points, + const uint points_len, + const uint dims, + const double error_threshold, + const uint *corners, + uint corners_len, + + double **r_cubic_array, uint *r_cubic_array_len, + uint **r_cubic_orig_index, + uint **r_corner_index_array, uint *r_corner_index_len) +{ + uint corners_buf[2]; + if (corners == NULL) { + assert(corners_len == 0); + corners_buf[0] = 0; + corners_buf[1] = points_len - 1; + corners = corners_buf; + corners_len = 2; + } + + CubicList clist = {0}; + clist.dims = dims; + +#ifdef USE_VLA + double tan_l[dims]; + double tan_r[dims]; +#else + double *tan_l = alloca(sizeof(double) * dims); + double *tan_r = alloca(sizeof(double) * dims); +#endif + +#ifdef USE_LENGTH_CACHE + double *points_length_cache = NULL; + uint points_length_cache_len_alloc = 0; +#endif + + uint *corner_index_array = NULL; + uint corner_index = 0; + if (r_corner_index_array && (corners != corners_buf)) { + corner_index_array = malloc(sizeof(uint) * corners_len); + corner_index_array[corner_index++] = corners[0]; + } + + for (uint i = 1; i < corners_len; i++) { + const uint points_offset_len = corners[i] - corners[i - 1] + 1; + const uint first_point = corners[i - 1]; + + assert(points_offset_len >= 1); + if (points_offset_len > 1) { + const double *pt_l = &points[first_point * dims]; + const double *pt_r = &points[(first_point + points_offset_len - 1) * dims]; + const double *pt_l_next = pt_l + dims; + const double *pt_r_prev = pt_r - dims; + + /* tan_l = (pt_l - pt_l_next).normalized() + * tan_r = (pt_r_prev - pt_r).normalized() + */ + normalize_vn_vnvn(tan_l, pt_l, pt_l_next, dims); + normalize_vn_vnvn(tan_r, pt_r_prev, pt_r, dims); + +#ifdef USE_LENGTH_CACHE + if (points_length_cache_len_alloc < points_offset_len) { + if (points_length_cache) { + free(points_length_cache); + } + points_length_cache = malloc(sizeof(double) * points_offset_len); + } + points_calc_coord_length_cache( + &points[first_point * dims], points_offset_len, dims, + points_length_cache); +#endif + + fit_cubic_to_points( + &points[first_point * dims], points_offset_len, +#ifdef USE_LENGTH_CACHE + points_length_cache, +#endif + tan_l, tan_r, error_threshold, dims, &clist); + } + else if (points_len == 1) { + assert(points_offset_len == 1); + assert(corners_len == 2); + assert(corners[0] == 0); + assert(corners[1] == 0); + const double *pt = &points[0]; + Cubic *cubic = cubic_alloc(dims); + cubic_init(cubic, pt, pt, pt, pt, dims); + cubic_list_prepend(&clist, cubic); + } + + if (corner_index_array) { + corner_index_array[corner_index++] = clist.len; + } + } + +#ifdef USE_LENGTH_CACHE + if (points_length_cache) { + free(points_length_cache); + } +#endif + +#ifdef USE_ORIG_INDEX_DATA + uint *cubic_orig_index = NULL; + if (r_cubic_orig_index) { + cubic_orig_index = malloc(sizeof(uint) * (clist.len + 1)); + } +#else + *r_cubic_orig_index = NULL; +#endif + + /* allocate a contiguous array and free the linked list */ + *r_cubic_array = cubic_list_as_array( + &clist +#ifdef USE_ORIG_INDEX_DATA + , corners[corners_len - 1], cubic_orig_index +#endif + ); + *r_cubic_array_len = clist.len + 1; + + cubic_list_clear(&clist); + +#ifdef USE_ORIG_INDEX_DATA + if (cubic_orig_index) { + *r_cubic_orig_index = cubic_orig_index; + } +#endif + + if (corner_index_array) { + assert(corner_index == corners_len); + *r_corner_index_array = corner_index_array; + *r_corner_index_len = corner_index; + } + + return 0; +} + +/** + * A version of #curve_fit_cubic_from_points_db to handle floats + */ +int curve_fit_cubic_from_points_fl( + const float *points, + const uint points_len, + const uint dims, + const float error_threshold, + const uint *corners, + const uint corners_len, + + float **r_cubic_array, uint *r_cubic_array_len, + uint **r_cubic_orig_index, + uint **r_corner_index_array, uint *r_corner_index_len) +{ + const uint points_flat_len = points_len * dims; + double *points_db = malloc(sizeof(double) * points_flat_len); + + for (uint i = 0; i < points_flat_len; i++) { + points_db[i] = (double)points[i]; + } + + double *cubic_array_db = NULL; + float *cubic_array_fl = NULL; + uint cubic_array_len = 0; + + int result = curve_fit_cubic_from_points_db( + points_db, points_len, dims, error_threshold, corners, corners_len, + &cubic_array_db, &cubic_array_len, + r_cubic_orig_index, + r_corner_index_array, r_corner_index_len); + free(points_db); + + if (!result) { + uint cubic_array_flat_len = cubic_array_len * 3 * dims; + cubic_array_fl = malloc(sizeof(float) * cubic_array_flat_len); + for (uint i = 0; i < cubic_array_flat_len; i++) { + cubic_array_fl[i] = (float)cubic_array_db[i]; + } + free(cubic_array_db); + } + + *r_cubic_array = cubic_array_fl; + *r_cubic_array_len = cubic_array_len; + + return result; +} + +/** \} */ diff --git a/extern/curve_fit_nd/intern/curve_fit_inline.h b/extern/curve_fit_nd/intern/curve_fit_inline.h new file mode 100644 index 00000000000..17aa02be3e5 --- /dev/null +++ b/extern/curve_fit_nd/intern/curve_fit_inline.h @@ -0,0 +1,262 @@ +/* + * Copyright (c) 2016, Blender Foundation. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the 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 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; +} + +/** \} */