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