caa38c9cf08d2f9e828ebe13908b26f53f2c064e
[blender.git] / source / blender / blenlib / intern / math_vector_inline.c
1 /*
2  * This program is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU General Public License
4  * as published by the Free Software Foundation; either version 2
5  * of the License, or (at your option) any later version.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10  * GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License
13  * along with this program; if not, write to the Free Software Foundation,
14  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15  *
16  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
17  * All rights reserved.
18  *
19  * The Original Code is: some of this file.
20  *
21  * */
22
23 /** \file
24  * \ingroup bli
25  */
26
27 #ifndef __MATH_VECTOR_INLINE_C__
28 #define __MATH_VECTOR_INLINE_C__
29
30 #include "BLI_math.h"
31
32 /********************************** Init *************************************/
33
34 MINLINE void zero_v2(float r[2])
35 {
36   r[0] = 0.0f;
37   r[1] = 0.0f;
38 }
39
40 MINLINE void zero_v3(float r[3])
41 {
42   r[0] = 0.0f;
43   r[1] = 0.0f;
44   r[2] = 0.0f;
45 }
46
47 MINLINE void zero_v4(float r[4])
48 {
49   r[0] = 0.0f;
50   r[1] = 0.0f;
51   r[2] = 0.0f;
52   r[3] = 0.0f;
53 }
54
55 MINLINE void copy_v2_v2(float r[2], const float a[2])
56 {
57   r[0] = a[0];
58   r[1] = a[1];
59 }
60
61 MINLINE void copy_v3_v3(float r[3], const float a[3])
62 {
63   r[0] = a[0];
64   r[1] = a[1];
65   r[2] = a[2];
66 }
67
68 MINLINE void copy_v3fl_v3s(float r[3], const short a[3])
69 {
70   r[0] = (float)a[0];
71   r[1] = (float)a[1];
72   r[2] = (float)a[2];
73 }
74
75 MINLINE void copy_v4_v4(float r[4], const float a[4])
76 {
77   r[0] = a[0];
78   r[1] = a[1];
79   r[2] = a[2];
80   r[3] = a[3];
81 }
82
83 MINLINE void copy_v2_fl(float r[2], float f)
84 {
85   r[0] = f;
86   r[1] = f;
87 }
88
89 MINLINE void copy_v3_fl(float r[3], float f)
90 {
91   r[0] = f;
92   r[1] = f;
93   r[2] = f;
94 }
95
96 MINLINE void copy_v4_fl(float r[4], float f)
97 {
98   r[0] = f;
99   r[1] = f;
100   r[2] = f;
101   r[3] = f;
102 }
103
104 /* unsigned char */
105 MINLINE void copy_v2_v2_uchar(unsigned char r[2], const unsigned char a[2])
106 {
107   r[0] = a[0];
108   r[1] = a[1];
109 }
110
111 MINLINE void copy_v3_v3_uchar(unsigned char r[3], const unsigned char a[3])
112 {
113   r[0] = a[0];
114   r[1] = a[1];
115   r[2] = a[2];
116 }
117
118 MINLINE void copy_v4_v4_uchar(unsigned char r[4], const unsigned char a[4])
119 {
120   r[0] = a[0];
121   r[1] = a[1];
122   r[2] = a[2];
123   r[3] = a[3];
124 }
125
126 /* char */
127 MINLINE void copy_v2_v2_char(char r[2], const char a[2])
128 {
129   r[0] = a[0];
130   r[1] = a[1];
131 }
132
133 MINLINE void copy_v3_v3_char(char r[3], const char a[3])
134 {
135   r[0] = a[0];
136   r[1] = a[1];
137   r[2] = a[2];
138 }
139
140 MINLINE void copy_v4_v4_char(char r[4], const char a[4])
141 {
142   r[0] = a[0];
143   r[1] = a[1];
144   r[2] = a[2];
145   r[3] = a[3];
146 }
147
148 /* short */
149
150 MINLINE void copy_v2_v2_short(short r[2], const short a[2])
151 {
152   r[0] = a[0];
153   r[1] = a[1];
154 }
155
156 MINLINE void copy_v3_v3_short(short r[3], const short a[3])
157 {
158   r[0] = a[0];
159   r[1] = a[1];
160   r[2] = a[2];
161 }
162
163 MINLINE void copy_v4_v4_short(short r[4], const short a[4])
164 {
165   r[0] = a[0];
166   r[1] = a[1];
167   r[2] = a[2];
168   r[3] = a[3];
169 }
170
171 /* int */
172 MINLINE void zero_v2_int(int r[2])
173 {
174   r[0] = 0;
175   r[1] = 0;
176 }
177
178 MINLINE void zero_v3_int(int r[3])
179 {
180   r[0] = 0;
181   r[1] = 0;
182   r[2] = 0;
183 }
184
185 MINLINE void copy_v2_v2_int(int r[2], const int a[2])
186 {
187   r[0] = a[0];
188   r[1] = a[1];
189 }
190
191 MINLINE void copy_v3_v3_int(int r[3], const int a[3])
192 {
193   r[0] = a[0];
194   r[1] = a[1];
195   r[2] = a[2];
196 }
197
198 MINLINE void copy_v4_v4_int(int r[4], const int a[4])
199 {
200   r[0] = a[0];
201   r[1] = a[1];
202   r[2] = a[2];
203   r[3] = a[3];
204 }
205
206 /* double */
207 MINLINE void zero_v3_db(double r[3])
208 {
209   r[0] = 0.0;
210   r[1] = 0.0;
211   r[2] = 0.0;
212 }
213
214 MINLINE void copy_v2_v2_db(double r[2], const double a[2])
215 {
216   r[0] = a[0];
217   r[1] = a[1];
218 }
219
220 MINLINE void copy_v3_v3_db(double r[3], const double a[3])
221 {
222   r[0] = a[0];
223   r[1] = a[1];
224   r[2] = a[2];
225 }
226
227 MINLINE void copy_v4_v4_db(double r[4], const double a[4])
228 {
229   r[0] = a[0];
230   r[1] = a[1];
231   r[2] = a[2];
232   r[3] = a[3];
233 }
234
235 /* int <-> float */
236 MINLINE void round_v2i_v2fl(int r[2], const float a[2])
237 {
238   r[0] = (int)roundf(a[0]);
239   r[1] = (int)roundf(a[1]);
240 }
241
242 MINLINE void copy_v2fl_v2i(float r[2], const int a[2])
243 {
244   r[0] = (float)a[0];
245   r[1] = (float)a[1];
246 }
247
248 /* double -> float */
249 MINLINE void copy_v2fl_v2db(float r[2], const double a[2])
250 {
251   r[0] = (float)a[0];
252   r[1] = (float)a[1];
253 }
254
255 MINLINE void copy_v3fl_v3db(float r[3], const double a[3])
256 {
257   r[0] = (float)a[0];
258   r[1] = (float)a[1];
259   r[2] = (float)a[2];
260 }
261
262 MINLINE void copy_v4fl_v4db(float r[4], const double a[4])
263 {
264   r[0] = (float)a[0];
265   r[1] = (float)a[1];
266   r[2] = (float)a[2];
267   r[3] = (float)a[3];
268 }
269
270 /* float -> double */
271 MINLINE void copy_v2db_v2fl(double r[2], const float a[2])
272 {
273   r[0] = (double)a[0];
274   r[1] = (double)a[1];
275 }
276
277 MINLINE void copy_v3db_v3fl(double r[3], const float a[3])
278 {
279   r[0] = (double)a[0];
280   r[1] = (double)a[1];
281   r[2] = (double)a[2];
282 }
283
284 MINLINE void copy_v4db_v4fl(double r[4], const float a[4])
285 {
286   r[0] = (double)a[0];
287   r[1] = (double)a[1];
288   r[2] = (double)a[2];
289   r[3] = (double)a[3];
290 }
291
292 MINLINE void swap_v2_v2(float a[2], float b[2])
293 {
294   SWAP(float, a[0], b[0]);
295   SWAP(float, a[1], b[1]);
296 }
297
298 MINLINE void swap_v3_v3(float a[3], float b[3])
299 {
300   SWAP(float, a[0], b[0]);
301   SWAP(float, a[1], b[1]);
302   SWAP(float, a[2], b[2]);
303 }
304
305 MINLINE void swap_v4_v4(float a[4], float b[4])
306 {
307   SWAP(float, a[0], b[0]);
308   SWAP(float, a[1], b[1]);
309   SWAP(float, a[2], b[2]);
310   SWAP(float, a[3], b[3]);
311 }
312
313 /* float args -> vec */
314 MINLINE void copy_v2_fl2(float v[2], float x, float y)
315 {
316   v[0] = x;
317   v[1] = y;
318 }
319
320 MINLINE void copy_v3_fl3(float v[3], float x, float y, float z)
321 {
322   v[0] = x;
323   v[1] = y;
324   v[2] = z;
325 }
326
327 MINLINE void copy_v4_fl4(float v[4], float x, float y, float z, float w)
328 {
329   v[0] = x;
330   v[1] = y;
331   v[2] = z;
332   v[3] = w;
333 }
334
335 /********************************* Arithmetic ********************************/
336
337 MINLINE void add_v2_fl(float r[2], float f)
338 {
339   r[0] += f;
340   r[1] += f;
341 }
342
343 MINLINE void add_v3_fl(float r[3], float f)
344 {
345   r[0] += f;
346   r[1] += f;
347   r[2] += f;
348 }
349
350 MINLINE void add_v4_fl(float r[4], float f)
351 {
352   r[0] += f;
353   r[1] += f;
354   r[2] += f;
355   r[3] += f;
356 }
357
358 MINLINE void add_v2_v2(float r[2], const float a[2])
359 {
360   r[0] += a[0];
361   r[1] += a[1];
362 }
363
364 MINLINE void add_v2_v2_db(double r[2], const double a[2])
365 {
366   r[0] += a[0];
367   r[1] += a[1];
368 }
369
370 MINLINE void add_v2_v2v2(float r[2], const float a[2], const float b[2])
371 {
372   r[0] = a[0] + b[0];
373   r[1] = a[1] + b[1];
374 }
375
376 MINLINE void add_v2_v2v2_int(int r[2], const int a[2], const int b[2])
377 {
378   r[0] = a[0] + b[0];
379   r[1] = a[1] + b[1];
380 }
381
382 MINLINE void add_v3_v3(float r[3], const float a[3])
383 {
384   r[0] += a[0];
385   r[1] += a[1];
386   r[2] += a[2];
387 }
388
389 MINLINE void add_v3_v3_db(double r[3], const double a[3])
390 {
391   r[0] += a[0];
392   r[1] += a[1];
393   r[2] += a[2];
394 }
395
396 MINLINE void add_v3_v3v3(float r[3], const float a[3], const float b[3])
397 {
398   r[0] = a[0] + b[0];
399   r[1] = a[1] + b[1];
400   r[2] = a[2] + b[2];
401 }
402
403 MINLINE void add_v3fl_v3fl_v3i(float r[3], const float a[3], const int b[3])
404 {
405   r[0] = a[0] + (float)b[0];
406   r[1] = a[1] + (float)b[1];
407   r[2] = a[2] + (float)b[2];
408 }
409
410 MINLINE void add_v3fl_v3fl_v3s(float r[3], const float a[3], const short b[3])
411 {
412   r[0] = a[0] + (float)b[0];
413   r[1] = a[1] + (float)b[1];
414   r[2] = a[2] + (float)b[2];
415 }
416
417 MINLINE void add_v4_v4(float r[4], const float a[4])
418 {
419   r[0] += a[0];
420   r[1] += a[1];
421   r[2] += a[2];
422   r[3] += a[3];
423 }
424
425 MINLINE void add_v4_v4v4(float r[4], const float a[4], const float b[4])
426 {
427   r[0] = a[0] + b[0];
428   r[1] = a[1] + b[1];
429   r[2] = a[2] + b[2];
430   r[3] = a[3] + b[3];
431 }
432
433 MINLINE void sub_v2_v2(float r[2], const float a[2])
434 {
435   r[0] -= a[0];
436   r[1] -= a[1];
437 }
438
439 MINLINE void sub_v2_v2v2(float r[2], const float a[2], const float b[2])
440 {
441   r[0] = a[0] - b[0];
442   r[1] = a[1] - b[1];
443 }
444
445 MINLINE void sub_v2_v2v2_db(double r[2], const double a[2], const double b[2])
446 {
447   r[0] = a[0] - b[0];
448   r[1] = a[1] - b[1];
449 }
450
451 MINLINE void sub_v2_v2v2_int(int r[2], const int a[2], const int b[2])
452 {
453   r[0] = a[0] - b[0];
454   r[1] = a[1] - b[1];
455 }
456
457 MINLINE void sub_v3_v3(float r[3], const float a[3])
458 {
459   r[0] -= a[0];
460   r[1] -= a[1];
461   r[2] -= a[2];
462 }
463
464 MINLINE void sub_v3_v3v3(float r[3], const float a[3], const float b[3])
465 {
466   r[0] = a[0] - b[0];
467   r[1] = a[1] - b[1];
468   r[2] = a[2] - b[2];
469 }
470
471 MINLINE void sub_v3_v3v3_int(int r[3], const int a[3], const int b[3])
472 {
473   r[0] = a[0] - b[0];
474   r[1] = a[1] - b[1];
475   r[2] = a[2] - b[2];
476 }
477
478 MINLINE void sub_v3_v3v3_db(double r[3], const double a[3], const double b[3])
479 {
480   r[0] = a[0] - b[0];
481   r[1] = a[1] - b[1];
482   r[2] = a[2] - b[2];
483 }
484
485 MINLINE void sub_v3db_v3fl_v3fl(double r[3], const float a[3], const float b[3])
486 {
487   r[0] = (double)a[0] - (double)b[0];
488   r[1] = (double)a[1] - (double)b[1];
489   r[2] = (double)a[2] - (double)b[2];
490 }
491
492 MINLINE void sub_v4_v4(float r[4], const float a[4])
493 {
494   r[0] -= a[0];
495   r[1] -= a[1];
496   r[2] -= a[2];
497   r[3] -= a[3];
498 }
499
500 MINLINE void sub_v4_v4v4(float r[4], const float a[4], const float b[4])
501 {
502   r[0] = a[0] - b[0];
503   r[1] = a[1] - b[1];
504   r[2] = a[2] - b[2];
505   r[3] = a[3] - b[3];
506 }
507
508 MINLINE void mul_v2_fl(float r[2], float f)
509 {
510   r[0] *= f;
511   r[1] *= f;
512 }
513
514 MINLINE void mul_v2_v2fl(float r[2], const float a[2], float f)
515 {
516   r[0] = a[0] * f;
517   r[1] = a[1] * f;
518 }
519
520 MINLINE void mul_v3_fl(float r[3], float f)
521 {
522   r[0] *= f;
523   r[1] *= f;
524   r[2] *= f;
525 }
526
527 MINLINE void mul_v3db_db(double r[3], double f)
528 {
529   r[0] *= f;
530   r[1] *= f;
531   r[2] *= f;
532 }
533
534 MINLINE void mul_v3_v3fl(float r[3], const float a[3], float f)
535 {
536   r[0] = a[0] * f;
537   r[1] = a[1] * f;
538   r[2] = a[2] * f;
539 }
540
541 MINLINE void mul_v2_v2(float r[2], const float a[2])
542 {
543   r[0] *= a[0];
544   r[1] *= a[1];
545 }
546
547 MINLINE void mul_v3_v3(float r[3], const float a[3])
548 {
549   r[0] *= a[0];
550   r[1] *= a[1];
551   r[2] *= a[2];
552 }
553
554 MINLINE void mul_v4_fl(float r[4], float f)
555 {
556   r[0] *= f;
557   r[1] *= f;
558   r[2] *= f;
559   r[3] *= f;
560 }
561
562 MINLINE void mul_v4_v4(float r[4], const float a[4])
563 {
564   r[0] *= a[0];
565   r[1] *= a[1];
566   r[2] *= a[2];
567   r[3] *= a[3];
568 }
569
570 MINLINE void mul_v4_v4fl(float r[4], const float a[4], float f)
571 {
572   r[0] = a[0] * f;
573   r[1] = a[1] * f;
574   r[2] = a[2] * f;
575   r[3] = a[3] * f;
576 }
577
578 /**
579  * Avoid doing:
580  *
581  * angle = atan2f(dvec[0], dvec[1]);
582  * angle_to_mat2(mat, angle);
583  *
584  * instead use a vector as a matrix.
585  */
586
587 MINLINE void mul_v2_v2_cw(float r[2], const float mat[2], const float vec[2])
588 {
589   BLI_assert(r != vec);
590
591   r[0] = mat[0] * vec[0] + (+mat[1]) * vec[1];
592   r[1] = mat[1] * vec[0] + (-mat[0]) * vec[1];
593 }
594
595 MINLINE void mul_v2_v2_ccw(float r[2], const float mat[2], const float vec[2])
596 {
597   BLI_assert(r != vec);
598
599   r[0] = mat[0] * vec[0] + (-mat[1]) * vec[1];
600   r[1] = mat[1] * vec[0] + (+mat[0]) * vec[1];
601 }
602
603 /**
604  * Convenience function to get the projected depth of a position.
605  * This avoids creating a temporary 4D vector and multiplying it - only for the 4th component.
606  *
607  * Matches logic for:
608  *
609  * \code{.c}
610  * float co_4d[4] = {co[0], co[1], co[2], 1.0};
611  * mul_m4_v4(mat, co_4d);
612  * return co_4d[3];
613  * \endcode
614  */
615 MINLINE float mul_project_m4_v3_zfac(const float mat[4][4], const float co[3])
616 {
617   return (mat[0][3] * co[0]) + (mat[1][3] * co[1]) + (mat[2][3] * co[2]) + mat[3][3];
618 }
619
620 /**
621  * Has the effect of #mul_m3_v3(), on a single axis.
622  */
623 MINLINE float dot_m3_v3_row_x(const float M[3][3], const float a[3])
624 {
625   return M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
626 }
627 MINLINE float dot_m3_v3_row_y(const float M[3][3], const float a[3])
628 {
629   return M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
630 }
631 MINLINE float dot_m3_v3_row_z(const float M[3][3], const float a[3])
632 {
633   return M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2];
634 }
635
636 /**
637  * Has the effect of #mul_mat3_m4_v3(), on a single axis.
638  * (no adding translation)
639  */
640 MINLINE float dot_m4_v3_row_x(const float M[4][4], const float a[3])
641 {
642   return M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
643 }
644 MINLINE float dot_m4_v3_row_y(const float M[4][4], const float a[3])
645 {
646   return M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
647 }
648 MINLINE float dot_m4_v3_row_z(const float M[4][4], const float a[3])
649 {
650   return M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2];
651 }
652
653 MINLINE void madd_v2_v2fl(float r[2], const float a[2], float f)
654 {
655   r[0] += a[0] * f;
656   r[1] += a[1] * f;
657 }
658
659 MINLINE void madd_v3_v3fl(float r[3], const float a[3], float f)
660 {
661   r[0] += a[0] * f;
662   r[1] += a[1] * f;
663   r[2] += a[2] * f;
664 }
665
666 MINLINE void madd_v3_v3v3(float r[3], const float a[3], const float b[3])
667 {
668   r[0] += a[0] * b[0];
669   r[1] += a[1] * b[1];
670   r[2] += a[2] * b[2];
671 }
672
673 MINLINE void madd_v2_v2v2fl(float r[2], const float a[2], const float b[2], float f)
674 {
675   r[0] = a[0] + b[0] * f;
676   r[1] = a[1] + b[1] * f;
677 }
678
679 MINLINE void madd_v3_v3v3fl(float r[3], const float a[3], const float b[3], float f)
680 {
681   r[0] = a[0] + b[0] * f;
682   r[1] = a[1] + b[1] * f;
683   r[2] = a[2] + b[2] * f;
684 }
685
686 MINLINE void madd_v3_v3v3v3(float r[3], const float a[3], const float b[3], const float c[3])
687 {
688   r[0] = a[0] + b[0] * c[0];
689   r[1] = a[1] + b[1] * c[1];
690   r[2] = a[2] + b[2] * c[2];
691 }
692
693 MINLINE void madd_v3fl_v3fl_v3fl_v3i(float r[3],
694                                      const float a[3],
695                                      const float b[3],
696                                      const int c[3])
697 {
698   r[0] = a[0] + b[0] * (float)c[0];
699   r[1] = a[1] + b[1] * (float)c[1];
700   r[2] = a[2] + b[2] * (float)c[2];
701 }
702
703 MINLINE void madd_v4_v4fl(float r[4], const float a[4], float f)
704 {
705   r[0] += a[0] * f;
706   r[1] += a[1] * f;
707   r[2] += a[2] * f;
708   r[3] += a[3] * f;
709 }
710
711 MINLINE void madd_v4_v4v4(float r[4], const float a[4], const float b[4])
712 {
713   r[0] += a[0] * b[0];
714   r[1] += a[1] * b[1];
715   r[2] += a[2] * b[2];
716   r[3] += a[3] * b[3];
717 }
718
719 MINLINE void mul_v3_v3v3(float r[3], const float v1[3], const float v2[3])
720 {
721   r[0] = v1[0] * v2[0];
722   r[1] = v1[1] * v2[1];
723   r[2] = v1[2] * v2[2];
724 }
725
726 MINLINE void mul_v2_v2v2(float r[2], const float a[2], const float b[2])
727 {
728   r[0] = a[0] * b[0];
729   r[1] = a[1] * b[1];
730 }
731
732 MINLINE void negate_v2(float r[2])
733 {
734   r[0] = -r[0];
735   r[1] = -r[1];
736 }
737
738 MINLINE void negate_v2_v2(float r[2], const float a[2])
739 {
740   r[0] = -a[0];
741   r[1] = -a[1];
742 }
743
744 MINLINE void negate_v3(float r[3])
745 {
746   r[0] = -r[0];
747   r[1] = -r[1];
748   r[2] = -r[2];
749 }
750
751 MINLINE void negate_v3_v3(float r[3], const float a[3])
752 {
753   r[0] = -a[0];
754   r[1] = -a[1];
755   r[2] = -a[2];
756 }
757
758 MINLINE void negate_v4(float r[4])
759 {
760   r[0] = -r[0];
761   r[1] = -r[1];
762   r[2] = -r[2];
763   r[3] = -r[3];
764 }
765
766 MINLINE void negate_v4_v4(float r[4], const float a[4])
767 {
768   r[0] = -a[0];
769   r[1] = -a[1];
770   r[2] = -a[2];
771   r[3] = -a[3];
772 }
773
774 /* could add more... */
775 MINLINE void negate_v3_short(short r[3])
776 {
777   r[0] = (short)-r[0];
778   r[1] = (short)-r[1];
779   r[2] = (short)-r[2];
780 }
781
782 MINLINE void negate_v3_db(double r[3])
783 {
784   r[0] = -r[0];
785   r[1] = -r[1];
786   r[2] = -r[2];
787 }
788
789 MINLINE void invert_v2(float r[2])
790 {
791   BLI_assert(!ELEM(0.0f, r[0], r[1]));
792   r[0] = 1.0f / r[0];
793   r[1] = 1.0f / r[1];
794 }
795
796 MINLINE void invert_v3(float r[3])
797 {
798   BLI_assert(!ELEM(0.0f, r[0], r[1], r[2]));
799   r[0] = 1.0f / r[0];
800   r[1] = 1.0f / r[1];
801   r[2] = 1.0f / r[2];
802 }
803
804 MINLINE void abs_v2(float r[2])
805 {
806   r[0] = fabsf(r[0]);
807   r[1] = fabsf(r[1]);
808 }
809
810 MINLINE void abs_v2_v2(float r[2], const float a[2])
811 {
812   r[0] = fabsf(a[0]);
813   r[1] = fabsf(a[1]);
814 }
815
816 MINLINE void abs_v3(float r[3])
817 {
818   r[0] = fabsf(r[0]);
819   r[1] = fabsf(r[1]);
820   r[2] = fabsf(r[2]);
821 }
822
823 MINLINE void abs_v3_v3(float r[3], const float a[3])
824 {
825   r[0] = fabsf(a[0]);
826   r[1] = fabsf(a[1]);
827   r[2] = fabsf(a[2]);
828 }
829
830 MINLINE void abs_v4(float r[4])
831 {
832   r[0] = fabsf(r[0]);
833   r[1] = fabsf(r[1]);
834   r[2] = fabsf(r[2]);
835   r[3] = fabsf(r[3]);
836 }
837
838 MINLINE void abs_v4_v4(float r[4], const float a[4])
839 {
840   r[0] = fabsf(a[0]);
841   r[1] = fabsf(a[1]);
842   r[2] = fabsf(a[2]);
843   r[3] = fabsf(a[3]);
844 }
845
846 MINLINE float dot_v2v2(const float a[2], const float b[2])
847 {
848   return a[0] * b[0] + a[1] * b[1];
849 }
850
851 MINLINE double dot_v2v2_db(const double a[2], const double b[2])
852 {
853   return a[0] * b[0] + a[1] * b[1];
854 }
855
856 MINLINE float dot_v3v3(const float a[3], const float b[3])
857 {
858   return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
859 }
860
861 MINLINE float dot_v3v3v3(const float p[3], const float a[3], const float b[3])
862 {
863   float vec1[3], vec2[3];
864
865   sub_v3_v3v3(vec1, a, p);
866   sub_v3_v3v3(vec2, b, p);
867   if (is_zero_v3(vec1) || is_zero_v3(vec2)) {
868     return 0.0f;
869   }
870   return dot_v3v3(vec1, vec2);
871 }
872
873 MINLINE float dot_v4v4(const float a[4], const float b[4])
874 {
875   return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
876 }
877
878 MINLINE double dot_v3db_v3fl(const double a[3], const float b[3])
879 {
880   return a[0] * (double)b[0] + a[1] * (double)b[1] + a[2] * (double)b[2];
881 }
882
883 MINLINE double dot_v3v3_db(const double a[3], const double b[3])
884 {
885   return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
886 }
887
888 MINLINE float cross_v2v2(const float a[2], const float b[2])
889 {
890   return a[0] * b[1] - a[1] * b[0];
891 }
892
893 MINLINE void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
894 {
895   BLI_assert(r != a && r != b);
896   r[0] = a[1] * b[2] - a[2] * b[1];
897   r[1] = a[2] * b[0] - a[0] * b[2];
898   r[2] = a[0] * b[1] - a[1] * b[0];
899 }
900
901 /* cross product suffers from severe precision loss when vectors are
902  * nearly parallel or opposite; doing the computation in double helps a lot */
903 MINLINE void cross_v3_v3v3_hi_prec(float r[3], const float a[3], const float b[3])
904 {
905   BLI_assert(r != a && r != b);
906   r[0] = (float)((double)a[1] * (double)b[2] - (double)a[2] * (double)b[1]);
907   r[1] = (float)((double)a[2] * (double)b[0] - (double)a[0] * (double)b[2]);
908   r[2] = (float)((double)a[0] * (double)b[1] - (double)a[1] * (double)b[0]);
909 }
910
911 MINLINE void cross_v3_v3v3_db(double r[3], const double a[3], const double b[3])
912 {
913   BLI_assert(r != a && r != b);
914   r[0] = a[1] * b[2] - a[2] * b[1];
915   r[1] = a[2] * b[0] - a[0] * b[2];
916   r[2] = a[0] * b[1] - a[1] * b[0];
917 }
918
919 /* Newell's Method */
920 /* excuse this fairly specific function,
921  * its used for polygon normals all over the place
922  * could use a better name */
923 MINLINE void add_newell_cross_v3_v3v3(float n[3], const float v_prev[3], const float v_curr[3])
924 {
925   n[0] += (v_prev[1] - v_curr[1]) * (v_prev[2] + v_curr[2]);
926   n[1] += (v_prev[2] - v_curr[2]) * (v_prev[0] + v_curr[0]);
927   n[2] += (v_prev[0] - v_curr[0]) * (v_prev[1] + v_curr[1]);
928 }
929
930 MINLINE void star_m3_v3(float rmat[3][3], float a[3])
931 {
932   rmat[0][0] = rmat[1][1] = rmat[2][2] = 0.0;
933   rmat[0][1] = -a[2];
934   rmat[0][2] = a[1];
935   rmat[1][0] = a[2];
936   rmat[1][2] = -a[0];
937   rmat[2][0] = -a[1];
938   rmat[2][1] = a[0];
939 }
940
941 /*********************************** Length **********************************/
942
943 MINLINE float len_squared_v2(const float v[2])
944 {
945   return v[0] * v[0] + v[1] * v[1];
946 }
947
948 MINLINE float len_squared_v3(const float v[3])
949 {
950   return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
951 }
952
953 MINLINE float len_manhattan_v2(const float v[2])
954 {
955   return fabsf(v[0]) + fabsf(v[1]);
956 }
957
958 MINLINE int len_manhattan_v2_int(const int v[2])
959 {
960   return abs(v[0]) + abs(v[1]);
961 }
962
963 MINLINE float len_manhattan_v3(const float v[3])
964 {
965   return fabsf(v[0]) + fabsf(v[1]) + fabsf(v[2]);
966 }
967
968 MINLINE float len_v2(const float v[2])
969 {
970   return sqrtf(v[0] * v[0] + v[1] * v[1]);
971 }
972
973 MINLINE float len_v2v2(const float v1[2], const float v2[2])
974 {
975   float x, y;
976
977   x = v1[0] - v2[0];
978   y = v1[1] - v2[1];
979   return sqrtf(x * x + y * y);
980 }
981
982 MINLINE double len_v2v2_db(const double v1[2], const double v2[2])
983 {
984   double x, y;
985
986   x = v1[0] - v2[0];
987   y = v1[1] - v2[1];
988   return sqrt(x * x + y * y);
989 }
990
991 MINLINE float len_v2v2_int(const int v1[2], const int v2[2])
992 {
993   float x, y;
994
995   x = (float)(v1[0] - v2[0]);
996   y = (float)(v1[1] - v2[1]);
997   return sqrtf(x * x + y * y);
998 }
999
1000 MINLINE float len_v3(const float a[3])
1001 {
1002   return sqrtf(dot_v3v3(a, a));
1003 }
1004
1005 MINLINE float len_squared_v2v2(const float a[2], const float b[2])
1006 {
1007   float d[2];
1008
1009   sub_v2_v2v2(d, b, a);
1010   return dot_v2v2(d, d);
1011 }
1012
1013 MINLINE double len_squared_v2v2_db(const double a[2], const double b[2])
1014 {
1015   double d[2];
1016
1017   sub_v2_v2v2_db(d, b, a);
1018   return dot_v2v2_db(d, d);
1019 }
1020
1021 MINLINE float len_squared_v3v3(const float a[3], const float b[3])
1022 {
1023   float d[3];
1024
1025   sub_v3_v3v3(d, b, a);
1026   return dot_v3v3(d, d);
1027 }
1028
1029 MINLINE float len_squared_v4v4(const float a[4], const float b[4])
1030 {
1031   float d[4];
1032
1033   sub_v4_v4v4(d, b, a);
1034   return dot_v4v4(d, d);
1035 }
1036
1037 MINLINE float len_manhattan_v2v2(const float a[2], const float b[2])
1038 {
1039   float d[2];
1040
1041   sub_v2_v2v2(d, b, a);
1042   return len_manhattan_v2(d);
1043 }
1044
1045 MINLINE int len_manhattan_v2v2_int(const int a[2], const int b[2])
1046 {
1047   int d[2];
1048
1049   sub_v2_v2v2_int(d, b, a);
1050   return len_manhattan_v2_int(d);
1051 }
1052
1053 MINLINE float len_manhattan_v3v3(const float a[3], const float b[3])
1054 {
1055   float d[3];
1056
1057   sub_v3_v3v3(d, b, a);
1058   return len_manhattan_v3(d);
1059 }
1060
1061 MINLINE float len_v3v3(const float a[3], const float b[3])
1062 {
1063   float d[3];
1064
1065   sub_v3_v3v3(d, b, a);
1066   return len_v3(d);
1067 }
1068
1069 MINLINE float normalize_v2_v2_length(float r[2], const float a[2], const float unit_length)
1070 {
1071   float d = dot_v2v2(a, a);
1072
1073   if (d > 1.0e-35f) {
1074     d = sqrtf(d);
1075     mul_v2_v2fl(r, a, unit_length / d);
1076   }
1077   else {
1078     zero_v2(r);
1079     d = 0.0f;
1080   }
1081
1082   return d;
1083 }
1084 MINLINE float normalize_v2_v2(float r[2], const float a[2])
1085 {
1086   return normalize_v2_v2_length(r, a, 1.0f);
1087 }
1088
1089 MINLINE float normalize_v2(float n[2])
1090 {
1091   return normalize_v2_v2(n, n);
1092 }
1093
1094 MINLINE float normalize_v2_length(float n[2], const float unit_length)
1095 {
1096   return normalize_v2_v2_length(n, n, unit_length);
1097 }
1098
1099 MINLINE float normalize_v3_v3_length(float r[3], const float a[3], const float unit_length)
1100 {
1101   float d = dot_v3v3(a, a);
1102
1103   /* a larger value causes normalize errors in a
1104    * scaled down models with camera extreme close */
1105   if (d > 1.0e-35f) {
1106     d = sqrtf(d);
1107     mul_v3_v3fl(r, a, unit_length / d);
1108   }
1109   else {
1110     zero_v3(r);
1111     d = 0.0f;
1112   }
1113
1114   return d;
1115 }
1116 MINLINE float normalize_v3_v3(float r[3], const float a[3])
1117 {
1118   return normalize_v3_v3_length(r, a, 1.0f);
1119 }
1120
1121 MINLINE double normalize_v3_length_d(double n[3], const double unit_length)
1122 {
1123   double d = n[0] * n[0] + n[1] * n[1] + n[2] * n[2];
1124
1125   /* a larger value causes normalize errors in a
1126    * scaled down models with camera extreme close */
1127   if (d > 1.0e-35) {
1128     double mul;
1129
1130     d = sqrt(d);
1131     mul = unit_length / d;
1132
1133     n[0] *= mul;
1134     n[1] *= mul;
1135     n[2] *= mul;
1136   }
1137   else {
1138     n[0] = n[1] = n[2] = 0;
1139     d = 0.0;
1140   }
1141
1142   return d;
1143 }
1144 MINLINE double normalize_v3_d(double n[3])
1145 {
1146   return normalize_v3_length_d(n, 1.0);
1147 }
1148
1149 MINLINE float normalize_v3_length(float n[3], const float unit_length)
1150 {
1151   return normalize_v3_v3_length(n, n, unit_length);
1152 }
1153
1154 MINLINE float normalize_v3(float n[3])
1155 {
1156   return normalize_v3_v3(n, n);
1157 }
1158
1159 MINLINE void normal_float_to_short_v2(short out[2], const float in[2])
1160 {
1161   out[0] = (short)(in[0] * 32767.0f);
1162   out[1] = (short)(in[1] * 32767.0f);
1163 }
1164
1165 MINLINE void normal_short_to_float_v3(float out[3], const short in[3])
1166 {
1167   out[0] = in[0] * (1.0f / 32767.0f);
1168   out[1] = in[1] * (1.0f / 32767.0f);
1169   out[2] = in[2] * (1.0f / 32767.0f);
1170 }
1171
1172 MINLINE void normal_float_to_short_v3(short out[3], const float in[3])
1173 {
1174   out[0] = (short)(in[0] * 32767.0f);
1175   out[1] = (short)(in[1] * 32767.0f);
1176   out[2] = (short)(in[2] * 32767.0f);
1177 }
1178
1179 MINLINE void normal_float_to_short_v4(short out[4], const float in[4])
1180 {
1181   out[0] = (short)(in[0] * 32767.0f);
1182   out[1] = (short)(in[1] * 32767.0f);
1183   out[2] = (short)(in[2] * 32767.0f);
1184   out[3] = (short)(in[3] * 32767.0f);
1185 }
1186
1187 /********************************* Comparison ********************************/
1188
1189 MINLINE bool is_zero_v2(const float v[2])
1190 {
1191   return (v[0] == 0.0f && v[1] == 0.0f);
1192 }
1193
1194 MINLINE bool is_zero_v3(const float v[3])
1195 {
1196   return (v[0] == 0.0f && v[1] == 0.0f && v[2] == 0.0f);
1197 }
1198
1199 MINLINE bool is_zero_v4(const float v[4])
1200 {
1201   return (v[0] == 0.0f && v[1] == 0.0f && v[2] == 0.0f && v[3] == 0.0f);
1202 }
1203
1204 MINLINE bool is_one_v3(const float v[3])
1205 {
1206   return (v[0] == 1.0f && v[1] == 1.0f && v[2] == 1.0f);
1207 }
1208
1209 /** \name Vector Comparison
1210  *
1211  * \note use ``value <= limit``, so a limit of zero doesn't fail on an exact match.
1212  * \{ */
1213
1214 MINLINE bool equals_v2v2(const float v1[2], const float v2[2])
1215 {
1216   return ((v1[0] == v2[0]) && (v1[1] == v2[1]));
1217 }
1218
1219 MINLINE bool equals_v3v3(const float v1[3], const float v2[3])
1220 {
1221   return ((v1[0] == v2[0]) && (v1[1] == v2[1]) && (v1[2] == v2[2]));
1222 }
1223
1224 MINLINE bool equals_v4v4(const float v1[4], const float v2[4])
1225 {
1226   return ((v1[0] == v2[0]) && (v1[1] == v2[1]) && (v1[2] == v2[2]) && (v1[3] == v2[3]));
1227 }
1228
1229 MINLINE bool equals_v2v2_int(const int v1[2], const int v2[2])
1230 {
1231   return ((v1[0] == v2[0]) && (v1[1] == v2[1]));
1232 }
1233
1234 MINLINE bool compare_v2v2(const float v1[2], const float v2[2], const float limit)
1235 {
1236   return (compare_ff(v1[0], v2[0], limit) && compare_ff(v1[1], v2[1], limit));
1237 }
1238
1239 MINLINE bool compare_v3v3(const float v1[3], const float v2[3], const float limit)
1240 {
1241   return (compare_ff(v1[0], v2[0], limit) && compare_ff(v1[1], v2[1], limit) &&
1242           compare_ff(v1[2], v2[2], limit));
1243 }
1244
1245 MINLINE bool compare_v4v4(const float v1[4], const float v2[4], const float limit)
1246 {
1247   return (compare_ff(v1[0], v2[0], limit) && compare_ff(v1[1], v2[1], limit) &&
1248           compare_ff(v1[2], v2[2], limit) && compare_ff(v1[3], v2[3], limit));
1249 }
1250
1251 MINLINE bool compare_v2v2_relative(const float v1[2],
1252                                    const float v2[2],
1253                                    const float limit,
1254                                    const int max_ulps)
1255 {
1256   return (compare_ff_relative(v1[0], v2[0], limit, max_ulps) &&
1257           compare_ff_relative(v1[1], v2[1], limit, max_ulps));
1258 }
1259
1260 MINLINE bool compare_v3v3_relative(const float v1[3],
1261                                    const float v2[3],
1262                                    const float limit,
1263                                    const int max_ulps)
1264 {
1265   return (compare_ff_relative(v1[0], v2[0], limit, max_ulps) &&
1266           compare_ff_relative(v1[1], v2[1], limit, max_ulps) &&
1267           compare_ff_relative(v1[2], v2[2], limit, max_ulps));
1268 }
1269
1270 MINLINE bool compare_v4v4_relative(const float v1[4],
1271                                    const float v2[4],
1272                                    const float limit,
1273                                    const int max_ulps)
1274 {
1275   return (compare_ff_relative(v1[0], v2[0], limit, max_ulps) &&
1276           compare_ff_relative(v1[1], v2[1], limit, max_ulps) &&
1277           compare_ff_relative(v1[2], v2[2], limit, max_ulps) &&
1278           compare_ff_relative(v1[3], v2[3], limit, max_ulps));
1279 }
1280
1281 MINLINE bool compare_len_v3v3(const float v1[3], const float v2[3], const float limit)
1282 {
1283   float d[3];
1284   sub_v3_v3v3(d, v1, v2);
1285   return (dot_v3v3(d, d) <= (limit * limit));
1286 }
1287
1288 /**
1289  * <pre>
1290  *        + l1
1291  *        |
1292  * neg <- | -> pos
1293  *        |
1294  *        + l2
1295  * </pre>
1296  *
1297  * \return Positive value when 'pt' is left-of-line
1298  * (looking from 'l1' -> 'l2').
1299  */
1300 MINLINE float line_point_side_v2(const float l1[2], const float l2[2], const float pt[2])
1301 {
1302   return (((l1[0] - pt[0]) * (l2[1] - pt[1])) - ((l2[0] - pt[0]) * (l1[1] - pt[1])));
1303 }
1304
1305 /** \} */
1306
1307 #endif /* __MATH_VECTOR_INLINE_C__ */