Line data Source code
1 : /*
2 : * Copyright (c) 2006-2016, Visualization and Multimedia Lab,
3 : * University of Zurich <http://vmml.ifi.uzh.ch>,
4 : * Eyescale Software GmbH,
5 : * Blue Brain Project, EPFL
6 : *
7 : * This file is part of VMMLib <https://github.com/VMML/vmmlib/>
8 : *
9 : * Redistribution and use in source and binary forms, with or without
10 : * modification, are permitted provided that the following conditions are met:
11 : *
12 : * Redistributions of source code must retain the above copyright notice, this
13 : * list of conditions and the following disclaimer. Redistributions in binary
14 : * form must reproduce the above copyright notice, this list of conditions and
15 : * the following disclaimer in the documentation and/or other materials provided
16 : * with the distribution. Neither the name of the Visualization and Multimedia
17 : * Lab, University of Zurich nor the names of its contributors may be used to
18 : * endorse or promote products derived from this software without specific prior
19 : * written permission.
20 : * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 : * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 : * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23 : * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24 : * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25 : * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26 : * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27 : * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28 : * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29 : * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 : * POSSIBILITY OF SUCH DAMAGE.
31 : */
32 :
33 : #ifndef __VMML__QUATERNION__HPP__
34 : #define __VMML__QUATERNION__HPP__
35 :
36 : #include <vmmlib/types.hpp>
37 : #include <vmmlib/vector.hpp> // used inline
38 :
39 : #include <algorithm>
40 : #include <cmath>
41 : #include <cstdlib>
42 : #include <iomanip>
43 : #include <iostream>
44 : #include <limits>
45 : #include <type_traits>
46 :
47 : namespace vmml
48 : {
49 : template <typename T>
50 : class Quaternion
51 : {
52 : public:
53 : /** Construct an identity quaternion */
54 2 : Quaternion()
55 2 : : array()
56 : {
57 2 : array[3] = 1.;
58 2 : }
59 : Quaternion(T x, T y, T z, T w);
60 :
61 : /** Construct a rotation quaternion */
62 : Quaternion(T angle, vector<3, T> axis);
63 :
64 : // uses the top-left 3x3 part of the supplied matrix as rotation matrix
65 : template <size_t M>
66 : Quaternion(const Matrix<M, M, T>& rotation_matrix_,
67 : typename std::enable_if<M >= 3>::type* = 0);
68 :
69 : /** @name Data Access */
70 : //@{
71 : /** @return true if the two quaternion are similar. */
72 : bool equals(const Quaternion& other,
73 : T tolerance = std::numeric_limits<T>::epsilon()) const;
74 :
75 2 : T x() const { return array[0]; }
76 2 : T y() const { return array[1]; }
77 2 : T z() const { return array[2]; }
78 2 : T w() const { return array[3]; }
79 : /** @return true if both quaternions are equal. */
80 : bool operator==(const Quaternion& a) const;
81 :
82 : /** @return true if both quaternions are not equal. */
83 : bool operator!=(const Quaternion& a) const;
84 :
85 : /** @return the negated quaternion of this quaternion. */
86 : Quaternion operator-() const;
87 :
88 : /** @return multiplicative inverse quaternion */
89 : Quaternion inverse() const;
90 :
91 : Quaternion getConjugate() const;
92 :
93 : T abs() const;
94 : T absSquare() const;
95 :
96 : /** @return the corresponding 3x3 rotation matrix. */
97 : Matrix<3, 3, T> getRotationMatrix() const;
98 : //@}
99 :
100 : void normalize();
101 :
102 : Quaternion& operator=(const Quaternion& other);
103 :
104 : /** @name quaternion/quaternion operations */
105 : //@{
106 : Quaternion operator+(const Quaternion<T>& a) const;
107 : Quaternion operator-(const Quaternion<T>& a) const;
108 :
109 : // caution: a * q != q * a in general
110 : Quaternion operator*(const Quaternion<T>& a) const;
111 : void operator+=(const Quaternion<T>& a);
112 : void operator-=(const Quaternion<T>& a);
113 :
114 : // caution: a *= q != q *= a in general
115 : void operator*=(const Quaternion<T>& a);
116 : //@}
117 :
118 : /** @name quaternion/scalar operations */
119 : //@{
120 : Quaternion operator*(T a) const;
121 : Quaternion operator/(T a) const;
122 :
123 : void operator*=(T a);
124 : void operator/=(T a);
125 : //@}
126 :
127 1 : friend std::ostream& operator<<(std::ostream& os, const Quaternion& q)
128 : {
129 1 : const std::ios::fmtflags flags = os.flags();
130 1 : const int prec = os.precision();
131 :
132 1 : os.setf(std::ios::right, std::ios::adjustfield);
133 1 : os.precision(5);
134 :
135 2 : os << "( " << std::setw(10) << q.x() << " " << std::setw(10) << q.y()
136 2 : << " " << std::setw(10) << q.z() << " " << std::setw(10) << q.w()
137 : << " "
138 1 : << ")";
139 :
140 1 : os.precision(prec);
141 1 : os.setf(flags);
142 1 : return os;
143 : }
144 :
145 : T array[4];
146 : };
147 : }
148 :
149 : #include <vmmlib/matrix.hpp>
150 :
151 : namespace vmml
152 : {
153 : /** @name Free quaternion functions */
154 : //@{
155 : template <typename T>
156 : T dot(const Quaternion<T>& p, const Quaternion<T>& q)
157 : {
158 : return p.array[3] * q.array[3] + p.array[0] * q.array[0] +
159 : p.array[1] * q.array[1] + p.array[2] * q.array[2];
160 : }
161 :
162 : template <typename T>
163 : vector<3, T> cross(const Quaternion<T>& p, const Quaternion<T>& q)
164 : {
165 : return vector<3, T>(p.array[1] * q.array[2] - p.array[2] * q.array[1],
166 : p.array[2] * q.array[0] - p.array[0] * q.array[2],
167 : p.array[0] * q.array[1] - p.array[1] * q.array[0]);
168 : }
169 : //@}
170 :
171 : template <typename T>
172 3 : Quaternion<T>::Quaternion(T x_, T y_, T z_, T w_)
173 : {
174 3 : array[0] = x_;
175 3 : array[1] = y_;
176 3 : array[2] = z_;
177 3 : array[3] = w_;
178 3 : }
179 :
180 : template <typename T>
181 : template <size_t M>
182 : Quaternion<T>::Quaternion(const Matrix<M, M, T>& rot,
183 : typename std::enable_if<M >= 3>::type*)
184 : {
185 : const T trace = rot(0, 0) + rot(1, 1) + rot(2, 2) + T(1);
186 : static const T TRACE_EPSILON = 1e-5;
187 :
188 : // very small traces may introduce a big numerical error
189 : if (trace > TRACE_EPSILON)
190 : {
191 : T s = 0.5 / std::sqrt(trace);
192 : array[0] = rot(2, 1) - rot(1, 2);
193 : array[0] *= s;
194 :
195 : array[1] = rot(0, 2) - rot(2, 0);
196 : array[1] *= s;
197 :
198 : array[2] = rot(1, 0) - rot(0, 1);
199 : array[2] *= s;
200 :
201 : array[3] = 0.25 / s;
202 : }
203 : else
204 : {
205 : const vector<3, T> diag(rot(0, 0), rot(1, 1), rot(2, 2));
206 : const size_t largest = diag.find_max_index();
207 :
208 : // 0, 0 is largest
209 : if (largest == 0)
210 : {
211 : const T s =
212 : 0.5 / std::sqrt(T(1) + rot(0, 0) - rot(1, 1) - rot(2, 2));
213 : array[0] = 0.25 / s;
214 :
215 : array[1] = rot(0, 1) + rot(1, 0);
216 : array[1] *= s;
217 :
218 : array[2] = rot(0, 2) + rot(2, 0);
219 : array[2] *= s;
220 :
221 : array[3] = rot(1, 2) - rot(2, 1);
222 : array[3] *= s;
223 : }
224 : else if (largest == 1)
225 : {
226 : const T s =
227 : 0.5 / std::sqrt(T(1) + rot(1, 1) - rot(0, 0) - rot(2, 2));
228 : array[0] = rot(0, 1) + rot(1, 0);
229 : array[0] *= s;
230 :
231 : array[1] = 0.25 / s;
232 :
233 : array[2] = rot(1, 2) + rot(2, 1);
234 : array[2] *= s;
235 :
236 : array[3] = rot(0, 2) - rot(2, 0);
237 : array[3] *= s;
238 : }
239 : // 2, 2 is largest
240 : else if (largest == 2)
241 : {
242 : const T s =
243 : 0.5 / std::sqrt(T(1) + rot(2, 2) - rot(0, 0) - rot(1, 1));
244 : array[0] = rot(0, 2) + rot(2, 0);
245 : array[0] *= s;
246 :
247 : array[1] = rot(1, 2) + rot(2, 1);
248 : array[1] *= s;
249 :
250 : array[2] = 0.25 / s;
251 :
252 : array[3] = rot(0, 1) - rot(1, 0);
253 : array[3] *= s;
254 : }
255 : else
256 : {
257 : throw std::runtime_error("no clue why, but should not get here");
258 : }
259 : }
260 : }
261 :
262 : template <typename T>
263 4 : Quaternion<T>::Quaternion(const T angle, vector<3, T> axis)
264 : {
265 4 : axis.normalize();
266 4 : const T sinAngle2 = std::sin(angle * 0.5);
267 4 : array[0] = axis.x() * sinAngle2;
268 4 : array[1] = axis.y() * sinAngle2;
269 4 : array[2] = axis.z() * sinAngle2;
270 4 : array[3] = std::cos(angle * 0.5);
271 4 : }
272 :
273 : template <typename T>
274 2 : bool Quaternion<T>::equals(const Quaternion& other, const T tolerance) const
275 : {
276 4 : return std::abs(array[0] - other.array[0]) <= tolerance &&
277 4 : std::abs(array[1] - other.array[1]) <= tolerance &&
278 6 : std::abs(array[2] - other.array[2]) <= tolerance &&
279 4 : std::abs(array[3] - other.array[3]) <= tolerance;
280 : }
281 :
282 : template <typename T>
283 3 : bool Quaternion<T>::operator==(const Quaternion& rhs) const
284 : {
285 9 : return array[0] == rhs.array[0] && array[1] == rhs.array[1] &&
286 9 : array[2] == rhs.array[2] && array[3] == rhs.array[3];
287 : }
288 :
289 : template <typename T>
290 : bool Quaternion<T>::operator!=(const Quaternion& a) const
291 : {
292 : return !this->operator==(a);
293 : }
294 :
295 : template <typename T>
296 : Quaternion<T> Quaternion<T>::getConjugate() const
297 : {
298 : return Quaternion<T>(-array[0], -array[1], -array[2], array[3]);
299 : }
300 :
301 : template <typename T>
302 : T Quaternion<T>::abs() const
303 : {
304 : return std::sqrt(absSquare());
305 : }
306 :
307 : template <typename T>
308 : T Quaternion<T>::absSquare() const
309 : {
310 : return array[0] * array[0] + array[1] * array[1] + array[2] * array[2] +
311 : array[3] * array[3];
312 : }
313 :
314 : template <typename T>
315 : Quaternion<T> Quaternion<T>::inverse() const
316 : {
317 : const Quaternion<T>& q = getConjugate();
318 : const T tmp = T(1) / absSquare();
319 : return q * tmp;
320 : }
321 :
322 : template <typename T>
323 : void Quaternion<T>::normalize()
324 : {
325 : T len = abs();
326 : if (len == T(0))
327 : return;
328 : len = T(1) / len;
329 : this->operator*=(len);
330 : }
331 :
332 : //
333 : // Quaternion/Quaternion operations
334 : //
335 :
336 : template <typename T>
337 : Quaternion<T> Quaternion<T>::operator+(const Quaternion<T>& rhs) const
338 : {
339 : return Quaternion(array[0] + rhs.array[0], array[1] + rhs.array[1],
340 : array[2] + rhs.array[2], array[3] + rhs.array[3]);
341 : }
342 :
343 : template <typename T>
344 : Quaternion<T> Quaternion<T>::operator-(const Quaternion<T>& rhs) const
345 : {
346 : return Quaternion(array[0] - rhs.array[0], array[1] - rhs.array[1],
347 : array[2] - rhs.array[2], array[3] - rhs.array[3]);
348 : }
349 :
350 : // returns Grasssmann product
351 : template <typename T>
352 1 : Quaternion<T> Quaternion<T>::operator*(const Quaternion<T>& rhs) const
353 : {
354 1 : Quaternion<T> ret(*this);
355 1 : ret *= rhs;
356 1 : return ret;
357 : }
358 :
359 : // Grassmann product
360 : template <typename T>
361 1 : void Quaternion<T>::operator*=(const Quaternion<T>& q)
362 : {
363 : // optimized version, 7 less mul, but 15 more add/subs
364 : // after Henrik Engstrom, from a gamedev.net article.
365 1 : const T& a_ = array[3];
366 1 : const T& b_ = array[0];
367 1 : const T& c = array[1];
368 1 : const T& d = array[2];
369 1 : const T& _x = q.array[3];
370 1 : const T& _y = q.array[0];
371 1 : const T& _z = q.array[1];
372 1 : const T& _w = q.array[2];
373 :
374 1 : const T tmp_00 = (d - c) * (_z - _w);
375 1 : const T tmp_01 = (a_ + b_) * (_x + _y);
376 1 : const T tmp_02 = (a_ - b_) * (_z + _w);
377 1 : const T tmp_03 = (c + d) * (_x - _y);
378 1 : const T tmp_04 = (d - b_) * (_y - _z);
379 1 : const T tmp_05 = (d + b_) * (_y + _z);
380 1 : const T tmp_06 = (a_ + c) * (_x - _w);
381 1 : const T tmp_07 = (a_ - c) * (_x + _w);
382 1 : const T tmp_08 = tmp_05 + tmp_06 + tmp_07;
383 1 : const T tmp_09 = 0.5 * (tmp_04 + tmp_08);
384 :
385 1 : array[0] = tmp_01 + tmp_09 - tmp_08;
386 1 : array[1] = tmp_02 + tmp_09 - tmp_07;
387 1 : array[2] = tmp_03 + tmp_09 - tmp_06;
388 1 : array[3] = tmp_00 + tmp_09 - tmp_05;
389 1 : }
390 :
391 : template <typename T>
392 : Quaternion<T> Quaternion<T>::operator-() const
393 : {
394 : return Quaternion(-array[0], -array[1], -array[2], -array[3]);
395 : }
396 :
397 : template <typename T>
398 : void Quaternion<T>::operator+=(const Quaternion<T>& q)
399 : {
400 : array[0] += q.array[0];
401 : array[1] += q.array[1];
402 : array[2] += q.array[2];
403 : array[3] += q.array[3];
404 : }
405 :
406 : template <typename T>
407 : void Quaternion<T>::operator-=(const Quaternion<T>& q)
408 : {
409 : array[0] -= q.array[0];
410 : array[1] -= q.array[1];
411 : array[2] -= q.array[2];
412 : array[3] -= q.array[3];
413 : }
414 :
415 : //
416 : // Quaternion/scalar operations
417 : //
418 :
419 : template <typename T>
420 : Quaternion<T> Quaternion<T>::operator*(const T a_) const
421 : {
422 : return Quaternion(array[0] * a_, array[1] * a_, array[2] * a_,
423 : array[3] * a_);
424 : }
425 :
426 : template <typename T>
427 : Quaternion<T> Quaternion<T>::operator/(T a_) const
428 : {
429 : if (a_ == T(0))
430 : throw std::runtime_error("Division by zero.");
431 :
432 : a_ = T(1) / a_;
433 : return Quaternion(array[0] * a_, array[1] * a_, array[2] * a_,
434 : array[3] * a_);
435 : }
436 :
437 : template <typename T>
438 : void Quaternion<T>::operator*=(T q)
439 : {
440 : array[0] *= q;
441 : array[1] *= q;
442 : array[2] *= q;
443 : array[3] *= q;
444 : }
445 :
446 : template <typename T>
447 : void Quaternion<T>::operator/=(T q)
448 : {
449 : if (q == T(0))
450 : throw std::runtime_error("Division by zero");
451 :
452 : q = T(1) / q;
453 : this->operator*=(q);
454 : }
455 :
456 : template <typename T>
457 1 : Matrix<3, 3, T> Quaternion<T>::getRotationMatrix() const
458 : {
459 1 : const T w2 = array[3] * array[3];
460 1 : const T x2 = array[0] * array[0];
461 1 : const T y2 = array[1] * array[1];
462 1 : const T z2 = array[2] * array[2];
463 1 : const T wx = array[3] * array[0];
464 1 : const T wy = array[3] * array[1];
465 1 : const T wz = array[3] * array[2];
466 1 : const T xy = array[0] * array[1];
467 1 : const T xz = array[0] * array[2];
468 1 : const T yz = array[1] * array[2];
469 :
470 1 : Matrix<3, 3, T> matrix;
471 1 : matrix(0, 0) = w2 + x2 - y2 - z2;
472 1 : matrix(0, 1) = 2. * (xy - wz);
473 1 : matrix(0, 2) = 2. * (xz + wy);
474 1 : matrix(1, 0) = 2. * (xy + wz);
475 1 : matrix(1, 1) = w2 - x2 + y2 - z2;
476 1 : matrix(1, 2) = 2. * (yz - wx);
477 1 : matrix(2, 0) = 2. * (xz - wy);
478 1 : matrix(2, 1) = 2. * (yz + wx);
479 1 : matrix(2, 2) = w2 - x2 - y2 + z2;
480 1 : return matrix;
481 : }
482 :
483 : template <typename T>
484 : Quaternion<T>& Quaternion<T>::operator=(const Quaternion& other)
485 : {
486 : ::memcpy(array, other.array, 4 * sizeof(T));
487 : return *this;
488 : }
489 : }
490 : #endif
|
