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 | www.openfoam.com
6 \\/ M anipulation |
7-------------------------------------------------------------------------------
8 Copyright (C) 2011-2016 OpenFOAM Foundation
9 Copyright (C) 2019-2022 OpenCFD Ltd.
10-------------------------------------------------------------------------------
11License
12 This file is part of OpenFOAM.
13
14 OpenFOAM is free software: you can redistribute it and/or modify it
15 under the terms of the GNU General Public License as published by
16 the Free Software Foundation, either version 3 of the License, or
17 (at your option) any later version.
18
19 OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
20 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
21 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
22 for more details.
23
24 You should have received a copy of the GNU General Public License
25 along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
26
27\*---------------------------------------------------------------------------*/
28
29// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
30
32:
33 w_(0),
34 v_(Zero)
35{}
36
37
38inline Foam::quaternion::quaternion(const scalar w, const vector& v)
39:
40 w_(w),
41 v_(v)
42{}
43
44
45inline Foam::quaternion::quaternion(const vector& d, const scalar theta)
46:
47 w_(cos(0.5*theta)),
48 v_(sin(0.5*theta)*Foam::normalised(d))
49{}
50
51
53(
54 const vector& d,
55 const scalar cosTheta,
56 const bool isNormalised
57)
58{
59 const scalar cosHalfTheta2 = 0.5*(cosTheta + 1);
60 w_ = sqrt(cosHalfTheta2);
61
62 if (isNormalised)
63 {
64 v_ = sqrt(1 - cosHalfTheta2)*d;
65 }
66 else
67 {
68 v_ = sqrt(1 - cosHalfTheta2)*Foam::normalised(d);
69 }
70}
71
72
73inline Foam::quaternion::quaternion(const scalar w)
74:
75 w_(w),
76 v_(Zero)
77{}
78
79
81:
82 w_(0),
83 v_(v)
84{}
85
86
88{
89 return quaternion(sqrt(1 - magSqr(v)), v);
90}
91
92
94(
95 const eulerOrder order,
96 const vector& angles
97)
98{
99 switch (order)
100 {
101 case ZYX:
102 {
103 operator=(quaternion(vector(0, 0, 1), angles.x()));
104 operator*=(quaternion(vector(0, 1, 0), angles.y()));
105 operator*=(quaternion(vector(1, 0, 0), angles.z()));
106 break;
107 }
108
109 case ZYZ:
110 {
111 operator=(quaternion(vector(0, 0, 1), angles.x()));
112 operator*=(quaternion(vector(0, 1, 0), angles.y()));
113 operator*=(quaternion(vector(0, 0, 1), angles.z()));
114 break;
115 }
116
117 case ZXY:
118 {
119 operator=(quaternion(vector(0, 0, 1), angles.x()));
120 operator*=(quaternion(vector(1, 0, 0), angles.y()));
121 operator*=(quaternion(vector(0, 1, 0), angles.z()));
122 break;
123 }
124
125 case ZXZ:
126 {
127 operator=(quaternion(vector(0, 0, 1), angles.x()));
128 operator*=(quaternion(vector(1, 0, 0), angles.y()));
129 operator*=(quaternion(vector(0, 0, 1), angles.z()));
130 break;
131 }
132
133 case YXZ:
134 {
135 operator=(quaternion(vector(0, 1, 0), angles.x()));
136 operator*=(quaternion(vector(1, 0, 0), angles.y()));
137 operator*=(quaternion(vector(0, 0, 1), angles.z()));
138 break;
139 }
140
141 case YXY:
142 {
143 operator=(quaternion(vector(0, 1, 0), angles.x()));
144 operator*=(quaternion(vector(1, 0, 0), angles.y()));
145 operator*=(quaternion(vector(0, 1, 0), angles.z()));
146 break;
147 }
148
149 case YZX:
150 {
151 operator=(quaternion(vector(0, 1, 0), angles.x()));
152 operator*=(quaternion(vector(0, 0, 1), angles.y()));
153 operator*=(quaternion(vector(1, 0, 0), angles.z()));
154 break;
155 }
156
157 case YZY:
158 {
159 operator=(quaternion(vector(0, 1, 0), angles.x()));
160 operator*=(quaternion(vector(0, 0, 1), angles.y()));
161 operator*=(quaternion(vector(0, 1, 0), angles.z()));
162 break;
163 }
164
165 case XYZ:
166 {
167 operator=(quaternion(vector(1, 0, 0), angles.x()));
168 operator*=(quaternion(vector(0, 1, 0), angles.y()));
169 operator*=(quaternion(vector(0, 0, 1), angles.z()));
170 break;
171 }
172
173 case XYX:
174 {
175 operator=(quaternion(vector(1, 0, 0), angles.x()));
176 operator*=(quaternion(vector(0, 1, 0), angles.y()));
177 operator*=(quaternion(vector(1, 0, 0), angles.z()));
178 break;
179 }
180
181 case XZY:
182 {
183 operator=(quaternion(vector(1, 0, 0), angles.x()));
184 operator*=(quaternion(vector(0, 0, 1), angles.y()));
185 operator*=(quaternion(vector(0, 1, 0), angles.z()));
186 break;
187 }
188
189 case XZX:
190 {
191 operator=(quaternion(vector(1, 0, 0), angles.x()));
192 operator*=(quaternion(vector(0, 0, 1), angles.y()));
193 operator*=(quaternion(vector(1, 0, 0), angles.z()));
194 break;
195 }
196
197 default:
199 << "Unknown euler rotation order "
200 << int(order) << abort(FatalError);
201 break;
202 }
203}
204
205
207(
209)
210{
211 scalar trace =
214 + rotationTensor.zz();
215
216 if (trace > 0)
217 {
218 scalar s = 0.5/Foam::sqrt(trace + 1.0);
219
220 w_ = 0.25/s;
221 v_[0] = (rotationTensor.zy() - rotationTensor.yz())*s;
222 v_[1] = (rotationTensor.xz() - rotationTensor.zx())*s;
223 v_[2] = (rotationTensor.yx() - rotationTensor.xy())*s;
224 }
225 else
226 {
227 if
228 (
231 )
232 {
233 const scalar s = 2.0*Foam::sqrt
234 (
235 1.0
239 );
240
241 w_ = (rotationTensor.zy() - rotationTensor.yz())/s;
242 v_[0] = 0.25*s;
243 v_[1] = (rotationTensor.xy() + rotationTensor.yx())/s;
244 v_[2] = (rotationTensor.xz() + rotationTensor.zx())/s;
245 }
246 else if
247 (
249 )
250 {
251 const scalar s = 2.0*Foam::sqrt
252 (
253 1.0
257 );
258
259 w_ = (rotationTensor.xz() - rotationTensor.zx())/s;
260 v_[0] = (rotationTensor.xy() + rotationTensor.yx())/s;
261 v_[1] = 0.25*s;
262 v_[2] = (rotationTensor.yz() + rotationTensor.zy())/s;
263 }
264 else
265 {
266 const scalar s = 2.0*Foam::sqrt
267 (
268 1.0
272 );
273
274 w_ = (rotationTensor.yx() - rotationTensor.xy())/s;
275 v_[0] = (rotationTensor.xz() + rotationTensor.zx())/s;
276 v_[1] = (rotationTensor.yz() + rotationTensor.zy())/s;
277 v_[2] = 0.25*s;
278 }
279 }
280}
281
282
283// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
284
285inline Foam::scalar Foam::quaternion::w() const noexcept
286{
287 return w_;
288}
289
290
292{
293 return v_;
294}
295
296
297inline Foam::scalar& Foam::quaternion::w() noexcept
298{
299 return w_;
300}
301
302
304{
305 return v_;
306}
307
308
310{
311 const scalar s(Foam::mag(*this));
312
313 if (s < ROOTVSMALL)
314 {
315 *this = Zero;
316 }
317 else
318 {
319 *this /= s;
320 }
321 return *this;
322}
323
324
325inline Foam::quaternion Foam::quaternion::mulq0v(const vector& u) const
326{
327 return quaternion(-(v() & u), w()*u + (v() ^ u));
328}
329
330
332{
333 return (mulq0v(u)*conjugate(*this)).v();
334}
335
336
338{
339 return (conjugate(*this).mulq0v(u)*(*this)).v();
340}
341
342
344{
345 return Foam::normalised((*this)*q);
346}
347
348
350(
351 const quaternion& q
352) const
353{
354 return Foam::normalised(conjugate(*this)*q);
355}
356
357
359{
360 const scalar w2 = sqr(w());
361 const scalar x2 = sqr(v().x());
362 const scalar y2 = sqr(v().y());
363 const scalar z2 = sqr(v().z());
364
365 const scalar txy = 2*v().x()*v().y();
366 const scalar twz = 2*w()*v().z();
367 const scalar txz = 2*v().x()*v().z();
368 const scalar twy = 2*w()*v().y();
369 const scalar tyz = 2*v().y()*v().z();
370 const scalar twx = 2*w()*v().x();
371
372 return tensor
373 (
374 w2 + x2 - y2 - z2, txy - twz, txz + twy,
375 txy + twz, w2 - x2 + y2 - z2, tyz - twx,
376 txz - twy, tyz + twx, w2 - x2 - y2 + z2
377 );
378}
379
380
381inline Foam::vector Foam::quaternion::twoAxes
382(
383 const scalar t11,
384 const scalar t12,
385 const scalar c2,
386 const scalar t31,
387 const scalar t32
388)
389{
390 return vector(atan2(t11, t12), acos(c2), atan2(t31, t32));
391}
392
393
394inline Foam::vector Foam::quaternion::threeAxes
395(
396 const scalar t11,
397 const scalar t12,
398 const scalar s2,
399 const scalar t31,
400 const scalar t32
401)
402{
403 return vector(atan2(t11, t12), asin(s2), atan2(t31, t32));
404}
405
406
408(
409 const eulerOrder order
410) const
411{
412 const scalar w2 = sqr(w());
413 const scalar x2 = sqr(v().x());
414 const scalar y2 = sqr(v().y());
415 const scalar z2 = sqr(v().z());
416
417 switch (order)
418 {
419 case ZYX:
420 {
421 return threeAxes
422 (
423 2*(v().x()*v().y() + w()*v().z()),
424 w2 + x2 - y2 - z2,
425 2*(w()*v().y() - v().x()*v().z()),
426 2*(v().y()*v().z() + w()*v().x()),
427 w2 - x2 - y2 + z2
428 );
429 break;
430 }
431
432 case ZYZ:
433 {
434 return twoAxes
435 (
436 2*(v().y()*v().z() - w()*v().x()),
437 2*(v().x()*v().z() + w()*v().y()),
438 w2 - x2 - y2 + z2,
439 2*(v().y()*v().z() + w()*v().x()),
440 2*(w()*v().y() - v().x()*v().z())
441 );
442 break;
443 }
444
445 case ZXY:
446 {
447 return threeAxes
448 (
449 2*(w()*v().z() - v().x()*v().y()),
450 w2 - x2 + y2 - z2,
451 2*(v().y()*v().z() + w()*v().x()),
452 2*(w()*v().y() - v().x()*v().z()),
453 w2 - x2 - y2 + z2
454 );
455 break;
456 }
457
458 case ZXZ:
459 {
460 return twoAxes
461 (
462 2*(v().x()*v().z() + w()*v().y()),
463 2*(w()*v().x() - v().y()*v().z()),
464 w2 - x2 - y2 + z2,
465 2*(v().x()*v().z() - w()*v().y()),
466 2*(v().y()*v().z() + w()*v().x())
467 );
468 break;
469 }
470
471 case YXZ:
472 {
473 return threeAxes
474 (
475 2*(v().x()*v().z() + w()*v().y()),
476 w2 - x2 - y2 + z2,
477 2*(w()*v().x() - v().y()*v().z()),
478 2*(v().x()*v().y() + w()*v().z()),
479 w2 - x2 + y2 - z2
480 );
481 break;
482 }
483
484 case YXY:
485 {
486 return twoAxes
487 (
488 2*(v().x()*v().y() - w()*v().z()),
489 2*(v().y()*v().z() + w()*v().x()),
490 w2 - x2 + y2 - z2,
491 2*(v().x()*v().y() + w()*v().z()),
492 2*(w()*v().x() - v().y()*v().z())
493 );
494 break;
495 }
496
497 case YZX:
498 {
499 return threeAxes
500 (
501 2*(w()*v().y() - v().x()*v().z()),
502 w2 + x2 - y2 - z2,
503 2*(v().x()*v().y() + w()*v().z()),
504 2*(w()*v().x() - v().y()*v().z()),
505 w2 - x2 + y2 - z2
506 );
507 break;
508 }
509
510 case YZY:
511 {
512 return twoAxes
513 (
514 2*(v().y()*v().z() + w()*v().x()),
515 2*(w()*v().z() - v().x()*v().y()),
516 w2 - x2 + y2 - z2,
517 2*(v().y()*v().z() - w()*v().x()),
518 2*(v().x()*v().y() + w()*v().z())
519 );
520 break;
521 }
522
523 case XYZ:
524 {
525 return threeAxes
526 (
527 2*(w()*v().x() - v().y()*v().z()),
528 w2 - x2 - y2 + z2,
529 2*(v().x()*v().z() + w()*v().y()),
530 2*(w()*v().z() - v().x()*v().y()),
531 w2 + x2 - y2 - z2
532 );
533 break;
534 }
535
536 case XYX:
537 {
538 return twoAxes
539 (
540 2*(v().x()*v().y() + w()*v().z()),
541 2*(w()*v().y() - v().x()*v().z()),
542 w2 + x2 - y2 - z2,
543 2*(v().x()*v().y() - w()*v().z()),
544 2*(v().x()*v().z() + w()*v().y())
545 );
546 break;
547 }
548
549 case XZY:
550 {
551 return threeAxes
552 (
553 2*(v().y()*v().z() + w()*v().x()),
554 w2 - x2 + y2 - z2,
555 2*(w()*v().z() - v().x()*v().y()),
556 2*(v().x()*v().z() + w()*v().y()),
557 w2 + x2 - y2 - z2
558 );
559 break;
560 }
561
562 case XZX:
563 {
564 return twoAxes
565 (
566 2*(v().x()*v().z() - w()*v().y()),
567 2*(v().x()*v().y() + w()*v().z()),
568 w2 + x2 - y2 - z2,
569 2*(v().x()*v().z() + w()*v().y()),
570 2*(w()*v().z() - v().x()*v().y())
571 );
572 break;
573 }
574
575 default:
577 << "Unknown euler rotation order "
578 << int(order) << abort(FatalError);
579 break;
580 }
581
582 return Zero;
583}
584
585
586// * * * * * * * * * * * * * * * Member Operators * * * * * * * * * * * * * //
587
589{
590 w_ += q.w_;
591 v_ += q.v_;
592}
593
595{
596 w_ -= q.w_;
597 v_ -= q.v_;
598}
599
601{
602 scalar w0 = w();
603 w() = w()*q.w() - (v() & q.v());
604 v() = w0*q.v() + q.w()*v() + (v() ^ q.v());
605}
606
608{
609 return operator*=(inv(q));
610}
611
612
613inline void Foam::quaternion::operator=(const scalar s)
614{
615 w_ = s;
616}
617
618
620{
621 v_ = v;
622}
623
624
626{
627 w_ = 0;
628 v_ = Zero;
629}
630
631
632inline void Foam::quaternion::operator*=(const scalar s)
633{
634 w_ *= s;
635 v_ *= s;
636}
637
638inline void Foam::quaternion::operator/=(const scalar s)
639{
640 w_ /= s;
641 v_ /= s;
642}
643
644
645// * * * * * * * * * * * * * * * Global Functions * * * * * * * * * * * * * //
646
647inline Foam::scalar Foam::magSqr(const quaternion& q)
648{
649 return magSqr(q.w()) + magSqr(q.v());
650}
651
652
653inline Foam::scalar Foam::mag(const quaternion& q)
654{
655 return sqrt(magSqr(q));
656}
657
658
660{
661 return quaternion(q.w(), -q.v());
662}
663
664
666{
667 const scalar s(Foam::magSqr(q));
668
669 if (s < VSMALL)
670 {
671 return Zero;
672 }
673 else
674 {
675 return quaternion(q.w()/s, -q.v()/s);
676 }
677}
678
679
681{
682 const scalar s(Foam::mag(q));
683
684 if (s < ROOTVSMALL)
685 {
686 return Zero;
687 }
688 else
689 {
690 return q/s;
691 }
692}
693
694
695// * * * * * * * * * * * * * * * Global Operators * * * * * * * * * * * * * //
696
697inline bool Foam::operator==(const quaternion& q1, const quaternion& q2)
698{
699 return (equal(q1.w(), q2.w()) && equal(q1.v(), q2.v()));
700}
701
702
703inline bool Foam::operator!=(const quaternion& q1, const quaternion& q2)
704{
705 return !operator==(q1, q2);
706}
707
708
709inline Foam::quaternion Foam::operator+
710(
711 const quaternion& q1,
712 const quaternion& q2
713)
714{
715 return quaternion(q1.w() + q2.w(), q1.v() + q2.v());
716}
717
718
720{
721 return quaternion(-q.w(), -q.v());
722}
723
724
725inline Foam::quaternion Foam::operator-
726(
727 const quaternion& q1,
728 const quaternion& q2
729)
730{
731 return quaternion(q1.w() - q2.w(), q1.v() - q2.v());
732}
733
734
735inline Foam::scalar Foam::operator&(const quaternion& q1, const quaternion& q2)
736{
737 return q1.w()*q2.w() + (q1.v() & q2.v());
738}
739
740
741inline Foam::quaternion Foam::operator*
742(
743 const quaternion& q1,
744 const quaternion& q2
745)
746{
747 return quaternion
748 (
749 q1.w()*q2.w() - (q1.v() & q2.v()),
750 q1.w()*q2.v() + q2.w()*q1.v() + (q1.v() ^ q2.v())
751 );
752}
753
754
755inline Foam::quaternion Foam::operator/
756(
757 const quaternion& q1,
758 const quaternion& q2
759)
760{
761 return q1*inv(q2);
762}
763
764
765inline Foam::quaternion Foam::operator*(const scalar s, const quaternion& q)
766{
767 return quaternion(s*q.w(), s*q.v());
768}
769
770
771inline Foam::quaternion Foam::operator*(const quaternion& q, const scalar s)
772{
773 return quaternion(s*q.w(), s*q.v());
774}
775
776
777inline Foam::quaternion Foam::operator/(const quaternion& q, const scalar s)
778{
779 return quaternion(q.w()/s, q.v()/s);
780}
781
782
783// ************************************************************************* //
scalar y
#define w2
Definition: blockCreate.C:35
#define w0
Definition: blockCreate.C:33
const Cmpt & xx() const
Definition: TensorI.H:153
const Cmpt & yx() const
Definition: TensorI.H:174
const Cmpt & yz() const
Definition: TensorI.H:188
const Cmpt & xz() const
Definition: TensorI.H:167
const Cmpt & zz() const
Definition: TensorI.H:209
const Cmpt & xy() const
Definition: TensorI.H:160
const Cmpt & zx() const
Definition: TensorI.H:195
const Cmpt & zy() const
Definition: TensorI.H:202
const Cmpt & yy() const
Definition: TensorI.H:181
const Cmpt & z() const
Access to the vector z component.
Definition: VectorI.H:85
const Cmpt & y() const
Access to the vector y component.
Definition: VectorI.H:79
const Cmpt & x() const
Access to the vector x component.
Definition: VectorI.H:73
virtual word unit() const
Return time unit.
Definition: crankConRod.C:143
Default transformation behaviour.
Quaternion class used to perform rotations in 3D space.
Definition: quaternion.H:58
void operator/=(const quaternion &q)
Definition: quaternionI.H:607
vector eulerAngles(const eulerOrder order) const
Definition: quaternionI.H:408
const vector & v() const noexcept
Vector part of the quaternion ( = axis of rotation)
Definition: quaternionI.H:291
tensor R() const
The rotation tensor corresponding to the quaternion.
Definition: quaternionI.H:358
void operator-=(const quaternion &q)
Definition: quaternionI.H:594
void operator+=(const quaternion &q)
Definition: quaternionI.H:588
void operator*=(const quaternion &q)
Definition: quaternionI.H:600
scalar w() const noexcept
Scalar part of the quaternion ( = cos(theta/2) for rotation)
Definition: quaternionI.H:285
quaternion & normalise()
Inplace normalise the quaternion by its magnitude.
Definition: quaternionI.H:309
quaternion & operator=(const quaternion &)=default
Copy assignment.
quaternion()=default
Default construct.
eulerOrder
Euler-angle rotation order.
Definition: quaternion.H:104
vector invTransform(const vector &v) const
Rotate the given vector anti-clockwise.
Definition: quaternionI.H:337
A class representing the concept of 0 (zero) that can be used to avoid manipulating objects known to ...
Definition: zero.H:63
#define FatalErrorInFunction
Report an error message using Foam::FatalError.
Definition: error.H:453
gmvFile<< "tracers "<< particles.size()<< nl;for(const passiveParticle &p :particles){ gmvFile<< p.position().x()<< " ";}gmvFile<< nl;for(const passiveParticle &p :particles){ gmvFile<< p.position().y()<< " ";}gmvFile<< nl;for(const passiveParticle &p :particles){ gmvFile<< p.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))
Namespace for OpenFOAM.
tmp< faMatrix< Type > > operator-(const faMatrix< Type > &)
Unary negation.
bool operator!=(const eddy &a, const eddy &b)
Definition: eddy.H:239
dimensionedScalar asin(const dimensionedScalar &ds)
dimensionedSymmTensor sqr(const dimensionedVector &dv)
quaternion normalised(const quaternion &q)
Return the normalised (unit) quaternion of the given quaternion.
Definition: quaternionI.H:680
tensor rotationTensor(const vector &n1, const vector &n2)
Rotational transformation tensor from vector n1 to n2.
Definition: transform.H:51
dimensionedScalar sin(const dimensionedScalar &ds)
tmp< faMatrix< Type > > operator*(const areaScalarField::Internal &, const faMatrix< Type > &)
Tensor< scalar > tensor
Definition: symmTensor.H:61
tmp< GeometricField< Type, fvPatchField, volMesh > > operator&(const fvMatrix< Type > &, const DimensionedField< Type, volMesh > &)
dimensionedScalar operator/(const scalar s1, const dimensionedScalar &ds2)
dimensionedScalar atan2(const dimensionedScalar &x, const dimensionedScalar &y)
bool equal(const T &s1, const T &s2)
Compare two values for equality.
Definition: doubleFloat.H:46
tmp< faMatrix< Type > > operator==(const faMatrix< Type > &, const faMatrix< Type > &)
dimensionedScalar sqrt(const dimensionedScalar &ds)
dimensioned< typename typeOfMag< Type >::type > mag(const dimensioned< Type > &dt)
errorManip< error > abort(error &err)
Definition: errorManip.H:144
static constexpr const zero Zero
Global zero (0)
Definition: zero.H:131
const direction noexcept
Definition: Scalar.H:223
error FatalError
dimensionedSphericalTensor inv(const dimensionedSphericalTensor &dt)
quaternion conjugate(const quaternion &q)
Return the conjugate of the given quaternion.
Definition: quaternionI.H:659
Vector< scalar > vector
Definition: vector.H:61
dimensioned< typename typeOfMag< Type >::type > magSqr(const dimensioned< Type > &dt)
dimensionedScalar cos(const dimensionedScalar &ds)
dimensionedScalar acos(const dimensionedScalar &ds)