Sculpt:
[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 //******************************* Interpolation *******************************/
36
37 void interp_v2_v2v2(float *target, float *a, float *b, float t)
38 {
39         float s = 1.0f-t;
40
41         target[0]= s*a[0] + t*b[0];
42         target[1]= s*a[1] + t*b[1];
43 }
44
45 /* weight 3 2D vectors,
46  * 'w' must be unit length but is not a vector, just 3 weights */
47 void interp_v2_v2v2v2(float p[2], float v1[2], float v2[2], float v3[2], float w[3])
48 {
49         p[0] = v1[0]*w[0] + v2[0]*w[1] + v3[0]*w[2];
50         p[1] = v1[1]*w[0] + v2[1]*w[1] + v3[1]*w[2];
51 }
52
53 void interp_v3_v3v3(float *target, float *a, float *b, float t)
54 {
55         float s = 1.0f-t;
56
57         target[0]= s*a[0] + t*b[0];
58         target[1]= s*a[1] + t*b[1];
59         target[2]= s*a[2] + t*b[2];
60 }
61
62 /* weight 3 vectors,
63  * 'w' must be unit length but is not a vector, just 3 weights */
64 void interp_v3_v3v3v3(float p[3], float v1[3], float v2[3], float v3[3], float w[3])
65 {
66         p[0] = v1[0]*w[0] + v2[0]*w[1] + v3[0]*w[2];
67         p[1] = v1[1]*w[0] + v2[1]*w[1] + v3[1]*w[2];
68         p[2] = v1[2]*w[0] + v2[2]*w[1] + v3[2]*w[2];
69 }
70
71 /* weight 3 vectors,
72  * 'w' must be unit length but is not a vector, just 3 weights */
73 void interp_v3_v3v3v3v3(float p[3], float v1[3], float v2[3], float v3[3], float v4[3], float w[4])
74 {
75         p[0] = v1[0]*w[0] + v2[0]*w[1] + v3[0]*w[2] + v4[0]*w[3];
76         p[1] = v1[1]*w[0] + v2[1]*w[1] + v3[1]*w[2] + v4[1]*w[3];
77         p[2] = v1[2]*w[0] + v2[2]*w[1] + v3[2]*w[2] + v4[2]*w[3];
78 }
79
80 void mid_v3_v3v3(float *v, float *v1, float *v2)
81 {
82         v[0]= 0.5f*(v1[0] + v2[0]);
83         v[1]= 0.5f*(v1[1] + v2[1]);
84         v[2]= 0.5f*(v1[2] + v2[2]);
85 }
86
87 /********************************* Comparison ********************************/
88
89 int is_zero_v3(float *v)
90 {
91         return (v[0] == 0 && v[1] == 0 && v[2] == 0);
92 }
93
94 int equals_v3v3(float *v1, float *v2)
95 {
96         return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
97 }
98
99 int compare_v3v3(float *v1, float *v2, float limit)
100 {
101         if(fabs(v1[0]-v2[0])<limit)
102                 if(fabs(v1[1]-v2[1])<limit)
103                         if(fabs(v1[2]-v2[2])<limit)
104                                 return 1;
105
106         return 0;
107 }
108
109 int compare_len_v3v3(float *v1, float *v2, float limit)
110 {
111     float x,y,z;
112
113         x=v1[0]-v2[0];
114         y=v1[1]-v2[1];
115         z=v1[2]-v2[2];
116
117         return ((x*x + y*y + z*z) < (limit*limit));
118 }
119
120 int compare_v4v4(float *v1, float *v2, float limit)
121 {
122         if(fabs(v1[0]-v2[0])<limit)
123                 if(fabs(v1[1]-v2[1])<limit)
124                         if(fabs(v1[2]-v2[2])<limit)
125                                 if(fabs(v1[3]-v2[3])<limit)
126                                         return 1;
127
128         return 0;
129 }
130
131 /********************************** Angles ***********************************/
132
133 /* Return the angle in radians between vecs 1-2 and 2-3 in radians
134    If v1 is a shoulder, v2 is the elbow and v3 is the hand,
135    this would return the angle at the elbow */
136 float angle_v3v3v3(float *v1, float *v2, float *v3)
137 {
138         float vec1[3], vec2[3];
139
140         sub_v3_v3v3(vec1, v2, v1);
141         sub_v3_v3v3(vec2, v2, v3);
142         normalize_v3(vec1);
143         normalize_v3(vec2);
144
145         return angle_normalized_v3v3(vec1, vec2);
146 }
147
148 float angle_v2v2v2(float *v1, float *v2, float *v3)
149 {
150         float vec1[2], vec2[2];
151
152         vec1[0] = v2[0]-v1[0];
153         vec1[1] = v2[1]-v1[1];
154         
155         vec2[0] = v2[0]-v3[0];
156         vec2[1] = v2[1]-v3[1];
157         
158         normalize_v2(vec1);
159         normalize_v2(vec2);
160
161         return angle_normalized_v2v2(vec1, vec2);
162 }
163
164 /* Return the shortest angle in radians between the 2 vectors */
165 float angle_v2v2(float *v1, float *v2)
166 {
167         float vec1[3], vec2[3];
168
169         copy_v3_v3(vec1, v1);
170         copy_v3_v3(vec2, v2);
171         normalize_v3(vec1);
172         normalize_v3(vec2);
173
174         return angle_normalized_v3v3(vec1, vec2);
175 }
176
177 float angle_normalized_v3v3(float *v1, float *v2)
178 {
179         /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
180         if (dot_v3v3(v1, v2) < 0.0f) {
181                 float vec[3];
182                 
183                 vec[0]= -v2[0];
184                 vec[1]= -v2[1];
185                 vec[2]= -v2[2];
186                 
187                 return (float)M_PI - 2.0f*(float)saasin(len_v3v3(vec, v1)/2.0f);
188         }
189         else
190                 return 2.0f*(float)saasin(len_v3v3(v2, v1)/2.0f);
191 }
192
193 float angle_normalized_v2v2(float *v1, float *v2)
194 {
195         /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
196         if (dot_v2v2(v1, v2) < 0.0f) {
197                 float vec[2];
198                 
199                 vec[0]= -v2[0];
200                 vec[1]= -v2[1];
201                 
202                 return (float)M_PI - 2.0f*saasin(len_v2v2(vec, v1)/2.0f);
203         }
204         else
205                 return 2.0f*(float)saasin(len_v2v2(v2, v1)/2.0f);
206 }
207
208 /********************************* Geometry **********************************/
209
210 /* Project v1 on v2 */
211 void project_v3_v3v3(float *c, float *v1, float *v2)
212 {
213         float mul;
214         mul = dot_v3v3(v1, v2) / dot_v3v3(v2, v2);
215         
216         c[0] = mul * v2[0];
217         c[1] = mul * v2[1];
218         c[2] = mul * v2[2];
219 }
220
221 /* Returns a vector bisecting the angle at v2 formed by v1, v2 and v3 */
222 void bisect_v3_v3v3v3(float *out, float *v1, float *v2, float *v3)
223 {
224         float d_12[3], d_23[3];
225         sub_v3_v3v3(d_12, v2, v1);
226         sub_v3_v3v3(d_23, v3, v2);
227         normalize_v3(d_12);
228         normalize_v3(d_23);
229         add_v3_v3v3(out, d_12, d_23);
230         normalize_v3(out);
231 }
232
233 /* Returns a reflection vector from a vector and a normal vector
234 reflect = vec - ((2 * DotVecs(vec, mirror)) * mirror)
235 */
236 void reflect_v3_v3v3(float *out, float *v1, float *v2)
237 {
238         float vec[3], normal[3];
239         float reflect[3] = {0.0f, 0.0f, 0.0f};
240         float dot2;
241
242         copy_v3_v3(vec, v1);
243         copy_v3_v3(normal, v2);
244
245         normalize_v3(normal);
246
247         dot2 = 2 * dot_v3v3(vec, normal);
248
249         reflect[0] = vec[0] - (dot2 * normal[0]);
250         reflect[1] = vec[1] - (dot2 * normal[1]);
251         reflect[2] = vec[2] - (dot2 * normal[2]);
252
253         copy_v3_v3(out, reflect);
254 }
255
256 void ortho_basis_v3v3_v3(float *v1, float *v2, float *v)
257 {
258         const float f = (float)sqrt(v[0]*v[0] + v[1]*v[1]);
259
260         if (f < 1e-35f) {
261                 // degenerate case
262                 v1[0] = (v[2] < 0.0f) ? -1.0f : 1.0f;
263                 v1[1] = v1[2] = v2[0] = v2[2] = 0.0f;
264                 v2[1] = 1.0f;
265         }
266         else  {
267                 const float d= 1.0f/f;
268
269                 v1[0] =  v[1]*d;
270                 v1[1] = -v[0]*d;
271                 v1[2] = 0.0f;
272                 v2[0] = -v[2]*v1[1];
273                 v2[1] = v[2]*v1[0];
274                 v2[2] = v[0]*v1[1] - v[1]*v1[0];
275         }
276 }
277
278 /*********************************** Other ***********************************/
279
280 void print_v2(char *str, float v[2])
281 {
282         printf("%s: %.3f %.3f\n", str, v[0], v[1]);
283 }
284
285 void print_v3(char *str, float v[3])
286 {
287         printf("%s: %.3f %.3f %.3f\n", str, v[0], v[1], v[2]);
288 }
289
290 void print_v4(char *str, float v[4])
291 {
292         printf("%s: %.3f %.3f %.3f %.3f\n", str, v[0], v[1], v[2], v[3]);
293 }
294
295 void minmax_v3_v3v3(float *min, float *max, float *vec)
296 {
297         if(min[0]>vec[0]) min[0]= vec[0];
298         if(min[1]>vec[1]) min[1]= vec[1];
299         if(min[2]>vec[2]) min[2]= vec[2];
300
301         if(max[0]<vec[0]) max[0]= vec[0];
302         if(max[1]<vec[1]) max[1]= vec[1];
303         if(max[2]<vec[2]) max[2]= vec[2];
304 }
305