Curve Fitting: minor change to re-fitting method
[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 #if defined(USE_KNOT_REFIT) && !defined(USE_KNOT_REFIT_REMOVE)
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 && !USE_KNOT_REFIT_REMOVE */
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], uint *r_error_index)
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, r_error_index);
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                 uint error_index_dummy;
367                 return knot_remove_error_value(
368                         tan_l, tan_r,
369                         &pd->points[knot_l->index * dims], points_offset_len,
370 #ifdef USE_LENGTH_CACHE
371                         &pd->points_length_cache[knot_l->index],
372 #else
373                         NULL,
374 #endif
375                         dims,
376                         r_handle_factors, &error_index_dummy);
377         }
378         else {
379                 /* No points between, use 1/3 handle length with no error as a fallback. */
380                 assert(points_offset_len == 2);
381 #ifdef USE_LENGTH_CACHE
382                 r_handle_factors[0] = r_handle_factors[1] = pd->points_length_cache[knot_l->index] / 3.0;
383 #else
384                 r_handle_factors[0] = r_handle_factors[1] = len_vnvn(
385                         &pd->points[(knot_l->index + 0) * dims],
386                         &pd->points[(knot_l->index + 1) * dims], dims) / 3.0;
387 #endif
388                 return 0.0;
389         }
390 }
391
392 #ifdef USE_KNOT_REFIT_REMOVE
393
394 static double knot_calc_curve_error_value_and_index(
395         const struct PointData *pd,
396         const struct Knot *knot_l, const struct Knot *knot_r,
397         const double *tan_l, const double *tan_r,
398         const uint dims,
399         double r_handle_factors[2],
400         uint *r_error_index)
401 {
402         const uint points_offset_len = ((knot_l->index < knot_r->index) ?
403                 (knot_r->index - knot_l->index) :
404                 ((knot_r->index + pd->points_len) - knot_l->index)) + 1;
405
406         if (points_offset_len != 2) {
407                 const double error_sq = knot_remove_error_value(
408                         tan_l, tan_r,
409                         &pd->points[knot_l->index * dims], points_offset_len,
410 #ifdef USE_LENGTH_CACHE
411                         &pd->points_length_cache[knot_l->index],
412 #else
413                         NULL,
414 #endif
415                         dims,
416                         r_handle_factors, r_error_index);
417
418                 /* Adjust the offset index to the global index & wrap if needed. */
419                 *r_error_index += knot_l->index;
420                 if (*r_error_index >= pd->points_len) {
421                         *r_error_index -= pd->points_len;
422                 }
423
424                 return error_sq;
425         }
426         else {
427                 /* No points between, use 1/3 handle length with no error as a fallback. */
428                 assert(points_offset_len == 2);
429 #ifdef USE_LENGTH_CACHE
430                 r_handle_factors[0] = r_handle_factors[1] = pd->points_length_cache[knot_l->index] / 3.0;
431 #else
432                 r_handle_factors[0] = r_handle_factors[1] = len_vnvn(
433                         &pd->points[(knot_l->index + 0) * dims],
434                         &pd->points[(knot_l->index + 1) * dims], dims) / 3.0;
435 #endif
436                 *r_error_index = 0;
437                 return 0.0;
438         }
439 }
440 #endif  /* USE_KNOT_REFIT_REMOVE */
441
442 struct KnotRemove_Params {
443         Heap *heap;
444         const struct PointData *pd;
445 #ifdef USE_TPOOL
446         struct ElemPool_KnotRemoveState *epool;
447 #endif
448 };
449
450 static void knot_remove_error_recalculate(
451         struct KnotRemove_Params *p,
452         struct Knot *k, const double error_sq_max,
453         const uint dims)
454 {
455         assert(k->can_remove);
456         double handles[2];
457
458         const double cost_sq = knot_calc_curve_error_value(
459                 p->pd, k->prev, k->next,
460                 k->prev->tan[1], k->next->tan[0],
461                 dims,
462                 handles);
463
464         if (cost_sq < error_sq_max) {
465                 struct KnotRemoveState *r;
466                 if (k->heap_node) {
467                         r = HEAP_node_ptr(k->heap_node);
468                         HEAP_remove(p->heap, k->heap_node);
469                 }
470                 else {
471 #ifdef USE_TPOOL
472                         r = rstate_pool_elem_alloc(p->epool);
473 #else
474                         r = malloc(sizeof(*r));
475 #endif
476
477                         r->index = k->index;
478                 }
479
480                 r->handles[0] = handles[0];
481                 r->handles[1] = handles[1];
482
483                 k->heap_node = HEAP_insert(p->heap, cost_sq, r);
484         }
485         else {
486                 if (k->heap_node) {
487                         struct KnotRemoveState *r;
488                         r = HEAP_node_ptr(k->heap_node);
489                         HEAP_remove(p->heap, k->heap_node);
490
491 #ifdef USE_TPOOL
492                         rstate_pool_elem_free(p->epool, r);
493 #else
494                         free(r);
495 #endif
496
497                         k->heap_node = NULL;
498                 }
499         }
500 }
501
502 /**
503  * Return length after being reduced.
504  */
505 static uint curve_incremental_simplify(
506         const struct PointData *pd,
507         struct Knot *knots, const uint knots_len, uint knots_len_remaining,
508         double error_sq_max, const uint dims)
509 {
510
511 #ifdef USE_TPOOL
512         struct ElemPool_KnotRemoveState epool;
513
514         rstate_pool_create(&epool, 0);
515 #endif
516
517         Heap *heap = HEAP_new(knots_len_remaining);
518
519         struct KnotRemove_Params params = {
520             .pd = pd,
521             .heap = heap,
522 #ifdef USE_TPOOL
523             .epool = &epool,
524 #endif
525         };
526
527         for (uint i = 0; i < knots_len; i++) {
528                 struct Knot *k = &knots[i];
529                 if (k->can_remove && (k->is_removed == false) && (k->is_corner == false)) {
530                         knot_remove_error_recalculate(&params, k, error_sq_max, dims);
531                 }
532         }
533
534         while (HEAP_is_empty(heap) == false) {
535                 struct Knot *k;
536
537                 {
538                         const double error_sq = HEAP_top_value(heap);
539                         struct KnotRemoveState *r = HEAP_popmin(heap);
540                         k = &knots[r->index];
541                         k->heap_node = NULL;
542                         k->prev->handles[1] = r->handles[0];
543                         k->next->handles[0] = r->handles[1];
544
545                         k->prev->error_sq_next = error_sq;
546
547 #ifdef USE_TPOOL
548                         rstate_pool_elem_free(&epool, r);
549 #else
550                         free(r);
551 #endif
552                 }
553
554                 if (UNLIKELY(knots_len_remaining <= 2)) {
555                         continue;
556                 }
557
558                 struct Knot *k_prev = k->prev;
559                 struct Knot *k_next = k->next;
560
561                 /* Remove ourselves */
562                 k_next->prev = k_prev;
563                 k_prev->next = k_next;
564
565                 k->next = NULL;
566                 k->prev = NULL;
567                 k->is_removed = true;
568
569                 if (k_prev->can_remove && (k_prev->is_corner == false) && (k_prev->prev && k_prev->next)) {
570                         knot_remove_error_recalculate(&params, k_prev, error_sq_max, dims);
571                 }
572
573                 if (k_next->can_remove && (k_next->is_corner == false) && (k_next->prev && k_next->next)) {
574                         knot_remove_error_recalculate(&params, k_next, error_sq_max, dims);
575                 }
576
577                 knots_len_remaining -= 1;
578         }
579
580 #ifdef USE_TPOOL
581         rstate_pool_destroy(&epool);
582 #endif
583
584         HEAP_free(heap, free);
585
586         return knots_len_remaining;
587 }
588
589
590 #ifdef USE_KNOT_REFIT
591
592 struct KnotRefit_Params {
593         Heap *heap;
594         const struct PointData *pd;
595 #ifdef USE_TPOOL
596         struct ElemPool_KnotRefitState *epool;
597 #endif
598 };
599
600 static void knot_refit_error_recalculate(
601         struct KnotRefit_Params *p,
602         struct Knot *knots, const uint knots_len,
603         struct Knot *k,
604         const double error_sq_max,
605         const uint dims)
606 {
607         assert(k->can_remove);
608
609 #ifdef USE_KNOT_REFIT_REMOVE
610         (void)knots_len;
611
612         uint refit_index = SPLIT_POINT_INVALID;
613         {
614                 double handles[2];
615
616                 /* First check if we can remove, this allows to refit and remove as we go. */
617                 const double cost_sq = knot_calc_curve_error_value_and_index(
618                         p->pd, k->prev, k->next,
619                         k->prev->tan[1], k->next->tan[0],
620                         dims,
621                         handles, &refit_index);
622
623                 if (cost_sq < error_sq_max) {
624                         struct KnotRefitState *r;
625                         if (k->heap_node) {
626                                 r = HEAP_node_ptr(k->heap_node);
627                                 HEAP_remove(p->heap, k->heap_node);
628                         }
629                         else {
630 #ifdef USE_TPOOL
631                                 r = refit_pool_elem_alloc(p->epool);
632 #else
633                                 r = malloc(sizeof(*r));
634 #endif
635                                 r->index = k->index;
636                         }
637
638                         r->index_refit = SPLIT_POINT_INVALID;
639
640                         r->handles_prev[0] = handles[0];
641                         r->handles_prev[1] = 0.0;  /* unused */
642                         r->handles_next[0] = 0.0;  /* unused */
643                         r->handles_next[1] = handles[1];
644
645                         r->error_sq[0] = r->error_sq[1] = cost_sq;
646
647                         /* Always perform removal before refitting, (make a negative number) */
648                         k->heap_node = HEAP_insert(p->heap, cost_sq - error_sq_max, r);
649
650                         return;
651                 }
652         }
653 #else
654         (void)error_sq_max;
655
656         const uint refit_index = knot_find_split_point(
657                  p->pd, k->prev, k->next,
658                  knots_len,
659                  dims);
660
661 #endif  /* USE_KNOT_REFIT_REMOVE */
662
663         if ((refit_index == SPLIT_POINT_INVALID) ||
664             (refit_index == k->index))
665         {
666                 goto remove;
667         }
668
669         struct Knot *k_refit = &knots[refit_index];
670
671         const double cost_sq_src_max = MAX2(k->prev->error_sq_next, k->error_sq_next);
672         assert(cost_sq_src_max <= error_sq_max);
673
674         double cost_sq_dst[2];
675         double handles_prev[2], handles_next[2];
676
677         if ((((cost_sq_dst[0] = knot_calc_curve_error_value(
678                    p->pd, k->prev, k_refit,
679                    k->prev->tan[1], k_refit->tan[0],
680                    dims,
681                    handles_prev)) < cost_sq_src_max) &&
682              ((cost_sq_dst[1] = knot_calc_curve_error_value(
683                    p->pd, k_refit, k->next,
684                    k_refit->tan[1], k->next->tan[0],
685                    dims,
686                    handles_next)) < cost_sq_src_max)))
687         {
688                 {
689                         struct KnotRefitState *r;
690                         if (k->heap_node) {
691                                 r = HEAP_node_ptr(k->heap_node);
692                                 HEAP_remove(p->heap, k->heap_node);
693                         }
694                         else {
695 #ifdef USE_TPOOL
696                                 r = refit_pool_elem_alloc(p->epool);
697 #else
698                                 r = malloc(sizeof(*r));
699 #endif
700                                 r->index = k->index;
701                         }
702
703                         r->index_refit = refit_index;
704
705                         r->handles_prev[0] = handles_prev[0];
706                         r->handles_prev[1] = handles_prev[1];
707
708                         r->handles_next[0] = handles_next[0];
709                         r->handles_next[1] = handles_next[1];
710
711                         r->error_sq[0] = cost_sq_dst[0];
712                         r->error_sq[1] = cost_sq_dst[1];
713
714                         const double cost_sq_dst_max = MAX2(cost_sq_dst[0], cost_sq_dst[1]);
715
716                         assert(cost_sq_dst_max < cost_sq_src_max);
717
718                         /* Weight for the greatest improvement */
719                         k->heap_node = HEAP_insert(p->heap, cost_sq_src_max - cost_sq_dst_max, r);
720                 }
721         }
722         else {
723 remove:
724                 if (k->heap_node) {
725                         struct KnotRefitState *r;
726                         r = HEAP_node_ptr(k->heap_node);
727                         HEAP_remove(p->heap, k->heap_node);
728
729 #ifdef USE_TPOOL
730                         refit_pool_elem_free(p->epool, r);
731 #else
732                         free(r);
733 #endif
734
735                         k->heap_node = NULL;
736                 }
737         }
738 }
739
740 /**
741  * Re-adjust the curves by re-fitting points.
742  * test the error from moving using points between the adjacent.
743  */
744 static uint curve_incremental_simplify_refit(
745         const struct PointData *pd,
746         struct Knot *knots, const uint knots_len, uint knots_len_remaining,
747         const double error_sq_max,
748         const uint dims)
749 {
750 #ifdef USE_TPOOL
751         struct ElemPool_KnotRefitState epool;
752
753         refit_pool_create(&epool, 0);
754 #endif
755
756         Heap *heap = HEAP_new(knots_len_remaining);
757
758         struct KnotRefit_Params params = {
759             .pd = pd,
760             .heap = heap,
761 #ifdef USE_TPOOL
762             .epool = &epool,
763 #endif
764         };
765
766         for (uint i = 0; i < knots_len; i++) {
767                 struct Knot *k = &knots[i];
768                 if (k->can_remove &&
769                     (k->is_removed == false) &&
770                     (k->is_corner == false) &&
771                     (k->prev && k->next))
772                 {
773                         knot_refit_error_recalculate(&params, knots, knots_len, k, error_sq_max, dims);
774                 }
775         }
776
777         while (HEAP_is_empty(heap) == false) {
778                 struct Knot *k_old, *k_refit;
779
780                 {
781                         struct KnotRefitState *r = HEAP_popmin(heap);
782                         k_old = &knots[r->index];
783                         k_old->heap_node = NULL;
784
785 #ifdef USE_KNOT_REFIT_REMOVE
786                         if (r->index_refit == SPLIT_POINT_INVALID) {
787                                 k_refit = NULL;
788                         }
789                         else
790 #endif
791                         {
792                                 k_refit = &knots[r->index_refit];
793                                 k_refit->handles[0] = r->handles_prev[1];
794                                 k_refit->handles[1] = r->handles_next[0];
795                         }
796
797                         k_old->prev->handles[1] = r->handles_prev[0];
798                         k_old->next->handles[0] = r->handles_next[1];
799
800 #ifdef USE_TPOOL
801                         refit_pool_elem_free(&epool, r);
802 #else
803                         free(r);
804 #endif
805                 }
806
807                 if (UNLIKELY(knots_len_remaining <= 2)) {
808                         continue;
809                 }
810
811                 struct Knot *k_prev = k_old->prev;
812                 struct Knot *k_next = k_old->next;
813
814                 k_old->next = NULL;
815                 k_old->prev = NULL;
816                 k_old->is_removed = true;
817
818 #ifdef USE_KNOT_REFIT_REMOVE
819                 if (k_refit == NULL) {
820                         k_next->prev = k_prev;
821                         k_prev->next = k_next;
822
823                         knots_len_remaining -= 1;
824                 }
825                 else
826 #endif
827                 {
828                         /* Remove ourselves */
829                         k_next->prev = k_refit;
830                         k_prev->next = k_refit;
831
832                         k_refit->prev = k_prev;
833                         k_refit->next = k_next;
834                         k_refit->is_removed = false;
835                 }
836
837                 if (k_prev->can_remove && (k_prev->is_corner == false) && (k_prev->prev && k_prev->next)) {
838                         knot_refit_error_recalculate(&params, knots, knots_len, k_prev, error_sq_max, dims);
839                 }
840
841                 if (k_next->can_remove && (k_next->is_corner == false) && (k_next->prev && k_next->next)) {
842                         knot_refit_error_recalculate(&params, knots, knots_len, k_next, error_sq_max, dims);
843                 }
844         }
845
846 #ifdef USE_TPOOL
847         refit_pool_destroy(&epool);
848 #endif
849
850         HEAP_free(heap, free);
851
852         return knots_len_remaining;
853 }
854
855 #endif  /* USE_KNOT_REFIT */
856
857
858 #ifdef USE_CORNER_DETECT
859
860 struct KnotCorner_Params {
861         Heap *heap;
862         const struct PointData *pd;
863 #ifdef USE_TPOOL
864         struct ElemPool_KnotCornerState *epool;
865 #endif
866 };
867
868 /**
869  * (Re)calculate the error incurred from turning this into a corner.
870  */
871 static void knot_corner_error_recalculate(
872         struct KnotCorner_Params *p,
873         struct Knot *k_split,
874         struct Knot *k_prev, struct Knot *k_next,
875         const double error_sq_max,
876         const uint dims)
877 {
878         assert(k_prev->can_remove && k_next->can_remove);
879
880         double handles_prev[2], handles_next[2];
881         /* Test skipping 'k_prev' by using points (k_prev->prev to k_split) */
882         double cost_sq_dst[2];
883
884         if (((cost_sq_dst[0] = knot_calc_curve_error_value(
885                   p->pd, k_prev, k_split,
886                   k_prev->tan[1], k_prev->tan[1],
887                   dims,
888                   handles_prev)) < error_sq_max) &&
889             ((cost_sq_dst[1] = knot_calc_curve_error_value(
890                   p->pd, k_split, k_next,
891                   k_next->tan[0], k_next->tan[0],
892                   dims,
893                   handles_next)) < error_sq_max))
894         {
895                 struct KnotCornerState *c;
896                 if (k_split->heap_node) {
897                         c = HEAP_node_ptr(k_split->heap_node);
898                         HEAP_remove(p->heap, k_split->heap_node);
899                 }
900                 else {
901 #ifdef USE_TPOOL
902                         c = corner_pool_elem_alloc(p->epool);
903 #else
904                         c = malloc(sizeof(*c));
905 #endif
906                         c->index = k_split->index;
907                 }
908
909                 c->index_adjacent[0] = k_prev->index;
910                 c->index_adjacent[1] = k_next->index;
911
912                 /* Need to store handle lengths for both sides */
913                 c->handles_prev[0] = handles_prev[0];
914                 c->handles_prev[1] = handles_prev[1];
915
916                 c->handles_next[0] = handles_next[0];
917                 c->handles_next[1] = handles_next[1];
918
919                 c->error_sq[0] = cost_sq_dst[0];
920                 c->error_sq[1] = cost_sq_dst[1];
921
922                 const double cost_max_sq = MAX2(cost_sq_dst[0], cost_sq_dst[1]);
923                 k_split->heap_node = HEAP_insert(p->heap, cost_max_sq, c);
924         }
925         else {
926                 if (k_split->heap_node) {
927                         struct KnotCornerState *c;
928                         c = HEAP_node_ptr(k_split->heap_node);
929                         HEAP_remove(p->heap, k_split->heap_node);
930 #ifdef USE_TPOOL
931                         corner_pool_elem_free(p->epool, c);
932 #else
933                         free(c);
934 #endif
935                         k_split->heap_node = NULL;
936                 }
937         }
938 }
939
940
941 /**
942  * Attempt to collapse close knots into corners,
943  * as long as they fall below the error threshold.
944  */
945 static uint curve_incremental_simplify_corners(
946         const struct PointData *pd,
947         struct Knot *knots, const uint knots_len, uint knots_len_remaining,
948         const double error_sq_max, const double error_sq_collapse_max,
949         const double corner_angle,
950         const uint dims,
951         uint *r_corner_index_len)
952 {
953 #ifdef USE_TPOOL
954         struct ElemPool_KnotCornerState epool;
955
956         corner_pool_create(&epool, 0);
957 #endif
958
959         Heap *heap = HEAP_new(0);
960
961         struct KnotCorner_Params params = {
962             .pd = pd,
963             .heap = heap,
964 #ifdef USE_TPOOL
965             .epool = &epool,
966 #endif
967         };
968
969 #ifdef USE_VLA
970         double plane_no[dims];
971         double k_proj_ref[dims];
972         double k_proj_split[dims];
973 #else
974         double *plane_no =       alloca(sizeof(double) * dims);
975         double *k_proj_ref =     alloca(sizeof(double) * dims);
976         double *k_proj_split =   alloca(sizeof(double) * dims);
977 #endif
978
979         const double corner_angle_cos = cos(corner_angle);
980
981         uint corner_index_len = 0;
982
983         for (uint i = 0; i < knots_len; i++) {
984                 if ((knots[i].is_removed == false) &&
985                     (knots[i].can_remove == true) &&
986                     (knots[i].next && knots[i].next->can_remove))
987                 {
988                         struct Knot *k_prev = &knots[i];
989                         struct Knot *k_next = k_prev->next;
990
991                         /* Angle outside threshold */
992                         if (dot_vnvn(k_prev->tan[0], k_next->tan[1], dims) < corner_angle_cos) {
993                                 /* Measure distance projected onto a plane,
994                                  * since the points may be offset along their own tangents. */
995                                 sub_vn_vnvn(plane_no, k_next->tan[0], k_prev->tan[1], dims);
996
997                                 /* Compare 2x so as to allow both to be changed by maximum of error_sq_max */
998                                 const uint split_index = knot_find_split_point_on_axis(
999                                         pd, k_prev, k_next,
1000                                         knots_len,
1001                                         plane_no,
1002                                         dims);
1003
1004                                 if (split_index != SPLIT_POINT_INVALID) {
1005                                         const double *co_prev  = &params.pd->points[k_prev->index * dims];
1006                                         const double *co_next  = &params.pd->points[k_next->index * dims];
1007                                         const double *co_split = &params.pd->points[split_index * dims];
1008
1009                                         project_vn_vnvn_normalized(k_proj_ref,   co_prev, k_prev->tan[1], dims);
1010                                         project_vn_vnvn_normalized(k_proj_split, co_split, k_prev->tan[1], dims);
1011
1012                                         if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_collapse_max) {
1013
1014                                                 project_vn_vnvn_normalized(k_proj_ref,   co_next, k_next->tan[0], dims);
1015                                                 project_vn_vnvn_normalized(k_proj_split, co_split, k_next->tan[0], dims);
1016
1017                                                 if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_collapse_max) {
1018
1019                                                         struct Knot *k_split = &knots[split_index];
1020
1021                                                         knot_corner_error_recalculate(
1022                                                                 &params,
1023                                                                 k_split, k_prev, k_next,
1024                                                                 error_sq_max,
1025                                                                 dims);
1026                                                 }
1027                                         }
1028                                 }
1029                         }
1030                 }
1031         }
1032
1033         while (HEAP_is_empty(heap) == false) {
1034                 struct KnotCornerState *c = HEAP_popmin(heap);
1035
1036                 struct Knot *k_split = &knots[c->index];
1037
1038                 /* Remove while collapsing */
1039                 struct Knot *k_prev  = &knots[c->index_adjacent[0]];
1040                 struct Knot *k_next  = &knots[c->index_adjacent[1]];
1041
1042                 /* Insert */
1043                 k_split->is_removed = false;
1044                 k_split->prev = k_prev;
1045                 k_split->next = k_next;
1046                 k_prev->next = k_split;
1047                 k_next->prev = k_split;
1048
1049                 /* Update tangents */
1050                 k_split->tan[0] = k_prev->tan[1];
1051                 k_split->tan[1] = k_next->tan[0];
1052
1053                 /* Own handles */
1054                 k_prev->handles[1]  = c->handles_prev[0];
1055                 k_split->handles[0] = c->handles_prev[1];
1056                 k_split->handles[1] = c->handles_next[0];
1057                 k_next->handles[0]  = c->handles_next[1];
1058
1059                 k_prev->error_sq_next  = c->error_sq[0];
1060                 k_split->error_sq_next = c->error_sq[1];
1061
1062                 k_split->heap_node = NULL;
1063
1064 #ifdef USE_TPOOL
1065                 corner_pool_elem_free(&epool, c);
1066 #else
1067                 free(c);
1068 #endif
1069
1070                 k_split->is_corner = true;
1071
1072                 knots_len_remaining++;
1073
1074                 corner_index_len++;
1075         }
1076
1077 #ifdef USE_TPOOL
1078         corner_pool_destroy(&epool);
1079 #endif
1080
1081         HEAP_free(heap, free);
1082
1083         *r_corner_index_len = corner_index_len;
1084
1085         return knots_len_remaining;
1086 }
1087
1088 #endif  /* USE_CORNER_DETECT */
1089
1090 int curve_fit_cubic_to_points_refit_db(
1091         const double *points,
1092         const uint    points_len,
1093         const uint    dims,
1094         const double  error_threshold,
1095         const uint    calc_flag,
1096         const uint   *corners,
1097         const uint    corners_len,
1098         const double  corner_angle,
1099
1100         double **r_cubic_array, uint *r_cubic_array_len,
1101         uint   **r_cubic_orig_index,
1102         uint   **r_corner_index_array, uint *r_corner_index_len)
1103 {
1104         const uint knots_len = points_len;
1105         struct Knot *knots = malloc(sizeof(Knot) * knots_len);
1106
1107 #ifndef USE_CORNER_DETECT
1108         (void)r_corner_index_array;
1109         (void)r_corner_index_len;
1110 #endif
1111
1112 (void)corners;
1113 (void)corners_len;
1114
1115         const bool is_cyclic = (calc_flag & CURVE_FIT_CALC_CYCLIC) != 0 && (points_len > 2);
1116 #ifdef USE_CORNER_DETECT
1117         const bool use_corner = (corner_angle < M_PI);
1118 #else
1119         (void)corner_angle;
1120 #endif
1121
1122         /* Over alloc the list x2 for cyclic curves,
1123          * so we can evaluate across the start/end */
1124         double *points_alloc = NULL;
1125         if (is_cyclic) {
1126                 points_alloc = malloc((sizeof(double) * points_len * dims) * 2);
1127                 memcpy(points_alloc,                       points,       sizeof(double) * points_len * dims);
1128                 memcpy(points_alloc + (points_len * dims), points_alloc, sizeof(double) * points_len * dims);
1129                 points = points_alloc;
1130         }
1131
1132         double *tangents = malloc(sizeof(double) * knots_len * 2 * dims);
1133
1134         {
1135                 double *t_step = tangents;
1136                 for (uint i = 0; i < knots_len; i++) {
1137                         knots[i].next = (knots + i) + 1;
1138                         knots[i].prev = (knots + i) - 1;
1139
1140                         knots[i].heap_node = NULL;
1141                         knots[i].index = i;
1142                         knots[i].can_remove = true;
1143                         knots[i].is_removed = false;
1144                         knots[i].is_corner = false;
1145                         knots[i].error_sq_next = 0.0;
1146                         knots[i].tan[0] = t_step; t_step += dims;
1147                         knots[i].tan[1] = t_step; t_step += dims;
1148                 }
1149                 assert(t_step == &tangents[knots_len * 2 * dims]);
1150         }
1151
1152         if (is_cyclic) {
1153                 knots[0].prev = &knots[knots_len - 1];
1154                 knots[knots_len - 1].next = &knots[0];
1155         }
1156         else {
1157                 knots[0].prev = NULL;
1158                 knots[knots_len - 1].next = NULL;
1159
1160                 /* always keep end-points */
1161                 knots[0].can_remove = false;
1162                 knots[knots_len - 1].can_remove = false;
1163         }
1164
1165 #ifdef USE_LENGTH_CACHE
1166         double *points_length_cache = malloc(sizeof(double) * points_len * (is_cyclic ? 2 : 1));
1167 #endif
1168
1169         /* Initialize tangents,
1170          * also set the values for knot handles since some may not collapse. */
1171         {
1172 #ifdef USE_VLA
1173                 double tan_prev[dims];
1174                 double tan_next[dims];
1175 #else
1176                 double *tan_prev = alloca(sizeof(double) * dims);
1177                 double *tan_next = alloca(sizeof(double) * dims);
1178 #endif
1179                 double len_prev, len_next;
1180
1181 #if 0
1182                 /* 2x normalize calculations, but correct */
1183
1184                 for (uint i = 0; i < knots_len; i++) {
1185                         Knot *k = &knots[i];
1186
1187                         if (k->prev) {
1188                                 sub_vn_vnvn(tan_prev, &points[k->prev->index * dims], &points[k->index * dims], dims);
1189 #ifdef USE_LENGTH_CACHE
1190                                 points_length_cache[i] =
1191 #endif
1192                                 len_prev = normalize_vn(tan_prev, dims);
1193                         }
1194                         else {
1195                                 zero_vn(tan_prev, dims);
1196                                 len_prev = 0.0;
1197                         }
1198
1199                         if (k->next) {
1200                                 sub_vn_vnvn(tan_next, &points[k->index * dims], &points[k->next->index * dims], dims);
1201                                 len_next = normalize_vn(tan_next, dims);
1202                         }
1203                         else {
1204                                 zero_vn(tan_next, dims);
1205                                 len_next = 0.0;
1206                         }
1207
1208                         add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
1209                         normalize_vn(k->tan[0], dims);
1210                         copy_vnvn(k->tan[1], k->tan[0], dims);
1211                         k->handles[0] = len_prev /  3;
1212                         k->handles[1] = len_next / -3;
1213                 }
1214 #else
1215                 if (knots_len < 2) {
1216                         /* NOP, set dummy values */
1217                         for (uint i = 0; i < knots_len; i++) {
1218                                 struct Knot *k = &knots[i];
1219                                 zero_vn(k->tan[0], dims);
1220                                 zero_vn(k->tan[1], dims);
1221                                 k->handles[0] = 0.0;
1222                                 k->handles[1] = 0.0;
1223 #ifdef USE_LENGTH_CACHE
1224                                 points_length_cache[i] = 0.0;
1225 #endif
1226                         }
1227                 }
1228                 else if (is_cyclic) {
1229                         len_prev = normalize_vn_vnvn(
1230                                 tan_prev, &points[(knots_len - 2) * dims], &points[(knots_len - 1) * dims], dims);
1231                         for (uint i_curr = knots_len - 1, i_next = 0; i_next < knots_len; i_curr = i_next++) {
1232                                 struct Knot *k = &knots[i_curr];
1233 #ifdef USE_LENGTH_CACHE
1234                                 points_length_cache[i_next] =
1235 #endif
1236                                 len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims);
1237
1238                                 add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
1239                                 normalize_vn(k->tan[0], dims);
1240                                 copy_vnvn(k->tan[1], k->tan[0], dims);
1241                                 k->handles[0] = len_prev /  3;
1242                                 k->handles[1] = len_next / -3;
1243
1244                                 copy_vnvn(tan_prev, tan_next, dims);
1245                                 len_prev = len_next;
1246                         }
1247                 }
1248                 else {
1249 #ifdef USE_LENGTH_CACHE
1250                         points_length_cache[0] = 0.0;
1251                         points_length_cache[1] =
1252 #endif
1253                         len_prev = normalize_vn_vnvn(
1254                                 tan_prev, &points[0 * dims], &points[1 * dims], dims);
1255                         copy_vnvn(knots[0].tan[0], tan_prev, dims);
1256                         copy_vnvn(knots[0].tan[1], tan_prev, dims);
1257                         knots[0].handles[0] = len_prev /  3;
1258                         knots[0].handles[1] = len_prev / -3;
1259
1260                         for (uint i_curr = 1, i_next = 2; i_next < knots_len; i_curr = i_next++) {
1261                                 struct Knot *k = &knots[i_curr];
1262
1263 #ifdef USE_LENGTH_CACHE
1264                                 points_length_cache[i_next] =
1265 #endif
1266                                 len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims);
1267
1268                                 add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
1269                                 normalize_vn(k->tan[0], dims);
1270                                 copy_vnvn(k->tan[1], k->tan[0], dims);
1271                                 k->handles[0] = len_prev /  3;
1272                                 k->handles[1] = len_next / -3;
1273
1274                                 copy_vnvn(tan_prev, tan_next, dims);
1275                                 len_prev = len_next;
1276                         }
1277                         copy_vnvn(knots[knots_len - 1].tan[0], tan_next, dims);
1278                         copy_vnvn(knots[knots_len - 1].tan[1], tan_next, dims);
1279
1280                         knots[knots_len - 1].handles[0] = len_next /  3;
1281                         knots[knots_len - 1].handles[1] = len_next / -3;
1282                 }
1283 #endif
1284         }
1285
1286 #ifdef USE_LENGTH_CACHE
1287         if (is_cyclic) {
1288                 memcpy(&points_length_cache[points_len], points_length_cache, sizeof(double) * points_len);
1289         }
1290 #endif
1291
1292
1293 #if 0
1294         for (uint i = 0; i < knots_len; i++) {
1295                 Knot *k = &knots[i];
1296                 printf("TAN %.8f %.8f %.8f %.8f\n", k->tan[0][0], k->tan[0][1], k->tan[1][0], k->tan[0][1]);
1297         }
1298 #endif
1299
1300         const struct PointData pd = {
1301                 .points = points,
1302                 .points_len = points_len,
1303 #ifdef USE_LENGTH_CACHE
1304                 .points_length_cache = points_length_cache,
1305 #endif
1306         };
1307
1308         uint knots_len_remaining = knots_len;
1309
1310         /* 'curve_incremental_simplify_refit' can be called here, but its very slow
1311          * just remove all within the threshold first. */
1312         knots_len_remaining = curve_incremental_simplify(
1313                 &pd, knots, knots_len, knots_len_remaining,
1314                 SQUARE(error_threshold), dims);
1315
1316 #ifdef USE_CORNER_DETECT
1317         if (use_corner) {
1318
1319 #ifdef DEBUG
1320                 for (uint i = 0; i < knots_len; i++) {
1321                         assert(knots[i].heap_node == NULL);
1322                 }
1323 #endif
1324
1325                 knots_len_remaining = curve_incremental_simplify_corners(
1326                         &pd, knots, knots_len, knots_len_remaining,
1327                         SQUARE(error_threshold), SQUARE(error_threshold * 3),
1328                         corner_angle,
1329                         dims,
1330                         r_corner_index_len);
1331         }
1332 #endif  /* USE_CORNER_DETECT */
1333
1334 #ifdef USE_KNOT_REFIT
1335         knots_len_remaining = curve_incremental_simplify_refit(
1336                 &pd, knots, knots_len, knots_len_remaining,
1337                 SQUARE(error_threshold),
1338                 dims);
1339 #endif  /* USE_KNOT_REFIT */
1340
1341
1342 #ifdef USE_CORNER_DETECT
1343         if (use_corner) {
1344                 if (is_cyclic == false) {
1345                         *r_corner_index_len += 2;
1346                 }
1347
1348                 uint *corner_index_array = malloc(sizeof(uint) * (*r_corner_index_len));
1349                 uint k_index = 0, c_index = 0;
1350                 uint i = 0;
1351
1352                 if (is_cyclic == false) {
1353                         corner_index_array[c_index++] = k_index;
1354                         k_index++;
1355                         i++;
1356                 }
1357
1358                 for (; i < knots_len; i++) {
1359                         if (knots[i].is_removed == false) {
1360                                 if (knots[i].is_corner == true) {
1361                                         corner_index_array[c_index++] = k_index;
1362                                 }
1363                                 k_index++;
1364                         }
1365                 }
1366
1367                 if (is_cyclic == false) {
1368                         corner_index_array[c_index++] = k_index;
1369                         k_index++;
1370                 }
1371
1372                 assert(c_index == *r_corner_index_len);
1373                 *r_corner_index_array = corner_index_array;
1374         }
1375 #endif  /* USE_CORNER_DETECT */
1376
1377 #ifdef USE_LENGTH_CACHE
1378         free(points_length_cache);
1379 #endif
1380
1381         uint *cubic_orig_index = NULL;
1382
1383         if (r_cubic_orig_index) {
1384                 cubic_orig_index = malloc(sizeof(uint) * knots_len_remaining);
1385         }
1386
1387         struct Knot *knots_first = NULL;
1388         {
1389                 struct Knot *k;
1390                 for (uint i = 0; i < knots_len; i++) {
1391                         if (knots[i].is_removed == false) {
1392                                 knots_first = &knots[i];
1393                                 break;
1394                         }
1395                 }
1396
1397                 if (cubic_orig_index) {
1398                         k = knots_first;
1399                         for (uint i = 0; i < knots_len_remaining; i++, k = k->next) {
1400                                 cubic_orig_index[i] = k->index;
1401                         }
1402                 }
1403         }
1404
1405         /* Correct unused handle endpoints - not essential, but nice behavior */
1406         if (is_cyclic == false) {
1407                 struct Knot *knots_last = knots_first;
1408                 while (knots_last->next) {
1409                         knots_last = knots_last->next;
1410                 }
1411                 knots_first->handles[0] = -knots_first->handles[1];
1412                 knots_last->handles[1]  = -knots_last->handles[0];
1413         }
1414
1415         /* 3x for one knot and two handles */
1416         double *cubic_array = malloc(sizeof(double) * knots_len_remaining * 3 * dims);
1417
1418         {
1419                 double *c_step = cubic_array;
1420                 struct Knot *k = knots_first;
1421                 for (uint i = 0; i < knots_len_remaining; i++, k = k->next) {
1422                         const double *p = &points[k->index * dims];
1423
1424                         madd_vn_vnvn_fl(c_step, p, k->tan[0], k->handles[0], dims);
1425                         c_step += dims;
1426                         copy_vnvn(c_step, p, dims);
1427                         c_step += dims;
1428                         madd_vn_vnvn_fl(c_step, p, k->tan[1], k->handles[1], dims);
1429                         c_step += dims;
1430                 }
1431                 assert(c_step == &cubic_array[knots_len_remaining * 3 * dims]);
1432         }
1433
1434         if (points_alloc) {
1435                 free(points_alloc);
1436                 points_alloc = NULL;
1437         }
1438
1439         free(knots);
1440         free(tangents);
1441
1442         if (r_cubic_orig_index) {
1443                 *r_cubic_orig_index = cubic_orig_index;
1444         }
1445
1446         *r_cubic_array = cubic_array;
1447         *r_cubic_array_len = knots_len_remaining;
1448
1449         return 0;
1450 }
1451
1452
1453 int curve_fit_cubic_to_points_refit_fl(
1454         const float          *points,
1455         const unsigned int    points_len,
1456         const unsigned int    dims,
1457         const float           error_threshold,
1458         const unsigned int    calc_flag,
1459         const unsigned int   *corners,
1460         unsigned int          corners_len,
1461         const float           corner_angle,
1462
1463         float **r_cubic_array, unsigned int *r_cubic_array_len,
1464         unsigned int   **r_cubic_orig_index,
1465         unsigned int   **r_corner_index_array, unsigned int *r_corner_index_len)
1466 {
1467         const uint points_flat_len = points_len * dims;
1468         double *points_db = malloc(sizeof(double) * points_flat_len);
1469
1470         copy_vndb_vnfl(points_db, points, points_flat_len);
1471
1472         double *cubic_array_db = NULL;
1473         float  *cubic_array_fl = NULL;
1474         uint    cubic_array_len = 0;
1475
1476         int result = curve_fit_cubic_to_points_refit_db(
1477                 points_db, points_len, dims, error_threshold, calc_flag, corners, corners_len,
1478                 corner_angle,
1479                 &cubic_array_db, &cubic_array_len,
1480                 r_cubic_orig_index,
1481                 r_corner_index_array, r_corner_index_len);
1482         free(points_db);
1483
1484         if (!result) {
1485                 uint cubic_array_flat_len = cubic_array_len * 3 * dims;
1486                 cubic_array_fl = malloc(sizeof(float) * cubic_array_flat_len);
1487                 for (uint i = 0; i < cubic_array_flat_len; i++) {
1488                         cubic_array_fl[i] = (float)cubic_array_db[i];
1489                 }
1490                 free(cubic_array_db);
1491         }
1492
1493         *r_cubic_array = cubic_array_fl;
1494         *r_cubic_array_len = cubic_array_len;
1495
1496         return result;
1497 }
1498