123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215 |
- /******************************************************************************
- Copyright (C) 2023 by Lain Bailey <lain@obsproject.com>
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 2 of the License, or
- (at your option) any later version.
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
- You should have received a copy of the GNU General Public License
- along with this program. If not, see <http://www.gnu.org/licenses/>.
- ******************************************************************************/
- #include "quat.h"
- #include "vec3.h"
- #include "matrix3.h"
- #include "matrix4.h"
- #include "axisang.h"
- static inline void quat_vec3(struct vec3 *v, const struct quat *q)
- {
- v->m = q->m;
- v->w = 0.0f;
- }
- void quat_mul(struct quat *dst, const struct quat *q1, const struct quat *q2)
- {
- struct vec3 q1axis, q2axis;
- struct vec3 temp1, temp2;
- quat_vec3(&q1axis, q1);
- quat_vec3(&q2axis, q2);
- vec3_mulf(&temp1, &q2axis, q1->w);
- vec3_mulf(&temp2, &q1axis, q2->w);
- vec3_add(&temp1, &temp1, &temp2);
- vec3_cross(&temp2, &q1axis, &q2axis);
- vec3_add((struct vec3 *)dst, &temp1, &temp2);
- dst->w = (q1->w * q2->w) - vec3_dot(&q1axis, &q2axis);
- }
- void quat_from_axisang(struct quat *dst, const struct axisang *aa)
- {
- float halfa = aa->w * 0.5f;
- float sine = sinf(halfa);
- dst->x = aa->x * sine;
- dst->y = aa->y * sine;
- dst->z = aa->z * sine;
- dst->w = cosf(halfa);
- }
- struct f4x4 {
- float ptr[4][4];
- };
- void quat_from_matrix3(struct quat *dst, const struct matrix3 *m)
- {
- quat_from_matrix4(dst, (const struct matrix4 *)m);
- }
- void quat_from_matrix4(struct quat *dst, const struct matrix4 *m)
- {
- float tr = (m->x.x + m->y.y + m->z.z);
- float inv_half;
- float four_d;
- int i, j, k;
- if (tr > 0.0f) {
- four_d = sqrtf(tr + 1.0f);
- dst->w = four_d * 0.5f;
- inv_half = 0.5f / four_d;
- dst->x = (m->y.z - m->z.y) * inv_half;
- dst->y = (m->z.x - m->x.z) * inv_half;
- dst->z = (m->x.y - m->y.x) * inv_half;
- } else {
- struct f4x4 *val = (struct f4x4 *)m;
- i = (m->x.x > m->y.y) ? 0 : 1;
- if (m->z.z > val->ptr[i][i])
- i = 2;
- j = (i + 1) % 3;
- k = (i + 2) % 3;
- /* ---------------------------------- */
- four_d = sqrtf((val->ptr[i][i] - val->ptr[j][j] - val->ptr[k][k]) + 1.0f);
- dst->ptr[i] = four_d * 0.5f;
- inv_half = 0.5f / four_d;
- dst->ptr[j] = (val->ptr[i][j] + val->ptr[j][i]) * inv_half;
- dst->ptr[k] = (val->ptr[i][k] + val->ptr[k][i]) * inv_half;
- dst->w = (val->ptr[j][k] - val->ptr[k][j]) * inv_half;
- }
- }
- void quat_get_dir(struct vec3 *dst, const struct quat *q)
- {
- struct matrix3 m;
- matrix3_from_quat(&m, q);
- vec3_copy(dst, &m.z);
- }
- void quat_set_look_dir(struct quat *dst, const struct vec3 *dir)
- {
- struct vec3 new_dir;
- struct quat xz_rot, yz_rot;
- bool xz_valid;
- bool yz_valid;
- struct axisang aa;
- vec3_norm(&new_dir, dir);
- vec3_neg(&new_dir, &new_dir);
- quat_identity(&xz_rot);
- quat_identity(&yz_rot);
- xz_valid = close_float(new_dir.x, 0.0f, EPSILON) || close_float(new_dir.z, 0.0f, EPSILON);
- yz_valid = close_float(new_dir.y, 0.0f, EPSILON);
- if (xz_valid) {
- axisang_set(&aa, 0.0f, 1.0f, 0.0f, atan2f(new_dir.x, new_dir.z));
- quat_from_axisang(&xz_rot, &aa);
- }
- if (yz_valid) {
- axisang_set(&aa, -1.0f, 0.0f, 0.0f, asinf(new_dir.y));
- quat_from_axisang(&yz_rot, &aa);
- }
- if (!xz_valid)
- quat_copy(dst, &yz_rot);
- else if (!yz_valid)
- quat_copy(dst, &xz_rot);
- else
- quat_mul(dst, &xz_rot, &yz_rot);
- }
- void quat_log(struct quat *dst, const struct quat *q)
- {
- float angle = acosf(q->w);
- float sine = sinf(angle);
- float w = q->w;
- quat_copy(dst, q);
- dst->w = 0.0f;
- if ((fabsf(w) < 1.0f) && (fabsf(sine) >= EPSILON)) {
- sine = angle / sine;
- quat_mulf(dst, dst, sine);
- }
- }
- void quat_exp(struct quat *dst, const struct quat *q)
- {
- float length = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z);
- float sine = sinf(length);
- quat_copy(dst, q);
- sine = (length > EPSILON) ? (sine / length) : 1.0f;
- quat_mulf(dst, dst, sine);
- dst->w = cosf(length);
- }
- void quat_interpolate(struct quat *dst, const struct quat *q1, const struct quat *q2, float t)
- {
- float dot = quat_dot(q1, q2);
- float anglef = acosf(dot);
- float sine, sinei, sinet, sineti;
- struct quat temp;
- if (anglef >= EPSILON) {
- sine = sinf(anglef);
- sinei = 1 / sine;
- sinet = sinf(anglef * t) * sinei;
- sineti = sinf(anglef * (1.0f - t)) * sinei;
- quat_mulf(&temp, q1, sineti);
- quat_mulf(dst, q2, sinet);
- quat_add(dst, &temp, dst);
- } else {
- quat_sub(&temp, q2, q1);
- quat_mulf(&temp, &temp, t);
- quat_add(dst, &temp, q1);
- }
- }
- void quat_get_tangent(struct quat *dst, const struct quat *prev, const struct quat *q, const struct quat *next)
- {
- struct quat temp;
- quat_sub(&temp, q, prev);
- quat_add(&temp, &temp, next);
- quat_sub(&temp, &temp, q);
- quat_mulf(dst, &temp, 0.5f);
- }
- void quat_interpolate_cubic(struct quat *dst, const struct quat *q1, const struct quat *q2, const struct quat *m1,
- const struct quat *m2, float t)
- {
- struct quat temp1, temp2;
- quat_interpolate(&temp1, q1, q2, t);
- quat_interpolate(&temp2, m1, m2, t);
- quat_interpolate(dst, &temp1, &temp2, 2.0f * (1.0f - t) * t);
- }
|