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-2016 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 
32 inline Foam::quaternion::quaternion(const scalar w, const vector& v)
33 :
34  w_(w),
35  v_(v)
36 {}
37 
38 
39 inline Foam::quaternion::quaternion(const vector& d, const scalar theta)
40 :
41  w_(cos(0.5*theta)),
42  v_((sin(0.5*theta)/mag(d))*d)
43 {}
44 
45 
47 (
48  const vector& d,
49  const scalar cosTheta,
50  const bool normalized
51 )
52 {
53  scalar cosHalfTheta2 = 0.5*(cosTheta + 1);
54  w_ = sqrt(cosHalfTheta2);
55 
56  if (normalized)
57  {
58  v_ = sqrt(1 - cosHalfTheta2)*d;
59  }
60  else
61  {
62  v_ = (sqrt(1 - cosHalfTheta2)/mag(d))*d;
63  }
64 }
65 
66 
67 inline Foam::quaternion::quaternion(const scalar w)
68 :
69  w_(w),
70  v_(Zero)
71 {}
72 
73 
75 :
76  w_(0),
77  v_(v)
78 {}
79 
80 
82 {
83  return quaternion(sqrt(1 - magSqr(v)), v);
84 }
85 
86 
88 (
89  const rotationSequence rs,
90  const vector& angles
91 )
92 {
93  switch(rs)
94  {
95  case ZYX:
96  operator=(quaternion(vector(0, 0, 1), angles.x()));
97  operator*=(quaternion(vector(0, 1, 0), angles.y()));
98  operator*=(quaternion(vector(1, 0, 0), angles.z()));
99  break;
100 
101  case ZYZ:
102  operator=(quaternion(vector(0, 0, 1), angles.x()));
103  operator*=(quaternion(vector(0, 1, 0), angles.y()));
104  operator*=(quaternion(vector(0, 0, 1), angles.z()));
105  break;
106 
107  case ZXY:
108  operator=(quaternion(vector(0, 0, 1), angles.x()));
109  operator*=(quaternion(vector(1, 0, 0), angles.y()));
110  operator*=(quaternion(vector(0, 1, 0), angles.z()));
111  break;
112 
113  case ZXZ:
114  operator=(quaternion(vector(0, 0, 1), angles.x()));
115  operator*=(quaternion(vector(1, 0, 0), angles.y()));
116  operator*=(quaternion(vector(0, 0, 1), angles.z()));
117  break;
118 
119  case YXZ:
120  operator=(quaternion(vector(0, 1, 0), angles.x()));
121  operator*=(quaternion(vector(1, 0, 0), angles.y()));
122  operator*=(quaternion(vector(0, 0, 1), angles.z()));
123  break;
124 
125  case YXY:
126  operator=(quaternion(vector(0, 1, 0), angles.x()));
127  operator*=(quaternion(vector(1, 0, 0), angles.y()));
128  operator*=(quaternion(vector(0, 1, 0), angles.z()));
129  break;
130 
131  case YZX:
132  operator=(quaternion(vector(0, 1, 0), angles.x()));
133  operator*=(quaternion(vector(0, 0, 1), angles.y()));
134  operator*=(quaternion(vector(1, 0, 0), angles.z()));
135  break;
136 
137  case YZY:
138  operator=(quaternion(vector(0, 1, 0), angles.x()));
139  operator*=(quaternion(vector(0, 0, 1), angles.y()));
140  operator*=(quaternion(vector(0, 1, 0), angles.z()));
141  break;
142 
143  case XYZ:
144  operator=(quaternion(vector(1, 0, 0), angles.x()));
145  operator*=(quaternion(vector(0, 1, 0), angles.y()));
146  operator*=(quaternion(vector(0, 0, 1), angles.z()));
147  break;
148 
149  case XYX:
150  operator=(quaternion(vector(1, 0, 0), angles.x()));
151  operator*=(quaternion(vector(0, 1, 0), angles.y()));
152  operator*=(quaternion(vector(1, 0, 0), angles.z()));
153  break;
154 
155  case XZY:
156  operator=(quaternion(vector(1, 0, 0), angles.x()));
157  operator*=(quaternion(vector(0, 0, 1), angles.y()));
158  operator*=(quaternion(vector(0, 1, 0), angles.z()));
159  break;
160 
161  case XZX:
162  operator=(quaternion(vector(1, 0, 0), angles.x()));
163  operator*=(quaternion(vector(0, 0, 1), angles.y()));
164  operator*=(quaternion(vector(1, 0, 0), angles.z()));
165  break;
166 
167  default:
169  << "Unknown rotation sequence " << rs << abort(FatalError);
170  break;
171  }
172 }
173 
174 
176 (
177  const tensor& rotationTensor
178 )
179 {
180  scalar trace =
181  rotationTensor.xx()
182  + rotationTensor.yy()
183  + rotationTensor.zz();
184 
185  if (trace > 0)
186  {
187  scalar s = 0.5/Foam::sqrt(trace + 1.0);
188 
189  w_ = 0.25/s;
190  v_[0] = (rotationTensor.zy() - rotationTensor.yz())*s;
191  v_[1] = (rotationTensor.xz() - rotationTensor.zx())*s;
192  v_[2] = (rotationTensor.yx() - rotationTensor.xy())*s;
193  }
194  else
195  {
196  if
197  (
198  rotationTensor.xx() > rotationTensor.yy()
199  && rotationTensor.xx() > rotationTensor.zz()
200  )
201  {
202  scalar s = 2.0*Foam::sqrt
203  (
204  1.0
205  + rotationTensor.xx()
206  - rotationTensor.yy()
207  - rotationTensor.zz()
208  );
209 
210  w_ = (rotationTensor.zy() - rotationTensor.yz())/s;
211  v_[0] = 0.25*s;
212  v_[1] = (rotationTensor.xy() + rotationTensor.yx())/s;
213  v_[2] = (rotationTensor.xz() + rotationTensor.zx())/s;
214  }
215  else if
216  (
217  rotationTensor.yy() > rotationTensor.zz()
218  )
219  {
220  scalar s = 2.0*Foam::sqrt
221  (
222  1.0
223  + rotationTensor.yy()
224  - rotationTensor.xx()
225  - rotationTensor.zz()
226  );
227 
228  w_ = (rotationTensor.xz() - rotationTensor.xz())/s;
229  v_[0] = (rotationTensor.xy() + rotationTensor.yx())/s;
230  v_[1] = 0.25*s;
231  v_[2] = (rotationTensor.yz() + rotationTensor.zy())/s;
232  }
233  else
234  {
235  scalar s = 2.0*Foam::sqrt
236  (
237  1.0
238  + rotationTensor.zz()
239  - rotationTensor.xx()
240  - rotationTensor.yy()
241  );
242 
243  w_ = (rotationTensor.yx() - rotationTensor.xy())/s;
244  v_[0] = (rotationTensor.xz() + rotationTensor.zx())/s;
245  v_[1] = (rotationTensor.yz() + rotationTensor.zy())/s;
246  v_[2] = 0.25*s;
247  }
248  }
249 }
250 
251 
252 // * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
253 
254 inline Foam::scalar Foam::quaternion::w() const
255 {
256  return w_;
257 }
258 
259 
260 inline const Foam::vector& Foam::quaternion::v() const
261 {
262  return v_;
263 }
264 
265 
266 inline Foam::scalar& Foam::quaternion::w()
267 {
268  return w_;
269 }
270 
271 
273 {
274  return v_;
275 }
276 
277 
279 {
280  return *this/mag(*this);
281 }
282 
283 
285 {
286  operator/=(mag(*this));
287 }
288 
289 
290 inline Foam::quaternion Foam::quaternion::mulq0v(const vector& u) const
291 {
292  return quaternion(-(v() & u), w()*u + (v() ^ u));
293 }
294 
295 
297 {
298  return (mulq0v(u)*conjugate(*this)).v();
299 }
300 
301 
303 {
304  return (conjugate(*this).mulq0v(u)*(*this)).v();
305 }
306 
307 
309 {
310  return Foam::normalize((*this)*q);
311 }
312 
313 
315 (
316  const quaternion& q
317 ) const
318 {
319  return Foam::normalize(conjugate(*this)*q);
320 }
321 
322 
324 {
325  const scalar w2 = sqr(w());
326  const scalar x2 = sqr(v().x());
327  const scalar y2 = sqr(v().y());
328  const scalar z2 = sqr(v().z());
329 
330  const scalar txy = 2*v().x()*v().y();
331  const scalar twz = 2*w()*v().z();
332  const scalar txz = 2*v().x()*v().z();
333  const scalar twy = 2*w()*v().y();
334  const scalar tyz = 2*v().y()*v().z();
335  const scalar twx = 2*w()*v().x();
336 
337  return tensor
338  (
339  w2 + x2 - y2 - z2, txy - twz, txz + twy,
340  txy + twz, w2 - x2 + y2 - z2, tyz - twx,
341  txz - twy, tyz + twx, w2 - x2 - y2 + z2
342  );
343 }
344 
345 
346 inline Foam::vector Foam::quaternion::twoAxes
347 (
348  const scalar t11,
349  const scalar t12,
350  const scalar c2,
351  const scalar t31,
352  const scalar t32
353 )
354 {
355  return vector(atan2(t11, t12), acos(c2), atan2(t31, t32));
356 }
357 
358 
359 inline Foam::vector Foam::quaternion::threeAxes
360 (
361  const scalar t11,
362  const scalar t12,
363  const scalar s2,
364  const scalar t31,
365  const scalar t32
366 )
367 {
368  return vector(atan2(t11, t12), asin(s2), atan2(t31, t32));
369 }
370 
371 
373 (
374  const rotationSequence rs
375 ) const
376 {
377  const scalar w2 = sqr(w());
378  const scalar x2 = sqr(v().x());
379  const scalar y2 = sqr(v().y());
380  const scalar z2 = sqr(v().z());
381 
382  switch(rs)
383  {
384  case ZYX:
385  return threeAxes
386  (
387  2*(v().x()*v().y() + w()*v().z()),
388  w2 + x2 - y2 - z2,
389  2*(w()*v().y() - v().x()*v().z()),
390  2*(v().y()*v().z() + w()*v().x()),
391  w2 - x2 - y2 + z2
392  );
393  break;
394 
395  case ZYZ:
396  return twoAxes
397  (
398  2*(v().y()*v().z() - w()*v().x()),
399  2*(v().x()*v().z() + w()*v().y()),
400  w2 - x2 - y2 + z2,
401  2*(v().y()*v().z() + w()*v().x()),
402  2*(w()*v().y() - v().x()*v().z())
403  );
404  break;
405 
406  case ZXY:
407  return threeAxes
408  (
409  2*(w()*v().z() - v().x()*v().y()),
410  w2 - x2 + y2 - z2,
411  2*(v().y()*v().z() + w()*v().x()),
412  2*(w()*v().y() - v().x()*v().z()),
413  w2 - x2 - y2 + z2
414  );
415  break;
416 
417  case ZXZ:
418  return twoAxes
419  (
420  2*(v().x()*v().z() + w()*v().y()),
421  2*(w()*v().x() - v().y()*v().z()),
422  w2 - x2 - y2 + z2,
423  2*(v().x()*v().z() - w()*v().y()),
424  2*(v().y()*v().z() + w()*v().x())
425  );
426  break;
427 
428  case YXZ:
429  return threeAxes
430  (
431  2*(v().x()*v().z() + w()*v().y()),
432  w2 - x2 - y2 + z2,
433  2*(w()*v().x() - v().y()*v().z()),
434  2*(v().x()*v().y() + w()*v().z()),
435  w2 - x2 + y2 - z2
436  );
437  break;
438 
439  case YXY:
440  return twoAxes
441  (
442  2*(v().x()*v().y() - w()*v().z()),
443  2*(v().y()*v().z() + w()*v().x()),
444  w2 - x2 + y2 - z2,
445  2*(v().x()*v().y() + w()*v().z()),
446  2*(w()*v().x() - v().y()*v().z())
447  );
448  break;
449 
450  case YZX:
451  return threeAxes
452  (
453  2*(w()*v().y() - v().x()*v().z()),
454  w2 + x2 - y2 - z2,
455  2*(v().x()*v().y() + w()*v().z()),
456  2*(w()*v().x() - v().y()*v().z()),
457  w2 - x2 + y2 - z2
458  );
459  break;
460 
461  case YZY:
462  return twoAxes
463  (
464  2*(v().y()*v().z() + w()*v().x()),
465  2*(w()*v().z() - v().x()*v().y()),
466  w2 - x2 + y2 - z2,
467  2*(v().y()*v().z() - w()*v().x()),
468  2*(v().x()*v().y() + w()*v().z())
469  );
470  break;
471 
472  case XYZ:
473  return threeAxes
474  (
475  2*(w()*v().x() - v().y()*v().z()),
476  w2 - x2 - y2 + z2,
477  2*(v().x()*v().z() + w()*v().y()),
478  2*(w()*v().z() - v().x()*v().y()),
479  w2 + x2 - y2 - z2
480  );
481  break;
482 
483  case XYX:
484  return twoAxes
485  (
486  2*(v().x()*v().y() + w()*v().z()),
487  2*(w()*v().y() - v().x()*v().z()),
488  w2 + x2 - y2 - z2,
489  2*(v().x()*v().y() - w()*v().z()),
490  2*(v().x()*v().z() + w()*v().y())
491  );
492  break;
493 
494  case XZY:
495  return threeAxes
496  (
497  2*(v().y()*v().z() + w()*v().x()),
498  w2 - x2 + y2 - z2,
499  2*(w()*v().z() - v().x()*v().y()),
500  2*(v().x()*v().z() + w()*v().y()),
501  w2 + x2 - y2 - z2
502  );
503  break;
504 
505  case XZX:
506  return twoAxes
507  (
508  2*(v().x()*v().z() - w()*v().y()),
509  2*(v().x()*v().y() + w()*v().z()),
510  w2 + x2 - y2 - z2,
511  2*(v().x()*v().z() + w()*v().y()),
512  2*(w()*v().z() - v().x()*v().y())
513  );
514  break;
515  default:
517  << "Unknown rotation sequence " << rs << abort(FatalError);
518  return Zero;
519  break;
520  }
521 }
522 
523 
524 // * * * * * * * * * * * * * * * Member Operators * * * * * * * * * * * * * //
525 
527 {
528  w_ = q.w_;
529  v_ = q.v_;
530 }
531 
533 {
534  w_ += q.w_;
535  v_ += q.v_;
536 }
537 
539 {
540  w_ -= q.w_;
541  v_ -= q.v_;
542 }
543 
545 {
546  scalar w0 = w();
547  w() = w()*q.w() - (v() & q.v());
548  v() = w0*q.v() + q.w()*v() + (v() ^ q.v());
549 }
550 
552 {
553  return operator*=(inv(q));
554 }
555 
556 
557 inline void Foam::quaternion::operator=(const scalar s)
558 {
559  w_ = s;
560 }
561 
562 
564 {
565  v_ = v;
566 }
567 
568 
569 inline void Foam::quaternion::operator*=(const scalar s)
570 {
571  w_ *= s;
572  v_ *= s;
573 }
574 
575 inline void Foam::quaternion::operator/=(const scalar s)
576 {
577  w_ /= s;
578  v_ /= s;
579 }
580 
581 
582 // * * * * * * * * * * * * * * * Global Functions * * * * * * * * * * * * * //
583 
584 inline Foam::scalar Foam::magSqr(const quaternion& q)
585 {
586  return magSqr(q.w()) + magSqr(q.v());
587 }
588 
589 
590 inline Foam::scalar Foam::mag(const quaternion& q)
591 {
592  return sqrt(magSqr(q));
593 }
594 
595 
597 {
598  return quaternion(q.w(), -q.v());
599 }
600 
601 
603 {
604  scalar magSqrq = magSqr(q);
605  return quaternion(q.w()/magSqrq, -q.v()/magSqrq);
606 }
607 
608 
610 {
611  return q/mag(q);
612 }
613 
614 
615 // * * * * * * * * * * * * * * * Global Operators * * * * * * * * * * * * * //
616 
617 inline bool Foam::operator==(const quaternion& q1, const quaternion& q2)
618 {
619  return (equal(q1.w(), q2.w()) && equal(q1.v(), q2.v()));
620 }
621 
622 
623 inline bool Foam::operator!=(const quaternion& q1, const quaternion& q2)
624 {
625  return !operator==(q1, q2);
626 }
627 
628 
629 inline Foam::quaternion Foam::operator+
630 (
631  const quaternion& q1,
632  const quaternion& q2
633 )
634 {
635  return quaternion(q1.w() + q2.w(), q1.v() + q2.v());
636 }
637 
638 
640 {
641  return quaternion(-q.w(), -q.v());
642 }
643 
644 
645 inline Foam::quaternion Foam::operator-
646 (
647  const quaternion& q1,
648  const quaternion& q2
649 )
650 {
651  return quaternion(q1.w() - q2.w(), q1.v() - q2.v());
652 }
653 
654 
655 inline Foam::scalar Foam::operator&(const quaternion& q1, const quaternion& q2)
656 {
657  return q1.w()*q2.w() + (q1.v() & q2.v());
658 }
659 
660 
661 inline Foam::quaternion Foam::operator*
662 (
663  const quaternion& q1,
664  const quaternion& q2
665 )
666 {
667  return quaternion
668  (
669  q1.w()*q2.w() - (q1.v() & q2.v()),
670  q1.w()*q2.v() + q2.w()*q1.v() + (q1.v() ^ q2.v())
671  );
672 }
673 
674 
675 inline Foam::quaternion Foam::operator/
676 (
677  const quaternion& q1,
678  const quaternion& q2
679 )
680 {
681  return q1*inv(q2);
682 }
683 
684 
685 inline Foam::quaternion Foam::operator*(const scalar s, const quaternion& q)
686 {
687  return quaternion(s*q.w(), s*q.v());
688 }
689 
690 
691 inline Foam::quaternion Foam::operator*(const quaternion& q, const scalar s)
692 {
693  return quaternion(s*q.w(), s*q.v());
694 }
695 
696 
697 inline Foam::quaternion Foam::operator/(const quaternion& q, const scalar s)
698 {
699  return quaternion(q.w()/s, q.v()/s);
700 }
701 
702 
703 // ************************************************************************* //
const Cmpt & z() const
Definition: VectorI.H:87
vector eulerAngles(const rotationSequence rs) const
Return a vector of euler angles corresponding to the.
Definition: quaternionI.H:373
dimensionedScalar acos(const dimensionedScalar &ds)
const Cmpt & yy() const
Definition: TensorI.H:181
const vector & v() const
Vector part of the quaternion ( = axis of rotation)
Definition: quaternionI.H:260
void operator=(const quaternion &)
Definition: quaternionI.H:526
const Cmpt & x() const
Definition: VectorI.H:75
error FatalError
#define FatalErrorInFunction
Report an error message using Foam::FatalError.
Definition: error.H:319
dimensionedSymmTensor sqr(const dimensionedVector &dv)
dimensionedSphericalTensor inv(const dimensionedSphericalTensor &dt)
static quaternion unit(const vector &v)
Return the unit quaternion (versor) from the given vector.
Definition: quaternionI.H:81
tensor R() const
The rotation tensor corresponding the quaternion.
Definition: quaternionI.H:323
dimensionedScalar sqrt(const dimensionedScalar &ds)
const Cmpt & zy() const
Definition: TensorI.H:202
const Cmpt & yz() const
Definition: TensorI.H:188
dimensionedScalar operator/(const scalar s1, const dimensionedScalar &ds2)
quaternion()
Construct null.
Definition: quaternionI.H:28
tmp< fvMatrix< Type > > operator*(const DimensionedField< scalar, volMesh > &, const fvMatrix< Type > &)
Vector< scalar > vector
A scalar version of the templated Vector.
Definition: vector.H:49
quaternion normalize(const quaternion &q)
Return the normalized (unit) quaternion of the given quaternion.
Definition: quaternionI.H:609
dimensionedScalar asin(const dimensionedScalar &ds)
void operator+=(const quaternion &)
Definition: quaternionI.H:532
scalar w() const
Scalar part of the quaternion ( = cos(theta/2) for rotation)
Definition: quaternionI.H:254
const Cmpt & xz() const
Definition: TensorI.H:167
scalar y
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))
quaternion conjugate(const quaternion &q)
Return the conjugate of the given quaternion.
Definition: quaternionI.H:596
dimensionedScalar cos(const dimensionedScalar &ds)
tmp< fvMatrix< Type > > operator==(const fvMatrix< Type > &, const fvMatrix< Type > &)
const Cmpt & xx() const
Definition: TensorI.H:153
const Cmpt & y() const
Definition: VectorI.H:81
Quaternion class used to perform rotations in 3D space.
Definition: quaternion.H:60
tmp< fvMatrix< Type > > operator-(const fvMatrix< Type > &)
const Cmpt & xy() const
Definition: TensorI.H:160
static const zero Zero
Definition: zero.H:91
const Cmpt & yx() const
Definition: TensorI.H:174
errorManip< error > abort(error &err)
Definition: errorManip.H:131
dimensioned< scalar > magSqr(const dimensioned< Type > &)
dimensionedScalar sin(const dimensionedScalar &ds)
dimensionedScalar atan2(const dimensionedScalar &x, const dimensionedScalar &y)
const Cmpt & zz() const
Definition: TensorI.H:209
vector transform(const vector &v) const
Rotate the given vector.
Definition: quaternionI.H:296
void operator/=(const quaternion &)
Definition: quaternionI.H:551
vector invTransform(const vector &v) const
Rotate the given vector anti-clockwise.
Definition: quaternionI.H:302
bool equal(const T &s1, const T &s2)
Definition: doubleFloat.H:62
tmp< GeometricField< Type, fvPatchField, volMesh > > operator&(const fvMatrix< Type > &, const DimensionedField< Type, volMesh > &)
rotationSequence
Euler-angle rotation sequence.
Definition: quaternion.H:101
dimensioned< scalar > mag(const dimensioned< Type > &)
const Cmpt & zx() const
Definition: TensorI.H:195
bool operator!=(const particle &, const particle &)
Definition: particle.C:145
Tensor< scalar > tensor
Tensor of scalars.
Definition: tensor.H:51
quaternion normalized() const
Definition: quaternionI.H:278
void operator*=(const quaternion &)
Definition: quaternionI.H:544
void operator-=(const quaternion &)
Definition: quaternionI.H:538