rayzor
view src/quat.h @ 20:6b11a3f8706e
foo
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Tue, 15 Apr 2014 01:36:03 +0300 |
| parents | |
| children |
line source
1 #ifndef QUATERNION_H_
2 #define QUATERNION_H_
4 class Quat;
6 inline Quat operator *(const Quat &a, const Quat &b);
8 class Quat{
9 public:
10 float x, y, z, w;
12 Quat() : x(0), y(0), z(0), w(1) {}
13 Quat(float xx, float yy, float zz, float ww) : x(xx), y(yy), z(zz), w(ww) {}
15 void set_identity()
16 {
17 x = y = z = 0.0f;
18 w = 1.0f;
19 }
21 Quat conjugate() const
22 {
23 return Quat(-x, -y, -z, w);
24 }
26 float length() const
27 {
28 return (float)sqrt(x * x + y * y + z * z + w * w);
29 }
31 float length_sq() const
32 {
33 return x * x + y * y + z * z + w * w;
34 }
36 void normalize()
37 {
38 float len = length();
39 if(len != 0.0) {
40 x /= len;
41 y /= len;
42 z /= len;
43 w /= len;
44 }
45 }
47 Quat inverse() const
48 {
49 Quat inv = conjugate();
50 float len_sq = length_sq();
51 if(len_sq != 0.0) {
52 inv.x /= len_sq;
53 inv.y /= len_sq;
54 inv.z /= len_sq;
55 inv.w /= len_sq;
56 }
57 return inv;
58 }
60 void set_rotation(float angle, float axis_x, float axis_y, float axis_z)
61 {
62 float half_angle = angle * 0.5;
63 float sin_half = sin(half_angle);
65 w = cos(half_angle);
66 x = axis_x * sin_half;
67 y = axis_y * sin_half;
68 z = axis_z * sin_half;
69 }
71 void rotate(float angle, float x, float y, float z)
72 {
73 Quat q;
74 q.set_rotation(angle, x, y, z);
75 *this = *this * q;
76 }
78 void rotate(const Quat &q)
79 {
80 *this = q * *this * q.conjugate();
81 }
83 Matrix4x4 get_matrix() const
84 {
85 return Matrix4x4(
86 1.0 - 2.0 * y*y - 2.0 * z*z, 2.0 * x * y - 2.0 * w * z, 2.0 * z * x + 2.0 * w * y, 0,
87 2.0 * x * y + 2.0 * w * z, 1.0 - 2.0 * x*x - 2.0 * z*z, 2.0 * y * z - 2.0 * w * x, 0,
88 2.0 * z * x - 2.0 * w * y, 2.0 * y * z + 2.0 * w * x, 1.0 - 2.0 * x*x - 2.0 * y*y, 0,
89 0, 0, 0, 1);
90 }
91 };
93 inline Quat operator *(const Quat &a, const Quat &b)
94 {
95 float dot = a.x * b.x + a.y * b.y + a.z * b.z;
96 float cross_x = a.y * b.z - a.z * b.y;
97 float cross_y = a.z * b.x - a.x * b.z;
98 float cross_z = a.x * b.y - a.y * b.x;
100 float w = a.w * b.w - dot;
101 float x = a.x * b.w + b.x * a.w + cross_x;
102 float y = a.y * b.w + b.y * a.w + cross_y;
103 float z = a.z * b.w + b.z * a.w + cross_z;
105 return Quat(x, y, z, w);
106 }
108 #endif // QUATERNION_H_