Merged changes in the trunk up to revision 45619.
[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 __MATH_VECTOR_INLINE_C__
34 #define __MATH_VECTOR_INLINE_C__
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 MINLINE void copy_v2_fl(float r[2], float f)
81 {
82         r[0] = f;
83         r[1] = f;
84 }
85
86 MINLINE void copy_v3_fl(float r[3], float f)
87 {
88         r[0] = f;
89         r[1] = f;
90         r[2] = f;
91 }
92
93 MINLINE void copy_v4_fl(float r[4], float f)
94 {
95         r[0] = f;
96         r[1] = f;
97         r[2] = f;
98         r[3] = f;
99 }
100
101 /* short */
102 MINLINE void copy_v2_v2_char(char r[2], const char a[2])
103 {
104         r[0] = a[0];
105         r[1] = a[1];
106 }
107
108 MINLINE void copy_v3_v3_char(char r[3], const char a[3])
109 {
110         r[0] = a[0];
111         r[1] = a[1];
112         r[2] = a[2];
113 }
114
115 MINLINE void copy_v4_v4_char(char r[4], const char a[4])
116 {
117         r[0] = a[0];
118         r[1] = a[1];
119         r[2] = a[2];
120         r[3] = a[3];
121 }
122
123 /* short */
124 MINLINE void copy_v2_v2_short(short r[2], const short a[2])
125 {
126         r[0] = a[0];
127         r[1] = a[1];
128 }
129
130 MINLINE void copy_v3_v3_short(short r[3], const short a[3])
131 {
132         r[0] = a[0];
133         r[1] = a[1];
134         r[2] = a[2];
135 }
136
137 MINLINE void copy_v4_v4_short(short r[4], const short a[4])
138 {
139         r[0] = a[0];
140         r[1] = a[1];
141         r[2] = a[2];
142         r[3] = a[3];
143 }
144
145 /* int */
146 MINLINE void copy_v2_v2_int(int r[2], const int a[2])
147 {
148         r[0] = a[0];
149         r[1] = a[1];
150 }
151
152 MINLINE void copy_v3_v3_int(int r[3], const int a[3])
153 {
154         r[0] = a[0];
155         r[1] = a[1];
156         r[2] = a[2];
157 }
158
159 MINLINE void copy_v4_v4_int(int r[4], const int a[4])
160 {
161         r[0] = a[0];
162         r[1] = a[1];
163         r[2] = a[2];
164         r[3] = a[3];
165 }
166
167 /* double -> float */
168 MINLINE void copy_v2fl_v2db(float r[2], const double a[2])
169 {
170         r[0] = (float)a[0];
171         r[1] = (float)a[1];
172 }
173
174 MINLINE void copy_v3fl_v3db(float r[3], const double a[3])
175 {
176         r[0] = (float)a[0];
177         r[1] = (float)a[1];
178         r[2] = (float)a[2];
179 }
180
181 MINLINE void copy_v4fl_v4db(float r[4], const double a[4])
182 {
183         r[0] = (float)a[0];
184         r[1] = (float)a[1];
185         r[2] = (float)a[2];
186         r[3] = (float)a[3];
187 }
188
189 /* float -> double */
190 MINLINE void copy_v2db_v2fl(double r[2], const float a[2])
191 {
192         r[0] = (double)a[0];
193         r[1] = (double)a[1];
194 }
195
196 MINLINE void copy_v3db_v3fl(double r[3], const float a[3])
197 {
198         r[0] = (double)a[0];
199         r[1] = (double)a[1];
200         r[2] = (double)a[2];
201 }
202
203 MINLINE void copy_v4db_v4fl(double r[4], const float a[4])
204 {
205         r[0] = (double)a[0];
206         r[1] = (double)a[1];
207         r[2] = (double)a[2];
208         r[3] = (double)a[3];
209 }
210
211 MINLINE void swap_v2_v2(float a[2], float b[2])
212 {
213         SWAP(float, a[0], b[0]);
214         SWAP(float, a[1], b[1]);
215 }
216
217 MINLINE void swap_v3_v3(float a[3], float b[3])
218 {
219         SWAP(float, a[0], b[0]);
220         SWAP(float, a[1], b[1]);
221         SWAP(float, a[2], b[2]);
222 }
223
224 MINLINE void swap_v4_v4(float a[4], float b[4])
225 {
226         SWAP(float, a[0], b[0]);
227         SWAP(float, a[1], b[1]);
228         SWAP(float, a[2], b[2]);
229         SWAP(float, a[3], b[3]);
230 }
231
232 /********************************* Arithmetic ********************************/
233
234 MINLINE void add_v3_fl(float r[3], float f)
235 {
236         r[0] += f;
237         r[1] += f;
238         r[2] += f;
239 }
240
241 MINLINE void add_v4_fl(float r[4], float f)
242 {
243         r[0] += f;
244         r[1] += f;
245         r[2] += f;
246         r[3] += f;
247 }
248
249 MINLINE void add_v2_v2(float r[2], const float a[2])
250 {
251         r[0] += a[0];
252         r[1] += a[1];
253 }
254
255 MINLINE void add_v2_v2v2(float r[2], const float a[2], const float b[2])
256 {
257         r[0] = a[0] + b[0];
258         r[1] = a[1] + b[1];
259 }
260
261 MINLINE void add_v3_v3(float r[3], const float a[3])
262 {
263         r[0] += a[0];
264         r[1] += a[1];
265         r[2] += a[2];
266 }
267
268 MINLINE void add_v3_v3v3(float r[3], const float a[3], const float b[3])
269 {
270         r[0] = a[0] + b[0];
271         r[1] = a[1] + b[1];
272         r[2] = a[2] + b[2];
273 }
274
275 MINLINE void sub_v2_v2(float r[2], const float a[2])
276 {
277         r[0] -= a[0];
278         r[1] -= a[1];
279 }
280
281 MINLINE void sub_v2_v2v2(float r[2], const float a[2], const float b[2])
282 {
283         r[0] = a[0] - b[0];
284         r[1] = a[1] - b[1];
285 }
286
287 MINLINE void sub_v3_v3(float r[3], const float a[3])
288 {
289         r[0] -= a[0];
290         r[1] -= a[1];
291         r[2] -= a[2];
292 }
293
294 MINLINE void sub_v3_v3v3(float r[3], const float a[3], const float b[3])
295 {
296         r[0] = a[0] - b[0];
297         r[1] = a[1] - b[1];
298         r[2] = a[2] - b[2];
299 }
300
301 MINLINE void sub_v4_v4(float r[4], const float a[4])
302 {
303         r[0] -= a[0];
304         r[1] -= a[1];
305         r[2] -= a[2];
306         r[3] -= a[3];
307 }
308
309 MINLINE void sub_v4_v4v4(float r[4], const float a[4], const float b[4])
310 {
311         r[0] = a[0] - b[0];
312         r[1] = a[1] - b[1];
313         r[2] = a[2] - b[2];
314         r[3] = a[3] - b[3];
315 }
316
317 MINLINE void mul_v2_fl(float r[2], float f)
318 {
319         r[0] *= f;
320         r[1] *= f;
321 }
322
323 MINLINE void mul_v2_v2fl(float r[2], const float a[2], float f)
324 {
325         r[0] = a[0] * f;
326         r[1] = a[1] * f;
327 }
328
329 MINLINE void mul_v3_fl(float r[3], float f)
330 {
331         r[0] *= f;
332         r[1] *= f;
333         r[2] *= f;
334 }
335
336 MINLINE void mul_v3_v3fl(float r[3], const float a[3], float f)
337 {
338         r[0] = a[0] * f;
339         r[1] = a[1] * f;
340         r[2] = a[2] * f;
341 }
342
343 MINLINE void mul_v2_v2(float r[2], const float a[2])
344 {
345         r[0] *= a[0];
346         r[1] *= a[1];
347 }
348
349 MINLINE void mul_v3_v3(float r[3], const float a[3])
350 {
351         r[0] *= a[0];
352         r[1] *= a[1];
353         r[2] *= a[2];
354 }
355
356 MINLINE void mul_v4_fl(float r[4], float f)
357 {
358         r[0] *= f;
359         r[1] *= f;
360         r[2] *= f;
361         r[3] *= f;
362 }
363
364 MINLINE void madd_v2_v2fl(float r[2], const float a[2], float f)
365 {
366         r[0] += a[0] * f;
367         r[1] += a[1] * f;
368 }
369
370 MINLINE void madd_v3_v3fl(float r[3], const float a[3], float f)
371 {
372         r[0] += a[0] * f;
373         r[1] += a[1] * f;
374         r[2] += a[2] * f;
375 }
376
377 MINLINE void madd_v3_v3v3(float r[3], const float a[3], const float b[3])
378 {
379         r[0] += a[0] * b[0];
380         r[1] += a[1] * b[1];
381         r[2] += a[2] * b[2];
382 }
383
384 MINLINE void madd_v2_v2v2fl(float r[2], const float a[2], const float b[2], float f)
385 {
386         r[0] = a[0] + b[0] * f;
387         r[1] = a[1] + b[1] * f;
388 }
389
390 MINLINE void madd_v3_v3v3fl(float r[3], const float a[3], const float b[3], float f)
391 {
392         r[0] = a[0] + b[0] * f;
393         r[1] = a[1] + b[1] * f;
394         r[2] = a[2] + b[2] * f;
395 }
396
397 MINLINE void madd_v3_v3v3v3(float r[3], const float a[3], const float b[3], const float c[3])
398 {
399         r[0] = a[0] + b[0] * c[0];
400         r[1] = a[1] + b[1] * c[1];
401         r[2] = a[2] + b[2] * c[2];
402 }
403
404 MINLINE void madd_v4_v4fl(float r[4], const float a[4], float f)
405 {
406         r[0] += a[0] * f;
407         r[1] += a[1] * f;
408         r[2] += a[2] * f;
409         r[3] += a[3] * f;
410 }
411
412 MINLINE void mul_v3_v3v3(float r[3], const float v1[3], const float v2[3])
413 {
414         r[0] = v1[0] * v2[0];
415         r[1] = v1[1] * v2[1];
416         r[2] = v1[2] * v2[2];
417 }
418
419 MINLINE void negate_v2(float r[3])
420 {
421         r[0] = -r[0];
422         r[1] = -r[1];
423 }
424
425 MINLINE void negate_v2_v2(float r[2], const float a[2])
426 {
427         r[0] = -a[0];
428         r[1] = -a[1];
429 }
430
431 MINLINE void negate_v3(float r[3])
432 {
433         r[0] = -r[0];
434         r[1] = -r[1];
435         r[2] = -r[2];
436 }
437
438 MINLINE void negate_v3_v3(float r[3], const float a[3])
439 {
440         r[0] = -a[0];
441         r[1] = -a[1];
442         r[2] = -a[2];
443 }
444
445 MINLINE void negate_v4(float r[4])
446 {
447         r[0] = -r[0];
448         r[1] = -r[1];
449         r[2] = -r[2];
450         r[3] = -r[3];
451 }
452
453 MINLINE void negate_v4_v4(float r[4], const float a[4])
454 {
455         r[0] = -a[0];
456         r[1] = -a[1];
457         r[2] = -a[2];
458         r[3] = -a[3];
459 }
460
461 MINLINE float dot_v2v2(const float a[2], const float b[2])
462 {
463         return a[0] * b[0] + a[1] * b[1];
464 }
465
466 MINLINE float dot_v3v3(const float a[3], const float b[3])
467 {
468         return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
469 }
470
471 MINLINE float cross_v2v2(const float a[2], const float b[2])
472 {
473         return a[0] * b[1] - a[1] * b[0];
474 }
475
476 MINLINE void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
477 {
478         r[0] = a[1] * b[2] - a[2] * b[1];
479         r[1] = a[2] * b[0] - a[0] * b[2];
480         r[2] = a[0] * b[1] - a[1] * b[0];
481 }
482
483 MINLINE void star_m3_v3(float rmat[][3], float a[3])
484 {
485         rmat[0][0] = rmat[1][1] = rmat[2][2] = 0.0;
486         rmat[0][1] = -a[2];
487         rmat[0][2] = a[1];
488         rmat[1][0] = a[2];
489         rmat[1][2] = -a[0];
490         rmat[2][0] = -a[1];
491         rmat[2][1] = a[0];
492 }
493
494 /*********************************** Length **********************************/
495
496 MINLINE float len_squared_v2(const float v[2])
497 {
498         return v[0] * v[0] + v[1] * v[1];
499 }
500
501 MINLINE float len_squared_v3(const float v[3])
502 {
503         return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
504 }
505
506 MINLINE float len_v2(const float v[2])
507 {
508         return (float)sqrtf(v[0] * v[0] + v[1] * v[1]);
509 }
510
511 MINLINE float len_v2v2(const float v1[2], const float v2[2])
512 {
513         float x, y;
514
515         x = v1[0] - v2[0];
516         y = v1[1] - v2[1];
517         return (float)sqrtf(x * x + y * y);
518 }
519
520 MINLINE float len_v3(const float a[3])
521 {
522         return sqrtf(dot_v3v3(a, a));
523 }
524
525 MINLINE float len_squared_v2v2(const float a[2], const float b[2])
526 {
527         float d[2];
528
529         sub_v2_v2v2(d, b, a);
530         return dot_v2v2(d, d);
531 }
532
533 MINLINE float len_v3v3(const float a[3], const float b[3])
534 {
535         float d[3];
536
537         sub_v3_v3v3(d, b, a);
538         return len_v3(d);
539 }
540
541 MINLINE float len_squared_v3v3(const float a[3], const float b[3])
542 {
543         float d[3];
544
545         sub_v3_v3v3(d, b, a);
546         return dot_v3v3(d, d);
547 }
548
549 MINLINE float normalize_v2_v2(float r[2], const float a[2])
550 {
551         float d = dot_v2v2(a, a);
552
553         if (d > 1.0e-35f) {
554                 d = sqrtf(d);
555                 mul_v2_v2fl(r, a, 1.0f / d);
556         }
557         else {
558                 zero_v2(r);
559                 d = 0.0f;
560         }
561
562         return d;
563 }
564
565 MINLINE float normalize_v2(float n[2])
566 {
567         return normalize_v2_v2(n, n);
568 }
569
570 MINLINE float normalize_v3_v3(float r[3], const float a[3])
571 {
572         float d = dot_v3v3(a, a);
573
574         /* a larger value causes normalize errors in a
575          * scaled down models with camera xtreme close */
576         if (d > 1.0e-35f) {
577                 d = sqrtf(d);
578                 mul_v3_v3fl(r, a, 1.0f / d);
579         }
580         else {
581                 zero_v3(r);
582                 d = 0.0f;
583         }
584
585         return d;
586 }
587
588 MINLINE double normalize_v3_d(double n[3])
589 {
590         double d = n[0] * n[0] + n[1] * n[1] + n[2] * n[2];
591
592         /* a larger value causes normalize errors in a
593          * scaled down models with camera xtreme close */
594         if (d > 1.0e-35) {
595                 double mul;
596
597                 d = sqrt(d);
598                 mul = 1.0 / d;
599
600                 n[0] *= mul;
601                 n[1] *= mul;
602                 n[2] *= mul;
603         }
604         else {
605                 n[0] = n[1] = n[2] = 0;
606                 d = 0.0;
607         }
608
609         return d;
610 }
611
612 MINLINE float normalize_v3(float n[3])
613 {
614         return normalize_v3_v3(n, n);
615 }
616
617 MINLINE void normal_short_to_float_v3(float out[3], const short in[3])
618 {
619         out[0] = in[0] * (1.0f / 32767.0f);
620         out[1] = in[1] * (1.0f / 32767.0f);
621         out[2] = in[2] * (1.0f / 32767.0f);
622 }
623
624 MINLINE void normal_float_to_short_v3(short out[3], const float in[3])
625 {
626         out[0] = (short) (in[0] * 32767.0f);
627         out[1] = (short) (in[1] * 32767.0f);
628         out[2] = (short) (in[2] * 32767.0f);
629 }
630
631 /********************************* Comparison ********************************/
632
633
634 MINLINE int is_zero_v2(const float v[3])
635 {
636         return (v[0] == 0 && v[1] == 0);
637 }
638
639 MINLINE int is_zero_v3(const float v[3])
640 {
641         return (v[0] == 0 && v[1] == 0 && v[2] == 0);
642 }
643
644 MINLINE int is_zero_v4(const float v[4])
645 {
646         return (v[0] == 0 && v[1] == 0 && v[2] == 0 && v[3] == 0);
647 }
648
649 MINLINE int is_one_v3(const float v[3])
650 {
651         return (v[0] == 1 && v[1] == 1 && v[2] == 1);
652 }
653
654 MINLINE int equals_v2v2(const float v1[2], const float v2[2])
655 {
656         return ((v1[0] == v2[0]) && (v1[1] == v2[1]));
657 }
658
659 MINLINE int equals_v3v3(const float v1[3], const float v2[3])
660 {
661         return ((v1[0] == v2[0]) && (v1[1] == v2[1]) && (v1[2] == v2[2]));
662 }
663
664 MINLINE int equals_v4v4(const float v1[4], const float v2[4])
665 {
666         return ((v1[0] == v2[0]) && (v1[1] == v2[1]) && (v1[2] == v2[2]) && (v1[3] == v2[3]));
667 }
668
669 MINLINE int compare_v3v3(const float v1[3], const float v2[3], const float limit)
670 {
671         if (fabsf(v1[0] - v2[0]) < limit)
672                 if (fabsf(v1[1] - v2[1]) < limit)
673                         if (fabsf(v1[2] - v2[2]) < limit)
674                                 return 1;
675
676         return 0;
677 }
678
679 MINLINE int compare_len_v3v3(const float v1[3], const float v2[3], const float limit)
680 {
681         float x, y, z;
682
683         x = v1[0] - v2[0];
684         y = v1[1] - v2[1];
685         z = v1[2] - v2[2];
686
687         return ((x * x + y * y + z * z) < (limit * limit));
688 }
689
690 MINLINE int compare_v4v4(const float v1[4], const float v2[4], const float limit)
691 {
692         if (fabsf(v1[0] - v2[0]) < limit)
693                 if (fabsf(v1[1] - v2[1]) < limit)
694                         if (fabsf(v1[2] - v2[2]) < limit)
695                                 if (fabsf(v1[3] - v2[3]) < limit)
696                                         return 1;
697
698         return 0;
699 }
700
701 MINLINE float line_point_side_v2(const float l1[2], const float l2[2], const float pt[2])
702 {
703         return (((l1[0] - pt[0]) * (l2[1] - pt[1])) -
704                 ((l2[0] - pt[0]) * (l1[1] - pt[1])));
705 }
706
707 #endif /* __MATH_VECTOR_INLINE_C__ */