diff options
author | Campbell Barton <ideasman42@gmail.com> | 2016-07-25 07:12:17 +0300 |
---|---|---|
committer | Campbell Barton <ideasman42@gmail.com> | 2016-07-25 07:55:08 +0300 |
commit | 2418daede5913c54bd9675eb23624487f6b0ad4c (patch) | |
tree | b3759b8bc89833aa4b8883d9690874e16a5c9bac /extern | |
parent | f23fecf3061a63d24815a63a378a636832a40ccd (diff) |
Curve Fitting: Add alternate 'refit' method
This is an alternative method for fitting a curve which incrementally simplifies the curve, then re-fits.
Generally gives better results, also improves corner detection.
Diffstat (limited to 'extern')
-rw-r--r-- | extern/curve_fit_nd/CMakeLists.txt | 6 | ||||
-rw-r--r-- | extern/curve_fit_nd/curve_fit_nd.h | 35 | ||||
-rw-r--r-- | extern/curve_fit_nd/intern/curve_fit_cubic.c | 33 | ||||
-rw-r--r-- | extern/curve_fit_nd/intern/curve_fit_cubic_refit.c | 1424 | ||||
-rw-r--r-- | extern/curve_fit_nd/intern/curve_fit_inline.h | 2 | ||||
-rw-r--r-- | extern/curve_fit_nd/intern/generic_alloc_impl.h | 220 | ||||
-rw-r--r-- | extern/curve_fit_nd/intern/generic_heap.c | 278 | ||||
-rw-r--r-- | extern/curve_fit_nd/intern/generic_heap.h | 54 |
8 files changed, 2041 insertions, 11 deletions
diff --git a/extern/curve_fit_nd/CMakeLists.txt b/extern/curve_fit_nd/CMakeLists.txt index 6669971aa2d..cc9efe1c470 100644 --- a/extern/curve_fit_nd/CMakeLists.txt +++ b/extern/curve_fit_nd/CMakeLists.txt @@ -26,10 +26,14 @@ set(INC_SYS set(SRC intern/curve_fit_cubic.c + intern/curve_fit_cubic_refit.c intern/curve_fit_corners_detect.c - intern/curve_fit_inline.h curve_fit_nd.h + intern/curve_fit_inline.h + intern/generic_alloc_impl.h + intern/generic_heap.c + intern/generic_heap.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 index 3649802a425..cfb1881fe00 100644 --- a/extern/curve_fit_nd/curve_fit_nd.h +++ b/extern/curve_fit_nd/curve_fit_nd.h @@ -86,6 +86,7 @@ int curve_fit_cubic_to_points_fl( * * \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 points_length_cache: Optional pre-calculated lengths between points. * \param error_threshold: the error threshold to allow for, * \param tan_l, tan_r: Normalized tangents the handles will be aligned to. * Note that tangents must both point along the direction of the \a points, @@ -98,6 +99,7 @@ int curve_fit_cubic_to_points_fl( int curve_fit_cubic_to_points_single_db( const double *points, const unsigned int points_len, + const double *points_length_cache, const unsigned int dims, const double error_threshold, const double tan_l[], @@ -110,6 +112,7 @@ int curve_fit_cubic_to_points_single_db( int curve_fit_cubic_to_points_single_fl( const float *points, const unsigned int points_len, + const float *points_length_cache, const unsigned int dims, const float error_threshold, const float tan_l[], @@ -121,8 +124,40 @@ int curve_fit_cubic_to_points_single_fl( enum { CURVE_FIT_CALC_HIGH_QUALIY = (1 << 0), + CURVE_FIT_CALC_CYCLIC = (1 << 1), }; + +/* curve_fit_cubic_refit.c */ + +int curve_fit_cubic_to_points_refit_db( + const double *points, + const unsigned int points_len, + const unsigned int dims, + const double error_threshold, + const unsigned int calc_flag, + const unsigned int *corners, + unsigned int corners_len, + const double corner_angle, + + 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_to_points_refit_fl( + const float *points, + const unsigned int points_len, + const unsigned int dims, + const float error_threshold, + const unsigned int calc_flag, + const unsigned int *corners, + unsigned int corners_len, + const float corner_angle, + + float **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); + /* curve_fit_corners_detect.c */ /** diff --git a/extern/curve_fit_nd/intern/curve_fit_cubic.c b/extern/curve_fit_nd/intern/curve_fit_cubic.c index 1a0f7dcfdee..24b216d32ff 100644 --- a/extern/curve_fit_nd/intern/curve_fit_cubic.c +++ b/extern/curve_fit_nd/intern/curve_fit_cubic.c @@ -474,7 +474,7 @@ static double points_calc_circumference_factor( * We could try support this but will likely cause extreme >1 scales which could cause other issues. */ // assert(angle >= len_tangent); double factor = (angle / len_tangent); - assert(factor < (M_PI / 2) + DBL_EPSILON); + assert(factor < (M_PI / 2) + (DBL_EPSILON * 10)); return factor; } else { @@ -876,7 +876,6 @@ static double points_calc_coord_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); @@ -1435,6 +1434,7 @@ int curve_fit_cubic_to_points_fl( int curve_fit_cubic_to_points_single_db( const double *points, const uint points_len, + const double *points_length_cache, const uint dims, const double error_threshold, const double tan_l[], @@ -1451,10 +1451,14 @@ int curve_fit_cubic_to_points_single_db( /* in this instance theres no advantage in using length cache, * since we're not recursively calculating values. */ #ifdef USE_LENGTH_CACHE - double *points_length_cache = malloc(sizeof(double) * points_len); - points_calc_coord_length_cache( - points, points_len, dims, - points_length_cache); + double *points_length_cache_alloc = NULL; + if (points_length_cache == NULL) { + points_length_cache_alloc = malloc(sizeof(double) * points_len); + points_calc_coord_length_cache( + points, points_len, dims, + points_length_cache_alloc); + points_length_cache = points_length_cache_alloc; + } #endif fit_cubic_to_points( @@ -1467,7 +1471,9 @@ int curve_fit_cubic_to_points_single_db( cubic, r_error_max_sq, &split_index); #ifdef USE_LENGTH_CACHE - free(points_length_cache); + if (points_length_cache_alloc) { + free(points_length_cache_alloc); + } #endif copy_vnvn(r_handle_l, CUBIC_PT(cubic, 1, dims), dims); @@ -1479,6 +1485,7 @@ int curve_fit_cubic_to_points_single_db( int curve_fit_cubic_to_points_single_fl( const float *points, const uint points_len, + const float *points_length_cache, const uint dims, const float error_threshold, const float tan_l[], @@ -1490,9 +1497,15 @@ int curve_fit_cubic_to_points_single_fl( { const uint points_flat_len = points_len * dims; double *points_db = malloc(sizeof(double) * points_flat_len); + double *points_length_cache_db = NULL; copy_vndb_vnfl(points_db, points, points_flat_len); + if (points_length_cache) { + points_length_cache_db = malloc(sizeof(double) * points_len); + copy_vndb_vnfl(points_length_cache_db, points_length_cache, points_len); + } + #ifdef USE_VLA double tan_l_db[dims]; double tan_r_db[dims]; @@ -1510,7 +1523,7 @@ int curve_fit_cubic_to_points_single_fl( copy_vndb_vnfl(tan_r_db, tan_r, dims); int result = curve_fit_cubic_to_points_single_db( - points_db, points_len, dims, + points_db, points_len, points_length_cache_db, dims, (double)error_threshold, tan_l_db, tan_r_db, r_handle_l_db, r_handle_r_db, @@ -1518,6 +1531,10 @@ int curve_fit_cubic_to_points_single_fl( free(points_db); + if (points_length_cache_db) { + free(points_length_cache_db); + } + copy_vnfl_vndb(r_handle_l, r_handle_l_db, dims); copy_vnfl_vndb(r_handle_r, r_handle_r_db, dims); *r_error_sq = (float)r_error_sq_db; diff --git a/extern/curve_fit_nd/intern/curve_fit_cubic_refit.c b/extern/curve_fit_nd/intern/curve_fit_cubic_refit.c new file mode 100644 index 00000000000..756c093b193 --- /dev/null +++ b/extern/curve_fit_nd/intern/curve_fit_cubic_refit.c @@ -0,0 +1,1424 @@ +/* + * Copyright (c) 2016, Campbell Barton. + * + * 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. + */ + +/** + * Curve Re-fitting Method + * ======================= + * + * This is a more processor intensive method of fitting, + * compared to #curve_fit_cubic_to_points_db, and works as follows: + * + * - First iteratively remove all points under the error threshold. + * - If corner calculation is enabled: + * - Find adjacent knots that exceed the angle limit + * - Find a 'split' point between the knots (could include the knots) + * - If copying the tangents to this split point doesn't exceed the error threshold: + * - Assign the tangents of the two knots to the split point, define it as a corner. + * (after this, we have many points which are too close). + * - Run a re-fit pass, where knots are re-positioned between their adjacent knots + * when their re-fit position has a lower 'error'. + * While re-fitting, remove knots that fall below the error threshold. + */ + +#include <math.h> +#include <float.h> +#include <stdbool.h> +#include <assert.h> + +#include <string.h> +#include <stdlib.h> + + +#include <stdio.h> + +#include "curve_fit_inline.h" +#include "../curve_fit_nd.h" + +#include "generic_heap.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 + +/* adjust the knots after simplifying */ +#define USE_KNOT_REFIT +/* remove knots under the error threshold while re-fitting */ +#define USE_KNOT_REFIT_REMOVE +/* detect corners over an angle threshold */ +#define USE_CORNER_DETECT +/* avoid re-calculating lengths multiple times */ +#define USE_LENGTH_CACHE +/* use pool allocator */ +#define USE_TPOOL + + +#define SPLIT_POINT_INVALID ((uint)-1) + +#define MAX2(x, y) ((x) > (y) ? (x) : (y)) + +#define SQUARE(a) ((a) * (a)) + +#ifdef __GNUC__ +# define UNLIKELY(x) __builtin_expect(!!(x), 0) +#else +# define UNLIKELY(x) (x) +#endif + + +typedef unsigned int uint; + +struct PointData { + const double *points; + uint points_len; +#ifdef USE_LENGTH_CACHE + const double *points_length_cache; +#endif +}; + +struct Knot { + struct Knot *next, *prev; + + HeapNode *heap_node; + + /** + * Currently the same, access as different for now + * since we may want to support different point/knot indices + */ + uint index; + + uint can_remove : 1; + uint is_removed : 1; + uint is_corner : 1; + + double handles[2]; + /** + * Store the error value, to see if we can improve on it + * (without having to re-calculate each time) + * + * This is the error between this knot and the next */ + double error_sq_next; + + /* Initially point to contiguous memory, however we may re-assign */ + double *tan[2]; +} Knot; + + +struct KnotRemoveState { + uint index; + /* Handles for prev/next knots */ + double handles[2]; +}; + +#ifdef USE_TPOOL +/* rstate_* pool allocator */ +#define TPOOL_IMPL_PREFIX rstate +#define TPOOL_ALLOC_TYPE struct KnotRemoveState +#define TPOOL_STRUCT ElemPool_KnotRemoveState +#include "generic_alloc_impl.h" +#undef TPOOL_IMPL_PREFIX +#undef TPOOL_ALLOC_TYPE +#undef TPOOL_STRUCT +#endif /* USE_TPOOL */ + +#ifdef USE_KNOT_REFIT +struct KnotRefitState { + uint index; + /** When SPLIT_POINT_INVALID - remove this item */ + uint index_refit; + /** Handles for prev/next knots */ + double handles_prev[2], handles_next[2]; + double error_sq[2]; +}; + +#ifdef USE_TPOOL +/* refit_* pool allocator */ +#define TPOOL_IMPL_PREFIX refit +#define TPOOL_ALLOC_TYPE struct KnotRefitState +#define TPOOL_STRUCT ElemPool_KnotRefitState +#include "generic_alloc_impl.h" +#undef TPOOL_IMPL_PREFIX +#undef TPOOL_ALLOC_TYPE +#undef TPOOL_STRUCT +#endif /* USE_TPOOL */ +#endif /* USE_KNOT_REFIT */ + + +#ifdef USE_CORNER_DETECT +/** Result of collapsing a corner. */ +struct KnotCornerState { + uint index; + /* Merge adjacent handles into this one (may be shared with the 'index') */ + uint index_adjacent[2]; + + /* Handles for prev/next knots */ + double handles_prev[2], handles_next[2]; + double error_sq[2]; +}; + +/* refit_* pool allocator */ +#ifdef USE_TPOOL +#define TPOOL_IMPL_PREFIX corner +#define TPOOL_ALLOC_TYPE struct KnotCornerState +#define TPOOL_STRUCT ElemPool_KnotCornerState +#include "generic_alloc_impl.h" +#undef TPOOL_IMPL_PREFIX +#undef TPOOL_ALLOC_TYPE +#undef TPOOL_STRUCT +#endif /* USE_TPOOL */ +#endif /* USE_CORNER_DETECT */ + + +/* Utility functions */ + +#ifdef USE_KNOT_REFIT +/** + * Find the most distant point between the 2 knots. + */ +static uint knot_find_split_point( + const struct PointData *pd, + const struct Knot *knot_l, const struct Knot *knot_r, + const uint knots_len, + const uint dims) +{ + uint split_point = SPLIT_POINT_INVALID; + double split_point_dist_best = -DBL_MAX; + + const double *offset = &pd->points[knot_l->index * dims]; + +#ifdef USE_VLA + double v_plane[dims]; + double v_proj[dims]; + double v_offset[dims]; +#else + double *v_plane = alloca(sizeof(double) * dims); + double *v_proj = alloca(sizeof(double) * dims); + double *v_offset = alloca(sizeof(double) * dims); +#endif + + sub_vn_vnvn( + v_plane, + &pd->points[knot_l->index * dims], + &pd->points[knot_r->index * dims], + dims); + + normalize_vn(v_plane, dims); + + const uint knots_end = knots_len - 1; + const struct Knot *k_step = knot_l; + do { + if (k_step->index != knots_end) { + k_step += 1; + } + else { + /* wrap around */ + k_step = k_step - knots_end; + } + + if (k_step != knot_r) { + sub_vn_vnvn(v_offset, &pd->points[k_step->index * dims], offset, dims); + project_plane_vn_vnvn_normalized(v_proj, v_offset, v_plane, dims); + + double split_point_dist_test = len_squared_vn(v_proj, dims); + if (split_point_dist_test > split_point_dist_best) { + split_point_dist_best = split_point_dist_test; + split_point = k_step->index; + } + } + else { + break; + } + + } while (true); + + return split_point; +} +#endif /* USE_KNOT_REFIT */ + + +#ifdef USE_CORNER_DETECT +/** + * Find the knot furthest from the line between \a knot_l & \a knot_r. + * This is to be used as a split point. + */ +static uint knot_find_split_point_on_axis( + const struct PointData *pd, + const struct Knot *knot_l, const struct Knot *knot_r, + const uint knots_len, + const double *plane_no, + const uint dims) +{ + uint split_point = SPLIT_POINT_INVALID; + double split_point_dist_best = -DBL_MAX; + + const uint knots_end = knots_len - 1; + const struct Knot *k_step = knot_l; + do { + if (k_step->index != knots_end) { + k_step += 1; + } + else { + /* wrap around */ + k_step = k_step - knots_end; + } + + if (k_step != knot_r) { + double split_point_dist_test = dot_vnvn(plane_no, &pd->points[k_step->index * dims], dims); + if (split_point_dist_test > split_point_dist_best) { + split_point_dist_best = split_point_dist_test; + split_point = k_step->index; + } + } + else { + break; + } + + } while (true); + + return split_point; +} +#endif /* USE_CORNER_DETECT */ + + +static double knot_remove_error_value( + const double *tan_l, const double *tan_r, + const double *points_offset, const uint points_offset_len, + const double *points_offset_length_cache, + const uint dims, + /* Avoid having to re-calculate again */ + double r_handle_factors[2]) +{ + double error_sq = FLT_MAX; + +#ifdef USE_VLA + double handle_factor_l[dims]; + double handle_factor_r[dims]; +#else + double *handle_factor_l = alloca(sizeof(double) * dims); + double *handle_factor_r = alloca(sizeof(double) * dims); +#endif + + curve_fit_cubic_to_points_single_db( + points_offset, points_offset_len, points_offset_length_cache, dims, 0.0, + tan_l, tan_r, + handle_factor_l, handle_factor_r, + &error_sq); + + assert(error_sq != FLT_MAX); + + isub_vnvn(handle_factor_l, points_offset, dims); + r_handle_factors[0] = dot_vnvn(tan_l, handle_factor_l, dims); + + isub_vnvn(handle_factor_r, &points_offset[(points_offset_len - 1) * dims], dims); + r_handle_factors[1] = dot_vnvn(tan_r, handle_factor_r, dims); + + return error_sq; +} + +static double knot_calc_curve_error_value( + const struct PointData *pd, + const struct Knot *knot_l, const struct Knot *knot_r, + const double *tan_l, const double *tan_r, + const uint dims, + double r_handle_factors[2]) +{ + const uint points_offset_len = ((knot_l->index < knot_r->index) ? + (knot_r->index - knot_l->index) : + ((knot_r->index + pd->points_len) - knot_l->index)) + 1; + + if (points_offset_len != 2) { + return knot_remove_error_value( + tan_l, tan_r, + &pd->points[knot_l->index * dims], points_offset_len, +#ifdef USE_LENGTH_CACHE + &pd->points_length_cache[knot_l->index], +#else + NULL, +#endif + dims, + r_handle_factors); + } + else { + /* No points between, use 1/3 handle length with no error as a fallback. */ + assert(points_offset_len == 2); +#ifdef USE_LENGTH_CACHE + r_handle_factors[0] = r_handle_factors[1] = pd->points_length_cache[knot_l->index] / 3.0; +#else + r_handle_factors[0] = r_handle_factors[1] = len_vnvn( + &pd->points[(knot_l->index + 0) * dims], + &pd->points[(knot_l->index + 1) * dims], dims) / 3.0; +#endif + return 0.0; + } +} + +struct KnotRemove_Params { + Heap *heap; + const struct PointData *pd; +#ifdef USE_TPOOL + struct ElemPool_KnotRemoveState *epool; +#endif +}; + +static void knot_remove_error_recalculate( + struct KnotRemove_Params *p, + struct Knot *k, const double error_sq_max, + const uint dims) +{ + assert(k->can_remove); + double handles[2]; + + const double cost_sq = knot_calc_curve_error_value( + p->pd, k->prev, k->next, + k->prev->tan[1], k->next->tan[0], + dims, + handles); + + if (cost_sq < error_sq_max) { + struct KnotRemoveState *r; + if (k->heap_node) { + r = HEAP_node_ptr(k->heap_node); + HEAP_remove(p->heap, k->heap_node); + } + else { +#ifdef USE_TPOOL + r = rstate_pool_elem_alloc(p->epool); +#else + r = malloc(sizeof(*r)); +#endif + + r->index = k->index; + } + + r->handles[0] = handles[0]; + r->handles[1] = handles[1]; + + k->heap_node = HEAP_insert(p->heap, cost_sq, r); + } + else { + if (k->heap_node) { + struct KnotRemoveState *r; + r = HEAP_node_ptr(k->heap_node); + HEAP_remove(p->heap, k->heap_node); + +#ifdef USE_TPOOL + rstate_pool_elem_free(p->epool, r); +#else + free(r); +#endif + + k->heap_node = NULL; + } + } +} + +/** + * Return length after being reduced. + */ +static uint curve_incremental_simplify( + const struct PointData *pd, + struct Knot *knots, const uint knots_len, uint knots_len_remaining, + double error_sq_max, const uint dims) +{ + +#ifdef USE_TPOOL + struct ElemPool_KnotRemoveState epool; + + rstate_pool_create(&epool, 0); +#endif + + Heap *heap = HEAP_new(knots_len); + + struct KnotRemove_Params params = { + .pd = pd, + .heap = heap, +#ifdef USE_TPOOL + .epool = &epool, +#endif + }; + + for (uint i = 0; i < knots_len; i++) { + struct Knot *k = &knots[i]; + if (k->can_remove && (k->is_removed == false) && (k->is_corner == false)) { + knot_remove_error_recalculate(¶ms, k, error_sq_max, dims); + } + } + + while (HEAP_is_empty(heap) == false) { + struct Knot *k; + + { + const double error_sq = HEAP_top_value(heap); + struct KnotRemoveState *r = HEAP_popmin(heap); + k = &knots[r->index]; + k->heap_node = NULL; + k->prev->handles[1] = r->handles[0]; + k->next->handles[0] = r->handles[1]; + + k->prev->error_sq_next = error_sq; + +#ifdef USE_TPOOL + rstate_pool_elem_free(&epool, r); +#else + free(r); +#endif + } + + if (UNLIKELY(knots_len_remaining <= 2)) { + continue; + } + + struct Knot *k_prev = k->prev; + struct Knot *k_next = k->next; + + /* Remove ourselves */ + k_next->prev = k_prev; + k_prev->next = k_next; + + k->next = NULL; + k->prev = NULL; + k->is_removed = true; + + if (k_prev->can_remove && (k_prev->is_corner == false) && (k_prev->prev && k_prev->next)) { + knot_remove_error_recalculate(¶ms, k_prev, error_sq_max, dims); + } + + if (k_next->can_remove && (k_next->is_corner == false) && (k_next->prev && k_next->next)) { + knot_remove_error_recalculate(¶ms, k_next, error_sq_max, dims); + } + + knots_len_remaining -= 1; + } + +#ifdef USE_TPOOL + rstate_pool_destroy(&epool); +#endif + + HEAP_free(heap, free); + + return knots_len_remaining; +} + + +#ifdef USE_KNOT_REFIT + +struct KnotRefit_Params { + Heap *heap; + const struct PointData *pd; +#ifdef USE_TPOOL + struct ElemPool_KnotRefitState *epool; +#endif +}; + +static void knot_refit_error_recalculate( + struct KnotRefit_Params *p, + struct Knot *knots, const uint knots_len, + struct Knot *k, + const double error_sq_max, + const uint dims) +{ + assert(k->can_remove); + +#ifdef USE_KNOT_REFIT_REMOVE + { + double handles[2]; + + /* First check if we can remove, this allows to refit and remove as we go. */ + const double cost_sq = knot_calc_curve_error_value( + p->pd, k->prev, k->next, + k->prev->tan[1], k->next->tan[0], + dims, + handles); + + if (cost_sq < error_sq_max) { + struct KnotRefitState *r; + if (k->heap_node) { + r = HEAP_node_ptr(k->heap_node); + HEAP_remove(p->heap, k->heap_node); + } + else { +#ifdef USE_TPOOL + r = refit_pool_elem_alloc(p->epool); +#else + r = malloc(sizeof(*r)); +#endif + r->index = k->index; + } + + r->index_refit = SPLIT_POINT_INVALID; + + r->handles_prev[0] = handles[0]; + r->handles_prev[1] = 0.0; /* unused */ + r->handles_next[0] = 0.0; /* unused */ + r->handles_next[1] = handles[1]; + + r->error_sq[0] = r->error_sq[1] = cost_sq; + + /* Always perform removal before refitting, (make a negative number) */ + k->heap_node = HEAP_insert(p->heap, cost_sq - error_sq_max, r); + + return; + } + } +#else + (void)error_sq_max; +#endif /* USE_KNOT_REFIT_REMOVE */ + + const uint refit_index = knot_find_split_point( + p->pd, k->prev, k->next, + knots_len, + dims); + + if ((refit_index == SPLIT_POINT_INVALID) || + (refit_index == k->index)) + { + goto remove; + } + + struct Knot *k_refit = &knots[refit_index]; + + const double cost_sq_src_max = MAX2(k->prev->error_sq_next, k->error_sq_next); + assert(cost_sq_src_max <= error_sq_max); + + double cost_sq_dst[2]; + double handles_prev[2], handles_next[2]; + + if ((((cost_sq_dst[0] = knot_calc_curve_error_value( + p->pd, k->prev, k_refit, + k->prev->tan[1], k_refit->tan[0], + dims, + handles_prev)) < cost_sq_src_max) && + ((cost_sq_dst[1] = knot_calc_curve_error_value( + p->pd, k_refit, k->next, + k_refit->tan[1], k->next->tan[0], + dims, + handles_next)) < cost_sq_src_max))) + { + { + struct KnotRefitState *r; + if (k->heap_node) { + r = HEAP_node_ptr(k->heap_node); + HEAP_remove(p->heap, k->heap_node); + } + else { +#ifdef USE_TPOOL + r = refit_pool_elem_alloc(p->epool); +#else + r = malloc(sizeof(*r)); +#endif + r->index = k->index; + } + + r->index_refit = refit_index; + + r->handles_prev[0] = handles_prev[0]; + r->handles_prev[1] = handles_prev[1]; + + r->handles_next[0] = handles_next[0]; + r->handles_next[1] = handles_next[1]; + + r->error_sq[0] = cost_sq_dst[0]; + r->error_sq[1] = cost_sq_dst[1]; + + const double cost_sq_dst_max = MAX2(cost_sq_dst[0], cost_sq_dst[1]); + + assert(cost_sq_dst_max < cost_sq_src_max); + + /* Weight for the greatest improvement */ + k->heap_node = HEAP_insert(p->heap, cost_sq_src_max - cost_sq_dst_max, r); + } + } + else { +remove: + if (k->heap_node) { + struct KnotRefitState *r; + r = HEAP_node_ptr(k->heap_node); + HEAP_remove(p->heap, k->heap_node); + +#ifdef USE_TPOOL + refit_pool_elem_free(p->epool, r); +#else + free(r); +#endif + + k->heap_node = NULL; + } + } +} + +/** + * Re-adjust the curves by re-fitting points. + * test the error from moving using points between the adjacent. + */ +static uint curve_incremental_simplify_refit( + const struct PointData *pd, + struct Knot *knots, const uint knots_len, uint knots_len_remaining, + const double error_sq_max, + const uint dims) +{ +#ifdef USE_TPOOL + struct ElemPool_KnotRefitState epool; + + refit_pool_create(&epool, 0); +#endif + + Heap *heap = HEAP_new(knots_len); + + struct KnotRefit_Params params = { + .pd = pd, + .heap = heap, +#ifdef USE_TPOOL + .epool = &epool, +#endif + }; + + for (uint i = 0; i < knots_len; i++) { + struct Knot *k = &knots[i]; + if (k->can_remove && + (k->is_removed == false) && + (k->is_corner == false) && + (k->prev && k->next)) + { + knot_refit_error_recalculate(¶ms, knots, knots_len, k, error_sq_max, dims); + } + } + + while (HEAP_is_empty(heap) == false) { + struct Knot *k_old, *k_refit; + + { + struct KnotRefitState *r = HEAP_popmin(heap); + k_old = &knots[r->index]; + k_old->heap_node = NULL; + +#ifdef USE_KNOT_REFIT_REMOVE + if (r->index_refit == SPLIT_POINT_INVALID) { + k_refit = NULL; + } + else +#endif + { + k_refit = &knots[r->index_refit]; + k_refit->handles[0] = r->handles_prev[1]; + k_refit->handles[1] = r->handles_next[0]; + } + + k_old->prev->handles[1] = r->handles_prev[0]; + k_old->next->handles[0] = r->handles_next[1]; + +#ifdef USE_TPOOL + refit_pool_elem_free(&epool, r); +#else + free(r); +#endif + } + + if (UNLIKELY(knots_len_remaining <= 2)) { + continue; + } + + struct Knot *k_prev = k_old->prev; + struct Knot *k_next = k_old->next; + + k_old->next = NULL; + k_old->prev = NULL; + k_old->is_removed = true; + +#ifdef USE_KNOT_REFIT_REMOVE + if (k_refit == NULL) { + k_next->prev = k_prev; + k_prev->next = k_next; + + knots_len_remaining -= 1; + } + else +#endif + { + /* Remove ourselves */ + k_next->prev = k_refit; + k_prev->next = k_refit; + + k_refit->prev = k_prev; + k_refit->next = k_next; + k_refit->is_removed = false; + } + + if (k_prev->can_remove && (k_prev->is_corner == false) && (k_prev->prev && k_prev->next)) { + knot_refit_error_recalculate(¶ms, knots, knots_len, k_prev, error_sq_max, dims); + } + + if (k_next->can_remove && (k_next->is_corner == false) && (k_next->prev && k_next->next)) { + knot_refit_error_recalculate(¶ms, knots, knots_len, k_next, error_sq_max, dims); + } + } + +#ifdef USE_TPOOL + refit_pool_destroy(&epool); +#endif + + HEAP_free(heap, free); + + return knots_len_remaining; +} + +#endif /* USE_KNOT_REFIT */ + + +#ifdef USE_CORNER_DETECT + +struct KnotCorner_Params { + Heap *heap; + const struct PointData *pd; +#ifdef USE_TPOOL + struct ElemPool_KnotCornerState *epool; +#endif +}; + +/** + * (Re)calculate the error incurred from turning this into a corner. + */ +static void knot_corner_error_recalculate( + struct KnotCorner_Params *p, + struct Knot *k_split, + struct Knot *k_prev, struct Knot *k_next, + const double error_sq_max, + const uint dims) +{ + assert(k_prev->can_remove && k_next->can_remove); + + double handles_prev[2], handles_next[2]; + /* Test skipping 'k_prev' by using points (k_prev->prev to k_split) */ + double cost_sq_dst[2]; + + if (((cost_sq_dst[0] = knot_calc_curve_error_value( + p->pd, k_prev, k_split, + k_prev->tan[1], k_prev->tan[1], + dims, + handles_prev)) < error_sq_max) && + ((cost_sq_dst[1] = knot_calc_curve_error_value( + p->pd, k_split, k_next, + k_next->tan[0], k_next->tan[0], + dims, + handles_next)) < error_sq_max)) + { + struct KnotCornerState *c; + if (k_split->heap_node) { + c = HEAP_node_ptr(k_split->heap_node); + HEAP_remove(p->heap, k_split->heap_node); + } + else { +#ifdef USE_TPOOL + c = corner_pool_elem_alloc(p->epool); +#else + c = malloc(sizeof(*c)); +#endif + c->index = k_split->index; + } + + c->index_adjacent[0] = k_prev->index; + c->index_adjacent[1] = k_next->index; + + /* Need to store handle lengths for both sides */ + c->handles_prev[0] = handles_prev[0]; + c->handles_prev[1] = handles_prev[1]; + + c->handles_next[0] = handles_next[0]; + c->handles_next[1] = handles_next[1]; + + c->error_sq[0] = cost_sq_dst[0]; + c->error_sq[1] = cost_sq_dst[1]; + + const double cost_max_sq = MAX2(cost_sq_dst[0], cost_sq_dst[1]); + k_split->heap_node = HEAP_insert(p->heap, cost_max_sq, c); + } + else { + if (k_split->heap_node) { + struct KnotCornerState *c; + c = HEAP_node_ptr(k_split->heap_node); + HEAP_remove(p->heap, k_split->heap_node); +#ifdef USE_TPOOL + corner_pool_elem_free(p->epool, c); +#else + free(c); +#endif + k_split->heap_node = NULL; + } + } +} + + +/** + * Attempt to collapse close knots into corners, + * as long as they fall below the error threshold. + */ +static uint curve_incremental_simplify_corners( + const struct PointData *pd, + struct Knot *knots, const uint knots_len, uint knots_len_remaining, + const double error_sq_max, const double error_sq_2x_max, + const double corner_angle, + const uint dims, + uint *r_corner_index_len) +{ +#ifdef USE_TPOOL + struct ElemPool_KnotCornerState epool; + + corner_pool_create(&epool, 0); +#endif + + Heap *heap = HEAP_new(0); + + struct KnotCorner_Params params = { + .pd = pd, + .heap = heap, +#ifdef USE_TPOOL + .epool = &epool, +#endif + }; + +#ifdef USE_VLA + double plane_no[dims]; + double k_proj_ref[dims]; + double k_proj_split[dims]; +#else + double *plane_no = alloca(sizeof(double) * dims); + double *k_proj_ref = alloca(sizeof(double) * dims); + double *k_proj_split = alloca(sizeof(double) * dims); +#endif + + const double corner_angle_cos = cos(corner_angle); + + uint corner_index_len = 0; + + for (uint i = 0; i < knots_len; i++) { + if ((knots[i].is_removed == false) && + (knots[i].can_remove == true) && + (knots[i].next && knots[i].next->can_remove)) + { + struct Knot *k_prev = &knots[i]; + struct Knot *k_next = k_prev->next; + + /* Angle outside threshold */ + if (dot_vnvn(k_prev->tan[0], k_next->tan[1], dims) < corner_angle_cos) { + /* Measure distance projected onto a plane, + * since the points may be offset along their own tangents. */ + sub_vn_vnvn(plane_no, k_next->tan[0], k_prev->tan[1], dims); + + /* Compare 2x so as to allow both to be changed by maximum of error_sq_max */ + const uint split_index = knot_find_split_point_on_axis( + pd, k_prev, k_next, + knots_len, + plane_no, + dims); + + if (split_index != SPLIT_POINT_INVALID) { + + project_vn_vnvn(k_proj_ref, &pd->points[k_prev->index * dims], k_prev->tan[1], dims); + project_vn_vnvn(k_proj_split, &pd->points[split_index * dims], k_prev->tan[1], dims); + + if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_2x_max) { + + project_vn_vnvn(k_proj_ref, &pd->points[k_next->index * dims], k_next->tan[0], dims); + project_vn_vnvn(k_proj_split, &pd->points[split_index * dims], k_next->tan[0], dims); + + if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_2x_max) { + + struct Knot *k_split = &knots[split_index]; + + knot_corner_error_recalculate( + ¶ms, + k_split, k_prev, k_next, + error_sq_max, + dims); + } + } + } + } + } + } + + while (HEAP_is_empty(heap) == false) { + struct KnotCornerState *c = HEAP_popmin(heap); + + struct Knot *k_split = &knots[c->index]; + + /* Remove while collapsing */ + struct Knot *k_prev = &knots[c->index_adjacent[0]]; + struct Knot *k_next = &knots[c->index_adjacent[1]]; + + /* Insert */ + k_split->is_removed = false; + k_split->prev = k_prev; + k_split->next = k_next; + k_prev->next = k_split; + k_next->prev = k_split; + + /* Update tangents */ + k_split->tan[0] = k_prev->tan[1]; + k_split->tan[1] = k_next->tan[0]; + + /* Own handles */ + k_prev->handles[1] = c->handles_prev[0]; + k_split->handles[0] = c->handles_prev[1]; + k_split->handles[1] = c->handles_next[0]; + k_next->handles[0] = c->handles_next[1]; + + k_prev->error_sq_next = c->error_sq[0]; + k_split->error_sq_next = c->error_sq[1]; + + k_split->heap_node = NULL; + +#ifdef USE_TPOOL + corner_pool_elem_free(&epool, c); +#else + free(c); +#endif + + k_split->is_corner = true; + + knots_len_remaining++; + + corner_index_len++; + } + +#ifdef USE_TPOOL + corner_pool_destroy(&epool); +#endif + + HEAP_free(heap, free); + + *r_corner_index_len = corner_index_len; + + return knots_len_remaining; +} + +#endif /* USE_CORNER_DETECT */ + +int curve_fit_cubic_to_points_refit_db( + const double *points, + const uint points_len, + const uint dims, + const double error_threshold, + const uint calc_flag, + const uint *corners, + const uint corners_len, + const double corner_angle, + + double **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 knots_len = points_len; + struct Knot *knots = malloc(sizeof(Knot) * knots_len); + knots[0].next = NULL; + +#ifndef USE_CORNER_DETECT + (void)r_corner_index_array; + (void)r_corner_index_len; +#endif + +(void)corners; +(void)corners_len; + + const bool is_cyclic = (calc_flag & CURVE_FIT_CALC_CYCLIC) != 0 && (points_len > 2); +#ifdef USE_CORNER_DETECT + const bool use_corner = (corner_angle < M_PI); +#else + (void)corner_angle; +#endif + + /* Over alloc the list x2 for cyclic curves, + * so we can evaluate across the start/end */ + double *points_alloc = NULL; + if (is_cyclic) { + points_alloc = malloc((sizeof(double) * points_len * dims) * 2); + memcpy(points_alloc, points, sizeof(double) * points_len * dims); + memcpy(points_alloc + (points_len * dims), points_alloc, sizeof(double) * points_len * dims); + points = points_alloc; + } + + double *tangents = malloc(sizeof(double) * knots_len * 2 * dims); + + { + double *t_step = tangents; + for (uint i = 0; i < knots_len; i++) { + knots[i].next = (knots + i) + 1; + knots[i].prev = (knots + i) - 1; + + knots[i].heap_node = NULL; + knots[i].index = i; + knots[i].index = i; + knots[i].can_remove = true; + knots[i].is_removed = false; + knots[i].is_corner = false; + knots[i].error_sq_next = 0.0; + knots[i].tan[0] = t_step; t_step += dims; + knots[i].tan[1] = t_step; t_step += dims; + } + assert(t_step == &tangents[knots_len * 2 * dims]); + } + + if (is_cyclic) { + knots[0].prev = &knots[knots_len - 1]; + knots[knots_len - 1].next = &knots[0]; + } + else { + knots[0].prev = NULL; + knots[knots_len - 1].next = NULL; + + /* always keep end-points */ + knots[0].can_remove = false; + knots[knots_len - 1].can_remove = false; + } + +#ifdef USE_LENGTH_CACHE + double *points_length_cache = malloc(sizeof(double) * points_len * (is_cyclic ? 2 : 1)); +#endif + + /* Initialize tangents, + * also set the values for knot handles since some may not collapse. */ + { +#ifdef USE_VLA + double tan_prev[dims]; + double tan_next[dims]; +#else + double *tan_prev = alloca(sizeof(double) * dims); + double *tan_next = alloca(sizeof(double) * dims); +#endif + double len_prev, len_next; + +#if 0 + /* 2x normalize calculations, but correct */ + + for (uint i = 0; i < knots_len; i++) { + Knot *k = &knots[i]; + + if (k->prev) { + sub_vn_vnvn(tan_prev, &points[k->prev->index * dims], &points[k->index * dims], dims); +#ifdef USE_LENGTH_CACHE + points_length_cache[i] = +#endif + len_prev = normalize_vn(tan_prev, dims); + } + else { + zero_vn(tan_prev, dims); + len_prev = 0.0; + } + + if (k->next) { + sub_vn_vnvn(tan_next, &points[k->index * dims], &points[k->next->index * dims], dims); + len_next = normalize_vn(tan_next, dims); + } + else { + zero_vn(tan_next, dims); + len_next = 0.0; + } + + add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims); + normalize_vn(k->tan[0], dims); + copy_vnvn(k->tan[1], k->tan[0], dims); + k->handles[0] = len_prev / 3; + k->handles[1] = len_next / 3; + } +#else + if (is_cyclic) { + len_prev = normalize_vn_vnvn(tan_prev, &points[(knots_len - 2) * dims], &points[(knots_len - 1) * dims], dims); + for (uint i_curr = knots_len - 1, i_next = 0; i_next < knots_len; i_curr = i_next++) { + struct Knot *k = &knots[i_curr]; +#ifdef USE_LENGTH_CACHE + points_length_cache[i_next] = +#endif + len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims); + + add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims); + normalize_vn(k->tan[0], dims); + copy_vnvn(k->tan[1], k->tan[0], dims); + k->handles[0] = len_prev / 3; + k->handles[1] = len_next / 3; + + copy_vnvn(tan_prev, tan_next, dims); + len_prev = len_next; + } + } + else { +#ifdef USE_LENGTH_CACHE + points_length_cache[0] = 0.0; + points_length_cache[1] = +#endif + len_prev = normalize_vn_vnvn(tan_prev, &points[0 * dims], &points[1 * dims], dims); + copy_vnvn(knots[0].tan[0], tan_prev, dims); + copy_vnvn(knots[0].tan[1], tan_prev, dims); + knots[0].handles[0] = len_prev / 3; + knots[0].handles[1] = len_next / 3; + + for (uint i_curr = 1, i_next = 2; i_next < knots_len; i_curr = i_next++) { + struct Knot *k = &knots[i_curr]; + +#ifdef USE_LENGTH_CACHE + points_length_cache[i_next] = +#endif + len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims); + + add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims); + normalize_vn(k->tan[0], dims); + copy_vnvn(k->tan[1], k->tan[0], dims); + k->handles[0] = len_prev / 3; + k->handles[1] = len_next / 3; + + copy_vnvn(tan_prev, tan_next, dims); + len_prev = len_next; + } + copy_vnvn(knots[knots_len - 1].tan[0], tan_next, dims); + copy_vnvn(knots[knots_len - 1].tan[1], tan_next, dims); + + knots[knots_len - 1].handles[0] = len_next / 3; + knots[knots_len - 1].handles[1] = len_next / 3; + } +#endif + } + +#ifdef USE_LENGTH_CACHE + if (is_cyclic) { + memcpy(&points_length_cache[points_len], points_length_cache, sizeof(double) * points_len); + } +#endif + + +#if 0 + for (uint i = 0; i < knots_len; i++) { + Knot *k = &knots[i]; + printf("TAN %.8f %.8f %.8f %.8f\n", k->tan[0][0], k->tan[0][1], k->tan[1][0], k->tan[0][1]); + } +#endif + + const struct PointData pd = { + .points = points, + .points_len = points_len, +#ifdef USE_LENGTH_CACHE + .points_length_cache = points_length_cache, +#endif + }; + + uint knots_len_remaining = knots_len; + + /* 'curve_incremental_simplify_refit' can be called here, but its very slow + * just remove all within the threshold first. */ + knots_len_remaining = curve_incremental_simplify( + &pd, knots, knots_len, knots_len_remaining, + SQUARE(error_threshold), dims); + +#ifdef USE_CORNER_DETECT + if (use_corner) { + for (uint i = 0; i < knots_len; i++) { + assert(knots[i].heap_node == NULL); + } + + knots_len_remaining = curve_incremental_simplify_corners( + &pd, knots, knots_len, knots_len_remaining, + SQUARE(error_threshold), SQUARE(error_threshold * 3), + corner_angle, + dims, + r_corner_index_len); + } +#endif /* USE_CORNER_DETECT */ + +#ifdef USE_KNOT_REFIT + knots_len_remaining = curve_incremental_simplify_refit( + &pd, knots, knots_len, knots_len_remaining, + SQUARE(error_threshold), + dims); +#endif /* USE_KNOT_REFIT */ + + +#ifdef USE_CORNER_DETECT + if (use_corner) { + if (is_cyclic == false) { + *r_corner_index_len += 2; + } + + uint *corner_index_array = malloc(sizeof(uint) * (*r_corner_index_len)); + uint k_index = 0, c_index = 0; + uint i = 0; + + if (is_cyclic == false) { + corner_index_array[c_index++] = k_index; + k_index++; + i++; + } + + for (; i < knots_len; i++) { + if (knots[i].is_removed == false) { + if (knots[i].is_corner == true) { + corner_index_array[c_index++] = k_index; + } + k_index++; + } + } + + if (is_cyclic == false) { + corner_index_array[c_index++] = k_index; + k_index++; + } + + assert(c_index == *r_corner_index_len); + *r_corner_index_array = corner_index_array; + } +#endif /* USE_CORNER_DETECT */ + +#ifdef USE_LENGTH_CACHE + free(points_length_cache); +#endif + + uint *cubic_orig_index = NULL; + + if (r_cubic_orig_index) { + cubic_orig_index = malloc(sizeof(uint) * knots_len_remaining); + } + + struct Knot *knots_first = NULL; + { + struct Knot *k; + for (uint i = 0; i < knots_len; i++) { + if (knots[i].is_removed == false) { + knots_first = &knots[i]; + break; + } + } + + if (cubic_orig_index) { + k = knots_first; + for (uint i = 0; i < knots_len_remaining; i++, k = k->next) { + cubic_orig_index[i] = k->index; + } + } + } + + /* Correct unused handle endpoints - not essential, but nice behavior */ + if (is_cyclic == false) { + struct Knot *knots_last = knots_first; + while (knots_last->next) { + knots_last = knots_last->next; + } + knots_first->handles[0] = -knots_first->handles[1]; + knots_last->handles[1] = -knots_last->handles[0]; + } + + /* 3x for one knot and two handles */ + double *cubic_array = malloc(sizeof(double) * knots_len_remaining * 3 * dims); + + { + double *c_step = cubic_array; + struct Knot *k = knots_first; + for (uint i = 0; i < knots_len_remaining; i++, k = k->next) { + const double *p = &points[k->index * dims]; + + madd_vn_vnvn_fl(c_step, p, k->tan[0], k->handles[0], dims); + c_step += dims; + copy_vnvn(c_step, p, dims); + c_step += dims; + madd_vn_vnvn_fl(c_step, p, k->tan[1], k->handles[1], dims); + c_step += dims; + } + assert(c_step == &cubic_array[knots_len_remaining * 3 * dims]); + } + + if (points_alloc) { + free(points_alloc); + points_alloc = NULL; + } + + free(knots); + free(tangents); + + if (r_cubic_orig_index) { + *r_cubic_orig_index = cubic_orig_index; + } + + *r_cubic_array = cubic_array; + *r_cubic_array_len = knots_len_remaining; + + return 0; +} + + +int curve_fit_cubic_to_points_refit_fl( + const float *points, + const unsigned int points_len, + const unsigned int dims, + const float error_threshold, + const unsigned int calc_flag, + const unsigned int *corners, + unsigned int corners_len, + const float corner_angle, + + float **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) +{ + const uint points_flat_len = points_len * dims; + double *points_db = malloc(sizeof(double) * points_flat_len); + + copy_vndb_vnfl(points_db, points, points_flat_len); + + double *cubic_array_db = NULL; + float *cubic_array_fl = NULL; + uint cubic_array_len = 0; + + int result = curve_fit_cubic_to_points_refit_db( + points_db, points_len, dims, error_threshold, calc_flag, corners, corners_len, + corner_angle, + &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 index c77e5c6e062..f9eaa4c647c 100644 --- a/extern/curve_fit_nd/intern/curve_fit_inline.h +++ b/extern/curve_fit_nd/intern/curve_fit_inline.h @@ -290,14 +290,12 @@ MINLINE bool equals_vnvn( return true; } -#if 0 MINLINE void project_vn_vnvn( double v_out[], const double p[], const double v_proj[], const uint dims) { const double mul = dot_vnvn(p, v_proj, dims) / dot_vnvn(v_proj, v_proj, dims); mul_vnvn_fl(v_out, v_proj, mul, dims); } -#endif MINLINE void project_vn_vnvn_normalized( double v_out[], const double p[], const double v_proj[], const uint dims) diff --git a/extern/curve_fit_nd/intern/generic_alloc_impl.h b/extern/curve_fit_nd/intern/generic_alloc_impl.h new file mode 100644 index 00000000000..687c154f14a --- /dev/null +++ b/extern/curve_fit_nd/intern/generic_alloc_impl.h @@ -0,0 +1,220 @@ +/* + * Copyright (c) 2016, Blender Foundation. + * + * 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 generic_alloc_impl.c + * \ingroup curve_fit + * + * Simple Memory Chunking Allocator + * ================================ + * + * Defines need to be set: + * - #TPOOL_IMPL_PREFIX: Prefix to use for the API. + * - #TPOOL_ALLOC_TYPE: Struct type this pool handles. + * - #TPOOL_STRUCT: Name for pool struct name. + * - #TPOOL_CHUNK_SIZE: Chunk size (optional), use 64kb when not defined. + * + * \note #TPOOL_ALLOC_TYPE must be at least ``sizeof(void *)``. + * + * Defines the API, uses #TPOOL_IMPL_PREFIX to prefix each function. + * + * - *_pool_create() + * - *_pool_destroy() + * - *_pool_clear() + * + * - *_pool_elem_alloc() + * - *_pool_elem_calloc() + * - *_pool_elem_free() + */ + +/* check we're not building directly */ +#if !defined(TPOOL_IMPL_PREFIX) || \ + !defined(TPOOL_ALLOC_TYPE) || \ + !defined(TPOOL_STRUCT) +# error "This file can't be compiled directly, include in another source file" +#endif + +#define _CONCAT_AUX(MACRO_ARG1, MACRO_ARG2) MACRO_ARG1 ## MACRO_ARG2 +#define _CONCAT(MACRO_ARG1, MACRO_ARG2) _CONCAT_AUX(MACRO_ARG1, MACRO_ARG2) +#define _TPOOL_PREFIX(id) _CONCAT(TPOOL_IMPL_PREFIX, _##id) + +/* local identifiers */ +#define pool_create _TPOOL_PREFIX(pool_create) +#define pool_destroy _TPOOL_PREFIX(pool_destroy) +#define pool_clear _TPOOL_PREFIX(pool_clear) + +#define pool_elem_alloc _TPOOL_PREFIX(pool_elem_alloc) +#define pool_elem_calloc _TPOOL_PREFIX(pool_elem_calloc) +#define pool_elem_free _TPOOL_PREFIX(pool_elem_free) + +/* private identifiers (only for this file, undefine after) */ +#define pool_alloc_chunk _TPOOL_PREFIX(pool_alloc_chunk) +#define TPoolChunk _TPOOL_PREFIX(TPoolChunk) +#define TPoolChunkElemFree _TPOOL_PREFIX(TPoolChunkElemFree) + +#ifndef TPOOL_CHUNK_SIZE +#define TPOOL_CHUNK_SIZE (1 << 16) /* 64kb */ +#define _TPOOL_CHUNK_SIZE_UNDEF +#endif + +#ifndef UNLIKELY +# ifdef __GNUC__ +# define UNLIKELY(x) __builtin_expect(!!(x), 0) +# else +# define UNLIKELY(x) (x) +# endif +#endif + +#ifdef __GNUC__ +# define MAYBE_UNUSED __attribute__((unused)) +#else +# define MAYBE_UNUSED +#endif + + +struct TPoolChunk { + struct TPoolChunk *prev; + unsigned int size; + unsigned int bufsize; + TPOOL_ALLOC_TYPE buf[0]; +}; + +struct TPoolChunkElemFree { + struct TPoolChunkElemFree *next; +}; + +struct TPOOL_STRUCT { + /* Always keep at least one chunk (never NULL) */ + struct TPoolChunk *chunk; + /* when NULL, allocate a new chunk */ + struct TPoolChunkElemFree *free; +}; + +/** + * Number of elems to include per #TPoolChunk when no reserved size is passed, + * or we allocate past the reserved number. + * + * \note Optimize number for 64kb allocs. + */ +#define _TPOOL_CHUNK_DEFAULT_NUM \ + (((1 << 16) - sizeof(struct TPoolChunk)) / sizeof(TPOOL_ALLOC_TYPE)) + + +/** \name Internal Memory Management + * \{ */ + +static struct TPoolChunk *pool_alloc_chunk( + unsigned int tot_elems, struct TPoolChunk *chunk_prev) +{ + struct TPoolChunk *chunk = malloc( + sizeof(struct TPoolChunk) + (sizeof(TPOOL_ALLOC_TYPE) * tot_elems)); + chunk->prev = chunk_prev; + chunk->bufsize = tot_elems; + chunk->size = 0; + return chunk; +} + +static TPOOL_ALLOC_TYPE *pool_elem_alloc(struct TPOOL_STRUCT *pool) +{ + TPOOL_ALLOC_TYPE *elem; + + if (pool->free) { + elem = (TPOOL_ALLOC_TYPE *)pool->free; + pool->free = pool->free->next; + } + else { + struct TPoolChunk *chunk = pool->chunk; + if (UNLIKELY(chunk->size == chunk->bufsize)) { + chunk = pool->chunk = pool_alloc_chunk(_TPOOL_CHUNK_DEFAULT_NUM, chunk); + } + elem = &chunk->buf[chunk->size++]; + } + + return elem; +} + +MAYBE_UNUSED +static TPOOL_ALLOC_TYPE *pool_elem_calloc(struct TPOOL_STRUCT *pool) +{ + TPOOL_ALLOC_TYPE *elem = pool_elem_alloc(pool); + memset(elem, 0, sizeof(*elem)); + return elem; +} + +static void pool_elem_free(struct TPOOL_STRUCT *pool, TPOOL_ALLOC_TYPE *elem) +{ + struct TPoolChunkElemFree *elem_free = (struct TPoolChunkElemFree *)elem; + elem_free->next = pool->free; + pool->free = elem_free; +} + +static void pool_create(struct TPOOL_STRUCT *pool, unsigned int tot_reserve) +{ + pool->chunk = pool_alloc_chunk((tot_reserve > 1) ? tot_reserve : _TPOOL_CHUNK_DEFAULT_NUM, NULL); + pool->free = NULL; +} + +MAYBE_UNUSED +static void pool_clear(struct TPOOL_STRUCT *pool) +{ + /* Remove all except the last chunk */ + while (pool->chunk->prev) { + struct TPoolChunk *chunk_prev = pool->chunk->prev; + free(pool->chunk); + pool->chunk = chunk_prev; + } + pool->chunk->size = 0; + pool->free = NULL; +} + +static void pool_destroy(struct TPOOL_STRUCT *pool) +{ + struct TPoolChunk *chunk = pool->chunk; + do { + struct TPoolChunk *chunk_prev; + chunk_prev = chunk->prev; + free(chunk); + chunk = chunk_prev; + } while (chunk); + + pool->chunk = NULL; + pool->free = NULL; +} + +/** \} */ + +#undef _TPOOL_CHUNK_DEFAULT_NUM +#undef _CONCAT_AUX +#undef _CONCAT +#undef _TPOOL_PREFIX + +#undef TPoolChunk +#undef TPoolChunkElemFree + +#ifdef _TPOOL_CHUNK_SIZE_UNDEF +# undef TPOOL_CHUNK_SIZE +# undef _TPOOL_CHUNK_SIZE_UNDEF +#endif diff --git a/extern/curve_fit_nd/intern/generic_heap.c b/extern/curve_fit_nd/intern/generic_heap.c new file mode 100644 index 00000000000..1e2efa5b43d --- /dev/null +++ b/extern/curve_fit_nd/intern/generic_heap.c @@ -0,0 +1,278 @@ +/* + * Copyright (c) 2016, Blender Foundation. + * + * 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 generic_heap.c + * \ingroup curve_fit + */ + +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <assert.h> + +#include "generic_heap.h" + +/* swap with a temp value */ +#define SWAP_TVAL(tval, a, b) { \ + (tval) = (a); \ + (a) = (b); \ + (b) = (tval); \ +} (void)0 + +#ifdef __GNUC__ +# define UNLIKELY(x) __builtin_expect(!!(x), 0) +#else +# define UNLIKELY(x) (x) +#endif + + +/***/ + +struct HeapNode { + void *ptr; + double value; + unsigned int index; +}; + +/* heap_* pool allocator */ +#define TPOOL_IMPL_PREFIX heap +#define TPOOL_ALLOC_TYPE HeapNode +#define TPOOL_STRUCT HeapMemPool +#include "generic_alloc_impl.h" +#undef TPOOL_IMPL_PREFIX +#undef TPOOL_ALLOC_TYPE +#undef TPOOL_STRUCT + +struct Heap { + unsigned int size; + unsigned int bufsize; + HeapNode **tree; + + struct HeapMemPool pool; +}; + +/** \name Internal Functions + * \{ */ + +#define HEAP_PARENT(i) (((i) - 1) >> 1) +#define HEAP_LEFT(i) (((i) << 1) + 1) +#define HEAP_RIGHT(i) (((i) << 1) + 2) +#define HEAP_COMPARE(a, b) ((a)->value < (b)->value) + +#if 0 /* UNUSED */ +#define HEAP_EQUALS(a, b) ((a)->value == (b)->value) +#endif + +static void heap_swap(Heap *heap, const unsigned int i, const unsigned int j) +{ + +#if 0 + SWAP(unsigned int, heap->tree[i]->index, heap->tree[j]->index); + SWAP(HeapNode *, heap->tree[i], heap->tree[j]); +#else + HeapNode **tree = heap->tree; + union { + unsigned int index; + HeapNode *node; + } tmp; + SWAP_TVAL(tmp.index, tree[i]->index, tree[j]->index); + SWAP_TVAL(tmp.node, tree[i], tree[j]); +#endif +} + +static void heap_down(Heap *heap, unsigned int i) +{ + /* size won't change in the loop */ + const unsigned int size = heap->size; + + while (1) { + const unsigned int l = HEAP_LEFT(i); + const unsigned int r = HEAP_RIGHT(i); + unsigned int smallest; + + smallest = ((l < size) && HEAP_COMPARE(heap->tree[l], heap->tree[i])) ? l : i; + + if ((r < size) && HEAP_COMPARE(heap->tree[r], heap->tree[smallest])) { + smallest = r; + } + + if (smallest == i) { + break; + } + + heap_swap(heap, i, smallest); + i = smallest; + } +} + +static void heap_up(Heap *heap, unsigned int i) +{ + while (i > 0) { + const unsigned int p = HEAP_PARENT(i); + + if (HEAP_COMPARE(heap->tree[p], heap->tree[i])) { + break; + } + heap_swap(heap, p, i); + i = p; + } +} + +/** \} */ + + +/** \name Public Heap API + * \{ */ + +/* use when the size of the heap is known in advance */ +Heap *HEAP_new(unsigned int tot_reserve) +{ + Heap *heap = malloc(sizeof(Heap)); + /* ensure we have at least one so we can keep doubling it */ + heap->size = 0; + heap->bufsize = tot_reserve ? tot_reserve : 1; + heap->tree = malloc(heap->bufsize * sizeof(HeapNode *)); + + heap_pool_create(&heap->pool, tot_reserve); + + return heap; +} + +void HEAP_free(Heap *heap, HeapFreeFP ptrfreefp) +{ + if (ptrfreefp) { + unsigned int i; + + for (i = 0; i < heap->size; i++) { + ptrfreefp(heap->tree[i]->ptr); + } + } + + heap_pool_destroy(&heap->pool); + + free(heap->tree); + free(heap); +} + +void HEAP_clear(Heap *heap, HeapFreeFP ptrfreefp) +{ + if (ptrfreefp) { + unsigned int i; + + for (i = 0; i < heap->size; i++) { + ptrfreefp(heap->tree[i]->ptr); + } + } + heap->size = 0; + + heap_pool_clear(&heap->pool); +} + +HeapNode *HEAP_insert(Heap *heap, double value, void *ptr) +{ + HeapNode *node; + + if (UNLIKELY(heap->size >= heap->bufsize)) { + heap->bufsize *= 2; + heap->tree = realloc(heap->tree, heap->bufsize * sizeof(*heap->tree)); + } + + node = heap_pool_elem_alloc(&heap->pool); + + node->ptr = ptr; + node->value = value; + node->index = heap->size; + + heap->tree[node->index] = node; + + heap->size++; + + heap_up(heap, node->index); + + return node; +} + +bool HEAP_is_empty(Heap *heap) +{ + return (heap->size == 0); +} + +unsigned int HEAP_size(Heap *heap) +{ + return heap->size; +} + +HeapNode *HEAP_top(Heap *heap) +{ + return heap->tree[0]; +} + +double HEAP_top_value(Heap *heap) +{ + return heap->tree[0]->value; +} + +void *HEAP_popmin(Heap *heap) +{ + void *ptr = heap->tree[0]->ptr; + + assert(heap->size != 0); + + heap_pool_elem_free(&heap->pool, heap->tree[0]); + + if (--heap->size) { + heap_swap(heap, 0, heap->size); + heap_down(heap, 0); + } + + return ptr; +} + +void HEAP_remove(Heap *heap, HeapNode *node) +{ + unsigned int i = node->index; + + assert(heap->size != 0); + + while (i > 0) { + unsigned int p = HEAP_PARENT(i); + + heap_swap(heap, p, i); + i = p; + } + + HEAP_popmin(heap); +} + +double HEAP_node_value(HeapNode *node) +{ + return node->value; +} + +void *HEAP_node_ptr(HeapNode *node) +{ + return node->ptr; +} diff --git a/extern/curve_fit_nd/intern/generic_heap.h b/extern/curve_fit_nd/intern/generic_heap.h new file mode 100644 index 00000000000..74327c761e4 --- /dev/null +++ b/extern/curve_fit_nd/intern/generic_heap.h @@ -0,0 +1,54 @@ +/* + * Copyright (c) 2016, Blender Foundation. + * + * 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 __GENERIC_HEAP_IMPL_H__ +#define __GENERIC_HEAP_IMPL_H__ + +/** \file generic_heap.h + * \ingroup curve_fit + */ + +struct Heap; +struct HeapNode; +typedef struct Heap Heap; +typedef struct HeapNode HeapNode; + +typedef void (*HeapFreeFP)(void *ptr); + +Heap *HEAP_new(unsigned int tot_reserve); +bool HEAP_is_empty(Heap *heap); +void HEAP_free(Heap *heap, HeapFreeFP ptrfreefp); +void *HEAP_node_ptr(HeapNode *node); +void HEAP_remove(Heap *heap, HeapNode *node); +HeapNode *HEAP_insert(Heap *heap, double value, void *ptr); +void *HEAP_popmin(Heap *heap); +void HEAP_clear(Heap *heap, HeapFreeFP ptrfreefp); +unsigned int HEAP_size(Heap *heap); +HeapNode *HEAP_top(Heap *heap); +double HEAP_top_value(Heap *heap); +double HEAP_node_value(HeapNode *node); + +#endif /* __GENERIC_HEAP_IMPL_H__ */ |