8336b529da37acf142b019455b31780fef017346
[blender.git] / source / blender / blenlib / intern / math_vector.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., 59 Temple Place - Suite 330, Boston, MA  02111-1307, 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 #include <float.h>
29 #include <stdio.h>
30 #include <stdlib.h>
31 #include <string.h>
32
33 #include "BLI_math.h"
34
35 /********************************** Init *************************************/
36
37 void zero_v2(float r[2])
38 {
39         r[0]= 0.0f;
40         r[1]= 0.0f;
41 }
42
43 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 void copy_v2_v2(float r[2], float a[2])
51 {
52         r[0]= a[0];
53         r[1]= a[1];
54 }
55
56 void copy_v3_v3(float r[3], float a[3])
57 {
58         r[0]= a[0];
59         r[1]= a[1];
60         r[2]= a[2];
61 }
62
63 /********************************* Arithmetic ********************************/
64
65 void add_v2_v2(float *r, float *a)
66 {
67         r[0] += a[0];
68         r[1] += a[1];
69 }
70
71 void add_v2_v2v2(float *r, float *a, float *b)
72 {
73         r[0]= a[0] + b[0];
74         r[1]= a[1] + b[1];
75 }
76
77 void add_v3_v3(float *r, float *a)
78 {
79         r[0] += a[0];
80         r[1] += a[1];
81         r[1] += a[1];
82 }
83
84 void add_v3_v3v3(float *r, float *a, float *b)
85 {
86         r[0]= a[0] + b[0];
87         r[1]= a[1] + b[1];
88         r[2]= a[2] + b[2];
89 }
90
91 void sub_v2_v2(float *r, float *a)
92 {
93         r[0] -= a[0];
94         r[1] -= a[1];
95 }
96
97 void sub_v2_v2v2(float *r, float *a, float *b)
98 {
99         r[0]= a[0] - b[0];
100         r[1]= a[1] - b[1];
101 }
102
103 void sub_v3_v3(float *r, float *a)
104 {
105         r[0] -= a[0];
106         r[1] -= a[1];
107         r[1] -= a[1];
108 }
109
110 void sub_v3_v3v3(float *r, float *a, float *b)
111 {
112         r[0]= a[0] - b[0];
113         r[1]= a[1] - b[1];
114         r[2]= a[2] - b[2];
115 }
116
117 void mul_v2_fl(float *v1, float f)
118 {
119         v1[0]*= f;
120         v1[1]*= f;
121 }
122
123 void mul_v3_fl(float r[3], float f)
124 {
125         r[0] *= f;
126         r[1] *= f;
127         r[2] *= f;
128 }
129
130 void mul_v3_v3fl(float r[3], float a[3], float f)
131 {
132         r[0]= a[0]*f;
133         r[1]= a[1]*f;
134         r[2]= a[2]*f;
135 }
136
137 void mul_v3_v3(float r[3], float a[3])
138 {
139         r[0] *= a[0];
140         r[1] *= a[1];
141         r[2] *= a[2];
142 }
143
144 void mul_v3_v3v3(float *v, float *v1, float *v2)
145 {
146         v[0] = v1[0] * v2[0];
147         v[1] = v1[1] * v2[1];
148         v[2] = v1[2] * v2[2];
149 }
150
151 void negate_v3(float r[3])
152 {
153         r[0]= -r[0];
154         r[1]= -r[1];
155         r[2]= -r[2];
156 }
157
158 void negate_v3_v3(float r[3], float a[3])
159 {
160         r[0]= -a[0];
161         r[1]= -a[1];
162         r[2]= -a[2];
163 }
164
165 float dot_v2v2(float *a, float *b)
166 {
167         return a[0]*b[0] + a[1]*b[1];
168 }
169
170 float dot_v3v3(float a[3], float b[3])
171 {
172         return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
173 }
174
175 void cross_v3_v3v3(float r[3], float a[3], float b[3])
176 {
177         r[0]= a[1]*b[2] - a[2]*b[1];
178         r[1]= a[2]*b[0] - a[0]*b[2];
179         r[2]= a[0]*b[1] - a[1]*b[0];
180 }
181
182 void star_m3_v3(float mat[][3], float *vec)
183 {
184         mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
185         mat[0][1]= -vec[2];     
186         mat[0][2]= vec[1];
187         mat[1][0]= vec[2];      
188         mat[1][2]= -vec[0];
189         mat[2][0]= -vec[1];     
190         mat[2][1]= vec[0];
191         
192 }
193
194 /*********************************** Length **********************************/
195
196 float len_v2(float *v)
197 {
198         return (float)sqrt(v[0]*v[0] + v[1]*v[1]);
199 }
200
201 float len_v2v2(float *v1, float *v2)
202 {
203         float x, y;
204
205         x = v1[0]-v2[0];
206         y = v1[1]-v2[1];
207         return (float)sqrt(x*x+y*y);
208 }
209
210 float len_v3(float a[3])
211 {
212         return sqrtf(dot_v3v3(a, a));
213 }
214
215 float len_v3v3(float a[3], float b[3])
216 {
217         float d[3];
218
219         sub_v3_v3v3(d, b, a);
220         return len_v3(d);
221 }
222
223 float normalize_v2(float n[2])
224 {
225         float d;
226         
227         d= n[0]*n[0]+n[1]*n[1];
228
229         if(d>1.0e-35f) {
230                 d= (float)sqrt(d);
231                 n[0]/=d; 
232                 n[1]/=d; 
233         } else {
234                 n[0]=n[1]= 0.0f;
235                 d= 0.0f;
236         }
237         return d;
238 }
239
240 float normalize_v3(float n[3])
241 {
242         float d= dot_v3v3(n, n);
243
244         /* a larger value causes normalize errors in a
245            scaled down models with camera xtreme close */
246         if(d > 1.0e-35f) {
247                 d= sqrtf(d);
248                 mul_v3_fl(n, 1.0f/d);
249         }
250         else {
251                 zero_v3(n);
252                 d= 0.0f;
253         }
254
255         return d;
256 }
257
258 /******************************* Interpolation *******************************/
259
260 void interp_v2_v2v2(float *target, const float *a, const float *b, const float t)
261 {
262         const float s = 1.0f-t;
263
264         target[0]= s*a[0] + t*b[0];
265         target[1]= s*a[1] + t*b[1];
266 }
267
268 /* weight 3 2D vectors,
269  * 'w' must be unit length but is not a vector, just 3 weights */
270 void interp_v2_v2v2v2(float p[2], const float v1[2], const float v2[2], const float v3[2], const float w[3])
271 {
272         p[0] = v1[0]*w[0] + v2[0]*w[1] + v3[0]*w[2];
273         p[1] = v1[1]*w[0] + v2[1]*w[1] + v3[1]*w[2];
274 }
275
276
277
278 void interp_v3_v3v3(float *target, const float *a, const float *b, const float t)
279 {
280         const float s = 1.0f-t;
281
282         target[0]= s*a[0] + t*b[0];
283         target[1]= s*a[1] + t*b[1];
284         target[2]= s*a[2] + t*b[2];
285 }
286
287 /* weight 3 vectors,
288  * 'w' must be unit length but is not a vector, just 3 weights */
289 void interp_v3_v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3])
290 {
291         p[0] = v1[0]*w[0] + v2[0]*w[1] + v3[0]*w[2];
292         p[1] = v1[1]*w[0] + v2[1]*w[1] + v3[1]*w[2];
293         p[2] = v1[2]*w[0] + v2[2]*w[1] + v3[2]*w[2];
294 }
295
296 void mid_v3_v3v3(float *v, float *v1, float *v2)
297 {
298         v[0]= 0.5f*(v1[0]+ v2[0]);
299         v[1]= 0.5f*(v1[1]+ v2[1]);
300         v[2]= 0.5f*(v1[2]+ v2[2]);
301 }
302
303 /********************************* Comparison ********************************/
304
305 int is_zero_v3(float *v)
306 {
307         return (v[0] == 0 && v[1] == 0 && v[2] == 0);
308 }
309
310 int equals_v3v3(float *v1, float *v2)
311 {
312         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
313 }
314
315 int compare_v3v3(float *v1, float *v2, float limit)
316 {
317         if(fabs(v1[0]-v2[0])<limit)
318                 if(fabs(v1[1]-v2[1])<limit)
319                         if(fabs(v1[2]-v2[2])<limit)
320                                 return 1;
321
322         return 0;
323 }
324
325 int compare_len_v3v3(float *v1, float *v2, float limit)
326 {
327     float x,y,z;
328
329         x=v1[0]-v2[0];
330         y=v1[1]-v2[1];
331         z=v1[2]-v2[2];
332
333         return ((x*x + y*y + z*z) < (limit*limit));
334 }
335
336 int compare_v4v4(float *v1, float *v2, float limit)
337 {
338         if(fabs(v1[0]-v2[0])<limit)
339                 if(fabs(v1[1]-v2[1])<limit)
340                         if(fabs(v1[2]-v2[2])<limit)
341                                 if(fabs(v1[3]-v2[3])<limit)
342                                         return 1;
343
344         return 0;
345 }
346
347 /********************************** Angles ***********************************/
348
349 /* Return the angle in radians between vecs 1-2 and 2-3 in radians
350    If v1 is a shoulder, v2 is the elbow and v3 is the hand,
351    this would return the angle at the elbow */
352 float angle_v3v3v3(float *v1, float *v2, float *v3)
353 {
354         float vec1[3], vec2[3];
355
356         sub_v3_v3v3(vec1, v2, v1);
357         sub_v3_v3v3(vec2, v2, v3);
358         normalize_v3(vec1);
359         normalize_v3(vec2);
360
361         return angle_normalized_v3v3(vec1, vec2);
362 }
363
364 float angle_v2v2v2(float *v1, float *v2, float *v3)
365 {
366         float vec1[2], vec2[2];
367
368         vec1[0] = v2[0]-v1[0];
369         vec1[1] = v2[1]-v1[1];
370         
371         vec2[0] = v2[0]-v3[0];
372         vec2[1] = v2[1]-v3[1];
373         
374         normalize_v2(vec1);
375         normalize_v2(vec2);
376
377         return angle_normalized_v2v2(vec1, vec2);
378 }
379
380 /* Return the shortest angle in radians between the 2 vectors */
381 float angle_v2v2(float *v1, float *v2)
382 {
383         float vec1[3], vec2[3];
384
385         copy_v3_v3(vec1, v1);
386         copy_v3_v3(vec2, v2);
387         normalize_v3(vec1);
388         normalize_v3(vec2);
389
390         return angle_normalized_v3v3(vec1, vec2);
391 }
392
393 float angle_normalized_v3v3(float *v1, float *v2)
394 {
395         /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
396         if (dot_v3v3(v1, v2) < 0.0f) {
397                 float vec[3];
398                 
399                 vec[0]= -v2[0];
400                 vec[1]= -v2[1];
401                 vec[2]= -v2[2];
402                 
403                 return (float)M_PI - 2.0f*(float)saasin(len_v3v3(vec, v1)/2.0f);
404         }
405         else
406                 return 2.0f*(float)saasin(len_v3v3(v2, v1)/2.0f);
407 }
408
409 float angle_normalized_v2v2(float *v1, float *v2)
410 {
411         /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
412         if (dot_v2v2(v1, v2) < 0.0f) {
413                 float vec[2];
414                 
415                 vec[0]= -v2[0];
416                 vec[1]= -v2[1];
417                 
418                 return (float)M_PI - 2.0f*saasin(len_v2v2(vec, v1)/2.0f);
419         }
420         else
421                 return 2.0f*(float)saasin(len_v2v2(v2, v1)/2.0f);
422 }
423
424 /********************************* Geometry **********************************/
425
426 /* Project v1 on v2 */
427 void project_v3_v3v3(float *c, float *v1, float *v2)
428 {
429         float mul;
430         mul = dot_v3v3(v1, v2) / dot_v3v3(v2, v2);
431         
432         c[0] = mul * v2[0];
433         c[1] = mul * v2[1];
434         c[2] = mul * v2[2];
435 }
436
437 /* Returns a vector bisecting the angle at v2 formed by v1, v2 and v3 */
438 void bisect_v3_v3v3v3(float *out, float *v1, float *v2, float *v3)
439 {
440         float d_12[3], d_23[3];
441         sub_v3_v3v3(d_12, v2, v1);
442         sub_v3_v3v3(d_23, v3, v2);
443         normalize_v3(d_12);
444         normalize_v3(d_23);
445         add_v3_v3v3(out, d_12, d_23);
446         normalize_v3(out);
447 }
448
449 /* Returns a reflection vector from a vector and a normal vector
450 reflect = vec - ((2 * DotVecs(vec, mirror)) * mirror)
451 */
452 void reflect_v3_v3v3(float *out, float *v1, float *v2)
453 {
454         float vec[3], normal[3];
455         float reflect[3] = {0.0f, 0.0f, 0.0f};
456         float dot2;
457
458         copy_v3_v3(vec, v1);
459         copy_v3_v3(normal, v2);
460
461         normalize_v3(normal);
462
463         dot2 = 2 * dot_v3v3(vec, normal);
464
465         reflect[0] = vec[0] - (dot2 * normal[0]);
466         reflect[1] = vec[1] - (dot2 * normal[1]);
467         reflect[2] = vec[2] - (dot2 * normal[2]);
468
469         copy_v3_v3(out, reflect);
470 }
471
472 void ortho_basis_v3v3_v3(float *v1, float *v2, float *v)
473 {
474         const float f = (float)sqrt(v[0]*v[0] + v[1]*v[1]);
475
476         if (f < 1e-35f) {
477                 // degenerate case
478                 v1[0] = (v[2] < 0.0f) ? -1.0f : 1.0f;
479                 v1[1] = v1[2] = v2[0] = v2[2] = 0.0f;
480                 v2[1] = 1.0f;
481         }
482         else  {
483                 const float d= 1.0f/f;
484
485                 v1[0] =  v[1]*d;
486                 v1[1] = -v[0]*d;
487                 v1[2] = 0.0f;
488                 v2[0] = -v[2]*v1[1];
489                 v2[1] = v[2]*v1[0];
490                 v2[2] = v[0]*v1[1] - v[1]*v1[0];
491         }
492 }
493
494 /*********************************** Other ***********************************/
495
496 void print_v2(char *str, float v[2])
497 {
498         printf("%s: %.3f %.3f\n", str, v[0], v[1]);
499 }
500
501 void print_v3(char *str, float v[3])
502 {
503         printf("%s: %.3f %.3f %.3f\n", str, v[0], v[1], v[2]);
504 }
505
506 void print_v4(char *str, float v[4])
507 {
508         printf("%s: %.3f %.3f %.3f %.3f\n", str, v[0], v[1], v[2], v[3]);
509 }
510
511 void normal_short_to_float_v3(float *out, short *in)
512 {
513         out[0] = in[0]*(1.0f/32767.0f);
514         out[1] = in[1]*(1.0f/32767.0f);
515         out[2] = in[2]*(1.0f/32767.0f);
516 }
517
518 void normal_float_to_short_v3(short *out, float *in)
519 {
520         out[0] = (short)(in[0]*32767.0f);
521         out[1] = (short)(in[1]*32767.0f);
522         out[2] = (short)(in[2]*32767.0f);
523 }
524
525 void minmax_v3_v3v3(float *min, float *max, float *vec)
526 {
527         if(min[0]>vec[0]) min[0]= vec[0];
528         if(min[1]>vec[1]) min[1]= vec[1];
529         if(min[2]>vec[2]) min[2]= vec[2];
530
531         if(max[0]<vec[0]) max[0]= vec[0];
532         if(max[1]<vec[1]) max[1]= vec[1];
533         if(max[2]<vec[2]) max[2]= vec[2];
534 }
535