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