1 /*
2 * This program is free software; you can redistribute it and/or
3 * modify it under the terms of the GNU General Public License
4 * as published by the Free Software Foundation; either version 2
5 * of the License, or (at your option) any later version.
6 *
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
11 *
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software Foundation,
14 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15 *
16 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
17 * All rights reserved.
18 *
19 * The Original Code is: some of this file.
20 *
21 * */
22
23 /** \file
24 * \ingroup bli
25 */
26
27 #include "BLI_math.h"
28
29 #include "BLI_strict_flags.h"
30
31 //******************************* Interpolation *******************************/
32
interp_v2_v2v2(float r[2],const float a[2],const float b[2],const float t)33 void interp_v2_v2v2(float r[2], const float a[2], const float b[2], const float t)
34 {
35 const float s = 1.0f - t;
36
37 r[0] = s * a[0] + t * b[0];
38 r[1] = s * a[1] + t * b[1];
39 }
40
41 /* weight 3 2D vectors,
42 * 'w' must be unit length but is not a vector, just 3 weights */
interp_v2_v2v2v2(float r[2],const float a[2],const float b[2],const float c[2],const float t[3])43 void interp_v2_v2v2v2(
44 float r[2], const float a[2], const float b[2], const float c[2], const float t[3])
45 {
46 r[0] = a[0] * t[0] + b[0] * t[1] + c[0] * t[2];
47 r[1] = a[1] * t[0] + b[1] * t[1] + c[1] * t[2];
48 }
49
interp_v3_v3v3(float r[3],const float a[3],const float b[3],const float t)50 void interp_v3_v3v3(float r[3], const float a[3], const float b[3], const float t)
51 {
52 const float s = 1.0f - t;
53
54 r[0] = s * a[0] + t * b[0];
55 r[1] = s * a[1] + t * b[1];
56 r[2] = s * a[2] + t * b[2];
57 }
58
interp_v4_v4v4(float r[4],const float a[4],const float b[4],const float t)59 void interp_v4_v4v4(float r[4], const float a[4], const float b[4], const float t)
60 {
61 const float s = 1.0f - t;
62
63 r[0] = s * a[0] + t * b[0];
64 r[1] = s * a[1] + t * b[1];
65 r[2] = s * a[2] + t * b[2];
66 r[3] = s * a[3] + t * b[3];
67 }
68
69 /**
70 * slerp, treat vectors as spherical coordinates
71 * \see #interp_qt_qtqt
72 *
73 * \return success
74 */
interp_v3_v3v3_slerp(float target[3],const float a[3],const float b[3],const float t)75 bool interp_v3_v3v3_slerp(float target[3], const float a[3], const float b[3], const float t)
76 {
77 float cosom, w[2];
78
79 BLI_ASSERT_UNIT_V3(a);
80 BLI_ASSERT_UNIT_V3(b);
81
82 cosom = dot_v3v3(a, b);
83
84 /* direct opposites */
85 if (UNLIKELY(cosom < (-1.0f + FLT_EPSILON))) {
86 return false;
87 }
88
89 interp_dot_slerp(t, cosom, w);
90
91 target[0] = w[0] * a[0] + w[1] * b[0];
92 target[1] = w[0] * a[1] + w[1] * b[1];
93 target[2] = w[0] * a[2] + w[1] * b[2];
94
95 return true;
96 }
interp_v2_v2v2_slerp(float target[2],const float a[2],const float b[2],const float t)97 bool interp_v2_v2v2_slerp(float target[2], const float a[2], const float b[2], const float t)
98 {
99 float cosom, w[2];
100
101 BLI_ASSERT_UNIT_V2(a);
102 BLI_ASSERT_UNIT_V2(b);
103
104 cosom = dot_v2v2(a, b);
105
106 /* direct opposites */
107 if (UNLIKELY(cosom < (1.0f + FLT_EPSILON))) {
108 return false;
109 }
110
111 interp_dot_slerp(t, cosom, w);
112
113 target[0] = w[0] * a[0] + w[1] * b[0];
114 target[1] = w[0] * a[1] + w[1] * b[1];
115
116 return true;
117 }
118
119 /**
120 * Same as #interp_v3_v3v3_slerp but uses fallback values for opposite vectors.
121 */
interp_v3_v3v3_slerp_safe(float target[3],const float a[3],const float b[3],const float t)122 void interp_v3_v3v3_slerp_safe(float target[3], const float a[3], const float b[3], const float t)
123 {
124 if (UNLIKELY(!interp_v3_v3v3_slerp(target, a, b, t))) {
125 /* Axis are aligned so any orthogonal vector is acceptable. */
126 float ab_ortho[3];
127 ortho_v3_v3(ab_ortho, a);
128 normalize_v3(ab_ortho);
129 if (t < 0.5f) {
130 if (UNLIKELY(!interp_v3_v3v3_slerp(target, a, ab_ortho, t * 2.0f))) {
131 BLI_assert(0);
132 copy_v3_v3(target, a);
133 }
134 }
135 else {
136 if (UNLIKELY(!interp_v3_v3v3_slerp(target, ab_ortho, b, (t - 0.5f) * 2.0f))) {
137 BLI_assert(0);
138 copy_v3_v3(target, b);
139 }
140 }
141 }
142 }
interp_v2_v2v2_slerp_safe(float target[2],const float a[2],const float b[2],const float t)143 void interp_v2_v2v2_slerp_safe(float target[2], const float a[2], const float b[2], const float t)
144 {
145 if (UNLIKELY(!interp_v2_v2v2_slerp(target, a, b, t))) {
146 /* Axis are aligned so any orthogonal vector is acceptable. */
147 float ab_ortho[2];
148 ortho_v2_v2(ab_ortho, a);
149 // normalize_v2(ab_ortho);
150 if (t < 0.5f) {
151 if (UNLIKELY(!interp_v2_v2v2_slerp(target, a, ab_ortho, t * 2.0f))) {
152 BLI_assert(0);
153 copy_v2_v2(target, a);
154 }
155 }
156 else {
157 if (UNLIKELY(!interp_v2_v2v2_slerp(target, ab_ortho, b, (t - 0.5f) * 2.0f))) {
158 BLI_assert(0);
159 copy_v2_v2(target, b);
160 }
161 }
162 }
163 }
164
165 /** \name Cubic curve interpolation (bezier spline).
166 * \{ */
167
interp_v2_v2v2v2v2_cubic(float p[2],const float v1[2],const float v2[2],const float v3[2],const float v4[2],const float u)168 void interp_v2_v2v2v2v2_cubic(float p[2],
169 const float v1[2],
170 const float v2[2],
171 const float v3[2],
172 const float v4[2],
173 const float u)
174 {
175 float q0[2], q1[2], q2[2], r0[2], r1[2];
176
177 interp_v2_v2v2(q0, v1, v2, u);
178 interp_v2_v2v2(q1, v2, v3, u);
179 interp_v2_v2v2(q2, v3, v4, u);
180
181 interp_v2_v2v2(r0, q0, q1, u);
182 interp_v2_v2v2(r1, q1, q2, u);
183
184 interp_v2_v2v2(p, r0, r1, u);
185 }
186
187 /** \} */
188
189 /* weight 3 vectors,
190 * 'w' must be unit length but is not a vector, just 3 weights */
interp_v3_v3v3v3(float p[3],const float v1[3],const float v2[3],const float v3[3],const float w[3])191 void interp_v3_v3v3v3(
192 float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3])
193 {
194 p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2];
195 p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2];
196 p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2];
197 }
198
199 /* weight 3 vectors,
200 * 'w' must be unit length but is not a vector, just 4 weights */
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])201 void interp_v3_v3v3v3v3(float p[3],
202 const float v1[3],
203 const float v2[3],
204 const float v3[3],
205 const float v4[3],
206 const float w[4])
207 {
208 p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3];
209 p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3];
210 p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3];
211 }
212
interp_v4_v4v4v4(float p[4],const float v1[4],const float v2[4],const float v3[4],const float w[3])213 void interp_v4_v4v4v4(
214 float p[4], const float v1[4], const float v2[4], const float v3[4], const float w[3])
215 {
216 p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2];
217 p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2];
218 p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2];
219 p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2];
220 }
221
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])222 void interp_v4_v4v4v4v4(float p[4],
223 const float v1[4],
224 const float v2[4],
225 const float v3[4],
226 const float v4[4],
227 const float w[4])
228 {
229 p[0] = v1[0] * w[0] + v2[0] * w[1] + v3[0] * w[2] + v4[0] * w[3];
230 p[1] = v1[1] * w[0] + v2[1] * w[1] + v3[1] * w[2] + v4[1] * w[3];
231 p[2] = v1[2] * w[0] + v2[2] * w[1] + v3[2] * w[2] + v4[2] * w[3];
232 p[3] = v1[3] * w[0] + v2[3] * w[1] + v3[3] * w[2] + v4[3] * w[3];
233 }
234
interp_v3_v3v3v3_uv(float p[3],const float v1[3],const float v2[3],const float v3[3],const float uv[2])235 void interp_v3_v3v3v3_uv(
236 float p[3], const float v1[3], const float v2[3], const float v3[3], const float uv[2])
237 {
238 p[0] = v1[0] + ((v2[0] - v1[0]) * uv[0]) + ((v3[0] - v1[0]) * uv[1]);
239 p[1] = v1[1] + ((v2[1] - v1[1]) * uv[0]) + ((v3[1] - v1[1]) * uv[1]);
240 p[2] = v1[2] + ((v2[2] - v1[2]) * uv[0]) + ((v3[2] - v1[2]) * uv[1]);
241 }
242
interp_v3_v3v3_uchar(uchar target[3],const uchar a[3],const uchar b[3],const float t)243 void interp_v3_v3v3_uchar(uchar target[3], const uchar a[3], const uchar b[3], const float t)
244 {
245 const float s = 1.0f - t;
246
247 target[0] = (char)floorf(s * a[0] + t * b[0]);
248 target[1] = (char)floorf(s * a[1] + t * b[1]);
249 target[2] = (char)floorf(s * a[2] + t * b[2]);
250 }
interp_v3_v3v3_char(char target[3],const char a[3],const char b[3],const float t)251 void interp_v3_v3v3_char(char target[3], const char a[3], const char b[3], const float t)
252 {
253 interp_v3_v3v3_uchar((uchar *)target, (const uchar *)a, (const uchar *)b, t);
254 }
255
interp_v4_v4v4_uchar(uchar target[4],const uchar a[4],const uchar b[4],const float t)256 void interp_v4_v4v4_uchar(uchar target[4], const uchar a[4], const uchar b[4], const float t)
257 {
258 const float s = 1.0f - t;
259
260 target[0] = (char)floorf(s * a[0] + t * b[0]);
261 target[1] = (char)floorf(s * a[1] + t * b[1]);
262 target[2] = (char)floorf(s * a[2] + t * b[2]);
263 target[3] = (char)floorf(s * a[3] + t * b[3]);
264 }
interp_v4_v4v4_char(char target[4],const char a[4],const char b[4],const float t)265 void interp_v4_v4v4_char(char target[4], const char a[4], const char b[4], const float t)
266 {
267 interp_v4_v4v4_uchar((uchar *)target, (const uchar *)a, (const uchar *)b, t);
268 }
269
mid_v3_v3v3(float r[3],const float a[3],const float b[3])270 void mid_v3_v3v3(float r[3], const float a[3], const float b[3])
271 {
272 r[0] = 0.5f * (a[0] + b[0]);
273 r[1] = 0.5f * (a[1] + b[1]);
274 r[2] = 0.5f * (a[2] + b[2]);
275 }
276
mid_v2_v2v2(float r[2],const float a[2],const float b[2])277 void mid_v2_v2v2(float r[2], const float a[2], const float b[2])
278 {
279 r[0] = 0.5f * (a[0] + b[0]);
280 r[1] = 0.5f * (a[1] + b[1]);
281 }
282
mid_v3_v3v3v3(float v[3],const float v1[3],const float v2[3],const float v3[3])283 void mid_v3_v3v3v3(float v[3], const float v1[3], const float v2[3], const float v3[3])
284 {
285 v[0] = (v1[0] + v2[0] + v3[0]) / 3.0f;
286 v[1] = (v1[1] + v2[1] + v3[1]) / 3.0f;
287 v[2] = (v1[2] + v2[2] + v3[2]) / 3.0f;
288 }
289
mid_v3_v3v3v3v3(float v[3],const float v1[3],const float v2[3],const float v3[3],const float v4[3])290 void mid_v3_v3v3v3v3(
291 float v[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
292 {
293 v[0] = (v1[0] + v2[0] + v3[0] + v4[0]) / 4.0f;
294 v[1] = (v1[1] + v2[1] + v3[1] + v4[1]) / 4.0f;
295 v[2] = (v1[2] + v2[2] + v3[2] + v4[2]) / 4.0f;
296 }
297
mid_v3_v3_array(float r[3],const float (* vec_arr)[3],const uint nbr)298 void mid_v3_v3_array(float r[3], const float (*vec_arr)[3], const uint nbr)
299 {
300 const float factor = 1.0f / (float)nbr;
301 zero_v3(r);
302
303 for (uint i = 0; i < nbr; i++) {
304 madd_v3_v3fl(r, vec_arr[i], factor);
305 }
306 }
307
308 /**
309 * Specialized function for calculating normals.
310 * Fast-path for:
311 *
312 * \code{.c}
313 * add_v3_v3v3(r, a, b);
314 * normalize_v3(r)
315 * mul_v3_fl(r, angle_normalized_v3v3(a, b) / M_PI_2);
316 * \endcode
317 *
318 * We can use the length of (a + b) to calculate the angle.
319 */
mid_v3_v3v3_angle_weighted(float r[3],const float a[3],const float b[3])320 void mid_v3_v3v3_angle_weighted(float r[3], const float a[3], const float b[3])
321 {
322 /* trick, we want the middle of 2 normals as well as the angle between them
323 * avoid multiple calculations by */
324 float angle;
325
326 /* double check they are normalized */
327 BLI_ASSERT_UNIT_V3(a);
328 BLI_ASSERT_UNIT_V3(b);
329
330 add_v3_v3v3(r, a, b);
331 angle = ((float)(1.0 / (M_PI / 2.0)) *
332 /* normally we would only multiply by 2,
333 * but instead of an angle make this 0-1 factor */
334 2.0f) *
335 acosf(normalize_v3(r) / 2.0f);
336 mul_v3_fl(r, angle);
337 }
338 /**
339 * Same as mid_v3_v3v3_angle_weighted
340 * but \a r is assumed to be accumulated normals, divided by their total.
341 */
mid_v3_angle_weighted(float r[3])342 void mid_v3_angle_weighted(float r[3])
343 {
344 /* trick, we want the middle of 2 normals as well as the angle between them
345 * avoid multiple calculations by */
346 float angle;
347
348 /* double check they are normalized */
349 BLI_assert(len_squared_v3(r) <= 1.0f + FLT_EPSILON);
350
351 angle = ((float)(1.0 / (M_PI / 2.0)) *
352 /* normally we would only multiply by 2,
353 * but instead of an angle make this 0-1 factor */
354 2.0f) *
355 acosf(normalize_v3(r));
356 mul_v3_fl(r, angle);
357 }
358
359 /**
360 * Equivalent to:
361 * interp_v3_v3v3(v, v1, v2, -1.0f);
362 */
363
flip_v4_v4v4(float v[4],const float v1[4],const float v2[4])364 void flip_v4_v4v4(float v[4], const float v1[4], const float v2[4])
365 {
366 v[0] = v1[0] + (v1[0] - v2[0]);
367 v[1] = v1[1] + (v1[1] - v2[1]);
368 v[2] = v1[2] + (v1[2] - v2[2]);
369 v[3] = v1[3] + (v1[3] - v2[3]);
370 }
371
flip_v3_v3v3(float v[3],const float v1[3],const float v2[3])372 void flip_v3_v3v3(float v[3], const float v1[3], const float v2[3])
373 {
374 v[0] = v1[0] + (v1[0] - v2[0]);
375 v[1] = v1[1] + (v1[1] - v2[1]);
376 v[2] = v1[2] + (v1[2] - v2[2]);
377 }
378
flip_v2_v2v2(float v[2],const float v1[2],const float v2[2])379 void flip_v2_v2v2(float v[2], const float v1[2], const float v2[2])
380 {
381 v[0] = v1[0] + (v1[0] - v2[0]);
382 v[1] = v1[1] + (v1[1] - v2[1]);
383 }
384
385 /********************************* Comparison ********************************/
386
is_finite_v2(const float v[2])387 bool is_finite_v2(const float v[2])
388 {
389 return (isfinite(v[0]) && isfinite(v[1]));
390 }
391
is_finite_v3(const float v[3])392 bool is_finite_v3(const float v[3])
393 {
394 return (isfinite(v[0]) && isfinite(v[1]) && isfinite(v[2]));
395 }
396
is_finite_v4(const float v[4])397 bool is_finite_v4(const float v[4])
398 {
399 return (isfinite(v[0]) && isfinite(v[1]) && isfinite(v[2]) && isfinite(v[3]));
400 }
401
402 /********************************** Angles ***********************************/
403
404 /* Return the angle in radians between vecs 1-2 and 2-3 in radians
405 * If v1 is a shoulder, v2 is the elbow and v3 is the hand,
406 * this would return the angle at the elbow.
407 *
408 * note that when v1/v2/v3 represent 3 points along a straight line
409 * that the angle returned will be pi (180deg), rather then 0.0
410 */
angle_v3v3v3(const float a[3],const float b[3],const float c[3])411 float angle_v3v3v3(const float a[3], const float b[3], const float c[3])
412 {
413 float vec1[3], vec2[3];
414
415 sub_v3_v3v3(vec1, b, a);
416 sub_v3_v3v3(vec2, b, c);
417 normalize_v3(vec1);
418 normalize_v3(vec2);
419
420 return angle_normalized_v3v3(vec1, vec2);
421 }
422
423 /* Quicker than full angle computation */
cos_v3v3v3(const float p1[3],const float p2[3],const float p3[3])424 float cos_v3v3v3(const float p1[3], const float p2[3], const float p3[3])
425 {
426 float vec1[3], vec2[3];
427
428 sub_v3_v3v3(vec1, p2, p1);
429 sub_v3_v3v3(vec2, p2, p3);
430 normalize_v3(vec1);
431 normalize_v3(vec2);
432
433 return dot_v3v3(vec1, vec2);
434 }
435
436 /* Return the shortest angle in radians between the 2 vectors */
angle_v3v3(const float a[3],const float b[3])437 float angle_v3v3(const float a[3], const float b[3])
438 {
439 float vec1[3], vec2[3];
440
441 normalize_v3_v3(vec1, a);
442 normalize_v3_v3(vec2, b);
443
444 return angle_normalized_v3v3(vec1, vec2);
445 }
446
angle_v2v2v2(const float a[2],const float b[2],const float c[2])447 float angle_v2v2v2(const float a[2], const float b[2], const float c[2])
448 {
449 float vec1[2], vec2[2];
450
451 vec1[0] = b[0] - a[0];
452 vec1[1] = b[1] - a[1];
453
454 vec2[0] = b[0] - c[0];
455 vec2[1] = b[1] - c[1];
456
457 normalize_v2(vec1);
458 normalize_v2(vec2);
459
460 return angle_normalized_v2v2(vec1, vec2);
461 }
462
463 /* Quicker than full angle computation */
cos_v2v2v2(const float p1[2],const float p2[2],const float p3[2])464 float cos_v2v2v2(const float p1[2], const float p2[2], const float p3[2])
465 {
466 float vec1[2], vec2[2];
467
468 sub_v2_v2v2(vec1, p2, p1);
469 sub_v2_v2v2(vec2, p2, p3);
470 normalize_v2(vec1);
471 normalize_v2(vec2);
472
473 return dot_v2v2(vec1, vec2);
474 }
475
476 /* Return the shortest angle in radians between the 2 vectors */
angle_v2v2(const float a[2],const float b[2])477 float angle_v2v2(const float a[2], const float b[2])
478 {
479 float vec1[2], vec2[2];
480
481 vec1[0] = a[0];
482 vec1[1] = a[1];
483
484 vec2[0] = b[0];
485 vec2[1] = b[1];
486
487 normalize_v2(vec1);
488 normalize_v2(vec2);
489
490 return angle_normalized_v2v2(vec1, vec2);
491 }
492
angle_signed_v2v2(const float v1[2],const float v2[2])493 float angle_signed_v2v2(const float v1[2], const float v2[2])
494 {
495 const float perp_dot = (v1[1] * v2[0]) - (v1[0] * v2[1]);
496 return atan2f(perp_dot, dot_v2v2(v1, v2));
497 }
498
angle_normalized_v3v3(const float v1[3],const float v2[3])499 float angle_normalized_v3v3(const float v1[3], const float v2[3])
500 {
501 /* double check they are normalized */
502 BLI_ASSERT_UNIT_V3(v1);
503 BLI_ASSERT_UNIT_V3(v2);
504
505 /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
506 if (dot_v3v3(v1, v2) >= 0.0f) {
507 return 2.0f * saasin(len_v3v3(v1, v2) / 2.0f);
508 }
509
510 float v2_n[3];
511 negate_v3_v3(v2_n, v2);
512 return (float)M_PI - 2.0f * saasin(len_v3v3(v1, v2_n) / 2.0f);
513 }
514
angle_normalized_v2v2(const float a[2],const float b[2])515 float angle_normalized_v2v2(const float a[2], const float b[2])
516 {
517 /* double check they are normalized */
518 BLI_ASSERT_UNIT_V2(a);
519 BLI_ASSERT_UNIT_V2(b);
520
521 /* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
522 if (dot_v2v2(a, b) >= 0.0f) {
523 return 2.0f * saasin(len_v2v2(a, b) / 2.0f);
524 }
525
526 float v2_n[2];
527 negate_v2_v2(v2_n, b);
528 return (float)M_PI - 2.0f * saasin(len_v2v2(a, v2_n) / 2.0f);
529 }
530
531 /**
532 * Angle between 2 vectors, about an axis (axis can be considered a plane).
533 */
angle_on_axis_v3v3_v3(const float v1[3],const float v2[3],const float axis[3])534 float angle_on_axis_v3v3_v3(const float v1[3], const float v2[3], const float axis[3])
535 {
536 float v1_proj[3], v2_proj[3];
537
538 /* project the vectors onto the axis */
539 project_plane_normalized_v3_v3v3(v1_proj, v1, axis);
540 project_plane_normalized_v3_v3v3(v2_proj, v2, axis);
541
542 return angle_v3v3(v1_proj, v2_proj);
543 }
544
angle_signed_on_axis_v3v3_v3(const float v1[3],const float v2[3],const float axis[3])545 float angle_signed_on_axis_v3v3_v3(const float v1[3], const float v2[3], const float axis[3])
546 {
547 float v1_proj[3], v2_proj[3], tproj[3];
548 float angle;
549
550 /* project the vectors onto the axis */
551 project_plane_normalized_v3_v3v3(v1_proj, v1, axis);
552 project_plane_normalized_v3_v3v3(v2_proj, v2, axis);
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 /**
566 * Angle between 2 vectors defined by 3 coords, about an axis (axis can be considered a plane).
567 */
angle_on_axis_v3v3v3_v3(const float v1[3],const float v2[3],const float v3[3],const float axis[3])568 float angle_on_axis_v3v3v3_v3(const float v1[3],
569 const float v2[3],
570 const float v3[3],
571 const float axis[3])
572 {
573 float vec1[3], vec2[3];
574
575 sub_v3_v3v3(vec1, v1, v2);
576 sub_v3_v3v3(vec2, v3, v2);
577
578 return angle_on_axis_v3v3_v3(vec1, vec2, axis);
579 }
580
angle_signed_on_axis_v3v3v3_v3(const float v1[3],const float v2[3],const float v3[3],const float axis[3])581 float angle_signed_on_axis_v3v3v3_v3(const float v1[3],
582 const float v2[3],
583 const float v3[3],
584 const float axis[3])
585 {
586 float vec1[3], vec2[3];
587
588 sub_v3_v3v3(vec1, v1, v2);
589 sub_v3_v3v3(vec2, v3, v2);
590
591 return angle_signed_on_axis_v3v3_v3(vec1, vec2, axis);
592 }
593
angle_tri_v3(float angles[3],const float v1[3],const float v2[3],const float v3[3])594 void angle_tri_v3(float angles[3], const float v1[3], const float v2[3], const float v3[3])
595 {
596 float ed1[3], ed2[3], ed3[3];
597
598 sub_v3_v3v3(ed1, v3, v1);
599 sub_v3_v3v3(ed2, v1, v2);
600 sub_v3_v3v3(ed3, v2, v3);
601
602 normalize_v3(ed1);
603 normalize_v3(ed2);
604 normalize_v3(ed3);
605
606 angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2);
607 angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3);
608 // face_angles[2] = M_PI - angle_normalized_v3v3(ed3, ed1);
609 angles[2] = (float)M_PI - (angles[0] + angles[1]);
610 }
611
angle_quad_v3(float angles[4],const float v1[3],const float v2[3],const float v3[3],const float v4[3])612 void angle_quad_v3(
613 float angles[4], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
614 {
615 float ed1[3], ed2[3], ed3[3], ed4[3];
616
617 sub_v3_v3v3(ed1, v4, v1);
618 sub_v3_v3v3(ed2, v1, v2);
619 sub_v3_v3v3(ed3, v2, v3);
620 sub_v3_v3v3(ed4, v3, v4);
621
622 normalize_v3(ed1);
623 normalize_v3(ed2);
624 normalize_v3(ed3);
625 normalize_v3(ed4);
626
627 angles[0] = (float)M_PI - angle_normalized_v3v3(ed1, ed2);
628 angles[1] = (float)M_PI - angle_normalized_v3v3(ed2, ed3);
629 angles[2] = (float)M_PI - angle_normalized_v3v3(ed3, ed4);
630 angles[3] = (float)M_PI - angle_normalized_v3v3(ed4, ed1);
631 }
632
angle_poly_v3(float * angles,const float * verts[3],int len)633 void angle_poly_v3(float *angles, const float *verts[3], int len)
634 {
635 int i;
636 float vec[3][3];
637
638 sub_v3_v3v3(vec[2], verts[len - 1], verts[0]);
639 normalize_v3(vec[2]);
640 for (i = 0; i < len; i++) {
641 sub_v3_v3v3(vec[i % 3], verts[i % len], verts[(i + 1) % len]);
642 normalize_v3(vec[i % 3]);
643 angles[i] = (float)M_PI - angle_normalized_v3v3(vec[(i + 2) % 3], vec[i % 3]);
644 }
645 }
646
647 /********************************* Geometry **********************************/
648
649 /**
650 * Project \a p onto \a v_proj
651 */
project_v2_v2v2(float out[2],const float p[2],const float v_proj[2])652 void project_v2_v2v2(float out[2], const float p[2], const float v_proj[2])
653 {
654 const float mul = dot_v2v2(p, v_proj) / dot_v2v2(v_proj, v_proj);
655
656 out[0] = mul * v_proj[0];
657 out[1] = mul * v_proj[1];
658 }
659
660 /**
661 * Project \a p onto \a v_proj
662 */
project_v3_v3v3(float out[3],const float p[3],const float v_proj[3])663 void project_v3_v3v3(float out[3], const float p[3], const float v_proj[3])
664 {
665 const float mul = dot_v3v3(p, v_proj) / dot_v3v3(v_proj, v_proj);
666
667 out[0] = mul * v_proj[0];
668 out[1] = mul * v_proj[1];
669 out[2] = mul * v_proj[2];
670 }
671
project_v3_v3v3_db(double out[3],const double p[3],const double v_proj[3])672 void project_v3_v3v3_db(double out[3], const double p[3], const double v_proj[3])
673 {
674 const double mul = dot_v3v3_db(p, v_proj) / dot_v3v3_db(v_proj, v_proj);
675
676 out[0] = mul * v_proj[0];
677 out[1] = mul * v_proj[1];
678 out[2] = mul * v_proj[2];
679 }
680
681 /**
682 * Project \a p onto a unit length \a v_proj
683 */
project_v2_v2v2_normalized(float out[2],const float p[2],const float v_proj[2])684 void project_v2_v2v2_normalized(float out[2], const float p[2], const float v_proj[2])
685 {
686 BLI_ASSERT_UNIT_V2(v_proj);
687 const float mul = dot_v2v2(p, v_proj);
688
689 out[0] = mul * v_proj[0];
690 out[1] = mul * v_proj[1];
691 }
692
693 /**
694 * Project \a p onto a unit length \a v_proj
695 */
project_v3_v3v3_normalized(float out[3],const float p[3],const float v_proj[3])696 void project_v3_v3v3_normalized(float out[3], const float p[3], const float v_proj[3])
697 {
698 BLI_ASSERT_UNIT_V3(v_proj);
699 const float mul = dot_v3v3(p, v_proj);
700
701 out[0] = mul * v_proj[0];
702 out[1] = mul * v_proj[1];
703 out[2] = mul * v_proj[2];
704 }
705
706 /**
707 * In this case plane is a 3D vector only (no 4th component).
708 *
709 * Projecting will make \a out a copy of \a p orthogonal to \a v_plane.
710 *
711 * \note If \a p is exactly perpendicular to \a v_plane, \a out will just be a copy of \a p.
712 *
713 * \note This function is a convenience to call:
714 * \code{.c}
715 * project_v3_v3v3(out, p, v_plane);
716 * sub_v3_v3v3(out, p, out);
717 * \endcode
718 */
project_plane_v3_v3v3(float out[3],const float p[3],const float v_plane[3])719 void project_plane_v3_v3v3(float out[3], const float p[3], const float v_plane[3])
720 {
721 const float mul = dot_v3v3(p, v_plane) / dot_v3v3(v_plane, v_plane);
722
723 out[0] = p[0] - (mul * v_plane[0]);
724 out[1] = p[1] - (mul * v_plane[1]);
725 out[2] = p[2] - (mul * v_plane[2]);
726 }
727
project_plane_v2_v2v2(float out[2],const float p[2],const float v_plane[2])728 void project_plane_v2_v2v2(float out[2], const float p[2], const float v_plane[2])
729 {
730 const float mul = dot_v2v2(p, v_plane) / dot_v2v2(v_plane, v_plane);
731
732 out[0] = p[0] - (mul * v_plane[0]);
733 out[1] = p[1] - (mul * v_plane[1]);
734 }
735
project_plane_normalized_v3_v3v3(float out[3],const float p[3],const float v_plane[3])736 void project_plane_normalized_v3_v3v3(float out[3], const float p[3], const float v_plane[3])
737 {
738 BLI_ASSERT_UNIT_V3(v_plane);
739 const float mul = dot_v3v3(p, v_plane);
740
741 out[0] = p[0] - (mul * v_plane[0]);
742 out[1] = p[1] - (mul * v_plane[1]);
743 out[2] = p[2] - (mul * v_plane[2]);
744 }
745
project_plane_normalized_v2_v2v2(float out[2],const float p[2],const float v_plane[2])746 void project_plane_normalized_v2_v2v2(float out[2], const float p[2], const float v_plane[2])
747 {
748 BLI_ASSERT_UNIT_V2(v_plane);
749 const float mul = dot_v2v2(p, v_plane);
750
751 out[0] = p[0] - (mul * v_plane[0]);
752 out[1] = p[1] - (mul * v_plane[1]);
753 }
754
755 /* project a vector on a plane defined by normal and a plane point p */
project_v3_plane(float out[3],const float plane_no[3],const float plane_co[3])756 void project_v3_plane(float out[3], const float plane_no[3], const float plane_co[3])
757 {
758 float vector[3];
759 float mul;
760
761 sub_v3_v3v3(vector, out, plane_co);
762 mul = dot_v3v3(vector, plane_no) / len_squared_v3(plane_no);
763
764 mul_v3_v3fl(vector, plane_no, mul);
765
766 sub_v3_v3(out, vector);
767 }
768
769 /* Returns a vector bisecting the angle at b formed by a, b and c */
bisect_v3_v3v3v3(float r[3],const float a[3],const float b[3],const float c[3])770 void bisect_v3_v3v3v3(float r[3], const float a[3], const float b[3], const float c[3])
771 {
772 float d_12[3], d_23[3];
773 sub_v3_v3v3(d_12, b, a);
774 sub_v3_v3v3(d_23, c, b);
775 normalize_v3(d_12);
776 normalize_v3(d_23);
777 add_v3_v3v3(r, d_12, d_23);
778 normalize_v3(r);
779 }
780
781 /**
782 * Returns a reflection vector from a vector and a normal vector
783 * reflect = vec - ((2 * dot(vec, mirror)) * mirror).
784 *
785 * <pre>
786 * v
787 * + ^
788 * \ |
789 * \|
790 * + normal: axis of reflection
791 * /
792 * /
793 * +
794 * out: result (negate for a 'bounce').
795 * </pre>
796 */
reflect_v3_v3v3(float out[3],const float v[3],const float normal[3])797 void reflect_v3_v3v3(float out[3], const float v[3], const float normal[3])
798 {
799 const float dot2 = 2.0f * dot_v3v3(v, normal);
800
801 BLI_ASSERT_UNIT_V3(normal);
802
803 out[0] = v[0] - (dot2 * normal[0]);
804 out[1] = v[1] - (dot2 * normal[1]);
805 out[2] = v[2] - (dot2 * normal[2]);
806 }
807
reflect_v3_v3v3_db(double out[3],const double v[3],const double normal[3])808 void reflect_v3_v3v3_db(double out[3], const double v[3], const double normal[3])
809 {
810 const double dot2 = 2.0 * dot_v3v3_db(v, normal);
811
812 /* BLI_ASSERT_UNIT_V3_DB(normal); this assert is not known? */
813
814 out[0] = v[0] - (dot2 * normal[0]);
815 out[1] = v[1] - (dot2 * normal[1]);
816 out[2] = v[2] - (dot2 * normal[2]);
817 }
818
819 /**
820 * Takes a vector and computes 2 orthogonal directions.
821 *
822 * \note if \a n is n unit length, computed values will be too.
823 */
ortho_basis_v3v3_v3(float r_n1[3],float r_n2[3],const float n[3])824 void ortho_basis_v3v3_v3(float r_n1[3], float r_n2[3], const float n[3])
825 {
826 const float eps = FLT_EPSILON;
827 const float f = len_squared_v2(n);
828
829 if (f > eps) {
830 const float d = 1.0f / sqrtf(f);
831
832 BLI_assert(isfinite(d));
833
834 r_n1[0] = n[1] * d;
835 r_n1[1] = -n[0] * d;
836 r_n1[2] = 0.0f;
837 r_n2[0] = -n[2] * r_n1[1];
838 r_n2[1] = n[2] * r_n1[0];
839 r_n2[2] = n[0] * r_n1[1] - n[1] * r_n1[0];
840 }
841 else {
842 /* degenerate case */
843 r_n1[0] = (n[2] < 0.0f) ? -1.0f : 1.0f;
844 r_n1[1] = r_n1[2] = r_n2[0] = r_n2[2] = 0.0f;
845 r_n2[1] = 1.0f;
846 }
847 }
848
849 /**
850 * Calculates \a p - a perpendicular vector to \a v
851 *
852 * \note return vector won't maintain same length.
853 */
ortho_v3_v3(float out[3],const float v[3])854 void ortho_v3_v3(float out[3], const float v[3])
855 {
856 const int axis = axis_dominant_v3_single(v);
857
858 BLI_assert(out != v);
859
860 switch (axis) {
861 case 0:
862 out[0] = -v[1] - v[2];
863 out[1] = v[0];
864 out[2] = v[0];
865 break;
866 case 1:
867 out[0] = v[1];
868 out[1] = -v[0] - v[2];
869 out[2] = v[1];
870 break;
871 case 2:
872 out[0] = v[2];
873 out[1] = v[2];
874 out[2] = -v[0] - v[1];
875 break;
876 }
877 }
878
879 /**
880 * no brainer compared to v3, just have for consistency.
881 */
ortho_v2_v2(float out[2],const float v[2])882 void ortho_v2_v2(float out[2], const float v[2])
883 {
884 BLI_assert(out != v);
885
886 out[0] = -v[1];
887 out[1] = v[0];
888 }
889
890 /**
891 * Rotate a point \a p by \a angle around origin (0, 0)
892 */
rotate_v2_v2fl(float r[2],const float p[2],const float angle)893 void rotate_v2_v2fl(float r[2], const float p[2], const float angle)
894 {
895 const float co = cosf(angle);
896 const float si = sinf(angle);
897
898 BLI_assert(r != p);
899
900 r[0] = co * p[0] - si * p[1];
901 r[1] = si * p[0] + co * p[1];
902 }
903
904 /**
905 * Rotate a point \a p by \a angle around an arbitrary unit length \a axis.
906 * http://local.wasp.uwa.edu.au/~pbourke/geometry/
907 */
rotate_normalized_v3_v3v3fl(float out[3],const float p[3],const float axis[3],const float angle)908 void rotate_normalized_v3_v3v3fl(float out[3],
909 const float p[3],
910 const float axis[3],
911 const float angle)
912 {
913 const float costheta = cosf(angle);
914 const float sintheta = sinf(angle);
915
916 /* double check they are normalized */
917 BLI_ASSERT_UNIT_V3(axis);
918
919 out[0] = ((costheta + (1 - costheta) * axis[0] * axis[0]) * p[0]) +
920 (((1 - costheta) * axis[0] * axis[1] - axis[2] * sintheta) * p[1]) +
921 (((1 - costheta) * axis[0] * axis[2] + axis[1] * sintheta) * p[2]);
922
923 out[1] = (((1 - costheta) * axis[0] * axis[1] + axis[2] * sintheta) * p[0]) +
924 ((costheta + (1 - costheta) * axis[1] * axis[1]) * p[1]) +
925 (((1 - costheta) * axis[1] * axis[2] - axis[0] * sintheta) * p[2]);
926
927 out[2] = (((1 - costheta) * axis[0] * axis[2] - axis[1] * sintheta) * p[0]) +
928 (((1 - costheta) * axis[1] * axis[2] + axis[0] * sintheta) * p[1]) +
929 ((costheta + (1 - costheta) * axis[2] * axis[2]) * p[2]);
930 }
931
rotate_v3_v3v3fl(float r[3],const float p[3],const float axis[3],const float angle)932 void rotate_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle)
933 {
934 BLI_assert(r != p);
935
936 float axis_n[3];
937
938 normalize_v3_v3(axis_n, axis);
939
940 rotate_normalized_v3_v3v3fl(r, p, axis_n, angle);
941 }
942
943 /*********************************** Other ***********************************/
944
print_v2(const char * str,const float v[2])945 void print_v2(const char *str, const float v[2])
946 {
947 printf("%s: %.8f %.8f\n", str, v[0], v[1]);
948 }
949
print_v3(const char * str,const float v[3])950 void print_v3(const char *str, const float v[3])
951 {
952 printf("%s: %.8f %.8f %.8f\n", str, v[0], v[1], v[2]);
953 }
954
print_v4(const char * str,const float v[4])955 void print_v4(const char *str, const float v[4])
956 {
957 printf("%s: %.8f %.8f %.8f %.8f\n", str, v[0], v[1], v[2], v[3]);
958 }
959
print_vn(const char * str,const float v[],const int n)960 void print_vn(const char *str, const float v[], const int n)
961 {
962 int i = 0;
963 printf("%s[%d]:", str, n);
964 while (i < n) {
965 printf(" %.8f", v[i++]);
966 }
967 printf("\n");
968 }
969
minmax_v4v4_v4(float min[4],float max[4],const float vec[4])970 void minmax_v4v4_v4(float min[4], float max[4], const float vec[4])
971 {
972 if (min[0] > vec[0]) {
973 min[0] = vec[0];
974 }
975 if (min[1] > vec[1]) {
976 min[1] = vec[1];
977 }
978 if (min[2] > vec[2]) {
979 min[2] = vec[2];
980 }
981 if (min[3] > vec[3]) {
982 min[3] = vec[3];
983 }
984
985 if (max[0] < vec[0]) {
986 max[0] = vec[0];
987 }
988 if (max[1] < vec[1]) {
989 max[1] = vec[1];
990 }
991 if (max[2] < vec[2]) {
992 max[2] = vec[2];
993 }
994 if (max[3] < vec[3]) {
995 max[3] = vec[3];
996 }
997 }
998
minmax_v3v3_v3(float min[3],float max[3],const float vec[3])999 void minmax_v3v3_v3(float min[3], float max[3], const float vec[3])
1000 {
1001 if (min[0] > vec[0]) {
1002 min[0] = vec[0];
1003 }
1004 if (min[1] > vec[1]) {
1005 min[1] = vec[1];
1006 }
1007 if (min[2] > vec[2]) {
1008 min[2] = vec[2];
1009 }
1010
1011 if (max[0] < vec[0]) {
1012 max[0] = vec[0];
1013 }
1014 if (max[1] < vec[1]) {
1015 max[1] = vec[1];
1016 }
1017 if (max[2] < vec[2]) {
1018 max[2] = vec[2];
1019 }
1020 }
1021
minmax_v2v2_v2(float min[2],float max[2],const float vec[2])1022 void minmax_v2v2_v2(float min[2], float max[2], const float vec[2])
1023 {
1024 if (min[0] > vec[0]) {
1025 min[0] = vec[0];
1026 }
1027 if (min[1] > vec[1]) {
1028 min[1] = vec[1];
1029 }
1030
1031 if (max[0] < vec[0]) {
1032 max[0] = vec[0];
1033 }
1034 if (max[1] < vec[1]) {
1035 max[1] = vec[1];
1036 }
1037 }
1038
minmax_v3v3_v3_array(float r_min[3],float r_max[3],const float (* vec_arr)[3],int nbr)1039 void minmax_v3v3_v3_array(float r_min[3], float r_max[3], const float (*vec_arr)[3], int nbr)
1040 {
1041 while (nbr--) {
1042 minmax_v3v3_v3(r_min, r_max, *vec_arr++);
1043 }
1044 }
1045
1046 /** ensure \a v1 is \a dist from \a v2 */
dist_ensure_v3_v3fl(float v1[3],const float v2[3],const float dist)1047 void dist_ensure_v3_v3fl(float v1[3], const float v2[3], const float dist)
1048 {
1049 if (!equals_v3v3(v2, v1)) {
1050 float nor[3];
1051
1052 sub_v3_v3v3(nor, v1, v2);
1053 normalize_v3(nor);
1054 madd_v3_v3v3fl(v1, v2, nor, dist);
1055 }
1056 }
1057
dist_ensure_v2_v2fl(float v1[2],const float v2[2],const float dist)1058 void dist_ensure_v2_v2fl(float v1[2], const float v2[2], const float dist)
1059 {
1060 if (!equals_v2v2(v2, v1)) {
1061 float nor[2];
1062
1063 sub_v2_v2v2(nor, v1, v2);
1064 normalize_v2(nor);
1065 madd_v2_v2v2fl(v1, v2, nor, dist);
1066 }
1067 }
1068
axis_sort_v3(const float axis_values[3],int r_axis_order[3])1069 void axis_sort_v3(const float axis_values[3], int r_axis_order[3])
1070 {
1071 float v[3];
1072 copy_v3_v3(v, axis_values);
1073
1074 #define SWAP_AXIS(a, b) \
1075 { \
1076 SWAP(float, v[a], v[b]); \
1077 SWAP(int, r_axis_order[a], r_axis_order[b]); \
1078 } \
1079 (void)0
1080
1081 if (v[0] < v[1]) {
1082 if (v[2] < v[0]) {
1083 SWAP_AXIS(0, 2);
1084 }
1085 }
1086 else {
1087 if (v[1] < v[2]) {
1088 SWAP_AXIS(0, 1);
1089 }
1090 else {
1091 SWAP_AXIS(0, 2);
1092 }
1093 }
1094 if (v[2] < v[1]) {
1095 SWAP_AXIS(1, 2);
1096 }
1097
1098 #undef SWAP_AXIS
1099 }
1100
1101 /***************************** Array Functions *******************************/
1102
sqr_db(double f)1103 MINLINE double sqr_db(double f)
1104 {
1105 return f * f;
1106 }
1107
dot_vn_vn(const float * array_src_a,const float * array_src_b,const int size)1108 double dot_vn_vn(const float *array_src_a, const float *array_src_b, const int size)
1109 {
1110 double d = 0.0f;
1111 const float *array_pt_a = array_src_a + (size - 1);
1112 const float *array_pt_b = array_src_b + (size - 1);
1113 int i = size;
1114 while (i--) {
1115 d += (double)(*(array_pt_a--) * *(array_pt_b--));
1116 }
1117 return d;
1118 }
1119
len_squared_vn(const float * array,const int size)1120 double len_squared_vn(const float *array, const int size)
1121 {
1122 double d = 0.0f;
1123 const float *array_pt = array + (size - 1);
1124 int i = size;
1125 while (i--) {
1126 d += sqr_db((double)(*(array_pt--)));
1127 }
1128 return d;
1129 }
1130
normalize_vn_vn(float * array_tar,const float * array_src,const int size)1131 float normalize_vn_vn(float *array_tar, const float *array_src, const int size)
1132 {
1133 const double d = len_squared_vn(array_src, size);
1134 float d_sqrt;
1135 if (d > 1.0e-35) {
1136 d_sqrt = (float)sqrt(d);
1137 mul_vn_vn_fl(array_tar, array_src, size, 1.0f / d_sqrt);
1138 }
1139 else {
1140 copy_vn_fl(array_tar, size, 0.0f);
1141 d_sqrt = 0.0f;
1142 }
1143 return d_sqrt;
1144 }
1145
normalize_vn(float * array_tar,const int size)1146 float normalize_vn(float *array_tar, const int size)
1147 {
1148 return normalize_vn_vn(array_tar, array_tar, size);
1149 }
1150
range_vn_i(int * array_tar,const int size,const int start)1151 void range_vn_i(int *array_tar, const int size, const int start)
1152 {
1153 int *array_pt = array_tar + (size - 1);
1154 int j = start + (size - 1);
1155 int i = size;
1156 while (i--) {
1157 *(array_pt--) = j--;
1158 }
1159 }
1160
range_vn_u(uint * array_tar,const int size,const uint start)1161 void range_vn_u(uint *array_tar, const int size, const uint start)
1162 {
1163 uint *array_pt = array_tar + (size - 1);
1164 uint j = start + (uint)(size - 1);
1165 int i = size;
1166 while (i--) {
1167 *(array_pt--) = j--;
1168 }
1169 }
1170
range_vn_fl(float * array_tar,const int size,const float start,const float step)1171 void range_vn_fl(float *array_tar, const int size, const float start, const float step)
1172 {
1173 float *array_pt = array_tar + (size - 1);
1174 int i = size;
1175 while (i--) {
1176 *(array_pt--) = start + step * (float)(i);
1177 }
1178 }
1179
negate_vn(float * array_tar,const int size)1180 void negate_vn(float *array_tar, const int size)
1181 {
1182 float *array_pt = array_tar + (size - 1);
1183 int i = size;
1184 while (i--) {
1185 *(array_pt--) *= -1.0f;
1186 }
1187 }
1188
negate_vn_vn(float * array_tar,const float * array_src,const int size)1189 void negate_vn_vn(float *array_tar, const float *array_src, const int size)
1190 {
1191 float *tar = array_tar + (size - 1);
1192 const float *src = array_src + (size - 1);
1193 int i = size;
1194 while (i--) {
1195 *(tar--) = -*(src--);
1196 }
1197 }
1198
mul_vn_vn(float * array_tar,const float * array_src,const int size)1199 void mul_vn_vn(float *array_tar, const float *array_src, const int size)
1200 {
1201 float *tar = array_tar + (size - 1);
1202 const float *src = array_src + (size - 1);
1203 int i = size;
1204 while (i--) {
1205 *(tar--) *= *(src--);
1206 }
1207 }
1208
mul_vn_vnvn(float * array_tar,const float * array_src_a,const float * array_src_b,const int size)1209 void mul_vn_vnvn(float *array_tar,
1210 const float *array_src_a,
1211 const float *array_src_b,
1212 const int size)
1213 {
1214 float *tar = array_tar + (size - 1);
1215 const float *src_a = array_src_a + (size - 1);
1216 const float *src_b = array_src_b + (size - 1);
1217 int i = size;
1218 while (i--) {
1219 *(tar--) = *(src_a--) * *(src_b--);
1220 }
1221 }
1222
mul_vn_fl(float * array_tar,const int size,const float f)1223 void mul_vn_fl(float *array_tar, const int size, const float f)
1224 {
1225 float *array_pt = array_tar + (size - 1);
1226 int i = size;
1227 while (i--) {
1228 *(array_pt--) *= f;
1229 }
1230 }
1231
mul_vn_vn_fl(float * array_tar,const float * array_src,const int size,const float f)1232 void mul_vn_vn_fl(float *array_tar, const float *array_src, const int size, const float f)
1233 {
1234 float *tar = array_tar + (size - 1);
1235 const float *src = array_src + (size - 1);
1236 int i = size;
1237 while (i--) {
1238 *(tar--) = *(src--) * f;
1239 }
1240 }
1241
add_vn_vn(float * array_tar,const float * array_src,const int size)1242 void add_vn_vn(float *array_tar, const float *array_src, const int size)
1243 {
1244 float *tar = array_tar + (size - 1);
1245 const float *src = array_src + (size - 1);
1246 int i = size;
1247 while (i--) {
1248 *(tar--) += *(src--);
1249 }
1250 }
1251
add_vn_vnvn(float * array_tar,const float * array_src_a,const float * array_src_b,const int size)1252 void add_vn_vnvn(float *array_tar,
1253 const float *array_src_a,
1254 const float *array_src_b,
1255 const int size)
1256 {
1257 float *tar = array_tar + (size - 1);
1258 const float *src_a = array_src_a + (size - 1);
1259 const float *src_b = array_src_b + (size - 1);
1260 int i = size;
1261 while (i--) {
1262 *(tar--) = *(src_a--) + *(src_b--);
1263 }
1264 }
1265
madd_vn_vn(float * array_tar,const float * array_src,const float f,const int size)1266 void madd_vn_vn(float *array_tar, const float *array_src, const float f, const int size)
1267 {
1268 float *tar = array_tar + (size - 1);
1269 const float *src = array_src + (size - 1);
1270 int i = size;
1271 while (i--) {
1272 *(tar--) += *(src--) * f;
1273 }
1274 }
1275
madd_vn_vnvn(float * array_tar,const float * array_src_a,const float * array_src_b,const float f,const int size)1276 void madd_vn_vnvn(float *array_tar,
1277 const float *array_src_a,
1278 const float *array_src_b,
1279 const float f,
1280 const int size)
1281 {
1282 float *tar = array_tar + (size - 1);
1283 const float *src_a = array_src_a + (size - 1);
1284 const float *src_b = array_src_b + (size - 1);
1285 int i = size;
1286 while (i--) {
1287 *(tar--) = *(src_a--) + (*(src_b--) * f);
1288 }
1289 }
1290
sub_vn_vn(float * array_tar,const float * array_src,const int size)1291 void sub_vn_vn(float *array_tar, const float *array_src, const int size)
1292 {
1293 float *tar = array_tar + (size - 1);
1294 const float *src = array_src + (size - 1);
1295 int i = size;
1296 while (i--) {
1297 *(tar--) -= *(src--);
1298 }
1299 }
1300
sub_vn_vnvn(float * array_tar,const float * array_src_a,const float * array_src_b,const int size)1301 void sub_vn_vnvn(float *array_tar,
1302 const float *array_src_a,
1303 const float *array_src_b,
1304 const int size)
1305 {
1306 float *tar = array_tar + (size - 1);
1307 const float *src_a = array_src_a + (size - 1);
1308 const float *src_b = array_src_b + (size - 1);
1309 int i = size;
1310 while (i--) {
1311 *(tar--) = *(src_a--) - *(src_b--);
1312 }
1313 }
1314
msub_vn_vn(float * array_tar,const float * array_src,const float f,const int size)1315 void msub_vn_vn(float *array_tar, const float *array_src, const float f, const int size)
1316 {
1317 float *tar = array_tar + (size - 1);
1318 const float *src = array_src + (size - 1);
1319 int i = size;
1320 while (i--) {
1321 *(tar--) -= *(src--) * f;
1322 }
1323 }
1324
msub_vn_vnvn(float * array_tar,const float * array_src_a,const float * array_src_b,const float f,const int size)1325 void msub_vn_vnvn(float *array_tar,
1326 const float *array_src_a,
1327 const float *array_src_b,
1328 const float f,
1329 const int size)
1330 {
1331 float *tar = array_tar + (size - 1);
1332 const float *src_a = array_src_a + (size - 1);
1333 const float *src_b = array_src_b + (size - 1);
1334 int i = size;
1335 while (i--) {
1336 *(tar--) = *(src_a--) - (*(src_b--) * f);
1337 }
1338 }
1339
interp_vn_vn(float * array_tar,const float * array_src,const float t,const int size)1340 void interp_vn_vn(float *array_tar, const float *array_src, const float t, const int size)
1341 {
1342 const float s = 1.0f - t;
1343 float *tar = array_tar + (size - 1);
1344 const float *src = array_src + (size - 1);
1345 int i = size;
1346 while (i--) {
1347 *(tar) = (s * *(tar)) + (t * *(src));
1348 tar--;
1349 src--;
1350 }
1351 }
1352
copy_vn_i(int * array_tar,const int size,const int val)1353 void copy_vn_i(int *array_tar, const int size, const int val)
1354 {
1355 int *tar = array_tar + (size - 1);
1356 int i = size;
1357 while (i--) {
1358 *(tar--) = val;
1359 }
1360 }
1361
copy_vn_short(short * array_tar,const int size,const short val)1362 void copy_vn_short(short *array_tar, const int size, const short val)
1363 {
1364 short *tar = array_tar + (size - 1);
1365 int i = size;
1366 while (i--) {
1367 *(tar--) = val;
1368 }
1369 }
1370
copy_vn_ushort(ushort * array_tar,const int size,const ushort val)1371 void copy_vn_ushort(ushort *array_tar, const int size, const ushort val)
1372 {
1373 ushort *tar = array_tar + (size - 1);
1374 int i = size;
1375 while (i--) {
1376 *(tar--) = val;
1377 }
1378 }
1379
copy_vn_uchar(uchar * array_tar,const int size,const uchar val)1380 void copy_vn_uchar(uchar *array_tar, const int size, const uchar val)
1381 {
1382 uchar *tar = array_tar + (size - 1);
1383 int i = size;
1384 while (i--) {
1385 *(tar--) = val;
1386 }
1387 }
1388
copy_vn_fl(float * array_tar,const int size,const float val)1389 void copy_vn_fl(float *array_tar, const int size, const float val)
1390 {
1391 float *tar = array_tar + (size - 1);
1392 int i = size;
1393 while (i--) {
1394 *(tar--) = val;
1395 }
1396 }
1397
1398 /** \name Double precision versions 'db'.
1399 * \{ */
1400
add_vn_vn_d(double * array_tar,const double * array_src,const int size)1401 void add_vn_vn_d(double *array_tar, const double *array_src, const int size)
1402 {
1403 double *tar = array_tar + (size - 1);
1404 const double *src = array_src + (size - 1);
1405 int i = size;
1406 while (i--) {
1407 *(tar--) += *(src--);
1408 }
1409 }
1410
add_vn_vnvn_d(double * array_tar,const double * array_src_a,const double * array_src_b,const int size)1411 void add_vn_vnvn_d(double *array_tar,
1412 const double *array_src_a,
1413 const double *array_src_b,
1414 const int size)
1415 {
1416 double *tar = array_tar + (size - 1);
1417 const double *src_a = array_src_a + (size - 1);
1418 const double *src_b = array_src_b + (size - 1);
1419 int i = size;
1420 while (i--) {
1421 *(tar--) = *(src_a--) + *(src_b--);
1422 }
1423 }
1424
mul_vn_db(double * array_tar,const int size,const double f)1425 void mul_vn_db(double *array_tar, const int size, const double f)
1426 {
1427 double *array_pt = array_tar + (size - 1);
1428 int i = size;
1429 while (i--) {
1430 *(array_pt--) *= f;
1431 }
1432 }
1433
interp_v3_v3v3_db(double target[3],const double a[3],const double b[3],const double t)1434 void interp_v3_v3v3_db(double target[3], const double a[3], const double b[3], const double t)
1435 {
1436 const double s = 1.0f - t;
1437
1438 target[0] = s * a[0] + t * b[0];
1439 target[1] = s * a[1] + t * b[1];
1440 target[2] = s * a[2] + t * b[2];
1441 }
1442
interp_v2_v2v2_db(double target[2],const double a[2],const double b[2],const double t)1443 void interp_v2_v2v2_db(double target[2], const double a[2], const double b[2], const double t)
1444 {
1445 const double s = 1.0f - t;
1446
1447 target[0] = s * a[0] + t * b[0];
1448 target[1] = s * a[1] + t * b[1];
1449 }
1450
1451 /** \} */
1452