Curve Fitting: minor change to re-fitting method
[blender.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_calc_point(
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_speed(
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] =  3.0 * ((p1[j] - p0[j]) * s * s + 2.0 *
282                                  (p2[j] - p0[j]) * s * t +
283                                  (p3[j] - p2[j]) * t * t);
284         }
285 }
286
287 static void cubic_calc_acceleration(
288         const Cubic *cubic, const double t, const uint dims,
289         double r_v[])
290 {
291         CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
292         const double s = 1.0 - t;
293         for (uint j = 0; j < dims; j++) {
294                 r_v[j] = 6.0 * ((p2[j] - 2.0 * p1[j] + p0[j]) * s +
295                                 (p3[j] - 2.0 * p2[j] + p1[j]) * t);
296         }
297 }
298
299 /**
300  * Returns a 'measure' of the maximum distance (squared) of the points specified
301  * by points_offset from the corresponding cubic(u[]) points.
302  */
303 static double cubic_calc_error(
304         const Cubic *cubic,
305         const double *points_offset,
306         const uint points_offset_len,
307         const double *u,
308         const uint dims,
309
310         uint *r_error_index)
311 {
312         double error_max_sq = 0.0;
313         uint   error_index = 0;
314
315         const double *pt_real = points_offset + dims;
316 #ifdef USE_VLA
317         double        pt_eval[dims];
318 #else
319         double       *pt_eval = alloca(sizeof(double) * dims);
320 #endif
321
322         for (uint i = 1; i < points_offset_len - 1; i++, pt_real += dims) {
323                 cubic_calc_point(cubic, u[i], dims, pt_eval);
324
325                 const double err_sq = len_squared_vnvn(pt_real, pt_eval, dims);
326                 if (err_sq >= error_max_sq) {
327                         error_max_sq = err_sq;
328                         error_index = i;
329                 }
330         }
331
332         *r_error_index = error_index;
333         return error_max_sq;
334 }
335
336 #ifdef USE_OFFSET_FALLBACK
337 /**
338  * A version #cubic_calc_error where we don't need the split-index and can exit early when over the limit.
339  */
340 static double cubic_calc_error_simple(
341         const Cubic *cubic,
342         const double *points_offset,
343         const uint points_offset_len,
344         const double *u,
345         const double error_threshold_sq,
346         const uint dims)
347
348 {
349         double error_max_sq = 0.0;
350
351         const double *pt_real = points_offset + dims;
352 #ifdef USE_VLA
353         double        pt_eval[dims];
354 #else
355         double       *pt_eval = alloca(sizeof(double) * dims);
356 #endif
357
358         for (uint i = 1; i < points_offset_len - 1; i++, pt_real += dims) {
359                 cubic_calc_point(cubic, u[i], dims, pt_eval);
360
361                 const double err_sq = len_squared_vnvn(pt_real, pt_eval, dims);
362                 if (err_sq >= error_threshold_sq) {
363                         return error_threshold_sq;
364                 }
365                 else if (err_sq >= error_max_sq) {
366                         error_max_sq = err_sq;
367                 }
368         }
369
370         return error_max_sq;
371 }
372 #endif
373
374 /**
375  * Bezier multipliers
376  */
377
378 static double B1(double u)
379 {
380         double tmp = 1.0 - u;
381         return 3.0 * u * tmp * tmp;
382 }
383
384 static double B2(double u)
385 {
386         return 3.0 * u * u * (1.0 - u);
387 }
388
389 static double B0plusB1(double u)
390 {
391     double tmp = 1.0 - u;
392     return tmp * tmp * (1.0 + 2.0 * u);
393 }
394
395 static double B2plusB3(double u)
396 {
397     return u * u * (3.0 - 2.0 * u);
398 }
399
400 static void points_calc_center_weighted(
401         const double *points_offset,
402         const uint    points_offset_len,
403         const uint    dims,
404
405         double r_center[])
406 {
407         /*
408          * Calculate a center that compensates for point spacing.
409          */
410
411         const double *pt_prev = &points_offset[(points_offset_len - 2) * dims];
412         const double *pt_curr = pt_prev + dims;
413         const double *pt_next = points_offset;
414
415         double w_prev = len_vnvn(pt_prev, pt_curr, dims);
416
417         zero_vn(r_center, dims);
418         double w_tot = 0.0;
419
420         for (uint i_next = 0; i_next < points_offset_len; i_next++) {
421                 const double w_next = len_vnvn(pt_curr, pt_next, dims);
422                 const double w = w_prev + w_next;
423                 w_tot += w;
424
425                 miadd_vn_vn_fl(r_center, pt_curr, w, dims);
426
427                 w_prev = w_next;
428
429                 pt_prev = pt_curr;
430                 pt_curr = pt_next;
431                 pt_next += dims;
432         }
433
434         if (w_tot != 0.0) {
435                 imul_vn_fl(r_center, 1.0 / w_tot, dims);
436         }
437 }
438
439 #ifdef USE_CIRCULAR_FALLBACK
440
441 /**
442  * Return a scale value, used to calculate how much the curve handles should be increased,
443  *
444  * This works by placing each end-point on an imaginary circle,
445  * the placement on the circle is based on the tangent vectors,
446  * where larger differences in tangent angle cover a larger part of the circle.
447  *
448  * Return the scale representing how much larger the distance around the circle is.
449  */
450 static double points_calc_circumference_factor(
451         const double  tan_l[],
452         const double  tan_r[],
453         const uint dims)
454 {
455         const double dot = dot_vnvn(tan_l, tan_r, dims);
456         const double len_tangent = dot < 0.0 ? len_vnvn(tan_l, tan_r, dims) : len_negated_vnvn(tan_l, tan_r, dims);
457         if (len_tangent > DBL_EPSILON) {
458                 /* only clamp to avoid precision error */
459                 double angle = acos(max(-fabs(dot), -1.0));
460                 /* Angle may be less than the length when the tangents define >180 degrees of the circle,
461                  * (tangents that point away from each other).
462                  * We could try support this but will likely cause extreme >1 scales which could cause other issues. */
463                 // assert(angle >= len_tangent);
464                 double factor = (angle / len_tangent);
465                 assert(factor < (M_PI / 2) + (DBL_EPSILON * 10));
466                 return factor;
467         }
468         else {
469                 /* tangents are exactly aligned (think two opposite sides of a circle). */
470                 return (M_PI / 2);
471         }
472 }
473
474 /**
475  * Return the value which the distance between points will need to be scaled by,
476  * to define a handle, given both points are on a perfect circle.
477  *
478  * \note the return value will need to be multiplied by 1.3... for correct results.
479  */
480 static double points_calc_circle_tangent_factor(
481         const double  tan_l[],
482         const double  tan_r[],
483         const uint dims)
484 {
485         const double eps = 1e-8;
486         const double tan_dot = dot_vnvn(tan_l, tan_r, dims);
487         if (tan_dot > 1.0 - eps) {
488                 /* no angle difference (use fallback, length wont make any difference) */
489                 return (1.0 / 3.0) * 0.75;
490         }
491         else if (tan_dot < -1.0 + eps) {
492                 /* parallel tangents (half-circle) */
493                 return (1.0 / 2.0);
494         }
495         else {
496                 /* non-aligned tangents, calculate handle length */
497                 const double angle = acos(tan_dot) / 2.0;
498
499                 /* could also use 'angle_sin = len_vnvn(tan_l, tan_r, dims) / 2.0' */
500                 const double angle_sin = sin(angle);
501                 const double angle_cos = cos(angle);
502                 return ((1.0 - angle_cos) / (angle_sin * 2.0)) / angle_sin;
503         }
504 }
505
506 /**
507  * Calculate the scale the handles, which serves as a best-guess
508  * used as a fallback when the least-square solution fails.
509  */
510 static double points_calc_cubic_scale(
511         const double v_l[], const double v_r[],
512         const double  tan_l[],
513         const double  tan_r[],
514         const double coords_length, uint dims)
515 {
516         const double len_direct = len_vnvn(v_l, v_r, dims);
517         const double len_circle_factor = points_calc_circle_tangent_factor(tan_l, tan_r, dims);
518
519         /* if this curve is a circle, this value doesn't need modification */
520         const double len_circle_handle = (len_direct * (len_circle_factor / 0.75));
521
522         /* scale by the difference from the circumference distance */
523         const double len_circle = len_direct * points_calc_circumference_factor(tan_l, tan_r, dims);
524         double scale_handle = (coords_length / len_circle);
525
526         /* Could investigate an accurate calculation here,
527          * though this gives close results */
528         scale_handle = ((scale_handle - 1.0) * 1.75) + 1.0;
529
530         return len_circle_handle * scale_handle;
531 }
532
533 static void cubic_from_points_fallback(
534         const double *points_offset,
535         const uint    points_offset_len,
536         const double  tan_l[],
537         const double  tan_r[],
538         const uint dims,
539
540         Cubic *r_cubic)
541 {
542         const double *p0 = &points_offset[0];
543         const double *p3 = &points_offset[(points_offset_len - 1) * dims];
544
545         double alpha = len_vnvn(p0, p3, dims) / 3.0;
546
547         double *p1 = CUBIC_PT(r_cubic, 1, dims);
548         double *p2 = CUBIC_PT(r_cubic, 2, dims);
549
550         copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims);
551         copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims);
552
553 #ifdef USE_ORIG_INDEX_DATA
554         r_cubic->orig_span = (points_offset_len - 1);
555 #endif
556
557         /* p1 = p0 - (tan_l * alpha);
558          * p2 = p3 + (tan_r * alpha);
559          */
560         msub_vn_vnvn_fl(p1, p0, tan_l, alpha, dims);
561         madd_vn_vnvn_fl(p2, p3, tan_r, alpha, dims);
562 }
563 #endif  /* USE_CIRCULAR_FALLBACK */
564
565
566 #ifdef USE_OFFSET_FALLBACK
567
568 static void cubic_from_points_offset_fallback(
569         const double *points_offset,
570         const uint    points_offset_len,
571         const double  tan_l[],
572         const double  tan_r[],
573         const uint dims,
574
575         Cubic *r_cubic)
576 {
577         const double *p0 = &points_offset[0];
578         const double *p3 = &points_offset[(points_offset_len - 1) * dims];
579
580 #ifdef USE_VLA
581         double dir_unit[dims];
582         double a[2][dims];
583         double tmp[dims];
584 #else
585         double *dir_unit = alloca(sizeof(double) * dims);
586         double *a[2] = {
587             alloca(sizeof(double) * dims),
588             alloca(sizeof(double) * dims),
589         };
590         double *tmp = alloca(sizeof(double) * dims);
591 #endif
592
593         const double dir_dist = normalize_vn_vnvn(dir_unit, p3, p0, dims);
594         project_plane_vn_vnvn_normalized(a[0], tan_l, dir_unit, dims);
595         project_plane_vn_vnvn_normalized(a[1], tan_r, dir_unit, dims);
596
597         /* only for better accuracy, not essential */
598         normalize_vn(a[0], dims);
599         normalize_vn(a[1], dims);
600
601         mul_vnvn_fl(a[1], a[1], -1, dims);
602
603         double dists[2] = {0, 0};
604
605         const double *pt = &points_offset[dims];
606         for (uint i = 1; i < points_offset_len - 1; i++, pt += dims) {
607                 for (uint k = 0; k < 2; k++) {
608                         sub_vn_vnvn(tmp, p0, pt, dims);
609                         project_vn_vnvn_normalized(tmp, tmp, a[k], dims);
610                         dists[k] = max(dists[k], dot_vnvn(tmp, a[k], dims));
611                 }
612         }
613
614         double alpha_l = (dists[0] / 0.75) / fabs(dot_vnvn(tan_l, a[0], dims));
615         double alpha_r = (dists[1] / 0.75) / fabs(dot_vnvn(tan_r, a[1], dims));
616
617         if (!(alpha_l > 0.0)) {
618                 alpha_l = dir_dist / 3.0;
619         }
620         if (!(alpha_r > 0.0)) {
621                 alpha_r = dir_dist / 3.0;
622         }
623
624         double *p1 = CUBIC_PT(r_cubic, 1, dims);
625         double *p2 = CUBIC_PT(r_cubic, 2, dims);
626
627         copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims);
628         copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims);
629
630 #ifdef USE_ORIG_INDEX_DATA
631         r_cubic->orig_span = (points_offset_len - 1);
632 #endif
633
634         /* p1 = p0 - (tan_l * alpha_l);
635          * p2 = p3 + (tan_r * alpha_r);
636          */
637         msub_vn_vnvn_fl(p1, p0, tan_l, alpha_l, dims);
638         madd_vn_vnvn_fl(p2, p3, tan_r, alpha_r, dims);
639 }
640
641 #endif  /* USE_OFFSET_FALLBACK */
642
643
644 /**
645  * Use least-squares method to find Bezier control points for region.
646  */
647 static void cubic_from_points(
648         const double *points_offset,
649         const uint    points_offset_len,
650 #ifdef USE_CIRCULAR_FALLBACK
651         const double  points_offset_coords_length,
652 #endif
653         const double *u_prime,
654         const double  tan_l[],
655         const double  tan_r[],
656         const uint dims,
657
658         Cubic *r_cubic)
659 {
660
661         const double *p0 = &points_offset[0];
662         const double *p3 = &points_offset[(points_offset_len - 1) * dims];
663
664         /* Point Pairs */
665         double alpha_l, alpha_r;
666 #ifdef USE_VLA
667         double a[2][dims];
668 #else
669         double *a[2] = {
670             alloca(sizeof(double) * dims),
671             alloca(sizeof(double) * dims),
672         };
673 #endif
674
675         {
676                 double x[2] = {0.0}, c[2][2] = {{0.0}};
677                 const double *pt = points_offset;
678
679                 for (uint i = 0; i < points_offset_len; i++, pt += dims) {
680                         mul_vnvn_fl(a[0], tan_l, B1(u_prime[i]), dims);
681                         mul_vnvn_fl(a[1], tan_r, B2(u_prime[i]), dims);
682
683                         const double b0_plus_b1 = B0plusB1(u_prime[i]);
684                         const double b2_plus_b3 = B2plusB3(u_prime[i]);
685
686                         /* inline dot product */
687                         for (uint j = 0; j < dims; j++) {
688                                 const double tmp = (pt[j] - (p0[j] * b0_plus_b1)) + (p3[j] * b2_plus_b3);
689
690                                 x[0] += a[0][j] * tmp;
691                                 x[1] += a[1][j] * tmp;
692
693                                 c[0][0] += a[0][j] * a[0][j];
694                                 c[0][1] += a[0][j] * a[1][j];
695                                 c[1][1] += a[1][j] * a[1][j];
696                         }
697
698                         c[1][0] = c[0][1];
699                 }
700
701                 double det_C0_C1 = c[0][0] * c[1][1] - c[0][1] * c[1][0];
702                 double det_C_0X  = x[1]    * c[0][0] - x[0]    * c[0][1];
703                 double det_X_C1  = x[0]    * c[1][1] - x[1]    * c[0][1];
704
705                 if (is_almost_zero(det_C0_C1)) {
706                         det_C0_C1 = c[0][0] * c[1][1] * 10e-12;
707                 }
708
709                 /* may still divide-by-zero, check below will catch nan values */
710                 alpha_l = det_X_C1 / det_C0_C1;
711                 alpha_r = det_C_0X / det_C0_C1;
712         }
713
714         /*
715          * The problem that the stupid values for alpha dare not put
716          * only when we realize that the sign and wrong,
717          * but even if the values are too high.
718          * But how do you evaluate it?
719          *
720          * Meanwhile, we should ensure that these values are sometimes
721          * so only problems absurd of approximation and not for bugs in the code.
722          */
723
724         bool use_clamp = true;
725
726         /* flip check to catch nan values */
727         if (!(alpha_l >= 0.0) ||
728             !(alpha_r >= 0.0))
729         {
730 #ifdef USE_CIRCULAR_FALLBACK
731                 double alpha_test = points_calc_cubic_scale(p0, p3, tan_l, tan_r, points_offset_coords_length, dims);
732                 if (!isfinite(alpha_test)) {
733                         alpha_test = len_vnvn(p0, p3, dims) / 3.0;
734                 }
735                 alpha_l = alpha_r = alpha_test;
736 #else
737                 alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
738 #endif
739
740                 /* skip clamping when we're using default handles */
741                 use_clamp = false;
742         }
743
744         double *p1 = CUBIC_PT(r_cubic, 1, dims);
745         double *p2 = CUBIC_PT(r_cubic, 2, dims);
746
747         copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims);
748         copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims);
749
750 #ifdef USE_ORIG_INDEX_DATA
751         r_cubic->orig_span = (points_offset_len - 1);
752 #endif
753
754         /* p1 = p0 - (tan_l * alpha_l);
755          * p2 = p3 + (tan_r * alpha_r);
756          */
757         msub_vn_vnvn_fl(p1, p0, tan_l, alpha_l, dims);
758         madd_vn_vnvn_fl(p2, p3, tan_r, alpha_r, dims);
759
760         /* ------------------------------------
761          * Clamping (we could make it optional)
762          */
763         if (use_clamp) {
764 #ifdef USE_VLA
765                 double center[dims];
766 #else
767                 double *center = alloca(sizeof(double) * dims);
768 #endif
769                 points_calc_center_weighted(points_offset, points_offset_len, dims, center);
770
771                 const double clamp_scale = 3.0;  /* clamp to 3x */
772                 double dist_sq_max = 0.0;
773
774                 {
775                         const double *pt = points_offset;
776                         for (uint i = 0; i < points_offset_len; i++, pt += dims) {
777 #if 0
778                                 double dist_sq_test = sq(len_vnvn(center, pt, dims) * clamp_scale);
779 #else
780                                 /* do inline */
781                                 double dist_sq_test = 0.0;
782                                 for (uint j = 0; j < dims; j++) {
783                                         dist_sq_test += sq((pt[j] - center[j]) * clamp_scale);
784                                 }
785 #endif
786                                 dist_sq_max = max(dist_sq_max, dist_sq_test);
787                         }
788                 }
789
790                 double p1_dist_sq = len_squared_vnvn(center, p1, dims);
791                 double p2_dist_sq = len_squared_vnvn(center, p2, dims);
792
793                 if (p1_dist_sq > dist_sq_max ||
794                     p2_dist_sq > dist_sq_max)
795                 {
796 #ifdef USE_CIRCULAR_FALLBACK
797                         double alpha_test = points_calc_cubic_scale(p0, p3, tan_l, tan_r, points_offset_coords_length, dims);
798                         if (!isfinite(alpha_test)) {
799                                 alpha_test = len_vnvn(p0, p3, dims) / 3.0;
800                         }
801                         alpha_l = alpha_r = alpha_test;
802 #else
803                         alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
804 #endif
805
806                         /*
807                          * p1 = p0 - (tan_l * alpha_l);
808                          * p2 = p3 + (tan_r * alpha_r);
809                          */
810                         for (uint j = 0; j < dims; j++) {
811                                 p1[j] = p0[j] - (tan_l[j] * alpha_l);
812                                 p2[j] = p3[j] + (tan_r[j] * alpha_r);
813                         }
814
815                         p1_dist_sq = len_squared_vnvn(center, p1, dims);
816                         p2_dist_sq = len_squared_vnvn(center, p2, dims);
817                 }
818
819                 /* clamp within the 3x radius */
820                 if (p1_dist_sq > dist_sq_max) {
821                         isub_vnvn(p1, center, dims);
822                         imul_vn_fl(p1, sqrt(dist_sq_max) / sqrt(p1_dist_sq), dims);
823                         iadd_vnvn(p1, center, dims);
824                 }
825                 if (p2_dist_sq > dist_sq_max) {
826                         isub_vnvn(p2, center, dims);
827                         imul_vn_fl(p2, sqrt(dist_sq_max) / sqrt(p2_dist_sq), dims);
828                         iadd_vnvn(p2, center, dims);
829                 }
830         }
831         /* end clamping */
832 }
833
834 #ifdef USE_LENGTH_CACHE
835 static void points_calc_coord_length_cache(
836         const double *points_offset,
837         const uint    points_offset_len,
838         const uint    dims,
839
840         double     *r_points_length_cache)
841 {
842         const double *pt_prev = points_offset;
843         const double *pt = pt_prev + dims;
844         r_points_length_cache[0] = 0.0;
845         for (uint i = 1; i < points_offset_len; i++) {
846                 r_points_length_cache[i] = len_vnvn(pt, pt_prev, dims);
847                 pt_prev = pt;
848                 pt += dims;
849         }
850 }
851 #endif  /* USE_LENGTH_CACHE */
852
853 /**
854  * \return the accumulated length of \a points_offset.
855  */
856 static double points_calc_coord_length(
857         const double *points_offset,
858         const uint    points_offset_len,
859         const uint    dims,
860 #ifdef USE_LENGTH_CACHE
861         const double *points_length_cache,
862 #endif
863         double *r_u)
864 {
865         const double *pt_prev = points_offset;
866         const double *pt = pt_prev + dims;
867         r_u[0] = 0.0;
868         for (uint i = 1, i_prev = 0; i < points_offset_len; i++) {
869                 double length;
870
871 #ifdef USE_LENGTH_CACHE
872                 length = points_length_cache[i];
873                 assert(len_vnvn(pt, pt_prev, dims) == points_length_cache[i]);
874 #else
875                 length = len_vnvn(pt, pt_prev, dims);
876 #endif
877
878                 r_u[i] = r_u[i_prev] + length;
879                 i_prev = i;
880                 pt_prev = pt;
881                 pt += dims;
882         }
883         assert(!is_almost_zero(r_u[points_offset_len - 1]));
884         const double w = r_u[points_offset_len - 1];
885         for (uint i = 1; i < points_offset_len; i++) {
886                 r_u[i] /= w;
887         }
888         return w;
889 }
890
891 /**
892  * Use Newton-Raphson iteration to find better root.
893  *
894  * \param cubic: Current fitted curve.
895  * \param p: Point to test against.
896  * \param u: Parameter value for \a p.
897  *
898  * \note Return value may be `nan` caller must check for this.
899  */
900 static double cubic_find_root(
901         const Cubic *cubic,
902         const double p[],
903         const double u,
904         const uint dims)
905 {
906         /* Newton-Raphson Method. */
907         /* all vectors */
908 #ifdef USE_VLA
909         double q0_u[dims];
910         double q1_u[dims];
911         double q2_u[dims];
912 #else
913         double *q0_u = alloca(sizeof(double) * dims);
914         double *q1_u = alloca(sizeof(double) * dims);
915         double *q2_u = alloca(sizeof(double) * dims);
916 #endif
917
918         cubic_calc_point(cubic, u, dims, q0_u);
919         cubic_calc_speed(cubic, u, dims, q1_u);
920         cubic_calc_acceleration(cubic, u, dims, q2_u);
921
922         /* may divide-by-zero, caller must check for that case */
923         /* u - ((q0_u - p) * q1_u) / (q1_u.length_squared() + (q0_u - p) * q2_u) */
924         isub_vnvn(q0_u, p, dims);
925         return u - dot_vnvn(q0_u, q1_u, dims) /
926                (len_squared_vn(q1_u, dims) + dot_vnvn(q0_u, q2_u, dims));
927 }
928
929 static int compare_double_fn(const void *a_, const void *b_)
930 {
931         const double *a = a_;
932         const double *b = b_;
933         if      (*a > *b) return  1;
934         else if (*a < *b) return -1;
935         else              return  0;
936 }
937
938 /**
939  * Given set of points and their parameterization, try to find a better parameterization.
940  */
941 static bool cubic_reparameterize(
942         const Cubic *cubic,
943         const double *points_offset,
944         const uint    points_offset_len,
945         const double *u,
946         const uint    dims,
947
948         double       *r_u_prime)
949 {
950         /*
951          * Recalculate the values of u[] based on the Newton Raphson method
952          */
953
954         const double *pt = points_offset;
955         for (uint i = 0; i < points_offset_len; i++, pt += dims) {
956                 r_u_prime[i] = cubic_find_root(cubic, pt, u[i], dims);
957                 if (!isfinite(r_u_prime[i])) {
958                         return false;
959                 }
960         }
961
962         qsort(r_u_prime, points_offset_len, sizeof(double), compare_double_fn);
963
964         if ((r_u_prime[0] < 0.0) ||
965             (r_u_prime[points_offset_len - 1] > 1.0))
966         {
967                 return false;
968         }
969
970         assert(r_u_prime[0] >= 0.0);
971         assert(r_u_prime[points_offset_len - 1] <= 1.0);
972         return true;
973 }
974
975
976 static bool fit_cubic_to_points(
977         const double *points_offset,
978         const uint    points_offset_len,
979 #ifdef USE_LENGTH_CACHE
980         const double *points_length_cache,
981 #endif
982         const double  tan_l[],
983         const double  tan_r[],
984         const double  error_threshold_sq,
985         const uint    dims,
986
987         Cubic *r_cubic, double *r_error_max_sq, uint *r_split_index)
988 {
989         const uint iteration_max = 4;
990
991         if (points_offset_len == 2) {
992                 CUBIC_VARS(r_cubic, dims, p0, p1, p2, p3);
993
994                 copy_vnvn(p0, &points_offset[0 * dims], dims);
995                 copy_vnvn(p3, &points_offset[1 * dims], dims);
996
997                 const double dist = len_vnvn(p0, p3, dims) / 3.0;
998                 msub_vn_vnvn_fl(p1, p0, tan_l, dist, dims);
999                 madd_vn_vnvn_fl(p2, p3, tan_r, dist, dims);
1000
1001 #ifdef USE_ORIG_INDEX_DATA
1002                 r_cubic->orig_span = 1;
1003 #endif
1004                 return true;
1005         }
1006
1007         double *u = malloc(sizeof(double) * points_offset_len);
1008
1009 #ifdef USE_CIRCULAR_FALLBACK
1010         const double points_offset_coords_length  =
1011 #endif
1012         points_calc_coord_length(
1013                 points_offset, points_offset_len, dims,
1014 #ifdef USE_LENGTH_CACHE
1015                 points_length_cache,
1016 #endif
1017                 u);
1018
1019         double error_max_sq;
1020         uint split_index;
1021
1022         /* Parameterize points, and attempt to fit curve */
1023         cubic_from_points(
1024                 points_offset, points_offset_len,
1025 #ifdef USE_CIRCULAR_FALLBACK
1026                 points_offset_coords_length,
1027 #endif
1028                 u, tan_l, tan_r, dims, r_cubic);
1029
1030         /* Find max deviation of points to fitted curve */
1031         error_max_sq = cubic_calc_error(
1032                 r_cubic, points_offset, points_offset_len, u, dims,
1033                 &split_index);
1034
1035         Cubic *cubic_test = alloca(cubic_alloc_size(dims));
1036
1037         /* Run this so we use the non-circular calculation when the circular-fallback
1038          * in 'cubic_from_points' failed to give a close enough result. */
1039 #ifdef USE_CIRCULAR_FALLBACK
1040         if (!(error_max_sq < error_threshold_sq)) {
1041                 /* Don't use the cubic calculated above, instead calculate a new fallback cubic,
1042                  * since this tends to give more balanced split_index along the curve.
1043                  * This is because the attempt to calcualte the cubic may contain spikes
1044                  * along the curve which may give a lop-sided maximum distance. */
1045                 cubic_from_points_fallback(
1046                         points_offset, points_offset_len,
1047                         tan_l, tan_r, dims, cubic_test);
1048                 const double error_max_sq_test = cubic_calc_error(
1049                         cubic_test, points_offset, points_offset_len, u, dims,
1050                         &split_index);
1051
1052                 /* intentionally use the newly calculated 'split_index',
1053                  * even if the 'error_max_sq_test' is worse. */
1054                 if (error_max_sq > error_max_sq_test) {
1055                         error_max_sq = error_max_sq_test;
1056                         cubic_copy(r_cubic, cubic_test, dims);
1057                 }
1058         }
1059 #endif
1060
1061         /* Test the offset fallback */
1062 #ifdef USE_OFFSET_FALLBACK
1063         if (!(error_max_sq < error_threshold_sq)) {
1064                 /* Using the offset from the curve to calculate cubic handle length may give better results
1065                  * try this as a second fallback. */
1066                 cubic_from_points_offset_fallback(
1067                         points_offset, points_offset_len,
1068                         tan_l, tan_r, dims, cubic_test);
1069                 const double error_max_sq_test = cubic_calc_error_simple(
1070                         cubic_test, points_offset, points_offset_len, u, error_max_sq, dims);
1071
1072                 if (error_max_sq > error_max_sq_test) {
1073                         error_max_sq = error_max_sq_test;
1074                         cubic_copy(r_cubic, cubic_test, dims);
1075                 }
1076         }
1077 #endif
1078
1079         *r_error_max_sq = error_max_sq;
1080         *r_split_index  = split_index;
1081
1082         if (!(error_max_sq < error_threshold_sq)) {
1083                 cubic_copy(cubic_test, r_cubic, dims);
1084
1085                 /* If error not too large, try some reparameterization and iteration */
1086                 double *u_prime = malloc(sizeof(double) * points_offset_len);
1087                 for (uint iter = 0; iter < iteration_max; iter++) {
1088                         if (!cubic_reparameterize(
1089                                 cubic_test, points_offset, points_offset_len, u, dims, u_prime))
1090                         {
1091                                 break;
1092                         }
1093
1094                         cubic_from_points(
1095                                 points_offset, points_offset_len,
1096 #ifdef USE_CIRCULAR_FALLBACK
1097                                 points_offset_coords_length,
1098 #endif
1099                                 u_prime, tan_l, tan_r, dims, cubic_test);
1100
1101                         const double error_max_sq_test = cubic_calc_error(
1102                                 cubic_test, points_offset, points_offset_len, u_prime, dims,
1103                                 &split_index);
1104
1105                         if (error_max_sq > error_max_sq_test) {
1106                                 error_max_sq = error_max_sq_test;
1107                                 cubic_copy(r_cubic, cubic_test, dims);
1108                                 *r_error_max_sq = error_max_sq;
1109                                 *r_split_index = split_index;
1110                         }
1111
1112                         if (!(error_max_sq < error_threshold_sq)) {
1113                                 /* continue */
1114                         }
1115                         else {
1116                                 assert((error_max_sq < error_threshold_sq));
1117                                 free(u_prime);
1118                                 free(u);
1119                                 return true;
1120                         }
1121
1122                         SWAP(double *, u, u_prime);
1123                 }
1124                 free(u_prime);
1125                 free(u);
1126
1127                 return false;
1128         }
1129         else {
1130                 free(u);
1131                 return true;
1132         }
1133 }
1134
1135 static void fit_cubic_to_points_recursive(
1136         const double *points_offset,
1137         const uint    points_offset_len,
1138 #ifdef USE_LENGTH_CACHE
1139         const double *points_length_cache,
1140 #endif
1141         const double  tan_l[],
1142         const double  tan_r[],
1143         const double  error_threshold_sq,
1144         const uint    calc_flag,
1145         const uint    dims,
1146         /* fill in the list */
1147         CubicList *clist)
1148 {
1149         Cubic *cubic = cubic_alloc(dims);
1150         uint split_index;
1151         double error_max_sq;
1152
1153         if (fit_cubic_to_points(
1154                 points_offset, points_offset_len,
1155 #ifdef USE_LENGTH_CACHE
1156                 points_length_cache,
1157 #endif
1158                 tan_l, tan_r,
1159                 (calc_flag & CURVE_FIT_CALC_HIGH_QUALIY) ? DBL_EPSILON : error_threshold_sq,
1160                 dims,
1161                 cubic, &error_max_sq, &split_index) ||
1162             (error_max_sq < error_threshold_sq))
1163         {
1164                 cubic_list_prepend(clist, cubic);
1165                 return;
1166         }
1167         cubic_free(cubic);
1168
1169
1170         /* Fitting failed -- split at max error point and fit recursively */
1171
1172         /* Check splinePoint is not an endpoint?
1173          *
1174          * This assert happens sometimes...
1175          * Look into it but disable for now. Campbell! */
1176
1177         // assert(split_index > 1)
1178 #ifdef USE_VLA
1179         double tan_center[dims];
1180 #else
1181         double *tan_center = alloca(sizeof(double) * dims);
1182 #endif
1183
1184         const double *pt_a = &points_offset[(split_index - 1) * dims];
1185         const double *pt_b = &points_offset[(split_index + 1) * dims];
1186
1187         assert(split_index < points_offset_len);
1188         if (equals_vnvn(pt_a, pt_b, dims)) {
1189                 pt_a += dims;
1190         }
1191
1192         {
1193 #ifdef USE_VLA
1194                 double tan_center_a[dims];
1195                 double tan_center_b[dims];
1196 #else
1197                 double *tan_center_a = alloca(sizeof(double) * dims);
1198                 double *tan_center_b = alloca(sizeof(double) * dims);
1199 #endif
1200                 const double *pt   = &points_offset[split_index * dims];
1201
1202                 /* tan_center = ((pt_a - pt).normalized() + (pt - pt_b).normalized()).normalized() */
1203                 normalize_vn_vnvn(tan_center_a, pt_a, pt, dims);
1204                 normalize_vn_vnvn(tan_center_b, pt, pt_b, dims);
1205                 add_vn_vnvn(tan_center, tan_center_a, tan_center_b, dims);
1206                 normalize_vn(tan_center, dims);
1207         }
1208
1209         fit_cubic_to_points_recursive(
1210                 points_offset, split_index + 1,
1211 #ifdef USE_LENGTH_CACHE
1212                 points_length_cache,
1213 #endif
1214                 tan_l, tan_center, error_threshold_sq, calc_flag, dims, clist);
1215         fit_cubic_to_points_recursive(
1216                 &points_offset[split_index * dims], points_offset_len - split_index,
1217 #ifdef USE_LENGTH_CACHE
1218                 points_length_cache + split_index,
1219 #endif
1220                 tan_center, tan_r, error_threshold_sq, calc_flag, dims, clist);
1221
1222 }
1223
1224 /** \} */
1225
1226
1227 /* -------------------------------------------------------------------- */
1228
1229 /** \name External API for Curve-Fitting
1230  * \{ */
1231
1232 /**
1233  * Main function:
1234  *
1235  * Take an array of 3d points.
1236  * return the cubic splines
1237  */
1238 int curve_fit_cubic_to_points_db(
1239         const double *points,
1240         const uint    points_len,
1241         const uint    dims,
1242         const double  error_threshold,
1243         const uint    calc_flag,
1244         const uint   *corners,
1245         uint          corners_len,
1246
1247         double **r_cubic_array, uint *r_cubic_array_len,
1248         uint **r_cubic_orig_index,
1249         uint **r_corner_index_array, uint *r_corner_index_len)
1250 {
1251         uint corners_buf[2];
1252         if (corners == NULL) {
1253                 assert(corners_len == 0);
1254                 corners_buf[0] = 0;
1255                 corners_buf[1] = points_len - 1;
1256                 corners = corners_buf;
1257                 corners_len = 2;
1258         }
1259
1260         CubicList clist = {0};
1261         clist.dims = dims;
1262
1263 #ifdef USE_VLA
1264         double tan_l[dims];
1265         double tan_r[dims];
1266 #else
1267         double *tan_l = alloca(sizeof(double) * dims);
1268         double *tan_r = alloca(sizeof(double) * dims);
1269 #endif
1270
1271 #ifdef USE_LENGTH_CACHE
1272         double *points_length_cache = NULL;
1273         uint    points_length_cache_len_alloc = 0;
1274 #endif
1275
1276         uint *corner_index_array = NULL;
1277         uint  corner_index = 0;
1278         if (r_corner_index_array && (corners != corners_buf)) {
1279                 corner_index_array = malloc(sizeof(uint) * corners_len);
1280                 corner_index_array[corner_index++] = corners[0];
1281         }
1282
1283         const double error_threshold_sq = sq(error_threshold);
1284
1285         for (uint i = 1; i < corners_len; i++) {
1286                 const uint points_offset_len = corners[i] - corners[i - 1] + 1;
1287                 const uint first_point = corners[i - 1];
1288
1289                 assert(points_offset_len >= 1);
1290                 if (points_offset_len > 1) {
1291                         const double *pt_l = &points[first_point * dims];
1292                         const double *pt_r = &points[(first_point + points_offset_len - 1) * dims];
1293                         const double *pt_l_next = pt_l + dims;
1294                         const double *pt_r_prev = pt_r - dims;
1295
1296                         /* tan_l = (pt_l - pt_l_next).normalized()
1297                          * tan_r = (pt_r_prev - pt_r).normalized()
1298                          */
1299                         normalize_vn_vnvn(tan_l, pt_l, pt_l_next, dims);
1300                         normalize_vn_vnvn(tan_r, pt_r_prev, pt_r, dims);
1301
1302 #ifdef USE_LENGTH_CACHE
1303                         if (points_length_cache_len_alloc < points_offset_len) {
1304                                 if (points_length_cache) {
1305                                         free(points_length_cache);
1306                                 }
1307                                 points_length_cache = malloc(sizeof(double) * points_offset_len);
1308                         }
1309                         points_calc_coord_length_cache(
1310                                 &points[first_point * dims], points_offset_len, dims,
1311                                 points_length_cache);
1312 #endif
1313
1314                         fit_cubic_to_points_recursive(
1315                                 &points[first_point * dims], points_offset_len,
1316 #ifdef USE_LENGTH_CACHE
1317                                 points_length_cache,
1318 #endif
1319                                 tan_l, tan_r, error_threshold_sq, calc_flag, dims, &clist);
1320                 }
1321                 else if (points_len == 1) {
1322                         assert(points_offset_len == 1);
1323                         assert(corners_len == 2);
1324                         assert(corners[0] == 0);
1325                         assert(corners[1] == 0);
1326                         const double *pt = &points[0];
1327                         Cubic *cubic = cubic_alloc(dims);
1328                         cubic_init(cubic, pt, pt, pt, pt, dims);
1329                         cubic_list_prepend(&clist, cubic);
1330                 }
1331
1332                 if (corner_index_array) {
1333                         corner_index_array[corner_index++] = clist.len;
1334                 }
1335         }
1336
1337 #ifdef USE_LENGTH_CACHE
1338         if (points_length_cache) {
1339                 free(points_length_cache);
1340         }
1341 #endif
1342
1343 #ifdef USE_ORIG_INDEX_DATA
1344         uint *cubic_orig_index = NULL;
1345         if (r_cubic_orig_index) {
1346                 cubic_orig_index = malloc(sizeof(uint) * (clist.len + 1));
1347         }
1348 #else
1349         *r_cubic_orig_index = NULL;
1350 #endif
1351
1352         /* allocate a contiguous array and free the linked list */
1353         *r_cubic_array = cubic_list_as_array(
1354                 &clist
1355 #ifdef USE_ORIG_INDEX_DATA
1356                 , corners[corners_len - 1], cubic_orig_index
1357 #endif
1358                 );
1359         *r_cubic_array_len = clist.len + 1;
1360
1361         cubic_list_clear(&clist);
1362
1363 #ifdef USE_ORIG_INDEX_DATA
1364         if (cubic_orig_index) {
1365                 *r_cubic_orig_index = cubic_orig_index;
1366         }
1367 #endif
1368
1369         if (corner_index_array) {
1370                 assert(corner_index == corners_len);
1371                 *r_corner_index_array = corner_index_array;
1372                 *r_corner_index_len = corner_index;
1373         }
1374
1375         return 0;
1376 }
1377
1378 /**
1379  * A version of #curve_fit_cubic_to_points_db to handle floats
1380  */
1381 int curve_fit_cubic_to_points_fl(
1382         const float  *points,
1383         const uint    points_len,
1384         const uint    dims,
1385         const float   error_threshold,
1386         const uint    calc_flag,
1387         const uint   *corners,
1388         const uint    corners_len,
1389
1390         float **r_cubic_array, uint *r_cubic_array_len,
1391         uint **r_cubic_orig_index,
1392         uint **r_corner_index_array, uint *r_corner_index_len)
1393 {
1394         const uint points_flat_len = points_len * dims;
1395         double *points_db = malloc(sizeof(double) * points_flat_len);
1396
1397         copy_vndb_vnfl(points_db, points, points_flat_len);
1398
1399         double *cubic_array_db = NULL;
1400         float  *cubic_array_fl = NULL;
1401         uint    cubic_array_len = 0;
1402
1403         int result = curve_fit_cubic_to_points_db(
1404                 points_db, points_len, dims, error_threshold, calc_flag, corners, corners_len,
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
1425 /**
1426  * Fit a single cubic to points.
1427  */
1428 int curve_fit_cubic_to_points_single_db(
1429         const double *points,
1430         const uint    points_len,
1431         const double *points_length_cache,
1432         const uint    dims,
1433         const double  error_threshold,
1434         const double tan_l[],
1435         const double tan_r[],
1436
1437         double  r_handle_l[],
1438         double  r_handle_r[],
1439         double *r_error_max_sq,
1440         uint   *r_error_index)
1441 {
1442         Cubic *cubic = alloca(cubic_alloc_size(dims));
1443
1444         /* in this instance theres no advantage in using length cache,
1445          * since we're not recursively calculating values. */
1446 #ifdef USE_LENGTH_CACHE
1447         double *points_length_cache_alloc = NULL;
1448         if (points_length_cache == NULL) {
1449                 points_length_cache_alloc = malloc(sizeof(double) * points_len);
1450                 points_calc_coord_length_cache(
1451                         points, points_len, dims,
1452                         points_length_cache_alloc);
1453                 points_length_cache = points_length_cache_alloc;
1454         }
1455 #endif
1456
1457         fit_cubic_to_points(
1458                 points, points_len,
1459 #ifdef USE_LENGTH_CACHE
1460                 points_length_cache,
1461 #endif
1462                 tan_l, tan_r, error_threshold, dims,
1463
1464                 cubic, r_error_max_sq, r_error_index);
1465
1466 #ifdef USE_LENGTH_CACHE
1467         if (points_length_cache_alloc) {
1468                 free(points_length_cache_alloc);
1469         }
1470 #endif
1471
1472         copy_vnvn(r_handle_l, CUBIC_PT(cubic, 1, dims), dims);
1473         copy_vnvn(r_handle_r, CUBIC_PT(cubic, 2, dims), dims);
1474
1475         return 0;
1476 }
1477
1478 int curve_fit_cubic_to_points_single_fl(
1479         const float  *points,
1480         const uint    points_len,
1481         const float  *points_length_cache,
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         uint   *r_error_index)
1491 {
1492         const uint points_flat_len = points_len * dims;
1493         double *points_db = malloc(sizeof(double) * points_flat_len);
1494         double *points_length_cache_db = NULL;
1495
1496         copy_vndb_vnfl(points_db, points, points_flat_len);
1497
1498         if (points_length_cache) {
1499                 points_length_cache_db = malloc(sizeof(double) * points_len);
1500                 copy_vndb_vnfl(points_length_cache_db, points_length_cache, points_len);
1501         }
1502
1503 #ifdef USE_VLA
1504         double tan_l_db[dims];
1505         double tan_r_db[dims];
1506         double r_handle_l_db[dims];
1507         double r_handle_r_db[dims];
1508 #else
1509         double *tan_l_db = alloca(sizeof(double) * dims);
1510         double *tan_r_db = alloca(sizeof(double) * dims);
1511         double *r_handle_l_db = alloca(sizeof(double) * dims);
1512         double *r_handle_r_db = alloca(sizeof(double) * dims);
1513 #endif
1514         double r_error_sq_db;
1515
1516         copy_vndb_vnfl(tan_l_db, tan_l, dims);
1517         copy_vndb_vnfl(tan_r_db, tan_r, dims);
1518
1519         int result = curve_fit_cubic_to_points_single_db(
1520                 points_db, points_len, points_length_cache_db, dims,
1521                 (double)error_threshold,
1522                 tan_l_db, tan_r_db,
1523                 r_handle_l_db, r_handle_r_db,
1524                 &r_error_sq_db,
1525                 r_error_index);
1526
1527         free(points_db);
1528
1529         if (points_length_cache_db) {
1530                 free(points_length_cache_db);
1531         }
1532
1533         copy_vnfl_vndb(r_handle_l, r_handle_l_db, dims);
1534         copy_vnfl_vndb(r_handle_r, r_handle_r_db, dims);
1535         *r_error_sq = (float)r_error_sq_db;
1536
1537         return result;
1538 }
1539
1540 /** \} */