Curve Fitting: avoid clamping fallback handles.
[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         bool use_clamp = true;
475
476         /* flip check to catch nan values */
477         if (!(alpha_l >= 0.0) ||
478             !(alpha_r >= 0.0))
479         {
480                 alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
481
482                 /* skip clamping when we're using default handles */
483                 use_clamp = false;
484         }
485
486         double *p1 = CUBIC_PT(r_cubic, 1, dims);
487         double *p2 = CUBIC_PT(r_cubic, 2, dims);
488
489         copy_vnvn(CUBIC_PT(r_cubic, 0, dims), p0, dims);
490         copy_vnvn(CUBIC_PT(r_cubic, 3, dims), p3, dims);
491
492 #ifdef USE_ORIG_INDEX_DATA
493         r_cubic->orig_span = (points_offset_len - 1);
494 #endif
495
496         /* p1 = p0 - (tan_l * alpha_l);
497          * p2 = p3 + (tan_r * alpha_r);
498          */
499         msub_vn_vnvn_fl(p1, p0, tan_l, alpha_l, dims);
500         madd_vn_vnvn_fl(p2, p3, tan_r, alpha_r, dims);
501
502         /* ------------------------------------
503          * Clamping (we could make it optional)
504          */
505         if (use_clamp) {
506 #ifdef USE_VLA
507                 double center[dims];
508 #else
509                 double *center = alloca(sizeof(double) * dims);
510 #endif
511                 points_calc_center_weighted(points_offset, points_offset_len, dims, center);
512
513                 const double clamp_scale = 3.0;  /* clamp to 3x */
514                 double dist_sq_max = 0.0;
515
516                 {
517                         const double *pt = points_offset;
518                         for (uint i = 0; i < points_offset_len; i++, pt += dims) {
519 #if 0
520                                 double dist_sq_test = sq(len_vnvn(center, pt, dims) * clamp_scale);
521 #else
522                                 /* do inline */
523                                 double dist_sq_test = 0.0;
524                                 for (uint j = 0; j < dims; j++) {
525                                         dist_sq_test += sq((pt[j] - center[j]) * clamp_scale);
526                                 }
527 #endif
528                                 dist_sq_max = max(dist_sq_max, dist_sq_test);
529                         }
530                 }
531
532                 double p1_dist_sq = len_squared_vnvn(center, p1, dims);
533                 double p2_dist_sq = len_squared_vnvn(center, p2, dims);
534
535                 if (p1_dist_sq > dist_sq_max ||
536                     p2_dist_sq > dist_sq_max)
537                 {
538
539                         alpha_l = alpha_r = len_vnvn(p0, p3, dims) / 3.0;
540
541                         /*
542                          * p1 = p0 - (tan_l * alpha_l);
543                          * p2 = p3 + (tan_r * alpha_r);
544                          */
545                         for (uint j = 0; j < dims; j++) {
546                                 p1[j] = p0[j] - (tan_l[j] * alpha_l);
547                                 p2[j] = p3[j] + (tan_r[j] * alpha_r);
548                         }
549
550                         p1_dist_sq = len_squared_vnvn(center, p1, dims);
551                         p2_dist_sq = len_squared_vnvn(center, p2, dims);
552                 }
553
554                 /* clamp within the 3x radius */
555                 if (p1_dist_sq > dist_sq_max) {
556                         isub_vnvn(p1, center, dims);
557                         imul_vn_fl(p1, sqrt(dist_sq_max) / sqrt(p1_dist_sq), dims);
558                         iadd_vnvn(p1, center, dims);
559                 }
560                 if (p2_dist_sq > dist_sq_max) {
561                         isub_vnvn(p2, center, dims);
562                         imul_vn_fl(p2, sqrt(dist_sq_max) / sqrt(p2_dist_sq), dims);
563                         iadd_vnvn(p2, center, dims);
564                 }
565         }
566         /* end clamping */
567 }
568
569 #ifdef USE_LENGTH_CACHE
570 static void points_calc_coord_length_cache(
571         const double *points_offset,
572         const uint    points_offset_len,
573         const uint    dims,
574
575         double     *r_points_length_cache)
576 {
577         const double *pt_prev = points_offset;
578         const double *pt = pt_prev + dims;
579         r_points_length_cache[0] = 0.0;
580         for (uint i = 1; i < points_offset_len; i++) {
581                 r_points_length_cache[i] = len_vnvn(pt, pt_prev, dims);
582                 pt_prev = pt;
583                 pt += dims;
584         }
585 }
586 #endif  /* USE_LENGTH_CACHE */
587
588
589 static void points_calc_coord_length(
590         const double *points_offset,
591         const uint    points_offset_len,
592         const uint    dims,
593 #ifdef USE_LENGTH_CACHE
594         const double *points_length_cache,
595 #endif
596         double *r_u)
597 {
598         const double *pt_prev = points_offset;
599         const double *pt = pt_prev + dims;
600         r_u[0] = 0.0;
601         for (uint i = 1, i_prev = 0; i < points_offset_len; i++) {
602                 double length;
603
604 #ifdef USE_LENGTH_CACHE
605                 length = points_length_cache[i];
606
607                 assert(len_vnvn(pt, pt_prev, dims) == points_length_cache[i]);
608 #else
609                 length = len_vnvn(pt, pt_prev, dims);
610 #endif
611
612                 r_u[i] = r_u[i_prev] + length;
613                 i_prev = i;
614                 pt_prev = pt;
615                 pt += dims;
616         }
617         assert(!is_almost_zero(r_u[points_offset_len - 1]));
618         const double w = r_u[points_offset_len - 1];
619         for (uint i = 0; i < points_offset_len; i++) {
620                 r_u[i] /= w;
621         }
622 }
623
624 /**
625  * Use Newton-Raphson iteration to find better root.
626  *
627  * \param cubic: Current fitted curve.
628  * \param p: Point to test against.
629  * \param u: Parameter value for \a p.
630  *
631  * \note Return value may be `nan` caller must check for this.
632  */
633 static double cubic_find_root(
634                 const Cubic *cubic,
635                 const double p[],
636                 const double u,
637                 const uint dims)
638 {
639         /* Newton-Raphson Method. */
640         /* all vectors */
641 #ifdef USE_VLA
642         double q0_u[dims];
643         double q1_u[dims];
644         double q2_u[dims];
645 #else
646         double *q0_u = alloca(sizeof(double) * dims);
647         double *q1_u = alloca(sizeof(double) * dims);
648         double *q2_u = alloca(sizeof(double) * dims);
649 #endif
650
651         cubic_calc_point(cubic, u, dims, q0_u);
652         cubic_calc_speed(cubic, u, dims, q1_u);
653         cubic_calc_acceleration(cubic, u, dims, q2_u);
654
655         /* may divide-by-zero, caller must check for that case */
656         /* u - ((q0_u - p) * q1_u) / (q1_u.length_squared() + (q0_u - p) * q2_u) */
657         isub_vnvn(q0_u, p, dims);
658         return u - dot_vnvn(q0_u, q1_u, dims) /
659                (len_squared_vn(q1_u, dims) + dot_vnvn(q0_u, q2_u, dims));
660 }
661
662 static int compare_double_fn(const void *a_, const void *b_)
663 {
664         const double *a = a_;
665         const double *b = b_;
666         if      (*a > *b) return  1;
667         else if (*a < *b) return -1;
668         else              return  0;
669 }
670
671 /**
672  * Given set of points and their parameterization, try to find a better parameterization.
673  */
674 static bool cubic_reparameterize(
675         const Cubic *cubic,
676         const double *points_offset,
677         const uint    points_offset_len,
678         const double *u,
679         const uint    dims,
680
681         double       *r_u_prime)
682 {
683         /*
684          * Recalculate the values of u[] based on the Newton Raphson method
685          */
686
687         const double *pt = points_offset;
688         for (uint i = 0; i < points_offset_len; i++, pt += dims) {
689                 r_u_prime[i] = cubic_find_root(cubic, pt, u[i], dims);
690                 if (!isfinite(r_u_prime[i])) {
691                         return false;
692                 }
693         }
694
695         qsort(r_u_prime, points_offset_len, sizeof(double), compare_double_fn);
696
697         if ((r_u_prime[0] < 0.0) ||
698             (r_u_prime[points_offset_len - 1] > 1.0))
699         {
700                 return false;
701         }
702
703         assert(r_u_prime[0] >= 0.0);
704         assert(r_u_prime[points_offset_len - 1] <= 1.0);
705         return true;
706 }
707
708
709 static bool fit_cubic_to_points(
710         const double *points_offset,
711         const uint    points_offset_len,
712 #ifdef USE_LENGTH_CACHE
713         const double *points_length_cache,
714 #endif
715         const double  tan_l[],
716         const double  tan_r[],
717         const double  error_threshold_sq,
718         const uint    dims,
719
720         Cubic *r_cubic, double *r_error_max_sq, uint *r_split_index)
721 {
722         const uint iteration_max = 4;
723
724         if (points_offset_len == 2) {
725                 CUBIC_VARS(r_cubic, dims, p0, p1, p2, p3);
726
727                 copy_vnvn(p0, &points_offset[0 * dims], dims);
728                 copy_vnvn(p3, &points_offset[1 * dims], dims);
729
730                 const double dist = len_vnvn(p0, p3, dims) / 3.0;
731                 msub_vn_vnvn_fl(p1, p0, tan_l, dist, dims);
732                 madd_vn_vnvn_fl(p2, p3, tan_r, dist, dims);
733
734 #ifdef USE_ORIG_INDEX_DATA
735                 r_cubic->orig_span = 1;
736 #endif
737                 return true;
738         }
739
740         double *u = malloc(sizeof(double) * points_offset_len);
741         points_calc_coord_length(
742                 points_offset, points_offset_len, dims,
743 #ifdef USE_LENGTH_CACHE
744                 points_length_cache,
745 #endif
746                 u);
747
748         double error_max_sq;
749         uint split_index;
750
751         /* Parameterize points, and attempt to fit curve */
752         cubic_from_points(
753                 points_offset, points_offset_len, u, tan_l, tan_r, dims, r_cubic);
754
755         /* Find max deviation of points to fitted curve */
756         error_max_sq = cubic_calc_error(
757                 r_cubic, points_offset, points_offset_len, u, dims,
758                 &split_index);
759
760         *r_error_max_sq = error_max_sq;
761         *r_split_index  = split_index;
762
763         if (error_max_sq < error_threshold_sq) {
764                 free(u);
765                 return true;
766         }
767         else {
768                 Cubic *cubic_test = alloca(cubic_alloc_size(dims));
769                 *cubic_test = *r_cubic;
770
771                 /* If error not too large, try some reparameterization and iteration */
772                 double *u_prime = malloc(sizeof(double) * points_offset_len);
773                 for (uint iter = 0; iter < iteration_max; iter++) {
774                         if (!cubic_reparameterize(
775                                 cubic_test, points_offset, points_offset_len, u, dims, u_prime))
776                         {
777                                 break;
778                         }
779
780                         cubic_from_points(
781                                 points_offset, points_offset_len, u_prime,
782                                 tan_l, tan_r, dims, cubic_test);
783                         error_max_sq = cubic_calc_error(
784                                 cubic_test, points_offset, points_offset_len, u_prime, dims,
785                                 &split_index);
786
787                         if (error_max_sq < error_threshold_sq) {
788                                 free(u_prime);
789                                 free(u);
790
791                                 *r_cubic = *cubic_test;
792                                 *r_error_max_sq = error_max_sq;
793                                 *r_split_index  = split_index;
794                                 return true;
795                         }
796                         else if (error_max_sq < *r_error_max_sq) {
797                                 *r_cubic = *cubic_test;
798                                 *r_error_max_sq = error_max_sq;
799                                 *r_split_index = split_index;
800                         }
801
802                         SWAP(double *, u, u_prime);
803                 }
804                 free(u_prime);
805                 free(u);
806
807                 return false;
808         }
809 }
810
811 static void fit_cubic_to_points_recursive(
812         const double *points_offset,
813         const uint    points_offset_len,
814 #ifdef USE_LENGTH_CACHE
815         const double *points_length_cache,
816 #endif
817         const double  tan_l[],
818         const double  tan_r[],
819         const double  error_threshold_sq,
820         const uint    dims,
821         /* fill in the list */
822         CubicList *clist)
823 {
824         Cubic *cubic = cubic_alloc(dims);
825         uint split_index;
826         double error_max_sq;
827
828         if (fit_cubic_to_points(
829                 points_offset, points_offset_len,
830 #ifdef USE_LENGTH_CACHE
831                 points_length_cache,
832 #endif
833                 tan_l, tan_r, error_threshold_sq, dims,
834                 cubic, &error_max_sq, &split_index))
835         {
836                 cubic_list_prepend(clist, cubic);
837                 return;
838         }
839         cubic_free(cubic);
840
841
842         /* Fitting failed -- split at max error point and fit recursively */
843
844         /* Check splinePoint is not an endpoint?
845          *
846          * This assert happens sometimes...
847          * Look into it but disable for now. Campbell! */
848
849         // assert(split_index > 1)
850 #ifdef USE_VLA
851         double tan_center[dims];
852 #else
853         double *tan_center = alloca(sizeof(double) * dims);
854 #endif
855
856         const double *pt_a = &points_offset[(split_index - 1) * dims];
857         const double *pt_b = &points_offset[(split_index + 1) * dims];
858
859         assert(split_index < points_offset_len);
860         if (equals_vnvn(pt_a, pt_b, dims)) {
861                 pt_a += dims;
862         }
863
864         {
865 #ifdef USE_VLA
866                 double tan_center_a[dims];
867                 double tan_center_b[dims];
868 #else
869                 double *tan_center_a = alloca(sizeof(double) * dims);
870                 double *tan_center_b = alloca(sizeof(double) * dims);
871 #endif
872                 const double *pt   = &points_offset[split_index * dims];
873
874                 /* tan_center = ((pt_a - pt).normalized() + (pt - pt_b).normalized()).normalized() */
875                 normalize_vn_vnvn(tan_center_a, pt_a, pt, dims);
876                 normalize_vn_vnvn(tan_center_b, pt, pt_b, dims);
877                 add_vn_vnvn(tan_center, tan_center_a, tan_center_b, dims);
878                 normalize_vn(tan_center, dims);
879         }
880
881         fit_cubic_to_points_recursive(
882                 points_offset, split_index + 1,
883 #ifdef USE_LENGTH_CACHE
884                 points_length_cache,
885 #endif
886                 tan_l, tan_center, error_threshold_sq, dims, clist);
887         fit_cubic_to_points_recursive(
888                 &points_offset[split_index * dims], points_offset_len - split_index,
889 #ifdef USE_LENGTH_CACHE
890                 points_length_cache + split_index,
891 #endif
892                 tan_center, tan_r, error_threshold_sq, dims, clist);
893
894 }
895
896 /** \} */
897
898
899 /* -------------------------------------------------------------------- */
900
901 /** \name External API for Curve-Fitting
902  * \{ */
903
904 /**
905  * Main function:
906  *
907  * Take an array of 3d points.
908  * return the cubic splines
909  */
910 int curve_fit_cubic_to_points_db(
911         const double *points,
912         const uint    points_len,
913         const uint    dims,
914         const double  error_threshold,
915         const uint   *corners,
916         uint          corners_len,
917
918         double **r_cubic_array, uint *r_cubic_array_len,
919         uint **r_cubic_orig_index,
920         uint **r_corner_index_array, uint *r_corner_index_len)
921 {
922         uint corners_buf[2];
923         if (corners == NULL) {
924                 assert(corners_len == 0);
925                 corners_buf[0] = 0;
926                 corners_buf[1] = points_len - 1;
927                 corners = corners_buf;
928                 corners_len = 2;
929         }
930
931         CubicList clist = {0};
932         clist.dims = dims;
933
934 #ifdef USE_VLA
935         double tan_l[dims];
936         double tan_r[dims];
937 #else
938         double *tan_l = alloca(sizeof(double) * dims);
939         double *tan_r = alloca(sizeof(double) * dims);
940 #endif
941
942 #ifdef USE_LENGTH_CACHE
943         double *points_length_cache = NULL;
944         uint    points_length_cache_len_alloc = 0;
945 #endif
946
947         uint *corner_index_array = NULL;
948         uint  corner_index = 0;
949         if (r_corner_index_array && (corners != corners_buf)) {
950                 corner_index_array = malloc(sizeof(uint) * corners_len);
951                 corner_index_array[corner_index++] = corners[0];
952         }
953
954         const double error_threshold_sq = sq(error_threshold);
955
956         for (uint i = 1; i < corners_len; i++) {
957                 const uint points_offset_len = corners[i] - corners[i - 1] + 1;
958                 const uint first_point = corners[i - 1];
959
960                 assert(points_offset_len >= 1);
961                 if (points_offset_len > 1) {
962                         const double *pt_l = &points[first_point * dims];
963                         const double *pt_r = &points[(first_point + points_offset_len - 1) * dims];
964                         const double *pt_l_next = pt_l + dims;
965                         const double *pt_r_prev = pt_r - dims;
966
967                         /* tan_l = (pt_l - pt_l_next).normalized()
968                          * tan_r = (pt_r_prev - pt_r).normalized()
969                          */
970                         normalize_vn_vnvn(tan_l, pt_l, pt_l_next, dims);
971                         normalize_vn_vnvn(tan_r, pt_r_prev, pt_r, dims);
972
973 #ifdef USE_LENGTH_CACHE
974                         if (points_length_cache_len_alloc < points_offset_len) {
975                                 if (points_length_cache) {
976                                         free(points_length_cache);
977                                 }
978                                 points_length_cache = malloc(sizeof(double) * points_offset_len);
979                         }
980                         points_calc_coord_length_cache(
981                                 &points[first_point * dims], points_offset_len, dims,
982                                 points_length_cache);
983 #endif
984
985                         fit_cubic_to_points_recursive(
986                                 &points[first_point * dims], points_offset_len,
987 #ifdef USE_LENGTH_CACHE
988                                 points_length_cache,
989 #endif
990                                 tan_l, tan_r, error_threshold_sq, dims, &clist);
991                 }
992                 else if (points_len == 1) {
993                         assert(points_offset_len == 1);
994                         assert(corners_len == 2);
995                         assert(corners[0] == 0);
996                         assert(corners[1] == 0);
997                         const double *pt = &points[0];
998                         Cubic *cubic = cubic_alloc(dims);
999                         cubic_init(cubic, pt, pt, pt, pt, dims);
1000                         cubic_list_prepend(&clist, cubic);
1001                 }
1002
1003                 if (corner_index_array) {
1004                         corner_index_array[corner_index++] = clist.len;
1005                 }
1006         }
1007
1008 #ifdef USE_LENGTH_CACHE
1009         if (points_length_cache) {
1010                 free(points_length_cache);
1011         }
1012 #endif
1013
1014 #ifdef USE_ORIG_INDEX_DATA
1015         uint *cubic_orig_index = NULL;
1016         if (r_cubic_orig_index) {
1017                 cubic_orig_index = malloc(sizeof(uint) * (clist.len + 1));
1018         }
1019 #else
1020         *r_cubic_orig_index = NULL;
1021 #endif
1022
1023         /* allocate a contiguous array and free the linked list */
1024         *r_cubic_array = cubic_list_as_array(
1025                 &clist
1026 #ifdef USE_ORIG_INDEX_DATA
1027                 , corners[corners_len - 1], cubic_orig_index
1028 #endif
1029                 );
1030         *r_cubic_array_len = clist.len + 1;
1031
1032         cubic_list_clear(&clist);
1033
1034 #ifdef USE_ORIG_INDEX_DATA
1035         if (cubic_orig_index) {
1036                 *r_cubic_orig_index = cubic_orig_index;
1037         }
1038 #endif
1039
1040         if (corner_index_array) {
1041                 assert(corner_index == corners_len);
1042                 *r_corner_index_array = corner_index_array;
1043                 *r_corner_index_len = corner_index;
1044         }
1045
1046         return 0;
1047 }
1048
1049 /**
1050  * A version of #curve_fit_cubic_to_points_db to handle floats
1051  */
1052 int curve_fit_cubic_to_points_fl(
1053         const float  *points,
1054         const uint    points_len,
1055         const uint    dims,
1056         const float   error_threshold,
1057         const uint   *corners,
1058         const uint    corners_len,
1059
1060         float **r_cubic_array, uint *r_cubic_array_len,
1061         uint **r_cubic_orig_index,
1062         uint **r_corner_index_array, uint *r_corner_index_len)
1063 {
1064         const uint points_flat_len = points_len * dims;
1065         double *points_db = malloc(sizeof(double) * points_flat_len);
1066
1067         copy_vndb_vnfl(points_db, points, points_flat_len);
1068
1069         double *cubic_array_db = NULL;
1070         float  *cubic_array_fl = NULL;
1071         uint    cubic_array_len = 0;
1072
1073         int result = curve_fit_cubic_to_points_db(
1074                 points_db, points_len, dims, error_threshold, corners, corners_len,
1075                 &cubic_array_db, &cubic_array_len,
1076                 r_cubic_orig_index,
1077                 r_corner_index_array, r_corner_index_len);
1078         free(points_db);
1079
1080         if (!result) {
1081                 uint cubic_array_flat_len = cubic_array_len * 3 * dims;
1082                 cubic_array_fl = malloc(sizeof(float) * cubic_array_flat_len);
1083                 for (uint i = 0; i < cubic_array_flat_len; i++) {
1084                         cubic_array_fl[i] = (float)cubic_array_db[i];
1085                 }
1086                 free(cubic_array_db);
1087         }
1088
1089         *r_cubic_array = cubic_array_fl;
1090         *r_cubic_array_len = cubic_array_len;
1091
1092         return result;
1093 }
1094
1095 /**
1096  * Fit a single cubic to points.
1097  */
1098 int curve_fit_cubic_to_points_single_db(
1099         const double *points,
1100         const uint    points_len,
1101         const uint    dims,
1102         const double  error_threshold,
1103         const double tan_l[],
1104         const double tan_r[],
1105
1106         double  r_handle_l[],
1107         double  r_handle_r[],
1108         double  *r_error_max_sq)
1109 {
1110         Cubic *cubic = alloca(cubic_alloc_size(dims));
1111
1112         uint split_index;
1113
1114         /* in this instance theres no advantage in using length cache,
1115          * since we're not recursively calculating values. */
1116 #ifdef USE_LENGTH_CACHE
1117         double *points_length_cache = malloc(sizeof(double) * points_len);
1118         points_calc_coord_length_cache(
1119                 points, points_len, dims,
1120                 points_length_cache);
1121 #endif
1122
1123         fit_cubic_to_points(
1124                 points, points_len,
1125 #ifdef USE_LENGTH_CACHE
1126                 points_length_cache,
1127 #endif
1128                 tan_l, tan_r, error_threshold, dims,
1129
1130                 cubic, r_error_max_sq, &split_index);
1131
1132 #ifdef USE_LENGTH_CACHE
1133         free(points_length_cache);
1134 #endif
1135
1136         copy_vnvn(r_handle_l, CUBIC_PT(cubic, 1, dims), dims);
1137         copy_vnvn(r_handle_r, CUBIC_PT(cubic, 2, dims), dims);
1138
1139         return 0;
1140 }
1141
1142 int curve_fit_cubic_to_points_single_fl(
1143         const float  *points,
1144         const uint    points_len,
1145         const uint    dims,
1146         const float   error_threshold,
1147         const float   tan_l[],
1148         const float   tan_r[],
1149
1150         float   r_handle_l[],
1151         float   r_handle_r[],
1152         float  *r_error_sq)
1153 {
1154         const uint points_flat_len = points_len * dims;
1155         double *points_db = malloc(sizeof(double) * points_flat_len);
1156
1157         copy_vndb_vnfl(points_db, points, points_flat_len);
1158
1159 #ifdef USE_VLA
1160         double tan_l_db[dims];
1161         double tan_r_db[dims];
1162         double r_handle_l_db[dims];
1163         double r_handle_r_db[dims];
1164 #else
1165         double *tan_l_db = alloca(sizeof(double) * dims);
1166         double *tan_r_db = alloca(sizeof(double) * dims);
1167         double *r_handle_l_db = alloca(sizeof(double) * dims);
1168         double *r_handle_r_db = alloca(sizeof(double) * dims);
1169 #endif
1170         double r_error_sq_db;
1171
1172         copy_vndb_vnfl(tan_l_db, tan_l, dims);
1173         copy_vndb_vnfl(tan_r_db, tan_r, dims);
1174
1175         int result = curve_fit_cubic_to_points_single_db(
1176                 points_db, points_len, dims,
1177                 (double)error_threshold,
1178                 tan_l_db, tan_r_db,
1179                 r_handle_l_db, r_handle_r_db,
1180                 &r_error_sq_db);
1181
1182         free(points_db);
1183
1184         copy_vnfl_vndb(r_handle_l, r_handle_l_db, dims);
1185         copy_vnfl_vndb(r_handle_r, r_handle_r_db, dims);
1186         *r_error_sq = (float)r_error_sq_db;
1187
1188         return result;
1189 }
1190
1191 /** \} */