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