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