1 /*-
2 * Copyright (c) 2012-2017 Ilya Kaliman
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED. IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 */
26
27 #ifndef LIBEFP_MATH_UTIL_H
28 #define LIBEFP_MATH_UTIL_H
29
30 #include <math.h>
31 #include <string.h>
32
33 #define PI 3.14159265358979323846
34 #define EPSILON 1.0e-8
35
36 #define VEC(x) ((vec_t *)(&(x)))
37 #define CVEC(x) ((const vec_t *)(&(x)))
38
39 typedef struct {
40 double x, y, z;
41 } vec_t;
42
43 typedef struct {
44 double x, y, z, a, b, c;
45 } six_t;
46
47 typedef struct {
48 double xx, xy, xz, yx, yy, yz, zx, zy, zz;
49 } mat_t;
50
51 static const vec_t vec_zero = { 0.0, 0.0, 0.0 };
52
53 static const six_t six_zero = { 0.0, 0.0, 0.0,
54 0.0, 0.0, 0.0 };
55
56 static const mat_t mat_zero = { 0.0, 0.0, 0.0,
57 0.0, 0.0, 0.0,
58 0.0, 0.0, 0.0 };
59
60 static const mat_t mat_identity = { 1.0, 0.0, 0.0,
61 0.0, 1.0, 0.0,
62 0.0, 0.0, 1.0 };
63
64 static inline int
eq(double a,double b)65 eq(double a, double b)
66 {
67 return fabs(a - b) < EPSILON;
68 }
69
70 static inline double
vec_get(const vec_t * vec,size_t idx)71 vec_get(const vec_t *vec, size_t idx)
72 {
73 return ((const double *)vec)[idx];
74 }
75
76 static inline void
vec_set(vec_t * vec,size_t idx,double val)77 vec_set(vec_t *vec, size_t idx, double val)
78 {
79 ((double *)vec)[idx] = val;
80 }
81
82 static inline void
vec_negate(vec_t * vec)83 vec_negate(vec_t *vec)
84 {
85 vec->x = -vec->x;
86 vec->y = -vec->y;
87 vec->z = -vec->z;
88 }
89
90 static inline void
vec_scale(vec_t * vec,double s)91 vec_scale(vec_t *vec, double s)
92 {
93 vec->x *= s;
94 vec->y *= s;
95 vec->z *= s;
96 }
97
98 static inline double
vec_dot(const vec_t * a,const vec_t * b)99 vec_dot(const vec_t *a, const vec_t *b)
100 {
101 return a->x * b->x + a->y * b->y + a->z * b->z;
102 }
103
104 static inline vec_t
vec_cross(const vec_t * a,const vec_t * b)105 vec_cross(const vec_t *a, const vec_t *b)
106 {
107 vec_t c = {
108 a->y * b->z - a->z * b->y,
109 a->z * b->x - a->x * b->z,
110 a->x * b->y - a->y * b->x
111 };
112 return c;
113 }
114
115 static inline void
vec_atomic_add(vec_t * a,const vec_t * b)116 vec_atomic_add(vec_t *a, const vec_t *b)
117 {
118 #ifdef _OPENMP
119 #pragma omp atomic
120 #endif
121 a->x += b->x;
122 #ifdef _OPENMP
123 #pragma omp atomic
124 #endif
125 a->y += b->y;
126 #ifdef _OPENMP
127 #pragma omp atomic
128 #endif
129 a->z += b->z;
130 }
131
132 static inline void
vec_atomic_sub(vec_t * a,const vec_t * b)133 vec_atomic_sub(vec_t *a, const vec_t *b)
134 {
135 #ifdef _OPENMP
136 #pragma omp atomic
137 #endif
138 a->x -= b->x;
139 #ifdef _OPENMP
140 #pragma omp atomic
141 #endif
142 a->y -= b->y;
143 #ifdef _OPENMP
144 #pragma omp atomic
145 #endif
146 a->z -= b->z;
147 }
148
149 static inline void
six_atomic_add_xyz(six_t * six,const vec_t * a)150 six_atomic_add_xyz(six_t *six, const vec_t *a)
151 {
152 vec_atomic_add((vec_t *)six, a);
153 }
154
155 static inline void
six_atomic_add_abc(six_t * six,const vec_t * a)156 six_atomic_add_abc(six_t *six, const vec_t *a)
157 {
158 vec_atomic_add(((vec_t *)six) + 1, a);
159 }
160
161 static inline void
six_atomic_sub_xyz(six_t * six,const vec_t * a)162 six_atomic_sub_xyz(six_t *six, const vec_t *a)
163 {
164 vec_atomic_sub((vec_t *)six, a);
165 }
166
167 static inline void
six_atomic_sub_abc(six_t * six,const vec_t * a)168 six_atomic_sub_abc(six_t *six, const vec_t *a)
169 {
170 vec_atomic_sub(((vec_t *)six) + 1, a);
171 }
172
173 static inline vec_t
vec_add(const vec_t * a,const vec_t * b)174 vec_add(const vec_t *a, const vec_t *b)
175 {
176 vec_t c = { a->x + b->x, a->y + b->y, a->z + b->z };
177 return c;
178 }
179
180 static inline vec_t
vec_sub(const vec_t * a,const vec_t * b)181 vec_sub(const vec_t *a, const vec_t *b)
182 {
183 vec_t c = { a->x - b->x, a->y - b->y, a->z - b->z };
184 return c;
185 }
186
187 static inline double
vec_len_2(const vec_t * a)188 vec_len_2(const vec_t *a)
189 {
190 return vec_dot(a, a);
191 }
192
193 static inline double
vec_len(const vec_t * a)194 vec_len(const vec_t *a)
195 {
196 return sqrt(vec_len_2(a));
197 }
198
199 static inline void
vec_normalize(vec_t * vec)200 vec_normalize(vec_t *vec)
201 {
202 double len = vec_len(vec);
203
204 vec->x /= len;
205 vec->y /= len;
206 vec->z /= len;
207 }
208
209 static inline double
vec_dist_2(const vec_t * a,const vec_t * b)210 vec_dist_2(const vec_t *a, const vec_t *b)
211 {
212 vec_t dr = vec_sub(a, b);
213 return vec_len_2(&dr);
214 }
215
216 static inline double
vec_dist(const vec_t * a,const vec_t * b)217 vec_dist(const vec_t *a, const vec_t *b)
218 {
219 return sqrt(vec_dist_2(a, b));
220 }
221
222 static inline double
vec_angle(const vec_t * a,const vec_t * b)223 vec_angle(const vec_t *a, const vec_t *b)
224 {
225 double dot = vec_dot(a, b);
226 vec_t cross = vec_cross(a, b);
227
228 return atan2(vec_len(&cross), dot);
229 }
230
231 static inline double
mat_get(const mat_t * mat,size_t a1,size_t a2)232 mat_get(const mat_t *mat, size_t a1, size_t a2)
233 {
234 return ((const double *)mat)[3 * a1 + a2];
235 }
236
237 static inline void
mat_set(mat_t * mat,size_t a1,size_t a2,double val)238 mat_set(mat_t *mat, size_t a1, size_t a2, double val)
239 {
240 ((double *)mat)[3 * a1 + a2] = val;
241 }
242
243 static inline vec_t
mat_vec(const mat_t * mat,const vec_t * vec)244 mat_vec(const mat_t *mat, const vec_t *vec)
245 {
246 vec_t out = { mat->xx * vec->x + mat->xy * vec->y + mat->xz * vec->z,
247 mat->yx * vec->x + mat->yy * vec->y + mat->yz * vec->z,
248 mat->zx * vec->x + mat->zy * vec->y + mat->zz * vec->z };
249 return out;
250 }
251
252 /* returns mat(t) * vec */
253 static inline vec_t
mat_trans_vec(const mat_t * mat,const vec_t * vec)254 mat_trans_vec(const mat_t *mat, const vec_t *vec)
255 {
256 vec_t out = { mat->xx * vec->x + mat->yx * vec->y + mat->zx * vec->z,
257 mat->xy * vec->x + mat->yy * vec->y + mat->zy * vec->z,
258 mat->xz * vec->x + mat->yz * vec->y + mat->zz * vec->z };
259 return out;
260 }
261
262 static inline mat_t
mat_mat(const mat_t * m1,const mat_t * m2)263 mat_mat(const mat_t *m1, const mat_t *m2)
264 {
265 mat_t out = { m1->xx * m2->xx + m1->xy * m2->yx + m1->xz * m2->zx,
266 m1->xx * m2->xy + m1->xy * m2->yy + m1->xz * m2->zy,
267 m1->xx * m2->xz + m1->xy * m2->yz + m1->xz * m2->zz,
268 m1->yx * m2->xx + m1->yy * m2->yx + m1->yz * m2->zx,
269 m1->yx * m2->xy + m1->yy * m2->yy + m1->yz * m2->zy,
270 m1->yx * m2->xz + m1->yy * m2->yz + m1->yz * m2->zz,
271 m1->zx * m2->xx + m1->zy * m2->yx + m1->zz * m2->zx,
272 m1->zx * m2->xy + m1->zy * m2->yy + m1->zz * m2->zy,
273 m1->zx * m2->xz + m1->zy * m2->yz + m1->zz * m2->zz };
274 return out;
275 }
276
277 /* returns m1(t) * m2 */
278 static inline mat_t
mat_trans_mat(const mat_t * m1,const mat_t * m2)279 mat_trans_mat(const mat_t *m1, const mat_t *m2)
280 {
281 mat_t out = { m1->xx * m2->xx + m1->yx * m2->yx + m1->zx * m2->zx,
282 m1->xx * m2->xy + m1->yx * m2->yy + m1->zx * m2->zy,
283 m1->xx * m2->xz + m1->yx * m2->yz + m1->zx * m2->zz,
284 m1->xy * m2->xx + m1->yy * m2->yx + m1->zy * m2->zx,
285 m1->xy * m2->xy + m1->yy * m2->yy + m1->zy * m2->zy,
286 m1->xy * m2->xz + m1->yy * m2->yz + m1->zy * m2->zz,
287 m1->xz * m2->xx + m1->yz * m2->yx + m1->zz * m2->zx,
288 m1->xz * m2->xy + m1->yz * m2->yy + m1->zz * m2->zy,
289 m1->xz * m2->xz + m1->yz * m2->yz + m1->zz * m2->zz };
290 return out;
291 }
292
293 static inline void
mat_negate(mat_t * mat)294 mat_negate(mat_t *mat)
295 {
296 mat->xx = -mat->xx;
297 mat->xy = -mat->xy;
298 mat->xz = -mat->xz;
299 mat->yx = -mat->yx;
300 mat->yy = -mat->yy;
301 mat->yz = -mat->yz;
302 mat->zx = -mat->zx;
303 mat->zy = -mat->zy;
304 mat->zz = -mat->zz;
305 }
306
307 static inline double
mat_det(const mat_t * mat)308 mat_det(const mat_t *mat)
309 {
310 return mat->xx * mat->yy * mat->zz +
311 mat->xy * mat->zx * mat->yz +
312 mat->yx * mat->xz * mat->zy -
313 mat->xz * mat->yy * mat->zx -
314 mat->zy * mat->yz * mat->xx -
315 mat->yx * mat->xy * mat->zz;
316 }
317
318 static inline mat_t
mat_transpose(const mat_t * mat)319 mat_transpose(const mat_t *mat)
320 {
321 mat_t out = { mat->xx, mat->yx, mat->zx,
322 mat->xy, mat->yy, mat->zy,
323 mat->xz, mat->yz, mat->zz };
324 return out;
325 }
326
327 static inline mat_t
mat_inv(const mat_t * mat)328 mat_inv(const mat_t *mat)
329 {
330 double det = 1.0 / mat_det(mat);
331
332 mat_t out = { det * (mat->yy * mat->zz - mat->yz * mat->zy),
333 det * (mat->zy * mat->xz - mat->zz * mat->xy),
334 det * (mat->xy * mat->yz - mat->xz * mat->yy),
335 det * (mat->yz * mat->zx - mat->yx * mat->zz),
336 det * (mat->zz * mat->xx - mat->zx * mat->xz),
337 det * (mat->xz * mat->yx - mat->xx * mat->yz),
338 det * (mat->yx * mat->zy - mat->yy * mat->zx),
339 det * (mat->zx * mat->xy - mat->zy * mat->xx),
340 det * (mat->xx * mat->yy - mat->xy * mat->yx) };
341
342 return out;
343 }
344
345 static inline void
euler_to_matrix(double a,double b,double c,mat_t * out)346 euler_to_matrix(double a, double b, double c, mat_t *out)
347 {
348 double sina = sin(a), cosa = cos(a);
349 double sinb = sin(b), cosb = cos(b);
350 double sinc = sin(c), cosc = cos(c);
351
352 out->xx = cosa * cosc - sina * cosb * sinc;
353 out->xy = -cosa * sinc - sina * cosb * cosc;
354 out->xz = sinb * sina;
355 out->yx = sina * cosc + cosa * cosb * sinc;
356 out->yy = -sina * sinc + cosa * cosb * cosc;
357 out->yz = -sinb * cosa;
358 out->zx = sinb * sinc;
359 out->zy = sinb * cosc;
360 out->zz = cosb;
361 }
362
363 static inline void
matrix_to_euler(const mat_t * rotmat,double * ea,double * eb,double * ec)364 matrix_to_euler(const mat_t *rotmat, double *ea, double *eb, double *ec)
365 {
366 double a, b, c, sinb;
367
368 b = acos(rotmat->zz);
369 sinb = sqrt(1.0 - rotmat->zz * rotmat->zz);
370
371 if (fabs(sinb) < 1.0e-7) {
372 a = atan2(-rotmat->xy, rotmat->xx);
373 c = 0.0;
374 }
375 else {
376 a = atan2(rotmat->xz, -rotmat->yz);
377 c = atan2(rotmat->zx, rotmat->zy);
378 }
379
380 *ea = a;
381 *eb = b;
382 *ec = c;
383 }
384
385 #endif /* LIBEFP_MATH_UTIL_H */
386