diff options
Diffstat (limited to 'include/math/quat.h')
-rw-r--r-- | include/math/quat.h | 30 |
1 files changed, 15 insertions, 15 deletions
diff --git a/include/math/quat.h b/include/math/quat.h index 4503abd..b284567 100644 --- a/include/math/quat.h +++ b/include/math/quat.h | |||
@@ -23,14 +23,14 @@ static inline quat quat_from_array(const R xyzw[4]) { | |||
23 | 23 | ||
24 | /// Construct a rotation quaternion. | 24 | /// Construct a rotation quaternion. |
25 | static inline quat qmake_rot(R angle, R x, R y, R z) { | 25 | static inline quat qmake_rot(R angle, R x, R y, R z) { |
26 | const R a = angle * 0.5; | 26 | const R a = angle * 0.5; |
27 | const R sa = sin(a); | 27 | const R sa = sin(a); |
28 | const R w = cos(a); | 28 | const R w = cos(a); |
29 | R mag = sqrt(x * x + y * y + z * z); | 29 | R mag = sqrt(x * x + y * y + z * z); |
30 | mag = mag == 0.0 ? 1.0 : mag; | 30 | mag = mag == 0.0 ? 1.0 : mag; |
31 | x = x * sa; | 31 | x = x * sa; |
32 | y = y * sa; | 32 | y = y * sa; |
33 | z = z * sa; | 33 | z = z * sa; |
34 | return (quat){x / mag, y / mag, z / mag, w}; | 34 | return (quat){x / mag, y / mag, z / mag, w}; |
35 | } | 35 | } |
36 | 36 | ||
@@ -46,7 +46,7 @@ static inline quat qmul(quat q1, quat q2) { | |||
46 | /// Invert the quaternion. | 46 | /// Invert the quaternion. |
47 | static inline quat qinv(quat q) { | 47 | static inline quat qinv(quat q) { |
48 | R magsq = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z; | 48 | R magsq = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z; |
49 | magsq = magsq == 0.0f ? 1.0f : magsq; | 49 | magsq = magsq == 0.0f ? 1.0f : magsq; |
50 | return (quat){-q.x / magsq, -q.y / magsq, -q.z / magsq, q.w / magsq}; | 50 | return (quat){-q.x / magsq, -q.y / magsq, -q.z / magsq, q.w / magsq}; |
51 | } | 51 | } |
52 | 52 | ||
@@ -55,9 +55,9 @@ static inline quat qconj(quat q) { return (quat){-q.x, -q.y, -q.z, q.w}; } | |||
55 | 55 | ||
56 | /// Rotate the given vector by the given unit quaternion. | 56 | /// Rotate the given vector by the given unit quaternion. |
57 | static inline vec3 qrot(quat q, vec3 v) { | 57 | static inline vec3 qrot(quat q, vec3 v) { |
58 | const quat p = qconj(q); | 58 | const quat p = qconj(q); |
59 | const quat qv = (quat){v.x, v.y, v.z, 0}; | 59 | const quat qv = (quat){v.x, v.y, v.z, 0}; |
60 | const quat u = qmul(qmul(q, qv), p); | 60 | const quat u = qmul(qmul(q, qv), p); |
61 | return vec3_make(u.x, u.y, u.z); | 61 | return vec3_make(u.x, u.y, u.z); |
62 | } | 62 | } |
63 | 63 | ||
@@ -72,8 +72,8 @@ static inline mat4 mat4_from_quat(quat q) { | |||
72 | const R wx = q.w * q.x; | 72 | const R wx = q.w * q.x; |
73 | const R wy = q.w * q.y; | 73 | const R wy = q.w * q.y; |
74 | const R wz = q.w * q.z; | 74 | const R wz = q.w * q.z; |
75 | return mat4_make(1 - 2 * yy - 2 * zz, 2 * xy - 2 * wz, 2 * xz + 2 * wy, 0, | 75 | return mat4_make( |
76 | 2 * xy + 2 * wz, 1 - 2 * xx - 2 * zz, 2 * yz - 2 * wx, 0, | 76 | 1 - 2 * yy - 2 * zz, 2 * xy - 2 * wz, 2 * xz + 2 * wy, 0, 2 * xy + 2 * wz, |
77 | 2 * xz - 2 * wy, 2 * yz + 2 * wx, 1 - 2 * xx - 2 * yy, 0, 0, | 77 | 1 - 2 * xx - 2 * zz, 2 * yz - 2 * wx, 0, 2 * xz - 2 * wy, 2 * yz + 2 * wx, |
78 | 0, 0, 1); | 78 | 1 - 2 * xx - 2 * yy, 0, 0, 0, 0, 1); |
79 | } | 79 | } |