Add lib for n-dimensional cubic curve fitting
[blender-staging.git] / extern / curve_fit_nd / intern / curve_fit_inline.h
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
29 /** \file curve_fit_inline.h
30  *  \ingroup curve_fit
31  */
32
33 /** \name Simple Vector Math Lib
34  * \{ */
35
36 #ifdef _MSC_VER
37 #  define MINLINE static __forceinline
38 #else
39 #  define MINLINE static inline
40 #endif
41
42 MINLINE double sq(const double d)
43 {
44         return d * d;
45 }
46
47 #ifndef _MSC_VER
48 MINLINE double min(const double a, const double b)
49 {
50         return b < a ? b : a;
51 }
52
53 MINLINE double max(const double a, const double b)
54 {
55         return a < b ? b : a;
56 }
57 #endif
58
59 MINLINE void zero_vn(
60                 double v0[], const uint dims)
61 {
62         for (uint j = 0; j < dims; j++) {
63                 v0[j] = 0.0;
64         }
65 }
66
67 MINLINE void flip_vn_vnvn(
68                 double v_out[], const double v0[], const double v1[], const uint dims)
69 {
70         for (uint j = 0; j < dims; j++) {
71                 v_out[j] = v0[j] + (v0[j] - v1[j]);
72         }
73 }
74
75 MINLINE void copy_vnvn(
76         double v0[], const double v1[], const uint dims)
77 {
78         for (uint j = 0; j < dims; j++) {
79                 v0[j] = v1[j];
80         }
81 }
82
83 MINLINE double dot_vnvn(
84         const double v0[], const double v1[], const uint dims)
85 {
86         double d = 0.0;
87         for (uint j = 0; j < dims; j++) {
88                 d += v0[j] * v1[j];
89         }
90         return d;
91 }
92
93 MINLINE void add_vn_vnvn(
94         double v_out[], const double v0[], const double v1[], const uint dims)
95 {
96         for (uint j = 0; j < dims; j++) {
97                 v_out[j] = v0[j] + v1[j];
98         }
99 }
100
101 MINLINE void sub_vn_vnvn(
102         double v_out[], const double v0[], const double v1[], const uint dims)
103 {
104         for (uint j = 0; j < dims; j++) {
105                 v_out[j] = v0[j] - v1[j];
106         }
107 }
108
109 MINLINE void iadd_vnvn(
110         double v0[], const double v1[], const uint dims)
111 {
112         for (uint j = 0; j < dims; j++) {
113                 v0[j] += v1[j];
114         }
115 }
116
117 MINLINE void isub_vnvn(
118         double v0[], const double v1[], const uint dims)
119 {
120         for (uint j = 0; j < dims; j++) {
121                 v0[j] -= v1[j];
122         }
123 }
124
125 MINLINE void madd_vn_vnvn_fl(
126         double v_out[],
127         const double v0[], const double v1[],
128         const double f, const uint dims)
129 {
130         for (uint j = 0; j < dims; j++) {
131                 v_out[j] = v0[j] + v1[j] * f;
132         }
133 }
134
135 MINLINE void msub_vn_vnvn_fl(
136         double v_out[],
137         const double v0[], const double v1[],
138         const double f, const uint dims)
139 {
140         for (uint j = 0; j < dims; j++) {
141                 v_out[j] = v0[j] - v1[j] * f;
142         }
143 }
144
145 MINLINE void miadd_vn_vn_fl(
146         double v_out[], const double v0[], double f, const uint dims)
147 {
148         for (uint j = 0; j < dims; j++) {
149                 v_out[j] += v0[j] * f;
150         }
151 }
152
153 #if 0
154 MINLINE void misub_vn_vn_fl(
155         double v_out[], const double v0[], double f, const uint dims)
156 {
157         for (uint j = 0; j < dims; j++) {
158                 v_out[j] -= v0[j] * f;
159         }
160 }
161 #endif
162
163 MINLINE void mul_vnvn_fl(
164         double v_out[],
165         const double v0[], const double f, const uint dims)
166 {
167         for (uint j = 0; j < dims; j++) {
168                 v_out[j] = v0[j] * f;
169         }
170 }
171
172 MINLINE void imul_vn_fl(double v0[], const double f, const uint dims)
173 {
174         for (uint j = 0; j < dims; j++) {
175                 v0[j] *= f;
176         }
177 }
178
179
180 MINLINE double len_squared_vnvn(
181                 const double v0[], const double v1[], const uint dims)
182 {
183         double d = 0.0;
184         for (uint j = 0; j < dims; j++) {
185                 d += sq(v0[j] - v1[j]);
186         }
187         return d;
188 }
189
190 MINLINE double len_squared_vn(
191         const double v0[], const uint dims)
192 {
193         double d = 0.0;
194         for (uint j = 0; j < dims; j++) {
195                 d += sq(v0[j]);
196         }
197         return d;
198 }
199
200 MINLINE double len_vnvn(
201         const double v0[], const double v1[], const uint dims)
202 {
203         return sqrt(len_squared_vnvn(v0, v1, dims));
204 }
205
206 #if 0
207 static double len_vn(
208                 const double v0[], const uint dims)
209 {
210         return sqrt(len_squared_vn(v0, dims));
211 }
212
213 MINLINE double normalize_vn(
214         double v0[], const uint dims)
215 {
216         double d = len_squared_vn(v0, dims);
217         if (d != 0.0 && ((d = sqrt(d)) != 0.0)) {
218                 imul_vn_fl(v0, 1.0 / d, dims);
219         }
220         return d;
221 }
222 #endif
223
224 /* v_out = (v0 - v1).normalized() */
225 MINLINE double normalize_vn_vnvn(
226         double v_out[],
227         const double v0[], const double v1[], const uint dims)
228 {
229         double d = 0.0;
230         for (uint j = 0; j < dims; j++) {
231                 double a = v0[j] - v1[j];
232                 d += sq(a);
233                 v_out[j] = a;
234         }
235         if (d != 0.0 && ((d = sqrt(d)) != 0.0)) {
236                 imul_vn_fl(v_out, 1.0 / d, dims);
237         }
238         return d;
239 }
240
241 MINLINE bool is_almost_zero_ex(double val, double eps)
242 {
243         return (-eps < val) && (val < eps);
244 }
245
246 MINLINE bool is_almost_zero(double val)
247 {
248         return is_almost_zero_ex(val, 1e-8);
249 }
250
251 MINLINE bool equals_vnvn(
252                 const double v0[], const double v1[], const uint dims)
253 {
254         for (uint j = 0; j < dims; j++) {
255                 if (v0[j] != v1[j]) {
256                         return false;
257                 }
258         }
259         return true;
260 }
261
262 /** \} */