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