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