obs-studio/libobs/graphics/quat.c

223 lines
5.3 KiB
C
Raw Normal View History

2013-09-30 19:37:13 -07:00
/******************************************************************************
Copyright (C) 2013 by Hugh Bailey <obs.jim@gmail.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
2013-09-30 19:37:13 -07:00
(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"
2013-09-30 19:37:13 -07:00
#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);
2013-09-30 19:37:13 -07:00
dst->x = aa->x * sine;
dst->y = aa->y * sine;
dst->z = aa->z * sine;
dst->w = cosf(halfa);
2013-09-30 19:37:13 -07:00
}
struct f4x4 {
2013-10-17 17:21:42 -07:00
float ptr[4][4];
2013-09-30 19:37:13 -07:00
};
2013-10-17 17:21:42 -07:00
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)
2013-09-30 19:37:13 -07:00
{
float tr = (m->x.x + m->y.y + m->z.z);
float inv_half;
float four_d;
int i, j, k;
2013-09-30 19:37:13 -07:00
if (tr > 0.0f) {
four_d = sqrtf(tr + 1.0f);
dst->w = four_d * 0.5f;
2013-09-30 19:37:13 -07:00
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;
2013-09-30 19:37:13 -07:00
} else {
struct f4x4 *val = (struct f4x4 *)m;
2013-09-30 19:37:13 -07:00
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;
2013-09-30 19:37:13 -07:00
/* ---------------------------------- */
four_d = sqrtf(
(val->ptr[i][i] - val->ptr[j][j] - val->ptr[k][k]) +
1.0f);
2013-09-30 19:37:13 -07:00
dst->ptr[i] = four_d * 0.5f;
2013-09-30 19:37:13 -07:00
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;
2013-09-30 19:37:13 -07:00
}
}
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;
2013-09-30 19:37:13 -07:00
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);
2013-09-30 19:37:13 -07:00
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));
2013-09-30 19:37:13 -07:00
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;
2013-09-30 19:37:13 -07:00
quat_copy(dst, q);
dst->w = 0.0f;
if ((fabsf(w) < 1.0f) && (fabsf(sine) >= EPSILON)) {
sine = angle / sine;
2013-09-30 19:37:13 -07:00
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);
2013-09-30 19:37:13 -07:00
quat_copy(dst, q);
sine = (length > EPSILON) ? (sine / length) : 1.0f;
2013-09-30 19:37:13 -07:00
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)
2013-09-30 19:37:13 -07:00
{
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;
2013-09-30 19:37:13 -07:00
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)
2013-09-30 19:37:13 -07:00
{
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)
2013-09-30 19:37:13 -07:00
{
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);
2013-09-30 19:37:13 -07:00
}