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