Developer Documentation

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
45
46 //=============================================================================
47 //
48 // CLASS Quaternion
49 //
50 //=============================================================================
51
52 #ifndef ACG_QUATERNION_HH
53 #define ACG_QUATERNION_HH
54
55
56 //== INCLUDES =================================================================
57
58 #include "VectorT.hh"
59 #include "Matrix4x4T.hh"
60
61
62 //== NAMESPACES ==============================================================
63
64 namespace ACG {
65
66
67 //== CLASS DEFINITION =========================================================
68
69
74 template <class Scalar>
75 class QuaternionT : public VectorT<Scalar,4>
76 {
77 public:
78
79 #define W Base::data()[0]
80 #define X Base::data()[1]
81 #define Y Base::data()[2]
82 #define Z Base::data()[3]
83
84
85 typedef VectorT<Scalar,4> Base;
86 typedef QuaternionT<Scalar> Quaternion;
87 typedef VectorT<Scalar,3> Vec3;
88 typedef VectorT<Scalar,4> Vec4;
89 typedef Matrix4x4T<Scalar> Matrix;
90
91
93 QuaternionT(Scalar _w=1.0, Scalar _x=0.0, Scalar _y=0.0, Scalar _z=0.0)
94 : Vec4(_w, _x, _y, _z) {}
95
97 QuaternionT(const Vec3& _p)
98 : Vec4(0, _p[0], _p[1], _p[2]) {}
99
101 QuaternionT(const Vec4& _p)
102 : Vec4(_p[0], _p[1], _p[2], _p[3]) {}
103
105 QuaternionT(Vec3 _axis, Scalar _angle)
106 {
107 _axis.normalize();
108 Scalar theta = 0.5 * _angle;
109 Scalar sin_theta = sin(theta);
110 W = cos(theta);
111 X = sin_theta * _axis[0];
112 Y = sin_theta * _axis[1];
113 Z = sin_theta * _axis[2];
114 }
115
117 template <class MatrixT>
118 QuaternionT(const MatrixT& _rot)
119 { init_from_matrix( _rot); }
120
121
123 void identity() { W=1.0; X=Y=Z=0.0; }
124
125
127 Quaternion conjugate() const
128 { return Quaternion(W, -X, -Y, -Z); }
129
130
132 Quaternion invert() const
133 { return conjugate()/Base::sqrnorm(); }
134
135
137 Quaternion operator*(const Quaternion& _q) const
138 { return Quaternion(W*_q.W - X*_q.X - Y*_q.Y - Z*_q.Z,
139 W*_q.X + X*_q.W + Y*_q.Z - Z*_q.Y,
140 W*_q.Y - X*_q.Z + Y*_q.W + Z*_q.X,
141 W*_q.Z + X*_q.Y - Y*_q.X + Z*_q.W); }
142
143
145 Quaternion& operator*=(const Quaternion& _q)
146 { return *this = *this * _q; }
147
148
150 template <class Vec3T>
151 Vec3T rotate(const Vec3T& _v) const
152 {
153 Quaternion q = *this * Quaternion(0,_v[0],_v[1],_v[2]) * conjugate();
154 return Vec3T(q[1], q[2], q[3]);
155 }
156
157
159 void axis_angle(Vec3& _axis, Scalar& _angle) const
160 {
161 if (fabs(W) > 0.999999)
162 {
163 _axis = Vec3(1,0,0);
164 _angle = 0;
165 }
166 else
167 {
168 _angle = 2.0 * acos(W);
169 _axis = Vec3(X, Y, Z).normalize();
170 }
171 }
172
173
174
176 Matrix rotation_matrix() const
177 {
178 Scalar
179 ww(W*W), xx(X*X), yy(Y*Y), zz(Z*Z), wx(W*X),
180 wy(W*Y), wz(W*Z), xy(X*Y), xz(X*Z), yz(Y*Z);
181
182 Matrix m;
183
184 m(0,0) = ww + xx - yy - zz;
185 m(1,0) = 2.0*(xy + wz);
186 m(2,0) = 2.0*(xz - wy);
187
188 m(0,1) = 2.0*(xy - wz);
189 m(1,1) = ww - xx + yy - zz;
190 m(2,1) = 2.0*(yz + wx);
191
192 m(0,2) = 2.0*(xz + wy);
193 m(1,2) = 2.0*(yz - wx);
194 m(2,2) = ww - xx - yy + zz;
195
196 m(0,3) = m(1,3) = m(2,3) = m(3,0) = m(3,1) = m(3,2) = 0.0;
197 m(3,3) = 1.0;
198
199 return m;
200 }
201
202
203
204 /*
206 Matrix right_mult_matrix() const
207 {
208 Matrix m;
209 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
210 m(1,0) = X; m(1,1) = W; m(1,2) = -Z; m(1,3) = Y;
211 m(2,0) = Y; m(2,1) = Z; m(2,2) = W; m(2,3) = -X;
212 m(3,0) = Z; m(3,1) = -Y; m(3,2) = X; m(3,3) = W;
213 return m;
214 }
215
216
218 Matrix left_mult_matrix() const
219 {
220 Matrix m;
221 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
222 m(1,0) = X; m(1,1) = W; m(1,2) = Z; m(1,3) = -Y;
223 m(2,0) = Y; m(2,1) = -Z; m(2,2) = W; m(2,3) = X;
224 m(3,0) = Z; m(3,1) = Y; m(3,2) = -X; m(3,3) = W;
225 return m;
226 }
227 */
229 Matrix right_mult_matrix() const
230 {
231 Matrix m;
232 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
233 m(1,0) = X; m(1,1) = W; m(1,2) = Z; m(1,3) = -Y;
234 m(2,0) = Y; m(2,1) = -Z; m(2,2) = W; m(2,3) = X;
235 m(3,0) = Z; m(3,1) = Y; m(3,2) = -X; m(3,3) = W;
236 return m;
237 }
238
239
241 Matrix left_mult_matrix() const
242 {
243 Matrix m;
244 m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
245 m(1,0) = X; m(1,1) = W; m(1,2) = -Z; m(1,3) = Y;
246 m(2,0) = Y; m(2,1) = Z; m(2,2) = W; m(2,3) = -X;
247 m(3,0) = Z; m(3,1) = -Y; m(3,2) = X; m(3,3) = W;
248 return m;
249 }
250
251
253 template<class MatrixT>
254 void init_from_matrix( const MatrixT& _rot)
255 {
256 Scalar trace = _rot(0,0) + _rot(1,1) + _rot(2,2);
257 if( trace > 0.0 )
258 {
259 Scalar s = 0.5 / sqrt(trace + 1.0);
260 W = 0.25 / s;
261 X = ( _rot(2,1) - _rot(1,2) ) * s;
262 Y = ( _rot(0,2) - _rot(2,0) ) * s;
263 Z = ( _rot(1,0) - _rot(0,1) ) * s;
264 }
265 else
266 {
267 if ( _rot(0,0) > _rot(1,1) && _rot(0,0) > _rot(2,2) )
268 {
269 Scalar s = 2.0 * sqrt( 1.0 + _rot(0,0) - _rot(1,1) - _rot(2,2));
270 W = (_rot(2,1) - _rot(1,2) ) / s;
271 X = 0.25 * s;
272 Y = (_rot(0,1) + _rot(1,0) ) / s;
273 Z = (_rot(0,2) + _rot(2,0) ) / s;
274 }
275 else
276 if (_rot(1,1) > _rot(2,2))
277 {
278 Scalar s = 2.0 * sqrt( 1.0 + _rot(1,1) - _rot(0,0) - _rot(2,2));
279 W = (_rot(0,2) - _rot(2,0) ) / s;
280 X = (_rot(0,1) + _rot(1,0) ) / s;
281 Y = 0.25 * s;
282 Z = (_rot(1,2) + _rot(2,1) ) / s;
283 }
284 else
285 {
286 Scalar s = 2.0 * sqrt( 1.0 + _rot(2,2) - _rot(0,0) - _rot(1,1) );
287 W = (_rot(1,0) - _rot(0,1) ) / s;
288 X = (_rot(0,2) + _rot(2,0) ) / s;
289 Y = (_rot(1,2) + _rot(2,1) ) / s;
290 Z = 0.25 * s;
291 }
292 }
293 }
294
295
297 Quaternion exponential() const
298 {
299 Vec3 n(X,Y,Z);
300 Scalar theta( n.norm());
301 Scalar sin_theta = sin(theta);
302 Scalar cos_theta = cos(theta);
303
304 if( theta > 1e-6 )
305 n *= sin_theta/theta;
306 else
307 n = Vec3(0,0,0);
308
309 return Quaternion( cos_theta, n[0], n[1], n[2]);
310 }
311
312
314 Quaternion logarithm() const
315 {
316 // clamp to valid input
317 double w = W;
318 if( w > 1.0) w = 1.0;
319 else if( w < -1.0) w = -1.0;
320
321 Scalar theta_half = acos(w);
322
323 Vec3 n(X,Y,Z);
324 Scalar n_norm( n.norm());
325
326 if( n_norm > 1e-6 )
327 n *= theta_half/n_norm;
328 else
329 n = Vec3(0,0,0);
330
331 return Quaternion( 0, n[0], n[1], n[2]);
332 }
333
334 void print_info()
335 {
336 // get axis, angle and matrix
337 Vec3 axis;
338 Scalar angle;
339 this->axis_angle( axis, angle);
340 Matrix m;
341 m = this->rotation_matrix();
342
343 std::cerr << "quaternion : " << (*this) << std::endl;
344 std::cerr << "length : " << this->norm() << std::endl;
345 std::cerr << "axis, angle: " << axis << ", " << angle*180.0/M_PI << "\n";
346 std::cerr << "rot matrix :\n";
347 std::cerr << m << std::endl;
348 }
349
350
351 #undef W
352 #undef X
353 #undef Y
354 #undef Z
355 };
356
357
358 typedef QuaternionT<float> Quaternionf;
359 typedef QuaternionT<double> Quaterniond;
360
361
362 //=============================================================================
363 } // namespace ACG
364 //=============================================================================
365 #endif // ACG_QUATERNION_HH defined
366 //=============================================================================
367
ACG::QuaternionT::exponential
Quaternion exponential() const
quaternion exponential (for unit quaternions)
Definition: QuaternionT.hh:297
ACG
Namespace providing different geometric functions concerning angles.
Definition: BaseObjectData.hh:68
ACG::QuaternionT::operator*
Quaternion operator*(const Quaternion &_q) const
quaternion * quaternion
Definition: QuaternionT.hh:137
ACG::QuaternionT::rotation_matrix
Matrix rotation_matrix() const
cast to rotation matrix
Definition: QuaternionT.hh:176
ACG::QuaternionT::conjugate
Quaternion conjugate() const
conjugate quaternion
Definition: QuaternionT.hh:127
OpenMesh::VectorT::norm
auto norm() const -> decltype(std::sqrt(std::declval< VectorT< S, DIM >>().sqrnorm()))
compute euclidean norm
Definition: Vector11T.hh:409
OpenMesh::VectorT::sqrnorm
decltype(std::declval< S >() *std::declval< S >()) sqrnorm() const
compute squared euclidean norm
Definition: Vector11T.hh:397
OpenMesh::VectorT::normalize
auto normalize() -> decltype(*this/=std::declval< VectorT< S, DIM >>().norm())
Definition: Vector11T.hh:429
ACG::QuaternionT::QuaternionT
QuaternionT(const Vec3 &_p)
construct from 3D point (pure imaginary quaternion)
Definition: QuaternionT.hh:97
ACG::QuaternionT
Definition: QuaternionT.hh:75
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:93
ACG::QuaternionT::left_mult_matrix
Matrix left_mult_matrix() const
get matrix for mult from left (q*p = Qp)
Definition: QuaternionT.hh:241
ACG::QuaternionT::right_mult_matrix
Matrix right_mult_matrix() const
get matrix for mult from right (p*q = Qp)
Definition: QuaternionT.hh:229
ACG::QuaternionT::init_from_matrix
void init_from_matrix(const MatrixT &_rot)
get quaternion from rotation matrix
Definition: QuaternionT.hh:254
ACG::QuaternionT::QuaternionT
QuaternionT(Vec3 _axis, Scalar _angle)
construct from rotation axis and angle (in radians)
Definition: QuaternionT.hh:105
ACG::QuaternionT::QuaternionT
QuaternionT(const MatrixT &_rot)
construct from rotation matrix (only valid for rotation matrices!)
Definition: QuaternionT.hh:118
ACG::QuaternionT::logarithm
Quaternion logarithm() const
quaternion logarithm (for unit quaternions)
Definition: QuaternionT.hh:314
ACG::QuaternionT::QuaternionT
QuaternionT(const Vec4 &_p)
construct from 4D vector
Definition: QuaternionT.hh:101
ACG::Matrix4x4T
Definition: Matrix4x4T.hh:92
ACG::QuaternionT::identity
void identity()
identity rotation
Definition: QuaternionT.hh:123
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:159
OpenMesh::VectorT
Definition: Vector11T.hh:83
ACG::QuaternionT::invert
Quaternion invert() const
invert quaternion
Definition: QuaternionT.hh:132
ACG::QuaternionT::rotate
Vec3T rotate(const Vec3T &_v) const
rotate vector
Definition: QuaternionT.hh:151
ACG::QuaternionT::operator*=
Quaternion & operator*=(const Quaternion &_q)
quaternion *= quaternion
Definition: QuaternionT.hh:145