1a0f7dcfdeef5909e50a457360c70baf7ff182ad
[blender-staging.git] / extern / curve_fit_nd / intern / curve_fit_cubic.c
1 /*
2  * Copyright (c) 2016, DWANGO Co., Ltd.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *     * Redistributions of source code must retain the above copyright
8  *       notice, this list of conditions and the following disclaimer.
9  *     * Redistributions in binary form must reproduce the above copyright
10  *       notice, this list of conditions and the following disclaimer in the
11  *       documentation and/or other materials provided with the distribution.
12  *     * Neither the name of the <organization> nor the
13  *       names of its contributors may be used to endorse or promote products
14  *       derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  */
27
28 /** \file curve_fit_cubic.c
29  *  \ingroup curve_fit
30  */
31
32 #ifdef _MSC_VER
33 #  define _USE_MATH_DEFINES
34 #endif
35
36 #include <math.h>
37 #include <float.h>
38 #include <stdbool.h>
39 #include <assert.h>
40
41 #include <string.h>
42 #include <stdlib.h>
43
44 #include "../curve_fit_nd.h"
45
46 /* Take curvature into account when calculating the least square solution isn't usable. */
47 #define USE_CIRCULAR_FALLBACK
48
49 /* Use the maximum distance of any points from the direct line between 2 points
50  * to calculate how long the handles need to be.
51  * Can do a 'perfect' reversal of subdivision when for curve has symmetrical handles and doesn't change direction
52  * (as with an 'S' shape). */
53 #define USE_OFFSET_FALLBACK
54
55 /* avoid re-calculating lengths multiple times */
56 #define USE_LENGTH_CACHE
57
58 /* store the indices in the cubic data so we can return the original indices,
59  * useful when the caller has data associated with the curve. */
60 #define USE_ORIG_INDEX_DATA
61
62 typedef unsigned int uint;
63
64 #include "curve_fit_inline.h"
65
66 #ifdef _MSC_VER
67 #  define alloca(size) _alloca(size)
68 #endif
69
70 #if !defined(_MSC_VER)
71 #  define USE_VLA
72 #endif
73
74 #ifdef USE_VLA
75 #  ifdef __GNUC__
76 #    pragma GCC diagnostic ignored "-Wvla"
77 #  endif
78 #else
79 #  ifdef __GNUC__
80 #    pragma GCC diagnostic error "-Wvla"
81 #  endif
82 #endif
83
84 #define SWAP(type, a, b)  {    \
85         type sw_ap;                \
86         sw_ap = (a);               \
87         (a) = (b);                 \
88         (b) = sw_ap;               \
89 } (void)0
90
91
92 /* -------------------------------------------------------------------- */
93
94 /** \name Cubic Type & Functions
95  * \{ */
96
97 typedef struct Cubic {
98         /* single linked lists */
99         struct Cubic *next;
100 #ifdef USE_ORIG_INDEX_DATA
101         uint orig_span;
102 #endif
103         /* 0: point_0, 1: handle_0, 2: handle_1, 3: point_1,
104          * each one is offset by 'dims' */
105         double pt_data[0];
106 } Cubic;
107
108 #define CUBIC_PT(cubic, index, dims) \
109         (&(cubic)->pt_data[(index) * (dims)])
110
111 #define CUBIC_VARS(c, dims, _p0, _p1, _p2, _p3) \
112         double \
113         *_p0 = (c)->pt_data, \
114         *_p1 = _p0 + (dims), \
115         *_p2 = _p1 + (dims), \
116         *_p3 = _p2 + (dims); ((void)0)
117 #define CUBIC_VARS_CONST(c, dims, _p0, _p1, _p2, _p3) \
118         const double \
119         *_p0 = (c)->pt_data, \
120         *_p1 = _p0 + (dims), \
121         *_p2 = _p1 + (dims), \
122         *_p3 = _p2 + (dims); ((void)0)
123
124
125 static size_t cubic_alloc_size(const uint dims)
126 {
127         return sizeof(Cubic) + (sizeof(double) * 4 * dims);
128 }
129
130 static Cubic *cubic_alloc(const uint dims)
131 {
132         return malloc(cubic_alloc_size(dims));
133 }
134
135 static void cubic_copy(Cubic *cubic_dst, const Cubic *cubic_src, const uint dims)
136 {
137         memcpy(cubic_dst, cubic_src, cubic_alloc_size(dims));
138 }
139
140 static void cubic_init(
141         Cubic *cubic,
142         const double p0[], const double p1[], const double p2[], const double p3[],
143         const uint dims)
144 {
145         copy_vnvn(CUBIC_PT(cubic, 0, dims), p0, dims);
146         copy_vnvn(CUBIC_PT(cubic, 1, dims), p1, dims);
147         copy_vnvn(CUBIC_PT(cubic, 2, dims), p2, dims);
148         copy_vnvn(CUBIC_PT(cubic, 3, dims), p3, dims);
149 }
150
151 static void cubic_free(Cubic *cubic)
152 {
153         free(cubic);
154 }
155
156 /** \} */
157
158
159 /* -------------------------------------------------------------------- */
160
161 /** \name CubicList Type & Functions
162  * \{ */
163
164 typedef struct CubicList {
165         struct Cubic *items;
166         uint          len;
167         uint          dims;
168 } CubicList;
169
170 static void cubic_list_prepend(CubicList *clist, Cubic *cubic)
171 {
172         cubic->next = clist->items;
173         clist->items = cubic;
174         clist->len++;
175 }
176
177 static double *cubic_list_as_array(
178         const CubicList *clist
179 #ifdef USE_ORIG_INDEX_DATA
180         ,
181         const uint index_last,
182         uint *r_orig_index
183 #endif
184         )
185 {
186         const uint dims = clist->dims;
187         const uint array_flat_len = (clist->len + 1) * 3 * dims;
188
189         double *array = malloc(sizeof(double) * array_flat_len);
190         const double *handle_prev = &((Cubic *)clist->items)->pt_data[dims];
191
192 #ifdef USE_ORIG_INDEX_DATA
193         uint orig_index_value = index_last;
194         uint orig_index_index = clist->len;
195         bool use_orig_index = (r_orig_index != NULL);
196 #endif
197
198         /* fill the array backwards */
199         const size_t array_chunk = 3 * dims;
200         double *array_iter = array + array_flat_len;
201         for (Cubic *citer = clist->items; citer; citer = citer->next) {
202                 array_iter -= array_chunk;
203                 memcpy(array_iter, &citer->pt_data[2 * dims], sizeof(double) * 2 * dims);
204                 memcpy(&array_iter[2 * dims], &handle_prev[dims], sizeof(double) * dims);
205                 handle_prev = citer->pt_data;
206
207 #ifdef USE_ORIG_INDEX_DATA
208                 if (use_orig_index) {
209                         r_orig_index[orig_index_index--] = orig_index_value;
210                         orig_index_value -= citer->orig_span;
211                 }
212 #endif
213         }
214
215 #ifdef USE_ORIG_INDEX_DATA
216         if (use_orig_index) {
217                 assert(orig_index_index == 0);
218                 assert(orig_index_value == 0 || index_last == 0);
219                 r_orig_index[orig_index_index] = index_last ? orig_index_value : 0;
220
221         }
222 #endif
223
224         /* flip tangent for first and last (we could leave at zero, but set to something useful) */
225
226         /* first */
227         array_iter -= array_chunk;
228         memcpy(&array_iter[dims], handle_prev, sizeof(double) * 2 * dims);
229         flip_vn_vnvn(&array_iter[0 * dims], &array_iter[1 * dims], &array_iter[2 * dims], dims);
230         assert(array == array_iter);
231
232         /* last */
233         array_iter += array_flat_len - (3 * dims);
234         flip_vn_vnvn(&array_iter[2 * dims], &array_iter[1 * dims], &array_iter[0 * dims], dims);
235
236         return array;
237 }
238
239 static void cubic_list_clear(CubicList *clist)
240 {
241         Cubic *cubic_next;
242         for (Cubic *citer = clist->items; citer; citer = cubic_next) {
243                 cubic_next = citer->next;
244                 cubic_free(citer);
245         }
246         clist->items = NULL;
247         clist->len  = 0;
248 }
249
250 /** \} */
251
252
253 /* -------------------------------------------------------------------- */
254
255 /** \name Cubic Evaluation
256  * \{ */
257
258 static void cubic_evaluate(
259         const Cubic *cubic, const double t, const uint dims,
260         double r_v[])
261 {
262         CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
263         const double s = 1.0 - t;
264
265         for (uint j = 0; j < dims; j++) {
266                 const double p01 = (p0[j] * s) + (p1[j] * t);
267                 const double p12 = (p1[j] * s) + (p2[j] * t);
268                 const double p23 = (p2[j] * s) + (p3[j] * t);
269                 r_v[j] = ((((p01 * s) + (p12 * t))) * s) +
270                          ((((p12 * s) + (p23 * t))) * t);
271         }
272 }
273
274 static void cubic_calc_point(
275         const Cubic *cubic, const double t, const uint dims,
276         double r_v[])
277 {
278         CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
279         const double s = 1.0 - t;
280         for (uint j = 0; j < dims; j++) {
281                 r_v[j] = p0[j] * s * s * s +
282                          3.0 * t * s * (s * p1[j] + t * p2[j]) + t * t * t * p3[j];
283         }
284 }
285
286 static void cubic_calc_speed(
287         const Cubic *cubic, const double t, const uint dims,
288         double r_v[])
289 {
290         CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
291         const double s = 1.0 - t;
292         for (uint j = 0; j < dims; j++) {
293                 r_v[j] =  3.0 * ((p1[j] - p0[j]) * s * s + 2.0 *
294                                  (p2[j] - p0[j]) * s * t +
295                                  (p3[j] - p2[j]) * t * t);
296         }
297 }
298
299 static void cubic_calc_acceleration(
300         const Cubic *cubic, const double t, const uint dims,
301         double r_v[])
302 {
303         CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
304         const double s = 1.0 - t;
305         for (uint j = 0; j < dims; j++) {
306                 r_v[j] = 6.0 * ((p2[j] - 2.0 * p1[j] + p0[j]) * s +
307                                 (p3[j] - 2.0 * p2[j] + p1[j]) * t);
308         }
309 }
310
311 /**
312  * Returns a 'measure' of the maximum distance (squared) of the points specified
313  * by points_offset from the corresponding cubic(u[]) points.
314  */
315 static double cubic_calc_error(
316         const Cubic *cubic,
317         const double *points_offset,
318         const uint points_offset_len,
319         const double *u,
320         const uint dims,
321
322         uint *r_error_index)
323 {
324         double error_max_sq = 0.0;
325         uint   error_index = 0;
326
327         const double *pt_real = points_offset + dims;
328 #ifdef USE_VLA
329         double        pt_eval[dims];
330 #else
331         double       *pt_eval = alloca(sizeof(double) * dims);
332 #endif
333
334         for (uint i = 1; i < points_offset_len - 1; i++, pt_real += dims) {
335                 cubic_evaluate(cubic, u[i], dims, pt_eval);
336
337                 const double err_sq = len_squared_vnvn(pt_real, pt_eval, dims);
338                 if (err_sq >= error_max_sq) {
339                         error_max_sq = err_sq;
340                         error_index = i;
341                 }
342         }
343
344         *r_error_index = error_index;
345         return error_max_sq;
346 }
347
348 #ifdef USE_OFFSET_FALLBACK
349 /**
350  * A version #cubic_calc_error where we don't need the split-index and can exit early when over the limit.
351  */
352 static double cubic_calc_error_simple(
353         const Cubic *cubic,
354         const double *points_offset,
355         const uint points_offset_len,
356         const double *u,
357         const double error_threshold_sq,
358         const uint dims)
359
360 {
361         double error_max_sq = 0.0;
362
363         const double *pt_real = points_offset + dims;
364 #ifdef USE_VLA
365         double        pt_eval[dims];
366 #else
367         double       *pt_eval = alloca(sizeof(double) * dims);
368 #endif
369
370         for (uint i = 1; i < points_offset_len - 1; i++, pt_real += dims) {
371                 cubic_evaluate(cubic, u[i], dims, pt_eval);
372
373                 const double err_sq = len_squared_vnvn(pt_real, pt_eval, dims);
374                 if (err_sq >= error_threshold_sq) {
375                         return error_threshold_sq;
376                 }
377                 else if (err_sq >= error_max_sq) {
378                         error_max_sq = err_sq;
379                 }
380         }
381
382         return error_max_sq;
383 }
384 #endif
385
386 /**
387  * Bezier multipliers
388  */
389
390 static double B1(double u)
391 {
392         double tmp = 1.0 - u;
393         return 3.0 * u * tmp * tmp;
394 }
395
396 static double B2(double u)
397 {
398         return 3.0 * u * u * (1.0 - u);
399 }
400
401 static double B0plusB1(double u)
402 {
403     double tmp = 1.0 - u;
404     return tmp * tmp * (1.0 + 2.0 * u);
405 }
406
407 static double B2plusB3(double u)
408 {
409     return u * u * (3.0 - 2.0 * u);
410 }
411
412 static void points_calc_center_weighted(
413         const double *points_offset,
414         const uint    points_offset_len,
415         const uint    dims,
416
417         double r_center[])
418 {
419         /*
420          * Calculate a center that compensates for point spacing.
421          */
422
423         const double *pt_prev = &points_offset[(points_offset_len - 2) * dims];
424         const double *pt_curr = pt_prev + dims;
425         const double *pt_next = points_offset;
426
427         double w_prev = len_vnvn(pt_prev, pt_curr, dims);
428
429         zero_vn(r_center, dims);
430         double w_tot = 0.0;
431
432         for (uint i_next = 0; i_next < points_offset_len; i_next++) {
433                 const double w_next = len_vnvn(pt_curr, pt_next, dims);
434                 const double w = w_prev + w_next;
435                 w_tot += w;
436
437                 miadd_vn_vn_fl(r_center, pt_curr, w, dims);
438
439                 w_prev = w_next;
440
441                 pt_prev = pt_curr;
442                 pt_curr = pt_next;
443                 pt_next += dims;
444         }
445
446         if (w_tot != 0.0) {
447                 imul_vn_fl(r_center, 1.0 / w_tot, dims);
448         }
449 }
450
451 #ifdef USE_CIRCULAR_FALLBACK
452
453 /**
454  * Return a scale value, used to calculate how much the curve handles should be increased,
455  *
456  * This works by placing each end-point on an imaginary circle,
457  * the placement on the circle is based on the tangent vectors,
458  * where larger differences in tangent angle cover a larger part of the circle.
459  *
460  * Return the scale representing how much larger the distance around the circle is.
461  */
462 static double points_calc_circumference_factor(
463         const double  tan_l[],
464         const double  tan_r[],
465         const uint dims)
466 {
467         const double dot = dot_vnvn(tan_l, tan_r, dims);
468         const double len_tangent = dot < 0.0 ? len_vnvn(tan_l, tan_r, dims) : len_negated_vnvn(tan_l, tan_r, dims);
469         if (len_tangent > DBL_EPSILON) {
470                 /* only clamp to avoid precision error */
471                 double angle = acos(max(-fabs(dot), -1.0));
472                 /* Angle may be less than the length when the tangents define >180 degrees of the circle,
473                  * (tangents that point away from each other).
474                  * We could try support this but will likely cause extreme >1 scales which could cause other issues. */
475                 // assert(angle >= len_tangent);
476                 double factor = (angle / len_tangent);
477                 assert(factor < (M_PI / 2) + DBL_EPSILON);
478                 return factor;
479         }
480         else {
481                 /* tangents are exactly aligned (think two opposite sides of a circle). */
482                 return (M_PI / 2);
483         }
484 }
485
486 /**
487  * Return the value which the distance between points will need to be scaled by,
488  * to define a handle, given both points are on a perfect circle.
489  *
490  * \note the return value will need to be multiplied by 1.3... for correct results.
491  */
492 static double points_calc_circle_tangent_factor(
493         const double  tan_l[],
494         const double  tan_r[],
495         const uint dims)
496 {
497         const double eps = 1e-8;
498         const double tan_dot = dot_vnvn(tan_l, tan_r, dims);
499         if (tan_dot > 1.0 - eps) {
500                 /* no angle difference (use fallback, length wont make any difference) */
501                 return (1.0 / 3.0) * 0.75;
502         }
503         else if (tan_dot < -1.0 + eps) {
504                 /* parallele tangents (half-circle) */
505                 return (1.0 / 2.0);
506         }
507         else {
508                 /* non-aligned tangents, calculate handle length */
509                 const double angle = acos(tan_dot) / 2.0;
510
511                 /* could also use 'angle_sin = len_vnvn(tan_l, tan_r, dims) / 2.0' */
512                 const double angle_sin = sin(angle);
513                 const double angle_cos = cos(angle);
514                 return ((1.0 - angle_cos) / (angle_sin * 2.0)) / angle_sin;
515         }
516 }
517
518 /**
519  * Calculate the scale the handles, which serves as a best-guess
520  * used as a fallback when the least-square solution fails.
521  */
522 static double points_calc_cubic_scale(
523         const double v_l[], const double v_r[],
524         const double  tan_l[],
525         const double  tan_r[],
526         const double coords_length, uint dims)
527 {
528         const double len_direct = len_vnvn(v_l, v_r, dims);
529         const double len_circle_factor = points_calc_circle_tangent_factor(tan_l, tan_r, dims);
530
531         /* if this curve is a circle, this value doesn't need modification */
532         const double len_circle_handle = (len_direct * (len_circle_factor / 0.75));
533
534         /* scale by the difference from the circumference distance */
535         const double len_circle = len_direct * points_calc_circumference_factor(tan_l, tan_r, dims);
536         double scale_handle = (coords_length / len_circle);
537
538         /* Could investigate an accurate calculation here,
539          * though this gives close results */
540         scale_handle = ((scale_handle - 1.0) * 1.75) + 1.0;
541
542         return len_circle_handle * scale_handle;
543 }
544
545 static void cubic_from_points_fallback(
546         const double *points_offset,
547         const uint    points_offset_len,
548         const double  tan_l[],
549         const double  tan_r[],
550         const uint dims,
551
552         Cubic *r_cubic)
553 {
554         const double *p0 = &points_offset[0];
555         const double *p3 = &points_offset[(points_offset_len - 1) * dims];
556
557         double alpha = len_vnvn(p0, p3, dims) / 3.0;
558
559         double *p1 = CUBIC_PT(r_cubic, 1, dims);
560         double *p2 = CUBIC_PT(r_cubic, 2, dims);
561
562         copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims);
563         copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims);
564
565 #ifdef USE_ORIG_INDEX_DATA
566         r_cubic->orig_span = (points_offset_len - 1);
567 #endif
568
569         /* p1 = p0 - (tan_l * alpha_l);
570          * p2 = p3 + (tan_r * alpha_r);
571          */
572         msub_vn_vnvn_fl(p1, p0, tan_l, alpha, dims);
573         madd_vn_vnvn_fl(p2, p3, tan_r, alpha, dims);
574 }
575 #endif  /* USE_CIRCULAR_FALLBACK */
576
577
578 #ifdef USE_OFFSET_FALLBACK
579
580 static void cubic_from_points_offset_fallback(
581         const double *points_offset,
582         const uint    points_offset_len,
583         const double  tan_l[],
584         const double  tan_r[],
585         const uint dims,
586
587         Cubic *r_cubic)
588 {
589         const double *p0 = &points_offset[0];
590         const double *p3 = &points_offset[(points_offset_len - 1) * dims];
591
592 #ifdef USE_VLA
593         double dir_unit[dims];
594         double a[2][dims];
595         double tmp[dims];
596 #else
597         double *dir_unit = alloca(sizeof(double) * dims);
598         double *a[2] = {
599             alloca(sizeof(double) * dims),
600             alloca(sizeof(double) * dims),
601         };
602         double *tmp = alloca(sizeof(double) * dims);
603 #endif
604
605         const double dir_dist = normalize_vn_vnvn(dir_unit, p3, p0, dims);
606         project_plane_vn_vnvn_normalized(a[0], tan_l, dir_unit, dims);
607         project_plane_vn_vnvn_normalized(a[1], tan_r, dir_unit, dims);
608
609         /* only for better accuracy, not essential */
610         normalize_vn(a[0], dims);
611         normalize_vn(a[1], dims);
612
613         mul_vnvn_fl(a[1], a[1], -1, dims);
614
615         double dists[2] = {0, 0};
616
617         const double *pt = points_offset;
618         for (uint i = 1; i < points_offset_len - 1; i++, pt += dims) {
619                 for (uint k = 0; k < 2; k++) {
620                         sub_vn_vnvn(tmp, p0, pt, dims);
621                         project_vn_vnvn_normalized(tmp, tmp, a[k], dims);
622                         dists[k] = max(dists[k], dot_vnvn(tmp, a[k], dims));
623                 }
624         }
625
626         float alpha_l = (dists[0] / 0.75) /  dot_vnvn(tan_l, a[0], dims);
627         float alpha_r = (dists[1] / 0.75) / -dot_vnvn(tan_r, a[1], dims);
628
629         if (!(alpha_l > 0.0f)) {
630                 alpha_l = dir_dist / 3.0;
631         }
632         if (!(alpha_r > 0.0f)) {
633                 alpha_r = dir_dist / 3.0;
634         }
635
636         double *p1 = CUBIC_PT(r_cubic, 1, dims);
637         double *p2 = CUBIC_PT(r_cubic, 2, dims);
638
639         copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims);
640         copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims);
641
642 #ifdef USE_ORIG_INDEX_DATA
643         r_cubic->orig_span = (points_offset_len - 1);
644 #endif
645
646         /* p1 = p0 - (tan_l * alpha_l);
647          * p2 = p3 + (tan_r * alpha_r);
648          */
649         msub_vn_vnvn_fl(p1, p0, tan_l, alpha_l, dims);
650         madd_vn_vnvn_fl(p2, p3, tan_r, alpha_r, dims);
651 }
652
653 #endif  /* USE_OFFSET_FALLBACK */
654
655
656 /**
657  * Use least-squares method to find Bezier control points for region.
658  */
659 static void cubic_from_points(
660         const double *points_offset,
661         const uint    points_offset_len,
662 #ifdef USE_CIRCULAR_FALLBACK
663         const double  points_offset_coords_length,
664 #endif
665         const double *u_prime,
666         const double  tan_l[],
667         const double  tan_r[],
668         const uint dims,
669
670         Cubic *r_cubic)
671 {
672
673         const double *p0 = &points_offset[0];
674         const double *p3 = &points_offset[(points_offset_len - 1) * dims];
675
676         /* Point Pairs */
677         double alpha_l, alpha_r;
678 #ifdef USE_VLA
679         double a[2][dims];
680         double tmp[dims];
681 #else
682         double *a[2] = {
683             alloca(sizeof(double) * dims),
684             alloca(sizeof(double) * dims),
685         };
686         double *tmp = alloca(sizeof(double) * dims);
687 #endif
688
689         {
690                 double x[2] = {0.0}, c[2][2] = {{0.0}};
691                 const double *pt = points_offset;
692
693                 for (uint i = 0; i < points_offset_len; i++, pt += dims) {
694                         mul_vnvn_fl(a[0], tan_l, B1(u_prime[i]), dims);
695                         mul_vnvn_fl(a[1], tan_r, B2(u_prime[i]), dims);
696
697                         c[0][0] += dot_vnvn(a[0], a[0], dims);
698                         c[0][1] += dot_vnvn(a[0], a[1], dims);
699                         c[1][1] += dot_vnvn(a[1], a[1], dims);
700
701                         c[1][0] = c[0][1];
702
703                         {
704                                 const double b0_plus_b1 = B0plusB1(u_prime[i]);
705                                 const double b2_plus_b3 = B2plusB3(u_prime[i]);
706                                 for (uint j = 0; j < dims; j++) {
707                                         tmp[j] = (pt[j] - (p0[j] * b0_plus_b1)) + (p3[j] * b2_plus_b3);
708                                 }
709
710                                 x[0] += dot_vnvn(a[0], tmp, dims);
711                                 x[1] += dot_vnvn(a[1], tmp, dims);
712                         }
713                 }
714
715                 double det_C0_C1 = c[0][0] * c[1][1] - c[0][1] * c[1][0];
716                 double det_C_0X  = x[1]    * c[0][0] - x[0]    * c[0][1];
717                 double det_X_C1  = x[0]    * c[1][1] - x[1]    * c[0][1];
718
719                 if (is_almost_zero(det_C0_C1)) {
720                         det_C0_C1 = c[0][0] * c[1][1] * 10e-12;
721                 }
722
723                 /* may still divide-by-zero, check below will catch nan values */
724                 alpha_l = det_X_C1 / det_C0_C1;
725                 alpha_r = det_C_0X / det_C0_C1;
726         }
727
728         /*
729          * The problem that the stupid values for alpha dare not put
730          * only when we realize that the sign and wrong,
731          * but even if the values are too high.
732          * But how do you evaluate it?
733          *
734          * Meanwhile, we should ensure that these values are sometimes
735          * so only problems absurd of approximation and not for bugs in the code.
736          */
737
738         bool use_clamp = true;
739
740         /* flip check to catch nan values */
741         if (!(alpha_l >= 0.0) ||
742             !(alpha_r >= 0.0))
743         {
744 #ifdef USE_CIRCULAR_FALLBACK
745                 alpha_l = alpha_r = points_calc_cubic_scale(p0, p3, tan_l, tan_r, points_offset_coords_length, dims);
746 #else
747                 alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
748 #endif
749
750                 /* skip clamping when we're using default handles */
751                 use_clamp = false;
752         }
753
754         double *p1 = CUBIC_PT(r_cubic, 1, dims);
755         double *p2 = CUBIC_PT(r_cubic, 2, dims);
756
757         copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims);
758         copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims);
759
760 #ifdef USE_ORIG_INDEX_DATA
761         r_cubic->orig_span = (points_offset_len - 1);
762 #endif
763
764         /* p1 = p0 - (tan_l * alpha_l);
765          * p2 = p3 + (tan_r * alpha_r);
766          */
767         msub_vn_vnvn_fl(p1, p0, tan_l, alpha_l, dims);
768         madd_vn_vnvn_fl(p2, p3, tan_r, alpha_r, dims);
769
770         /* ------------------------------------
771          * Clamping (we could make it optional)
772          */
773         if (use_clamp) {
774 #ifdef USE_VLA
775                 double center[dims];
776 #else
777                 double *center = alloca(sizeof(double) * dims);
778 #endif
779                 points_calc_center_weighted(points_offset, points_offset_len, dims, center);
780
781                 const double clamp_scale = 3.0;  /* clamp to 3x */
782                 double dist_sq_max = 0.0;
783
784                 {
785                         const double *pt = points_offset;
786                         for (uint i = 0; i < points_offset_len; i++, pt += dims) {
787 #if 0
788                                 double dist_sq_test = sq(len_vnvn(center, pt, dims) * clamp_scale);
789 #else
790                                 /* do inline */
791                                 double dist_sq_test = 0.0;
792                                 for (uint j = 0; j < dims; j++) {
793                                         dist_sq_test += sq((pt[j] - center[j]) * clamp_scale);
794                                 }
795 #endif
796                                 dist_sq_max = max(dist_sq_max, dist_sq_test);
797                         }
798                 }
799
800                 double p1_dist_sq = len_squared_vnvn(center, p1, dims);
801                 double p2_dist_sq = len_squared_vnvn(center, p2, dims);
802
803                 if (p1_dist_sq > dist_sq_max ||
804                     p2_dist_sq > dist_sq_max)
805                 {
806 #ifdef USE_CIRCULAR_FALLBACK
807                         alpha_l = alpha_r = points_calc_cubic_scale(p0, p3, tan_l, tan_r, points_offset_coords_length, dims);
808 #else
809                         alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
810 #endif
811
812                         /*
813                          * p1 = p0 - (tan_l * alpha_l);
814                          * p2 = p3 + (tan_r * alpha_r);
815                          */
816                         for (uint j = 0; j < dims; j++) {
817                                 p1[j] = p0[j] - (tan_l[j] * alpha_l);
818                                 p2[j] = p3[j] + (tan_r[j] * alpha_r);
819                         }
820
821                         p1_dist_sq = len_squared_vnvn(center, p1, dims);
822                         p2_dist_sq = len_squared_vnvn(center, p2, dims);
823                 }
824
825                 /* clamp within the 3x radius */
826                 if (p1_dist_sq > dist_sq_max) {
827                         isub_vnvn(p1, center, dims);
828                         imul_vn_fl(p1, sqrt(dist_sq_max) / sqrt(p1_dist_sq), dims);
829                         iadd_vnvn(p1, center, dims);
830                 }
831                 if (p2_dist_sq > dist_sq_max) {
832                         isub_vnvn(p2, center, dims);
833                         imul_vn_fl(p2, sqrt(dist_sq_max) / sqrt(p2_dist_sq), dims);
834                         iadd_vnvn(p2, center, dims);
835                 }
836         }
837         /* end clamping */
838 }
839
840 #ifdef USE_LENGTH_CACHE
841 static void points_calc_coord_length_cache(
842         const double *points_offset,
843         const uint    points_offset_len,
844         const uint    dims,
845
846         double     *r_points_length_cache)
847 {
848         const double *pt_prev = points_offset;
849         const double *pt = pt_prev + dims;
850         r_points_length_cache[0] = 0.0;
851         for (uint i = 1; i < points_offset_len; i++) {
852                 r_points_length_cache[i] = len_vnvn(pt, pt_prev, dims);
853                 pt_prev = pt;
854                 pt += dims;
855         }
856 }
857 #endif  /* USE_LENGTH_CACHE */
858
859 /**
860  * \return the accumulated length of \a points_offset.
861  */
862 static double points_calc_coord_length(
863         const double *points_offset,
864         const uint    points_offset_len,
865         const uint    dims,
866 #ifdef USE_LENGTH_CACHE
867         const double *points_length_cache,
868 #endif
869         double *r_u)
870 {
871         const double *pt_prev = points_offset;
872         const double *pt = pt_prev + dims;
873         r_u[0] = 0.0;
874         for (uint i = 1, i_prev = 0; i < points_offset_len; i++) {
875                 double length;
876
877 #ifdef USE_LENGTH_CACHE
878                 length = points_length_cache[i];
879
880                 assert(len_vnvn(pt, pt_prev, dims) == points_length_cache[i]);
881 #else
882                 length = len_vnvn(pt, pt_prev, dims);
883 #endif
884
885                 r_u[i] = r_u[i_prev] + length;
886                 i_prev = i;
887                 pt_prev = pt;
888                 pt += dims;
889         }
890         assert(!is_almost_zero(r_u[points_offset_len - 1]));
891         const double w = r_u[points_offset_len - 1];
892         for (uint i = 0; i < points_offset_len; i++) {
893                 r_u[i] /= w;
894         }
895         return w;
896 }
897
898 /**
899  * Use Newton-Raphson iteration to find better root.
900  *
901  * \param cubic: Current fitted curve.
902  * \param p: Point to test against.
903  * \param u: Parameter value for \a p.
904  *
905  * \note Return value may be `nan` caller must check for this.
906  */
907 static double cubic_find_root(
908         const Cubic *cubic,
909         const double p[],
910         const double u,
911         const uint dims)
912 {
913         /* Newton-Raphson Method. */
914         /* all vectors */
915 #ifdef USE_VLA
916         double q0_u[dims];
917         double q1_u[dims];
918         double q2_u[dims];
919 #else
920         double *q0_u = alloca(sizeof(double) * dims);
921         double *q1_u = alloca(sizeof(double) * dims);
922         double *q2_u = alloca(sizeof(double) * dims);
923 #endif
924
925         cubic_calc_point(cubic, u, dims, q0_u);
926         cubic_calc_speed(cubic, u, dims, q1_u);
927         cubic_calc_acceleration(cubic, u, dims, q2_u);
928
929         /* may divide-by-zero, caller must check for that case */
930         /* u - ((q0_u - p) * q1_u) / (q1_u.length_squared() + (q0_u - p) * q2_u) */
931         isub_vnvn(q0_u, p, dims);
932         return u - dot_vnvn(q0_u, q1_u, dims) /
933                (len_squared_vn(q1_u, dims) + dot_vnvn(q0_u, q2_u, dims));
934 }
935
936 static int compare_double_fn(const void *a_, const void *b_)
937 {
938         const double *a = a_;
939         const double *b = b_;
940         if      (*a > *b) return  1;
941         else if (*a < *b) return -1;
942         else              return  0;
943 }
944
945 /**
946  * Given set of points and their parameterization, try to find a better parameterization.
947  */
948 static bool cubic_reparameterize(
949         const Cubic *cubic,
950         const double *points_offset,
951         const uint    points_offset_len,
952         const double *u,
953         const uint    dims,
954
955         double       *r_u_prime)
956 {
957         /*
958          * Recalculate the values of u[] based on the Newton Raphson method
959          */
960
961         const double *pt = points_offset;
962         for (uint i = 0; i < points_offset_len; i++, pt += dims) {
963                 r_u_prime[i] = cubic_find_root(cubic, pt, u[i], dims);
964                 if (!isfinite(r_u_prime[i])) {
965                         return false;
966                 }
967         }
968
969         qsort(r_u_prime, points_offset_len, sizeof(double), compare_double_fn);
970
971         if ((r_u_prime[0] < 0.0) ||
972             (r_u_prime[points_offset_len - 1] > 1.0))
973         {
974                 return false;
975         }
976
977         assert(r_u_prime[0] >= 0.0);
978         assert(r_u_prime[points_offset_len - 1] <= 1.0);
979         return true;
980 }
981
982
983 static bool fit_cubic_to_points(
984         const double *points_offset,
985         const uint    points_offset_len,
986 #ifdef USE_LENGTH_CACHE
987         const double *points_length_cache,
988 #endif
989         const double  tan_l[],
990         const double  tan_r[],
991         const double  error_threshold_sq,
992         const uint    dims,
993
994         Cubic *r_cubic, double *r_error_max_sq, uint *r_split_index)
995 {
996         const uint iteration_max = 4;
997
998         if (points_offset_len == 2) {
999                 CUBIC_VARS(r_cubic, dims, p0, p1, p2, p3);
1000
1001                 copy_vnvn(p0, &points_offset[0 * dims], dims);
1002                 copy_vnvn(p3, &points_offset[1 * dims], dims);
1003
1004                 const double dist = len_vnvn(p0, p3, dims) / 3.0;
1005                 msub_vn_vnvn_fl(p1, p0, tan_l, dist, dims);
1006                 madd_vn_vnvn_fl(p2, p3, tan_r, dist, dims);
1007
1008 #ifdef USE_ORIG_INDEX_DATA
1009                 r_cubic->orig_span = 1;
1010 #endif
1011                 return true;
1012         }
1013
1014         double *u = malloc(sizeof(double) * points_offset_len);
1015
1016 #ifdef USE_CIRCULAR_FALLBACK
1017         const double points_offset_coords_length  =
1018 #endif
1019         points_calc_coord_length(
1020                 points_offset, points_offset_len, dims,
1021 #ifdef USE_LENGTH_CACHE
1022                 points_length_cache,
1023 #endif
1024                 u);
1025
1026         double error_max_sq;
1027         uint split_index;
1028
1029         /* Parameterize points, and attempt to fit curve */
1030         cubic_from_points(
1031                 points_offset, points_offset_len,
1032 #ifdef USE_CIRCULAR_FALLBACK
1033                 points_offset_coords_length,
1034 #endif
1035                 u, tan_l, tan_r, dims, r_cubic);
1036
1037         /* Find max deviation of points to fitted curve */
1038         error_max_sq = cubic_calc_error(
1039                 r_cubic, points_offset, points_offset_len, u, dims,
1040                 &split_index);
1041
1042         Cubic *cubic_test = alloca(cubic_alloc_size(dims));
1043
1044         /* Run this so we use the non-circular calculation when the circular-fallback
1045          * in 'cubic_from_points' failed to give a close enough result. */
1046 #ifdef USE_CIRCULAR_FALLBACK
1047         if (!(error_max_sq < error_threshold_sq)) {
1048                 /* Don't use the cubic calculated above, instead calculate a new fallback cubic,
1049                  * since this tends to give more balanced split_index along the curve.
1050                  * This is because the attempt to calcualte the cubic may contain spikes
1051                  * along the curve which may give a lop-sided maximum distance. */
1052                 cubic_from_points_fallback(
1053                         points_offset, points_offset_len,
1054                         tan_l, tan_r, dims, cubic_test);
1055                 const double error_max_sq_test = cubic_calc_error(
1056                         cubic_test, points_offset, points_offset_len, u, dims,
1057                         &split_index);
1058
1059                 /* intentionally use the newly calculated 'split_index',
1060                  * even if the 'error_max_sq_test' is worse. */
1061                 if (error_max_sq > error_max_sq_test) {
1062                         error_max_sq = error_max_sq_test;
1063                         cubic_copy(r_cubic, cubic_test, dims);
1064                 }
1065         }
1066 #endif
1067
1068         /* Test the offset fallback */
1069 #ifdef USE_OFFSET_FALLBACK
1070         if (!(error_max_sq < error_threshold_sq)) {
1071                 /* Using the offset from the curve to calculate cubic handle length may give better results
1072                  * try this as a second fallback. */
1073                 cubic_from_points_offset_fallback(
1074                         points_offset, points_offset_len,
1075                         tan_l, tan_r, dims, cubic_test);
1076                 const double error_max_sq_test = cubic_calc_error_simple(
1077                         cubic_test, points_offset, points_offset_len, u, error_max_sq, dims);
1078
1079                 if (error_max_sq > error_max_sq_test) {
1080                         error_max_sq = error_max_sq_test;
1081                         cubic_copy(r_cubic, cubic_test, dims);
1082                 }
1083         }
1084 #endif
1085
1086         *r_error_max_sq = error_max_sq;
1087         *r_split_index  = split_index;
1088
1089         if (!(error_max_sq < error_threshold_sq)) {
1090                 cubic_copy(cubic_test, r_cubic, dims);
1091
1092                 /* If error not too large, try some reparameterization and iteration */
1093                 double *u_prime = malloc(sizeof(double) * points_offset_len);
1094                 for (uint iter = 0; iter < iteration_max; iter++) {
1095                         if (!cubic_reparameterize(
1096                                 cubic_test, points_offset, points_offset_len, u, dims, u_prime))
1097                         {
1098                                 break;
1099                         }
1100
1101                         cubic_from_points(
1102                                 points_offset, points_offset_len,
1103 #ifdef USE_CIRCULAR_FALLBACK
1104                                 points_offset_coords_length,
1105 #endif
1106                                 u_prime, tan_l, tan_r, dims, cubic_test);
1107
1108                         const double error_max_sq_test = cubic_calc_error(
1109                                 cubic_test, points_offset, points_offset_len, u_prime, dims,
1110                                 &split_index);
1111
1112                         if (error_max_sq > error_max_sq_test) {
1113                                 error_max_sq = error_max_sq_test;
1114                                 cubic_copy(r_cubic, cubic_test, dims);
1115                                 *r_error_max_sq = error_max_sq;
1116                                 *r_split_index = split_index;
1117                         }
1118
1119                         if (!(error_max_sq < error_threshold_sq)) {
1120                                 /* continue */
1121                         }
1122                         else {
1123                                 assert((error_max_sq < error_threshold_sq));
1124                                 free(u_prime);
1125                                 free(u);
1126                                 return true;
1127                         }
1128
1129                         SWAP(double *, u, u_prime);
1130                 }
1131                 free(u_prime);
1132                 free(u);
1133
1134                 return false;
1135         }
1136         else {
1137                 free(u);
1138                 return true;
1139         }
1140 }
1141
1142 static void fit_cubic_to_points_recursive(
1143         const double *points_offset,
1144         const uint    points_offset_len,
1145 #ifdef USE_LENGTH_CACHE
1146         const double *points_length_cache,
1147 #endif
1148         const double  tan_l[],
1149         const double  tan_r[],
1150         const double  error_threshold_sq,
1151         const uint    calc_flag,
1152         const uint    dims,
1153         /* fill in the list */
1154         CubicList *clist)
1155 {
1156         Cubic *cubic = cubic_alloc(dims);
1157         uint split_index;
1158         double error_max_sq;
1159
1160         if (fit_cubic_to_points(
1161                 points_offset, points_offset_len,
1162 #ifdef USE_LENGTH_CACHE
1163                 points_length_cache,
1164 #endif
1165                 tan_l, tan_r,
1166                 (calc_flag & CURVE_FIT_CALC_HIGH_QUALIY) ? DBL_EPSILON : error_threshold_sq,
1167                 dims,
1168                 cubic, &error_max_sq, &split_index) ||
1169             (error_max_sq < error_threshold_sq))
1170         {
1171                 cubic_list_prepend(clist, cubic);
1172                 return;
1173         }
1174         cubic_free(cubic);
1175
1176
1177         /* Fitting failed -- split at max error point and fit recursively */
1178
1179         /* Check splinePoint is not an endpoint?
1180          *
1181          * This assert happens sometimes...
1182          * Look into it but disable for now. Campbell! */
1183
1184         // assert(split_index > 1)
1185 #ifdef USE_VLA
1186         double tan_center[dims];
1187 #else
1188         double *tan_center = alloca(sizeof(double) * dims);
1189 #endif
1190
1191         const double *pt_a = &points_offset[(split_index - 1) * dims];
1192         const double *pt_b = &points_offset[(split_index + 1) * dims];
1193
1194         assert(split_index < points_offset_len);
1195         if (equals_vnvn(pt_a, pt_b, dims)) {
1196                 pt_a += dims;
1197         }
1198
1199         {
1200 #ifdef USE_VLA
1201                 double tan_center_a[dims];
1202                 double tan_center_b[dims];
1203 #else
1204                 double *tan_center_a = alloca(sizeof(double) * dims);
1205                 double *tan_center_b = alloca(sizeof(double) * dims);
1206 #endif
1207                 const double *pt   = &points_offset[split_index * dims];
1208
1209                 /* tan_center = ((pt_a - pt).normalized() + (pt - pt_b).normalized()).normalized() */
1210                 normalize_vn_vnvn(tan_center_a, pt_a, pt, dims);
1211                 normalize_vn_vnvn(tan_center_b, pt, pt_b, dims);
1212                 add_vn_vnvn(tan_center, tan_center_a, tan_center_b, dims);
1213                 normalize_vn(tan_center, dims);
1214         }
1215
1216         fit_cubic_to_points_recursive(
1217                 points_offset, split_index + 1,
1218 #ifdef USE_LENGTH_CACHE
1219                 points_length_cache,
1220 #endif
1221                 tan_l, tan_center, error_threshold_sq, calc_flag, dims, clist);
1222         fit_cubic_to_points_recursive(
1223                 &points_offset[split_index * dims], points_offset_len - split_index,
1224 #ifdef USE_LENGTH_CACHE
1225                 points_length_cache + split_index,
1226 #endif
1227                 tan_center, tan_r, error_threshold_sq, calc_flag, dims, clist);
1228
1229 }
1230
1231 /** \} */
1232
1233
1234 /* -------------------------------------------------------------------- */
1235
1236 /** \name External API for Curve-Fitting
1237  * \{ */
1238
1239 /**
1240  * Main function:
1241  *
1242  * Take an array of 3d points.
1243  * return the cubic splines
1244  */
1245 int curve_fit_cubic_to_points_db(
1246         const double *points,
1247         const uint    points_len,
1248         const uint    dims,
1249         const double  error_threshold,
1250         const uint    calc_flag,
1251         const uint   *corners,
1252         uint          corners_len,
1253
1254         double **r_cubic_array, uint *r_cubic_array_len,
1255         uint **r_cubic_orig_index,
1256         uint **r_corner_index_array, uint *r_corner_index_len)
1257 {
1258         uint corners_buf[2];
1259         if (corners == NULL) {
1260                 assert(corners_len == 0);
1261                 corners_buf[0] = 0;
1262                 corners_buf[1] = points_len - 1;
1263                 corners = corners_buf;
1264                 corners_len = 2;
1265         }
1266
1267         CubicList clist = {0};
1268         clist.dims = dims;
1269
1270 #ifdef USE_VLA
1271         double tan_l[dims];
1272         double tan_r[dims];
1273 #else
1274         double *tan_l = alloca(sizeof(double) * dims);
1275         double *tan_r = alloca(sizeof(double) * dims);
1276 #endif
1277
1278 #ifdef USE_LENGTH_CACHE
1279         double *points_length_cache = NULL;
1280         uint    points_length_cache_len_alloc = 0;
1281 #endif
1282
1283         uint *corner_index_array = NULL;
1284         uint  corner_index = 0;
1285         if (r_corner_index_array && (corners != corners_buf)) {
1286                 corner_index_array = malloc(sizeof(uint) * corners_len);
1287                 corner_index_array[corner_index++] = corners[0];
1288         }
1289
1290         const double error_threshold_sq = sq(error_threshold);
1291
1292         for (uint i = 1; i < corners_len; i++) {
1293                 const uint points_offset_len = corners[i] - corners[i - 1] + 1;
1294                 const uint first_point = corners[i - 1];
1295
1296                 assert(points_offset_len >= 1);
1297                 if (points_offset_len > 1) {
1298                         const double *pt_l = &points[first_point * dims];
1299                         const double *pt_r = &points[(first_point + points_offset_len - 1) * dims];
1300                         const double *pt_l_next = pt_l + dims;
1301                         const double *pt_r_prev = pt_r - dims;
1302
1303                         /* tan_l = (pt_l - pt_l_next).normalized()
1304                          * tan_r = (pt_r_prev - pt_r).normalized()
1305                          */
1306                         normalize_vn_vnvn(tan_l, pt_l, pt_l_next, dims);
1307                         normalize_vn_vnvn(tan_r, pt_r_prev, pt_r, dims);
1308
1309 #ifdef USE_LENGTH_CACHE
1310                         if (points_length_cache_len_alloc < points_offset_len) {
1311                                 if (points_length_cache) {
1312                                         free(points_length_cache);
1313                                 }
1314                                 points_length_cache = malloc(sizeof(double) * points_offset_len);
1315                         }
1316                         points_calc_coord_length_cache(
1317                                 &points[first_point * dims], points_offset_len, dims,
1318                                 points_length_cache);
1319 #endif
1320
1321                         fit_cubic_to_points_recursive(
1322                                 &points[first_point * dims], points_offset_len,
1323 #ifdef USE_LENGTH_CACHE
1324                                 points_length_cache,
1325 #endif
1326                                 tan_l, tan_r, error_threshold_sq, calc_flag, dims, &clist);
1327                 }
1328                 else if (points_len == 1) {
1329                         assert(points_offset_len == 1);
1330                         assert(corners_len == 2);
1331                         assert(corners[0] == 0);
1332                         assert(corners[1] == 0);
1333                         const double *pt = &points[0];
1334                         Cubic *cubic = cubic_alloc(dims);
1335                         cubic_init(cubic, pt, pt, pt, pt, dims);
1336                         cubic_list_prepend(&clist, cubic);
1337                 }
1338
1339                 if (corner_index_array) {
1340                         corner_index_array[corner_index++] = clist.len;
1341                 }
1342         }
1343
1344 #ifdef USE_LENGTH_CACHE
1345         if (points_length_cache) {
1346                 free(points_length_cache);
1347         }
1348 #endif
1349
1350 #ifdef USE_ORIG_INDEX_DATA
1351         uint *cubic_orig_index = NULL;
1352         if (r_cubic_orig_index) {
1353                 cubic_orig_index = malloc(sizeof(uint) * (clist.len + 1));
1354         }
1355 #else
1356         *r_cubic_orig_index = NULL;
1357 #endif
1358
1359         /* allocate a contiguous array and free the linked list */
1360         *r_cubic_array = cubic_list_as_array(
1361                 &clist
1362 #ifdef USE_ORIG_INDEX_DATA
1363                 , corners[corners_len - 1], cubic_orig_index
1364 #endif
1365                 );
1366         *r_cubic_array_len = clist.len + 1;
1367
1368         cubic_list_clear(&clist);
1369
1370 #ifdef USE_ORIG_INDEX_DATA
1371         if (cubic_orig_index) {
1372                 *r_cubic_orig_index = cubic_orig_index;
1373         }
1374 #endif
1375
1376         if (corner_index_array) {
1377                 assert(corner_index == corners_len);
1378                 *r_corner_index_array = corner_index_array;
1379                 *r_corner_index_len = corner_index;
1380         }
1381
1382         return 0;
1383 }
1384
1385 /**
1386  * A version of #curve_fit_cubic_to_points_db to handle floats
1387  */
1388 int curve_fit_cubic_to_points_fl(
1389         const float  *points,
1390         const uint    points_len,
1391         const uint    dims,
1392         const float   error_threshold,
1393         const uint    calc_flag,
1394         const uint   *corners,
1395         const uint    corners_len,
1396
1397         float **r_cubic_array, uint *r_cubic_array_len,
1398         uint **r_cubic_orig_index,
1399         uint **r_corner_index_array, uint *r_corner_index_len)
1400 {
1401         const uint points_flat_len = points_len * dims;
1402         double *points_db = malloc(sizeof(double) * points_flat_len);
1403
1404         copy_vndb_vnfl(points_db, points, points_flat_len);
1405
1406         double *cubic_array_db = NULL;
1407         float  *cubic_array_fl = NULL;
1408         uint    cubic_array_len = 0;
1409
1410         int result = curve_fit_cubic_to_points_db(
1411                 points_db, points_len, dims, error_threshold, calc_flag, corners, corners_len,
1412                 &cubic_array_db, &cubic_array_len,
1413                 r_cubic_orig_index,
1414                 r_corner_index_array, r_corner_index_len);
1415         free(points_db);
1416
1417         if (!result) {
1418                 uint cubic_array_flat_len = cubic_array_len * 3 * dims;
1419                 cubic_array_fl = malloc(sizeof(float) * cubic_array_flat_len);
1420                 for (uint i = 0; i < cubic_array_flat_len; i++) {
1421                         cubic_array_fl[i] = (float)cubic_array_db[i];
1422                 }
1423                 free(cubic_array_db);
1424         }
1425
1426         *r_cubic_array = cubic_array_fl;
1427         *r_cubic_array_len = cubic_array_len;
1428
1429         return result;
1430 }
1431
1432 /**
1433  * Fit a single cubic to points.
1434  */
1435 int curve_fit_cubic_to_points_single_db(
1436         const double *points,
1437         const uint    points_len,
1438         const uint    dims,
1439         const double  error_threshold,
1440         const double tan_l[],
1441         const double tan_r[],
1442
1443         double  r_handle_l[],
1444         double  r_handle_r[],
1445         double  *r_error_max_sq)
1446 {
1447         Cubic *cubic = alloca(cubic_alloc_size(dims));
1448
1449         uint split_index;
1450
1451         /* in this instance theres no advantage in using length cache,
1452          * since we're not recursively calculating values. */
1453 #ifdef USE_LENGTH_CACHE
1454         double *points_length_cache = malloc(sizeof(double) * points_len);
1455         points_calc_coord_length_cache(
1456                 points, points_len, dims,
1457                 points_length_cache);
1458 #endif
1459
1460         fit_cubic_to_points(
1461                 points, points_len,
1462 #ifdef USE_LENGTH_CACHE
1463                 points_length_cache,
1464 #endif
1465                 tan_l, tan_r, error_threshold, dims,
1466
1467                 cubic, r_error_max_sq, &split_index);
1468
1469 #ifdef USE_LENGTH_CACHE
1470         free(points_length_cache);
1471 #endif
1472
1473         copy_vnvn(r_handle_l, CUBIC_PT(cubic, 1, dims), dims);
1474         copy_vnvn(r_handle_r, CUBIC_PT(cubic, 2, dims), dims);
1475
1476         return 0;
1477 }
1478
1479 int curve_fit_cubic_to_points_single_fl(
1480         const float  *points,
1481         const uint    points_len,
1482         const uint    dims,
1483         const float   error_threshold,
1484         const float   tan_l[],
1485         const float   tan_r[],
1486
1487         float   r_handle_l[],
1488         float   r_handle_r[],
1489         float  *r_error_sq)
1490 {
1491         const uint points_flat_len = points_len * dims;
1492         double *points_db = malloc(sizeof(double) * points_flat_len);
1493
1494         copy_vndb_vnfl(points_db, points, points_flat_len);
1495
1496 #ifdef USE_VLA
1497         double tan_l_db[dims];
1498         double tan_r_db[dims];
1499         double r_handle_l_db[dims];
1500         double r_handle_r_db[dims];
1501 #else
1502         double *tan_l_db = alloca(sizeof(double) * dims);
1503         double *tan_r_db = alloca(sizeof(double) * dims);
1504         double *r_handle_l_db = alloca(sizeof(double) * dims);
1505         double *r_handle_r_db = alloca(sizeof(double) * dims);
1506 #endif
1507         double r_error_sq_db;
1508
1509         copy_vndb_vnfl(tan_l_db, tan_l, dims);
1510         copy_vndb_vnfl(tan_r_db, tan_r, dims);
1511
1512         int result = curve_fit_cubic_to_points_single_db(
1513                 points_db, points_len, dims,
1514                 (double)error_threshold,
1515                 tan_l_db, tan_r_db,
1516                 r_handle_l_db, r_handle_r_db,
1517                 &r_error_sq_db);
1518
1519         free(points_db);
1520
1521         copy_vnfl_vndb(r_handle_l, r_handle_l_db, dims);
1522         copy_vnfl_vndb(r_handle_r, r_handle_r_db, dims);
1523         *r_error_sq = (float)r_error_sq_db;
1524
1525         return result;
1526 }
1527
1528 /** \} */