Cleanup: whicespace
[blender.git] / extern / curve_fit_nd / intern / curve_fit_corners_detect.c
1 /*
2  * Copyright (c) 2016, Blender Foundation.
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_corners_detect.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 typedef unsigned int uint;
43
44 #include "curve_fit_inline.h"
45
46 #ifdef _MSC_VER
47 #  define alloca(size) _alloca(size)
48 #endif
49
50 #if !defined(_MSC_VER)
51 #  define USE_VLA
52 #endif
53
54 #ifdef USE_VLA
55 #  ifdef __GNUC__
56 #    pragma GCC diagnostic ignored "-Wvla"
57 #  endif
58 #else
59 #  ifdef __GNUC__
60 #    pragma GCC diagnostic error "-Wvla"
61 #  endif
62 #endif
63
64 /* -------------------------------------------------------------------- */
65
66 /** \name Simple Vector Math Lib
67  * \{ */
68
69 static double cos_vnvnvn(
70         const double v0[], const double v1[], const double v2[],
71         const uint dims)
72 {
73 #ifdef USE_VLA
74         double dvec0[dims];
75         double dvec1[dims];
76 #else
77         double *dvec0 = alloca(sizeof(double) * dims);
78         double *dvec1 = alloca(sizeof(double) * dims);
79 #endif
80         normalize_vn_vnvn(dvec0, v0, v1, dims);
81         normalize_vn_vnvn(dvec1, v1, v2, dims);
82         double d = dot_vnvn(dvec0, dvec1, dims);
83         /* sanity check */
84         d = max(-1.0, min(1.0, d));
85         return d;
86 }
87
88 static double angle_vnvnvn(
89         const double v0[], const double v1[], const double v2[],
90         const uint dims)
91 {
92         return acos(cos_vnvnvn(v0, v1, v2, dims));
93 }
94
95
96 static bool isect_line_sphere_vn(
97         const double l1[],
98         const double l2[],
99         const double sp[],
100         const double r,
101         uint dims,
102
103         double r_p1[]
104 #if 0   /* UNUSED */
105         double r_p2[]
106 #endif
107         )
108 {
109 #ifdef USE_VLA
110         double ldir[dims];
111         double tvec[dims];
112 #else
113         double *ldir = alloca(sizeof(double) * dims);
114         double *tvec = alloca(sizeof(double) * dims);
115 #endif
116
117         sub_vn_vnvn(ldir, l2, l1, dims);
118
119         sub_vn_vnvn(tvec, l1, sp, dims);
120         const double a = len_squared_vn(ldir, dims);
121         const double b = 2.0 * dot_vnvn(ldir, tvec, dims);
122         const double c = len_squared_vn(sp, dims) + len_squared_vn(l1, dims) - (2.0 * dot_vnvn(sp, l1, dims)) - sq(r);
123
124         const double i = b * b - 4.0 * a * c;
125
126         if ((i < 0.0) || (a == 0.0)) {
127                 return false;
128         }
129         else if (i == 0.0) {
130                 /* one intersection */
131                 const double mu = -b / (2.0 * a);
132                 mul_vnvn_fl(r_p1, ldir, mu, dims);
133                 iadd_vnvn(r_p1, l1, dims);
134                 return true;
135         }
136         else if (i > 0.0) {
137                 /* # avoid calc twice */
138                 const double i_sqrt = sqrt(i);
139                 double mu;
140
141                 /* Note: when l1 is inside the sphere and l2 is outside.
142                  * the first intersection point will always be between the pair. */
143
144                 /* first intersection */
145                 mu = (-b + i_sqrt) / (2.0 * a);
146                 mul_vnvn_fl(r_p1, ldir, mu, dims);
147                 iadd_vnvn(r_p1, l1, dims);
148 #if 0
149                 /* second intersection */
150                 mu = (-b - i_sqrt) / (2.0 * a);
151                 mul_vnvn_fl(r_p2, ldir, mu, dims);
152                 iadd_vnvn(r_p2, l1, dims);
153 #endif
154                 return true;
155         }
156         else {
157                 return false;
158         }
159 }
160
161 /** \} */
162
163
164 /* -------------------------------------------------------------------- */
165
166
167 static bool point_corner_measure(
168         const double *points,
169         const uint    points_len,
170         const uint i,
171         const uint i_prev_init,
172         const uint i_next_init,
173         const double radius,
174         const uint samples_max,
175         const uint dims,
176
177         double r_p_prev[], uint *r_i_prev_next,
178         double r_p_next[], uint *r_i_next_prev)
179 {
180         const double *p = &points[i * dims];
181         uint sample;
182
183
184         uint i_prev = i_prev_init;
185         uint i_prev_next = i_prev + 1;
186         sample = 0;
187         while (true) {
188                 if ((i_prev == -1) || (sample++ > samples_max)) {
189                         return false;
190                 }
191                 else if (len_squared_vnvn(p, &points[i_prev * dims], dims) < radius) {
192                         i_prev -= 1;
193                 }
194                 else {
195                         break;
196                 }
197         }
198
199         uint i_next = i_next_init;
200         uint i_next_prev = i_next - 1;
201         sample = 0;
202         while (true) {
203                 if ((i_next == points_len) || (sample++ > samples_max)) {
204                         return false;
205                 }
206                 else if (len_squared_vnvn(p, &points[i_next * dims], dims) < radius) {
207                         i_next += 1;
208                 }
209                 else {
210                         break;
211                 }
212         }
213
214         /* find points on the sphere */
215         if (!isect_line_sphere_vn(
216                 &points[i_prev * dims], &points[i_prev_next * dims], p, radius, dims,
217                 r_p_prev))
218         {
219                 return false;
220         }
221
222         if (!isect_line_sphere_vn(
223                 &points[i_next * dims], &points[i_next_prev * dims], p, radius, dims,
224                 r_p_next))
225         {
226                 return false;
227         }
228
229         *r_i_prev_next = i_prev_next;
230         *r_i_next_prev = i_next_prev;
231
232         return true;
233 }
234
235
236 static double point_corner_angle(
237         const double *points,
238         const uint    points_len,
239         const uint i,
240         const double radius_mid,
241         const double radius_max,
242         const double angle_threshold,
243         const double angle_threshold_cos,
244         /* prevent locking up when for example `radius_min` is very large
245          * (possibly larger then the curve).
246          * In this case we would end up checking every point from every other point,
247          * never reaching one that was outside the `radius_min`. */
248
249         /* prevent locking up when for e */
250         const uint samples_max,
251
252         const uint dims)
253 {
254         assert(angle_threshold_cos == cos(angle_threshold));
255
256         if (i == 0 || i == points_len - 1) {
257                 return 0.0;
258         }
259
260         const double *p = &points[i * dims];
261
262         /* initial test */
263         if (cos_vnvnvn(&points[(i - 1) * dims], p, &points[(i + 1) * dims], dims) > angle_threshold_cos) {
264                 return 0.0;
265         }
266
267 #ifdef USE_VLA
268         double p_mid_prev[dims];
269         double p_mid_next[dims];
270 #else
271         double *p_mid_prev = alloca(sizeof(double) * dims);
272         double *p_mid_next = alloca(sizeof(double) * dims);
273 #endif
274
275         uint i_mid_prev_next, i_mid_next_prev;
276         if (point_corner_measure(
277                 points, points_len,
278                 i, i - 1, i + 1,
279                 radius_mid,
280                 samples_max,
281                 dims,
282
283                 p_mid_prev, &i_mid_prev_next,
284                 p_mid_next, &i_mid_next_prev))
285         {
286                 const double angle_mid_cos = cos_vnvnvn(p_mid_prev, p, p_mid_next, dims);
287
288                 /* compare as cos and flip direction */
289
290                 /* if (angle_mid > angle_threshold) { */
291                 if (angle_mid_cos < angle_threshold_cos) {
292 #ifdef USE_VLA
293                         double p_max_prev[dims];
294                         double p_max_next[dims];
295 #else
296                         double *p_max_prev = alloca(sizeof(double) * dims);
297                         double *p_max_next = alloca(sizeof(double) * dims);
298 #endif
299
300                         uint i_max_prev_next, i_max_next_prev;
301                         if (point_corner_measure(
302                                 points, points_len,
303                                 i, i - 1, i + 1,
304                                 radius_max,
305                                 samples_max,
306                                 dims,
307
308                                 p_max_prev, &i_max_prev_next,
309                                 p_max_next, &i_max_next_prev))
310                         {
311                                 const double angle_mid = acos(angle_mid_cos);
312                                 const double angle_max = angle_vnvnvn(p_max_prev, p, p_max_next, dims) / 2.0;
313                                 const double angle_diff = angle_mid - angle_max;
314                                 if (angle_diff > angle_threshold) {
315                                         return angle_diff;
316                                 }
317                         }
318                 }
319         }
320
321         return 0.0;
322 }
323
324
325 int curve_fit_corners_detect_db(
326         const double *points,
327         const uint    points_len,
328         const uint dims,
329         const double radius_min,  /* ignore values below this */
330         const double radius_max,  /* ignore values above this */
331         const uint samples_max,
332         const double angle_threshold,
333
334         uint **r_corners,
335         uint  *r_corners_len)
336 {
337         const double angle_threshold_cos = cos(angle_threshold);
338         uint corners_len = 0;
339
340         /* Use the difference in angle between the mid-max radii
341          * to detect the difference between a corner and a sharp turn. */
342         const double radius_mid = (radius_min + radius_max) / 2.0;
343
344         /* we could ignore first/last- but simple to keep aligned with the point array */
345         double *points_angle = malloc(sizeof(double) * points_len);
346         points_angle[0] = 0.0;
347
348         *r_corners = NULL;
349         *r_corners_len = 0;
350
351         for (uint i = 0; i < points_len; i++) {
352                 points_angle[i] =  point_corner_angle(
353                         points, points_len, i,
354                         radius_mid, radius_max,
355                         angle_threshold, angle_threshold_cos,
356                         samples_max,
357                         dims);
358
359                 if (points_angle[i] != 0.0) {
360                         corners_len++;
361                 }
362         }
363
364         if (corners_len == 0) {
365                 free(points_angle);
366                 return 0;
367         }
368
369         /* Clean angle limits!
370          *
371          * How this works:
372          * - Find contiguous 'corners' (where the distance is less or equal to the error threshold).
373          * - Keep track of the corner with the highest angle
374          * - Clear every other angle (so they're ignored when setting corners). */
375         {
376                 const double radius_min_sq = sq(radius_min);
377                 uint i_span_start = 0;
378                 while (i_span_start < points_len) {
379                         uint i_span_end = i_span_start;
380                         if (points_angle[i_span_start] != 0.0) {
381                                 uint i_next = i_span_start + 1;
382                                 uint i_best = i_span_start;
383                                 while (i_next < points_len) {
384                                         if ((points_angle[i_next] == 0.0) ||
385                                             (len_squared_vnvn(
386                                                  &points[(i_next - 1) * dims],
387                                                  &points[i_next * dims], dims) > radius_min_sq))
388                                         {
389                                                 break;
390                                         }
391                                         else {
392                                                 if (points_angle[i_best] < points_angle[i_next]) {
393                                                         i_best = i_next;
394                                                 }
395                                                 i_span_end = i_next;
396                                                 i_next += 1;
397                                         }
398                                 }
399
400                                 if (i_span_start != i_span_end) {
401                                         uint i = i_span_start;
402                                         while (i <= i_span_end) {
403                                                 if (i != i_best) {
404                                                         /* we could use some other error code */
405                                                         assert(points_angle[i] != 0.0);
406                                                         points_angle[i] = 0.0;
407                                                         corners_len--;
408                                                 }
409                                                 i += 1;
410                                         }
411                                 }
412                         }
413                         i_span_start = i_span_end + 1;
414                 }
415         }
416         /* End angle limit cleaning! */
417
418         corners_len += 2;  /* first and last */
419         uint *corners = malloc(sizeof(uint) * corners_len);
420         uint i_corner = 0;
421         corners[i_corner++] = 0;
422         for (uint i = 0; i < points_len; i++) {
423                 if (points_angle[i] != 0.0) {
424                         corners[i_corner++] = i;
425                 }
426         }
427         corners[i_corner++] = points_len - 1;
428         assert(i_corner == corners_len);
429
430         free(points_angle);
431
432         *r_corners = corners;
433         *r_corners_len = corners_len;
434
435         return 0;
436 }
437
438 int curve_fit_corners_detect_fl(
439         const float *points,
440         const uint   points_len,
441         const uint dims,
442         const float radius_min,  /* ignore values below this */
443         const float radius_max,  /* ignore values above this */
444         const uint samples_max,
445         const float angle_threshold,
446
447         uint **r_corners,
448         uint  *r_corners_len)
449 {
450         const uint points_flat_len = points_len * dims;
451         double *points_db = malloc(sizeof(double) * points_flat_len);
452
453         for (uint i = 0; i < points_flat_len; i++) {
454                 points_db[i] = (double)points[i];
455         }
456
457         int result = curve_fit_corners_detect_db(
458                 points_db, points_len,
459                 dims,
460                 radius_min, radius_max,
461                 samples_max,
462                 angle_threshold,
463                 r_corners, r_corners_len);
464
465         free(points_db);
466
467         return result;
468 }