Fix error copying cubic data
[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 #include <math.h>
33 #include <float.h>
34 #include <stdbool.h>
35 #include <assert.h>
36
37 #include <string.h>
38 #include <stdlib.h>
39
40 #include "../curve_fit_nd.h"
41
42 /* avoid re-calculating lengths multiple times */
43 #define USE_LENGTH_CACHE
44
45 /* store the indices in the cubic data so we can return the original indices,
46  * useful when the caller has data assosiated with the curve. */
47 #define USE_ORIG_INDEX_DATA
48
49 typedef unsigned int uint;
50
51 #include "curve_fit_inline.h"
52
53 #ifdef _MSC_VER
54 #  define alloca(size) _alloca(size)
55 #endif
56
57 #if !defined(_MSC_VER)
58 #  define USE_VLA
59 #endif
60
61 #ifdef USE_VLA
62 #  ifdef __GNUC__
63 #    pragma GCC diagnostic ignored "-Wvla"
64 #  endif
65 #else
66 #  ifdef __GNUC__
67 #    pragma GCC diagnostic error "-Wvla"
68 #  endif
69 #endif
70
71 #define SWAP(type, a, b)  {    \
72         type sw_ap;                \
73         sw_ap = (a);               \
74         (a) = (b);                 \
75         (b) = sw_ap;               \
76 } (void)0
77
78
79 /* -------------------------------------------------------------------- */
80
81 /** \name Cubic Type & Functions
82  * \{ */
83
84 typedef struct Cubic {
85         /* single linked lists */
86         struct Cubic *next;
87 #ifdef USE_ORIG_INDEX_DATA
88         uint orig_span;
89 #endif
90         /* 0: point_0, 1: handle_0, 2: handle_1, 3: point_1,
91          * each one is offset by 'dims' */
92         double pt_data[0];
93 } Cubic;
94
95 #define CUBIC_PT(cubic, index, dims) \
96         (&(cubic)->pt_data[(index) * (dims)])
97
98 #define CUBIC_VARS(c, dims, _p0, _p1, _p2, _p3) \
99         double \
100         *_p0 = (c)->pt_data, \
101         *_p1 = _p0 + (dims), \
102         *_p2 = _p1 + (dims), \
103         *_p3 = _p2 + (dims); ((void)0)
104 #define CUBIC_VARS_CONST(c, dims, _p0, _p1, _p2, _p3) \
105         const double \
106         *_p0 = (c)->pt_data, \
107         *_p1 = _p0 + (dims), \
108         *_p2 = _p1 + (dims), \
109         *_p3 = _p2 + (dims); ((void)0)
110
111
112 static size_t cubic_alloc_size(const uint dims)
113 {
114         return sizeof(Cubic) + (sizeof(double) * 4 * dims);
115 }
116
117 static Cubic *cubic_alloc(const uint dims)
118 {
119         return malloc(cubic_alloc_size(dims));
120 }
121
122 static void cubic_copy(Cubic *cubic_dst, const Cubic *cubic_src, const uint dims)
123 {
124         memcpy(cubic_dst, cubic_src, cubic_alloc_size(dims));
125 }
126
127 static void cubic_init(
128         Cubic *cubic,
129         const double p0[], const double p1[], const double p2[], const double p3[],
130         const uint dims)
131 {
132         copy_vnvn(CUBIC_PT(cubic, 0, dims), p0, dims);
133         copy_vnvn(CUBIC_PT(cubic, 1, dims), p1, dims);
134         copy_vnvn(CUBIC_PT(cubic, 2, dims), p2, dims);
135         copy_vnvn(CUBIC_PT(cubic, 3, dims), p3, dims);
136 }
137
138 static void cubic_free(Cubic *cubic)
139 {
140         free(cubic);
141 }
142
143 /** \} */
144
145
146 /* -------------------------------------------------------------------- */
147
148 /** \name CubicList Type & Functions
149  * \{ */
150
151 typedef struct CubicList {
152         struct Cubic *items;
153         uint          len;
154         uint          dims;
155 } CubicList;
156
157 static void cubic_list_prepend(CubicList *clist, Cubic *cubic)
158 {
159         cubic->next = clist->items;
160         clist->items = cubic;
161         clist->len++;
162 }
163
164 static double *cubic_list_as_array(
165         const CubicList *clist
166 #ifdef USE_ORIG_INDEX_DATA
167         ,
168         const uint index_last,
169         uint *r_orig_index
170 #endif
171         )
172 {
173         const uint dims = clist->dims;
174         const uint array_flat_len = (clist->len + 1) * 3 * dims;
175
176         double *array = malloc(sizeof(double) * array_flat_len);
177         const double *handle_prev = &((Cubic *)clist->items)->pt_data[dims];
178
179 #ifdef USE_ORIG_INDEX_DATA
180         uint orig_index_value = index_last;
181         uint orig_index_index = clist->len;
182         bool use_orig_index = (r_orig_index != NULL);
183 #endif
184
185         /* fill the array backwards */
186         const size_t array_chunk = 3 * dims;
187         double *array_iter = array + array_flat_len;
188         for (Cubic *citer = clist->items; citer; citer = citer->next) {
189                 array_iter -= array_chunk;
190                 memcpy(array_iter, &citer->pt_data[2 * dims], sizeof(double) * 2 * dims);
191                 memcpy(&array_iter[2 * dims], &handle_prev[dims], sizeof(double) * dims);
192                 handle_prev = citer->pt_data;
193
194 #ifdef USE_ORIG_INDEX_DATA
195                 if (use_orig_index) {
196                         r_orig_index[orig_index_index--] = orig_index_value;
197                         orig_index_value -= citer->orig_span;
198                 }
199 #endif
200         }
201
202 #ifdef USE_ORIG_INDEX_DATA
203         if (use_orig_index) {
204                 assert(orig_index_index == 0);
205                 assert(orig_index_value == 0 || index_last == 0);
206                 r_orig_index[orig_index_index] = index_last ? orig_index_value : 0;
207
208         }
209 #endif
210
211         /* flip tangent for first and last (we could leave at zero, but set to something useful) */
212
213         /* first */
214         array_iter -= array_chunk;
215         memcpy(&array_iter[dims], handle_prev, sizeof(double) * 2 * dims);
216         flip_vn_vnvn(&array_iter[0 * dims], &array_iter[1 * dims], &array_iter[2 * dims], dims);
217         assert(array == array_iter);
218
219         /* last */
220         array_iter += array_flat_len - (3 * dims);
221         flip_vn_vnvn(&array_iter[2 * dims], &array_iter[1 * dims], &array_iter[0 * dims], dims);
222
223         return array;
224 }
225
226 static void cubic_list_clear(CubicList *clist)
227 {
228         Cubic *cubic_next;
229         for (Cubic *citer = clist->items; citer; citer = cubic_next) {
230                 cubic_next = citer->next;
231                 cubic_free(citer);
232         }
233         clist->items = NULL;
234         clist->len  = 0;
235 }
236
237 /** \} */
238
239
240 /* -------------------------------------------------------------------- */
241
242 /** \name Cubic Evaluation
243  * \{ */
244
245 static void cubic_evaluate(
246         const Cubic *cubic, const double t, const uint dims,
247         double r_v[])
248 {
249         CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
250         const double s = 1.0 - t;
251
252         for (uint j = 0; j < dims; j++) {
253                 const double p01 = (p0[j] * s) + (p1[j] * t);
254                 const double p12 = (p1[j] * s) + (p2[j] * t);
255                 const double p23 = (p2[j] * s) + (p3[j] * t);
256                 r_v[j] = ((((p01 * s) + (p12 * t))) * s) +
257                          ((((p12 * s) + (p23 * t))) * t);
258         }
259 }
260
261 static void cubic_calc_point(
262         const Cubic *cubic, const double t, const uint dims,
263         double r_v[])
264 {
265         CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
266         const double s = 1.0 - t;
267         for (uint j = 0; j < dims; j++) {
268                 r_v[j] = p0[j] * s * s * s +
269                          3.0 * t * s * (s * p1[j] + t * p2[j]) + t * t * t * p3[j];
270         }
271 }
272
273 static void cubic_calc_speed(
274         const Cubic *cubic, const double t, const uint dims,
275         double r_v[])
276 {
277         CUBIC_VARS_CONST(cubic, dims, p0, p1, p2, p3);
278         const double s = 1.0 - t;
279         for (uint j = 0; j < dims; j++) {
280                 r_v[j] =  3.0 * ((p1[j] - p0[j]) * s * s + 2.0 *
281                                  (p2[j] - p0[j]) * s * t +
282                                  (p3[j] - p2[j]) * t * t);
283         }
284 }
285
286 static void cubic_calc_acceleration(
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] = 6.0 * ((p2[j] - 2.0 * p1[j] + p0[j]) * s +
294                                 (p3[j] - 2.0 * p2[j] + p1[j]) * t);
295         }
296 }
297
298 /**
299  * Returns a 'measure' of the maximum distance (squared) of the points specified
300  * by points_offset from the corresponding cubic(u[]) points.
301  */
302 static double cubic_calc_error(
303         const Cubic *cubic,
304         const double *points_offset,
305         const uint points_offset_len,
306         const double *u,
307         const uint dims,
308
309         uint *r_error_index)
310 {
311         double error_max_sq = 0.0;
312         uint   error_index = 0;
313
314         const double *pt_real = points_offset + dims;
315 #ifdef USE_VLA
316         double        pt_eval[dims];
317 #else
318         double       *pt_eval = alloca(sizeof(double) * dims);
319 #endif
320
321         for (uint i = 1; i < points_offset_len - 1; i++, pt_real += dims) {
322                 cubic_evaluate(cubic, u[i], dims, pt_eval);
323
324                 const double err_sq = len_squared_vnvn(pt_real, pt_eval, dims);
325                 if (err_sq >= error_max_sq) {
326                         error_max_sq = err_sq;
327                         error_index = i;
328                 }
329         }
330
331         *r_error_index = error_index;
332         return error_max_sq;
333 }
334
335 /**
336  * Bezier multipliers
337  */
338
339 static double B1(double u)
340 {
341         double tmp = 1.0 - u;
342         return 3.0 * u * tmp * tmp;
343 }
344
345 static double B2(double u)
346 {
347         return 3.0 * u * u * (1.0 - u);
348 }
349
350 static double B0plusB1(double u)
351 {
352     double tmp = 1.0 - u;
353     return tmp * tmp * (1.0 + 2.0 * u);
354 }
355
356 static double B2plusB3(double u)
357 {
358     return u * u * (3.0 - 2.0 * u);
359 }
360
361 static void points_calc_center_weighted(
362         const double *points_offset,
363         const uint    points_offset_len,
364         const uint    dims,
365
366         double r_center[])
367 {
368         /*
369          * Calculate a center that compensates for point spacing.
370          */
371
372         const double *pt_prev = &points_offset[(points_offset_len - 2) * dims];
373         const double *pt_curr = pt_prev + dims;
374         const double *pt_next = points_offset;
375
376         double w_prev = len_vnvn(pt_prev, pt_curr, dims);
377
378         zero_vn(r_center, dims);
379         double w_tot = 0.0;
380
381         for (uint i_next = 0; i_next < points_offset_len; i_next++) {
382                 const double w_next = len_vnvn(pt_curr, pt_next, dims);
383                 const double w = w_prev + w_next;
384                 w_tot += w;
385
386                 miadd_vn_vn_fl(r_center, pt_curr, w, dims);
387
388                 w_prev = w_next;
389
390                 pt_prev = pt_curr;
391                 pt_curr = pt_next;
392                 pt_next += dims;
393         }
394
395         if (w_tot != 0.0) {
396                 imul_vn_fl(r_center, 1.0 / w_tot, dims);
397         }
398 }
399
400 /**
401  * Use least-squares method to find Bezier control points for region.
402  */
403 static void cubic_from_points(
404         const double *points_offset,
405         const uint    points_offset_len,
406         const double *u_prime,
407         const double  tan_l[],
408         const double  tan_r[],
409         const uint dims,
410
411         Cubic *r_cubic)
412 {
413
414         const double *p0 = &points_offset[0];
415         const double *p3 = &points_offset[(points_offset_len - 1) * dims];
416
417         /* Point Pairs */
418         double alpha_l, alpha_r;
419 #ifdef USE_VLA
420         double a[2][dims];
421         double tmp[dims];
422 #else
423         double *a[2] = {
424             alloca(sizeof(double) * dims),
425             alloca(sizeof(double) * dims),
426         };
427         double *tmp = alloca(sizeof(double) * dims);
428 #endif
429
430         {
431                 double x[2] = {0.0}, c[2][2] = {{0.0}};
432                 const double *pt = points_offset;
433
434                 for (uint i = 0; i < points_offset_len; i++, pt += dims) {
435                         mul_vnvn_fl(a[0], tan_l, B1(u_prime[i]), dims);
436                         mul_vnvn_fl(a[1], tan_r, B2(u_prime[i]), dims);
437
438                         c[0][0] += dot_vnvn(a[0], a[0], dims);
439                         c[0][1] += dot_vnvn(a[0], a[1], dims);
440                         c[1][1] += dot_vnvn(a[1], a[1], dims);
441
442                         c[1][0] = c[0][1];
443
444                         {
445                                 const double b0_plus_b1 = B0plusB1(u_prime[i]);
446                                 const double b2_plus_b3 = B2plusB3(u_prime[i]);
447                                 for (uint j = 0; j < dims; j++) {
448                                         tmp[j] = (pt[j] - (p0[j] * b0_plus_b1)) + (p3[j] * b2_plus_b3);
449                                 }
450
451                                 x[0] += dot_vnvn(a[0], tmp, dims);
452                                 x[1] += dot_vnvn(a[1], tmp, dims);
453                         }
454                 }
455
456                 double det_C0_C1 = c[0][0] * c[1][1] - c[0][1] * c[1][0];
457                 double det_C_0X  = x[1]    * c[0][0] - x[0]    * c[0][1];
458                 double det_X_C1  = x[0]    * c[1][1] - x[1]    * c[0][1];
459
460                 if (is_almost_zero(det_C0_C1)) {
461                         det_C0_C1 = c[0][0] * c[1][1] * 10e-12;
462                 }
463
464                 /* may still divide-by-zero, check below will catch nan values */
465                 alpha_l = det_X_C1 / det_C0_C1;
466                 alpha_r = det_C_0X / det_C0_C1;
467         }
468
469         /*
470          * The problem that the stupid values for alpha dare not put
471          * only when we realize that the sign and wrong,
472          * but even if the values are too high.
473          * But how do you evaluate it?
474          *
475          * Meanwhile, we should ensure that these values are sometimes
476          * so only problems absurd of approximation and not for bugs in the code.
477          */
478
479         bool use_clamp = true;
480
481         /* flip check to catch nan values */
482         if (!(alpha_l >= 0.0) ||
483             !(alpha_r >= 0.0))
484         {
485                 alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
486
487                 /* skip clamping when we're using default handles */
488                 use_clamp = false;
489         }
490
491         double *p1 = CUBIC_PT(r_cubic, 1, dims);
492         double *p2 = CUBIC_PT(r_cubic, 2, dims);
493
494         copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims);
495         copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims);
496
497 #ifdef USE_ORIG_INDEX_DATA
498         r_cubic->orig_span = (points_offset_len - 1);
499 #endif
500
501         /* p1 = p0 - (tan_l * alpha_l);
502          * p2 = p3 + (tan_r * alpha_r);
503          */
504         msub_vn_vnvn_fl(p1, p0, tan_l, alpha_l, dims);
505         madd_vn_vnvn_fl(p2, p3, tan_r, alpha_r, dims);
506
507         /* ------------------------------------
508          * Clamping (we could make it optional)
509          */
510         if (use_clamp) {
511 #ifdef USE_VLA
512                 double center[dims];
513 #else
514                 double *center = alloca(sizeof(double) * dims);
515 #endif
516                 points_calc_center_weighted(points_offset, points_offset_len, dims, center);
517
518                 const double clamp_scale = 3.0;  /* clamp to 3x */
519                 double dist_sq_max = 0.0;
520
521                 {
522                         const double *pt = points_offset;
523                         for (uint i = 0; i < points_offset_len; i++, pt += dims) {
524 #if 0
525                                 double dist_sq_test = sq(len_vnvn(center, pt, dims) * clamp_scale);
526 #else
527                                 /* do inline */
528                                 double dist_sq_test = 0.0;
529                                 for (uint j = 0; j < dims; j++) {
530                                         dist_sq_test += sq((pt[j] - center[j]) * clamp_scale);
531                                 }
532 #endif
533                                 dist_sq_max = max(dist_sq_max, dist_sq_test);
534                         }
535                 }
536
537                 double p1_dist_sq = len_squared_vnvn(center, p1, dims);
538                 double p2_dist_sq = len_squared_vnvn(center, p2, dims);
539
540                 if (p1_dist_sq > dist_sq_max ||
541                     p2_dist_sq > dist_sq_max)
542                 {
543
544                         alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
545
546                         /*
547                          * p1 = p0 - (tan_l * alpha_l);
548                          * p2 = p3 + (tan_r * alpha_r);
549                          */
550                         for (uint j = 0; j < dims; j++) {
551                                 p1[j] = p0[j] - (tan_l[j] * alpha_l);
552                                 p2[j] = p3[j] + (tan_r[j] * alpha_r);
553                         }
554
555                         p1_dist_sq = len_squared_vnvn(center, p1, dims);
556                         p2_dist_sq = len_squared_vnvn(center, p2, dims);
557                 }
558
559                 /* clamp within the 3x radius */
560                 if (p1_dist_sq > dist_sq_max) {
561                         isub_vnvn(p1, center, dims);
562                         imul_vn_fl(p1, sqrt(dist_sq_max) / sqrt(p1_dist_sq), dims);
563                         iadd_vnvn(p1, center, dims);
564                 }
565                 if (p2_dist_sq > dist_sq_max) {
566                         isub_vnvn(p2, center, dims);
567                         imul_vn_fl(p2, sqrt(dist_sq_max) / sqrt(p2_dist_sq), dims);
568                         iadd_vnvn(p2, center, dims);
569                 }
570         }
571         /* end clamping */
572 }
573
574 #ifdef USE_LENGTH_CACHE
575 static void points_calc_coord_length_cache(
576         const double *points_offset,
577         const uint    points_offset_len,
578         const uint    dims,
579
580         double     *r_points_length_cache)
581 {
582         const double *pt_prev = points_offset;
583         const double *pt = pt_prev + dims;
584         r_points_length_cache[0] = 0.0;
585         for (uint i = 1; i < points_offset_len; i++) {
586                 r_points_length_cache[i] = len_vnvn(pt, pt_prev, dims);
587                 pt_prev = pt;
588                 pt += dims;
589         }
590 }
591 #endif  /* USE_LENGTH_CACHE */
592
593
594 static void points_calc_coord_length(
595         const double *points_offset,
596         const uint    points_offset_len,
597         const uint    dims,
598 #ifdef USE_LENGTH_CACHE
599         const double *points_length_cache,
600 #endif
601         double *r_u)
602 {
603         const double *pt_prev = points_offset;
604         const double *pt = pt_prev + dims;
605         r_u[0] = 0.0;
606         for (uint i = 1, i_prev = 0; i < points_offset_len; i++) {
607                 double length;
608
609 #ifdef USE_LENGTH_CACHE
610                 length = points_length_cache[i];
611
612                 assert(len_vnvn(pt, pt_prev, dims) == points_length_cache[i]);
613 #else
614                 length = len_vnvn(pt, pt_prev, dims);
615 #endif
616
617                 r_u[i] = r_u[i_prev] + length;
618                 i_prev = i;
619                 pt_prev = pt;
620                 pt += dims;
621         }
622         assert(!is_almost_zero(r_u[points_offset_len - 1]));
623         const double w = r_u[points_offset_len - 1];
624         for (uint i = 0; i < points_offset_len; i++) {
625                 r_u[i] /= w;
626         }
627 }
628
629 /**
630  * Use Newton-Raphson iteration to find better root.
631  *
632  * \param cubic: Current fitted curve.
633  * \param p: Point to test against.
634  * \param u: Parameter value for \a p.
635  *
636  * \note Return value may be `nan` caller must check for this.
637  */
638 static double cubic_find_root(
639                 const Cubic *cubic,
640                 const double p[],
641                 const double u,
642                 const uint dims)
643 {
644         /* Newton-Raphson Method. */
645         /* all vectors */
646 #ifdef USE_VLA
647         double q0_u[dims];
648         double q1_u[dims];
649         double q2_u[dims];
650 #else
651         double *q0_u = alloca(sizeof(double) * dims);
652         double *q1_u = alloca(sizeof(double) * dims);
653         double *q2_u = alloca(sizeof(double) * dims);
654 #endif
655
656         cubic_calc_point(cubic, u, dims, q0_u);
657         cubic_calc_speed(cubic, u, dims, q1_u);
658         cubic_calc_acceleration(cubic, u, dims, q2_u);
659
660         /* may divide-by-zero, caller must check for that case */
661         /* u - ((q0_u - p) * q1_u) / (q1_u.length_squared() + (q0_u - p) * q2_u) */
662         isub_vnvn(q0_u, p, dims);
663         return u - dot_vnvn(q0_u, q1_u, dims) /
664                (len_squared_vn(q1_u, dims) + dot_vnvn(q0_u, q2_u, dims));
665 }
666
667 static int compare_double_fn(const void *a_, const void *b_)
668 {
669         const double *a = a_;
670         const double *b = b_;
671         if      (*a > *b) return  1;
672         else if (*a < *b) return -1;
673         else              return  0;
674 }
675
676 /**
677  * Given set of points and their parameterization, try to find a better parameterization.
678  */
679 static bool cubic_reparameterize(
680         const Cubic *cubic,
681         const double *points_offset,
682         const uint    points_offset_len,
683         const double *u,
684         const uint    dims,
685
686         double       *r_u_prime)
687 {
688         /*
689          * Recalculate the values of u[] based on the Newton Raphson method
690          */
691
692         const double *pt = points_offset;
693         for (uint i = 0; i < points_offset_len; i++, pt += dims) {
694                 r_u_prime[i] = cubic_find_root(cubic, pt, u[i], dims);
695                 if (!isfinite(r_u_prime[i])) {
696                         return false;
697                 }
698         }
699
700         qsort(r_u_prime, points_offset_len, sizeof(double), compare_double_fn);
701
702         if ((r_u_prime[0] < 0.0) ||
703             (r_u_prime[points_offset_len - 1] > 1.0))
704         {
705                 return false;
706         }
707
708         assert(r_u_prime[0] >= 0.0);
709         assert(r_u_prime[points_offset_len - 1] <= 1.0);
710         return true;
711 }
712
713
714 static bool fit_cubic_to_points(
715         const double *points_offset,
716         const uint    points_offset_len,
717 #ifdef USE_LENGTH_CACHE
718         const double *points_length_cache,
719 #endif
720         const double  tan_l[],
721         const double  tan_r[],
722         const double  error_threshold_sq,
723         const uint    dims,
724
725         Cubic *r_cubic, double *r_error_max_sq, uint *r_split_index)
726 {
727         const uint iteration_max = 4;
728
729         if (points_offset_len == 2) {
730                 CUBIC_VARS(r_cubic, dims, p0, p1, p2, p3);
731
732                 copy_vnvn(p0, &points_offset[0 * dims], dims);
733                 copy_vnvn(p3, &points_offset[1 * dims], dims);
734
735                 const double dist = len_vnvn(p0, p3, dims) / 3.0;
736                 msub_vn_vnvn_fl(p1, p0, tan_l, dist, dims);
737                 madd_vn_vnvn_fl(p2, p3, tan_r, dist, dims);
738
739 #ifdef USE_ORIG_INDEX_DATA
740                 r_cubic->orig_span = 1;
741 #endif
742                 return true;
743         }
744
745         double *u = malloc(sizeof(double) * points_offset_len);
746         points_calc_coord_length(
747                 points_offset, points_offset_len, dims,
748 #ifdef USE_LENGTH_CACHE
749                 points_length_cache,
750 #endif
751                 u);
752
753         double error_max_sq;
754         uint split_index;
755
756         /* Parameterize points, and attempt to fit curve */
757         cubic_from_points(
758                 points_offset, points_offset_len, u, tan_l, tan_r, dims, r_cubic);
759
760         /* Find max deviation of points to fitted curve */
761         error_max_sq = cubic_calc_error(
762                 r_cubic, points_offset, points_offset_len, u, dims,
763                 &split_index);
764
765         *r_error_max_sq = error_max_sq;
766         *r_split_index  = split_index;
767
768         if (error_max_sq < error_threshold_sq) {
769                 free(u);
770                 return true;
771         }
772         else {
773                 Cubic *cubic_test = alloca(cubic_alloc_size(dims));
774                 cubic_copy(cubic_test, r_cubic, dims);
775
776                 /* If error not too large, try some reparameterization and iteration */
777                 double *u_prime = malloc(sizeof(double) * points_offset_len);
778                 for (uint iter = 0; iter < iteration_max; iter++) {
779                         if (!cubic_reparameterize(
780                                 cubic_test, points_offset, points_offset_len, u, dims, u_prime))
781                         {
782                                 break;
783                         }
784
785                         cubic_from_points(
786                                 points_offset, points_offset_len, u_prime,
787                                 tan_l, tan_r, dims, cubic_test);
788                         error_max_sq = cubic_calc_error(
789                                 cubic_test, points_offset, points_offset_len, u_prime, dims,
790                                 &split_index);
791
792                         if (error_max_sq < error_threshold_sq) {
793                                 free(u_prime);
794                                 free(u);
795
796                                 cubic_copy(r_cubic, cubic_test, dims);
797                                 *r_error_max_sq = error_max_sq;
798                                 *r_split_index  = split_index;
799                                 return true;
800                         }
801                         else if (error_max_sq < *r_error_max_sq) {
802                                 cubic_copy(r_cubic, cubic_test, dims);
803                                 *r_error_max_sq = error_max_sq;
804                                 *r_split_index = split_index;
805                         }
806
807                         SWAP(double *, u, u_prime);
808                 }
809                 free(u_prime);
810                 free(u);
811
812                 return false;
813         }
814 }
815
816 static void fit_cubic_to_points_recursive(
817         const double *points_offset,
818         const uint    points_offset_len,
819 #ifdef USE_LENGTH_CACHE
820         const double *points_length_cache,
821 #endif
822         const double  tan_l[],
823         const double  tan_r[],
824         const double  error_threshold_sq,
825         const uint    dims,
826         /* fill in the list */
827         CubicList *clist)
828 {
829         Cubic *cubic = cubic_alloc(dims);
830         uint split_index;
831         double error_max_sq;
832
833         if (fit_cubic_to_points(
834                 points_offset, points_offset_len,
835 #ifdef USE_LENGTH_CACHE
836                 points_length_cache,
837 #endif
838                 tan_l, tan_r, error_threshold_sq, dims,
839                 cubic, &error_max_sq, &split_index))
840         {
841                 cubic_list_prepend(clist, cubic);
842                 return;
843         }
844         cubic_free(cubic);
845
846
847         /* Fitting failed -- split at max error point and fit recursively */
848
849         /* Check splinePoint is not an endpoint?
850          *
851          * This assert happens sometimes...
852          * Look into it but disable for now. Campbell! */
853
854         // assert(split_index > 1)
855 #ifdef USE_VLA
856         double tan_center[dims];
857 #else
858         double *tan_center = alloca(sizeof(double) * dims);
859 #endif
860
861         const double *pt_a = &points_offset[(split_index - 1) * dims];
862         const double *pt_b = &points_offset[(split_index + 1) * dims];
863
864         assert(split_index < points_offset_len);
865         if (equals_vnvn(pt_a, pt_b, dims)) {
866                 pt_a += dims;
867         }
868
869         {
870 #ifdef USE_VLA
871                 double tan_center_a[dims];
872                 double tan_center_b[dims];
873 #else
874                 double *tan_center_a = alloca(sizeof(double) * dims);
875                 double *tan_center_b = alloca(sizeof(double) * dims);
876 #endif
877                 const double *pt   = &points_offset[split_index * dims];
878
879                 /* tan_center = ((pt_a - pt).normalized() + (pt - pt_b).normalized()).normalized() */
880                 normalize_vn_vnvn(tan_center_a, pt_a, pt, dims);
881                 normalize_vn_vnvn(tan_center_b, pt, pt_b, dims);
882                 add_vn_vnvn(tan_center, tan_center_a, tan_center_b, dims);
883                 normalize_vn(tan_center, dims);
884         }
885
886         fit_cubic_to_points_recursive(
887                 points_offset, split_index + 1,
888 #ifdef USE_LENGTH_CACHE
889                 points_length_cache,
890 #endif
891                 tan_l, tan_center, error_threshold_sq, dims, clist);
892         fit_cubic_to_points_recursive(
893                 &points_offset[split_index * dims], points_offset_len - split_index,
894 #ifdef USE_LENGTH_CACHE
895                 points_length_cache + split_index,
896 #endif
897                 tan_center, tan_r, error_threshold_sq, dims, clist);
898
899 }
900
901 /** \} */
902
903
904 /* -------------------------------------------------------------------- */
905
906 /** \name External API for Curve-Fitting
907  * \{ */
908
909 /**
910  * Main function:
911  *
912  * Take an array of 3d points.
913  * return the cubic splines
914  */
915 int curve_fit_cubic_to_points_db(
916         const double *points,
917         const uint    points_len,
918         const uint    dims,
919         const double  error_threshold,
920         const uint   *corners,
921         uint          corners_len,
922
923         double **r_cubic_array, uint *r_cubic_array_len,
924         uint **r_cubic_orig_index,
925         uint **r_corner_index_array, uint *r_corner_index_len)
926 {
927         uint corners_buf[2];
928         if (corners == NULL) {
929                 assert(corners_len == 0);
930                 corners_buf[0] = 0;
931                 corners_buf[1] = points_len - 1;
932                 corners = corners_buf;
933                 corners_len = 2;
934         }
935
936         CubicList clist = {0};
937         clist.dims = dims;
938
939 #ifdef USE_VLA
940         double tan_l[dims];
941         double tan_r[dims];
942 #else
943         double *tan_l = alloca(sizeof(double) * dims);
944         double *tan_r = alloca(sizeof(double) * dims);
945 #endif
946
947 #ifdef USE_LENGTH_CACHE
948         double *points_length_cache = NULL;
949         uint    points_length_cache_len_alloc = 0;
950 #endif
951
952         uint *corner_index_array = NULL;
953         uint  corner_index = 0;
954         if (r_corner_index_array && (corners != corners_buf)) {
955                 corner_index_array = malloc(sizeof(uint) * corners_len);
956                 corner_index_array[corner_index++] = corners[0];
957         }
958
959         const double error_threshold_sq = sq(error_threshold);
960
961         for (uint i = 1; i < corners_len; i++) {
962                 const uint points_offset_len = corners[i] - corners[i - 1] + 1;
963                 const uint first_point = corners[i - 1];
964
965                 assert(points_offset_len >= 1);
966                 if (points_offset_len > 1) {
967                         const double *pt_l = &points[first_point * dims];
968                         const double *pt_r = &points[(first_point + points_offset_len - 1) * dims];
969                         const double *pt_l_next = pt_l + dims;
970                         const double *pt_r_prev = pt_r - dims;
971
972                         /* tan_l = (pt_l - pt_l_next).normalized()
973                          * tan_r = (pt_r_prev - pt_r).normalized()
974                          */
975                         normalize_vn_vnvn(tan_l, pt_l, pt_l_next, dims);
976                         normalize_vn_vnvn(tan_r, pt_r_prev, pt_r, dims);
977
978 #ifdef USE_LENGTH_CACHE
979                         if (points_length_cache_len_alloc < points_offset_len) {
980                                 if (points_length_cache) {
981                                         free(points_length_cache);
982                                 }
983                                 points_length_cache = malloc(sizeof(double) * points_offset_len);
984                         }
985                         points_calc_coord_length_cache(
986                                 &points[first_point * dims], points_offset_len, dims,
987                                 points_length_cache);
988 #endif
989
990                         fit_cubic_to_points_recursive(
991                                 &points[first_point * dims], points_offset_len,
992 #ifdef USE_LENGTH_CACHE
993                                 points_length_cache,
994 #endif
995                                 tan_l, tan_r, error_threshold_sq, dims, &clist);
996                 }
997                 else if (points_len == 1) {
998                         assert(points_offset_len == 1);
999                         assert(corners_len == 2);
1000                         assert(corners[0] == 0);
1001                         assert(corners[1] == 0);
1002                         const double *pt = &points[0];
1003                         Cubic *cubic = cubic_alloc(dims);
1004                         cubic_init(cubic, pt, pt, pt, pt, dims);
1005                         cubic_list_prepend(&clist, cubic);
1006                 }
1007
1008                 if (corner_index_array) {
1009                         corner_index_array[corner_index++] = clist.len;
1010                 }
1011         }
1012
1013 #ifdef USE_LENGTH_CACHE
1014         if (points_length_cache) {
1015                 free(points_length_cache);
1016         }
1017 #endif
1018
1019 #ifdef USE_ORIG_INDEX_DATA
1020         uint *cubic_orig_index = NULL;
1021         if (r_cubic_orig_index) {
1022                 cubic_orig_index = malloc(sizeof(uint) * (clist.len + 1));
1023         }
1024 #else
1025         *r_cubic_orig_index = NULL;
1026 #endif
1027
1028         /* allocate a contiguous array and free the linked list */
1029         *r_cubic_array = cubic_list_as_array(
1030                 &clist
1031 #ifdef USE_ORIG_INDEX_DATA
1032                 , corners[corners_len - 1], cubic_orig_index
1033 #endif
1034                 );
1035         *r_cubic_array_len = clist.len + 1;
1036
1037         cubic_list_clear(&clist);
1038
1039 #ifdef USE_ORIG_INDEX_DATA
1040         if (cubic_orig_index) {
1041                 *r_cubic_orig_index = cubic_orig_index;
1042         }
1043 #endif
1044
1045         if (corner_index_array) {
1046                 assert(corner_index == corners_len);
1047                 *r_corner_index_array = corner_index_array;
1048                 *r_corner_index_len = corner_index;
1049         }
1050
1051         return 0;
1052 }
1053
1054 /**
1055  * A version of #curve_fit_cubic_to_points_db to handle floats
1056  */
1057 int curve_fit_cubic_to_points_fl(
1058         const float  *points,
1059         const uint    points_len,
1060         const uint    dims,
1061         const float   error_threshold,
1062         const uint   *corners,
1063         const uint    corners_len,
1064
1065         float **r_cubic_array, uint *r_cubic_array_len,
1066         uint **r_cubic_orig_index,
1067         uint **r_corner_index_array, uint *r_corner_index_len)
1068 {
1069         const uint points_flat_len = points_len * dims;
1070         double *points_db = malloc(sizeof(double) * points_flat_len);
1071
1072         copy_vndb_vnfl(points_db, points, points_flat_len);
1073
1074         double *cubic_array_db = NULL;
1075         float  *cubic_array_fl = NULL;
1076         uint    cubic_array_len = 0;
1077
1078         int result = curve_fit_cubic_to_points_db(
1079                 points_db, points_len, dims, error_threshold, corners, corners_len,
1080                 &cubic_array_db, &cubic_array_len,
1081                 r_cubic_orig_index,
1082                 r_corner_index_array, r_corner_index_len);
1083         free(points_db);
1084
1085         if (!result) {
1086                 uint cubic_array_flat_len = cubic_array_len * 3 * dims;
1087                 cubic_array_fl = malloc(sizeof(float) * cubic_array_flat_len);
1088                 for (uint i = 0; i < cubic_array_flat_len; i++) {
1089                         cubic_array_fl[i] = (float)cubic_array_db[i];
1090                 }
1091                 free(cubic_array_db);
1092         }
1093
1094         *r_cubic_array = cubic_array_fl;
1095         *r_cubic_array_len = cubic_array_len;
1096
1097         return result;
1098 }
1099
1100 /**
1101  * Fit a single cubic to points.
1102  */
1103 int curve_fit_cubic_to_points_single_db(
1104         const double *points,
1105         const uint    points_len,
1106         const uint    dims,
1107         const double  error_threshold,
1108         const double tan_l[],
1109         const double tan_r[],
1110
1111         double  r_handle_l[],
1112         double  r_handle_r[],
1113         double  *r_error_max_sq)
1114 {
1115         Cubic *cubic = alloca(cubic_alloc_size(dims));
1116
1117         uint split_index;
1118
1119         /* in this instance theres no advantage in using length cache,
1120          * since we're not recursively calculating values. */
1121 #ifdef USE_LENGTH_CACHE
1122         double *points_length_cache = malloc(sizeof(double) * points_len);
1123         points_calc_coord_length_cache(
1124                 points, points_len, dims,
1125                 points_length_cache);
1126 #endif
1127
1128         fit_cubic_to_points(
1129                 points, points_len,
1130 #ifdef USE_LENGTH_CACHE
1131                 points_length_cache,
1132 #endif
1133                 tan_l, tan_r, error_threshold, dims,
1134
1135                 cubic, r_error_max_sq, &split_index);
1136
1137 #ifdef USE_LENGTH_CACHE
1138         free(points_length_cache);
1139 #endif
1140
1141         copy_vnvn(r_handle_l, CUBIC_PT(cubic, 1, dims), dims);
1142         copy_vnvn(r_handle_r, CUBIC_PT(cubic, 2, dims), dims);
1143
1144         return 0;
1145 }
1146
1147 int curve_fit_cubic_to_points_single_fl(
1148         const float  *points,
1149         const uint    points_len,
1150         const uint    dims,
1151         const float   error_threshold,
1152         const float   tan_l[],
1153         const float   tan_r[],
1154
1155         float   r_handle_l[],
1156         float   r_handle_r[],
1157         float  *r_error_sq)
1158 {
1159         const uint points_flat_len = points_len * dims;
1160         double *points_db = malloc(sizeof(double) * points_flat_len);
1161
1162         copy_vndb_vnfl(points_db, points, points_flat_len);
1163
1164 #ifdef USE_VLA
1165         double tan_l_db[dims];
1166         double tan_r_db[dims];
1167         double r_handle_l_db[dims];
1168         double r_handle_r_db[dims];
1169 #else
1170         double *tan_l_db = alloca(sizeof(double) * dims);
1171         double *tan_r_db = alloca(sizeof(double) * dims);
1172         double *r_handle_l_db = alloca(sizeof(double) * dims);
1173         double *r_handle_r_db = alloca(sizeof(double) * dims);
1174 #endif
1175         double r_error_sq_db;
1176
1177         copy_vndb_vnfl(tan_l_db, tan_l, dims);
1178         copy_vndb_vnfl(tan_r_db, tan_r, dims);
1179
1180         int result = curve_fit_cubic_to_points_single_db(
1181                 points_db, points_len, dims,
1182                 (double)error_threshold,
1183                 tan_l_db, tan_r_db,
1184                 r_handle_l_db, r_handle_r_db,
1185                 &r_error_sq_db);
1186
1187         free(points_db);
1188
1189         copy_vnfl_vndb(r_handle_l, r_handle_l_db, dims);
1190         copy_vnfl_vndb(r_handle_r, r_handle_r_db, dims);
1191         *r_error_sq = (float)r_error_sq_db;
1192
1193         return result;
1194 }
1195
1196 /** \} */