Merging r58196 through r58265 from trunk into soc-2013-depsgraph_mt
[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
31
32 #include "BLI_math.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         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         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         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 /* weight 3 vectors,
72  * 'w' must be unit length but is not a vector, just 3 weights */
73 void interp_v3_v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3])
74 {
75         p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2];
76         p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2];
77         p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2];
78 }
79
80 /* weight 3 vectors,
81  * 'w' must be unit length but is not a vector, just 4 weights */
82 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])
83 {
84         p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3];
85         p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3];
86         p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3];
87 }
88
89 void interp_v4_v4v4v4(float p[4], const float v1[4], const float v2[4], const float v3[4], const float w[3])
90 {
91         p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2];
92         p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2];
93         p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2];
94         p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2];
95 }
96
97 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])
98 {
99         p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3];
100         p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3];
101         p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3];
102         p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2] + v4[3] * w[3];
103 }
104
105 void interp_v3_v3v3v3_uv(float p[3], const float v1[3], const float v2[3], const float v3[3], const float uv[2])
106 {
107         p[0] = v1[0] + ((v2[0] - v1[0]) * uv[0]) + ((v3[0] - v1[0]) * uv[1]);
108         p[1] = v1[1] + ((v2[1] - v1[1]) * uv[0]) + ((v3[1] - v1[1]) * uv[1]);
109         p[2] = v1[2] + ((v2[2] - v1[2]) * uv[0]) + ((v3[2] - v1[2]) * uv[1]);
110 }
111
112 void mid_v3_v3v3(float v[3], const float v1[3], const float v2[3])
113 {
114         v[0] = 0.5f * (v1[0] + v2[0]);
115         v[1] = 0.5f * (v1[1] + v2[1]);
116         v[2] = 0.5f * (v1[2] + v2[2]);
117 }
118
119 void mid_v2_v2v2(float v[2], const float v1[2], const float v2[2])
120 {
121         v[0] = 0.5f * (v1[0] + v2[0]);
122         v[1] = 0.5f * (v1[1] + v2[1]);
123 }
124
125 void mid_v3_v3v3v3(float v[3], const float v1[3], const float v2[3], const float v3[3])
126 {
127         v[0] = (v1[0] + v2[0] + v3[0]) / 3.0f;
128         v[1] = (v1[1] + v2[1] + v3[1]) / 3.0f;
129         v[2] = (v1[2] + v2[2] + v3[2]) / 3.0f;
130 }
131
132 /**
133  * Specialized function for calculating normals.
134  * fastpath for:
135  *
136  * \code{.c}
137  * add_v3_v3v3(r, a, b);
138  * normalize_v3(r)
139  * mul_v3_fl(r, angle_normalized_v3v3(a, b) / M_PI_2);
140  * \endcode
141  *
142  * We can use the length of (a + b) to calculate the angle.
143  */
144 void mid_v3_v3v3_angle_weighted(float r[3], const float a[3], const float b[3])
145 {
146         /* trick, we want the middle of 2 normals as well as the angle between them
147          * avoid multiple calculations by */
148         float angle;
149
150         /* double check they are normalized */
151         BLI_ASSERT_UNIT_V3(a);
152         BLI_ASSERT_UNIT_V3(b);
153
154         add_v3_v3v3(r, a, b);
155         angle = ((float)(1.0 / (M_PI / 2.0)) *
156                  /* normally we would only multiply by 2,
157                   * but instead of an angle make this 0-1 factor */
158                  2.0f) *
159                 acosf(normalize_v3(r) / 2.0f);
160         mul_v3_fl(r, angle);
161 }
162 /**
163  * Same as mid_v3_v3v3_angle_weighted
164  * but \a r is assumed to be accumulated normals, divided by their total.
165  */
166 void mid_v3_angle_weighted(float r[3])
167 {
168         /* trick, we want the middle of 2 normals as well as the angle between them
169          * avoid multiple calculations by */
170         float angle;
171
172         /* double check they are normalized */
173         BLI_assert(len_squared_v3(r) <= 1.0f + FLT_EPSILON);
174
175         angle = ((float)(1.0 / (M_PI / 2.0)) *
176                  /* normally we would only multiply by 2,
177                   * but instead of an angle make this 0-1 factor */
178                  2.0f) *
179                 acosf(normalize_v3(r));
180         mul_v3_fl(r, angle);
181 }
182
183 /**
184  * Equivalent to:
185  * interp_v3_v3v3(v, v1, v2, -1.0f);
186  */
187
188 void flip_v4_v4v4(float v[4], const float v1[4], const float v2[4])
189 {
190         v[0] = v1[0] + (v1[0] - v2[0]);
191         v[1] = v1[1] + (v1[1] - v2[1]);
192         v[2] = v1[2] + (v1[2] - v2[2]);
193         v[3] = v1[3] + (v1[3] - v2[3]);
194 }
195
196 void flip_v3_v3v3(float v[3], const float v1[3], const float v2[3])
197 {
198         v[0] = v1[0] + (v1[0] - v2[0]);
199         v[1] = v1[1] + (v1[1] - v2[1]);
200         v[2] = v1[2] + (v1[2] - v2[2]);
201 }
202
203 void flip_v2_v2v2(float v[2], const float v1[2], const float v2[2])
204 {
205         v[0] = v1[0] + (v1[0] - v2[0]);
206         v[1] = v1[1] + (v1[1] - v2[1]);
207 }
208
209 /********************************** Angles ***********************************/
210
211 /* Return the angle in radians between vecs 1-2 and 2-3 in radians
212  * If v1 is a shoulder, v2 is the elbow and v3 is the hand,
213  * this would return the angle at the elbow.
214  *
215  * note that when v1/v2/v3 represent 3 points along a straight line
216  * that the angle returned will be pi (180deg), rather then 0.0
217  */
218 float angle_v3v3v3(const float v1[3], const float v2[3], const float v3[3])
219 {
220         float vec1[3], vec2[3];
221
222         sub_v3_v3v3(vec1, v2, v1);
223         sub_v3_v3v3(vec2, v2, v3);
224         normalize_v3(vec1);
225         normalize_v3(vec2);
226
227         return angle_normalized_v3v3(vec1, vec2);
228 }
229
230 /* Quicker than full angle computation */
231 float cos_v3v3v3(const float p1[3], const float p2[3], const float p3[3])
232 {
233         float vec1[3], vec2[3];
234
235         sub_v3_v3v3(vec1, p2, p1);
236         sub_v3_v3v3(vec2, p2, p3);
237         normalize_v3(vec1);
238         normalize_v3(vec2);
239
240         return dot_v3v3(vec1, vec2);
241 }
242
243 /* Return the shortest angle in radians between the 2 vectors */
244 float angle_v3v3(const float v1[3], const float v2[3])
245 {
246         float vec1[3], vec2[3];
247
248         normalize_v3_v3(vec1, v1);
249         normalize_v3_v3(vec2, v2);
250
251         return angle_normalized_v3v3(vec1, vec2);
252 }
253
254 float angle_v2v2v2(const float v1[2], const float v2[2], const float v3[2])
255 {
256         float vec1[2], vec2[2];
257
258         vec1[0] = v2[0] - v1[0];
259         vec1[1] = v2[1] - v1[1];
260
261         vec2[0] = v2[0] - v3[0];
262         vec2[1] = v2[1] - v3[1];
263
264         normalize_v2(vec1);
265         normalize_v2(vec2);
266
267         return angle_normalized_v2v2(vec1, vec2);
268 }
269
270 /* Return the shortest angle in radians between the 2 vectors */
271 float angle_v2v2(const float v1[2], const float v2[2])
272 {
273         float vec1[2], vec2[2];
274
275         vec1[0] = v1[0];
276         vec1[1] = v1[1];
277
278         vec2[0] = v2[0];
279         vec2[1] = v2[1];
280
281         normalize_v2(vec1);
282         normalize_v2(vec2);
283
284         return angle_normalized_v2v2(vec1, vec2);
285 }
286
287 float angle_signed_v2v2(const float v1[2], const float v2[2])
288 {
289         const float perp_dot = (v1[1] * v2[0]) - (v1[0] * v2[1]);
290         return atan2f(perp_dot, dot_v2v2(v1, v2));
291 }
292
293 float angle_normalized_v3v3(const float v1[3], const float v2[3])
294 {
295         /* double check they are normalized */
296         BLI_ASSERT_UNIT_V3(v1);
297         BLI_ASSERT_UNIT_V3(v2);
298
299         /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
300         if (dot_v3v3(v1, v2) < 0.0f) {
301                 float vec[3];
302
303                 vec[0] = -v2[0];
304                 vec[1] = -v2[1];
305                 vec[2] = -v2[2];
306
307                 return (float)M_PI - 2.0f * (float)saasin(len_v3v3(vec, v1) / 2.0f);
308         }
309         else
310                 return 2.0f * (float)saasin(len_v3v3(v2, v1) / 2.0f);
311 }
312
313 float angle_normalized_v2v2(const float v1[2], const float v2[2])
314 {
315         /* double check they are normalized */
316         BLI_ASSERT_UNIT_V2(v1);
317         BLI_ASSERT_UNIT_V2(v2);
318
319         /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
320         if (dot_v2v2(v1, v2) < 0.0f) {
321                 float vec[2];
322
323                 vec[0] = -v2[0];
324                 vec[1] = -v2[1];
325
326                 return (float)M_PI - 2.0f * saasin(len_v2v2(vec, v1) / 2.0f);
327         }
328         else
329                 return 2.0f * (float)saasin(len_v2v2(v2, v1) / 2.0f);
330 }
331
332 /**
333  * angle between 2 vectors defined by 3 coords, about an axis. */
334 float angle_on_axis_v3v3v3_v3(const float v1[3], const float v2[3], const float v3[3], const float axis[3])
335 {
336         float v1_proj[3], v2_proj[3], tproj[3];
337
338         sub_v3_v3v3(v1_proj, v1, v2);
339         sub_v3_v3v3(v2_proj, v3, v2);
340
341         /* project the vectors onto the axis */
342         project_v3_v3v3(tproj, v1_proj, axis);
343         sub_v3_v3(v1_proj, tproj);
344
345         project_v3_v3v3(tproj, v2_proj, axis);
346         sub_v3_v3(v2_proj, tproj);
347
348         return angle_v3v3(v1_proj, v2_proj);
349 }
350
351 void angle_tri_v3(float angles[3], const float v1[3], const float v2[3], const float v3[3])
352 {
353         float ed1[3], ed2[3], ed3[3];
354
355         sub_v3_v3v3(ed1, v3, v1);
356         sub_v3_v3v3(ed2, v1, v2);
357         sub_v3_v3v3(ed3, v2, v3);
358
359         normalize_v3(ed1);
360         normalize_v3(ed2);
361         normalize_v3(ed3);
362
363         angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2);
364         angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3);
365         // face_angles[2] = M_PI - angle_normalized_v3v3(ed3, ed1);
366         angles[2] = (float)M_PI - (angles[0] + angles[1]);
367 }
368
369 void angle_quad_v3(float angles[4], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
370 {
371         float ed1[3], ed2[3], ed3[3], ed4[3];
372
373         sub_v3_v3v3(ed1, v4, v1);
374         sub_v3_v3v3(ed2, v1, v2);
375         sub_v3_v3v3(ed3, v2, v3);
376         sub_v3_v3v3(ed4, v3, v4);
377
378         normalize_v3(ed1);
379         normalize_v3(ed2);
380         normalize_v3(ed3);
381         normalize_v3(ed4);
382
383         angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2);
384         angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3);
385         angles[2] = (float)M_PI - angle_normalized_v3v3(ed3, ed4);
386         angles[3] = (float)M_PI - angle_normalized_v3v3(ed4, ed1);
387 }
388
389 void angle_poly_v3(float *angles, const float *verts[3], int len)
390 {
391         int i;
392         float vec[3][3];
393
394         sub_v3_v3v3(vec[2], verts[len - 1], verts[0]);
395         normalize_v3(vec[2]);
396         for (i = 0; i < len; i++) {
397                 sub_v3_v3v3(vec[i % 3], verts[i % len], verts[(i + 1) % len]);
398                 normalize_v3(vec[i % 3]);
399                 angles[i] = (float)M_PI - angle_normalized_v3v3(vec[(i + 2) % 3], vec[i % 3]);
400         }
401 }
402
403 /********************************* Geometry **********************************/
404
405 /* Project v1 on v2 */
406 void project_v2_v2v2(float c[2], const float v1[2], const float v2[2])
407 {
408         float mul;
409         mul = dot_v2v2(v1, v2) / dot_v2v2(v2, v2);
410
411         c[0] = mul * v2[0];
412         c[1] = mul * v2[1];
413 }
414
415 /* Project v1 on v2 */
416 void project_v3_v3v3(float c[3], const float v1[3], const float v2[3])
417 {
418         float mul;
419         mul = dot_v3v3(v1, v2) / dot_v3v3(v2, v2);
420
421         c[0] = mul * v2[0];
422         c[1] = mul * v2[1];
423         c[2] = mul * v2[2];
424 }
425
426 /* project a vector on a plane defined by normal and a plane point p */
427 void project_v3_plane(float v[3], const float n[3], const float p[3])
428 {
429         float vector[3];
430         float mul;
431
432         sub_v3_v3v3(vector, v, p);
433         mul = dot_v3v3(vector, n) / len_squared_v3(n);
434
435         mul_v3_v3fl(vector, n, mul);
436
437         sub_v3_v3(v, vector);
438 }
439
440 /* Returns a vector bisecting the angle at v2 formed by v1, v2 and v3 */
441 void bisect_v3_v3v3v3(float out[3], const float v1[3], const float v2[3], const float v3[3])
442 {
443         float d_12[3], d_23[3];
444         sub_v3_v3v3(d_12, v2, v1);
445         sub_v3_v3v3(d_23, v3, v2);
446         normalize_v3(d_12);
447         normalize_v3(d_23);
448         add_v3_v3v3(out, d_12, d_23);
449         normalize_v3(out);
450 }
451
452 /* Returns a reflection vector from a vector and a normal vector
453  * reflect = vec - ((2 * DotVecs(vec, mirror)) * mirror)
454  */
455 void reflect_v3_v3v3(float out[3], const float v1[3], const float v2[3])
456 {
457         float vec[3], normal[3];
458         float reflect[3] = {0.0f, 0.0f, 0.0f};
459         float dot2;
460
461         copy_v3_v3(vec, v1);
462         copy_v3_v3(normal, v2);
463
464         dot2 = 2 * dot_v3v3(vec, normal);
465
466         reflect[0] = vec[0] - (dot2 * normal[0]);
467         reflect[1] = vec[1] - (dot2 * normal[1]);
468         reflect[2] = vec[2] - (dot2 * normal[2]);
469
470         copy_v3_v3(out, reflect);
471 }
472
473 void ortho_basis_v3v3_v3(float v1[3], float v2[3], const float v[3])
474 {
475         const float f = (float)sqrt(v[0] * v[0] + v[1] * v[1]);
476
477         if (f < 1e-35f) {
478                 // degenerate case
479                 v1[0] = (v[2] < 0.0f) ? -1.0f : 1.0f;
480                 v1[1] = v1[2] = v2[0] = v2[2] = 0.0f;
481                 v2[1] = 1.0f;
482         }
483         else {
484                 const float d = 1.0f / f;
485
486                 v1[0] = v[1] * d;
487                 v1[1] = -v[0] * d;
488                 v1[2] = 0.0f;
489                 v2[0] = -v[2] * v1[1];
490                 v2[1] = v[2] * v1[0];
491                 v2[2] = v[0] * v1[1] - v[1] * v1[0];
492         }
493 }
494
495 /* Rotate a point p by angle theta around an arbitrary axis r
496  * http://local.wasp.uwa.edu.au/~pbourke/geometry/
497  */
498 void rotate_normalized_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle)
499 {
500         const float costheta = cos(angle);
501         const float sintheta = sin(angle);
502
503         /* double check they are normalized */
504         BLI_ASSERT_UNIT_V3(axis);
505
506         r[0] = ((costheta + (1 - costheta) * axis[0] * axis[0]) * p[0]) +
507                (((1 - costheta) * axis[0] * axis[1] - axis[2] * sintheta) * p[1]) +
508                (((1 - costheta) * axis[0] * axis[2] + axis[1] * sintheta) * p[2]);
509
510         r[1] = (((1 - costheta) * axis[0] * axis[1] + axis[2] * sintheta) * p[0]) +
511                ((costheta + (1 - costheta) * axis[1] * axis[1]) * p[1]) +
512                (((1 - costheta) * axis[1] * axis[2] - axis[0] * sintheta) * p[2]);
513
514         r[2] = (((1 - costheta) * axis[0] * axis[2] - axis[1] * sintheta) * p[0]) +
515                (((1 - costheta) * axis[1] * axis[2] + axis[0] * sintheta) * p[1]) +
516                ((costheta + (1 - costheta) * axis[2] * axis[2]) * p[2]);
517 }
518
519 void rotate_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle)
520 {
521         float axis_n[3];
522
523         normalize_v3_v3(axis_n, axis);
524
525         rotate_normalized_v3_v3v3fl(r, p, axis_n, angle);
526 }
527
528 /*********************************** Other ***********************************/
529
530 void print_v2(const char *str, const float v[2])
531 {
532         printf("%s: %.3f %.3f\n", str, v[0], v[1]);
533 }
534
535 void print_v3(const char *str, const float v[3])
536 {
537         printf("%s: %.3f %.3f %.3f\n", str, v[0], v[1], v[2]);
538 }
539
540 void print_v4(const char *str, const float v[4])
541 {
542         printf("%s: %.3f %.3f %.3f %.3f\n", str, v[0], v[1], v[2], v[3]);
543 }
544
545 void print_vn(const char *str, const float v[], const int n)
546 {
547         int i = 0;
548         printf("%s[%d]:", str, n);
549         while (i < n) {
550                 printf(" %.3f", v[i++]);
551         }
552         printf("\n");
553 }
554
555 void minmax_v3v3_v3(float min[3], float max[3], const float vec[3])
556 {
557         if (min[0] > vec[0]) min[0] = vec[0];
558         if (min[1] > vec[1]) min[1] = vec[1];
559         if (min[2] > vec[2]) min[2] = vec[2];
560
561         if (max[0] < vec[0]) max[0] = vec[0];
562         if (max[1] < vec[1]) max[1] = vec[1];
563         if (max[2] < vec[2]) max[2] = vec[2];
564 }
565
566 void minmax_v2v2_v2(float min[2], float max[2], const float vec[2])
567 {
568         if (min[0] > vec[0]) min[0] = vec[0];
569         if (min[1] > vec[1]) min[1] = vec[1];
570
571         if (max[0] < vec[0]) max[0] = vec[0];
572         if (max[1] < vec[1]) max[1] = vec[1];
573 }
574
575 /** ensure \a v1 is \a dist from \a v2 */
576 void dist_ensure_v3_v3fl(float v1[3], const float v2[3], const float dist)
577 {
578         if (!equals_v3v3(v2, v1)) {
579                 float nor[3];
580
581                 sub_v3_v3v3(nor, v1, v2);
582                 normalize_v3(nor);
583                 madd_v3_v3v3fl(v1, v2, nor, dist);
584         }
585 }
586
587 void dist_ensure_v2_v2fl(float v1[2], const float v2[2], const float dist)
588 {
589         if (!equals_v2v2(v2, v1)) {
590                 float nor[2];
591
592                 sub_v2_v2v2(nor, v1, v2);
593                 normalize_v2(nor);
594                 madd_v2_v2v2fl(v1, v2, nor, dist);
595         }
596 }
597
598 void axis_sort_v3(const float axis_values[3], int r_axis_order[3])
599 {
600         float v[3];
601         copy_v3_v3(v, axis_values);
602
603 #define SWAP_AXIS(a, b) { \
604         SWAP(float, v[a],            v[b]); \
605         SWAP(int,   r_axis_order[a], r_axis_order[b]); \
606 } (void)0
607
608         if (v[0] < v[1]) {
609                 if (v[2] < v[0]) {  SWAP_AXIS(0, 2); }
610         }
611         else {
612                 if (v[1] < v[2]) { SWAP_AXIS(0, 1); }
613                 else             { SWAP_AXIS(0, 2); }
614         }
615         if (v[2] < v[1])     { SWAP_AXIS(1, 2); }
616
617 #undef SWAP_AXIS
618 }
619
620 /***************************** Array Functions *******************************/
621
622 double dot_vn_vn(const float *array_src_a, const float *array_src_b, const int size)
623 {
624         double d = 0.0f;
625         const float *array_pt_a = array_src_a + (size - 1);
626         const float *array_pt_b = array_src_b + (size - 1);
627         int i = size;
628         while (i--) {
629                 d += (double)(*(array_pt_a--) * *(array_pt_b--));
630         }
631         return d;
632 }
633
634 float normalize_vn_vn(float *array_tar, const float *array_src, const int size)
635 {
636         double d = dot_vn_vn(array_tar, array_src, size);
637         float d_sqrt;
638         if (d > 1.0e-35) {
639                 d_sqrt = (float)sqrt(d);
640                 mul_vn_vn_fl(array_tar, array_src, size, 1.0f / d_sqrt);
641         }
642         else {
643                 fill_vn_fl(array_tar, size, 0.0f);
644                 d_sqrt = 0.0f;
645         }
646         return d_sqrt;
647 }
648
649 float normalize_vn(float *array_tar, const int size)
650 {
651         return normalize_vn_vn(array_tar, array_tar, size);
652 }
653
654 void range_vn_i(int *array_tar, const int size, const int start)
655 {
656         int *array_pt = array_tar + (size - 1);
657         int j = start + (size - 1);
658         int i = size;
659         while (i--) {
660                 *(array_pt--) = j--;
661         }
662 }
663
664 void range_vn_fl(float *array_tar, const int size, const float start, const float step)
665 {
666         float *array_pt = array_tar + (size - 1);
667         int i = size;
668         while (i--) {
669                 *(array_pt--) = start + step * (float)(i);
670         }
671 }
672
673 void negate_vn(float *array_tar, const int size)
674 {
675         float *array_pt = array_tar + (size - 1);
676         int i = size;
677         while (i--) {
678                 *(array_pt--) *= -1.0f;
679         }
680 }
681
682 void negate_vn_vn(float *array_tar, const float *array_src, const int size)
683 {
684         float *tar = array_tar + (size - 1);
685         const float *src = array_src + (size - 1);
686         int i = size;
687         while (i--) {
688                 *(tar--) = -*(src--);
689         }
690 }
691
692 void mul_vn_fl(float *array_tar, const int size, const float f)
693 {
694         float *array_pt = array_tar + (size - 1);
695         int i = size;
696         while (i--) {
697                 *(array_pt--) *= f;
698         }
699 }
700
701 void mul_vn_vn_fl(float *array_tar, const float *array_src, const int size, const float f)
702 {
703         float *tar = array_tar + (size - 1);
704         const float *src = array_src + (size - 1);
705         int i = size;
706         while (i--) {
707                 *(tar--) = *(src--) * f;
708         }
709 }
710
711 void add_vn_vn(float *array_tar, const float *array_src, const int size)
712 {
713         float *tar = array_tar + (size - 1);
714         const float *src = array_src + (size - 1);
715         int i = size;
716         while (i--) {
717                 *(tar--) += *(src--);
718         }
719 }
720
721 void add_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const int size)
722 {
723         float *tar = array_tar + (size - 1);
724         const float *src_a = array_src_a + (size - 1);
725         const float *src_b = array_src_b + (size - 1);
726         int i = size;
727         while (i--) {
728                 *(tar--) = *(src_a--) + *(src_b--);
729         }
730 }
731
732 void madd_vn_vn(float *array_tar, const float *array_src, const float f, const int size)
733 {
734         float *tar = array_tar + (size - 1);
735         const float *src = array_src + (size - 1);
736         int i = size;
737         while (i--) {
738                 *(tar--) += *(src--) * f;
739         }
740 }
741
742 void madd_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const float f, const int size)
743 {
744         float *tar = array_tar + (size - 1);
745         const float *src_a = array_src_a + (size - 1);
746         const float *src_b = array_src_b + (size - 1);
747         int i = size;
748         while (i--) {
749                 *(tar--) = *(src_a--) + (*(src_b--) * f);
750         }
751 }
752
753 void sub_vn_vn(float *array_tar, const float *array_src, const int size)
754 {
755         float *tar = array_tar + (size - 1);
756         const float *src = array_src + (size - 1);
757         int i = size;
758         while (i--) {
759                 *(tar--) -= *(src--);
760         }
761 }
762
763 void sub_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const int size)
764 {
765         float *tar = array_tar + (size - 1);
766         const float *src_a = array_src_a + (size - 1);
767         const float *src_b = array_src_b + (size - 1);
768         int i = size;
769         while (i--) {
770                 *(tar--) = *(src_a--) - *(src_b--);
771         }
772 }
773
774 void msub_vn_vn(float *array_tar, const float *array_src, const float f, const int size)
775 {
776         float *tar = array_tar + (size - 1);
777         const float *src = array_src + (size - 1);
778         int i = size;
779         while (i--) {
780                 *(tar--) -= *(src--) * f;
781         }
782 }
783
784 void msub_vn_vnvn(float *array_tar, const float *array_src_a, const float *array_src_b, const float f, const int size)
785 {
786         float *tar = array_tar + (size - 1);
787         const float *src_a = array_src_a + (size - 1);
788         const float *src_b = array_src_b + (size - 1);
789         int i = size;
790         while (i--) {
791                 *(tar--) = *(src_a--) - (*(src_b--) * f);
792         }
793 }
794
795 void interp_vn_vn(float *array_tar, const float *array_src, const float t, const int size)
796 {
797         const float s = 1.0f - t;
798         float *tar = array_tar + (size - 1);
799         const float *src = array_src + (size - 1);
800         int i = size;
801         while (i--) {
802                 *(tar) = (s * *(tar)) + (t * *(src));
803                 tar--;
804                 src--;
805         }
806 }
807
808 void fill_vn_i(int *array_tar, const int size, const int val)
809 {
810         int *tar = array_tar + (size - 1);
811         int i = size;
812         while (i--) {
813                 *(tar--) = val;
814         }
815 }
816
817 void fill_vn_ushort(unsigned short *array_tar, const int size, const unsigned short val)
818 {
819         unsigned short *tar = array_tar + (size - 1);
820         int i = size;
821         while (i--) {
822                 *(tar--) = val;
823         }
824 }
825
826 void fill_vn_fl(float *array_tar, const int size, const float val)
827 {
828         float *tar = array_tar + (size - 1);
829         int i = size;
830         while (i--) {
831                 *(tar--) = val;
832         }
833 }