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