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