1 // Copyright 2013, Fredrik Hultin.
2 // Copyright 2013, Jakob Bornecrantz.
3 // SPDX-License-Identifier: BSL-1.0
4 /*
5 * OpenHMD - Free and Open Source API and drivers for immersive technology.
6 */
7
8 /* Math Code Implementation */
9
10
11 #include <string.h>
12 #include "openhmdi.h"
13
14 // vector
15
ovec3f_get_length(const vec3f * me)16 float ovec3f_get_length(const vec3f* me)
17 {
18 return sqrtf(POW2(me->x) + POW2(me->y) + POW2(me->z));
19 }
20
ovec3f_normalize_me(vec3f * me)21 void ovec3f_normalize_me(vec3f* me)
22 {
23 if(me->x == 0 && me->y == 0 && me->z == 0)
24 return;
25
26 float len = ovec3f_get_length(me);
27 me->x /= len;
28 me->y /= len;
29 me->z /= len;
30 }
31
ovec3f_subtract(const vec3f * a,const vec3f * b,vec3f * out)32 void ovec3f_subtract(const vec3f* a, const vec3f* b, vec3f* out)
33 {
34 for(int i = 0; i < 3; i++)
35 out->arr[i] = a->arr[i] - b->arr[i];
36 }
37
ovec3f_get_dot(const vec3f * me,const vec3f * vec)38 float ovec3f_get_dot(const vec3f* me, const vec3f* vec)
39 {
40 return me->x * vec->x + me->y * vec->y + me->z * vec->z;
41 }
42
ovec3f_get_angle(const vec3f * me,const vec3f * vec)43 float ovec3f_get_angle(const vec3f* me, const vec3f* vec)
44 {
45 float dot = ovec3f_get_dot(me, vec);
46 float lengths = ovec3f_get_length(me) * ovec3f_get_length(vec);
47
48 if(lengths == 0)
49 return 0;
50
51 return acosf(dot / lengths);
52 }
53
54
55 // quaternion
56
oquatf_init_axis(quatf * me,const vec3f * vec,float angle)57 void oquatf_init_axis(quatf* me, const vec3f* vec, float angle)
58 {
59 vec3f norm = *vec;
60 ovec3f_normalize_me(&norm);
61
62 me->x = norm.x * sinf(angle / 2.0f);
63 me->y = norm.y * sinf(angle / 2.0f);
64 me->z = norm.z * sinf(angle / 2.0f);
65 me->w = cosf(angle / 2.0f);
66 }
67
oquatf_get_rotated(const quatf * me,const vec3f * vec,vec3f * out_vec)68 void oquatf_get_rotated(const quatf* me, const vec3f* vec, vec3f* out_vec)
69 {
70 quatf q = {{vec->x * me->w + vec->z * me->y - vec->y * me->z,
71 vec->y * me->w + vec->x * me->z - vec->z * me->x,
72 vec->z * me->w + vec->y * me->x - vec->x * me->y,
73 vec->x * me->x + vec->y * me->y + vec->z * me->z}};
74
75 out_vec->x = me->w * q.x + me->x * q.w + me->y * q.z - me->z * q.y;
76 out_vec->y = me->w * q.y + me->y * q.w + me->z * q.x - me->x * q.z;
77 out_vec->z = me->w * q.z + me->z * q.w + me->x * q.y - me->y * q.x;
78 }
79
oquatf_mult(const quatf * me,const quatf * q,quatf * out_q)80 void oquatf_mult(const quatf* me, const quatf* q, quatf* out_q)
81 {
82 out_q->x = me->w * q->x + me->x * q->w + me->y * q->z - me->z * q->y;
83 out_q->y = me->w * q->y - me->x * q->z + me->y * q->w + me->z * q->x;
84 out_q->z = me->w * q->z + me->x * q->y - me->y * q->x + me->z * q->w;
85 out_q->w = me->w * q->w - me->x * q->x - me->y * q->y - me->z * q->z;
86 }
87
oquatf_mult_me(quatf * me,const quatf * q)88 void oquatf_mult_me(quatf* me, const quatf* q)
89 {
90 quatf tmp = *me;
91 oquatf_mult(&tmp, q, me);
92 }
93
oquatf_normalize_me(quatf * me)94 void oquatf_normalize_me(quatf* me)
95 {
96 float len = oquatf_get_length(me);
97 me->x /= len;
98 me->y /= len;
99 me->z /= len;
100 me->w /= len;
101 }
102
oquatf_get_length(const quatf * me)103 float oquatf_get_length(const quatf* me)
104 {
105 return sqrtf(me->x * me->x + me->y * me->y + me->z * me->z + me->w * me->w);
106 }
107
oquatf_get_dot(const quatf * me,const quatf * q)108 float oquatf_get_dot(const quatf* me, const quatf* q)
109 {
110 return me->x * q->x + me->y * q->y + me->z * q->z + me->w * q->w;
111 }
112
oquatf_inverse(quatf * me)113 void oquatf_inverse(quatf* me)
114 {
115 float dot = oquatf_get_dot(me, me);
116
117 // conjugate
118 for(int i = 0; i < 3; i++)
119 me->arr[i] = -me->arr[i];
120
121 for(int i = 0; i < 4; i++)
122 me->arr[i] /= dot;
123 }
124
oquatf_diff(const quatf * me,const quatf * q,quatf * out_q)125 void oquatf_diff(const quatf* me, const quatf* q, quatf* out_q)
126 {
127 quatf inv = *me;
128 oquatf_inverse(&inv);
129 oquatf_mult(&inv, q, out_q);
130 }
131
oquatf_slerp(float fT,const quatf * rkP,const quatf * rkQ,bool shortestPath,quatf * out_q)132 void oquatf_slerp (float fT, const quatf* rkP, const quatf* rkQ, bool shortestPath, quatf* out_q)
133 {
134 float fCos = oquatf_get_dot(rkP, rkQ);
135 quatf rkT;
136
137 // Do we need to invert rotation?
138 if (fCos < 0.0f && shortestPath)
139 {
140 fCos = -fCos;
141 rkT = *rkQ;
142 oquatf_inverse(&rkT);
143 }
144 else
145 {
146 rkT = *rkQ;
147 }
148
149 if (fabsf(fCos) < 1 - 0.001f)
150 {
151 // Standard case (slerp)
152 float fSin = sqrtf(1 - (fCos*fCos));
153 float fAngle = atan2f(fSin, fCos);
154 float fInvSin = 1.0f / fSin;
155 float fCoeff0 = sin((1.0f - fT) * fAngle) * fInvSin;
156 float fCoeff1 = sin(fT * fAngle) * fInvSin;
157
158 out_q->x = fCoeff0 * rkP->x + fCoeff1 * rkT.x;
159 out_q->y = fCoeff0 * rkP->y + fCoeff1 * rkT.y;
160 out_q->z = fCoeff0 * rkP->z + fCoeff1 * rkT.z;
161 out_q->w = fCoeff0 * rkP->w + fCoeff1 * rkT.w;
162
163 //return fCoeff0 * rkP + fCoeff1 * rkT;
164 }
165 else
166 {
167 // There are two situations:
168 // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
169 // interpolation safely.
170 // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
171 // are an infinite number of possibilities interpolation. but we haven't
172 // have method to fix this case, so just use linear interpolation here.
173 //Quaternion t = (1.0f - fT) * rkP + fT * rkT;
174
175 out_q->x = (1.0f - fT) * rkP->x + fT * rkT.x;
176 out_q->y = (1.0f - fT) * rkP->y + fT * rkT.y;
177 out_q->z = (1.0f - fT) * rkP->z + fT * rkT.z;
178 out_q->w = (1.0f - fT) * rkP->w + fT * rkT.w;
179
180 oquatf_normalize_me(out_q);
181
182 // taking the complement requires renormalisation
183 //t.normalise();
184 //return t;
185 }
186 }
187
oquatf_get_mat4x4(const quatf * me,const vec3f * point,float mat[4][4])188 void oquatf_get_mat4x4(const quatf* me, const vec3f* point, float mat[4][4])
189 {
190 mat[0][0] = 1 - 2 * me->y * me->y - 2 * me->z * me->z;
191 mat[0][1] = 2 * me->x * me->y - 2 * me->w * me->z;
192 mat[0][2] = 2 * me->x * me->z + 2 * me->w * me->y;
193 mat[0][3] = point->x;
194
195 mat[1][0] = 2 * me->x * me->y + 2 * me->w * me->z;
196 mat[1][1] = 1 - 2 * me->x * me->x - 2 * me->z * me->z;
197 mat[1][2] = 2 * me->y * me->z - 2 * me->w * me->x;
198 mat[1][3] = point->y;
199
200 mat[2][0] = 2 * me->x * me->z - 2 * me->w * me->y;
201 mat[2][1] = 2 * me->y * me->z + 2 * me->w * me->x;
202 mat[2][2] = 1 - 2 * me->x * me->x - 2 * me->y * me->y;
203 mat[2][3] = point->z;
204
205 mat[3][0] = 0;
206 mat[3][1] = 0;
207 mat[3][2] = 0;
208 mat[3][3] = 1;
209 }
210
211
212 // matrix
213
omat4x4f_init_ident(mat4x4f * me)214 void omat4x4f_init_ident(mat4x4f* me)
215 {
216 memset(me, 0, sizeof(*me));
217 me->m[0][0] = 1.0f;
218 me->m[1][1] = 1.0f;
219 me->m[2][2] = 1.0f;
220 me->m[3][3] = 1.0f;
221 }
222
omat4x4f_init_perspective(mat4x4f * me,float fovy_rad,float aspect,float znear,float zfar)223 void omat4x4f_init_perspective(mat4x4f* me, float fovy_rad, float aspect, float znear, float zfar)
224 {
225 float sine, cotangent, delta, half_fov;
226
227 half_fov = fovy_rad / 2.0f;
228 delta = zfar - znear;
229 sine = sinf(half_fov);
230
231 if ((delta == 0.0f) || (sine == 0.0f) || (aspect == 0.0f)) {
232 omat4x4f_init_ident(me);
233 return;
234 }
235
236 cotangent = cosf(half_fov) / sine;
237
238 me->m[0][0] = cotangent / aspect;
239 me->m[0][1] = 0;
240 me->m[0][2] = 0;
241 me->m[0][3] = 0;
242
243 me->m[1][0] = 0;
244 me->m[1][1] = cotangent;
245 me->m[1][2] = 0;
246 me->m[1][3] = 0;
247
248 me->m[2][0] = 0;
249 me->m[2][1] = 0;
250 me->m[2][2] = -(zfar + znear) / delta;
251 me->m[2][3] = -2.0f * znear * zfar / delta;
252
253 me->m[3][0] = 0;
254 me->m[3][1] = 0;
255 me->m[3][2] = -1.0f;
256 me->m[3][3] = 0;
257 }
258
omat4x4f_init_frustum(mat4x4f * me,float left,float right,float bottom,float top,float znear,float zfar)259 void omat4x4f_init_frustum(mat4x4f* me, float left, float right, float bottom, float top, float znear, float zfar)
260 {
261 omat4x4f_init_ident(me);
262
263 float delta_x = right - left;
264 float delta_y = top - bottom;
265 float delta_z = zfar - znear;
266 if ((delta_x == 0.0f) || (delta_y == 0.0f) || (delta_z == 0.0f)) {
267 /* can't divide by zero, so just give back identity */
268 return;
269 }
270
271 me->m[0][0] = 2.0f * znear / delta_x;
272 me->m[0][1] = 0;
273 me->m[0][2] = (right + left) / delta_x;
274 me->m[0][3] = 0;
275
276 me->m[1][0] = 0;
277 me->m[1][1] = 2.0f * znear / delta_y;
278 me->m[1][2] = (top + bottom) / delta_y;
279 me->m[1][3] = 0;
280
281 me->m[2][0] = 0;
282 me->m[2][1] = 0;
283 me->m[2][2] = -(zfar + znear) / delta_z;
284 me->m[2][3] = -2.0f * zfar * znear / delta_z;
285
286 me->m[3][0] = 0;
287 me->m[3][1] = 0;
288 me->m[3][2] = -1.0f;
289 me->m[3][3] = 0;
290 }
291
omat4x4f_init_look_at(mat4x4f * me,const quatf * rot,const vec3f * eye)292 void omat4x4f_init_look_at(mat4x4f* me, const quatf* rot, const vec3f* eye)
293 {
294 quatf q;
295 vec3f p;
296
297 q.x = -rot->x;
298 q.y = -rot->y;
299 q.z = -rot->z;
300 q.w = rot->w;
301
302 p.x = -eye->x;
303 p.y = -eye->y;
304 p.z = -eye->z;
305
306 me->m[0][0] = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
307 me->m[0][1] = 2 * q.x * q.y - 2 * q.w * q.z;
308 me->m[0][2] = 2 * q.x * q.z + 2 * q.w * q.y;
309 me->m[0][3] = p.x * me->m[0][0] + p.y * me->m[0][1] + p.z * me->m[0][2];
310
311 me->m[1][0] = 2 * q.x * q.y + 2 * q.w * q.z;
312 me->m[1][1] = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
313 me->m[1][2] = 2 * q.y * q.z - 2 * q.w * q.x;
314 me->m[1][3] = p.x * me->m[1][0] + p.y * me->m[1][1] + p.z * me->m[1][2];
315
316 me->m[2][0] = 2 * q.x * q.z - 2 * q.w * q.y;
317 me->m[2][1] = 2 * q.y * q.z + 2 * q.w * q.x;
318 me->m[2][2] = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
319 me->m[2][3] = p.x * me->m[2][0] + p.y * me->m[2][1] + p.z * me->m[2][2];
320
321 me->m[3][0] = 0;
322 me->m[3][1] = 0;
323 me->m[3][2] = 0;
324 me->m[3][3] = 1;
325 }
326
omat4x4f_init_translate(mat4x4f * me,float x,float y,float z)327 void omat4x4f_init_translate(mat4x4f* me, float x, float y, float z)
328 {
329 omat4x4f_init_ident(me);
330 me->m[0][3] = x;
331 me->m[1][3] = y;
332 me->m[2][3] = z;
333 }
334
omat4x4f_transpose(const mat4x4f * m,mat4x4f * o)335 void omat4x4f_transpose(const mat4x4f* m, mat4x4f* o)
336 {
337 o->m[0][0] = m->m[0][0];
338 o->m[1][0] = m->m[0][1];
339 o->m[2][0] = m->m[0][2];
340 o->m[3][0] = m->m[0][3];
341
342 o->m[0][1] = m->m[1][0];
343 o->m[1][1] = m->m[1][1];
344 o->m[2][1] = m->m[1][2];
345 o->m[3][1] = m->m[1][3];
346
347 o->m[0][2] = m->m[2][0];
348 o->m[1][2] = m->m[2][1];
349 o->m[2][2] = m->m[2][2];
350 o->m[3][2] = m->m[2][3];
351
352 o->m[0][3] = m->m[3][0];
353 o->m[1][3] = m->m[3][1];
354 o->m[2][3] = m->m[3][2];
355 o->m[3][3] = m->m[3][3];
356 }
357
omat4x4f_mult(const mat4x4f * l,const mat4x4f * r,mat4x4f * o)358 void omat4x4f_mult(const mat4x4f* l, const mat4x4f* r, mat4x4f *o)
359 {
360 for(int i = 0; i < 4; i++){
361 float a0 = l->m[i][0], a1 = l->m[i][1], a2 = l->m[i][2], a3 = l->m[i][3];
362 o->m[i][0] = a0 * r->m[0][0] + a1 * r->m[1][0] + a2 * r->m[2][0] + a3 * r->m[3][0];
363 o->m[i][1] = a0 * r->m[0][1] + a1 * r->m[1][1] + a2 * r->m[2][1] + a3 * r->m[3][1];
364 o->m[i][2] = a0 * r->m[0][2] + a1 * r->m[1][2] + a2 * r->m[2][2] + a3 * r->m[3][2];
365 o->m[i][3] = a0 * r->m[0][3] + a1 * r->m[1][3] + a2 * r->m[2][3] + a3 * r->m[3][3];
366 }
367 }
368
369
370 // filter queue
371
ofq_init(filter_queue * me,int size)372 void ofq_init(filter_queue* me, int size)
373 {
374 memset(me, 0, sizeof(filter_queue));
375 me->size = size;
376 }
377
ofq_add(filter_queue * me,const vec3f * vec)378 void ofq_add(filter_queue* me, const vec3f* vec)
379 {
380 me->elems[me->at] = *vec;
381 me->at = ((me->at + 1) % me->size);
382 }
383
ofq_get_mean(const filter_queue * me,vec3f * vec)384 void ofq_get_mean(const filter_queue* me, vec3f* vec)
385 {
386 vec->x = vec->y = vec->z = 0;
387 for(int i = 0; i < me->size; i++){
388 vec->x += me->elems[i].x;
389 vec->y += me->elems[i].y;
390 vec->z += me->elems[i].z;
391 }
392
393 vec->x /= (float)me->size;
394 vec->y /= (float)me->size;
395 vec->z /= (float)me->size;
396 }
397