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/enable_if.hpp>
37 : #include <vmmlib/types.hpp>
38 : #include <vmmlib/vector.hpp> // used inline
39 :
40 : #include <algorithm>
41 : #include <cmath>
42 : #include <cstdlib>
43 : #include <iomanip>
44 : #include <iostream>
45 : #include <limits>
46 :
47 : namespace vmml
48 : {
49 : template < typename T > class Quaternion
50 : {
51 : public:
52 : /** Construct an identity quaternion */
53 2 : Quaternion() : array() { array[3] = 1.; }
54 : Quaternion( T x, T y, T z, T w );
55 :
56 : /** Construct a rotation quaternion */
57 : Quaternion( T angle, vector< 3, T > axis );
58 :
59 : // uses the top-left 3x3 part of the supplied matrix as rotation matrix
60 : template< size_t M >
61 : Quaternion( const Matrix< M, M, T >& rotation_matrix_,
62 : typename enable_if< M >= 3 >::type* = 0 );
63 :
64 : /** @name Data Access */
65 : //@{
66 : /** @return true if the two quaternion are similar. */
67 : bool equals( const Quaternion& other,
68 : T tolerance = std::numeric_limits< T >::epsilon( )) const;
69 :
70 0 : T x() const { return array[0]; }
71 0 : T y() const { return array[1]; }
72 0 : T z() const { return array[2]; }
73 0 : T w() const { return array[3]; }
74 :
75 : /** @return true if both quaternions are equal. */
76 : bool operator==( const Quaternion& a ) const;
77 :
78 : /** @return true if both quaternions are not equal. */
79 : bool operator!=( const Quaternion& a ) const;
80 :
81 : /** @return the negated quaternion of this quaternion. */
82 : Quaternion operator-() const;
83 :
84 : /** @return multiplicative inverse quaternion */
85 : Quaternion inverse() const;
86 :
87 : Quaternion getConjugate() const;
88 :
89 : T abs() const;
90 : T absSquare() const;
91 :
92 : /** @return the corresponding 3x3 rotation matrix. */
93 : Matrix< 3, 3, T > getRotationMatrix() const;
94 : //@}
95 :
96 : void normalize();
97 :
98 : Quaternion& operator=(const Quaternion& other);
99 :
100 : /** @name quaternion/quaternion operations */
101 : //@{
102 : Quaternion operator+( const Quaternion< T >& a ) const;
103 : Quaternion operator-( const Quaternion< T >& a ) const;
104 :
105 : // caution: a * q != q * a in general
106 : Quaternion operator*( const Quaternion< T >& a ) const;
107 : void operator+=( const Quaternion< T >& a );
108 : void operator-=( const Quaternion< T >& a );
109 :
110 : // caution: a *= q != q *= a in general
111 : void operator*=( const Quaternion< T >& a );
112 : //@}
113 :
114 : /** @name quaternion/scalar operations */
115 : //@{
116 : Quaternion operator*( T a ) const;
117 : Quaternion operator/( T a ) const;
118 :
119 : void operator*=( T a );
120 : void operator/=( T a );
121 : //@}
122 :
123 0 : friend std::ostream& operator<< ( std::ostream& os, const Quaternion& q )
124 : {
125 0 : const std::ios::fmtflags flags = os.flags();
126 0 : const int prec = os.precision();
127 :
128 0 : os.setf( std::ios::right, std::ios::adjustfield );
129 0 : os.precision( 5 );
130 :
131 0 : os << "( "
132 0 : << std::setw(10) << q.x() << " "
133 0 : << std::setw(10) << q.y() << " "
134 0 : << std::setw(10) << q.z() << " "
135 0 : << std::setw(10) << q.w() << " "
136 0 : << ")";
137 :
138 0 : os.precision( prec );
139 0 : os.setf( flags );
140 0 : return os;
141 : }
142 :
143 : private:
144 : T array[4];
145 : };
146 : }
147 :
148 : #include <vmmlib/matrix.hpp>
149 :
150 : namespace vmml
151 : {
152 : /** @name Free quaternion functions */
153 : //@{
154 : template < typename T >
155 : T dot( const Quaternion< T >& p, const Quaternion< T >& q )
156 : {
157 : return p.array[3] * q.array[3] + p.array[0] * q.array[0] +
158 : p.array[1] * q.array[1] + p.array[2] * q.array[2];
159 : }
160 :
161 : template < typename T >
162 : vector< 3, T > cross( const Quaternion< T >& p, const Quaternion< T >& q )
163 : {
164 : return vector< 3, T >( p.array[1] * q.array[2] - p.array[2] * q.array[1],
165 : p.array[2] * q.array[0] - p.array[0] * q.array[2],
166 : p.array[0] * q.array[1] - p.array[1] * q.array[0] );
167 : }
168 : //@}
169 :
170 3 : template < typename T > Quaternion< T >::Quaternion( T x_, T y_, T z_, T w_ )
171 : {
172 3 : array[0] = x_;
173 3 : array[1] = y_;
174 3 : array[2] = z_;
175 3 : array[3] = w_;
176 3 : }
177 :
178 : template< typename T > template< size_t M >
179 : Quaternion< T >::Quaternion( const Matrix< M, M, T >& rot,
180 : typename enable_if< M >= 3 >::type* )
181 : {
182 : const T trace = rot( 0, 0 ) + rot( 1, 1 ) + rot( 2,2 ) + T( 1 );
183 : static const T TRACE_EPSILON = 1e-5;
184 :
185 : // very small traces may introduce a big numerical error
186 : if( trace > TRACE_EPSILON )
187 : {
188 : T s = 0.5 / std::sqrt( trace );
189 : array[0] = rot( 2, 1 ) - rot( 1, 2 );
190 : array[0] *= s;
191 :
192 : array[1] = rot( 0, 2 ) - rot( 2, 0 );
193 : array[1] *= s;
194 :
195 : array[2] = rot( 1, 0 ) - rot( 0, 1 );
196 : array[2] *= s;
197 :
198 : array[3] = 0.25 / s;
199 : }
200 : else
201 : {
202 : const vector< 3, T > diag( rot( 0, 0 ), rot( 1, 1 ), rot( 2, 2 ) );
203 : const size_t largest = diag.find_max_index();
204 :
205 : // 0, 0 is largest
206 : if ( largest == 0 )
207 : {
208 : const T s = 0.5 / std::sqrt( T( 1 ) + rot( 0,0 ) - rot( 1,1 ) -
209 : rot( 2,2 ));
210 : array[0] = 0.25 / s;
211 :
212 : array[1] = rot( 0,1 ) + rot( 1,0 );
213 : array[1] *= s;
214 :
215 : array[2] = rot( 0,2 ) + rot( 2,0 );
216 : array[2] *= s;
217 :
218 : array[3] = rot( 1,2 ) - rot( 2,1 );
219 : array[3] *= s;
220 : }
221 : else if ( largest == 1 )
222 : {
223 : const T s = 0.5 / std::sqrt( T( 1 ) + rot( 1,1 ) - rot( 0,0 ) -
224 : rot( 2,2 ));
225 : array[0] = rot( 0,1 ) + rot( 1,0 );
226 : array[0] *= s;
227 :
228 : array[1] = 0.25 / s;
229 :
230 : array[2] = rot( 1,2 ) + rot( 2,1 );
231 : array[2] *= s;
232 :
233 : array[3] = rot( 0,2 ) - rot( 2,0 );
234 : array[3] *= s;
235 : }
236 : // 2, 2 is largest
237 : else if ( largest == 2 )
238 : {
239 : const T s = 0.5 / std::sqrt( T( 1 ) + rot( 2,2 ) - rot( 0,0 ) -
240 : rot( 1,1 ));
241 : array[0] = rot( 0,2 ) + rot( 2,0 );
242 : array[0] *= s;
243 :
244 : array[1] = rot( 1,2 ) + rot( 2,1 );
245 : array[1] *= s;
246 :
247 : array[2] = 0.25 / s;
248 :
249 : array[3] = rot( 0,1 ) - rot( 1,0 );
250 : array[3] *= s;
251 : }
252 : else
253 : {
254 : throw std::runtime_error( "no clue why, but should not get here" );
255 : }
256 : }
257 : }
258 :
259 :
260 : template< typename T >
261 4 : Quaternion< T >::Quaternion( const T angle, vector< 3, T > axis )
262 : {
263 4 : axis.normalize();
264 4 : const T sinAngle2 = std::sin( angle * 0.5 );
265 4 : array[0] = axis.x() * sinAngle2;
266 4 : array[1] = axis.y() * sinAngle2;
267 4 : array[2] = axis.z() * sinAngle2;
268 4 : array[3] = std::cos( angle * 0.5 );
269 4 : }
270 :
271 : template< typename T >
272 2 : bool Quaternion< T >::equals( const Quaternion& other, const T tolerance ) const
273 : {
274 4 : return std::abs( array[0] - other.array[0] ) <= tolerance &&
275 4 : std::abs( array[1] - other.array[1] ) <= tolerance &&
276 6 : std::abs( array[2] - other.array[2] ) <= tolerance &&
277 4 : std::abs( array[3] - other.array[3] ) <= tolerance;
278 : }
279 :
280 : template < typename T >
281 3 : bool Quaternion< T >::operator==( const Quaternion& rhs ) const
282 : {
283 9 : return array[0] == rhs.array[0] && array[1] == rhs.array[1] &&
284 9 : array[2] == rhs.array[2] && array[3] == rhs.array[3];
285 : }
286 :
287 : template < typename T >
288 : bool Quaternion< T >::operator!=( const Quaternion& a ) const
289 : {
290 : return ! this->operator==( a );
291 : }
292 :
293 : template < typename T > Quaternion< T > Quaternion< T >::getConjugate() const
294 : {
295 : return Quaternion< T >( -array[0], -array[1], -array[2], array[3] );
296 : }
297 :
298 : template < typename T > T Quaternion< T >::abs() const
299 : {
300 : return std::sqrt( absSquare( ));
301 : }
302 :
303 : template < typename T >T Quaternion< T >::absSquare() const
304 : {
305 : return array[0] * array[0] + array[1] * array[1] +
306 : array[2] * array[2] + array[3] * array[3];
307 : }
308 :
309 : template < typename T > Quaternion< T > Quaternion< T >::inverse() const
310 : {
311 : const Quaternion< T >& q = getConjugate();
312 : const T tmp = T( 1 ) / absSquare();
313 : return q * tmp;
314 : }
315 :
316 : template < typename T > void Quaternion< T >::normalize()
317 : {
318 : T len = abs();
319 : if( len == T( 0 ))
320 : return;
321 : len = T( 1 ) / len;
322 : this->operator*=( len );
323 : }
324 :
325 : //
326 : // Quaternion/Quaternion operations
327 : //
328 :
329 : template < typename T >
330 : Quaternion< T > Quaternion< T >::operator+( const Quaternion< T >& rhs ) const
331 : {
332 : return Quaternion( array[0] + rhs.array[0], array[1] + rhs.array[1],
333 : array[2] + rhs.array[2], array[3] + rhs.array[3] );
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 : // returns Grasssmann product
344 : template < typename T >
345 1 : Quaternion< T > Quaternion< T >::operator*( const Quaternion< T >& rhs ) const
346 : {
347 1 : Quaternion< T > ret( *this );
348 1 : ret *= rhs;
349 1 : return ret;
350 : }
351 :
352 : // Grassmann product
353 : template < typename T >
354 1 : void Quaternion< T >::operator*=( const Quaternion< T >& q )
355 : {
356 : // optimized version, 7 less mul, but 15 more add/subs
357 : // after Henrik Engstrom, from a gamedev.net article.
358 1 : const T& a_ = array[ 3 ];
359 1 : const T& b_ = array[ 0 ];
360 1 : const T& c = array[ 1 ];
361 1 : const T& d = array[ 2 ];
362 1 : const T& _x = q.array[ 3 ];
363 1 : const T& _y = q.array[ 0 ];
364 1 : const T& _z = q.array[ 1 ];
365 1 : const T& _w = q.array[ 2 ];
366 :
367 1 : const T tmp_00 = (d - c) * (_z - _w);
368 1 : const T tmp_01 = (a_ + b_) * (_x + _y);
369 1 : const T tmp_02 = (a_ - b_) * (_z + _w);
370 1 : const T tmp_03 = (c + d) * (_x - _y);
371 1 : const T tmp_04 = (d - b_) * (_y - _z);
372 1 : const T tmp_05 = (d + b_) * (_y + _z);
373 1 : const T tmp_06 = (a_ + c) * (_x - _w);
374 1 : const T tmp_07 = (a_ - c) * (_x + _w);
375 1 : const T tmp_08 = tmp_05 + tmp_06 + tmp_07;
376 1 : const T tmp_09 = 0.5 * (tmp_04 + tmp_08);
377 :
378 1 : array[ 0 ] = tmp_01 + tmp_09 - tmp_08;
379 1 : array[ 1 ] = tmp_02 + tmp_09 - tmp_07;
380 1 : array[ 2 ] = tmp_03 + tmp_09 - tmp_06;
381 1 : array[ 3 ] = tmp_00 + tmp_09 - tmp_05;
382 1 : }
383 :
384 : template < typename T > Quaternion< T > Quaternion< T >::operator-() const
385 : {
386 : return Quaternion( -array[0], -array[1], -array[2], -array[3] );
387 : }
388 :
389 : template < typename T >
390 : void Quaternion< T >::operator+=( const Quaternion< T >& q )
391 : {
392 : array[ 0 ] += q.array[ 0 ];
393 : array[ 1 ] += q.array[ 1 ];
394 : array[ 2 ] += q.array[ 2 ];
395 : array[ 3 ] += q.array[ 3 ];
396 : }
397 :
398 : template < typename T >
399 : void Quaternion< T >::operator-=( const Quaternion< T >& q )
400 : {
401 : array[ 0 ] -= q.array[ 0 ];
402 : array[ 1 ] -= q.array[ 1 ];
403 : array[ 2 ] -= q.array[ 2 ];
404 : array[ 3 ] -= q.array[ 3 ];
405 : }
406 :
407 : //
408 : // Quaternion/scalar operations
409 : //
410 :
411 : template < typename T >
412 : Quaternion< T > Quaternion< T >::operator*( const T a_ ) const
413 : {
414 : return Quaternion( array[0] * a_, array[1] * a_,
415 : array[2] * a_, array[3] * a_ );
416 : }
417 :
418 : template < typename T >
419 : Quaternion< T > Quaternion< T >::operator/( T a_ ) const
420 : {
421 : if ( a_ == T( 0 ))
422 : throw std::runtime_error( "Division by zero." );
423 :
424 : a_ = T( 1 ) / a_;
425 : return Quaternion( array[0] * a_, array[1] * a_, array[2] * a_,
426 : array[3] * a_ );
427 : }
428 :
429 : template < typename T > void Quaternion< T >::operator*=( T q )
430 : {
431 : array[ 0 ] *= q;
432 : array[ 1 ] *= q;
433 : array[ 2 ] *= q;
434 : array[ 3 ] *= q;
435 : }
436 :
437 : template < typename T > void Quaternion< T >::operator/=( T q )
438 : {
439 : if ( q == T( 0 ))
440 : throw std::runtime_error( "Division by zero" );
441 :
442 : q = T( 1 ) / q;
443 : this->operator*=( q );
444 : }
445 :
446 : template < typename T >
447 1 : Matrix< 3, 3, T > Quaternion< T >::getRotationMatrix() const
448 : {
449 1 : const T w2 = array[3] * array[3];
450 1 : const T x2 = array[0] * array[0];
451 1 : const T y2 = array[1] * array[1];
452 1 : const T z2 = array[2] * array[2];
453 1 : const T wx = array[3] * array[0];
454 1 : const T wy = array[3] * array[1];
455 1 : const T wz = array[3] * array[2];
456 1 : const T xy = array[0] * array[1];
457 1 : const T xz = array[0] * array[2];
458 1 : const T yz = array[1] * array[2];
459 :
460 1 : Matrix< 3, 3, T > matrix;
461 1 : matrix( 0, 0 ) = w2 + x2 - y2 - z2;
462 1 : matrix( 0, 1 ) = 2. * (xy - wz);
463 1 : matrix( 0, 2 ) = 2. * (xz + wy);
464 1 : matrix( 1, 0 ) = 2. * (xy + wz);
465 1 : matrix( 1, 1 ) = w2 - x2 + y2 - z2;
466 1 : matrix( 1, 2 ) = 2. * (yz - wx);
467 1 : matrix( 2, 0 ) = 2. * (xz - wy);
468 1 : matrix( 2, 1 ) = 2. * (yz + wx);
469 1 : matrix( 2, 2 ) = w2 - x2 - y2 + z2;
470 1 : return matrix;
471 : }
472 :
473 : template < typename T >
474 : Quaternion< T >& Quaternion< T >::operator=(const Quaternion& other)
475 : {
476 : ::memcpy( array, other.array, 4 * sizeof( T ) );
477 : return *this;
478 : }
479 :
480 : }
481 : #endif
|