3e464327b2ec25634ccb50914505632459c9da00
[blender.git] / source / blender / blenlib / intern / math_vector.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.c
27  *  \ingroup bli
28  */
29
30 #include "BLI_math.h"
31
32 #include "BLI_strict_flags.h"
33
34 //******************************* Interpolation *******************************/
35
36 void interp_v2_v2v2(float target[2], const float a[2], const float b[2], const float t)
37 {
38         const float s = 1.0f - t;
39
40         target[0] = s * a[0] + t * b[0];
41         target[1] = s * a[1] + t * b[1];
42 }
43
44 /* weight 3 2D vectors,
45  * 'w' must be unit length but is not a vector, just 3 weights */
46 void interp_v2_v2v2v2(float p[2], const float v1[2], const float v2[2], const float v3[2], const float w[3])
47 {
48         p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2];
49         p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2];
50 }
51
52 void interp_v3_v3v3(float target[3], const float a[3], const float b[3], const float t)
53 {
54         const float s = 1.0f - t;
55
56         target[0] = s * a[0] + t * b[0];
57         target[1] = s * a[1] + t * b[1];
58         target[2] = s * a[2] + t * b[2];
59 }
60
61 void interp_v4_v4v4(float target[4], const float a[4], const float b[4], const float t)
62 {
63         const float s = 1.0f - t;
64
65         target[0] = s * a[0] + t * b[0];
66         target[1] = s * a[1] + t * b[1];
67         target[2] = s * a[2] + t * b[2];
68         target[3] = s * a[3] + t * b[3];
69 }
70
71 /**
72  * slerp, treat vectors as spherical coordinates
73  * \see #interp_qt_qtqt
74  *
75  * \return success
76  */
77 bool interp_v3_v3v3_slerp(float target[3], const float a[3], const float b[3], const float t)
78 {
79         float cosom, w[2];
80
81         BLI_ASSERT_UNIT_V3(a);
82         BLI_ASSERT_UNIT_V3(b);
83
84         cosom = dot_v3v3(a, b);
85
86         /* direct opposites */
87         if (UNLIKELY(cosom < (-1.0f + FLT_EPSILON))) {
88                 return false;
89         }
90
91         interp_dot_slerp(t, cosom, w);
92
93         target[0] = w[0] * a[0] + w[1] * b[0];
94         target[1] = w[0] * a[1] + w[1] * b[1];
95         target[2] = w[0] * a[2] + w[1] * b[2];
96
97         return true;
98 }
99 bool interp_v2_v2v2_slerp(float target[2], const float a[2], const float b[2], const float t)
100 {
101         float cosom, w[2];
102
103         BLI_ASSERT_UNIT_V2(a);
104         BLI_ASSERT_UNIT_V2(b);
105
106         cosom = dot_v2v2(a, b);
107
108         /* direct opposites */
109         if (UNLIKELY(cosom < (1.0f + FLT_EPSILON))) {
110                 return false;
111         }
112
113         interp_dot_slerp(t, cosom, w);
114
115         target[0] = w[0] * a[0] + w[1] * b[0];
116         target[1] = w[0] * a[1] + w[1] * b[1];
117
118         return true;
119 }
120
121 /**
122  * Same as #interp_v3_v3v3_slerp but uses fallback values for opposite vectors.
123  */
124 void interp_v3_v3v3_slerp_safe(float target[3], const float a[3], const float b[3], const float t)
125 {
126         if (UNLIKELY(!interp_v3_v3v3_slerp(target, a, b, t))) {
127                 /* axis are aligned so any otho vector is acceptable */
128                 float ab_ortho[3];
129                 ortho_v3_v3(ab_ortho, a);
130                 normalize_v3(ab_ortho);
131                 if (t < 0.5f) {
132                         if (UNLIKELY(!interp_v3_v3v3_slerp(target, a, ab_ortho, t * 2.0f))) {
133                                 BLI_assert(0);
134                                 copy_v3_v3(target, a);
135                         }
136                 }
137                 else {
138                         if (UNLIKELY(!interp_v3_v3v3_slerp(target, ab_ortho, b, (t - 0.5f) * 2.0f))) {
139                                 BLI_assert(0);
140                                 copy_v3_v3(target, b);
141                         }
142                 }
143         }
144 }
145 void interp_v2_v2v2_slerp_safe(float target[2], const float a[2], const float b[2], const float t)
146 {
147         if (UNLIKELY(!interp_v2_v2v2_slerp(target, a, b, t))) {
148                 /* axis are aligned so any otho vector is acceptable */
149                 float ab_ortho[2];
150                 ortho_v2_v2(ab_ortho, a);
151                 // normalize_v2(ab_ortho);
152                 if (t < 0.5f) {
153                         if (UNLIKELY(!interp_v2_v2v2_slerp(target, a, ab_ortho, t * 2.0f))) {
154                                 BLI_assert(0);
155                                 copy_v2_v2(target, a);
156                         }
157                 }
158                 else {
159                         if (UNLIKELY(!interp_v2_v2v2_slerp(target, ab_ortho, b, (t - 0.5f) * 2.0f))) {
160                                 BLI_assert(0);
161                                 copy_v2_v2(target, b);
162                         }
163                 }
164         }
165 }
166
167 /* weight 3 vectors,
168  * 'w' must be unit length but is not a vector, just 3 weights */
169 void interp_v3_v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3])
170 {
171         p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2];
172         p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2];
173         p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2];
174 }
175
176 /* weight 3 vectors,
177  * 'w' must be unit length but is not a vector, just 4 weights */
178 void interp_v3_v3v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3], const float w[4])
179 {
180         p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3];
181         p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3];
182         p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3];
183 }
184
185 void interp_v4_v4v4v4(float p[4], const float v1[4], const float v2[4], const float v3[4], const float w[3])
186 {
187         p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2];
188         p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2];
189         p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2];
190         p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2];
191 }
192
193 void interp_v4_v4v4v4v4(float p[4], const float v1[4], const float v2[4], const float v3[4], const float v4[4], const float w[4])
194 {
195         p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3];
196         p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3];
197         p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3];
198         p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2] + v4[3] * w[3];
199 }
200
201 void interp_v3_v3v3v3_uv(float p[3], const float v1[3], const float v2[3], const float v3[3], const float uv[2])
202 {
203         p[0] = v1[0] + ((v2[0] - v1[0]) * uv[0]) + ((v3[0] - v1[0]) * uv[1]);
204         p[1] = v1[1] + ((v2[1] - v1[1]) * uv[0]) + ((v3[1] - v1[1]) * uv[1]);
205         p[2] = v1[2] + ((v2[2] - v1[2]) * uv[0]) + ((v3[2] - v1[2]) * uv[1]);
206 }
207
208 void interp_v3_v3v3_uchar(char unsigned target[3], const unsigned char a[3], const unsigned char b[3], const float t)
209 {
210         const float s = 1.0f - t;
211
212         target[0] = (char)floorf(s * a[0] + t * b[0]);
213         target[1] = (char)floorf(s * a[1] + t * b[1]);
214         target[2] = (char)floorf(s * a[2] + t * b[2]);
215 }
216 void interp_v3_v3v3_char(char target[3], const char a[3], const char b[3], const float t)
217 {
218         interp_v3_v3v3_uchar((unsigned char *)target, (const unsigned char *)a, (const unsigned char *)b, t);
219 }
220
221 void interp_v4_v4v4_uchar(char unsigned target[4], const unsigned char a[4], const unsigned char b[4], const float t)
222 {
223         const float s = 1.0f - t;
224
225         target[0] = (char)floorf(s * a[0] + t * b[0]);
226         target[1] = (char)floorf(s * a[1] + t * b[1]);
227         target[2] = (char)floorf(s * a[2] + t * b[2]);
228         target[3] = (char)floorf(s * a[3] + t * b[3]);
229 }
230 void interp_v4_v4v4_char(char target[4], const char a[4], const char b[4], const float t)
231 {
232         interp_v4_v4v4_uchar((unsigned char *)target, (const unsigned char *)a, (const unsigned char *)b, t);
233 }
234
235 void mid_v3_v3v3(float v[3], const float v1[3], const float v2[3])
236 {
237         v[0] = 0.5f * (v1[0] + v2[0]);
238         v[1] = 0.5f * (v1[1] + v2[1]);
239         v[2] = 0.5f * (v1[2] + v2[2]);
240 }
241
242 void mid_v2_v2v2(float v[2], const float v1[2], const float v2[2])
243 {
244         v[0] = 0.5f * (v1[0] + v2[0]);
245         v[1] = 0.5f * (v1[1] + v2[1]);
246 }
247
248 void mid_v3_v3v3v3(float v[3], const float v1[3], const float v2[3], const float v3[3])
249 {
250         v[0] = (v1[0] + v2[0] + v3[0]) / 3.0f;
251         v[1] = (v1[1] + v2[1] + v3[1]) / 3.0f;
252         v[2] = (v1[2] + v2[2] + v3[2]) / 3.0f;
253 }
254
255 void mid_v3_v3v3v3v3(float v[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
256 {
257         v[0] = (v1[0] + v2[0] + v3[0] + v4[0]) / 4.0f;
258         v[1] = (v1[1] + v2[1] + v3[1] + v4[1]) / 4.0f;
259         v[2] = (v1[2] + v2[2] + v3[2] + v4[2]) / 4.0f;
260 }
261
262 /**
263  * Specialized function for calculating normals.
264  * fastpath for:
265  *
266  * \code{.c}
267  * add_v3_v3v3(r, a, b);
268  * normalize_v3(r)
269  * mul_v3_fl(r, angle_normalized_v3v3(a, b) / M_PI_2);
270  * \endcode
271  *
272  * We can use the length of (a + b) to calculate the angle.
273  */
274 void mid_v3_v3v3_angle_weighted(float r[3], const float a[3], const float b[3])
275 {
276         /* trick, we want the middle of 2 normals as well as the angle between them
277          * avoid multiple calculations by */
278         float angle;
279
280         /* double check they are normalized */
281         BLI_ASSERT_UNIT_V3(a);
282         BLI_ASSERT_UNIT_V3(b);
283
284         add_v3_v3v3(r, a, b);
285         angle = ((float)(1.0 / (M_PI / 2.0)) *
286                  /* normally we would only multiply by 2,
287                   * but instead of an angle make this 0-1 factor */
288                  2.0f) *
289                 acosf(normalize_v3(r) / 2.0f);
290         mul_v3_fl(r, angle);
291 }
292 /**
293  * Same as mid_v3_v3v3_angle_weighted
294  * but \a r is assumed to be accumulated normals, divided by their total.
295  */
296 void mid_v3_angle_weighted(float r[3])
297 {
298         /* trick, we want the middle of 2 normals as well as the angle between them
299          * avoid multiple calculations by */
300         float angle;
301
302         /* double check they are normalized */
303         BLI_assert(len_squared_v3(r) <= 1.0f + FLT_EPSILON);
304
305         angle = ((float)(1.0 / (M_PI / 2.0)) *
306                  /* normally we would only multiply by 2,
307                   * but instead of an angle make this 0-1 factor */
308                  2.0f) *
309                 acosf(normalize_v3(r));
310         mul_v3_fl(r, angle);
311 }
312
313 /**
314  * Equivalent to:
315  * interp_v3_v3v3(v, v1, v2, -1.0f);
316  */
317
318 void flip_v4_v4v4(float v[4], const float v1[4], const float v2[4])
319 {
320         v[0] = v1[0] + (v1[0] - v2[0]);
321         v[1] = v1[1] + (v1[1] - v2[1]);
322         v[2] = v1[2] + (v1[2] - v2[2]);
323         v[3] = v1[3] + (v1[3] - v2[3]);
324 }
325
326 void flip_v3_v3v3(float v[3], const float v1[3], const float v2[3])
327 {
328         v[0] = v1[0] + (v1[0] - v2[0]);
329         v[1] = v1[1] + (v1[1] - v2[1]);
330         v[2] = v1[2] + (v1[2] - v2[2]);
331 }
332
333 void flip_v2_v2v2(float v[2], const float v1[2], const float v2[2])
334 {
335         v[0] = v1[0] + (v1[0] - v2[0]);
336         v[1] = v1[1] + (v1[1] - v2[1]);
337 }
338
339 /********************************** Angles ***********************************/
340
341 /* Return the angle in radians between vecs 1-2 and 2-3 in radians
342  * If v1 is a shoulder, v2 is the elbow and v3 is the hand,
343  * this would return the angle at the elbow.
344  *
345  * note that when v1/v2/v3 represent 3 points along a straight line
346  * that the angle returned will be pi (180deg), rather then 0.0
347  */
348 float angle_v3v3v3(const float v1[3], const float v2[3], const float v3[3])
349 {
350         float vec1[3], vec2[3];
351
352         sub_v3_v3v3(vec1, v2, v1);
353         sub_v3_v3v3(vec2, v2, v3);
354         normalize_v3(vec1);
355         normalize_v3(vec2);
356
357         return angle_normalized_v3v3(vec1, vec2);
358 }
359
360 /* Quicker than full angle computation */
361 float cos_v3v3v3(const float p1[3], const float p2[3], const float p3[3])
362 {
363         float vec1[3], vec2[3];
364
365         sub_v3_v3v3(vec1, p2, p1);
366         sub_v3_v3v3(vec2, p2, p3);
367         normalize_v3(vec1);
368         normalize_v3(vec2);
369
370         return dot_v3v3(vec1, vec2);
371 }
372
373 /* Return the shortest angle in radians between the 2 vectors */
374 float angle_v3v3(const float v1[3], const float v2[3])
375 {
376         float vec1[3], vec2[3];
377
378         normalize_v3_v3(vec1, v1);
379         normalize_v3_v3(vec2, v2);
380
381         return angle_normalized_v3v3(vec1, vec2);
382 }
383
384 float angle_v2v2v2(const float v1[2], const float v2[2], const float v3[2])
385 {
386         float vec1[2], vec2[2];
387
388         vec1[0] = v2[0] - v1[0];
389         vec1[1] = v2[1] - v1[1];
390
391         vec2[0] = v2[0] - v3[0];
392         vec2[1] = v2[1] - v3[1];
393
394         normalize_v2(vec1);
395         normalize_v2(vec2);
396
397         return angle_normalized_v2v2(vec1, vec2);
398 }
399
400 /* Quicker than full angle computation */
401 float cos_v2v2v2(const float p1[2], const float p2[2], const float p3[2])
402 {
403         float vec1[2], vec2[2];
404
405         sub_v2_v2v2(vec1, p2, p1);
406         sub_v2_v2v2(vec2, p2, p3);
407         normalize_v2(vec1);
408         normalize_v2(vec2);
409
410         return dot_v2v2(vec1, vec2);
411 }
412
413 /* Return the shortest angle in radians between the 2 vectors */
414 float angle_v2v2(const float v1[2], const float v2[2])
415 {
416         float vec1[2], vec2[2];
417
418         vec1[0] = v1[0];
419         vec1[1] = v1[1];
420
421         vec2[0] = v2[0];
422         vec2[1] = v2[1];
423
424         normalize_v2(vec1);
425         normalize_v2(vec2);
426
427         return angle_normalized_v2v2(vec1, vec2);
428 }
429
430 float angle_signed_v2v2(const float v1[2], const float v2[2])
431 {
432         const float perp_dot = (v1[1] * v2[0]) - (v1[0] * v2[1]);
433         return atan2f(perp_dot, dot_v2v2(v1, v2));
434 }
435
436 float angle_normalized_v3v3(const float v1[3], const float v2[3])
437 {
438         /* double check they are normalized */
439         BLI_ASSERT_UNIT_V3(v1);
440         BLI_ASSERT_UNIT_V3(v2);
441
442         /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
443         if (dot_v3v3(v1, v2) >= 0.0f) {
444                 return 2.0f * saasin(len_v3v3(v1, v2) / 2.0f);
445         }
446         else {
447                 float v2_n[3];
448                 negate_v3_v3(v2_n, v2);
449                 return (float)M_PI - 2.0f * saasin(len_v3v3(v1, v2_n) / 2.0f);
450         }
451 }
452
453 float angle_normalized_v2v2(const float v1[2], const float v2[2])
454 {
455         /* double check they are normalized */
456         BLI_ASSERT_UNIT_V2(v1);
457         BLI_ASSERT_UNIT_V2(v2);
458
459         /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
460         if (dot_v2v2(v1, v2) >= 0.0f) {
461                 return 2.0f * saasin(len_v2v2(v1, v2) / 2.0f);
462         }
463         else {
464                 float v2_n[2];
465                 negate_v2_v2(v2_n, v2);
466                 return (float)M_PI - 2.0f * saasin(len_v2v2(v1, v2_n) / 2.0f);
467         }
468 }
469
470 /**
471  * angle between 2 vectors defined by 3 coords, about an axis. */
472 float angle_on_axis_v3v3v3_v3(const float v1[3], const float v2[3], const float v3[3], const float axis[3])
473 {
474         float v1_proj[3], v2_proj[3], tproj[3];
475
476         sub_v3_v3v3(v1_proj, v1, v2);
477         sub_v3_v3v3(v2_proj, v3, v2);
478
479         /* project the vectors onto the axis */
480         project_v3_v3v3(tproj, v1_proj, axis);
481         sub_v3_v3(v1_proj, tproj);
482
483         project_v3_v3v3(tproj, v2_proj, axis);
484         sub_v3_v3(v2_proj, tproj);
485
486         return angle_v3v3(v1_proj, v2_proj);
487 }
488
489 float angle_signed_on_axis_v3v3v3_v3(const float v1[3], const float v2[3], const float v3[3], const float axis[3])
490 {
491         float v1_proj[3], v2_proj[3], tproj[3];
492         float angle;
493
494         sub_v3_v3v3(v1_proj, v1, v2);
495         sub_v3_v3v3(v2_proj, v3, v2);
496
497         /* project the vectors onto the axis */
498         project_v3_v3v3(tproj, v1_proj, axis);
499         sub_v3_v3(v1_proj, tproj);
500
501         project_v3_v3v3(tproj, v2_proj, axis);
502         sub_v3_v3(v2_proj, tproj);
503
504         angle = angle_v3v3(v1_proj, v2_proj);
505
506         /* calculate the sign (reuse 'tproj') */
507         cross_v3_v3v3(tproj, v2_proj, v1_proj);
508         if (dot_v3v3(tproj, axis) < 0.0f) {
509                 angle = ((float)(M_PI * 2.0)) - angle;
510         }
511
512         return angle;
513 }
514
515 void angle_tri_v3(float angles[3], const float v1[3], const float v2[3], const float v3[3])
516 {
517         float ed1[3], ed2[3], ed3[3];
518
519         sub_v3_v3v3(ed1, v3, v1);
520         sub_v3_v3v3(ed2, v1, v2);
521         sub_v3_v3v3(ed3, v2, v3);
522
523         normalize_v3(ed1);
524         normalize_v3(ed2);
525         normalize_v3(ed3);
526
527         angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2);
528         angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3);
529         // face_angles[2] = M_PI - angle_normalized_v3v3(ed3, ed1);
530         angles[2] = (float)M_PI - (angles[0] + angles[1]);
531 }
532
533 void angle_quad_v3(float angles[4], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
534 {
535         float ed1[3], ed2[3], ed3[3], ed4[3];
536
537         sub_v3_v3v3(ed1, v4, v1);
538         sub_v3_v3v3(ed2, v1, v2);
539         sub_v3_v3v3(ed3, v2, v3);
540         sub_v3_v3v3(ed4, v3, v4);
541
542         normalize_v3(ed1);
543         normalize_v3(ed2);
544         normalize_v3(ed3);
545         normalize_v3(ed4);
546
547         angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2);
548         angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3);
549         angles[2] = (float)M_PI - angle_normalized_v3v3(ed3, ed4);
550         angles[3] = (float)M_PI - angle_normalized_v3v3(ed4, ed1);
551 }
552
553 void angle_poly_v3(float *angles, const float *verts[3], int len)
554 {
555         int i;
556         float vec[3][3];
557
558         sub_v3_v3v3(vec[2], verts[len - 1], verts[0]);
559         normalize_v3(vec[2]);
560         for (i = 0; i < len; i++) {
561                 sub_v3_v3v3(vec[i % 3], verts[i % len], verts[(i + 1) % len]);
562                 normalize_v3(vec[i % 3]);
563                 angles[i] = (float)M_PI - angle_normalized_v3v3(vec[(i + 2) % 3], vec[i % 3]);
564         }
565 }
566
567 /********************************* Geometry **********************************/
568
569 /* Project v1 on v2 */
570 void project_v2_v2v2(float c[2], const float v1[2], const float v2[2])
571 {
572         const float mul = dot_v2v2(v1, v2) / dot_v2v2(v2, v2);
573
574         c[0] = mul * v2[0];
575         c[1] = mul * v2[1];
576 }
577
578 /* Project v1 on v2 */
579 void project_v3_v3v3(float c[3], const float v1[3], const float v2[3])
580 {
581         const float mul = dot_v3v3(v1, v2) / dot_v3v3(v2, v2);
582
583         c[0] = mul * v2[0];
584         c[1] = mul * v2[1];
585         c[2] = mul * v2[2];
586 }
587
588 /**
589  * In this case plane is a 3D vector only (no 4th component).
590  *
591  * Projecting will make \a c a copy of \a v orthogonal to \a v_plane.
592  *
593  * \note If \a v is exactly perpendicular to \a v_plane, \a c will just be a copy of \a v.
594  *
595  * \note This function is a convenience to call:
596  * \code{.c}
597  * project_v3_v3v3(c, v, v_plane);
598  * sub_v3_v3v3(c, v, c);
599  * \endcode
600  */
601 void project_plane_v3_v3v3(float c[3], const float v[3], const float v_plane[3])
602 {
603         const float mul = dot_v3v3(v, v_plane) / dot_v3v3(v_plane, v_plane);
604
605         c[0] = v[0] - (mul * v_plane[0]);
606         c[1] = v[1] - (mul * v_plane[1]);
607         c[2] = v[2] - (mul * v_plane[2]);
608 }
609
610 void project_plane_v2_v2v2(float c[2], const float v[2], const float v_plane[2])
611 {
612         const float mul = dot_v2v2(v, v_plane) / dot_v2v2(v_plane, v_plane);
613
614         c[0] = v[0] - (mul * v_plane[0]);
615         c[1] = v[1] - (mul * v_plane[1]);
616 }
617
618 /* project a vector on a plane defined by normal and a plane point p */
619 void project_v3_plane(float v[3], const float n[3], const float p[3])
620 {
621         float vector[3];
622         float mul;
623
624         sub_v3_v3v3(vector, v, p);
625         mul = dot_v3v3(vector, n) / len_squared_v3(n);
626
627         mul_v3_v3fl(vector, n, mul);
628
629         sub_v3_v3(v, vector);
630 }
631
632 /* Returns a vector bisecting the angle at v2 formed by v1, v2 and v3 */
633 void bisect_v3_v3v3v3(float out[3], const float v1[3], const float v2[3], const float v3[3])
634 {
635         float d_12[3], d_23[3];
636         sub_v3_v3v3(d_12, v2, v1);
637         sub_v3_v3v3(d_23, v3, v2);
638         normalize_v3(d_12);
639         normalize_v3(d_23);
640         add_v3_v3v3(out, d_12, d_23);
641         normalize_v3(out);
642 }
643
644 /**
645  * Returns a reflection vector from a vector and a normal vector
646  * reflect = vec - ((2 * DotVecs(vec, mirror)) * mirror)
647  */
648 void reflect_v3_v3v3(float out[3], const float vec[3], const float normal[3])
649 {
650         const float dot2 = 2.0f * dot_v3v3(vec, normal);
651
652         BLI_ASSERT_UNIT_V3(normal);
653
654         out[0] = vec[0] - (dot2 * normal[0]);
655         out[1] = vec[1] - (dot2 * normal[1]);
656         out[2] = vec[2] - (dot2 * normal[2]);
657 }
658
659 /**
660  * Takes a vector and computes 2 orthogonal directions.
661  *
662  * \note if \a n is n unit length, computed values will be too.
663  */
664 void ortho_basis_v3v3_v3(float r_n1[3], float r_n2[3], const float n[3])
665 {
666         const float eps = FLT_EPSILON;
667         const float f = len_squared_v2(n);
668
669         if (f > eps) {
670                 const float d = 1.0f / sqrtf(f);
671
672                 BLI_assert(finite(d));
673
674                 r_n1[0] =  n[1] * d;
675                 r_n1[1] = -n[0] * d;
676                 r_n1[2] =  0.0f;
677                 r_n2[0] = -n[2] * r_n1[1];
678                 r_n2[1] =  n[2] * r_n1[0];
679                 r_n2[2] =  n[0] * r_n1[1] - n[1] * r_n1[0];
680         }
681         else {
682                 /* degenerate case */
683                 r_n1[0] = (n[2] < 0.0f) ? -1.0f : 1.0f;
684                 r_n1[1] = r_n1[2] = r_n2[0] = r_n2[2] = 0.0f;
685                 r_n2[1] = 1.0f;
686         }
687 }
688
689 /**
690  * Calculates \a p - a perpendicular vector to \a v
691  *
692  * \note return vector won't maintain same length.
693  */
694 void ortho_v3_v3(float p[3], const float v[3])
695 {
696         const int axis = axis_dominant_v3_single(v);
697
698         BLI_assert(p != v);
699
700         switch (axis) {
701                 case 0:
702                         p[0] = -v[1] - v[2];
703                         p[1] =  v[0];
704                         p[2] =  v[0];
705                         break;
706                 case 1:
707                         p[0] =  v[1];
708                         p[1] = -v[0] - v[2];
709                         p[2] =  v[1];
710                         break;
711                 case 2:
712                         p[0] =  v[2];
713                         p[1] =  v[2];
714                         p[2] = -v[0] - v[1];
715                         break;
716         }
717 }
718
719 /**
720  * no brainer compared to v3, just have for consistency.
721  */
722 void ortho_v2_v2(float p[2], const float v[2])
723 {
724         BLI_assert(p != v);
725
726         p[0] = -v[1];
727         p[1] =  v[0];
728 }
729
730 /* Rotate a point p by angle theta around an arbitrary axis r
731  * http://local.wasp.uwa.edu.au/~pbourke/geometry/
732  */
733 void rotate_normalized_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle)
734 {
735         const float costheta = cosf(angle);
736         const float sintheta = sinf(angle);
737
738         /* double check they are normalized */
739         BLI_ASSERT_UNIT_V3(axis);
740
741         r[0] = ((costheta + (1 - costheta) * axis[0] * axis[0]) * p[0]) +
742                (((1 - costheta) * axis[0] * axis[1] - axis[2] * sintheta) * p[1]) +
743                (((1 - costheta) * axis[0] * axis[2] + axis[1] * sintheta) * p[2]);
744
745         r[1] = (((1 - costheta) * axis[0] * axis[1] + axis[2] * sintheta) * p[0]) +
746                ((costheta + (1 - costheta) * axis[1] * axis[1]) * p[1]) +
747                (((1 - costheta) * axis[1] * axis[2] - axis[0] * sintheta) * p[2]);
748
749         r[2] = (((1 - costheta) * axis[0] * axis[2] - axis[1] * sintheta) * p[0]) +
750                (((1 - costheta) * axis[1] * axis[2] + axis[0] * sintheta) * p[1]) +
751                ((costheta + (1 - costheta) * axis[2] * axis[2]) * p[2]);
752 }
753
754 void rotate_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle)
755 {
756         float axis_n[3];
757
758         normalize_v3_v3(axis_n, axis);
759
760         rotate_normalized_v3_v3v3fl(r, p, axis_n, angle);
761 }
762
763 /*********************************** Other ***********************************/
764
765 void print_v2(const char *str, const float v[2])
766 {
767         printf("%s: %.8f %.8f\n", str, v[0], v[1]);
768 }
769
770 void print_v3(const char *str, const float v[3])
771 {
772         printf("%s: %.8f %.8f %.8f\n", str, v[0], v[1], v[2]);
773 }
774
775 void print_v4(const char *str, const float v[4])
776 {
777         printf("%s: %.8f %.8f %.8f %.8f\n", str, v[0], v[1], v[2], v[3]);
778 }
779
780 void print_vn(const char *str, const float v[], const int n)
781 {
782         int i = 0;
783         printf("%s[%d]:", str, n);
784         while (i < n) {
785                 printf(" %.8f", v[i++]);
786         }
787         printf("\n");
788 }
789
790 void minmax_v3v3_v3(float min[3], float max[3], const float vec[3])
791 {
792         if (min[0] > vec[0]) min[0] = vec[0];
793         if (min[1] > vec[1]) min[1] = vec[1];
794         if (min[2] > vec[2]) min[2] = vec[2];
795
796         if (max[0] < vec[0]) max[0] = vec[0];
797         if (max[1] < vec[1]) max[1] = vec[1];
798         if (max[2] < vec[2]) max[2] = vec[2];
799 }
800
801 void minmax_v2v2_v2(float min[2], float max[2], const float vec[2])
802 {
803         if (min[0] > vec[0]) min[0] = vec[0];
804         if (min[1] > vec[1]) min[1] = vec[1];
805
806         if (max[0] < vec[0]) max[0] = vec[0];
807         if (max[1] < vec[1]) max[1] = vec[1];
808 }
809
810 void minmax_v3v3_v3_array(float r_min[3], float r_max[3], const float (*vec_arr)[3], int nbr)
811 {
812         while (nbr--) {
813                 minmax_v3v3_v3(r_min, r_max, *vec_arr++);
814         }
815 }
816
817 /** ensure \a v1 is \a dist from \a v2 */
818 void dist_ensure_v3_v3fl(float v1[3], const float v2[3], const float dist)
819 {
820         if (!equals_v3v3(v2, v1)) {
821                 float nor[3];
822
823                 sub_v3_v3v3(nor, v1, v2);
824                 normalize_v3(nor);
825                 madd_v3_v3v3fl(v1, v2, nor, dist);
826         }
827 }
828
829 void dist_ensure_v2_v2fl(float v1[2], const float v2[2], const float dist)
830 {
831         if (!equals_v2v2(v2, v1)) {
832                 float nor[2];
833
834                 sub_v2_v2v2(nor, v1, v2);
835                 normalize_v2(nor);
836                 madd_v2_v2v2fl(v1, v2, nor, dist);
837         }
838 }
839
840 void axis_sort_v3(const float axis_values[3], int r_axis_order[3])
841 {
842         float v[3];
843         copy_v3_v3(v, axis_values);
844
845 #define SWAP_AXIS(a, b) { \
846         SWAP(float, v[a],            v[b]); \
847         SWAP(int,   r_axis_order[a], r_axis_order[b]); \
848 } (void)0
849
850         if (v[0] < v[1]) {
851                 if (v[2] < v[0]) {  SWAP_AXIS(0, 2); }
852         }
853         else {
854                 if (v[1] < v[2]) { SWAP_AXIS(0, 1); }
855                 else             { SWAP_AXIS(0, 2); }
856         }
857         if (v[2] < v[1])     { SWAP_AXIS(1, 2); }
858
859 #undef SWAP_AXIS
860 }
861
862 /***************************** Array Functions *******************************/
863
864 MINLINE double sqr_db(double f)
865 {
866         return f * f;
867 }
868
869 double dot_vn_vn(const float *array_src_a, const float *array_src_b, const int size)
870 {
871         double d = 0.0f;
872         const float *array_pt_a = array_src_a + (size - 1);
873         const float *array_pt_b = array_src_b + (size - 1);
874         int i = size;
875         while (i--) {
876                 d += (double)(*(array_pt_a--) * *(array_pt_b--));
877         }
878         return d;
879 }
880
881 double len_squared_vn(const float *array, const int size)
882 {
883         double d = 0.0f;
884         const float *array_pt = array + (size - 1);
885         int i = size;
886         while (i--) {
887                 d += sqr_db((double)(*(array_pt--)));
888         }
889         return d;
890 }
891
892 float normalize_vn_vn(float *array_tar, const float *array_src, const int size)
893 {
894         const double d = len_squared_vn(array_src, size);
895         float d_sqrt;
896         if (d > 1.0e-35) {
897                 d_sqrt = (float)sqrt(d);
898                 mul_vn_vn_fl(array_tar, array_src, size, 1.0f / d_sqrt);
899         }
900         else {
901                 copy_vn_fl(array_tar, size, 0.0f);
902                 d_sqrt = 0.0f;
903         }
904         return d_sqrt;
905 }
906
907 float normalize_vn(float *array_tar, const int size)
908 {
909         return normalize_vn_vn(array_tar, array_tar, size);
910 }
911
912 void range_vn_i(int *array_tar, const int size, const int start)
913 {
914         int *array_pt = array_tar + (size - 1);
915         int j = start + (size - 1);
916         int i = size;
917         while (i--) {
918                 *(array_pt--) = j--;
919         }
920 }
921
922 void range_vn_u(unsigned int *array_tar, const int size, const unsigned int start)
923 {
924         unsigned int *array_pt = array_tar + (size - 1);
925         unsigned int j = start + (unsigned int)(size - 1);
926         int i = size;
927         while (i--) {
928                 *(array_pt--) = j--;
929         }
930 }
931
932 void range_vn_fl(float *array_tar, const int size, const float start, const float step)
933 {
934         float *array_pt = array_tar + (size - 1);
935         int i = size;
936         while (i--) {
937                 *(array_pt--) = start + step * (float)(i);
938         }
939 }
940
941 void negate_vn(float *array_tar, const int size)
942 {
943         float *array_pt = array_tar + (size - 1);
944         int i = size;
945         while (i--) {
946                 *(array_pt--) *= -1.0f;
947         }
948 }
949
950 void negate_vn_vn(float *array_tar, const float *array_src, const int size)
951 {
952         float *tar = array_tar + (size - 1);
953         const float *src = array_src + (size - 1);
954         int i = size;
955         while (i--) {
956                 *(tar--) = -*(src--);
957         }
958 }
959
960 void mul_vn_fl(float *array_tar, const int size, const float f)
961 {
962         float *array_pt = array_tar + (size - 1);
963         int i = size;
964         while (i--) {
965                 *(array_pt--) *= f;
966         }
967 }
968
969 void mul_vn_vn_fl(float *array_tar, const float *array_src, const int size, const float f)
970 {
971         float *tar = array_tar + (size - 1);
972         const float *src = array_src + (size - 1);
973         int i = size;
974         while (i--) {
975                 *(tar--) = *(src--) * f;
976         }
977 }
978
979 void add_vn_vn(float *array_tar, const float *array_src, const int size)
980 {
981         float *tar = array_tar + (size - 1);
982         const float *src = array_src + (size - 1);
983         int i = size;
984         while (i--) {
985                 *(tar--) += *(src--);
986         }
987 }
988
989 void add_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const int size)
990 {
991         float *tar = array_tar + (size - 1);
992         const float *src_a = array_src_a + (size - 1);
993         const float *src_b = array_src_b + (size - 1);
994         int i = size;
995         while (i--) {
996                 *(tar--) = *(src_a--) + *(src_b--);
997         }
998 }
999
1000 void madd_vn_vn(float *array_tar, const float *array_src, const float f, const int size)
1001 {
1002         float *tar = array_tar + (size - 1);
1003         const float *src = array_src + (size - 1);
1004         int i = size;
1005         while (i--) {
1006                 *(tar--) += *(src--) * f;
1007         }
1008 }
1009
1010 void madd_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const float f, const int size)
1011 {
1012         float *tar = array_tar + (size - 1);
1013         const float *src_a = array_src_a + (size - 1);
1014         const float *src_b = array_src_b + (size - 1);
1015         int i = size;
1016         while (i--) {
1017                 *(tar--) = *(src_a--) + (*(src_b--) * f);
1018         }
1019 }
1020
1021 void sub_vn_vn(float *array_tar, const float *array_src, const int size)
1022 {
1023         float *tar = array_tar + (size - 1);
1024         const float *src = array_src + (size - 1);
1025         int i = size;
1026         while (i--) {
1027                 *(tar--) -= *(src--);
1028         }
1029 }
1030
1031 void sub_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const int size)
1032 {
1033         float *tar = array_tar + (size - 1);
1034         const float *src_a = array_src_a + (size - 1);
1035         const float *src_b = array_src_b + (size - 1);
1036         int i = size;
1037         while (i--) {
1038                 *(tar--) = *(src_a--) - *(src_b--);
1039         }
1040 }
1041
1042 void msub_vn_vn(float *array_tar, const float *array_src, const float f, const int size)
1043 {
1044         float *tar = array_tar + (size - 1);
1045         const float *src = array_src + (size - 1);
1046         int i = size;
1047         while (i--) {
1048                 *(tar--) -= *(src--) * f;
1049         }
1050 }
1051
1052 void msub_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const float f, const int size)
1053 {
1054         float *tar = array_tar + (size - 1);
1055         const float *src_a = array_src_a + (size - 1);
1056         const float *src_b = array_src_b + (size - 1);
1057         int i = size;
1058         while (i--) {
1059                 *(tar--) = *(src_a--) - (*(src_b--) * f);
1060         }
1061 }
1062
1063 void interp_vn_vn(float *array_tar, const float *array_src, const float t, const int size)
1064 {
1065         const float s = 1.0f - t;
1066         float *tar = array_tar + (size - 1);
1067         const float *src = array_src + (size - 1);
1068         int i = size;
1069         while (i--) {
1070                 *(tar) = (s * *(tar)) + (t * *(src));
1071                 tar--;
1072                 src--;
1073         }
1074 }
1075
1076 void copy_vn_i(int *array_tar, const int size, const int val)
1077 {
1078         int *tar = array_tar + (size - 1);
1079         int i = size;
1080         while (i--) {
1081                 *(tar--) = val;
1082         }
1083 }
1084
1085 void copy_vn_short(short *array_tar, const int size, const short val)
1086 {
1087         short *tar = array_tar + (size - 1);
1088         int i = size;
1089         while (i--) {
1090                 *(tar--) = val;
1091         }
1092 }
1093
1094 void copy_vn_ushort(unsigned short *array_tar, const int size, const unsigned short val)
1095 {
1096         unsigned short *tar = array_tar + (size - 1);
1097         int i = size;
1098         while (i--) {
1099                 *(tar--) = val;
1100         }
1101 }
1102
1103 void copy_vn_uchar(unsigned char *array_tar, const int size, const unsigned char val)
1104 {
1105         unsigned char *tar = array_tar + (size - 1);
1106         int i = size;
1107         while (i--) {
1108                 *(tar--) = val;
1109         }
1110 }
1111
1112 void copy_vn_fl(float *array_tar, const int size, const float val)
1113 {
1114         float *tar = array_tar + (size - 1);
1115         int i = size;
1116         while (i--) {
1117                 *(tar--) = val;
1118         }
1119 }
1120
1121 /** \name Double precision versions 'db'.
1122  * \{ */
1123
1124 void add_vn_vn_d(double *array_tar, const double *array_src, const int size)
1125 {
1126         double *tar = array_tar + (size - 1);
1127         const double *src = array_src + (size - 1);
1128         int i = size;
1129         while (i--) {
1130                 *(tar--) += *(src--);
1131         }
1132 }
1133
1134 void add_vn_vnvn_d(double *array_tar, const double *array_src_a, const double *array_src_b, const int size)
1135 {
1136         double *tar = array_tar + (size - 1);
1137         const double *src_a = array_src_a + (size - 1);
1138         const double *src_b = array_src_b + (size - 1);
1139         int i = size;
1140         while (i--) {
1141                 *(tar--) = *(src_a--) + *(src_b--);
1142         }
1143 }
1144
1145 void mul_vn_db(double *array_tar, const int size, const double f)
1146 {
1147         double *array_pt = array_tar + (size - 1);
1148         int i = size;
1149         while (i--) {
1150                 *(array_pt--) *= f;
1151         }
1152 }
1153
1154 /** \} */