1 /******************************************************************************
2 Copyright (C) 2013 by Hugh Bailey <obs.jim@gmail.com>
3
4 This program is free software: you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation, either version 2 of the License, or
7 (at your option) any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with this program. If not, see <http://www.gnu.org/licenses/>.
16 ******************************************************************************/
17
18 #include "quat.h"
19 #include "vec3.h"
20 #include "matrix3.h"
21 #include "matrix4.h"
22 #include "axisang.h"
23
quat_vec3(struct vec3 * v,const struct quat * q)24 static inline void quat_vec3(struct vec3 *v, const struct quat *q)
25 {
26 v->m = q->m;
27 v->w = 0.0f;
28 }
29
quat_mul(struct quat * dst,const struct quat * q1,const struct quat * q2)30 void quat_mul(struct quat *dst, const struct quat *q1, const struct quat *q2)
31 {
32 struct vec3 q1axis, q2axis;
33 struct vec3 temp1, temp2;
34
35 quat_vec3(&q1axis, q1);
36 quat_vec3(&q2axis, q2);
37
38 vec3_mulf(&temp1, &q2axis, q1->w);
39 vec3_mulf(&temp2, &q1axis, q2->w);
40 vec3_add(&temp1, &temp1, &temp2);
41 vec3_cross(&temp2, &q1axis, &q2axis);
42 vec3_add((struct vec3 *)dst, &temp1, &temp2);
43
44 dst->w = (q1->w * q2->w) - vec3_dot(&q1axis, &q2axis);
45 }
46
quat_from_axisang(struct quat * dst,const struct axisang * aa)47 void quat_from_axisang(struct quat *dst, const struct axisang *aa)
48 {
49 float halfa = aa->w * 0.5f;
50 float sine = sinf(halfa);
51
52 dst->x = aa->x * sine;
53 dst->y = aa->y * sine;
54 dst->z = aa->z * sine;
55 dst->w = cosf(halfa);
56 }
57
58 struct f4x4 {
59 float ptr[4][4];
60 };
61
quat_from_matrix3(struct quat * dst,const struct matrix3 * m)62 void quat_from_matrix3(struct quat *dst, const struct matrix3 *m)
63 {
64 quat_from_matrix4(dst, (const struct matrix4 *)m);
65 }
66
quat_from_matrix4(struct quat * dst,const struct matrix4 * m)67 void quat_from_matrix4(struct quat *dst, const struct matrix4 *m)
68 {
69 float tr = (m->x.x + m->y.y + m->z.z);
70 float inv_half;
71 float four_d;
72 int i, j, k;
73
74 if (tr > 0.0f) {
75 four_d = sqrtf(tr + 1.0f);
76 dst->w = four_d * 0.5f;
77
78 inv_half = 0.5f / four_d;
79 dst->x = (m->y.z - m->z.y) * inv_half;
80 dst->y = (m->z.x - m->x.z) * inv_half;
81 dst->z = (m->x.y - m->y.x) * inv_half;
82 } else {
83 struct f4x4 *val = (struct f4x4 *)m;
84
85 i = (m->x.x > m->y.y) ? 0 : 1;
86
87 if (m->z.z > val->ptr[i][i])
88 i = 2;
89
90 j = (i + 1) % 3;
91 k = (i + 2) % 3;
92
93 /* ---------------------------------- */
94
95 four_d = sqrtf(
96 (val->ptr[i][i] - val->ptr[j][j] - val->ptr[k][k]) +
97 1.0f);
98
99 dst->ptr[i] = four_d * 0.5f;
100
101 inv_half = 0.5f / four_d;
102 dst->ptr[j] = (val->ptr[i][j] + val->ptr[j][i]) * inv_half;
103 dst->ptr[k] = (val->ptr[i][k] + val->ptr[k][i]) * inv_half;
104 dst->w = (val->ptr[j][k] - val->ptr[k][j]) * inv_half;
105 }
106 }
107
quat_get_dir(struct vec3 * dst,const struct quat * q)108 void quat_get_dir(struct vec3 *dst, const struct quat *q)
109 {
110 struct matrix3 m;
111 matrix3_from_quat(&m, q);
112 vec3_copy(dst, &m.z);
113 }
114
quat_set_look_dir(struct quat * dst,const struct vec3 * dir)115 void quat_set_look_dir(struct quat *dst, const struct vec3 *dir)
116 {
117 struct vec3 new_dir;
118 struct quat xz_rot, yz_rot;
119 bool xz_valid;
120 bool yz_valid;
121 struct axisang aa;
122
123 vec3_norm(&new_dir, dir);
124 vec3_neg(&new_dir, &new_dir);
125
126 quat_identity(&xz_rot);
127 quat_identity(&yz_rot);
128
129 xz_valid = close_float(new_dir.x, 0.0f, EPSILON) ||
130 close_float(new_dir.z, 0.0f, EPSILON);
131 yz_valid = close_float(new_dir.y, 0.0f, EPSILON);
132
133 if (xz_valid) {
134 axisang_set(&aa, 0.0f, 1.0f, 0.0f,
135 atan2f(new_dir.x, new_dir.z));
136
137 quat_from_axisang(&xz_rot, &aa);
138 }
139 if (yz_valid) {
140 axisang_set(&aa, -1.0f, 0.0f, 0.0f, asinf(new_dir.y));
141 quat_from_axisang(&yz_rot, &aa);
142 }
143
144 if (!xz_valid)
145 quat_copy(dst, &yz_rot);
146 else if (!yz_valid)
147 quat_copy(dst, &xz_rot);
148 else
149 quat_mul(dst, &xz_rot, &yz_rot);
150 }
151
quat_log(struct quat * dst,const struct quat * q)152 void quat_log(struct quat *dst, const struct quat *q)
153 {
154 float angle = acosf(q->w);
155 float sine = sinf(angle);
156 float w = q->w;
157
158 quat_copy(dst, q);
159 dst->w = 0.0f;
160
161 if ((fabsf(w) < 1.0f) && (fabsf(sine) >= EPSILON)) {
162 sine = angle / sine;
163 quat_mulf(dst, dst, sine);
164 }
165 }
166
quat_exp(struct quat * dst,const struct quat * q)167 void quat_exp(struct quat *dst, const struct quat *q)
168 {
169 float length = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z);
170 float sine = sinf(length);
171
172 quat_copy(dst, q);
173 sine = (length > EPSILON) ? (sine / length) : 1.0f;
174 quat_mulf(dst, dst, sine);
175 dst->w = cosf(length);
176 }
177
quat_interpolate(struct quat * dst,const struct quat * q1,const struct quat * q2,float t)178 void quat_interpolate(struct quat *dst, const struct quat *q1,
179 const struct quat *q2, float t)
180 {
181 float dot = quat_dot(q1, q2);
182 float anglef = acosf(dot);
183 float sine, sinei, sinet, sineti;
184 struct quat temp;
185
186 if (anglef >= EPSILON) {
187 sine = sinf(anglef);
188 sinei = 1 / sine;
189 sinet = sinf(anglef * t) * sinei;
190 sineti = sinf(anglef * (1.0f - t)) * sinei;
191
192 quat_mulf(&temp, q1, sineti);
193 quat_mulf(dst, q2, sinet);
194 quat_add(dst, &temp, dst);
195 } else {
196 quat_sub(&temp, q2, q1);
197 quat_mulf(&temp, &temp, t);
198 quat_add(dst, &temp, q1);
199 }
200 }
201
quat_get_tangent(struct quat * dst,const struct quat * prev,const struct quat * q,const struct quat * next)202 void quat_get_tangent(struct quat *dst, const struct quat *prev,
203 const struct quat *q, const struct quat *next)
204 {
205 struct quat temp;
206
207 quat_sub(&temp, q, prev);
208 quat_add(&temp, &temp, next);
209 quat_sub(&temp, &temp, q);
210 quat_mulf(dst, &temp, 0.5f);
211 }
212
quat_interpolate_cubic(struct quat * dst,const struct quat * q1,const struct quat * q2,const struct quat * m1,const struct quat * m2,float t)213 void quat_interpolate_cubic(struct quat *dst, const struct quat *q1,
214 const struct quat *q2, const struct quat *m1,
215 const struct quat *m2, float t)
216 {
217 struct quat temp1, temp2;
218
219 quat_interpolate(&temp1, q1, q2, t);
220 quat_interpolate(&temp2, m1, m2, t);
221 quat_interpolate(dst, &temp1, &temp2, 2.0f * (1.0f - t) * t);
222 }
223