merge with/from trunk at r35190
[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_H
31 #define BLI_MATH_VECTOR_INLINE_H
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], const 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, const float *a)
101 {
102         r[0] += a[0];
103         r[1] += a[1];
104 }
105
106 MINLINE void add_v2_v2v2(float *r, const float *a, const 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, const 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, const float *a, const 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, const float *a)
127 {
128         r[0] -= a[0];
129         r[1] -= a[1];
130 }
131
132 MINLINE void sub_v2_v2v2(float *r, const float *a, const 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, const 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 sub_v4_v4(float r[4], const float a[4])
153 {
154         r[0] -= a[0];
155         r[1] -= a[1];
156         r[2] -= a[2];
157         r[3] -= a[3];
158 }
159
160 MINLINE void sub_v4_v4v4(float r[3], const float a[3], const float b[3])
161 {
162         r[0]= a[0] - b[0];
163         r[1]= a[1] - b[1];
164         r[2]= a[2] - b[2];
165         r[3]= a[3] - b[3];
166 }
167
168
169 MINLINE void mul_v2_fl(float *v1, float f)
170 {
171         v1[0]*= f;
172         v1[1]*= f;
173 }
174
175 MINLINE void mul_v2_v2fl(float r[2], const float a[2], float f)
176 {
177         r[0]= a[0]*f;
178         r[1]= a[1]*f;
179 }
180
181 MINLINE void mul_v3_fl(float r[3], float f)
182 {
183         r[0] *= f;
184         r[1] *= f;
185         r[2] *= f;
186 }
187
188 MINLINE void mul_v3_v3fl(float r[3], const float a[3], float f)
189 {
190         r[0]= a[0]*f;
191         r[1]= a[1]*f;
192         r[2]= a[2]*f;
193 }
194
195 MINLINE void mul_v2_v2(float r[2], const float a[2])
196 {
197         r[0] *= a[0];
198         r[1] *= a[1];
199 }
200
201 MINLINE void mul_v3_v3(float r[3], const float a[3])
202 {
203         r[0] *= a[0];
204         r[1] *= a[1];
205         r[2] *= a[2];
206 }
207
208 MINLINE void mul_v4_fl(float r[4], float f)
209 {
210         r[0]*= f;
211         r[1]*= f;
212         r[2]*= f;
213         r[3]*= f;
214 }
215
216 MINLINE void madd_v2_v2fl(float r[2], const float a[2], float f)
217 {
218         r[0] += a[0]*f;
219         r[1] += a[1]*f;
220 }
221
222 MINLINE void madd_v3_v3fl(float r[3], const float a[3], float f)
223 {
224         r[0] += a[0]*f;
225         r[1] += a[1]*f;
226         r[2] += a[2]*f;
227 }
228
229 MINLINE void madd_v3_v3v3(float r[3], const float a[3], const float b[3])
230 {
231         r[0] += a[0]*b[0];
232         r[1] += a[1]*b[1];
233         r[2] += a[2]*b[2];
234 }
235
236 MINLINE void madd_v2_v2v2fl(float r[2], const float a[2], const float b[2], float f)
237 {
238         r[0] = a[0] + b[0]*f;
239         r[1] = a[1] + b[1]*f;
240 }
241
242 MINLINE void madd_v3_v3v3fl(float r[3], const float a[3], const float b[3], float f)
243 {
244         r[0] = a[0] + b[0]*f;
245         r[1] = a[1] + b[1]*f;
246         r[2] = a[2] + b[2]*f;
247 }
248
249 MINLINE void madd_v3_v3v3v3(float r[3], const float a[3], const float b[3], const float c[3])
250 {
251         r[0] = a[0] + b[0]*c[0];
252         r[1] = a[1] + b[1]*c[1];
253         r[2] = a[2] + b[2]*c[2];
254 }
255
256 MINLINE void madd_v4_v4fl(float r[4], const float a[4], float f)
257 {
258         r[0] += a[0]*f;
259         r[1] += a[1]*f;
260         r[2] += a[2]*f;
261         r[3] += a[3]*f;
262 }
263
264 MINLINE void mul_v3_v3v3(float *v, const float *v1, const float *v2)
265 {
266         v[0] = v1[0] * v2[0];
267         v[1] = v1[1] * v2[1];
268         v[2] = v1[2] * v2[2];
269 }
270
271 MINLINE void negate_v3(float r[3])
272 {
273         r[0]= -r[0];
274         r[1]= -r[1];
275         r[2]= -r[2];
276 }
277
278 MINLINE void negate_v3_v3(float r[3], const float a[3])
279 {
280         r[0]= -a[0];
281         r[1]= -a[1];
282         r[2]= -a[2];
283 }
284
285 MINLINE void negate_v4(float r[4])
286 {
287         r[0]= -r[0];
288         r[1]= -r[1];
289         r[2]= -r[2];
290         r[3]= -r[3];
291 }
292
293 MINLINE void negate_v4_v4(float r[4], const float a[4])
294 {
295         r[0]= -a[0];
296         r[1]= -a[1];
297         r[2]= -a[2];
298         r[3]= -a[3];
299 }
300
301 MINLINE float dot_v2v2(const float a[2], const float b[2])
302 {
303         return a[0]*b[0] + a[1]*b[1];
304 }
305
306 MINLINE float dot_v3v3(const float a[3], const float b[3])
307 {
308         return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
309 }
310
311 MINLINE float cross_v2v2(const float a[2], const float b[2])
312 {
313          return a[0]*b[1] - a[1]*b[0];
314 }
315
316 MINLINE void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
317 {
318         r[0]= a[1]*b[2] - a[2]*b[1];
319         r[1]= a[2]*b[0] - a[0]*b[2];
320         r[2]= a[0]*b[1] - a[1]*b[0];
321 }
322
323 MINLINE void star_m3_v3(float mat[][3], float *vec)
324 {
325         mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
326         mat[0][1]= -vec[2];     
327         mat[0][2]= vec[1];
328         mat[1][0]= vec[2];      
329         mat[1][2]= -vec[0];
330         mat[2][0]= -vec[1];     
331         mat[2][1]= vec[0];
332 }
333
334 /*********************************** Length **********************************/
335
336 MINLINE float len_v2(const float v[2])
337 {
338         return (float)sqrtf(v[0]*v[0] + v[1]*v[1]);
339 }
340
341 MINLINE float len_v2v2(const float v1[2], const float v2[2])
342 {
343         float x, y;
344
345         x = v1[0]-v2[0];
346         y = v1[1]-v2[1];
347         return (float)sqrtf(x*x+y*y);
348 }
349
350 MINLINE float len_v3(const float a[3])
351 {
352         return sqrtf(dot_v3v3(a, a));
353 }
354
355 MINLINE float len_squared_v2v2(const float a[3], const float b[3])
356 {
357         float d[2];
358
359         sub_v2_v2v2(d, b, a);
360         return dot_v2v2(d, d);
361 }
362
363 MINLINE float len_v3v3(const float a[3], const float b[3])
364 {
365         float d[3];
366
367         sub_v3_v3v3(d, b, a);
368         return len_v3(d);
369 }
370
371 MINLINE float len_squared_v3v3(const float a[3], const float b[3])
372 {
373         float d[3];
374
375         sub_v3_v3v3(d, b, a);
376         return dot_v3v3(d, d);
377 }
378
379 MINLINE float normalize_v2_v2(float r[2], const float a[2])
380 {
381         float d= dot_v2v2(a, a);
382
383         if(d > 1.0e-35f) {
384                 d= sqrtf(d);
385                 mul_v2_v2fl(r, a, 1.0f/d);
386         } else {
387                 zero_v2(r);
388                 d= 0.0f;
389         }
390
391         return d;
392 }
393
394 MINLINE float normalize_v2(float n[2])
395 {
396         return normalize_v2_v2(n, n);
397 }
398
399 MINLINE float normalize_v3_v3(float r[3], const float a[3])
400 {
401         float d= dot_v3v3(a, a);
402
403         /* a larger value causes normalize errors in a
404            scaled down models with camera xtreme close */
405         if(d > 1.0e-35f) {
406                 d= sqrtf(d);
407                 mul_v3_v3fl(r, a, 1.0f/d);
408         }
409         else {
410                 zero_v3(r);
411                 d= 0.0f;
412         }
413
414         return d;
415 }
416
417 MINLINE double normalize_dv3(double n[3])
418 {
419         double d= n[0]*n[0] + n[1]*n[1] + n[2]*n[2];
420
421         /* a larger value causes normalize errors in a
422            scaled down models with camera xtreme close */
423         if(d > 1.0e-35f) {
424                 double mul;
425
426                 d= sqrt(d);
427                 mul = 1.0 / d;
428
429                 n[0] *= mul;
430                 n[1] *= mul;
431                 n[2] *= mul;
432         } else {
433                 n[0] = n[1] = n[2] = 0;
434                 d= 0.0;
435         }
436
437         return d;
438 }
439
440 MINLINE float normalize_v3(float n[3])
441 {
442         return normalize_v3_v3(n, n);
443 }
444
445 MINLINE void normal_short_to_float_v3(float *out, const short *in)
446 {
447         out[0] = in[0]*(1.0f/32767.0f);
448         out[1] = in[1]*(1.0f/32767.0f);
449         out[2] = in[2]*(1.0f/32767.0f);
450 }
451
452 MINLINE void normal_float_to_short_v3(short *out, const float *in)
453 {
454         out[0] = (short)(in[0]*32767.0f);
455         out[1] = (short)(in[1]*32767.0f);
456         out[2] = (short)(in[2]*32767.0f);
457 }
458
459 /********************************* Comparison ********************************/
460
461 MINLINE int is_zero_v3(const float v[3])
462 {
463         return (v[0] == 0 && v[1] == 0 && v[2] == 0);
464 }
465
466 MINLINE int is_zero_v4(const float v[4])
467 {
468         return (v[0] == 0 && v[1] == 0 && v[2] == 0 && v[3] == 0);
469 }
470
471 MINLINE int is_one_v3(const float *v)
472 {
473         return (v[0] == 1 && v[1] == 1 && v[2] == 1);
474 }
475
476 MINLINE int equals_v2v2(const float *v1, const float *v2)
477 {
478         return ((v1[0]==v2[0]) && (v1[1]==v2[1]));
479 }
480
481 MINLINE int equals_v3v3(const float *v1, const float *v2)
482 {
483         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
484 }
485
486 MINLINE int equals_v4v4(const float *v1, const float *v2)
487 {
488         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]) && (v1[3]==v2[3]));
489 }
490
491 MINLINE int compare_v3v3(const float *v1, const float *v2, const float limit)
492 {
493         if(fabs(v1[0]-v2[0])<limit)
494                 if(fabs(v1[1]-v2[1])<limit)
495                         if(fabs(v1[2]-v2[2])<limit)
496                                 return 1;
497
498         return 0;
499 }
500
501 MINLINE int compare_len_v3v3(const float *v1, const float *v2, const float limit)
502 {
503         float x,y,z;
504
505         x=v1[0]-v2[0];
506         y=v1[1]-v2[1];
507         z=v1[2]-v2[2];
508
509         return ((x*x + y*y + z*z) < (limit*limit));
510 }
511
512 MINLINE int compare_v4v4(const float *v1, const float *v2, const float limit)
513 {
514         if(fabs(v1[0]-v2[0])<limit)
515                 if(fabs(v1[1]-v2[1])<limit)
516                         if(fabs(v1[2]-v2[2])<limit)
517                                 if(fabs(v1[3]-v2[3])<limit)
518                                         return 1;
519
520         return 0;
521 }
522
523 MINLINE float line_point_side_v2(const float *l1, const float *l2, const float *pt)
524 {
525         return  ((l1[0]-pt[0]) * (l2[1]-pt[1])) -
526                         ((l2[0]-pt[0]) * (l1[1]-pt[1]));
527 }
528
529 #endif /* BLI_MATH_VECTOR_INLINE_H */
530