svn merge ^/trunk/blender -r41230:41266
[blender.git] / source / blender / blenlib / intern / math_vector_inline.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  
21  * The Original Code is: some of this file.
22  *
23  * ***** END GPL LICENSE BLOCK *****
24  * */
25
26 /** \file blender/blenlib/intern/math_vector_inline.c
27  *  \ingroup bli
28  */
29
30
31 #include "BLI_math.h"
32
33 #ifndef BLI_MATH_VECTOR_INLINE_H
34 #define BLI_MATH_VECTOR_INLINE_H
35
36 /********************************** Init *************************************/
37
38 MINLINE void zero_v2(float r[2])
39 {
40         r[0]= 0.0f;
41         r[1]= 0.0f;
42 }
43
44 MINLINE void zero_v3(float r[3])
45 {
46         r[0]= 0.0f;
47         r[1]= 0.0f;
48         r[2]= 0.0f;
49 }
50
51 MINLINE void zero_v4(float r[4])
52 {
53         r[0]= 0.0f;
54         r[1]= 0.0f;
55         r[2]= 0.0f;
56         r[3]= 0.0f;
57 }
58
59 MINLINE void copy_v2_v2(float r[2], const float a[2])
60 {
61         r[0]= a[0];
62         r[1]= a[1];
63 }
64
65 MINLINE void copy_v3_v3(float r[3], const float a[3])
66 {
67         r[0]= a[0];
68         r[1]= a[1];
69         r[2]= a[2];
70 }
71
72 MINLINE void copy_v4_v4(float r[4], const float a[4])
73 {
74         r[0]= a[0];
75         r[1]= a[1];
76         r[2]= a[2];
77         r[3]= a[3];
78 }
79
80 /* short */
81 MINLINE void copy_v2_v2_short(short r[2], const short a[2])
82 {
83         r[0]= a[0];
84         r[1]= a[1];
85 }
86
87 MINLINE void copy_v3_v3_short(short r[3], const short a[3])
88 {
89         r[0]= a[0];
90         r[1]= a[1];
91         r[2]= a[2];
92 }
93
94 MINLINE void copy_v4_v4_short(short r[4], const short a[4])
95 {
96         r[0]= a[0];
97         r[1]= a[1];
98         r[2]= a[2];
99         r[3]= a[3];
100 }
101
102 MINLINE void swap_v2_v2(float a[2], float b[2])
103 {
104         SWAP(float, a[0], b[0]);
105         SWAP(float, a[1], b[1]);
106 }
107
108 MINLINE void swap_v3_v3(float a[3], float b[3])
109 {
110         SWAP(float, a[0], b[0]);
111         SWAP(float, a[1], b[1]);
112         SWAP(float, a[2], b[2]);
113 }
114
115 MINLINE void swap_v4_v4(float a[4], float b[4])
116 {
117         SWAP(float, a[0], b[0]);
118         SWAP(float, a[1], b[1]);
119         SWAP(float, a[2], b[2]);
120         SWAP(float, a[3], b[3]);
121 }
122
123 /********************************* Arithmetic ********************************/
124
125 MINLINE void add_v3_fl(float r[3], float f)
126 {
127         r[0] += f;
128         r[1] += f;
129         r[2] += f;
130 }
131
132 MINLINE void add_v4_fl(float r[4], float f)
133 {
134         r[0] += f;
135         r[1] += f;
136         r[2] += f;
137         r[3] += f;
138 }
139
140 MINLINE void add_v2_v2(float *r, const float *a)
141 {
142         r[0] += a[0];
143         r[1] += a[1];
144 }
145
146 MINLINE void add_v2_v2v2(float *r, const float *a, const float *b)
147 {
148         r[0]= a[0] + b[0];
149         r[1]= a[1] + b[1];
150 }
151
152 MINLINE void add_v3_v3(float *r, const float *a)
153 {
154         r[0] += a[0];
155         r[1] += a[1];
156         r[2] += a[2];
157 }
158
159 MINLINE void add_v3_v3v3(float r[3], const float a[3], const float b[3])
160 {
161         r[0]= a[0] + b[0];
162         r[1]= a[1] + b[1];
163         r[2]= a[2] + b[2];
164 }
165
166 MINLINE void sub_v2_v2(float r[2], const float a[2])
167 {
168         r[0] -= a[0];
169         r[1] -= a[1];
170 }
171
172 MINLINE void sub_v2_v2v2(float r[2], const float a[2], const float b[2])
173 {
174         r[0]= a[0] - b[0];
175         r[1]= a[1] - b[1];
176 }
177
178 MINLINE void sub_v3_v3(float r[3], const float a[3])
179 {
180         r[0] -= a[0];
181         r[1] -= a[1];
182         r[2] -= a[2];
183 }
184
185 MINLINE void sub_v3_v3v3(float r[3], const float a[3], const float b[3])
186 {
187         r[0]= a[0] - b[0];
188         r[1]= a[1] - b[1];
189         r[2]= a[2] - b[2];
190 }
191
192 MINLINE void sub_v4_v4(float r[4], const float a[4])
193 {
194         r[0] -= a[0];
195         r[1] -= a[1];
196         r[2] -= a[2];
197         r[3] -= a[3];
198 }
199
200 MINLINE void sub_v4_v4v4(float r[4], const float a[4], const float b[4])
201 {
202         r[0]= a[0] - b[0];
203         r[1]= a[1] - b[1];
204         r[2]= a[2] - b[2];
205         r[3]= a[3] - b[3];
206 }
207
208
209 MINLINE void mul_v2_fl(float r[2], float f)
210 {
211         r[0]*= f;
212         r[1]*= f;
213 }
214
215 MINLINE void mul_v2_v2fl(float r[2], const float a[2], float f)
216 {
217         r[0]= a[0]*f;
218         r[1]= a[1]*f;
219 }
220
221 MINLINE void mul_v3_fl(float r[3], float f)
222 {
223         r[0] *= f;
224         r[1] *= f;
225         r[2] *= f;
226 }
227
228 MINLINE void mul_v3_v3fl(float r[3], const float a[3], float f)
229 {
230         r[0]= a[0]*f;
231         r[1]= a[1]*f;
232         r[2]= a[2]*f;
233 }
234
235 MINLINE void mul_v2_v2(float r[2], const float a[2])
236 {
237         r[0] *= a[0];
238         r[1] *= a[1];
239 }
240
241 MINLINE void mul_v3_v3(float r[3], const float a[3])
242 {
243         r[0] *= a[0];
244         r[1] *= a[1];
245         r[2] *= a[2];
246 }
247
248 MINLINE void mul_v4_fl(float r[4], float f)
249 {
250         r[0]*= f;
251         r[1]*= f;
252         r[2]*= f;
253         r[3]*= f;
254 }
255
256 MINLINE void madd_v2_v2fl(float r[2], const float a[2], float f)
257 {
258         r[0] += a[0]*f;
259         r[1] += a[1]*f;
260 }
261
262 MINLINE void madd_v3_v3fl(float r[3], const float a[3], float f)
263 {
264         r[0] += a[0]*f;
265         r[1] += a[1]*f;
266         r[2] += a[2]*f;
267 }
268
269 MINLINE void madd_v3_v3v3(float r[3], const float a[3], const float b[3])
270 {
271         r[0] += a[0]*b[0];
272         r[1] += a[1]*b[1];
273         r[2] += a[2]*b[2];
274 }
275
276 MINLINE void madd_v2_v2v2fl(float r[2], const float a[2], const float b[2], float f)
277 {
278         r[0] = a[0] + b[0]*f;
279         r[1] = a[1] + b[1]*f;
280 }
281
282 MINLINE void madd_v3_v3v3fl(float r[3], const float a[3], const float b[3], float f)
283 {
284         r[0] = a[0] + b[0]*f;
285         r[1] = a[1] + b[1]*f;
286         r[2] = a[2] + b[2]*f;
287 }
288
289 MINLINE void madd_v3_v3v3v3(float r[3], const float a[3], const float b[3], const float c[3])
290 {
291         r[0] = a[0] + b[0]*c[0];
292         r[1] = a[1] + b[1]*c[1];
293         r[2] = a[2] + b[2]*c[2];
294 }
295
296 MINLINE void madd_v4_v4fl(float r[4], const float a[4], float f)
297 {
298         r[0] += a[0]*f;
299         r[1] += a[1]*f;
300         r[2] += a[2]*f;
301         r[3] += a[3]*f;
302 }
303
304 MINLINE void mul_v3_v3v3(float r[3], const float v1[3], const float v2[3])
305 {
306         r[0] = v1[0] * v2[0];
307         r[1] = v1[1] * v2[1];
308         r[2] = v1[2] * v2[2];
309 }
310
311 MINLINE void negate_v2(float r[3])
312 {
313         r[0]= -r[0];
314         r[1]= -r[1];
315 }
316
317 MINLINE void negate_v2_v2(float r[2], const float a[2])
318 {
319         r[0]= -a[0];
320         r[1]= -a[1];
321 }
322
323 MINLINE void negate_v3(float r[3])
324 {
325         r[0]= -r[0];
326         r[1]= -r[1];
327         r[2]= -r[2];
328 }
329
330 MINLINE void negate_v3_v3(float r[3], const float a[3])
331 {
332         r[0]= -a[0];
333         r[1]= -a[1];
334         r[2]= -a[2];
335 }
336
337 MINLINE void negate_v4(float r[4])
338 {
339         r[0]= -r[0];
340         r[1]= -r[1];
341         r[2]= -r[2];
342         r[3]= -r[3];
343 }
344
345 MINLINE void negate_v4_v4(float r[4], const float a[4])
346 {
347         r[0]= -a[0];
348         r[1]= -a[1];
349         r[2]= -a[2];
350         r[3]= -a[3];
351 }
352
353 MINLINE float dot_v2v2(const float a[2], const float b[2])
354 {
355         return a[0]*b[0] + a[1]*b[1];
356 }
357
358 MINLINE float dot_v3v3(const float a[3], const float b[3])
359 {
360         return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
361 }
362
363 MINLINE float cross_v2v2(const float a[2], const float b[2])
364 {
365         return a[0]*b[1] - a[1]*b[0];
366 }
367
368 MINLINE void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
369 {
370         r[0]= a[1]*b[2] - a[2]*b[1];
371         r[1]= a[2]*b[0] - a[0]*b[2];
372         r[2]= a[0]*b[1] - a[1]*b[0];
373 }
374
375 MINLINE void star_m3_v3(float rmat[][3], float a[3])
376 {
377         rmat[0][0]= rmat[1][1]= rmat[2][2]= 0.0;
378         rmat[0][1]= -a[2];
379         rmat[0][2]=  a[1];
380         rmat[1][0]=  a[2];
381         rmat[1][2]= -a[0];
382         rmat[2][0]= -a[1];
383         rmat[2][1]=  a[0];
384 }
385
386 /*********************************** Length **********************************/
387
388 MINLINE float len_v2(const float v[2])
389 {
390         return (float)sqrtf(v[0]*v[0] + v[1]*v[1]);
391 }
392
393 MINLINE float len_v2v2(const float v1[2], const float v2[2])
394 {
395         float x, y;
396
397         x = v1[0]-v2[0];
398         y = v1[1]-v2[1];
399         return (float)sqrtf(x*x+y*y);
400 }
401
402 MINLINE float len_v3(const float a[3])
403 {
404         return sqrtf(dot_v3v3(a, a));
405 }
406
407 MINLINE float len_squared_v2v2(const float a[3], const float b[3])
408 {
409         float d[2];
410
411         sub_v2_v2v2(d, b, a);
412         return dot_v2v2(d, d);
413 }
414
415 MINLINE float len_v3v3(const float a[3], const float b[3])
416 {
417         float d[3];
418
419         sub_v3_v3v3(d, b, a);
420         return len_v3(d);
421 }
422
423 MINLINE float len_squared_v3v3(const float a[3], const float b[3])
424 {
425         float d[3];
426
427         sub_v3_v3v3(d, b, a);
428         return dot_v3v3(d, d);
429 }
430
431 MINLINE float normalize_v2_v2(float r[2], const float a[2])
432 {
433         float d= dot_v2v2(a, a);
434
435         if(d > 1.0e-35f) {
436                 d= sqrtf(d);
437                 mul_v2_v2fl(r, a, 1.0f/d);
438         } else {
439                 zero_v2(r);
440                 d= 0.0f;
441         }
442
443         return d;
444 }
445
446 MINLINE float normalize_v2(float n[2])
447 {
448         return normalize_v2_v2(n, n);
449 }
450
451 MINLINE float normalize_v3_v3(float r[3], const float a[3])
452 {
453         float d= dot_v3v3(a, a);
454
455         /* a larger value causes normalize errors in a
456            scaled down models with camera xtreme close */
457         if(d > 1.0e-35f) {
458                 d= sqrtf(d);
459                 mul_v3_v3fl(r, a, 1.0f/d);
460         }
461         else {
462                 zero_v3(r);
463                 d= 0.0f;
464         }
465
466         return d;
467 }
468
469 MINLINE double normalize_dv3(double n[3])
470 {
471         double d= n[0]*n[0] + n[1]*n[1] + n[2]*n[2];
472
473         /* a larger value causes normalize errors in a
474            scaled down models with camera xtreme close */
475         if(d > 1.0e-35) {
476                 double mul;
477
478                 d= sqrt(d);
479                 mul = 1.0 / d;
480
481                 n[0] *= mul;
482                 n[1] *= mul;
483                 n[2] *= mul;
484         } else {
485                 n[0] = n[1] = n[2] = 0;
486                 d= 0.0;
487         }
488
489         return d;
490 }
491
492 MINLINE float normalize_v3(float n[3])
493 {
494         return normalize_v3_v3(n, n);
495 }
496
497 MINLINE void normal_short_to_float_v3(float out[3], const short in[3])
498 {
499         out[0] = in[0]*(1.0f/32767.0f);
500         out[1] = in[1]*(1.0f/32767.0f);
501         out[2] = in[2]*(1.0f/32767.0f);
502 }
503
504 MINLINE void normal_float_to_short_v3(short out[3], const float in[3])
505 {
506         out[0] = (short)(in[0]*32767.0f);
507         out[1] = (short)(in[1]*32767.0f);
508         out[2] = (short)(in[2]*32767.0f);
509 }
510
511 /********************************* Comparison ********************************/
512
513 MINLINE int is_zero_v3(const float v[3])
514 {
515         return (v[0] == 0 && v[1] == 0 && v[2] == 0);
516 }
517
518 MINLINE int is_zero_v4(const float v[4])
519 {
520         return (v[0] == 0 && v[1] == 0 && v[2] == 0 && v[3] == 0);
521 }
522
523 MINLINE int is_one_v3(const float v[3])
524 {
525         return (v[0] == 1 && v[1] == 1 && v[2] == 1);
526 }
527
528 MINLINE int equals_v2v2(const float v1[2], const float v2[2])
529 {
530         return ((v1[0]==v2[0]) && (v1[1]==v2[1]));
531 }
532
533 MINLINE int equals_v3v3(const float v1[3], const float v2[3])
534 {
535         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
536 }
537
538 MINLINE int equals_v4v4(const float v1[4], const float v2[4])
539 {
540         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]) && (v1[3]==v2[3]));
541 }
542
543 MINLINE int compare_v3v3(const float v1[3], const float v2[3], const float limit)
544 {
545         if(fabsf(v1[0]-v2[0])<limit)
546                 if(fabsf(v1[1]-v2[1])<limit)
547                         if(fabsf(v1[2]-v2[2])<limit)
548                                 return 1;
549
550         return 0;
551 }
552
553 MINLINE int compare_len_v3v3(const float v1[3], const float v2[3], const float limit)
554 {
555         float x,y,z;
556
557         x=v1[0]-v2[0];
558         y=v1[1]-v2[1];
559         z=v1[2]-v2[2];
560
561         return ((x*x + y*y + z*z) < (limit*limit));
562 }
563
564 MINLINE int compare_v4v4(const float v1[4], const float v2[4], const float limit)
565 {
566         if(fabsf(v1[0]-v2[0])<limit)
567                 if(fabsf(v1[1]-v2[1])<limit)
568                         if(fabsf(v1[2]-v2[2])<limit)
569                                 if(fabsf(v1[3]-v2[3])<limit)
570                                         return 1;
571
572         return 0;
573 }
574
575 MINLINE float line_point_side_v2(const float l1[2], const float l2[2], const float pt[2])
576 {
577         return  ((l1[0]-pt[0]) * (l2[1]-pt[1])) -
578                         ((l2[0]-pt[0]) * (l1[1]-pt[1]));
579 }
580
581 #endif /* BLI_MATH_VECTOR_INLINE_H */
582