| rev |
line source |
|
nuclear@1
|
1 /*
|
|
nuclear@1
|
2 libvmath - a vector math library
|
|
nuclear@1
|
3 Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
|
|
nuclear@1
|
4
|
|
nuclear@1
|
5 This program is free software: you can redistribute it and/or modify
|
|
nuclear@1
|
6 it under the terms of the GNU Lesser General Public License as published
|
|
nuclear@1
|
7 by the Free Software Foundation, either version 3 of the License, or
|
|
nuclear@1
|
8 (at your option) any later version.
|
|
nuclear@1
|
9
|
|
nuclear@1
|
10 This program is distributed in the hope that it will be useful,
|
|
nuclear@1
|
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
nuclear@1
|
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
nuclear@1
|
13 GNU Lesser General Public License for more details.
|
|
nuclear@1
|
14
|
|
nuclear@1
|
15 You should have received a copy of the GNU Lesser General Public License
|
|
nuclear@1
|
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
nuclear@1
|
17 */
|
|
nuclear@1
|
18
|
|
nuclear@1
|
19 #include "vector.h"
|
|
nuclear@1
|
20 #include "matrix.h"
|
|
nuclear@1
|
21
|
|
nuclear@1
|
22 #ifdef __cplusplus
|
|
nuclear@1
|
23 extern "C" {
|
|
nuclear@1
|
24 #endif /* __cplusplus */
|
|
nuclear@1
|
25
|
|
nuclear@1
|
26 static inline quat_t quat_mul(quat_t q1, quat_t q2)
|
|
nuclear@1
|
27 {
|
|
nuclear@1
|
28 quat_t res;
|
|
nuclear@1
|
29 vec3_t v1 = quat_vec(q1);
|
|
nuclear@1
|
30 vec3_t v2 = quat_vec(q2);
|
|
nuclear@1
|
31
|
|
nuclear@1
|
32 res.w = q1.w * q2.w - v3_dot(v1, v2);
|
|
nuclear@1
|
33 /* resvec = v2 * q1 + v1 * q2 + cross(v1, v2) */
|
|
nuclear@1
|
34 res.x = v2.x * q1.w + v1.x * q2.w + (v1.y * v2.z - v1.z * v2.y);
|
|
nuclear@1
|
35 res.y = v2.y * q1.w + v1.y * q2.w + (v1.z * v2.x - v1.x * v2.z);
|
|
nuclear@1
|
36 res.z = v2.z * q1.w + v1.z * q2.w + (v1.x * v2.y - v1.y * v2.x);
|
|
nuclear@1
|
37 return res;
|
|
nuclear@1
|
38 }
|
|
nuclear@1
|
39
|
|
nuclear@1
|
40 static inline quat_t quat_conjugate(quat_t q)
|
|
nuclear@1
|
41 {
|
|
nuclear@1
|
42 q.x = -q.x;
|
|
nuclear@1
|
43 q.y = -q.y;
|
|
nuclear@1
|
44 q.z = -q.z;
|
|
nuclear@1
|
45 return q;
|
|
nuclear@1
|
46 }
|
|
nuclear@1
|
47
|
|
nuclear@1
|
48 static inline quat_t quat_inverse(quat_t q)
|
|
nuclear@1
|
49 {
|
|
nuclear@1
|
50 scalar_t lensq = quat_length_sq(q);
|
|
nuclear@1
|
51 q = quat_conjugate(q);
|
|
nuclear@1
|
52 q.x /= lensq;
|
|
nuclear@1
|
53 q.y /= lensq;
|
|
nuclear@1
|
54 q.z /= lensq;
|
|
nuclear@1
|
55 q.w /= lensq;
|
|
nuclear@1
|
56 return q;
|
|
nuclear@1
|
57 }
|
|
nuclear@1
|
58
|
|
nuclear@1
|
59 static inline void quat_to_mat3(mat3_t res, quat_t q)
|
|
nuclear@1
|
60 {
|
|
nuclear@1
|
61 m3_cons(res, 1.0 - 2.0 * q.y*q.y - 2.0 * q.z*q.z, 2.0 * q.x * q.y - 2.0 * q.w * q.z, 2.0 * q.z * q.x + 2.0 * q.w * q.y,
|
|
nuclear@1
|
62 2.0 * q.x * q.y + 2.0 * q.w * q.z, 1.0 - 2.0 * q.x*q.x - 2.0 * q.z*q.z, 2.0 * q.y * q.z - 2.0 * q.w * q.x,
|
|
nuclear@1
|
63 2.0 * q.z * q.x - 2.0 * q.w * q.y, 2.0 * q.y * q.z + 2.0 * q.w * q.x, 1.0 - 2.0 * q.x*q.x - 2.0 * q.y*q.y);
|
|
nuclear@1
|
64 }
|
|
nuclear@1
|
65
|
|
nuclear@1
|
66 static inline void quat_to_mat4(mat4_t res, quat_t q)
|
|
nuclear@1
|
67 {
|
|
nuclear@1
|
68 m4_cons(res, 1.0 - 2.0 * q.y*q.y - 2.0 * q.z*q.z, 2.0 * q.x * q.y - 2.0 * q.w * q.z, 2.0 * q.z * q.x + 2.0 * q.w * q.y, 0,
|
|
nuclear@1
|
69 2.0 * q.x * q.y + 2.0 * q.w * q.z, 1.0 - 2.0 * q.x*q.x - 2.0 * q.z*q.z, 2.0 * q.y * q.z - 2.0 * q.w * q.x, 0,
|
|
nuclear@1
|
70 2.0 * q.z * q.x - 2.0 * q.w * q.y, 2.0 * q.y * q.z + 2.0 * q.w * q.x, 1.0 - 2.0 * q.x*q.x - 2.0 * q.y*q.y, 0,
|
|
nuclear@1
|
71 0, 0, 0, 1);
|
|
nuclear@1
|
72 }
|
|
nuclear@1
|
73
|
|
nuclear@1
|
74 #ifdef __cplusplus
|
|
nuclear@1
|
75 } /* extern "C" */
|
|
nuclear@1
|
76
|
|
nuclear@1
|
77 inline Quaternion lerp(const Quaternion &a, const Quaternion &b, scalar_t t)
|
|
nuclear@1
|
78 {
|
|
nuclear@1
|
79 return slerp(a, b, t);
|
|
nuclear@1
|
80 }
|
|
nuclear@1
|
81 #endif /* __cplusplus */
|
