diff options
Diffstat (limited to 'extern/curve_fit_nd/intern/curve_fit_corners_detect.c')
| -rw-r--r-- | extern/curve_fit_nd/intern/curve_fit_corners_detect.c | 468 |
1 files changed, 468 insertions, 0 deletions
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..67ed3561a65 --- /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 cos_vnvnvn( + 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(cos_vnvnvn(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 (cos_vnvnvn(&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 = cos_vnvnvn(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; +} |