1 /*
2 * Copyright (c) 2021 Kevin Townsend
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <math.h>
8 #include <errno.h>
9 #include <stdio.h>
10 #include <zsl/zsl.h>
11 #include <zsl/vectors.h>
12 #include <zsl/orientation/quaternions.h>
13
14 /**
15 * @brief Helper function to compare float values.
16 *
17 * @param a First float too compare.
18 * @param b Second float to compare.
19 * @param epsilon Allowed deviatin between first and second values.
20
21 * @return true If values are the same within the limits of epsilon.
22 * @return false If values are different to an extent greater than epsilon.
23 */
24 static bool
zsl_quat_val_is_equal(zsl_real_t a,zsl_real_t b,zsl_real_t epsilon)25 zsl_quat_val_is_equal(zsl_real_t a, zsl_real_t b, zsl_real_t epsilon)
26 {
27 zsl_real_t c;
28
29 c = a - b;
30
31 if (ZSL_ABS(c) < epsilon) {
32 return 1;
33 } else {
34 return 0;
35 }
36 }
37
zsl_quat_init(struct zsl_quat * q,enum zsl_quat_type type)38 void zsl_quat_init(struct zsl_quat *q, enum zsl_quat_type type)
39 {
40 switch (type) {
41 case ZSL_QUAT_TYPE_IDENTITY:
42 q->r = 1.0;
43 q->i = 0.0;
44 q->j = 0.0;
45 q->k = 0.0;
46 break;
47 case ZSL_QUAT_TYPE_EMPTY:
48 default:
49 q->r = 0.0;
50 q->i = 0.0;
51 q->j = 0.0;
52 q->k = 0.0;
53 break;
54 }
55 }
56
zsl_quat_magn(struct zsl_quat * q)57 zsl_real_t zsl_quat_magn(struct zsl_quat *q)
58 {
59 return ZSL_SQRT(q->r * q->r + q->i * q->i + q->j * q->j + q->k * q->k);
60 }
61
zsl_quat_to_unit(struct zsl_quat * q,struct zsl_quat * qn)62 int zsl_quat_to_unit(struct zsl_quat *q, struct zsl_quat *qn)
63 {
64 int rc = 0;
65 zsl_real_t m = zsl_quat_magn(q);
66
67 if (ZSL_ABS(m) < 1E-6) {
68 qn->r = 0.0;
69 qn->i = 0.0;
70 qn->j = 0.0;
71 qn->k = 0.0;
72 } else {
73 qn->r = q->r / m;
74 qn->i = q->i / m;
75 qn->j = q->j / m;
76 qn->k = q->k / m;
77 }
78
79 return rc;
80 }
81
zsl_quat_to_unit_d(struct zsl_quat * q)82 int zsl_quat_to_unit_d(struct zsl_quat *q)
83 {
84 return zsl_quat_to_unit(q, q);
85 }
86
zsl_quat_is_unit(struct zsl_quat * q)87 bool zsl_quat_is_unit(struct zsl_quat *q)
88 {
89 zsl_real_t unit_len;
90
91 /* Verify that sqrt(r^2+i^2+j^2+k^2) = 1.0. */
92 unit_len = ZSL_SQRT(
93 q->r * q->r +
94 q->i * q->i +
95 q->j * q->j +
96 q->k * q->k);
97
98 return zsl_quat_val_is_equal(unit_len, 1.0, 1E-6);
99 }
100
zsl_quat_scale(struct zsl_quat * q,zsl_real_t s,struct zsl_quat * qs)101 int zsl_quat_scale(struct zsl_quat *q, zsl_real_t s, struct zsl_quat *qs)
102 {
103 int rc = 0;
104
105 qs->r = q->r * s;
106 qs->i = q->i * s;
107 qs->j = q->j * s;
108 qs->k = q->k * s;
109
110 return rc;
111 }
112
zsl_quat_scale_d(struct zsl_quat * q,zsl_real_t s)113 int zsl_quat_scale_d(struct zsl_quat *q, zsl_real_t s)
114 {
115 return zsl_quat_scale(q, s, q);
116 }
117
zsl_quat_mult(struct zsl_quat * qa,struct zsl_quat * qb,struct zsl_quat * qm)118 int zsl_quat_mult(struct zsl_quat *qa, struct zsl_quat *qb,
119 struct zsl_quat *qm)
120 {
121 int rc = 0;
122
123 /* Make copies so this function can be used as a destructive one. */
124 struct zsl_quat qac;
125 struct zsl_quat qbc;
126
127 qac.r = qa->r;
128 qac.i = qa->i;
129 qac.j = qa->j;
130 qac.k = qa->k;
131
132 qbc.r = qb->r;
133 qbc.i = qb->i;
134 qbc.j = qb->j;
135 qbc.k = qb->k;
136
137 qm->i = qac.r * qbc.i + qac.i * qbc.r + qac.j * qbc.k - qac.k * qbc.j;
138 qm->j = qac.r * qbc.j - qac.i * qbc.k + qac.j * qbc.r + qac.k * qbc.i;
139 qm->k = qac.r * qbc.k + qac.i * qbc.j - qac.j * qbc.i + qac.k * qbc.r;
140 qm->r = qac.r * qbc.r - qac.i * qbc.i - qac.j * qbc.j - qac.k * qbc.k;
141
142 return rc;
143 }
144
zsl_quat_exp(struct zsl_quat * q,struct zsl_quat * qe)145 int zsl_quat_exp(struct zsl_quat *q, struct zsl_quat *qe)
146 {
147 int rc = 0;
148
149 ZSL_VECTOR_DEF(v, 3);
150 zsl_real_t vmag; /* Magnitude of v. */
151 zsl_real_t vsin; /* Sine of vm. */
152 zsl_real_t rexp; /* Exponent of q->r. */
153
154 /* Populate the XYZ vector using ijk from q. */
155 v.data[0] = q->i;
156 v.data[1] = q->j;
157 v.data[2] = q->k;
158
159 /* Calculate magnitude of v. */
160 vmag = zsl_vec_norm(&v);
161
162 /* Normalise v to unit vector. */
163 zsl_vec_to_unit(&v);
164
165 vsin = ZSL_SIN(vmag);
166 rexp = ZSL_EXP(q->r);
167
168 qe->r = ZSL_COS(vmag) * rexp;
169 qe->i = v.data[0] * vsin * rexp;
170 qe->j = v.data[1] * vsin * rexp;
171 qe->k = v.data[2] * vsin * rexp;
172
173 return rc;
174 }
175
zsl_quat_log(struct zsl_quat * q,struct zsl_quat * ql)176 int zsl_quat_log(struct zsl_quat *q, struct zsl_quat *ql)
177 {
178 int rc = 0;
179
180 ZSL_VECTOR_DEF(v, 3); /* Vector part of unit quat q. */
181 zsl_real_t qmag; /* Magnitude of q. */
182 zsl_real_t racos; /* Acos of q->r/qmag. */
183
184 /* Populate the XYZ vector using ijk from q. */
185 v.data[0] = q->i;
186 v.data[1] = q->j;
187 v.data[2] = q->k;
188
189 /* Normalise v to unit vector. */
190 zsl_vec_to_unit(&v);
191
192 /* Calculate magnitude of input quat. */
193 qmag = zsl_quat_magn(q);
194
195 racos = ZSL_ACOS(q->r / qmag);
196
197 ql->r = ZSL_LOG(qmag);
198 ql->i = v.data[0] * racos;
199 ql->j = v.data[1] * racos;
200 ql->k = v.data[2] * racos;
201
202 return rc;
203 }
204
zsl_quat_pow(struct zsl_quat * q,zsl_real_t exp,struct zsl_quat * qout)205 int zsl_quat_pow(struct zsl_quat *q, zsl_real_t exp,
206 struct zsl_quat *qout)
207 {
208 int rc = 0;
209
210 struct zsl_quat qlog;
211 struct zsl_quat qsc;
212
213 rc = zsl_quat_log(q, &qlog);
214 if (rc) {
215 goto err;
216 }
217
218 rc = zsl_quat_scale(&qlog, exp, &qsc);
219 if (rc) {
220 goto err;
221 }
222
223 rc = zsl_quat_exp(&qsc, qout);
224 if (rc) {
225 zsl_quat_init(qout, ZSL_QUAT_TYPE_EMPTY);
226 goto err;
227 }
228
229 err:
230 return rc;
231 }
232
zsl_quat_conj(struct zsl_quat * q,struct zsl_quat * qc)233 int zsl_quat_conj(struct zsl_quat *q, struct zsl_quat *qc)
234 {
235 int rc = 0;
236
237 /* TODO: Check for div by zero before running this! */
238 qc->r = q->r;
239 qc->i = q->i * -1.0;
240 qc->j = q->j * -1.0;
241 qc->k = q->k * -1.0;
242
243 return rc;
244 }
245
zsl_quat_inv(struct zsl_quat * q,struct zsl_quat * qi)246 int zsl_quat_inv(struct zsl_quat *q, struct zsl_quat *qi)
247 {
248 int rc = 0;
249 zsl_real_t m = zsl_quat_magn(q);
250
251 if (ZSL_ABS(m) < 1E-6) {
252 /* Set to all 0's. */
253 zsl_quat_init(qi, ZSL_QUAT_TYPE_EMPTY);
254 } else {
255 /* TODO: Check for div by zero before running this! */
256 m *= m;
257 qi->r = q->r / m;
258 qi->i = q->i / -m;
259 qi->j = q->j / -m;
260 qi->k = q->k / -m;
261 }
262
263 return rc;
264 }
265
zsl_quat_inv_d(struct zsl_quat * q)266 int zsl_quat_inv_d(struct zsl_quat *q)
267 {
268 return zsl_quat_inv(q, q);
269 }
270
zsl_quat_diff(struct zsl_quat * qa,struct zsl_quat * qb,struct zsl_quat * qd)271 int zsl_quat_diff(struct zsl_quat *qa, struct zsl_quat *qb,
272 struct zsl_quat *qd)
273 {
274 int rc;
275 struct zsl_quat qi;
276
277 rc = zsl_quat_inv(qa, &qi);
278 if (rc) {
279 goto err;
280 }
281
282 /* Note: order matters here!*/
283 rc = zsl_quat_mult(&qi, qb, qd);
284
285 err:
286 return rc;
287 }
288
zsl_quat_rot(struct zsl_quat * qa,struct zsl_quat * qb,struct zsl_quat * qr)289 int zsl_quat_rot(struct zsl_quat *qa, struct zsl_quat *qb, struct zsl_quat *qr)
290 {
291 int rc = 0;
292
293 #if CONFIG_ZSL_BOUNDS_CHECKS
294 if (ZSL_ABS(qb->r) > 1E-6) {
295 rc = -EINVAL;
296 goto err;
297 }
298 #endif
299
300 struct zsl_quat qm;
301 struct zsl_quat qn;
302 struct zsl_quat qi;
303
304 zsl_quat_to_unit(qa, &qn);
305 zsl_quat_mult(&qn, qb, &qm);
306 zsl_quat_inv(&qn, &qi);
307 zsl_quat_mult(&qm, &qi, qr);
308
309 #if CONFIG_ZSL_BOUNDS_CHECKS
310 err:
311 #endif
312 return rc;
313 }
314
zsl_quat_lerp(struct zsl_quat * qa,struct zsl_quat * qb,zsl_real_t t,struct zsl_quat * qi)315 int zsl_quat_lerp(struct zsl_quat *qa, struct zsl_quat *qb,
316 zsl_real_t t, struct zsl_quat *qi)
317 {
318 int rc = 0;
319
320 #if CONFIG_ZSL_BOUNDS_CHECKS
321 /* Make sure t is between 0 and 1 (included). */
322 if (t < 0.0 || t > 1.0) {
323 rc = -EINVAL;
324 goto err;
325 }
326 #endif
327
328 struct zsl_quat q1, q2;
329
330 /* Turn input quaternions into unit quaternions. */
331 struct zsl_quat qa_u;
332 struct zsl_quat qb_u;
333 zsl_quat_to_unit(qa, &qa_u);
334 zsl_quat_to_unit(qb, &qb_u);
335
336 /* Calculate intermediate quats. */
337 zsl_quat_scale(&qa_u, 1.0 - t, &q1);
338 zsl_quat_scale(&qb_u, t, &q2);
339
340 /* Final result = q1 + q2. */
341 qi->r = q1.r + q2.r;
342 qi->i = q1.i + q2.i;
343 qi->j = q1.j + q2.j;
344 qi->k = q1.k + q2.k;
345
346 /* Normalize output quaternion. */
347 zsl_quat_to_unit_d(qi);
348
349 #if CONFIG_ZSL_BOUNDS_CHECKS
350 err:
351 #endif
352 return rc;
353 }
354
zsl_quat_slerp(struct zsl_quat * qa,struct zsl_quat * qb,zsl_real_t t,struct zsl_quat * qi)355 int zsl_quat_slerp(struct zsl_quat *qa, struct zsl_quat *qb,
356 zsl_real_t t, struct zsl_quat *qi)
357 {
358 int rc = 0;
359
360 #if CONFIG_ZSL_BOUNDS_CHECKS
361 /* Make sure t is between 0 and 1 (included). */
362 if (t < 0.0 || t > 1.0) {
363 rc = -EINVAL;
364 goto err;
365 }
366 #endif
367
368 struct zsl_quat q1, q2; /* Interim quats. */
369 zsl_real_t dot; /* Dot product bewteen qa and qb. */
370 zsl_real_t phi; /* arccos(dot). */
371 zsl_real_t phi_s; /* sin(phi). */
372 zsl_real_t phi_st; /* sin(phi * (t)). */
373 zsl_real_t phi_smt; /* sin(phi * (1.0 - t)). */
374
375 /*
376 * Unit quaternion slerp = qa * (qa^-1 * qb)^t
377 *
378 * We get there in a round-about way in this code, but we avoid pushing
379 * and popping values on the stack with trivial calls to helper functions.
380 */
381
382 /* Turn input quaternions into unit quaternions. */
383 struct zsl_quat qa_u;
384 struct zsl_quat qb_u;
385 zsl_quat_to_unit(qa, &qa_u);
386 zsl_quat_to_unit(qb, &qb_u);
387
388 /* When t = 0.0 or t = 1.0, just memcpy qa or qb. */
389 if (t == 0.0) {
390 qi->r = qa_u.r;
391 qi->i = qa_u.i;
392 qi->j = qa_u.j;
393 qi->k = qa_u.k;
394 goto err;
395 } else if (t == 1.0) {
396 qi->r = qb_u.r;
397 qi->i = qb_u.i;
398 qi->j = qb_u.j;
399 qi->k = qb_u.k;
400 goto err;
401 }
402
403 /* Compute the dot product of the two normalized input quaternions. */
404 dot = qa_u.r * qb_u.r + qa_u.i * qb_u.i + qa_u.j * qb_u.j + qa_u.k * qb_u.k;
405
406 /* The value dot is always between -1 and 1. If dot = 1.0, qa = qb and there
407 * is no interpolation. */
408 if (ZSL_ABS(dot - 1.0) < 1E-6) {
409 qi->r = qa_u.r;
410 qi->i = qa_u.i;
411 qi->j = qa_u.j;
412 qi->k = qa_u.k;
413 goto err;
414 }
415
416 /* If dot = -1, then qa = - qb and the interpolation is invald. */
417 if (ZSL_ABS(dot + 1.0) < 1E-6) {
418 rc = -EINVAL;
419 goto err;
420 }
421
422 /*
423 * Slerp often has problems with angles close to zero. Consider handling
424 * those edge cases slightly differently?
425 */
426
427 /* Calculate these once before-hand. */
428 phi = ZSL_ACOS(dot);
429 phi_s = ZSL_SIN(phi);
430 phi_st = ZSL_SIN(phi * t);
431 phi_smt = ZSL_SIN(phi * (1.0 - t));
432
433 /* Calculate intermediate quats. */
434 zsl_quat_scale(&qa_u, phi_smt / phi_s, &q1);
435 zsl_quat_scale(&qb_u, phi_st / phi_s, &q2);
436
437 /* Final result = q1 + q2. */
438 qi->r = q1.r + q2.r;
439 qi->i = q1.i + q2.i;
440 qi->j = q1.j + q2.j;
441 qi->k = q1.k + q2.k;
442
443 err:
444 return rc;
445 }
446
zsl_quat_from_ang_vel(struct zsl_vec * w,struct zsl_quat * qin,zsl_real_t t,struct zsl_quat * qout)447 int zsl_quat_from_ang_vel(struct zsl_vec *w, struct zsl_quat *qin,
448 zsl_real_t t, struct zsl_quat *qout)
449 {
450 int rc = 0;
451
452 #if CONFIG_ZSL_BOUNDS_CHECKS
453 /* Make sure time is positive or zero and the angular velocity is a
454 * tridimensional vector. */
455 if (w->sz != 3 || t < 0.0) {
456 rc = -EINVAL;
457 goto err;
458 }
459 #endif
460
461 struct zsl_quat qin2;
462 struct zsl_quat qout2;
463 struct zsl_quat wq;
464 struct zsl_quat wquat = {
465 .r = 0.0,
466 .i = w->data[0],
467 .j = w->data[1],
468 .k = w->data[2]
469 };
470
471 zsl_quat_to_unit(qin, &qin2);
472 zsl_quat_mult(&wquat, &qin2, &wq);
473 zsl_quat_scale_d(&wq, 0.5 * t);
474 qout2.r = qin2.r + wq.r;
475 qout2.i = qin2.i + wq.i;
476 qout2.j = qin2.j + wq.j;
477 qout2.k = qin2.k + wq.k;
478
479 zsl_quat_to_unit(&qout2, qout);
480
481 #if CONFIG_ZSL_BOUNDS_CHECKS
482 err:
483 #endif
484 return rc;
485 }
486
zsl_quat_from_ang_mom(struct zsl_vec * l,struct zsl_quat * qin,zsl_real_t * i,zsl_real_t t,struct zsl_quat * qout)487 int zsl_quat_from_ang_mom(struct zsl_vec *l, struct zsl_quat *qin,
488 zsl_real_t *i, zsl_real_t t, struct zsl_quat *qout)
489 {
490 int rc = 0;
491
492 #if CONFIG_ZSL_BOUNDS_CHECKS
493 /* Make sure time is positive or zero and the angular velocity is a
494 * tridimensional vector. Inertia can't be negative or zero. */
495 if (l->sz != 3 || t < 0.0 || *i <= 0.0) {
496 rc = -EINVAL;
497 goto err;
498 }
499 #endif
500
501 ZSL_VECTOR_DEF(w, 3);
502 zsl_vec_copy(&w, l);
503 zsl_vec_scalar_div(&w, *i);
504 zsl_quat_from_ang_vel(&w, qin, t, qout);
505
506 #if CONFIG_ZSL_BOUNDS_CHECKS
507 err:
508 #endif
509 return rc;
510 }
511
zsl_quat_to_euler(struct zsl_quat * q,struct zsl_euler * e)512 int zsl_quat_to_euler(struct zsl_quat *q, struct zsl_euler *e)
513 {
514 int rc = 0;
515 struct zsl_quat qn;
516
517 zsl_quat_to_unit(q, &qn);
518 zsl_real_t gl = qn.i * qn.k + qn.j * qn.r;
519 zsl_real_t v = 2. * gl;
520
521 if (v > 1.0) {
522 v = 1.0;
523 } else if (v < -1.0) {
524 v = -1.0;
525 }
526
527 e->y = ZSL_ASIN(v);
528
529 /* Gimbal lock case. */
530 if (ZSL_ABS(gl - 0.5) < 1E-6 || ZSL_ABS(gl + 0.5) < 1E-6) {
531 e->x = ZSL_ATAN2(2.0 * (qn.j * qn.k + qn.i * qn.r),
532 1.0 - 2.0 * (qn.i * qn.i + qn.k * qn.k));
533 e->z = 0.0;
534 return rc;
535 }
536
537 e->x = ZSL_ATAN2(2.0 * (qn.i * qn.r - qn.j * qn.k),
538 1.0 - 2.0 * (qn.i * qn.i + qn.j * qn.j));
539 e->z = ZSL_ATAN2(2.0 * (qn.k * qn.r - qn.i * qn.j),
540 1.0 - 2.0 * (qn.j * qn.j + qn.k * qn.k));
541
542 return rc;
543 }
544
zsl_quat_from_euler(struct zsl_euler * e,struct zsl_quat * q)545 int zsl_quat_from_euler(struct zsl_euler *e, struct zsl_quat *q)
546 {
547 int rc = 0;
548
549 zsl_real_t roll_c = ZSL_COS(e->x * 0.5);
550 zsl_real_t roll_s = ZSL_SIN(e->x * 0.5);
551 zsl_real_t pitch_c = ZSL_COS(e->y * 0.5);
552 zsl_real_t pitch_s = ZSL_SIN(e->y * 0.5);
553 zsl_real_t yaw_c = ZSL_COS(e->z * 0.5);
554 zsl_real_t yaw_s = ZSL_SIN(e->z * 0.5);
555
556 q->r = roll_c * pitch_c * yaw_c - roll_s * pitch_s * yaw_s;
557 q->i = roll_s * pitch_c * yaw_c + roll_c * pitch_s * yaw_s;
558 q->j = roll_c * pitch_s * yaw_c - roll_s * pitch_c * yaw_s;
559 q->k = roll_c * pitch_c * yaw_s + roll_s * pitch_s * yaw_c;
560
561 return rc;
562 }
563
zsl_quat_to_rot_mtx(struct zsl_quat * q,struct zsl_mtx * m)564 int zsl_quat_to_rot_mtx(struct zsl_quat *q, struct zsl_mtx *m)
565 {
566 int rc = 0;
567
568 #if CONFIG_ZSL_BOUNDS_CHECKS
569 /* Make sure that the rotation matrix has an appropriate shape and size. */
570 if ((m->sz_cols != 3) || (m->sz_rows != 3)) {
571 rc = -EINVAL;
572 goto err;
573 }
574 #endif
575
576 /* Note: This can be optimised by pre-calculating shared values. */
577
578 /* Row 0. */
579 zsl_mtx_set(m, 0, 0, 1.0 - 2.0 * (q->j * q->j + q->k * q->k));
580 zsl_mtx_set(m, 0, 1, 2.0 * (q->i * q->j - q->k * q->r));
581 zsl_mtx_set(m, 0, 2, 2.0 * (q->i * q->k + q->j * q->r));
582
583 /* Row 1. */
584 zsl_mtx_set(m, 1, 0, 2.0 * (q->i * q->j + q->k * q->r));
585 zsl_mtx_set(m, 1, 1, 1.0 - 2.0 * (q->i * q->i + q->k * q->k));
586 zsl_mtx_set(m, 1, 2, 2.0 * (q->j * q->k - q->i * q->r));
587
588 /* Row 2. */
589 zsl_mtx_set(m, 2, 0, 2.0 * (q->i * q->k - q->j * q->r));
590 zsl_mtx_set(m, 2, 1, 2.0 * (q->j * q->k + q->i * q->r));
591 zsl_mtx_set(m, 2, 2, 1.0 - 2.0 * (q->i * q->i + q->j * q->j));
592
593 #if CONFIG_ZSL_BOUNDS_CHECKS
594 err:
595 #endif
596 return rc;
597 }
598
zsl_quat_from_rot_mtx(struct zsl_mtx * m,struct zsl_quat * q)599 int zsl_quat_from_rot_mtx(struct zsl_mtx *m, struct zsl_quat *q)
600 {
601 int rc = 0;
602
603 #if CONFIG_ZSL_BOUNDS_CHECKS
604 /* Make sure that the rotation matrix has an appropriate shape and size. */
605 if ((m->sz_cols != 3) || (m->sz_rows != 3)) {
606 rc = -EINVAL;
607 goto err;
608 }
609 #endif
610
611 /* Convert rotation matrix to unit quaternion. */
612 q->r = 0.5 * ZSL_SQRT(m->data[0] + m->data[4] + m->data[8] + 1.0);
613 q->i = 0.5 * ZSL_SQRT(m->data[0] - m->data[4] - m->data[8] + 1.0);
614 q->j = 0.5 * ZSL_SQRT(-m->data[0] + m->data[4] - m->data[8] + 1.0);
615 q->k = 0.5 * ZSL_SQRT(-m->data[0] - m->data[4] + m->data[8] + 1.0);
616
617 if (ZSL_ABS(m->data[7] - m->data[5]) > 1E-6) {
618 /* Multiply by the sign of m21 - m12. */
619 q->i *= (m->data[7] - m->data[5]) / ZSL_ABS(m->data[7] - m->data[5]);
620 }
621
622 if (ZSL_ABS(m->data[2] - m->data[6]) > 1E-6) {
623 /* Multiply by the sign of m02 - m20. */
624 q->j *= (m->data[2] - m->data[6]) / ZSL_ABS(m->data[2] - m->data[6]);
625 }
626
627 if (ZSL_ABS(m->data[3] - m->data[1]) > 1E-6) {
628 /* Multiply by the sign of m10 - m01. */
629 q->k *= (m->data[3] - m->data[1]) / ZSL_ABS(m->data[3] - m->data[1]);
630 }
631
632 #if CONFIG_ZSL_BOUNDS_CHECKS
633 err:
634 #endif
635 return rc;
636 }
637
zsl_quat_to_axis_angle(struct zsl_quat * q,struct zsl_vec * a,zsl_real_t * b)638 int zsl_quat_to_axis_angle(struct zsl_quat *q, struct zsl_vec *a,
639 zsl_real_t *b)
640 {
641 int rc = 0;
642
643 #if CONFIG_ZSL_BOUNDS_CHECKS
644 /* Make sure that the axis vector is size 3. */
645 if (a->sz != 3) {
646 rc = -EINVAL;
647 goto err;
648 }
649 #endif
650
651 struct zsl_quat qn;
652 zsl_quat_to_unit(q, &qn);
653
654 if (ZSL_ABS(qn.r - 1.0) < 1E-6) {
655 a->data[0] = 0.0;
656 a->data[1] = 0.0;
657 a->data[2] = 0.0;
658 *b = 0.0;
659 return 0;
660 }
661
662 zsl_real_t s = ZSL_SQRT(1.0 - (qn.r * qn.r));
663 *b = 2.0 * ZSL_ACOS(qn.r);
664 a->data[0] = qn.i / s;
665 a->data[1] = qn.j / s;
666 a->data[2] = qn.k / s;
667
668 #if CONFIG_ZSL_BOUNDS_CHECKS
669 err:
670 #endif
671 return rc;
672 }
673
zsl_quat_from_axis_angle(struct zsl_vec * a,zsl_real_t * b,struct zsl_quat * q)674 int zsl_quat_from_axis_angle(struct zsl_vec *a, zsl_real_t *b,
675 struct zsl_quat *q)
676 {
677 int rc = 0;
678
679 #if CONFIG_ZSL_BOUNDS_CHECKS
680 /* Make sure that the axis vector is size 3. */
681 if (a->sz != 3) {
682 rc = -EINVAL;
683 goto err;
684 }
685 #endif
686
687 zsl_real_t norm = ZSL_SQRT(a->data[0] * a->data[0] +
688 a->data[1] * a->data[1] +
689 a->data[2] * a->data[2]);
690
691 if (norm < 1E-6) {
692 q->r = 0.0;
693 q->i = 0.0;
694 q->j = 0.0;
695 q->k = 0.0;
696 return 0;
697 }
698
699 ZSL_VECTOR_DEF(an, 3);
700 zsl_vec_copy(&an, a);
701 zsl_vec_scalar_div(&an, norm);
702
703 q->r = ZSL_COS(*b / 2.0);
704 q->i = an.data[0] * ZSL_SIN(*b / 2.0);
705 q->j = an.data[1] * ZSL_SIN(*b / 2.0);
706 q->k = an.data[2] * ZSL_SIN(*b / 2.0);
707
708 #if CONFIG_ZSL_BOUNDS_CHECKS
709 err:
710 #endif
711 return rc;
712 }
713
zsl_quat_print(struct zsl_quat * q)714 int zsl_quat_print(struct zsl_quat *q)
715 {
716 printf("%.16f + %.16f i + %.16f j + %.16f k\n", q->r, q->i, q->j, q->k);
717
718 return 0;
719 }
720
