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