quaternionI.H
Go to the documentation of this file.
1 /*---------------------------------------------------------------------------*\
2  ========= |
3  \\ / F ield | OpenFOAM: The Open Source CFD Toolbox
4  \\ / O peration |
5  \\ / A nd | Copyright (C) 2011-2013 OpenFOAM Foundation
6  \\/ M anipulation |
7 -------------------------------------------------------------------------------
8 License
9  This file is part of OpenFOAM.
10 
11  OpenFOAM is free software: you can redistribute it and/or modify it
12  under the terms of the GNU General Public License as published by
13  the Free Software Foundation, either version 3 of the License, or
14  (at your option) any later version.
15 
16  OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
18  FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
19  for more details.
20 
21  You should have received a copy of the GNU General Public License
22  along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
23 
24 \*---------------------------------------------------------------------------*/
25 
26 // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
27 
29 {}
30 
31 inline Foam::quaternion::quaternion(const scalar w, const vector& v)
32 :
33  w_(w),
34  v_(v)
35 {}
36 
37 inline Foam::quaternion::quaternion(const vector& d, const scalar theta)
38 :
39  w_(cos(0.5*theta)),
40  v_((sin(0.5*theta)/mag(d))*d)
41 {}
42 
44 (
45  const vector& d,
46  const scalar cosTheta,
47  const bool normalized
48 )
49 {
50  scalar cosHalfTheta2 = 0.5*(cosTheta + 1);
51  w_ = sqrt(cosHalfTheta2);
52 
53  if (normalized)
54  {
55  v_ = sqrt(1 - cosHalfTheta2)*d;
56  }
57  else
58  {
59  v_ = (sqrt(1 - cosHalfTheta2)/mag(d))*d;
60  }
61 }
62 
63 inline Foam::quaternion::quaternion(const scalar w)
64 :
65  w_(w),
66  v_(vector::zero)
67 {}
68 
70 :
71  w_(0),
72  v_(v)
73 {}
74 
76 (
77  const scalar angleX,
78  const scalar angleY,
79  const scalar angleZ
80 )
81 {
82  operator=(quaternion(vector(1, 0, 0), angleX));
83  operator*=(quaternion(vector(0, 1, 0), angleY));
84  operator*=(quaternion(vector(0, 0, 1), angleZ));
85 }
86 
88 (
89  const tensor& rotationTensor
90 )
91 {
92  scalar trace =
93  rotationTensor.xx()
94  + rotationTensor.yy()
95  + rotationTensor.zz();
96 
97  if (trace > 0)
98  {
99  scalar s = 0.5/Foam::sqrt(trace + 1.0);
100 
101  w_ = 0.25/s;
102  v_[0] = (rotationTensor.zy() - rotationTensor.yz())*s;
103  v_[1] = (rotationTensor.xz() - rotationTensor.zx())*s;
104  v_[2] = (rotationTensor.yx() - rotationTensor.xy())*s;
105  }
106  else
107  {
108  if
109  (
110  rotationTensor.xx() > rotationTensor.yy()
111  && rotationTensor.xx() > rotationTensor.zz()
112  )
113  {
114  scalar s = 2.0*Foam::sqrt
115  (
116  1.0
117  + rotationTensor.xx()
118  - rotationTensor.yy()
119  - rotationTensor.zz()
120  );
121 
122  w_ = (rotationTensor.zy() - rotationTensor.yz())/s;
123  v_[0] = 0.25*s;
124  v_[1] = (rotationTensor.xy() + rotationTensor.yx())/s;
125  v_[2] = (rotationTensor.xz() + rotationTensor.zx())/s;
126  }
127  else if
128  (
129  rotationTensor.yy() > rotationTensor.zz()
130  )
131  {
132  scalar s = 2.0*Foam::sqrt
133  (
134  1.0
135  + rotationTensor.yy()
136  - rotationTensor.xx()
137  - rotationTensor.zz()
138  );
139 
140  w_ = (rotationTensor.xz() - rotationTensor.xz())/s;
141  v_[0] = (rotationTensor.xy() + rotationTensor.yx())/s;
142  v_[1] = 0.25*s;
143  v_[2] = (rotationTensor.yz() + rotationTensor.zy())/s;
144  }
145  else
146  {
147  scalar s = 2.0*Foam::sqrt
148  (
149  1.0
150  + rotationTensor.zz()
151  - rotationTensor.xx()
152  - rotationTensor.yy()
153  );
154 
155  w_ = (rotationTensor.yx() - rotationTensor.xy())/s;
156  v_[0] = (rotationTensor.xz() + rotationTensor.zx())/s;
157  v_[1] = (rotationTensor.yz() + rotationTensor.zy())/s;
158  v_[2] = 0.25*s;
159  }
160  }
161 }
162 
163 
164 // * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
165 
166 inline Foam::scalar Foam::quaternion::w() const
167 {
168  return w_;
169 }
170 
171 
172 inline const Foam::vector& Foam::quaternion::v() const
173 {
174  return v_;
175 }
176 
177 
178 inline Foam::scalar& Foam::quaternion::w()
179 {
180  return w_;
181 }
182 
183 
185 {
186  return v_;
187 }
188 
189 
191 {
192  return *this/mag(*this);
193 }
194 
195 
197 {
198  operator/=(mag(*this));
199 }
200 
201 
202 inline Foam::quaternion Foam::quaternion::mulq0v(const vector& u) const
203 {
204  return quaternion(-(v() & u), w()*u + (v() ^ u));
205 }
206 
207 
209 {
210  return (mulq0v(u)*conjugate(*this)).v();
211 }
212 
213 
215 {
216  return (conjugate(*this).mulq0v(u)*(*this)).v();
217 }
218 
219 
221 {
222  return Foam::normalize((*this)*q);
223 }
224 
225 
227 (
228  const quaternion& q
229 ) const
230 {
231  return Foam::normalize(conjugate(*this)*q);
232 }
233 
234 
236 {
237  scalar w2 = sqr(w());
238  scalar x2 = sqr(v().x());
239  scalar y2 = sqr(v().y());
240  scalar z2 = sqr(v().z());
241 
242  scalar txy = 2*v().x()*v().y();
243  scalar twz = 2*w()*v().z();
244  scalar txz = 2*v().x()*v().z();
245  scalar twy = 2*w()*v().y();
246  scalar tyz = 2*v().y()*v().z();
247  scalar twx = 2*w()*v().x();
248 
249  return tensor
250  (
251  w2 + x2 - y2 - z2, txy - twz, txz + twy,
252  txy + twz, w2 - x2 + y2 - z2, tyz - twx,
253  txz - twy, tyz + twx, w2 - x2 - y2 + z2
254  );
255 }
256 
257 
259 {
260  vector angles(vector::zero);
261 
262  const scalar& w = q.w();
263  const vector& v = q.v();
264 
265  angles[0] = Foam::atan2
266  (
267  2*(w*v.x() + v.y()*v.z()),
268  1 - 2*(sqr(v.x()) + sqr(v.y()))
269  );
270  angles[1] = Foam::asin(2*(w*v.y() - v.z()*v.x()));
271  angles[2] = Foam::atan2
272  (
273  2*(w*v.z() + v.x()*v.y()),
274  1 - 2*(sqr(v.y()) + sqr(v.z()))
275  );
276 
277  // Convert to degrees
278 // forAll(angles, aI)
279 // {
280 // angles[aI] = Foam::radToDeg(angles[aI]);
281 // }
282 
283  return angles;
284 }
285 
286 
287 // * * * * * * * * * * * * * * * Member Operators * * * * * * * * * * * * * //
288 
290 {
291  w_ = q.w_;
292  v_ = q.v_;
293 }
294 
296 {
297  w_ += q.w_;
298  v_ += q.v_;
299 }
300 
302 {
303  w_ -= q.w_;
304  v_ -= q.v_;
305 }
306 
308 {
309  scalar w0 = w();
310  w() = w()*q.w() - (v() & q.v());
311  v() = w0*q.v() + q.w()*v() + (v() ^ q.v());
312 }
313 
315 {
316  return operator*=(inv(q));
317 }
318 
319 
320 inline void Foam::quaternion::operator=(const scalar s)
321 {
322  w_ = s;
323 }
324 
325 
327 {
328  v_ = v;
329 }
330 
331 
332 inline void Foam::quaternion::operator*=(const scalar s)
333 {
334  w_ *= s;
335  v_ *= s;
336 }
337 
338 inline void Foam::quaternion::operator/=(const scalar s)
339 {
340  w_ /= s;
341  v_ /= s;
342 }
343 
344 
345 // * * * * * * * * * * * * * * * Global Functions * * * * * * * * * * * * * //
346 
347 inline Foam::scalar Foam::magSqr(const quaternion& q)
348 {
349  return magSqr(q.w()) + magSqr(q.v());
350 }
351 
352 
353 inline Foam::scalar Foam::mag(const quaternion& q)
354 {
355  return sqrt(magSqr(q));
356 }
357 
358 
360 {
361  return quaternion(q.w(), -q.v());
362 }
363 
364 
366 {
367  scalar magSqrq = magSqr(q);
368  return quaternion(q.w()/magSqrq, -q.v()/magSqrq);
369 }
370 
371 
373 {
374  return q/mag(q);
375 }
376 
377 
378 // * * * * * * * * * * * * * * * Global Operators * * * * * * * * * * * * * //
379 
380 inline bool Foam::operator==(const quaternion& q1, const quaternion& q2)
381 {
382  return (equal(q1.w(), q2.w()) && equal(q1.v(), q2.v()));
383 }
384 
385 
386 inline bool Foam::operator!=(const quaternion& q1, const quaternion& q2)
387 {
388  return !operator==(q1, q2);
389 }
390 
391 
392 inline Foam::quaternion Foam::operator+
393 (
394  const quaternion& q1,
395  const quaternion& q2
396 )
397 {
398  return quaternion(q1.w() + q2.w(), q1.v() + q2.v());
399 }
400 
401 
403 {
404  return quaternion(-q.w(), -q.v());
405 }
406 
407 
408 inline Foam::quaternion Foam::operator-
409 (
410  const quaternion& q1,
411  const quaternion& q2
412 )
413 {
414  return quaternion(q1.w() - q2.w(), q1.v() - q2.v());
415 }
416 
417 
418 inline Foam::scalar Foam::operator&(const quaternion& q1, const quaternion& q2)
419 {
420  return q1.w()*q2.w() + (q1.v() & q2.v());
421 }
422 
423 
424 inline Foam::quaternion Foam::operator*
425 (
426  const quaternion& q1,
427  const quaternion& q2
428 )
429 {
430  return quaternion
431  (
432  q1.w()*q2.w() - (q1.v() & q2.v()),
433  q1.w()*q2.v() + q2.w()*q1.v() + (q1.v() ^ q2.v())
434  );
435 }
436 
437 
438 inline Foam::quaternion Foam::operator/
439 (
440  const quaternion& q1,
441  const quaternion& q2
442 )
443 {
444  return q1*inv(q2);
445 }
446 
447 
448 inline Foam::quaternion Foam::operator*(const scalar s, const quaternion& q)
449 {
450  return quaternion(s*q.w(), s*q.v());
451 }
452 
453 
454 inline Foam::quaternion Foam::operator*(const quaternion& q, const scalar s)
455 {
456  return quaternion(s*q.w(), s*q.v());
457 }
458 
459 
460 inline Foam::quaternion Foam::operator/(const quaternion& q, const scalar s)
461 {
462  return quaternion(q.w()/s, q.v()/s);
463 }
464 
465 
466 // ************************************************************************* //
void operator-=(const quaternion &)
Definition: quaternionI.H:301
dimensionedScalar sqrt(const dimensionedScalar &ds)
tmp< fvMatrix< Type > > operator*(const DimensionedField< scalar, volMesh > &, const fvMatrix< Type > &)
const Cmpt & xy() const
Definition: TensorI.H:167
tmp< fvMatrix< Type > > operator==(const fvMatrix< Type > &, const fvMatrix< Type > &)
vector eulerAngles(const quaternion &q) const
Return a vector of euler angles (rotations in radians about.
Definition: quaternionI.H:258
gmvFile<< "tracers "<< particles.size()<< nl;forAllConstIter(Cloud< passiveParticle >, particles, iter){gmvFile<< iter().position().x()<< " ";}gmvFile<< nl;forAllConstIter(Cloud< passiveParticle >, particles, iter){gmvFile<< iter().position().y()<< " ";}gmvFile<< nl;forAllConstIter(Cloud< passiveParticle >, particles, iter){gmvFile<< iter().position().z()<< " ";}gmvFile<< nl;forAll(lagrangianScalarNames, i){word name=lagrangianScalarNames[i];IOField< scalar > s(IOobject( name, runTime.timeName(), cloud::prefix, mesh, IOobject::MUST_READ, IOobject::NO_WRITE ))
tmp< GeometricField< Type, fvPatchField, volMesh > > operator&(const fvMatrix< Type > &, const DimensionedField< Type, volMesh > &)
const Cmpt & zz() const
Definition: TensorI.H:216
dimensioned< scalar > mag(const dimensioned< Type > &)
Tensor< scalar > tensor
Tensor of scalars.
Definition: tensor.H:51
dimensionedScalar atan2(const dimensionedScalar &x, const dimensionedScalar &y)
void operator/=(const quaternion &)
Definition: quaternionI.H:314
dimensioned< scalar > magSqr(const dimensioned< Type > &)
const vector & v() const
Vector part of the quaternion ( = axis of rotation)
Definition: quaternionI.H:172
Quaternion class used to perform rotations in 3D space.
Definition: quaternion.H:60
const Cmpt & yy() const
Definition: TensorI.H:188
void operator*=(const quaternion &)
Definition: quaternionI.H:307
tensor R() const
The rotation tensor corresponding the quaternion.
Definition: quaternionI.H:235
tmp< fvMatrix< Type > > operator-(const fvMatrix< Type > &)
vector transform(const vector &v) const
Rotate the given vector.
Definition: quaternionI.H:208
const Cmpt & y() const
Definition: VectorI.H:71
dimensionedSphericalTensor inv(const dimensionedSphericalTensor &dt)
scalar w() const
Scalar part of the quaternion ( = cos(theta/2) for rotation)
Definition: quaternionI.H:166
dimensionedScalar operator/(const scalar s1, const dimensionedScalar &ds2)
quaternion()
Construct null.
Definition: quaternionI.H:28
bool operator!=(const particle &, const particle &)
Definition: particle.C:147
A class representing the concept of 0 used to avoid unnecessary manipulations for objects that are kn...
Definition: zero.H:47
dimensionedScalar cos(const dimensionedScalar &ds)
const Cmpt & x() const
Definition: VectorI.H:65
const Cmpt & yx() const
Definition: TensorI.H:181
const Cmpt & z() const
Definition: VectorI.H:77
void operator=(const quaternion &)
Definition: quaternionI.H:289
quaternion conjugate(const quaternion &q)
Return the conjugate of the given quaternion.
Definition: quaternionI.H:359
quaternion normalized() const
Definition: quaternionI.H:190
scalar y
const Cmpt & zy() const
Definition: TensorI.H:209
static const Vector zero
Definition: Vector.H:80
Vector< scalar > vector
A scalar version of the templated Vector.
Definition: vector.H:49
vector invTransform(const vector &v) const
Rotate the given vector anti-clockwise.
Definition: quaternionI.H:214
dimensionedSymmTensor sqr(const dimensionedVector &dv)
const Cmpt & xx() const
Definition: TensorI.H:160
const Cmpt & yz() const
Definition: TensorI.H:195
quaternion normalize(const quaternion &q)
Return the normalized (unit) quaternion of the given quaternion.
Definition: quaternionI.H:372
void operator+=(const quaternion &)
Definition: quaternionI.H:295
const Cmpt & xz() const
Definition: TensorI.H:174
dimensionedScalar asin(const dimensionedScalar &ds)
bool equal(const T &s1, const T &s2)
Definition: doubleFloat.H:62
const Cmpt & zx() const
Definition: TensorI.H:202
dimensionedScalar sin(const dimensionedScalar &ds)