1 /*
2 * Copyright (C) 2003 Robert Kooima
3 *
4 * NEVERBALL is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published
6 * by the Free Software Foundation; either version 2 of the License,
7 * or (at your option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
13 */
14
15 #include <stdio.h>
16 #include <math.h>
17
18 #include "vec3.h"
19
20 #define A 10
21 #define B 11
22 #define C 12
23 #define D 13
24 #define E 14
25 #define F 15
26
27 #define TINY 1e-5
28
29 /*---------------------------------------------------------------------------*/
30
v_nrm(float * n,const float * v)31 void v_nrm(float *n, const float *v)
32 {
33 float d = v_len(v);
34
35 if (d == 0.0f)
36 {
37 n[0] = 0.0f;
38 n[1] = 0.0f;
39 n[2] = 0.0f;
40 }
41 else
42 {
43 n[0] = v[0] / d;
44 n[1] = v[1] / d;
45 n[2] = v[2] / d;
46 }
47 }
48
v_crs(float * u,const float * v,const float * w)49 void v_crs(float *u, const float *v, const float *w)
50 {
51 u[0] = v[1] * w[2] - v[2] * w[1];
52 u[1] = v[2] * w[0] - v[0] * w[2];
53 u[2] = v[0] * w[1] - v[1] * w[0];
54 }
55
56 /*---------------------------------------------------------------------------*/
57
m_cpy(float * M,const float * N)58 void m_cpy(float *M, const float *N)
59 {
60 M[0] = N[0]; M[1] = N[1]; M[2] = N[2]; M[3] = N[3];
61 M[4] = N[4]; M[5] = N[5]; M[6] = N[6]; M[7] = N[7];
62 M[8] = N[8]; M[9] = N[9]; M[A] = N[A]; M[B] = N[B];
63 M[C] = N[C]; M[D] = N[D]; M[E] = N[E]; M[F] = N[F];
64 }
65
m_xps(float * M,const float * N)66 void m_xps(float *M, const float *N)
67 {
68 M[0] = N[0]; M[1] = N[4]; M[2] = N[8]; M[3] = N[C];
69 M[4] = N[1]; M[5] = N[5]; M[6] = N[9]; M[7] = N[D];
70 M[8] = N[2]; M[9] = N[6]; M[A] = N[A]; M[B] = N[E];
71 M[C] = N[3]; M[D] = N[7]; M[E] = N[B]; M[F] = N[F];
72 }
73
m_inv(float * I,const float * N)74 int m_inv(float *I, const float *N)
75 {
76 double T[16], M[16];
77 double d;
78 int i;
79
80 for (i = 0; i < 16; i++)
81 M[i] = N[i];
82
83 T[0] = +(M[5] * (M[A] * M[F] - M[B] * M[E]) -
84 M[9] * (M[6] * M[F] - M[7] * M[E]) +
85 M[D] * (M[6] * M[B] - M[7] * M[A]));
86 T[1] = -(M[4] * (M[A] * M[F] - M[B] * M[E]) -
87 M[8] * (M[6] * M[F] - M[7] * M[E]) +
88 M[C] * (M[6] * M[B] - M[7] * M[A]));
89 T[2] = +(M[4] * (M[9] * M[F] - M[B] * M[D]) -
90 M[8] * (M[5] * M[F] - M[7] * M[D]) +
91 M[C] * (M[5] * M[B] - M[7] * M[9]));
92 T[3] = -(M[4] * (M[9] * M[E] - M[A] * M[D]) -
93 M[8] * (M[5] * M[E] - M[6] * M[D]) +
94 M[C] * (M[5] * M[A] - M[6] * M[9]));
95
96 T[4] = -(M[1] * (M[A] * M[F] - M[B] * M[E]) -
97 M[9] * (M[2] * M[F] - M[3] * M[E]) +
98 M[D] * (M[2] * M[B] - M[3] * M[A]));
99 T[5] = +(M[0] * (M[A] * M[F] - M[B] * M[E]) -
100 M[8] * (M[2] * M[F] - M[3] * M[E]) +
101 M[C] * (M[2] * M[B] - M[3] * M[A]));
102 T[6] = -(M[0] * (M[9] * M[F] - M[B] * M[D]) -
103 M[8] * (M[1] * M[F] - M[3] * M[D]) +
104 M[C] * (M[1] * M[B] - M[3] * M[9]));
105 T[7] = +(M[0] * (M[9] * M[E] - M[A] * M[D]) -
106 M[8] * (M[1] * M[E] - M[2] * M[D]) +
107 M[C] * (M[1] * M[A] - M[2] * M[9]));
108
109 T[8] = +(M[1] * (M[6] * M[F] - M[7] * M[E]) -
110 M[5] * (M[2] * M[F] - M[3] * M[E]) +
111 M[D] * (M[2] * M[7] - M[3] * M[6]));
112 T[9] = -(M[0] * (M[6] * M[F] - M[7] * M[E]) -
113 M[4] * (M[2] * M[F] - M[3] * M[E]) +
114 M[C] * (M[2] * M[7] - M[3] * M[6]));
115 T[A] = +(M[0] * (M[5] * M[F] - M[7] * M[D]) -
116 M[4] * (M[1] * M[F] - M[3] * M[D]) +
117 M[C] * (M[1] * M[7] - M[3] * M[5]));
118 T[B] = -(M[0] * (M[5] * M[E] - M[6] * M[D]) -
119 M[4] * (M[1] * M[E] - M[2] * M[D]) +
120 M[C] * (M[1] * M[6] - M[2] * M[5]));
121
122 T[C] = -(M[1] * (M[6] * M[B] - M[7] * M[A]) -
123 M[5] * (M[2] * M[B] - M[3] * M[A]) +
124 M[9] * (M[2] * M[7] - M[3] * M[6]));
125 T[D] = +(M[0] * (M[6] * M[B] - M[7] * M[A]) -
126 M[4] * (M[2] * M[B] - M[3] * M[A]) +
127 M[8] * (M[2] * M[7] - M[3] * M[6]));
128 T[E] = -(M[0] * (M[5] * M[B] - M[7] * M[9]) -
129 M[4] * (M[1] * M[B] - M[3] * M[9]) +
130 M[8] * (M[1] * M[7] - M[3] * M[5]));
131 T[F] = +(M[0] * (M[5] * M[A] - M[6] * M[9]) -
132 M[4] * (M[1] * M[A] - M[2] * M[9]) +
133 M[8] * (M[1] * M[6] - M[2] * M[5]));
134
135 d = M[0] * T[0] + M[4] * T[4] + M[8] * T[8] + M[C] * T[C];
136
137 if (fabs(d) > TINY)
138 {
139 I[0] = T[0] / d;
140 I[1] = T[4] / d;
141 I[2] = T[8] / d;
142 I[3] = T[C] / d;
143 I[4] = T[1] / d;
144 I[5] = T[5] / d;
145 I[6] = T[9] / d;
146 I[7] = T[D] / d;
147 I[8] = T[2] / d;
148 I[9] = T[6] / d;
149 I[A] = T[A] / d;
150 I[B] = T[E] / d;
151 I[C] = T[3] / d;
152 I[D] = T[7] / d;
153 I[E] = T[B] / d;
154 I[F] = T[F] / d;
155
156 return 1;
157 }
158 return 0;
159 }
160
161 /*---------------------------------------------------------------------------*/
162
m_ident(float * M)163 void m_ident(float *M)
164 {
165 M[0] = 1.f; M[4] = 0.f; M[8] = 0.f; M[C] = 0.f;
166 M[1] = 0.f; M[5] = 1.f; M[9] = 0.f; M[D] = 0.f;
167 M[2] = 0.f; M[6] = 0.f; M[A] = 1.f; M[E] = 0.f;
168 M[3] = 0.f; M[7] = 0.f; M[B] = 0.f; M[F] = 1.f;
169 }
170
m_basis(float * M,const float e0[3],const float e1[3],const float e2[3])171 void m_basis(float *M,
172 const float e0[3],
173 const float e1[3],
174 const float e2[3])
175 {
176 M[0] = e0[0]; M[4] = e1[0]; M[8] = e2[0]; M[C] = 0.f;
177 M[1] = e0[1]; M[5] = e1[1]; M[9] = e2[1]; M[D] = 0.f;
178 M[2] = e0[2]; M[6] = e1[2]; M[A] = e2[2]; M[E] = 0.f;
179 M[3] = 0.f; M[7] = 0.f; M[B] = 0.f; M[F] = 1.f;
180 }
181
182 /*---------------------------------------------------------------------------*/
183
m_xlt(float * M,const float * v)184 void m_xlt(float *M, const float *v)
185 {
186 M[0] = 1.f; M[4] = 0.f; M[8] = 0.f; M[C] = v[0];
187 M[1] = 0.f; M[5] = 1.f; M[9] = 0.f; M[D] = v[1];
188 M[2] = 0.f; M[6] = 0.f; M[A] = 1.f; M[E] = v[2];
189 M[3] = 0.f; M[7] = 0.f; M[B] = 0.f; M[F] = 1.f;
190 }
191
m_scl(float * M,const float * v)192 void m_scl(float *M, const float *v)
193 {
194 M[0] = v[0]; M[4] = 0.f; M[8] = 0.f; M[C] = 0.f;
195 M[1] = 0.f; M[5] = v[1]; M[9] = 0.f; M[D] = 0.f;
196 M[2] = 0.f; M[6] = 0.f; M[A] = v[2]; M[E] = 0.f;
197 M[3] = 0.f; M[7] = 0.f; M[B] = 0.f; M[F] = 1.f;
198 }
199
m_rot(float * M,const float * v,float a)200 void m_rot(float *M, const float *v, float a)
201 {
202 float u[3];
203 float U[16];
204 float S[16];
205
206 const float s = fsinf(a);
207 const float c = fcosf(a);
208
209 v_nrm(u, v);
210
211 U[0] = u[0] * u[0]; U[4] = u[0] * u[1]; U[8] = u[0] * u[2];
212 U[1] = u[1] * u[0]; U[5] = u[1] * u[1]; U[9] = u[1] * u[2];
213 U[2] = u[2] * u[0]; U[6] = u[2] * u[1]; U[A] = u[2] * u[2];
214
215 S[0] = 0.f; S[4] = -u[2]; S[8] = u[1];
216 S[1] = u[2]; S[5] = 0.f; S[9] = -u[0];
217 S[2] = -u[1]; S[6] = u[0]; S[A] = 0.f;
218
219 M[0] = U[0] + c * (1.f - U[0]) + s * S[0];
220 M[1] = U[1] + c * (0.f - U[1]) + s * S[1];
221 M[2] = U[2] + c * (0.f - U[2]) + s * S[2];
222 M[3] = 0.f;
223 M[4] = U[4] + c * (0.f - U[4]) + s * S[4];
224 M[5] = U[5] + c * (1.f - U[5]) + s * S[5];
225 M[6] = U[6] + c * (0.f - U[6]) + s * S[6];
226 M[7] = 0.f;
227 M[8] = U[8] + c * (0.f - U[8]) + s * S[8];
228 M[9] = U[9] + c * (0.f - U[9]) + s * S[9];
229 M[A] = U[A] + c * (1.f - U[A]) + s * S[A];
230 M[B] = 0.f;
231 M[C] = 0.f;
232 M[D] = 0.f;
233 M[E] = 0.f;
234 M[F] = 1.f;
235 }
236
237 /*---------------------------------------------------------------------------*/
238
m_mult(float * M,const float * N,const float * O)239 void m_mult(float *M, const float *N, const float *O)
240 {
241 M[0] = N[0] * O[0] + N[4] * O[1] + N[8] * O[2] + N[C] * O[3];
242 M[1] = N[1] * O[0] + N[5] * O[1] + N[9] * O[2] + N[D] * O[3];
243 M[2] = N[2] * O[0] + N[6] * O[1] + N[A] * O[2] + N[E] * O[3];
244 M[3] = N[3] * O[0] + N[7] * O[1] + N[B] * O[2] + N[F] * O[3];
245
246 M[4] = N[0] * O[4] + N[4] * O[5] + N[8] * O[6] + N[C] * O[7];
247 M[5] = N[1] * O[4] + N[5] * O[5] + N[9] * O[6] + N[D] * O[7];
248 M[6] = N[2] * O[4] + N[6] * O[5] + N[A] * O[6] + N[E] * O[7];
249 M[7] = N[3] * O[4] + N[7] * O[5] + N[B] * O[6] + N[F] * O[7];
250
251 M[8] = N[0] * O[8] + N[4] * O[9] + N[8] * O[A] + N[C] * O[B];
252 M[9] = N[1] * O[8] + N[5] * O[9] + N[9] * O[A] + N[D] * O[B];
253 M[A] = N[2] * O[8] + N[6] * O[9] + N[A] * O[A] + N[E] * O[B];
254 M[B] = N[3] * O[8] + N[7] * O[9] + N[B] * O[A] + N[F] * O[B];
255
256 M[C] = N[0] * O[C] + N[4] * O[D] + N[8] * O[E] + N[C] * O[F];
257 M[D] = N[1] * O[C] + N[5] * O[D] + N[9] * O[E] + N[D] * O[F];
258 M[E] = N[2] * O[C] + N[6] * O[D] + N[A] * O[E] + N[E] * O[F];
259 M[F] = N[3] * O[C] + N[7] * O[D] + N[B] * O[E] + N[F] * O[F];
260 }
261
m_pxfm(float * v,const float * M,const float * w)262 void m_pxfm(float *v, const float *M, const float *w)
263 {
264 float d = w[0] * M[3] + w[1] * M[7] + w[2] * M[B] + M[F];
265
266 v[0] = (w[0] * M[0] + w[1] * M[4] + w[2] * M[8] + M[C]) / d;
267 v[1] = (w[0] * M[1] + w[1] * M[5] + w[2] * M[9] + M[D]) / d;
268 v[2] = (w[0] * M[2] + w[1] * M[6] + w[2] * M[A] + M[E]) / d;
269 }
270
m_vxfm(float * v,const float * M,const float * w)271 void m_vxfm(float *v, const float *M, const float *w)
272 {
273 v[0] = (w[0] * M[0] + w[1] * M[4] + w[2] * M[8]);
274 v[1] = (w[0] * M[1] + w[1] * M[5] + w[2] * M[9]);
275 v[2] = (w[0] * M[2] + w[1] * M[6] + w[2] * M[A]);
276 }
277
278 /*---------------------------------------------------------------------------*/
279
q_as_axisangle(const float q[4],float u[3],float * a)280 void q_as_axisangle(const float q[4], float u[3], float *a)
281 {
282 *a = 2.0f * facosf(q[0]);
283 v_nrm(u, q + 1);
284 }
285
q_by_axisangle(float q[4],const float u[3],float a)286 void q_by_axisangle(float q[4], const float u[3], float a)
287 {
288 float c = fcosf(a * 0.5f);
289 float s = fsinf(a * 0.5f);
290 float n[3];
291
292 v_nrm(n, u);
293
294 q[0] = c;
295 q[1] = n[0] * s;
296 q[2] = n[1] * s;
297 q[3] = n[2] * s;
298 }
299
q_nrm(float q[4],const float r[4])300 void q_nrm(float q[4], const float r[4])
301 {
302 float d = q_len(r);
303
304 if (d == 0.0f)
305 {
306 q[0] = 1.0f;
307 q[1] = 0.0f;
308 q[2] = 0.0f;
309 q[3] = 0.0f;
310 }
311 else
312 {
313 q[0] = r[0] / d;
314 q[1] = r[1] / d;
315 q[2] = r[2] / d;
316 q[3] = r[3] / d;
317 }
318 }
319
q_mul(float q[4],const float a[4],const float b[4])320 void q_mul(float q[4], const float a[4], const float b[4])
321 {
322 q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
323 q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
324 q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
325 q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
326 }
327
q_rot(float v[3],const float r[4],const float w[3])328 void q_rot(float v[3], const float r[4], const float w[3])
329 {
330 float a[4], b[4], c[4];
331
332 a[0] = 0.0f;
333 a[1] = w[0];
334 a[2] = w[1];
335 a[3] = w[2];
336
337 q_mul(b, r, a);
338
339 a[0] = r[0];
340 a[1] = -r[1];
341 a[2] = -r[2];
342 a[3] = -r[3];
343
344 q_mul(c, b, a);
345
346 v[0] = c[1];
347 v[1] = c[2];
348 v[2] = c[3];
349 }
350
q_euler(float v[3],const float q[4])351 void q_euler(float v[3], const float q[4])
352 {
353 float m11 = (2 * q[0] * q[0]) + (2 * q[1] * q[1]) - 1;
354 float m12 = (2 * q[1] * q[2]) + (2 * q[0] * q[3]);
355 float m13 = (2 * q[1] * q[3]) - (2 * q[0] * q[2]);
356 float m23 = (2 * q[2] * q[3]) + (2 * q[0] * q[1]);
357 float m33 = (2 * q[0] * q[0]) + (2 * q[3] * q[3]) - 1;
358
359 v[0] = fatan2f(m12, m11);
360 v[1] = fasinf(-m13);
361 v[2] = fatan2f(m23, m33);
362 }
363
364 /*
365 * Spherical linear interpolation
366 */
q_slerp(float q[4],const float a[4],const float b[4],float t)367 void q_slerp(float q[4], const float a[4], const float b[4], float t)
368 {
369 float c, r, s, u, v;
370 int i = +1;
371
372 if (t <= 0.0f)
373 {
374 q_cpy(q, a);
375 return;
376 }
377
378 if (1.0f <= t)
379 {
380 q_cpy(q, b);
381 return;
382 }
383
384 /*
385 * a . b = |a||b| cos A
386 * |a| = |b| = 1
387 */
388
389 c = q_dot(a, b);
390
391 /* Ensure the shortest path. */
392
393 if (c < 0)
394 {
395 c = -c;
396 i = -1;
397 }
398
399 /* Fall back to normalized lerp for very similar orientations. */
400
401 if (1.0f - c < (float) TINY)
402 {
403 u = 1.0f - t;
404 v = t;
405 }
406 else
407 {
408 r = facosf(c);
409 s = fsinf(r);
410 u = fsinf((1.0f - t) * r) / s;
411 v = fsinf((t) * r) / s * i;
412 }
413
414 q[0] = a[0] * u + b[0] * v;
415 q[1] = a[1] * u + b[1] * v;
416 q[2] = a[2] * u + b[2] * v;
417 q[3] = a[3] * u + b[3] * v;
418
419 q_nrm(q, q);
420 }
421
422 /*---------------------------------------------------------------------------*/
423