bf1ab99995f68bf4b089d4a3dfc7b8c7b1a06daf
[blender.git] / 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_remaining);
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_remaining);
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_collapse_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                                         const double *co_prev  = &params.pd->points[k_prev->index * dims];
951                                         const double *co_next  = &params.pd->points[k_next->index * dims];
952                                         const double *co_split = &params.pd->points[split_index * dims];
953
954                                         project_vn_vnvn_normalized(k_proj_ref,   co_prev, k_prev->tan[1], dims);
955                                         project_vn_vnvn_normalized(k_proj_split, co_split, k_prev->tan[1], dims);
956
957                                         if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_collapse_max) {
958
959                                                 project_vn_vnvn_normalized(k_proj_ref,   co_next, k_next->tan[0], dims);
960                                                 project_vn_vnvn_normalized(k_proj_split, co_split, k_next->tan[0], dims);
961
962                                                 if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_collapse_max) {
963
964                                                         struct Knot *k_split = &knots[split_index];
965
966                                                         knot_corner_error_recalculate(
967                                                                 &params,
968                                                                 k_split, k_prev, k_next,
969                                                                 error_sq_max,
970                                                                 dims);
971                                                 }
972                                         }
973                                 }
974                         }
975                 }
976         }
977
978         while (HEAP_is_empty(heap) == false) {
979                 struct KnotCornerState *c = HEAP_popmin(heap);
980
981                 struct Knot *k_split = &knots[c->index];
982
983                 /* Remove while collapsing */
984                 struct Knot *k_prev  = &knots[c->index_adjacent[0]];
985                 struct Knot *k_next  = &knots[c->index_adjacent[1]];
986
987                 /* Insert */
988                 k_split->is_removed = false;
989                 k_split->prev = k_prev;
990                 k_split->next = k_next;
991                 k_prev->next = k_split;
992                 k_next->prev = k_split;
993
994                 /* Update tangents */
995                 k_split->tan[0] = k_prev->tan[1];
996                 k_split->tan[1] = k_next->tan[0];
997
998                 /* Own handles */
999                 k_prev->handles[1]  = c->handles_prev[0];
1000                 k_split->handles[0] = c->handles_prev[1];
1001                 k_split->handles[1] = c->handles_next[0];
1002                 k_next->handles[0]  = c->handles_next[1];
1003
1004                 k_prev->error_sq_next  = c->error_sq[0];
1005                 k_split->error_sq_next = c->error_sq[1];
1006
1007                 k_split->heap_node = NULL;
1008
1009 #ifdef USE_TPOOL
1010                 corner_pool_elem_free(&epool, c);
1011 #else
1012                 free(c);
1013 #endif
1014
1015                 k_split->is_corner = true;
1016
1017                 knots_len_remaining++;
1018
1019                 corner_index_len++;
1020         }
1021
1022 #ifdef USE_TPOOL
1023         corner_pool_destroy(&epool);
1024 #endif
1025
1026         HEAP_free(heap, free);
1027
1028         *r_corner_index_len = corner_index_len;
1029
1030         return knots_len_remaining;
1031 }
1032
1033 #endif  /* USE_CORNER_DETECT */
1034
1035 int curve_fit_cubic_to_points_refit_db(
1036         const double *points,
1037         const uint    points_len,
1038         const uint    dims,
1039         const double  error_threshold,
1040         const uint    calc_flag,
1041         const uint   *corners,
1042         const uint    corners_len,
1043         const double  corner_angle,
1044
1045         double **r_cubic_array, uint *r_cubic_array_len,
1046         uint   **r_cubic_orig_index,
1047         uint   **r_corner_index_array, uint *r_corner_index_len)
1048 {
1049         const uint knots_len = points_len;
1050         struct Knot *knots = malloc(sizeof(Knot) * knots_len);
1051
1052 #ifndef USE_CORNER_DETECT
1053         (void)r_corner_index_array;
1054         (void)r_corner_index_len;
1055 #endif
1056
1057 (void)corners;
1058 (void)corners_len;
1059
1060         const bool is_cyclic = (calc_flag & CURVE_FIT_CALC_CYCLIC) != 0 && (points_len > 2);
1061 #ifdef USE_CORNER_DETECT
1062         const bool use_corner = (corner_angle < M_PI);
1063 #else
1064         (void)corner_angle;
1065 #endif
1066
1067         /* Over alloc the list x2 for cyclic curves,
1068          * so we can evaluate across the start/end */
1069         double *points_alloc = NULL;
1070         if (is_cyclic) {
1071                 points_alloc = malloc((sizeof(double) * points_len * dims) * 2);
1072                 memcpy(points_alloc,                       points,       sizeof(double) * points_len * dims);
1073                 memcpy(points_alloc + (points_len * dims), points_alloc, sizeof(double) * points_len * dims);
1074                 points = points_alloc;
1075         }
1076
1077         double *tangents = malloc(sizeof(double) * knots_len * 2 * dims);
1078
1079         {
1080                 double *t_step = tangents;
1081                 for (uint i = 0; i < knots_len; i++) {
1082                         knots[i].next = (knots + i) + 1;
1083                         knots[i].prev = (knots + i) - 1;
1084
1085                         knots[i].heap_node = NULL;
1086                         knots[i].index = i;
1087                         knots[i].can_remove = true;
1088                         knots[i].is_removed = false;
1089                         knots[i].is_corner = false;
1090                         knots[i].error_sq_next = 0.0;
1091                         knots[i].tan[0] = t_step; t_step += dims;
1092                         knots[i].tan[1] = t_step; t_step += dims;
1093                 }
1094                 assert(t_step == &tangents[knots_len * 2 * dims]);
1095         }
1096
1097         if (is_cyclic) {
1098                 knots[0].prev = &knots[knots_len - 1];
1099                 knots[knots_len - 1].next = &knots[0];
1100         }
1101         else {
1102                 knots[0].prev = NULL;
1103                 knots[knots_len - 1].next = NULL;
1104
1105                 /* always keep end-points */
1106                 knots[0].can_remove = false;
1107                 knots[knots_len - 1].can_remove = false;
1108         }
1109
1110 #ifdef USE_LENGTH_CACHE
1111         double *points_length_cache = malloc(sizeof(double) * points_len * (is_cyclic ? 2 : 1));
1112 #endif
1113
1114         /* Initialize tangents,
1115          * also set the values for knot handles since some may not collapse. */
1116         {
1117 #ifdef USE_VLA
1118                 double tan_prev[dims];
1119                 double tan_next[dims];
1120 #else
1121                 double *tan_prev = alloca(sizeof(double) * dims);
1122                 double *tan_next = alloca(sizeof(double) * dims);
1123 #endif
1124                 double len_prev, len_next;
1125
1126 #if 0
1127                 /* 2x normalize calculations, but correct */
1128
1129                 for (uint i = 0; i < knots_len; i++) {
1130                         Knot *k = &knots[i];
1131
1132                         if (k->prev) {
1133                                 sub_vn_vnvn(tan_prev, &points[k->prev->index * dims], &points[k->index * dims], dims);
1134 #ifdef USE_LENGTH_CACHE
1135                                 points_length_cache[i] =
1136 #endif
1137                                 len_prev = normalize_vn(tan_prev, dims);
1138                         }
1139                         else {
1140                                 zero_vn(tan_prev, dims);
1141                                 len_prev = 0.0;
1142                         }
1143
1144                         if (k->next) {
1145                                 sub_vn_vnvn(tan_next, &points[k->index * dims], &points[k->next->index * dims], dims);
1146                                 len_next = normalize_vn(tan_next, dims);
1147                         }
1148                         else {
1149                                 zero_vn(tan_next, dims);
1150                                 len_next = 0.0;
1151                         }
1152
1153                         add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
1154                         normalize_vn(k->tan[0], dims);
1155                         copy_vnvn(k->tan[1], k->tan[0], dims);
1156                         k->handles[0] = len_prev /  3;
1157                         k->handles[1] = len_next / -3;
1158                 }
1159 #else
1160                 if (knots_len < 2) {
1161                         /* NOP, set dummy values */
1162                         for (uint i = 0; i < knots_len; i++) {
1163                                 struct Knot *k = &knots[i];
1164                                 zero_vn(k->tan[0], dims);
1165                                 zero_vn(k->tan[1], dims);
1166                                 k->handles[0] = 0.0;
1167                                 k->handles[1] = 0.0;
1168 #ifdef USE_LENGTH_CACHE
1169                                 points_length_cache[i] = 0.0;
1170 #endif
1171                         }
1172                 }
1173                 else if (is_cyclic) {
1174                         len_prev = normalize_vn_vnvn(
1175                                 tan_prev, &points[(knots_len - 2) * dims], &points[(knots_len - 1) * dims], dims);
1176                         for (uint i_curr = knots_len - 1, i_next = 0; i_next < knots_len; i_curr = i_next++) {
1177                                 struct Knot *k = &knots[i_curr];
1178 #ifdef USE_LENGTH_CACHE
1179                                 points_length_cache[i_next] =
1180 #endif
1181                                 len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims);
1182
1183                                 add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
1184                                 normalize_vn(k->tan[0], dims);
1185                                 copy_vnvn(k->tan[1], k->tan[0], dims);
1186                                 k->handles[0] = len_prev /  3;
1187                                 k->handles[1] = len_next / -3;
1188
1189                                 copy_vnvn(tan_prev, tan_next, dims);
1190                                 len_prev = len_next;
1191                         }
1192                 }
1193                 else {
1194 #ifdef USE_LENGTH_CACHE
1195                         points_length_cache[0] = 0.0;
1196                         points_length_cache[1] =
1197 #endif
1198                         len_prev = normalize_vn_vnvn(
1199                                 tan_prev, &points[0 * dims], &points[1 * dims], dims);
1200                         copy_vnvn(knots[0].tan[0], tan_prev, dims);
1201                         copy_vnvn(knots[0].tan[1], tan_prev, dims);
1202                         knots[0].handles[0] = len_prev /  3;
1203                         knots[0].handles[1] = len_prev / -3;
1204
1205                         for (uint i_curr = 1, i_next = 2; i_next < knots_len; i_curr = i_next++) {
1206                                 struct Knot *k = &knots[i_curr];
1207
1208 #ifdef USE_LENGTH_CACHE
1209                                 points_length_cache[i_next] =
1210 #endif
1211                                 len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims);
1212
1213                                 add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
1214                                 normalize_vn(k->tan[0], dims);
1215                                 copy_vnvn(k->tan[1], k->tan[0], dims);
1216                                 k->handles[0] = len_prev /  3;
1217                                 k->handles[1] = len_next / -3;
1218
1219                                 copy_vnvn(tan_prev, tan_next, dims);
1220                                 len_prev = len_next;
1221                         }
1222                         copy_vnvn(knots[knots_len - 1].tan[0], tan_next, dims);
1223                         copy_vnvn(knots[knots_len - 1].tan[1], tan_next, dims);
1224
1225                         knots[knots_len - 1].handles[0] = len_next /  3;
1226                         knots[knots_len - 1].handles[1] = len_next / -3;
1227                 }
1228 #endif
1229         }
1230
1231 #ifdef USE_LENGTH_CACHE
1232         if (is_cyclic) {
1233                 memcpy(&points_length_cache[points_len], points_length_cache, sizeof(double) * points_len);
1234         }
1235 #endif
1236
1237
1238 #if 0
1239         for (uint i = 0; i < knots_len; i++) {
1240                 Knot *k = &knots[i];
1241                 printf("TAN %.8f %.8f %.8f %.8f\n", k->tan[0][0], k->tan[0][1], k->tan[1][0], k->tan[0][1]);
1242         }
1243 #endif
1244
1245         const struct PointData pd = {
1246                 .points = points,
1247                 .points_len = points_len,
1248 #ifdef USE_LENGTH_CACHE
1249                 .points_length_cache = points_length_cache,
1250 #endif
1251         };
1252
1253         uint knots_len_remaining = knots_len;
1254
1255         /* 'curve_incremental_simplify_refit' can be called here, but its very slow
1256          * just remove all within the threshold first. */
1257         knots_len_remaining = curve_incremental_simplify(
1258                 &pd, knots, knots_len, knots_len_remaining,
1259                 SQUARE(error_threshold), dims);
1260
1261 #ifdef USE_CORNER_DETECT
1262         if (use_corner) {
1263
1264 #ifdef DEBUG
1265                 for (uint i = 0; i < knots_len; i++) {
1266                         assert(knots[i].heap_node == NULL);
1267                 }
1268 #endif
1269
1270                 knots_len_remaining = curve_incremental_simplify_corners(
1271                         &pd, knots, knots_len, knots_len_remaining,
1272                         SQUARE(error_threshold), SQUARE(error_threshold * 3),
1273                         corner_angle,
1274                         dims,
1275                         r_corner_index_len);
1276         }
1277 #endif  /* USE_CORNER_DETECT */
1278
1279 #ifdef USE_KNOT_REFIT
1280         knots_len_remaining = curve_incremental_simplify_refit(
1281                 &pd, knots, knots_len, knots_len_remaining,
1282                 SQUARE(error_threshold),
1283                 dims);
1284 #endif  /* USE_KNOT_REFIT */
1285
1286
1287 #ifdef USE_CORNER_DETECT
1288         if (use_corner) {
1289                 if (is_cyclic == false) {
1290                         *r_corner_index_len += 2;
1291                 }
1292
1293                 uint *corner_index_array = malloc(sizeof(uint) * (*r_corner_index_len));
1294                 uint k_index = 0, c_index = 0;
1295                 uint i = 0;
1296
1297                 if (is_cyclic == false) {
1298                         corner_index_array[c_index++] = k_index;
1299                         k_index++;
1300                         i++;
1301                 }
1302
1303                 for (; i < knots_len; i++) {
1304                         if (knots[i].is_removed == false) {
1305                                 if (knots[i].is_corner == true) {
1306                                         corner_index_array[c_index++] = k_index;
1307                                 }
1308                                 k_index++;
1309                         }
1310                 }
1311
1312                 if (is_cyclic == false) {
1313                         corner_index_array[c_index++] = k_index;
1314                         k_index++;
1315                 }
1316
1317                 assert(c_index == *r_corner_index_len);
1318                 *r_corner_index_array = corner_index_array;
1319         }
1320 #endif  /* USE_CORNER_DETECT */
1321
1322 #ifdef USE_LENGTH_CACHE
1323         free(points_length_cache);
1324 #endif
1325
1326         uint *cubic_orig_index = NULL;
1327
1328         if (r_cubic_orig_index) {
1329                 cubic_orig_index = malloc(sizeof(uint) * knots_len_remaining);
1330         }
1331
1332         struct Knot *knots_first = NULL;
1333         {
1334                 struct Knot *k;
1335                 for (uint i = 0; i < knots_len; i++) {
1336                         if (knots[i].is_removed == false) {
1337                                 knots_first = &knots[i];
1338                                 break;
1339                         }
1340                 }
1341
1342                 if (cubic_orig_index) {
1343                         k = knots_first;
1344                         for (uint i = 0; i < knots_len_remaining; i++, k = k->next) {
1345                                 cubic_orig_index[i] = k->index;
1346                         }
1347                 }
1348         }
1349
1350         /* Correct unused handle endpoints - not essential, but nice behavior */
1351         if (is_cyclic == false) {
1352                 struct Knot *knots_last = knots_first;
1353                 while (knots_last->next) {
1354                         knots_last = knots_last->next;
1355                 }
1356                 knots_first->handles[0] = -knots_first->handles[1];
1357                 knots_last->handles[1]  = -knots_last->handles[0];
1358         }
1359
1360         /* 3x for one knot and two handles */
1361         double *cubic_array = malloc(sizeof(double) * knots_len_remaining * 3 * dims);
1362
1363         {
1364                 double *c_step = cubic_array;
1365                 struct Knot *k = knots_first;
1366                 for (uint i = 0; i < knots_len_remaining; i++, k = k->next) {
1367                         const double *p = &points[k->index * dims];
1368
1369                         madd_vn_vnvn_fl(c_step, p, k->tan[0], k->handles[0], dims);
1370                         c_step += dims;
1371                         copy_vnvn(c_step, p, dims);
1372                         c_step += dims;
1373                         madd_vn_vnvn_fl(c_step, p, k->tan[1], k->handles[1], dims);
1374                         c_step += dims;
1375                 }
1376                 assert(c_step == &cubic_array[knots_len_remaining * 3 * dims]);
1377         }
1378
1379         if (points_alloc) {
1380                 free(points_alloc);
1381                 points_alloc = NULL;
1382         }
1383
1384         free(knots);
1385         free(tangents);
1386
1387         if (r_cubic_orig_index) {
1388                 *r_cubic_orig_index = cubic_orig_index;
1389         }
1390
1391         *r_cubic_array = cubic_array;
1392         *r_cubic_array_len = knots_len_remaining;
1393
1394         return 0;
1395 }
1396
1397
1398 int curve_fit_cubic_to_points_refit_fl(
1399         const float          *points,
1400         const unsigned int    points_len,
1401         const unsigned int    dims,
1402         const float           error_threshold,
1403         const unsigned int    calc_flag,
1404         const unsigned int   *corners,
1405         unsigned int          corners_len,
1406         const float           corner_angle,
1407
1408         float **r_cubic_array, unsigned int *r_cubic_array_len,
1409         unsigned int   **r_cubic_orig_index,
1410         unsigned int   **r_corner_index_array, unsigned int *r_corner_index_len)
1411 {
1412         const uint points_flat_len = points_len * dims;
1413         double *points_db = malloc(sizeof(double) * points_flat_len);
1414
1415         copy_vndb_vnfl(points_db, points, points_flat_len);
1416
1417         double *cubic_array_db = NULL;
1418         float  *cubic_array_fl = NULL;
1419         uint    cubic_array_len = 0;
1420
1421         int result = curve_fit_cubic_to_points_refit_db(
1422                 points_db, points_len, dims, error_threshold, calc_flag, corners, corners_len,
1423                 corner_angle,
1424                 &cubic_array_db, &cubic_array_len,
1425                 r_cubic_orig_index,
1426                 r_corner_index_array, r_corner_index_len);
1427         free(points_db);
1428
1429         if (!result) {
1430                 uint cubic_array_flat_len = cubic_array_len * 3 * dims;
1431                 cubic_array_fl = malloc(sizeof(float) * cubic_array_flat_len);
1432                 for (uint i = 0; i < cubic_array_flat_len; i++) {
1433                         cubic_array_fl[i] = (float)cubic_array_db[i];
1434                 }
1435                 free(cubic_array_db);
1436         }
1437
1438         *r_cubic_array = cubic_array_fl;
1439         *r_cubic_array_len = cubic_array_len;
1440
1441         return result;
1442 }
1443