extern/curve_fit_nd/intern/curve_fit_cubic_refit.c

   1 /*
   2  * Copyright (c) 2016, Campbell Barton.
   3  *
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions are met:
   6  *     * Redistributions of source code must retain the above copyright
   7  *       notice, this list of conditions and the following disclaimer.
   8  *     * Redistributions in binary form must reproduce the above copyright
   9  *       notice, this list of conditions and the following disclaimer in the
  10  *       documentation and/or other materials provided with the distribution.
  11  *     * Neither the name of the <organization> nor the
  12  *       names of its contributors may be used to endorse or promote products
  13  *       derived from this software without specific prior written permission.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  17  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  18  * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
  19  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  20  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  21  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  22  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  23  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  24  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  25  */
  26
  27 /**
  28  * Curve Re-fitting Method
  29  * =======================
  30  *
  31  * This is a more processor intensive method of fitting,
  32  * compared to #curve_fit_cubic_to_points_db, and works as follows:
  33  *
  34  * - First iteratively remove all points under the error threshold.
  35  * - If corner calculation is enabled:
  36  *   - Find adjacent knots that exceed the angle limit
  37  *   - Find a 'split' point between the knots (could include the knots)
  38  *   - If copying the tangents to this split point doesn't exceed the error threshold:
  39  *     - Assign the tangents of the two knots to the split point, define it as a corner.
  40  *       (after this, we have many points which are too close).
  41  * - Run a re-fit pass, where knots are re-positioned between their adjacent knots
  42  *   when their re-fit position has a lower 'error'.
  43  *   While re-fitting, remove knots that fall below the error threshold.
  44  */
  45
  46 #ifdef _MSC_VER
  47 #  define _USE_MATH_DEFINES
  48 #endif
  49
  50 #include <math.h>
  51 #include <float.h>
  52 #include <stdbool.h>
  53 #include <assert.h>
  54
  55 #include <string.h>
  56 #include <stdlib.h>
  57
  58 typedef unsigned int uint;
  59
  60 #include "curve_fit_inline.h"
  61 #include "../curve_fit_nd.h"
  62
  63 #include "generic_heap.h"
  64
  65 #ifdef _MSC_VER
  66 #  define alloca(size) _alloca(size)
  67 #endif
  68
  69 #if !defined(_MSC_VER)
  70 #  define USE_VLA
  71 #endif
  72
  73 #ifdef USE_VLA
  74 #  ifdef __GNUC__
  75 #    pragma GCC diagnostic ignored "-Wvla"
  76 #  endif
  77 #else
  78 #  ifdef __GNUC__
  79 #    pragma GCC diagnostic error "-Wvla"
  80 #  endif
  81 #endif
  82
  83 /* adjust the knots after simplifying */
  84 #define USE_KNOT_REFIT
  85 /* remove knots under the error threshold while re-fitting */
  86 #define USE_KNOT_REFIT_REMOVE
  87 /* detect corners over an angle threshold */
  88 #define USE_CORNER_DETECT
  89 /* avoid re-calculating lengths multiple times */
  90 #define USE_LENGTH_CACHE
  91 /* use pool allocator */
  92 #define USE_TPOOL
  93
  94
  95 #define SPLIT_POINT_INVALID ((uint)-1)
  96
  97 #define MAX2(x, y) ((x) > (y) ? (x) : (y))
  98
  99 #define SQUARE(a) ((a) * (a))
 100
 101 #ifdef __GNUC__
 102 #  define UNLIKELY(x)     __builtin_expect(!!(x), 0)
 103 #else
 104 #  define UNLIKELY(x)     (x)
 105 #endif
 106
 107 struct PointData {
 108         const double *points;
 109         uint          points_len;
 110 #ifdef USE_LENGTH_CACHE
 111         const double *points_length_cache;
 112 #endif
 113 };
 114
 115 struct Knot {
 116         struct Knot *next, *prev;
 117
 118         HeapNode *heap_node;
 119
 120         /**
 121          * Currently the same, access as different for now
 122          * since we may want to support different point/knot indices
 123          */
 124         uint index;
 125
 126         uint can_remove : 1;
 127         uint is_removed : 1;
 128         uint is_corner  : 1;
 129
 130         double handles[2];
 131         /**
 132          * Store the error value, to see if we can improve on it
 133          * (without having to re-calculate each time)
 134          *
 135          * This is the error between this knot and the next */
 136         double error_sq_next;
 137
 138         /* Initially point to contiguous memory, however we may re-assign */
 139         double *tan[2];
 140 } Knot;
 141
 142
 143 struct KnotRemoveState {
 144         uint index;
 145         /* Handles for prev/next knots */
 146         double handles[2];
 147 };
 148
 149 #ifdef USE_TPOOL
 150 /* rstate_* pool allocator */
 151 #define TPOOL_IMPL_PREFIX  rstate
 152 #define TPOOL_ALLOC_TYPE   struct KnotRemoveState
 153 #define TPOOL_STRUCT       ElemPool_KnotRemoveState
 154 #include "generic_alloc_impl.h"
 155 #undef TPOOL_IMPL_PREFIX
 156 #undef TPOOL_ALLOC_TYPE
 157 #undef TPOOL_STRUCT
 158 #endif  /* USE_TPOOL */
 159
 160 #ifdef USE_KNOT_REFIT
 161 struct KnotRefitState {
 162         uint index;
 163         /** When SPLIT_POINT_INVALID - remove this item */
 164         uint index_refit;
 165         /** Handles for prev/next knots */
 166         double handles_prev[2], handles_next[2];
 167         double error_sq[2];
 168 };
 169
 170 #ifdef USE_TPOOL
 171 /* refit_* pool allocator */
 172 #define TPOOL_IMPL_PREFIX  refit
 173 #define TPOOL_ALLOC_TYPE   struct KnotRefitState
 174 #define TPOOL_STRUCT       ElemPool_KnotRefitState
 175 #include "generic_alloc_impl.h"
 176 #undef TPOOL_IMPL_PREFIX
 177 #undef TPOOL_ALLOC_TYPE
 178 #undef TPOOL_STRUCT
 179 #endif  /* USE_TPOOL */
 180 #endif  /* USE_KNOT_REFIT */
 181
 182
 183 #ifdef USE_CORNER_DETECT
 184 /** Result of collapsing a corner. */
 185 struct KnotCornerState {
 186         uint index;
 187         /* Merge adjacent handles into this one (may be shared with the 'index') */
 188         uint index_adjacent[2];
 189
 190         /* Handles for prev/next knots */
 191         double handles_prev[2], handles_next[2];
 192         double error_sq[2];
 193 };
 194
 195 /* refit_* pool allocator */
 196 #ifdef USE_TPOOL
 197 #define TPOOL_IMPL_PREFIX  corner
 198 #define TPOOL_ALLOC_TYPE   struct KnotCornerState
 199 #define TPOOL_STRUCT       ElemPool_KnotCornerState
 200 #include "generic_alloc_impl.h"
 201 #undef TPOOL_IMPL_PREFIX
 202 #undef TPOOL_ALLOC_TYPE
 203 #undef TPOOL_STRUCT
 204 #endif  /* USE_TPOOL */
 205 #endif  /* USE_CORNER_DETECT */
 206
 207
 208 /* Utility functions */
 209
 210 #ifdef USE_KNOT_REFIT
 211 /**
 212  * Find the most distant point between the 2 knots.
 213  */
 214 static uint knot_find_split_point(
 215         const struct PointData *pd,
 216         const struct Knot *knot_l, const struct Knot *knot_r,
 217         const uint knots_len,
 218         const uint dims)
 219 {
 220         uint split_point = SPLIT_POINT_INVALID;
 221         double split_point_dist_best = -DBL_MAX;
 222
 223         const double *offset = &pd->points[knot_l->index * dims];
 224
 225 #ifdef USE_VLA
 226         double v_plane[dims];
 227         double v_proj[dims];
 228         double v_offset[dims];
 229 #else
 230         double *v_plane =   alloca(sizeof(double) * dims);
 231         double *v_proj =    alloca(sizeof(double) * dims);
 232         double *v_offset =  alloca(sizeof(double) * dims);
 233 #endif
 234
 235         sub_vn_vnvn(
 236                 v_plane,
 237                 &pd->points[knot_l->index * dims],
 238                 &pd->points[knot_r->index * dims],
 239                 dims);
 240
 241         normalize_vn(v_plane, dims);
 242
 243         const uint knots_end = knots_len - 1;
 244         const struct Knot *k_step = knot_l;
 245         do {
 246                 if (k_step->index != knots_end) {
 247                         k_step += 1;
 248                 }
 249                 else {
 250                         /* wrap around */
 251                         k_step = k_step - knots_end;
 252                 }
 253
 254                 if (k_step != knot_r) {
 255                         sub_vn_vnvn(v_offset, &pd->points[k_step->index * dims], offset, dims);
 256                         project_plane_vn_vnvn_normalized(v_proj, v_offset, v_plane, dims);
 257
 258                         double split_point_dist_test = len_squared_vn(v_proj, dims);
 259                         if (split_point_dist_test > split_point_dist_best) {
 260                                 split_point_dist_best = split_point_dist_test;
 261                                 split_point = k_step->index;
 262                         }
 263                 }
 264                 else {
 265                         break;
 266                 }
 267
 268         } while (true);
 269
 270         return split_point;
 271 }
 272 #endif  /* USE_KNOT_REFIT */
 273
 274
 275 #ifdef USE_CORNER_DETECT
 276 /**
 277  * Find the knot furthest from the line between \a knot_l & \a knot_r.
 278  * This is to be used as a split point.
 279  */
 280 static uint knot_find_split_point_on_axis(
 281         const struct PointData *pd,
 282         const struct Knot *knot_l, const struct Knot *knot_r,
 283         const uint knots_len,
 284         const double *plane_no,
 285         const uint dims)
 286 {
 287         uint split_point = SPLIT_POINT_INVALID;
 288         double split_point_dist_best = -DBL_MAX;
 289
 290         const uint knots_end = knots_len - 1;
 291         const struct Knot *k_step = knot_l;
 292         do {
 293                 if (k_step->index != knots_end) {
 294                         k_step += 1;
 295                 }
 296                 else {
 297                         /* wrap around */
 298                         k_step = k_step - knots_end;
 299                 }
 300
 301                 if (k_step != knot_r) {
 302                         double split_point_dist_test = dot_vnvn(plane_no, &pd->points[k_step->index * dims], dims);
 303                         if (split_point_dist_test > split_point_dist_best) {
 304                                 split_point_dist_best = split_point_dist_test;
 305                                 split_point = k_step->index;
 306                         }
 307                 }
 308                 else {
 309                         break;
 310                 }
 311
 312         } while (true);
 313
 314         return split_point;
 315 }
 316 #endif  /* USE_CORNER_DETECT */
 317
 318
 319 static double knot_remove_error_value(
 320         const double *tan_l, const double *tan_r,
 321         const double *points_offset, const uint points_offset_len,
 322         const double *points_offset_length_cache,
 323         const uint dims,
 324         /* Avoid having to re-calculate again */
 325         double r_handle_factors[2])
 326 {
 327         double error_sq = FLT_MAX;
 328
 329 #ifdef USE_VLA
 330         double handle_factor_l[dims];
 331         double handle_factor_r[dims];
 332 #else
 333         double *handle_factor_l = alloca(sizeof(double) * dims);
 334         double *handle_factor_r = alloca(sizeof(double) * dims);
 335 #endif
 336
 337         curve_fit_cubic_to_points_single_db(
 338                 points_offset, points_offset_len, points_offset_length_cache, dims, 0.0,
 339                 tan_l, tan_r,
 340                 handle_factor_l, handle_factor_r,
 341                 &error_sq);
 342
 343         assert(error_sq != FLT_MAX);
 344
 345         isub_vnvn(handle_factor_l, points_offset, dims);
 346         r_handle_factors[0] = dot_vnvn(tan_l, handle_factor_l, dims);
 347
 348         isub_vnvn(handle_factor_r, &points_offset[(points_offset_len - 1) * dims], dims);
 349         r_handle_factors[1] = dot_vnvn(tan_r, handle_factor_r, dims);
 350
 351         return error_sq;
 352 }
 353
 354 static double knot_calc_curve_error_value(
 355         const struct PointData *pd,
 356         const struct Knot *knot_l, const struct Knot *knot_r,
 357         const double *tan_l, const double *tan_r,
 358         const uint dims,
 359         double r_handle_factors[2])
 360 {
 361         const uint points_offset_len = ((knot_l->index < knot_r->index) ?
 362                 (knot_r->index - knot_l->index) :
 363                 ((knot_r->index + pd->points_len) - knot_l->index)) + 1;
 364
 365         if (points_offset_len != 2) {
 366                 return knot_remove_error_value(
 367                         tan_l, tan_r,
 368                         &pd->points[knot_l->index * dims], points_offset_len,
 369 #ifdef USE_LENGTH_CACHE
 370                         &pd->points_length_cache[knot_l->index],
 371 #else
 372                         NULL,
 373 #endif
 374                         dims,
 375                         r_handle_factors);
 376         }
 377         else {
 378                 /* No points between, use 1/3 handle length with no error as a fallback. */
 379                 assert(points_offset_len == 2);
 380 #ifdef USE_LENGTH_CACHE
 381                 r_handle_factors[0] = r_handle_factors[1] = pd->points_length_cache[knot_l->index] / 3.0;
 382 #else
 383                 r_handle_factors[0] = r_handle_factors[1] = len_vnvn(
 384                         &pd->points[(knot_l->index + 0) * dims],
 385                         &pd->points[(knot_l->index + 1) * dims], dims) / 3.0;
 386 #endif
 387                 return 0.0;
 388         }
 389 }
 390
 391 struct KnotRemove_Params {
 392         Heap *heap;
 393         const struct PointData *pd;
 394 #ifdef USE_TPOOL
 395         struct ElemPool_KnotRemoveState *epool;
 396 #endif
 397 };
 398
 399 static void knot_remove_error_recalculate(
 400         struct KnotRemove_Params *p,
 401         struct Knot *k, const double error_sq_max,
 402         const uint dims)
 403 {
 404         assert(k->can_remove);
 405         double handles[2];
 406
 407         const double cost_sq = knot_calc_curve_error_value(
 408                 p->pd, k->prev, k->next,
 409                 k->prev->tan[1], k->next->tan[0],
 410                 dims,
 411                 handles);
 412
 413         if (cost_sq < error_sq_max) {
 414                 struct KnotRemoveState *r;
 415                 if (k->heap_node) {
 416                         r = HEAP_node_ptr(k->heap_node);
 417                         HEAP_remove(p->heap, k->heap_node);
 418                 }
 419                 else {
 420 #ifdef USE_TPOOL
 421                         r = rstate_pool_elem_alloc(p->epool);
 422 #else
 423                         r = malloc(sizeof(*r));
 424 #endif
 425
 426                         r->index = k->index;
 427                 }
 428
 429                 r->handles[0] = handles[0];
 430                 r->handles[1] = handles[1];
 431
 432                 k->heap_node = HEAP_insert(p->heap, cost_sq, r);
 433         }
 434         else {
 435                 if (k->heap_node) {
 436                         struct KnotRemoveState *r;
 437                         r = HEAP_node_ptr(k->heap_node);
 438                         HEAP_remove(p->heap, k->heap_node);
 439
 440 #ifdef USE_TPOOL
 441                         rstate_pool_elem_free(p->epool, r);
 442 #else
 443                         free(r);
 444 #endif
 445
 446                         k->heap_node = NULL;
 447                 }
 448         }
 449 }
 450
 451 /**
 452  * Return length after being reduced.
 453  */
 454 static uint curve_incremental_simplify(
 455         const struct PointData *pd,
 456         struct Knot *knots, const uint knots_len, uint knots_len_remaining,
 457         double error_sq_max, const uint dims)
 458 {
 459
 460 #ifdef USE_TPOOL
 461         struct ElemPool_KnotRemoveState epool;
 462
 463         rstate_pool_create(&epool, 0);
 464 #endif
 465
 466         Heap *heap = HEAP_new(knots_len);
 467
 468         struct KnotRemove_Params params = {
 469             .pd = pd,
 470             .heap = heap,
 471 #ifdef USE_TPOOL
 472             .epool = &epool,
 473 #endif
 474         };
 475
 476         for (uint i = 0; i < knots_len; i++) {
 477                 struct Knot *k = &knots[i];
 478                 if (k->can_remove && (k->is_removed == false) && (k->is_corner == false)) {
 479                         knot_remove_error_recalculate(&params, k, error_sq_max, dims);
 480                 }
 481         }
 482
 483         while (HEAP_is_empty(heap) == false) {
 484                 struct Knot *k;
 485
 486                 {
 487                         const double error_sq = HEAP_top_value(heap);
 488                         struct KnotRemoveState *r = HEAP_popmin(heap);
 489                         k = &knots[r->index];
 490                         k->heap_node = NULL;
 491                         k->prev->handles[1] = r->handles[0];
 492                         k->next->handles[0] = r->handles[1];
 493
 494                         k->prev->error_sq_next = error_sq;
 495
 496 #ifdef USE_TPOOL
 497                         rstate_pool_elem_free(&epool, r);
 498 #else
 499                         free(r);
 500 #endif
 501                 }
 502
 503                 if (UNLIKELY(knots_len_remaining <= 2)) {
 504                         continue;
 505                 }
 506
 507                 struct Knot *k_prev = k->prev;
 508                 struct Knot *k_next = k->next;
 509
 510                 /* Remove ourselves */
 511                 k_next->prev = k_prev;
 512                 k_prev->next = k_next;
 513
 514                 k->next = NULL;
 515                 k->prev = NULL;
 516                 k->is_removed = true;
 517
 518                 if (k_prev->can_remove && (k_prev->is_corner == false) && (k_prev->prev && k_prev->next)) {
 519                         knot_remove_error_recalculate(&params, k_prev, error_sq_max, dims);
 520                 }
 521
 522                 if (k_next->can_remove && (k_next->is_corner == false) && (k_next->prev && k_next->next)) {
 523                         knot_remove_error_recalculate(&params, k_next, error_sq_max, dims);
 524                 }
 525
 526                 knots_len_remaining -= 1;
 527         }
 528
 529 #ifdef USE_TPOOL
 530         rstate_pool_destroy(&epool);
 531 #endif
 532
 533         HEAP_free(heap, free);
 534
 535         return knots_len_remaining;
 536 }
 537
 538
 539 #ifdef USE_KNOT_REFIT
 540
 541 struct KnotRefit_Params {
 542         Heap *heap;
 543         const struct PointData *pd;
 544 #ifdef USE_TPOOL
 545         struct ElemPool_KnotRefitState *epool;
 546 #endif
 547 };
 548
 549 static void knot_refit_error_recalculate(
 550         struct KnotRefit_Params *p,
 551         struct Knot *knots, const uint knots_len,
 552         struct Knot *k,
 553         const double error_sq_max,
 554         const uint dims)
 555 {
 556         assert(k->can_remove);
 557
 558 #ifdef USE_KNOT_REFIT_REMOVE
 559         {
 560                 double handles[2];
 561
 562                 /* First check if we can remove, this allows to refit and remove as we go. */
 563                 const double cost_sq = knot_calc_curve_error_value(
 564                         p->pd, k->prev, k->next,
 565                         k->prev->tan[1], k->next->tan[0],
 566                         dims,
 567                         handles);
 568
 569                 if (cost_sq < error_sq_max) {
 570                         struct KnotRefitState *r;
 571                         if (k->heap_node) {
 572                                 r = HEAP_node_ptr(k->heap_node);
 573                                 HEAP_remove(p->heap, k->heap_node);
 574                         }
 575                         else {
 576 #ifdef USE_TPOOL
 577                                 r = refit_pool_elem_alloc(p->epool);
 578 #else
 579                                 r = malloc(sizeof(*r));
 580 #endif
 581                                 r->index = k->index;
 582                         }
 583
 584                         r->index_refit = SPLIT_POINT_INVALID;
 585
 586                         r->handles_prev[0] = handles[0];
 587                         r->handles_prev[1] = 0.0;  /* unused */
 588                         r->handles_next[0] = 0.0;  /* unused */
 589                         r->handles_next[1] = handles[1];
 590
 591                         r->error_sq[0] = r->error_sq[1] = cost_sq;
 592
 593                         /* Always perform removal before refitting, (make a negative number) */
 594                         k->heap_node = HEAP_insert(p->heap, cost_sq - error_sq_max, r);
 595
 596                         return;
 597                 }
 598         }
 599 #else
 600         (void)error_sq_max;
 601 #endif  /* USE_KNOT_REFIT_REMOVE */
 602
 603         const uint refit_index = knot_find_split_point(
 604                  p->pd, k->prev, k->next,
 605                  knots_len,
 606                  dims);
 607
 608         if ((refit_index == SPLIT_POINT_INVALID) ||
 609             (refit_index == k->index))
 610         {
 611                 goto remove;
 612         }
 613
 614         struct Knot *k_refit = &knots[refit_index];
 615
 616         const double cost_sq_src_max = MAX2(k->prev->error_sq_next, k->error_sq_next);
 617         assert(cost_sq_src_max <= error_sq_max);
 618
 619         double cost_sq_dst[2];
 620         double handles_prev[2], handles_next[2];
 621
 622         if ((((cost_sq_dst[0] = knot_calc_curve_error_value(
 623                    p->pd, k->prev, k_refit,
 624                    k->prev->tan[1], k_refit->tan[0],
 625                    dims,
 626                    handles_prev)) < cost_sq_src_max) &&
 627              ((cost_sq_dst[1] = knot_calc_curve_error_value(
 628                    p->pd, k_refit, k->next,
 629                    k_refit->tan[1], k->next->tan[0],
 630                    dims,
 631                    handles_next)) < cost_sq_src_max)))
 632         {
 633                 {
 634                         struct KnotRefitState *r;
 635                         if (k->heap_node) {
 636                                 r = HEAP_node_ptr(k->heap_node);
 637                                 HEAP_remove(p->heap, k->heap_node);
 638                         }
 639                         else {
 640 #ifdef USE_TPOOL
 641                                 r = refit_pool_elem_alloc(p->epool);
 642 #else
 643                                 r = malloc(sizeof(*r));
 644 #endif
 645                                 r->index = k->index;
 646                         }
 647
 648                         r->index_refit = refit_index;
 649
 650                         r->handles_prev[0] = handles_prev[0];
 651                         r->handles_prev[1] = handles_prev[1];
 652
 653                         r->handles_next[0] = handles_next[0];
 654                         r->handles_next[1] = handles_next[1];
 655
 656                         r->error_sq[0] = cost_sq_dst[0];
 657                         r->error_sq[1] = cost_sq_dst[1];
 658
 659                         const double cost_sq_dst_max = MAX2(cost_sq_dst[0], cost_sq_dst[1]);
 660
 661                         assert(cost_sq_dst_max < cost_sq_src_max);
 662
 663                         /* Weight for the greatest improvement */
 664                         k->heap_node = HEAP_insert(p->heap, cost_sq_src_max - cost_sq_dst_max, r);
 665                 }
 666         }
 667         else {
 668 remove:
 669                 if (k->heap_node) {
 670                         struct KnotRefitState *r;
 671                         r = HEAP_node_ptr(k->heap_node);
 672                         HEAP_remove(p->heap, k->heap_node);
 673
 674 #ifdef USE_TPOOL
 675                         refit_pool_elem_free(p->epool, r);
 676 #else
 677                         free(r);
 678 #endif
 679
 680                         k->heap_node = NULL;
 681                 }
 682         }
 683 }
 684
 685 /**
 686  * Re-adjust the curves by re-fitting points.
 687  * test the error from moving using points between the adjacent.
 688  */
 689 static uint curve_incremental_simplify_refit(
 690         const struct PointData *pd,
 691         struct Knot *knots, const uint knots_len, uint knots_len_remaining,
 692         const double error_sq_max,
 693         const uint dims)
 694 {
 695 #ifdef USE_TPOOL
 696         struct ElemPool_KnotRefitState epool;
 697
 698         refit_pool_create(&epool, 0);
 699 #endif
 700
 701         Heap *heap = HEAP_new(knots_len);
 702
 703         struct KnotRefit_Params params = {
 704             .pd = pd,
 705             .heap = heap,
 706 #ifdef USE_TPOOL
 707             .epool = &epool,
 708 #endif
 709         };
 710
 711         for (uint i = 0; i < knots_len; i++) {
 712                 struct Knot *k = &knots[i];
 713                 if (k->can_remove &&
 714                     (k->is_removed == false) &&
 715                     (k->is_corner == false) &&
 716                     (k->prev && k->next))
 717                 {
 718                         knot_refit_error_recalculate(&params, knots, knots_len, k, error_sq_max, dims);
 719                 }
 720         }
 721
 722         while (HEAP_is_empty(heap) == false) {
 723                 struct Knot *k_old, *k_refit;
 724
 725                 {
 726                         struct KnotRefitState *r = HEAP_popmin(heap);
 727                         k_old = &knots[r->index];
 728                         k_old->heap_node = NULL;
 729
 730 #ifdef USE_KNOT_REFIT_REMOVE
 731                         if (r->index_refit == SPLIT_POINT_INVALID) {
 732                                 k_refit = NULL;
 733                         }
 734                         else
 735 #endif
 736                         {
 737                                 k_refit = &knots[r->index_refit];
 738                                 k_refit->handles[0] = r->handles_prev[1];
 739                                 k_refit->handles[1] = r->handles_next[0];
 740                         }
 741
 742                         k_old->prev->handles[1] = r->handles_prev[0];
 743                         k_old->next->handles[0] = r->handles_next[1];
 744
 745 #ifdef USE_TPOOL
 746                         refit_pool_elem_free(&epool, r);
 747 #else
 748                         free(r);
 749 #endif
 750                 }
 751
 752                 if (UNLIKELY(knots_len_remaining <= 2)) {
 753                         continue;
 754                 }
 755
 756                 struct Knot *k_prev = k_old->prev;
 757                 struct Knot *k_next = k_old->next;
 758
 759                 k_old->next = NULL;
 760                 k_old->prev = NULL;
 761                 k_old->is_removed = true;
 762
 763 #ifdef USE_KNOT_REFIT_REMOVE
 764                 if (k_refit == NULL) {
 765                         k_next->prev = k_prev;
 766                         k_prev->next = k_next;
 767
 768                         knots_len_remaining -= 1;
 769                 }
 770                 else
 771 #endif
 772                 {
 773                         /* Remove ourselves */
 774                         k_next->prev = k_refit;
 775                         k_prev->next = k_refit;
 776
 777                         k_refit->prev = k_prev;
 778                         k_refit->next = k_next;
 779                         k_refit->is_removed = false;
 780                 }
 781
 782                 if (k_prev->can_remove && (k_prev->is_corner == false) && (k_prev->prev && k_prev->next)) {
 783                         knot_refit_error_recalculate(&params, knots, knots_len, k_prev, error_sq_max, dims);
 784                 }
 785
 786                 if (k_next->can_remove && (k_next->is_corner == false) && (k_next->prev && k_next->next)) {
 787                         knot_refit_error_recalculate(&params, knots, knots_len, k_next, error_sq_max, dims);
 788                 }
 789         }
 790
 791 #ifdef USE_TPOOL
 792         refit_pool_destroy(&epool);
 793 #endif
 794
 795         HEAP_free(heap, free);
 796
 797         return knots_len_remaining;
 798 }
 799
 800 #endif  /* USE_KNOT_REFIT */
 801
 802
 803 #ifdef USE_CORNER_DETECT
 804
 805 struct KnotCorner_Params {
 806         Heap *heap;
 807         const struct PointData *pd;
 808 #ifdef USE_TPOOL
 809         struct ElemPool_KnotCornerState *epool;
 810 #endif
 811 };
 812
 813 /**
 814  * (Re)calculate the error incurred from turning this into a corner.
 815  */
 816 static void knot_corner_error_recalculate(
 817         struct KnotCorner_Params *p,
 818         struct Knot *k_split,
 819         struct Knot *k_prev, struct Knot *k_next,
 820         const double error_sq_max,
 821         const uint dims)
 822 {
 823         assert(k_prev->can_remove && k_next->can_remove);
 824
 825         double handles_prev[2], handles_next[2];
 826         /* Test skipping 'k_prev' by using points (k_prev->prev to k_split) */
 827         double cost_sq_dst[2];
 828
 829         if (((cost_sq_dst[0] = knot_calc_curve_error_value(
 830                   p->pd, k_prev, k_split,
 831                   k_prev->tan[1], k_prev->tan[1],
 832                   dims,
 833                   handles_prev)) < error_sq_max) &&
 834             ((cost_sq_dst[1] = knot_calc_curve_error_value(
 835                   p->pd, k_split, k_next,
 836                   k_next->tan[0], k_next->tan[0],
 837                   dims,
 838                   handles_next)) < error_sq_max))
 839         {
 840                 struct KnotCornerState *c;
 841                 if (k_split->heap_node) {
 842                         c = HEAP_node_ptr(k_split->heap_node);
 843                         HEAP_remove(p->heap, k_split->heap_node);
 844                 }
 845                 else {
 846 #ifdef USE_TPOOL
 847                         c = corner_pool_elem_alloc(p->epool);
 848 #else
 849                         c = malloc(sizeof(*c));
 850 #endif
 851                         c->index = k_split->index;
 852                 }
 853
 854                 c->index_adjacent[0] = k_prev->index;
 855                 c->index_adjacent[1] = k_next->index;
 856
 857                 /* Need to store handle lengths for both sides */
 858                 c->handles_prev[0] = handles_prev[0];
 859                 c->handles_prev[1] = handles_prev[1];
 860
 861                 c->handles_next[0] = handles_next[0];
 862                 c->handles_next[1] = handles_next[1];
 863
 864                 c->error_sq[0] = cost_sq_dst[0];
 865                 c->error_sq[1] = cost_sq_dst[1];
 866
 867                 const double cost_max_sq = MAX2(cost_sq_dst[0], cost_sq_dst[1]);
 868                 k_split->heap_node = HEAP_insert(p->heap, cost_max_sq, c);
 869         }
 870         else {
 871                 if (k_split->heap_node) {
 872                         struct KnotCornerState *c;
 873                         c = HEAP_node_ptr(k_split->heap_node);
 874                         HEAP_remove(p->heap, k_split->heap_node);
 875 #ifdef USE_TPOOL
 876                         corner_pool_elem_free(p->epool, c);
 877 #else
 878                         free(c);
 879 #endif
 880                         k_split->heap_node = NULL;
 881                 }
 882         }
 883 }
 884
 885
 886 /**
 887  * Attempt to collapse close knots into corners,
 888  * as long as they fall below the error threshold.
 889  */
 890 static uint curve_incremental_simplify_corners(
 891         const struct PointData *pd,
 892         struct Knot *knots, const uint knots_len, uint knots_len_remaining,
 893         const double error_sq_max, const double error_sq_2x_max,
 894         const double corner_angle,
 895         const uint dims,
 896         uint *r_corner_index_len)
 897 {
 898 #ifdef USE_TPOOL
 899         struct ElemPool_KnotCornerState epool;
 900
 901         corner_pool_create(&epool, 0);
 902 #endif
 903
 904         Heap *heap = HEAP_new(0);
 905
 906         struct KnotCorner_Params params = {
 907             .pd = pd,
 908             .heap = heap,
 909 #ifdef USE_TPOOL
 910             .epool = &epool,
 911 #endif
 912         };
 913
 914 #ifdef USE_VLA
 915         double plane_no[dims];
 916         double k_proj_ref[dims];
 917         double k_proj_split[dims];
 918 #else
 919         double *plane_no =       alloca(sizeof(double) * dims);
 920         double *k_proj_ref =     alloca(sizeof(double) * dims);
 921         double *k_proj_split =   alloca(sizeof(double) * dims);
 922 #endif
 923
 924         const double corner_angle_cos = cos(corner_angle);
 925
 926         uint corner_index_len = 0;
 927
 928         for (uint i = 0; i < knots_len; i++) {
 929                 if ((knots[i].is_removed == false) &&
 930                     (knots[i].can_remove == true) &&
 931                     (knots[i].next && knots[i].next->can_remove))
 932                 {
 933                         struct Knot *k_prev = &knots[i];
 934                         struct Knot *k_next = k_prev->next;
 935
 936                         /* Angle outside threshold */
 937                         if (dot_vnvn(k_prev->tan[0], k_next->tan[1], dims) < corner_angle_cos) {
 938                                 /* Measure distance projected onto a plane,
 939                                  * since the points may be offset along their own tangents. */
 940                                 sub_vn_vnvn(plane_no, k_next->tan[0], k_prev->tan[1], dims);
 941
 942                                 /* Compare 2x so as to allow both to be changed by maximum of error_sq_max */
 943                                 const uint split_index = knot_find_split_point_on_axis(
 944                                         pd, k_prev, k_next,
 945                                         knots_len,
 946                                         plane_no,
 947                                         dims);
 948
 949                                 if (split_index != SPLIT_POINT_INVALID) {
 950
 951                                         project_vn_vnvn(k_proj_ref,   &pd->points[k_prev->index * dims], k_prev->tan[1], dims);
 952                                         project_vn_vnvn(k_proj_split, &pd->points[split_index   * dims], k_prev->tan[1], dims);
 953
 954                                         if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_2x_max) {
 955
 956                                                 project_vn_vnvn(k_proj_ref,   &pd->points[k_next->index * dims], k_next->tan[0], dims);
 957                                                 project_vn_vnvn(k_proj_split, &pd->points[split_index   * dims], k_next->tan[0], dims);
 958
 959                                                 if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_2x_max) {
 960
 961                                                         struct Knot *k_split = &knots[split_index];
 962
 963                                                         knot_corner_error_recalculate(
 964                                                                 &params,
 965                                                                 k_split, k_prev, k_next,
 966                                                                 error_sq_max,
 967                                                                 dims);
 968                                                 }
 969                                         }
 970                                 }
 971                         }
 972                 }
 973         }
 974
 975         while (HEAP_is_empty(heap) == false) {
 976                 struct KnotCornerState *c = HEAP_popmin(heap);
 977
 978                 struct Knot *k_split = &knots[c->index];
 979
 980                 /* Remove while collapsing */
 981                 struct Knot *k_prev  = &knots[c->index_adjacent[0]];
 982                 struct Knot *k_next  = &knots[c->index_adjacent[1]];
 983
 984                 /* Insert */
 985                 k_split->is_removed = false;
 986                 k_split->prev = k_prev;
 987                 k_split->next = k_next;
 988                 k_prev->next = k_split;
 989                 k_next->prev = k_split;
 990
 991                 /* Update tangents */
 992                 k_split->tan[0] = k_prev->tan[1];
 993                 k_split->tan[1] = k_next->tan[0];
 994
 995                 /* Own handles */
 996                 k_prev->handles[1]  = c->handles_prev[0];
 997                 k_split->handles[0] = c->handles_prev[1];
 998                 k_split->handles[1] = c->handles_next[0];
 999                 k_next->handles[0]  = c->handles_next[1];
1000
1001                 k_prev->error_sq_next  = c->error_sq[0];
1002                 k_split->error_sq_next = c->error_sq[1];
1003
1004                 k_split->heap_node = NULL;
1005
1006 #ifdef USE_TPOOL
1007                 corner_pool_elem_free(&epool, c);
1008 #else
1009                 free(c);
1010 #endif
1011
1012                 k_split->is_corner = true;
1013
1014                 knots_len_remaining++;
1015
1016                 corner_index_len++;
1017         }
1018
1019 #ifdef USE_TPOOL
1020         corner_pool_destroy(&epool);
1021 #endif
1022
1023         HEAP_free(heap, free);
1024
1025         *r_corner_index_len = corner_index_len;
1026
1027         return knots_len_remaining;
1028 }
1029
1030 #endif  /* USE_CORNER_DETECT */
1031
1032 int curve_fit_cubic_to_points_refit_db(
1033         const double *points,
1034         const uint    points_len,
1035         const uint    dims,
1036         const double  error_threshold,
1037         const uint    calc_flag,
1038         const uint   *corners,
1039         const uint    corners_len,
1040         const double  corner_angle,
1041
1042         double **r_cubic_array, uint *r_cubic_array_len,
1043         uint   **r_cubic_orig_index,
1044         uint   **r_corner_index_array, uint *r_corner_index_len)
1045 {
1046         const uint knots_len = points_len;
1047         struct Knot *knots = malloc(sizeof(Knot) * knots_len);
1048         knots[0].next = NULL;
1049
1050 #ifndef USE_CORNER_DETECT
1051         (void)r_corner_index_array;
1052         (void)r_corner_index_len;
1053 #endif
1054
1055 (void)corners;
1056 (void)corners_len;
1057
1058         const bool is_cyclic = (calc_flag & CURVE_FIT_CALC_CYCLIC) != 0 && (points_len > 2);
1059 #ifdef USE_CORNER_DETECT
1060         const bool use_corner = (corner_angle < M_PI);
1061 #else
1062         (void)corner_angle;
1063 #endif
1064
1065         /* Over alloc the list x2 for cyclic curves,
1066          * so we can evaluate across the start/end */
1067         double *points_alloc = NULL;
1068         if (is_cyclic) {
1069                 points_alloc = malloc((sizeof(double) * points_len * dims) * 2);
1070                 memcpy(points_alloc,                       points,       sizeof(double) * points_len * dims);
1071                 memcpy(points_alloc + (points_len * dims), points_alloc, sizeof(double) * points_len * dims);
1072                 points = points_alloc;
1073         }
1074
1075         double *tangents = malloc(sizeof(double) * knots_len * 2 * dims);
1076
1077         {
1078                 double *t_step = tangents;
1079                 for (uint i = 0; i < knots_len; i++) {
1080                         knots[i].next = (knots + i) + 1;
1081                         knots[i].prev = (knots + i) - 1;
1082
1083                         knots[i].heap_node = NULL;
1084                         knots[i].index = i;
1085                         knots[i].index = i;
1086                         knots[i].can_remove = true;
1087                         knots[i].is_removed = false;
1088                         knots[i].is_corner = false;
1089                         knots[i].error_sq_next = 0.0;
1090                         knots[i].tan[0] = t_step; t_step += dims;
1091                         knots[i].tan[1] = t_step; t_step += dims;
1092                 }
1093                 assert(t_step == &tangents[knots_len * 2 * dims]);
1094         }
1095
1096         if (is_cyclic) {
1097                 knots[0].prev = &knots[knots_len - 1];
1098                 knots[knots_len - 1].next = &knots[0];
1099         }
1100         else {
1101                 knots[0].prev = NULL;
1102                 knots[knots_len - 1].next = NULL;
1103
1104                 /* always keep end-points */
1105                 knots[0].can_remove = false;
1106                 knots[knots_len - 1].can_remove = false;
1107         }
1108
1109 #ifdef USE_LENGTH_CACHE
1110         double *points_length_cache = malloc(sizeof(double) * points_len * (is_cyclic ? 2 : 1));
1111 #endif
1112
1113         /* Initialize tangents,
1114          * also set the values for knot handles since some may not collapse. */
1115         {
1116 #ifdef USE_VLA
1117                 double tan_prev[dims];
1118                 double tan_next[dims];
1119 #else
1120                 double *tan_prev = alloca(sizeof(double) * dims);
1121                 double *tan_next = alloca(sizeof(double) * dims);
1122 #endif
1123                 double len_prev, len_next;
1124
1125 #if 0
1126                 /* 2x normalize calculations, but correct */
1127
1128                 for (uint i = 0; i < knots_len; i++) {
1129                         Knot *k = &knots[i];
1130
1131                         if (k->prev) {
1132                                 sub_vn_vnvn(tan_prev, &points[k->prev->index * dims], &points[k->index * dims], dims);
1133 #ifdef USE_LENGTH_CACHE
1134                                 points_length_cache[i] =
1135 #endif
1136                                 len_prev = normalize_vn(tan_prev, dims);
1137                         }
1138                         else {
1139                                 zero_vn(tan_prev, dims);
1140                                 len_prev = 0.0;
1141                         }
1142
1143                         if (k->next) {
1144                                 sub_vn_vnvn(tan_next, &points[k->index * dims], &points[k->next->index * dims], dims);
1145                                 len_next = normalize_vn(tan_next, dims);
1146                         }
1147                         else {
1148                                 zero_vn(tan_next, dims);
1149                                 len_next = 0.0;
1150                         }
1151
1152                         add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
1153                         normalize_vn(k->tan[0], dims);
1154                         copy_vnvn(k->tan[1], k->tan[0], dims);
1155                         k->handles[0] = len_prev / 3;
1156                         k->handles[1] = len_next / 3;
1157                 }
1158 #else
1159                 if (knots_len < 2) {
1160                         /* NOP, set dummy values */
1161                         for (uint i = 0; i < knots_len; i++) {
1162                                 struct Knot *k = &knots[i];
1163                                 zero_vn(k->tan[0], dims);
1164                                 zero_vn(k->tan[1], dims);
1165                                 k->handles[0] = 0.0;
1166                                 k->handles[1] = 0.0;
1167 #ifdef USE_LENGTH_CACHE
1168                                 points_length_cache[i] = 0.0;
1169 #endif
1170                         }
1171                 }
1172                 else if (is_cyclic) {
1173                         len_prev = normalize_vn_vnvn(
1174                                 tan_prev, &points[(knots_len - 2) * dims], &points[(knots_len - 1) * dims], dims);
1175                         for (uint i_curr = knots_len - 1, i_next = 0; i_next < knots_len; i_curr = i_next++) {
1176                                 struct Knot *k = &knots[i_curr];
1177 #ifdef USE_LENGTH_CACHE
1178                                 points_length_cache[i_next] =
1179 #endif
1180                                 len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims);
1181
1182                                 add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
1183                                 normalize_vn(k->tan[0], dims);
1184                                 copy_vnvn(k->tan[1], k->tan[0], dims);
1185                                 k->handles[0] = len_prev / 3;
1186                                 k->handles[1] = len_next / 3;
1187
1188                                 copy_vnvn(tan_prev, tan_next, dims);
1189                                 len_prev = len_next;
1190                         }
1191                 }
1192                 else {
1193 #ifdef USE_LENGTH_CACHE
1194                         points_length_cache[0] = 0.0;
1195                         points_length_cache[1] =
1196 #endif
1197                         len_prev = normalize_vn_vnvn(
1198                                 tan_prev, &points[0 * dims], &points[1 * dims], dims);
1199                         copy_vnvn(knots[0].tan[0], tan_prev, dims);
1200                         copy_vnvn(knots[0].tan[1], tan_prev, dims);
1201                         knots[0].handles[0] = len_prev / 3;
1202                         knots[0].handles[1] = len_prev / 3;
1203
1204                         for (uint i_curr = 1, i_next = 2; i_next < knots_len; i_curr = i_next++) {
1205                                 struct Knot *k = &knots[i_curr];
1206
1207 #ifdef USE_LENGTH_CACHE
1208                                 points_length_cache[i_next] =
1209 #endif
1210                                 len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims);
1211
1212                                 add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
1213                                 normalize_vn(k->tan[0], dims);
1214                                 copy_vnvn(k->tan[1], k->tan[0], dims);
1215                                 k->handles[0] = len_prev / 3;
1216                                 k->handles[1] = len_next / 3;
1217
1218                                 copy_vnvn(tan_prev, tan_next, dims);
1219                                 len_prev = len_next;
1220                         }
1221                         copy_vnvn(knots[knots_len - 1].tan[0], tan_next, dims);
1222                         copy_vnvn(knots[knots_len - 1].tan[1], tan_next, dims);
1223
1224                         knots[knots_len - 1].handles[0] = len_next / 3;
1225                         knots[knots_len - 1].handles[1] = len_next / 3;
1226                 }
1227 #endif
1228         }
1229
1230 #ifdef USE_LENGTH_CACHE
1231         if (is_cyclic) {
1232                 memcpy(&points_length_cache[points_len], points_length_cache, sizeof(double) * points_len);
1233         }
1234 #endif
1235
1236
1237 #if 0
1238         for (uint i = 0; i < knots_len; i++) {
1239                 Knot *k = &knots[i];
1240                 printf("TAN %.8f %.8f %.8f %.8f\n", k->tan[0][0], k->tan[0][1], k->tan[1][0], k->tan[0][1]);
1241         }
1242 #endif
1243
1244         const struct PointData pd = {
1245                 .points = points,
1246                 .points_len = points_len,
1247 #ifdef USE_LENGTH_CACHE
1248                 .points_length_cache = points_length_cache,
1249 #endif
1250         };
1251
1252         uint knots_len_remaining = knots_len;
1253
1254         /* 'curve_incremental_simplify_refit' can be called here, but its very slow
1255          * just remove all within the threshold first. */
1256         knots_len_remaining = curve_incremental_simplify(
1257                 &pd, knots, knots_len, knots_len_remaining,
1258                 SQUARE(error_threshold), dims);
1259
1260 #ifdef USE_CORNER_DETECT
1261         if (use_corner) {
1262                 for (uint i = 0; i < knots_len; i++) {
1263                         assert(knots[i].heap_node == NULL);
1264                 }
1265
1266                 knots_len_remaining = curve_incremental_simplify_corners(
1267                         &pd, knots, knots_len, knots_len_remaining,
1268                         SQUARE(error_threshold), SQUARE(error_threshold * 3),
1269                         corner_angle,
1270                         dims,
1271                         r_corner_index_len);
1272         }
1273 #endif  /* USE_CORNER_DETECT */
1274
1275 #ifdef USE_KNOT_REFIT
1276         knots_len_remaining = curve_incremental_simplify_refit(
1277                 &pd, knots, knots_len, knots_len_remaining,
1278                 SQUARE(error_threshold),
1279                 dims);
1280 #endif  /* USE_KNOT_REFIT */
1281
1282
1283 #ifdef USE_CORNER_DETECT
1284         if (use_corner) {
1285                 if (is_cyclic == false) {
1286                         *r_corner_index_len += 2;
1287                 }
1288
1289                 uint *corner_index_array = malloc(sizeof(uint) * (*r_corner_index_len));
1290                 uint k_index = 0, c_index = 0;
1291                 uint i = 0;
1292
1293                 if (is_cyclic == false) {
1294                         corner_index_array[c_index++] = k_index;
1295                         k_index++;
1296                         i++;
1297                 }
1298
1299                 for (; i < knots_len; i++) {
1300                         if (knots[i].is_removed == false) {
1301                                 if (knots[i].is_corner == true) {
1302                                         corner_index_array[c_index++] = k_index;
1303                                 }
1304                                 k_index++;
1305                         }
1306                 }
1307
1308                 if (is_cyclic == false) {
1309                         corner_index_array[c_index++] = k_index;
1310                         k_index++;
1311                 }
1312
1313                 assert(c_index == *r_corner_index_len);
1314                 *r_corner_index_array = corner_index_array;
1315         }
1316 #endif  /* USE_CORNER_DETECT */
1317
1318 #ifdef USE_LENGTH_CACHE
1319         free(points_length_cache);
1320 #endif
1321
1322         uint *cubic_orig_index = NULL;
1323
1324         if (r_cubic_orig_index) {
1325                 cubic_orig_index = malloc(sizeof(uint) * knots_len_remaining);
1326         }
1327
1328         struct Knot *knots_first = NULL;
1329         {
1330                 struct Knot *k;
1331                 for (uint i = 0; i < knots_len; i++) {
1332                         if (knots[i].is_removed == false) {
1333                                 knots_first = &knots[i];
1334                                 break;
1335                         }
1336                 }
1337
1338                 if (cubic_orig_index) {
1339                         k = knots_first;
1340                         for (uint i = 0; i < knots_len_remaining; i++, k = k->next) {
1341                                 cubic_orig_index[i] = k->index;
1342                         }
1343                 }
1344         }
1345
1346         /* Correct unused handle endpoints - not essential, but nice behavior */
1347         if (is_cyclic == false) {
1348                 struct Knot *knots_last = knots_first;
1349                 while (knots_last->next) {
1350                         knots_last = knots_last->next;
1351                 }
1352                 knots_first->handles[0] = -knots_first->handles[1];
1353                 knots_last->handles[1]  = -knots_last->handles[0];
1354         }
1355
1356         /* 3x for one knot and two handles */
1357         double *cubic_array = malloc(sizeof(double) * knots_len_remaining * 3 * dims);
1358
1359         {
1360                 double *c_step = cubic_array;
1361                 struct Knot *k = knots_first;
1362                 for (uint i = 0; i < knots_len_remaining; i++, k = k->next) {
1363                         const double *p = &points[k->index * dims];
1364
1365                         madd_vn_vnvn_fl(c_step, p, k->tan[0], k->handles[0], dims);
1366                         c_step += dims;
1367                         copy_vnvn(c_step, p, dims);
1368                         c_step += dims;
1369                         madd_vn_vnvn_fl(c_step, p, k->tan[1], k->handles[1], dims);
1370                         c_step += dims;
1371                 }
1372                 assert(c_step == &cubic_array[knots_len_remaining * 3 * dims]);
1373         }
1374
1375         if (points_alloc) {
1376                 free(points_alloc);
1377                 points_alloc = NULL;
1378         }
1379
1380         free(knots);
1381         free(tangents);
1382
1383         if (r_cubic_orig_index) {
1384                 *r_cubic_orig_index = cubic_orig_index;
1385         }
1386
1387         *r_cubic_array = cubic_array;
1388         *r_cubic_array_len = knots_len_remaining;
1389
1390         return 0;
1391 }
1392
1393
1394 int curve_fit_cubic_to_points_refit_fl(
1395         const float          *points,
1396         const unsigned int    points_len,
1397         const unsigned int    dims,
1398         const float           error_threshold,
1399         const unsigned int    calc_flag,
1400         const unsigned int   *corners,
1401         unsigned int          corners_len,
1402         const float           corner_angle,
1403
1404         float **r_cubic_array, unsigned int *r_cubic_array_len,
1405         unsigned int   **r_cubic_orig_index,
1406         unsigned int   **r_corner_index_array, unsigned int *r_corner_index_len)
1407 {
1408         const uint points_flat_len = points_len * dims;
1409         double *points_db = malloc(sizeof(double) * points_flat_len);
1410
1411         copy_vndb_vnfl(points_db, points, points_flat_len);
1412
1413         double *cubic_array_db = NULL;
1414         float  *cubic_array_fl = NULL;
1415         uint    cubic_array_len = 0;
1416
1417         int result = curve_fit_cubic_to_points_refit_db(
1418                 points_db, points_len, dims, error_threshold, calc_flag, corners, corners_len,
1419                 corner_angle,
1420                 &cubic_array_db, &cubic_array_len,
1421                 r_cubic_orig_index,
1422                 r_corner_index_array, r_corner_index_len);
1423         free(points_db);
1424
1425         if (!result) {
1426                 uint cubic_array_flat_len = cubic_array_len * 3 * dims;
1427                 cubic_array_fl = malloc(sizeof(float) * cubic_array_flat_len);
1428                 for (uint i = 0; i < cubic_array_flat_len; i++) {
1429                         cubic_array_fl[i] = (float)cubic_array_db[i];
1430                 }
1431                 free(cubic_array_db);
1432         }
1433
1434         *r_cubic_array = cubic_array_fl;
1435         *r_cubic_array_len = cubic_array_len;
1436
1437         return result;
1438 }
1439