1 #include "vector.h"

2 #include "matrix.h"

3

4 #ifdef __cplusplus

5 extern "C" {

6 #endif /* __cplusplus */

7

8 static inline quat_t quat_mul(quat_t q1, quat_t q2)

9 {

10 quat_t res;

11 vec3_t v1 = quat_vec(q1);

12 vec3_t v2 = quat_vec(q2);

13

14 res.w = q1.w * q2.w - v3_dot(v1, v2);

15 /* resvec = v2 * q1 + v1 * q2 + cross(v1, v2) */

16 res.x = v2.x * q1.w + v1.x * q2.w + (v1.y * v2.z - v1.z * v2.y);

17 res.y = v2.y * q1.w + v1.y * q2.w + (v1.z * v2.x - v1.x * v2.z);

18 res.z = v2.z * q1.w + v1.z * q2.w + (v1.x * v2.y - v1.y * v2.x);

19 return res;

20 }

21

22 static inline quat_t quat_conjugate(quat_t q)

23 {

24 q.x = -q.x;

25 q.y = -q.y;

26 q.z = -q.z;

27 return q;

28 }

29

30 static inline quat_t quat_inverse(quat_t q)

31 {

32 scalar_t lensq = quat_length_sq(q);

33 q = quat_conjugate(q);

34 q.x /= lensq;

35 q.y /= lensq;

36 q.z /= lensq;

37 q.w /= lensq;

38 return q;

39 }

40

41 static inline void quat_to_mat3(mat3_t res, quat_t q)

42 {

43 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,

44 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,

45 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);

46 }

47

48 static inline void quat_to_mat4(mat4_t res, quat_t q)

49 {

50 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,

51 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,

52 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,

53 0, 0, 0, 1);

54 }

55

56 #ifdef __cplusplus

57 } /* extern "C" */

58

59 inline Quaternion lerp(const Quaternion &a, const Quaternion &b, scalar_t t)

60 {

61 return slerp(a, b, t);

62 }

63 #endif /* __cplusplus */