correct fsf address
[blender.git] / source / blender / blenlib / intern / math_vector_inline.c
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19  *
20  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
21  * All rights reserved.
22  
23  * The Original Code is: some of this file.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  * */
27
28 #include "BLI_math.h"
29
30 #ifndef BLI_MATH_VECTOR_INLINE
31 #define BLI_MATH_VECTOR_INLINE
32
33 /********************************** Init *************************************/
34
35 MINLINE void zero_v2(float r[2])
36 {
37         r[0]= 0.0f;
38         r[1]= 0.0f;
39 }
40
41 MINLINE void zero_v3(float r[3])
42 {
43         r[0]= 0.0f;
44         r[1]= 0.0f;
45         r[2]= 0.0f;
46 }
47
48 MINLINE void zero_v4(float r[4])
49 {
50         r[0]= 0.0f;
51         r[1]= 0.0f;
52         r[2]= 0.0f;
53         r[3]= 0.0f;
54 }
55
56 MINLINE void copy_v2_v2(float r[2], const float a[2])
57 {
58         r[0]= a[0];
59         r[1]= a[1];
60 }
61
62 MINLINE void copy_v3_v3(float r[3], const float a[3])
63 {
64         r[0]= a[0];
65         r[1]= a[1];
66         r[2]= a[2];
67 }
68
69 MINLINE void copy_v4_v4(float r[4], float a[4])
70 {
71         r[0]= a[0];
72         r[1]= a[1];
73         r[2]= a[2];
74         r[3]= a[3];
75 }
76
77 MINLINE void swap_v2_v2(float a[2], float b[2])
78 {
79         SWAP(float, a[0], b[0]);
80         SWAP(float, a[1], b[1]);
81 }
82
83 MINLINE void swap_v3_v3(float a[3], float b[3])
84 {
85         SWAP(float, a[0], b[0]);
86         SWAP(float, a[1], b[1]);
87         SWAP(float, a[2], b[2]);
88 }
89
90 MINLINE void swap_v4_v4(float a[4], float b[4])
91 {
92         SWAP(float, a[0], b[0]);
93         SWAP(float, a[1], b[1]);
94         SWAP(float, a[2], b[2]);
95         SWAP(float, a[3], b[3]);
96 }
97
98 /********************************* Arithmetic ********************************/
99
100 MINLINE void add_v2_v2(float *r, float *a)
101 {
102         r[0] += a[0];
103         r[1] += a[1];
104 }
105
106 MINLINE void add_v2_v2v2(float *r, float *a, float *b)
107 {
108         r[0]= a[0] + b[0];
109         r[1]= a[1] + b[1];
110 }
111
112 MINLINE void add_v3_v3(float *r, float *a)
113 {
114         r[0] += a[0];
115         r[1] += a[1];
116         r[2] += a[2];
117 }
118
119 MINLINE void add_v3_v3v3(float *r, float *a, float *b)
120 {
121         r[0]= a[0] + b[0];
122         r[1]= a[1] + b[1];
123         r[2]= a[2] + b[2];
124 }
125
126 MINLINE void sub_v2_v2(float *r, float *a)
127 {
128         r[0] -= a[0];
129         r[1] -= a[1];
130 }
131
132 MINLINE void sub_v2_v2v2(float *r, float *a, float *b)
133 {
134         r[0]= a[0] - b[0];
135         r[1]= a[1] - b[1];
136 }
137
138 MINLINE void sub_v3_v3(float *r, float *a)
139 {
140         r[0] -= a[0];
141         r[1] -= a[1];
142         r[2] -= a[2];
143 }
144
145 MINLINE void sub_v3_v3v3(float r[3], const float a[3], const float b[3])
146 {
147         r[0]= a[0] - b[0];
148         r[1]= a[1] - b[1];
149         r[2]= a[2] - b[2];
150 }
151
152 MINLINE void mul_v2_fl(float *v1, float f)
153 {
154         v1[0]*= f;
155         v1[1]*= f;
156 }
157
158 MINLINE void mul_v2_v2fl(float r[2], const float a[2], float f)
159 {
160         r[0]= a[0]*f;
161         r[1]= a[1]*f;
162 }
163
164 MINLINE void mul_v3_fl(float r[3], float f)
165 {
166         r[0] *= f;
167         r[1] *= f;
168         r[2] *= f;
169 }
170
171 MINLINE void mul_v3_v3fl(float r[3], const float a[3], float f)
172 {
173         r[0]= a[0]*f;
174         r[1]= a[1]*f;
175         r[2]= a[2]*f;
176 }
177
178 MINLINE void mul_v2_v2(float r[2], const float a[2])
179 {
180         r[0] *= a[0];
181         r[1] *= a[1];
182 }
183
184 MINLINE void mul_v3_v3(float r[3], float a[3])
185 {
186         r[0] *= a[0];
187         r[1] *= a[1];
188         r[2] *= a[2];
189 }
190
191 MINLINE void madd_v3_v3fl(float r[3], float a[3], float f)
192 {
193         r[0] += a[0]*f;
194         r[1] += a[1]*f;
195         r[2] += a[2]*f;
196 }
197
198 MINLINE void madd_v3_v3v3(float r[3], float a[3], float b[3])
199 {
200         r[0] += a[0]*b[0];
201         r[1] += a[1]*b[1];
202         r[2] += a[2]*b[2];
203 }
204
205 MINLINE void madd_v2_v2v2fl(float r[2], const float a[2], const float b[2], const float f)
206 {
207         r[0] = a[0] + b[0]*f;
208         r[1] = a[1] + b[1]*f;
209 }
210
211 MINLINE void madd_v3_v3v3fl(float r[3], float a[3], float b[3], float f)
212 {
213         r[0] = a[0] + b[0]*f;
214         r[1] = a[1] + b[1]*f;
215         r[2] = a[2] + b[2]*f;
216 }
217
218 MINLINE void madd_v3_v3v3v3(float r[3], float a[3], float b[3], float c[3])
219 {
220         r[0] = a[0] + b[0]*c[0];
221         r[1] = a[1] + b[1]*c[1];
222         r[2] = a[2] + b[2]*c[2];
223 }
224
225 MINLINE void mul_v3_v3v3(float *v, float *v1, float *v2)
226 {
227         v[0] = v1[0] * v2[0];
228         v[1] = v1[1] * v2[1];
229         v[2] = v1[2] * v2[2];
230 }
231
232 MINLINE void negate_v3(float r[3])
233 {
234         r[0]= -r[0];
235         r[1]= -r[1];
236         r[2]= -r[2];
237 }
238
239 MINLINE void negate_v3_v3(float r[3], const float a[3])
240 {
241         r[0]= -a[0];
242         r[1]= -a[1];
243         r[2]= -a[2];
244 }
245
246 MINLINE float dot_v2v2(const float a[2], const float b[2])
247 {
248         return a[0]*b[0] + a[1]*b[1];
249 }
250
251 MINLINE float dot_v3v3(const float a[3], const float b[3])
252 {
253         return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
254 }
255
256 MINLINE float cross_v2v2(const float a[2], const float b[2])
257 {
258          return a[0]*b[1] - a[1]*b[0];
259 }
260
261 MINLINE void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
262 {
263         r[0]= a[1]*b[2] - a[2]*b[1];
264         r[1]= a[2]*b[0] - a[0]*b[2];
265         r[2]= a[0]*b[1] - a[1]*b[0];
266 }
267
268 MINLINE void star_m3_v3(float mat[][3], float *vec)
269 {
270         mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
271         mat[0][1]= -vec[2];     
272         mat[0][2]= vec[1];
273         mat[1][0]= vec[2];      
274         mat[1][2]= -vec[0];
275         mat[2][0]= -vec[1];     
276         mat[2][1]= vec[0];
277 }
278
279 /*********************************** Length **********************************/
280
281 MINLINE float len_v2(const float v[2])
282 {
283         return (float)sqrt(v[0]*v[0] + v[1]*v[1]);
284 }
285
286 MINLINE float len_v2v2(const float v1[2], const float v2[2])
287 {
288         float x, y;
289
290         x = v1[0]-v2[0];
291         y = v1[1]-v2[1];
292         return (float)sqrt(x*x+y*y);
293 }
294
295 MINLINE float len_v3(const float a[3])
296 {
297         return sqrtf(dot_v3v3(a, a));
298 }
299
300 MINLINE float len_v3v3(const float a[3], const float b[3])
301 {
302         float d[3];
303
304         sub_v3_v3v3(d, b, a);
305         return len_v3(d);
306 }
307
308 MINLINE float normalize_v2_v2(float r[2], const float a[2])
309 {
310         float d= dot_v2v2(a, a);
311
312         if(d > 1.0e-35f) {
313                 d= sqrtf(d);
314                 mul_v2_v2fl(r, a, 1.0f/d);
315         } else {
316                 zero_v2(r);
317                 d= 0.0f;
318         }
319
320         return d;
321 }
322
323 MINLINE float normalize_v2(float n[2])
324 {
325         return normalize_v2_v2(n, n);
326 }
327
328 MINLINE float normalize_v3_v3(float r[3], const float a[3])
329 {
330         float d= dot_v3v3(a, a);
331
332         /* a larger value causes normalize errors in a
333            scaled down models with camera xtreme close */
334         if(d > 1.0e-35f) {
335                 d= sqrtf(d);
336                 mul_v3_v3fl(r, a, 1.0f/d);
337         }
338         else {
339                 zero_v3(r);
340                 d= 0.0f;
341         }
342
343         return d;
344 }
345
346 MINLINE float normalize_v3(float n[3])
347 {
348         return normalize_v3_v3(n, n);
349 }
350
351 MINLINE void normal_short_to_float_v3(float *out, short *in)
352 {
353         out[0] = in[0]*(1.0f/32767.0f);
354         out[1] = in[1]*(1.0f/32767.0f);
355         out[2] = in[2]*(1.0f/32767.0f);
356 }
357
358 MINLINE void normal_float_to_short_v3(short *out, float *in)
359 {
360         out[0] = (short)(in[0]*32767.0f);
361         out[1] = (short)(in[1]*32767.0f);
362         out[2] = (short)(in[2]*32767.0f);
363 }
364
365 /********************************* Comparison ********************************/
366
367 MINLINE int is_zero_v3(float *v)
368 {
369         return (v[0] == 0 && v[1] == 0 && v[2] == 0);
370 }
371
372 MINLINE int is_one_v3(float *v)
373 {
374         return (v[0] == 1 && v[1] == 1 && v[2] == 1);
375 }
376
377 MINLINE int equals_v3v3(float *v1, float *v2)
378 {
379         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
380 }
381
382 MINLINE int compare_v3v3(float *v1, float *v2, float limit)
383 {
384         if(fabs(v1[0]-v2[0])<limit)
385                 if(fabs(v1[1]-v2[1])<limit)
386                         if(fabs(v1[2]-v2[2])<limit)
387                                 return 1;
388
389         return 0;
390 }
391
392 MINLINE int compare_len_v3v3(float *v1, float *v2, float limit)
393 {
394     float x,y,z;
395
396         x=v1[0]-v2[0];
397         y=v1[1]-v2[1];
398         z=v1[2]-v2[2];
399
400         return ((x*x + y*y + z*z) < (limit*limit));
401 }
402
403 MINLINE int compare_v4v4(float *v1, float *v2, float limit)
404 {
405         if(fabs(v1[0]-v2[0])<limit)
406                 if(fabs(v1[1]-v2[1])<limit)
407                         if(fabs(v1[2]-v2[2])<limit)
408                                 if(fabs(v1[3]-v2[3])<limit)
409                                         return 1;
410
411         return 0;
412 }
413
414 #endif /* BLI_MATH_VECTOR_INLINE */
415