goat3d

view libs/vmath/quat.cc @ 51:fa5c52ea9d59

find changesets by author, revision, files, or words in the commit message
foo
author John Tsiombikas <nuclear@member.fsf.org>
date Fri, 17 Jan 2014 18:16:09 +0200
parents
children
line source
1 #include "quat.h"
2 #include "vmath.h"
3
4 Quaternion::Quaternion()
5 {
6 s = 1.0;
7 v.x = v.y = v.z = 0.0;
8 }
9
10 Quaternion::Quaternion(scalar_t s, const Vector3 &v)
11 {
12 this->s = s;
13 this->v = v;
14 }
15
16 Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z)
17 {
18 v.x = x;
19 v.y = y;
20 v.z = z;
21 this->s = s;
22 }
23
24 Quaternion::Quaternion(const Vector3 &axis, scalar_t angle)
25 {
26 set_rotation(axis, angle);
27 }
28
29 Quaternion::Quaternion(const quat_t &quat)
30 {
31 v.x = quat.x;
32 v.y = quat.y;
33 v.z = quat.z;
34 s = quat.w;
35 }
36
37 Quaternion Quaternion::operator +(const Quaternion &quat) const
38 {
39 return Quaternion(s + quat.s, v + quat.v);
40 }
41
42 Quaternion Quaternion::operator -(const Quaternion &quat) const
43 {
44 return Quaternion(s - quat.s, v - quat.v);
45 }
46
47 Quaternion Quaternion::operator -() const
48 {
49 return Quaternion(-s, -v);
50 }
51
52 /** Quaternion Multiplication:
53 * Q1*Q2 = [s1*s2 - v1.v2, s1*v2 + s2*v1 + v1(x)v2]
54 */
55 Quaternion Quaternion::operator *(const Quaternion &quat) const
56 {
57 Quaternion newq;
58 newq.s = s * quat.s - dot_product(v, quat.v);
59 newq.v = quat.v * s + v * quat.s + cross_product(v, quat.v);
60 return newq;
61 }
62
63 void Quaternion::operator +=(const Quaternion &quat)
64 {
65 *this = Quaternion(s + quat.s, v + quat.v);
66 }
67
68 void Quaternion::operator -=(const Quaternion &quat)
69 {
70 *this = Quaternion(s - quat.s, v - quat.v);
71 }
72
73 void Quaternion::operator *=(const Quaternion &quat)
74 {
75 *this = *this * quat;
76 }
77
78 void Quaternion::reset_identity()
79 {
80 s = 1.0;
81 v.x = v.y = v.z = 0.0;
82 }
83
84 Quaternion Quaternion::conjugate() const
85 {
86 return Quaternion(s, -v);
87 }
88
89 scalar_t Quaternion::length() const
90 {
91 return (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
92 }
93
94 /** Q * ~Q = ||Q||^2 */
95 scalar_t Quaternion::length_sq() const
96 {
97 return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
98 }
99
100 void Quaternion::normalize()
101 {
102 scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
103 v.x /= len;
104 v.y /= len;
105 v.z /= len;
106 s /= len;
107 }
108
109 Quaternion Quaternion::normalized() const
110 {
111 Quaternion nq = *this;
112 scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
113 nq.v.x /= len;
114 nq.v.y /= len;
115 nq.v.z /= len;
116 nq.s /= len;
117 return nq;
118 }
119
120 /** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */
121 Quaternion Quaternion::inverse() const
122 {
123 Quaternion inv = conjugate();
124 scalar_t lensq = length_sq();
125 inv.v /= lensq;
126 inv.s /= lensq;
127
128 return inv;
129 }
130
131
132 void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle)
133 {
134 scalar_t half_angle = angle / 2.0;
135 s = cos(half_angle);
136 v = axis * sin(half_angle);
137 }
138
139 void Quaternion::rotate(const Vector3 &axis, scalar_t angle)
140 {
141 Quaternion q;
142 scalar_t half_angle = angle / 2.0;
143 q.s = cos(half_angle);
144 q.v = axis * sin(half_angle);
145
146 *this *= q;
147 }
148
149 void Quaternion::rotate(const Quaternion &q)
150 {
151 *this = q * *this * q.conjugate();
152 }
153
154 Matrix3x3 Quaternion::get_rotation_matrix() const
155 {
156 return Matrix3x3(
157 1.0 - 2.0 * v.y*v.y - 2.0 * v.z*v.z, 2.0 * v.x * v.y - 2.0 * s * v.z, 2.0 * v.z * v.x + 2.0 * s * v.y,
158 2.0 * v.x * v.y + 2.0 * s * v.z, 1.0 - 2.0 * v.x*v.x - 2.0 * v.z*v.z, 2.0 * v.y * v.z - 2.0 * s * v.x,
159 2.0 * v.z * v.x - 2.0 * s * v.y, 2.0 * v.y * v.z + 2.0 * s * v.x, 1.0 - 2.0 * v.x*v.x - 2.0 * v.y*v.y);
160 }
161
162
163 /** Spherical linear interpolation (slerp) */
164 Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, scalar_t t)
165 {
166 Quaternion q1;
167 scalar_t dot = q1.s * q2.s + q1.v.x * q2.v.x + q1.v.y * q2.v.y + q1.v.z * q2.v.z;
168
169 if(dot < 0.0) {
170 /* make sure we interpolate across the shortest arc */
171 q1 = -quat1;
172 dot = -dot;
173 } else {
174 q1 = quat1;
175 }
176
177 /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
178 * floating point imprecisions
179 */
180 if(dot < -1.0) dot = -1.0;
181 if(dot > 1.0) dot = 1.0;
182
183 scalar_t angle = acos(dot);
184 scalar_t a, b;
185
186 scalar_t sin_angle = sin(angle);
187 if(fabs(sin_angle) < SMALL_NUMBER) {
188 /* for very small angles or completely opposite orientations
189 * use linear interpolation to avoid div/zero (in the first case it makes sense,
190 * the second case is pretty much undefined anyway I guess ...
191 */
192 a = 1.0f - t;
193 b = t;
194 } else {
195 a = sin((1.0f - t) * angle) / sin_angle;
196 b = sin(t * angle) / sin_angle;
197 }
198
199 scalar_t x = q1.v.x * a + q2.v.x * b;
200 scalar_t y = q1.v.y * a + q2.v.y * b;
201 scalar_t z = q1.v.z * a + q2.v.z * b;
202 scalar_t s = q1.s * a + q2.s * b;
203
204 return Quaternion(s, Vector3(x, y, z));
205 }
206
207
208 std::ostream &operator <<(std::ostream &out, const Quaternion &q)
209 {
210 out << "(" << q.s << ", " << q.v << ")";
211 return out;
212 }