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