Developer Documentation

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
QuaternionT.hh
1 /*===========================================================================*\
2 * *
3 * OpenFlipper *
4 * Copyright (c) 2001-2015, RWTH-Aachen University *
5 * Department of Computer Graphics and Multimedia *
6 * All rights reserved. *
7 * www.openflipper.org *
8 * *
9 *---------------------------------------------------------------------------*
10 * This file is part of OpenFlipper. *
11 *---------------------------------------------------------------------------*
12 * *
13 * Redistribution and use in source and binary forms, with or without *
14 * modification, are permitted provided that the following conditions *
15 * are met: *
16 * *
17 * 1. Redistributions of source code must retain the above copyright notice, *
18 * this list of conditions and the following disclaimer. *
19 * *
20 * 2. Redistributions in binary form must reproduce the above copyright *
21 * notice, this list of conditions and the following disclaimer in the *
22 * documentation and/or other materials provided with the distribution. *
23 * *
24 * 3. Neither the name of the copyright holder nor the names of its *
25 * contributors may be used to endorse or promote products derived from *
26 * this software without specific prior written permission. *
27 * *
28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS *
29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
30 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A *
31 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
32 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, *
33 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, *
34 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR *
35 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF *
36 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING *
37 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
38 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
39 * *
40 \*===========================================================================*/
41
42 /*===========================================================================*\
43 * *
44 * $Revision$ *
45 * $Author$ *
46 * $Date$ *
47 * *
48 \*===========================================================================*/
49
50
51
52 //=============================================================================
53 //
54 // CLASS Quaternion
55 //
56 //=============================================================================
57
58 #ifndef ACG_QUATERNION_HH
59 #define ACG_QUATERNION_HH
60
61
62 //== INCLUDES =================================================================
63
64 #include "VectorT.hh"
65 #include "Matrix4x4T.hh"
66
67
68 //== NAMESPACES ==============================================================
69
70 namespace ACG {
71
72
73 //== CLASS DEFINITION =========================================================
74
75
80 template <class Scalar>
81 class QuaternionT : public VectorT<Scalar,4>
82 {
83 public:
84
85 #define W Base::data()[0]
86 #define X Base::data()[1]
87 #define Y Base::data()[2]
88 #define Z Base::data()[3]
89
90
91 typedef VectorT<Scalar,4> Base;
92 typedef QuaternionT<Scalar> Quaternion;
93 typedef VectorT<Scalar,3> Vec3;
94 typedef VectorT<Scalar,4> Vec4;
95 typedef Matrix4x4T<Scalar> Matrix;
96
97
99 QuaternionT(Scalar _w=1.0, Scalar _x=0.0, Scalar _y=0.0, Scalar _z=0.0)
100 : Vec4(_w, _x, _y, _z) {}
101
103 QuaternionT(const Vec3& _p)
104 : Vec4(0, _p[0], _p[1], _p[2]) {}
105
107 QuaternionT(const Vec4& _p)
108 : Vec4(_p[0], _p[1], _p[2], _p[3]) {}
109
111 QuaternionT(Vec3 _axis, Scalar _angle)
112 {
113 _axis.normalize();
114 Scalar theta = 0.5 * _angle;
115 Scalar sin_theta = sin(theta);
116 W = cos(theta);
117 X = sin_theta * _axis[0];
118 Y = sin_theta * _axis[1];
119 Z = sin_theta * _axis[2];
120 }
121
123 template <class MatrixT>
124 QuaternionT(const MatrixT& _rot)
125 { init_from_matrix( _rot); }
126
127
129 void identity() { W=1.0; X=Y=Z=0.0; }
130
131
133 Quaternion conjugate() const
134 { return Quaternion(W, -X, -Y, -Z); }
135
136
138 Quaternion invert() const
139 { return conjugate()/Base::sqrnorm(); }
140
141
143 Quaternion operator*(const Quaternion& _q) const
144 { return Quaternion(W*_q.W - X*_q.X - Y*_q.Y - Z*_q.Z,
145 W*_q.X + X*_q.W + Y*_q.Z - Z*_q.Y,
146 W*_q.Y - X*_q.Z + Y*_q.W + Z*_q.X,
147 W*_q.Z + X*_q.Y - Y*_q.X + Z*_q.W); }
148
149
151 Quaternion& operator*=(const Quaternion& _q)
152 { return *this = *this * _q; }
153
154
156 template <class Vec3T>
157 Vec3T rotate(const Vec3T& _v) const
158 {
159 Quaternion q = *this * Quaternion(0,_v[0],_v[1],_v[2]) * conjugate();
160 return Vec3T(q[1], q[2], q[3]);
161 }
162
163
165 void axis_angle(Vec3& _axis, Scalar& _angle) const
166 {
167 if (fabs(W) > 0.999999)
168 {
169 _axis = Vec3(1,0,0);
170 _angle = 0;
171 }
172 else
173 {
174 _angle = 2.0 * acos(W);
175 _axis = Vec3(X, Y, Z).normalize();
176 }
177 }
178
179
180
182 Matrix rotation_matrix() const
183 {
184 Scalar
185 ww(W*W), xx(X*X), yy(Y*Y), zz(Z*Z), wx(W*X),
186 wy(W*Y), wz(W*Z), xy(X*Y), xz(X*Z), yz(Y*Z);
187
188 Matrix m;
189
190 m(0,0) = ww + xx - yy - zz;
191 m(1,0) = 2.0*(xy + wz);
192 m(2,0) = 2.0*(xz - wy);
193
194 m(0,1) = 2.0*(xy - wz);
195 m(1,1) = ww - xx + yy - zz;
196 m(2,1) = 2.0*(yz + wx);
197
198 m(0,2) = 2.0*(xz + wy);
199 m(1,2) = 2.0*(yz - wx);
200 m(2,2) = ww - xx - yy + zz;
201
202 m(0,3) = m(1,3) = m(2,3) = m(3,0) = m(3,1) = m(3,2) = 0.0;
203 m(3,3) = 1.0;
204
205 return m;
206 }
207
208
209
210 /*
212 Matrix right_mult_matrix() const
213 {
214 Matrix m;
215 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
216 m(1,0) = X; m(1,1) = W; m(1,2) = -Z; m(1,3) = Y;
217 m(2,0) = Y; m(2,1) = Z; m(2,2) = W; m(2,3) = -X;
218 m(3,0) = Z; m(3,1) = -Y; m(3,2) = X; m(3,3) = W;
219 return m;
220 }
221
222
224 Matrix left_mult_matrix() const
225 {
226 Matrix m;
227 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
228 m(1,0) = X; m(1,1) = W; m(1,2) = Z; m(1,3) = -Y;
229 m(2,0) = Y; m(2,1) = -Z; m(2,2) = W; m(2,3) = X;
230 m(3,0) = Z; m(3,1) = Y; m(3,2) = -X; m(3,3) = W;
231 return m;
232 }
233 */
235 Matrix right_mult_matrix() const
236 {
237 Matrix m;
238 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
239 m(1,0) = X; m(1,1) = W; m(1,2) = Z; m(1,3) = -Y;
240 m(2,0) = Y; m(2,1) = -Z; m(2,2) = W; m(2,3) = X;
241 m(3,0) = Z; m(3,1) = Y; m(3,2) = -X; m(3,3) = W;
242 return m;
243 }
244
245
247 Matrix left_mult_matrix() const
248 {
249 Matrix m;
250 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
251 m(1,0) = X; m(1,1) = W; m(1,2) = -Z; m(1,3) = Y;
252 m(2,0) = Y; m(2,1) = Z; m(2,2) = W; m(2,3) = -X;
253 m(3,0) = Z; m(3,1) = -Y; m(3,2) = X; m(3,3) = W;
254 return m;
255 }
256
257
259 template<class MatrixT>
260 void init_from_matrix( const MatrixT& _rot)
261 {
262 Scalar trace = _rot(0,0) + _rot(1,1) + _rot(2,2);
263 if( trace > 0.0 )
264 {
265 Scalar s = 0.5 / sqrt(trace + 1.0);
266 W = 0.25 / s;
267 X = ( _rot(2,1) - _rot(1,2) ) * s;
268 Y = ( _rot(0,2) - _rot(2,0) ) * s;
269 Z = ( _rot(1,0) - _rot(0,1) ) * s;
270 }
271 else
272 {
273 if ( _rot(0,0) > _rot(1,1) && _rot(0,0) > _rot(2,2) )
274 {
275 Scalar s = 2.0 * sqrt( 1.0 + _rot(0,0) - _rot(1,1) - _rot(2,2));
276 W = (_rot(2,1) - _rot(1,2) ) / s;
277 X = 0.25 * s;
278 Y = (_rot(0,1) + _rot(1,0) ) / s;
279 Z = (_rot(0,2) + _rot(2,0) ) / s;
280 }
281 else
282 if (_rot(1,1) > _rot(2,2))
283 {
284 Scalar s = 2.0 * sqrt( 1.0 + _rot(1,1) - _rot(0,0) - _rot(2,2));
285 W = (_rot(0,2) - _rot(2,0) ) / s;
286 X = (_rot(0,1) + _rot(1,0) ) / s;
287 Y = 0.25 * s;
288 Z = (_rot(1,2) + _rot(2,1) ) / s;
289 }
290 else
291 {
292 Scalar s = 2.0 * sqrt( 1.0 + _rot(2,2) - _rot(0,0) - _rot(1,1) );
293 W = (_rot(1,0) - _rot(0,1) ) / s;
294 X = (_rot(0,2) + _rot(2,0) ) / s;
295 Y = (_rot(1,2) + _rot(2,1) ) / s;
296 Z = 0.25 * s;
297 }
298 }
299 }
300
301
303 Quaternion exponential() const
304 {
305 Vec3 n(X,Y,Z);
306 Scalar theta( n.norm());
307 Scalar sin_theta = sin(theta);
308 Scalar cos_theta = cos(theta);
309
310 if( theta > 1e-6 )
311 n *= sin_theta/theta;
312 else
313 n = Vec3(0,0,0);
314
315 return Quaternion( cos_theta, n[0], n[1], n[2]);
316 }
317
318
320 Quaternion logarithm() const
321 {
322 // clamp to valid input
323 double w = W;
324 if( w > 1.0) w = 1.0;
325 else if( w < -1.0) w = -1.0;
326
327 Scalar theta_half = acos(w);
328
329 Vec3 n(X,Y,Z);
330 Scalar n_norm( n.norm());
331
332 if( n_norm > 1e-6 )
333 n *= theta_half/n_norm;
334 else
335 n = Vec3(0,0,0);
336
337 return Quaternion( 0, n[0], n[1], n[2]);
338 }
339
340 void print_info()
341 {
342 // get axis, angle and matrix
343 Vec3 axis;
344 Scalar angle;
345 this->axis_angle( axis, angle);
346 Matrix m;
347 m = this->rotation_matrix();
348
349 std::cerr << "quaternion : " << (*this) << std::endl;
350 std::cerr << "length : " << this->norm() << std::endl;
351 std::cerr << "axis, angle: " << axis << ", " << angle*180.0/M_PI << "\n";
352 std::cerr << "rot matrix :\n";
353 std::cerr << m << std::endl;
354 }
355
356
357 #undef W
358 #undef X
359 #undef Y
360 #undef Z
361 };
362
363
364 typedef QuaternionT<float> Quaternionf;
365 typedef QuaternionT<double> Quaterniond;
366
367
368 //=============================================================================
369 } // namespace ACG
370 //=============================================================================
371 #endif // ACG_QUATERNION_HH defined
372 //=============================================================================
373
ACG::QuaternionT::logarithm
Quaternion logarithm() const
quaternion logarithm (for unit quaternions)
Definition: QuaternionT.hh:320
ACG
Namespace providing different geometric functions concerning angles.
Definition: DBSCANT.cc:51
ACG::QuaternionT::identity
void identity()
identity rotation
Definition: QuaternionT.hh:129
ACG::QuaternionT::operator*
Quaternion operator*(const Quaternion &_q) const
quaternion * quaternion
Definition: QuaternionT.hh:143
ACG::QuaternionT::rotation_matrix
Matrix rotation_matrix() const
cast to rotation matrix
Definition: QuaternionT.hh:182
ACG::QuaternionT::invert
Quaternion invert() const
invert quaternion
Definition: QuaternionT.hh:138
ACG::QuaternionT::left_mult_matrix
Matrix left_mult_matrix() const
get matrix for mult from left (q*p = Qp)
Definition: QuaternionT.hh:247
ACG::QuaternionT::QuaternionT
QuaternionT(Vec3 _axis, Scalar _angle)
construct from rotation axis and angle (in radians)
Definition: QuaternionT.hh:111
ACG::QuaternionT::QuaternionT
QuaternionT(const Vec3 &_p)
construct from 3D point (pure imaginary quaternion)
Definition: QuaternionT.hh:103
ACG::QuaternionT::rotate
Vec3T rotate(const Vec3T &_v) const
rotate vector
Definition: QuaternionT.hh:157
ACG::QuaternionT::conjugate
Quaternion conjugate() const
conjugate quaternion
Definition: QuaternionT.hh:133
ACG::QuaternionT::QuaternionT
QuaternionT(const Vec4 &_p)
construct from 4D vector
Definition: QuaternionT.hh:107
OpenMesh::VectorT
Definition: Vector11T.hh:83
ACG::QuaternionT::axis_angle
void axis_angle(Vec3 &_axis, Scalar &_angle) const
get rotation axis and angle (only valid for unit quaternions!)
Definition: QuaternionT.hh:165
ACG::QuaternionT::operator*=
Quaternion & operator*=(const Quaternion &_q)
quaternion *= quaternion
Definition: QuaternionT.hh:151
ACG::QuaternionT::QuaternionT
QuaternionT(Scalar _w=1.0, Scalar _x=0.0, Scalar _y=0.0, Scalar _z=0.0)
construct from 4 Scalars (default)
Definition: QuaternionT.hh:99
ACG::QuaternionT::init_from_matrix
void init_from_matrix(const MatrixT &_rot)
get quaternion from rotation matrix
Definition: QuaternionT.hh:260
ACG::QuaternionT::QuaternionT
QuaternionT(const MatrixT &_rot)
construct from rotation matrix (only valid for rotation matrices!)
Definition: QuaternionT.hh:124
ACG::QuaternionT::right_mult_matrix
Matrix right_mult_matrix() const
get matrix for mult from right (p*q = Qp)
Definition: QuaternionT.hh:235
ACG::Matrix4x4T
Definition: Matrix4x4T.hh:98
ACG::QuaternionT::exponential
Quaternion exponential() const
quaternion exponential (for unit quaternions)
Definition: QuaternionT.hh:303
ACG::QuaternionT
Definition: QuaternionT.hh:81