1 /*

2 libvmath - a vector math library

3 Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>

4

5 This program is free software: you can redistribute it and/or modify

6 it under the terms of the GNU Lesser General Public License as published

7 by the Free Software Foundation, either version 3 of the License, or

8 (at your option) any later version.

9

10 This program is distributed in the hope that it will be useful,

11 but WITHOUT ANY WARRANTY; without even the implied warranty of

12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

13 GNU Lesser General Public License for more details.

14

15 You should have received a copy of the GNU Lesser General Public License

16 along with this program. If not, see <http://www.gnu.org/licenses/>.

17 */

18

19 #include "vector.h"

20 #include "matrix.h"

21

22 #ifdef __cplusplus

23 extern "C" {

24 #endif /* __cplusplus */

25

26 static VMATH_INLINE quat_t quat_mul(quat_t q1, quat_t q2)

27 {

28 quat_t res;

29 vec3_t v1 = quat_vec(q1);

30 vec3_t v2 = quat_vec(q2);

31

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

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

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

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

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

37 return res;

38 }

39

40 static VMATH_INLINE quat_t quat_conjugate(quat_t q)

41 {

42 q.x = -q.x;

43 q.y = -q.y;

44 q.z = -q.z;

45 return q;

46 }

47

48 static VMATH_INLINE quat_t quat_inverse(quat_t q)

49 {

50 scalar_t lensq = quat_length_sq(q);

51 q = quat_conjugate(q);

52 q.x /= lensq;

53 q.y /= lensq;

54 q.z /= lensq;

55 q.w /= lensq;

56 return q;

57 }

58

59 static VMATH_INLINE void quat_to_mat3(mat3_t res, quat_t q)

60 {

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,

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,

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

64 }

65

66 static VMATH_INLINE void quat_to_mat4(mat4_t res, quat_t q)

67 {

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,

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,

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,

71 0, 0, 0, 1);

72 }

73

74 #ifdef __cplusplus

75 } /* extern "C" */

76

77 VMATH_INLINE Quaternion lerp(const Quaternion &a, const Quaternion &b, scalar_t t)

78 {

79 return slerp(a, b, t);

80 }

81 #endif /* __cplusplus */