nuclear@29: #include "vector.h" nuclear@29: #include "matrix.h" nuclear@29: nuclear@29: #ifdef __cplusplus nuclear@29: extern "C" { nuclear@29: #endif /* __cplusplus */ nuclear@29: nuclear@29: static inline quat_t quat_mul(quat_t q1, quat_t q2) nuclear@29: { nuclear@29: quat_t res; nuclear@29: vec3_t v1 = quat_vec(q1); nuclear@29: vec3_t v2 = quat_vec(q2); nuclear@29: nuclear@29: res.w = q1.w * q2.w - v3_dot(v1, v2); nuclear@29: /* resvec = v2 * q1 + v1 * q2 + cross(v1, v2) */ nuclear@29: res.x = v2.x * q1.w + v1.x * q2.w + (v1.y * v2.z - v1.z * v2.y); nuclear@29: res.y = v2.y * q1.w + v1.y * q2.w + (v1.z * v2.x - v1.x * v2.z); nuclear@29: res.z = v2.z * q1.w + v1.z * q2.w + (v1.x * v2.y - v1.y * v2.x); nuclear@29: return res; nuclear@29: } nuclear@29: nuclear@29: static inline quat_t quat_conjugate(quat_t q) nuclear@29: { nuclear@29: q.x = -q.x; nuclear@29: q.y = -q.y; nuclear@29: q.z = -q.z; nuclear@29: return q; nuclear@29: } nuclear@29: nuclear@29: static inline quat_t quat_inverse(quat_t q) nuclear@29: { nuclear@29: scalar_t lensq = quat_length_sq(q); nuclear@29: q = quat_conjugate(q); nuclear@29: q.x /= lensq; nuclear@29: q.y /= lensq; nuclear@29: q.z /= lensq; nuclear@29: q.w /= lensq; nuclear@29: return q; nuclear@29: } nuclear@29: nuclear@29: static inline void quat_to_mat3(mat3_t res, quat_t q) nuclear@29: { nuclear@29: 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@29: 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@29: 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@29: } nuclear@29: nuclear@29: static inline void quat_to_mat4(mat4_t res, quat_t q) nuclear@29: { nuclear@29: 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@29: 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@29: 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@29: 0, 0, 0, 1); nuclear@29: } nuclear@29: nuclear@29: #ifdef __cplusplus nuclear@29: } /* extern "C" */ nuclear@29: nuclear@29: inline Quaternion lerp(const Quaternion &a, const Quaternion &b, scalar_t t) nuclear@29: { nuclear@29: return slerp(a, b, t); nuclear@29: } nuclear@29: #endif /* __cplusplus */