From b4ebcd770f9ebd74b4e0f629f43e20b85999ecd4 Mon Sep 17 00:00:00 2001
From: Henry Weller <http://cfd.direct>
Date: Mon, 14 Mar 2016 08:07:42 +0000
Subject: [PATCH] quaternion: Added generalized construction from and
conversion to Euler-angles
The particular rotation sequence is specified via the enumeration:
//- Euler-angle rotation sequence
enum rotationSequence
{
ZYX, ZYZ, ZXY, ZXZ, YXZ, YXY, YZX, YZY, XYZ, XYX, XZY, XZX
};
and provided as an argument to the constructor from Euler-angles
//- Construct a quaternion given the three Euler angles:
inline quaternion
(
const rotationSequence rs,
const vector& angles
);
and conversion to Euler-angles:
//- Return a vector of euler angles corresponding to the
// specified rotation sequence
inline vector eulerAngles(const rotationSequence rs) const;
---
.../test/quaternion/Test-quaternion.C | 62 +++-
.../primitives/quaternion/quaternion.H | 37 ++-
.../primitives/quaternion/quaternionI.H | 307 +++++++++++++++---
.../solidBodyMotionFunctions/SDA/SDA.C | 2 +-
.../linearMotion/linearMotion.C | 2 +-
.../oscillatingLinearMotion.C | 2 +-
.../oscillatingRotatingMotion.C | 2 +-
.../tabulated6DoFMotion/tabulated6DoFMotion.C | 2 +-
8 files changed, 366 insertions(+), 50 deletions(-)
diff --git a/applications/test/quaternion/Test-quaternion.C b/applications/test/quaternion/Test-quaternion.C
index df616dfa333..31a6d3ff46b 100644
--- a/applications/test/quaternion/Test-quaternion.C
+++ b/applications/test/quaternion/Test-quaternion.C
@@ -1,10 +1,43 @@
+/*---------------------------------------------------------------------------*\
+ ========= |
+ \\ / F ield | OpenFOAM: The Open Source CFD Toolbox
+ \\ / O peration |
+ \\ / A nd | Copyright (C) 2011-2016 OpenFOAM Foundation
+ \\/ M anipulation |
+-------------------------------------------------------------------------------
+License
+ This file is part of OpenFOAM.
+
+ OpenFOAM is free software: you can redistribute it and/or modify it
+ under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
+
+Application
+ Test-quaternion
+
+Description
+ Test application for quaternions.
+
+\*---------------------------------------------------------------------------*/
+
#include "quaternion.H"
#include "septernion.H"
#include "IOstreams.H"
using namespace Foam;
-int main()
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+int main(int argc, char *argv[])
{
quaternion q(vector(1, 0, 0), 0.7853981);
Info<< "q " << q << endl;
@@ -36,5 +69,32 @@ int main()
*septernion(vector(0, 1, 0))).transform(v)
<< endl;
+ Info<< "Test conversion from and to Euler-angles" << endl;
+ vector angles(0.1, 0.2, 0.3);
+ for (int rs=quaternion::ZYX; rs<quaternion::XZX; ++rs)
+ {
+ if
+ (
+ mag
+ (
+ angles
+ - quaternion(quaternion::rotationSequence(rs), angles)
+ .eulerAngles(quaternion::rotationSequence(rs))
+ )
+ > SMALL
+ )
+ {
+ FatalErrorInFunction
+ << "Inconsistent conversion for rotation sequence "
+ << rs << exit(FatalError)
+ << endl;
+ }
+ }
+
+ Info<< "End\n" << endl;
+
return 0;
}
+
+
+// ************************************************************************* //
diff --git a/src/OpenFOAM/primitives/quaternion/quaternion.H b/src/OpenFOAM/primitives/quaternion/quaternion.H
index 1e67c9bbcf3..33e74c04a0d 100644
--- a/src/OpenFOAM/primitives/quaternion/quaternion.H
+++ b/src/OpenFOAM/primitives/quaternion/quaternion.H
@@ -72,12 +72,38 @@ class quaternion
//- Multiply vector v by quaternion as if v is a pure quaternion
inline quaternion mulq0v(const vector& v) const;
+ //- Conversion of two-axis rotation components into Euler-angles
+ inline static vector twoAxes
+ (
+ const scalar t11,
+ const scalar t12,
+ const scalar c2,
+ const scalar t31,
+ const scalar t32
+ );
+
+ //- Conversion of three-axis rotation components into Euler-angles
+ inline static vector threeAxes
+ (
+ const scalar t11,
+ const scalar t12,
+ const scalar s2,
+ const scalar t31,
+ const scalar t32
+ );
+
public:
//- Component type
typedef scalar cmptType;
+ //- Euler-angle rotation sequence
+ enum rotationSequence
+ {
+ ZYX, ZYZ, ZXY, ZXZ, YXZ, YXY, YZX, YZY, XYZ, XYX, XZY, XZX
+ };
+
// Member constants
@@ -123,9 +149,8 @@ public:
//- Construct a quaternion given the three Euler angles
inline quaternion
(
- const scalar angleX,
- const scalar angleY,
- const scalar angleZ
+ const rotationSequence rs,
+ const vector& angles
);
//- Construct a quaternion from a rotation tensor
@@ -148,9 +173,9 @@ public:
//- The rotation tensor corresponding the quaternion
inline tensor R() const;
- //- Return a vector of euler angles (rotations in radians about
- // the x, y and z axes.
- inline vector eulerAngles(const quaternion& q) const;
+ //- Return a vector of euler angles corresponding to the
+ // specified rotation sequence
+ inline vector eulerAngles(const rotationSequence rs) const;
inline quaternion normalized() const;
diff --git a/src/OpenFOAM/primitives/quaternion/quaternionI.H b/src/OpenFOAM/primitives/quaternion/quaternionI.H
index 025fa638cd0..3fa4013503f 100644
--- a/src/OpenFOAM/primitives/quaternion/quaternionI.H
+++ b/src/OpenFOAM/primitives/quaternion/quaternionI.H
@@ -2,7 +2,7 @@
========= |
\\ / F ield | OpenFOAM: The Open Source CFD Toolbox
\\ / O peration |
- \\ / A nd | Copyright (C) 2011-2013 OpenFOAM Foundation
+ \\ / A nd | Copyright (C) 2011-2016 OpenFOAM Foundation
\\/ M anipulation |
-------------------------------------------------------------------------------
License
@@ -28,18 +28,21 @@ License
inline Foam::quaternion::quaternion()
{}
+
inline Foam::quaternion::quaternion(const scalar w, const vector& v)
:
w_(w),
v_(v)
{}
+
inline Foam::quaternion::quaternion(const vector& d, const scalar theta)
:
w_(cos(0.5*theta)),
v_((sin(0.5*theta)/mag(d))*d)
{}
+
inline Foam::quaternion::quaternion
(
const vector& d,
@@ -60,30 +63,109 @@ inline Foam::quaternion::quaternion
}
}
+
inline Foam::quaternion::quaternion(const scalar w)
:
w_(w),
v_(vector::zero)
{}
+
inline Foam::quaternion::quaternion(const vector& v)
:
w_(0),
v_(v)
{}
+
inline Foam::quaternion::quaternion
(
- const scalar angleX,
- const scalar angleY,
- const scalar angleZ
+ const rotationSequence rs,
+ const vector& angles
)
{
- operator=(quaternion(vector(1, 0, 0), angleX));
- operator*=(quaternion(vector(0, 1, 0), angleY));
- operator*=(quaternion(vector(0, 0, 1), angleZ));
+ switch(rs)
+ {
+ case ZYX:
+ operator=(quaternion(vector(0, 0, 1), angles.x()));
+ operator*=(quaternion(vector(0, 1, 0), angles.y()));
+ operator*=(quaternion(vector(1, 0, 0), angles.z()));
+ break;
+
+ case ZYZ:
+ operator=(quaternion(vector(0, 0, 1), angles.x()));
+ operator*=(quaternion(vector(0, 1, 0), angles.y()));
+ operator*=(quaternion(vector(0, 0, 1), angles.z()));
+ break;
+
+ case ZXY:
+ operator=(quaternion(vector(0, 0, 1), angles.x()));
+ operator*=(quaternion(vector(1, 0, 0), angles.y()));
+ operator*=(quaternion(vector(0, 1, 0), angles.z()));
+ break;
+
+ case ZXZ:
+ operator=(quaternion(vector(0, 0, 1), angles.x()));
+ operator*=(quaternion(vector(1, 0, 0), angles.y()));
+ operator*=(quaternion(vector(0, 0, 1), angles.z()));
+ break;
+
+ case YXZ:
+ operator=(quaternion(vector(0, 1, 0), angles.x()));
+ operator*=(quaternion(vector(1, 0, 0), angles.y()));
+ operator*=(quaternion(vector(0, 0, 1), angles.z()));
+ break;
+
+ case YXY:
+ operator=(quaternion(vector(0, 1, 0), angles.x()));
+ operator*=(quaternion(vector(1, 0, 0), angles.y()));
+ operator*=(quaternion(vector(0, 1, 0), angles.z()));
+ break;
+
+ case YZX:
+ operator=(quaternion(vector(0, 1, 0), angles.x()));
+ operator*=(quaternion(vector(0, 0, 1), angles.y()));
+ operator*=(quaternion(vector(1, 0, 0), angles.z()));
+ break;
+
+ case YZY:
+ operator=(quaternion(vector(0, 1, 0), angles.x()));
+ operator*=(quaternion(vector(0, 0, 1), angles.y()));
+ operator*=(quaternion(vector(0, 1, 0), angles.z()));
+ break;
+
+ case XYZ:
+ operator=(quaternion(vector(1, 0, 0), angles.x()));
+ operator*=(quaternion(vector(0, 1, 0), angles.y()));
+ operator*=(quaternion(vector(0, 0, 1), angles.z()));
+ break;
+
+ case XYX:
+ operator=(quaternion(vector(1, 0, 0), angles.x()));
+ operator*=(quaternion(vector(0, 1, 0), angles.y()));
+ operator*=(quaternion(vector(1, 0, 0), angles.z()));
+ break;
+
+ case XZY:
+ operator=(quaternion(vector(1, 0, 0), angles.x()));
+ operator*=(quaternion(vector(0, 0, 1), angles.y()));
+ operator*=(quaternion(vector(0, 1, 0), angles.z()));
+ break;
+
+ case XZX:
+ operator=(quaternion(vector(1, 0, 0), angles.x()));
+ operator*=(quaternion(vector(0, 0, 1), angles.y()));
+ operator*=(quaternion(vector(1, 0, 0), angles.z()));
+ break;
+
+ default:
+ FatalErrorInFunction
+ << "Unknown rotation sequence " << rs << abort(FatalError);
+ break;
+ }
}
+
inline Foam::quaternion::quaternion
(
const tensor& rotationTensor
@@ -234,17 +316,17 @@ inline Foam::quaternion Foam::quaternion::invTransform
inline Foam::tensor Foam::quaternion::R() const
{
- scalar w2 = sqr(w());
- scalar x2 = sqr(v().x());
- scalar y2 = sqr(v().y());
- scalar z2 = sqr(v().z());
+ const scalar w2 = sqr(w());
+ const scalar x2 = sqr(v().x());
+ const scalar y2 = sqr(v().y());
+ const scalar z2 = sqr(v().z());
- scalar txy = 2*v().x()*v().y();
- scalar twz = 2*w()*v().z();
- scalar txz = 2*v().x()*v().z();
- scalar twy = 2*w()*v().y();
- scalar tyz = 2*v().y()*v().z();
- scalar twx = 2*w()*v().x();
+ const scalar txy = 2*v().x()*v().y();
+ const scalar twz = 2*w()*v().z();
+ const scalar txz = 2*v().x()*v().z();
+ const scalar twy = 2*w()*v().y();
+ const scalar tyz = 2*v().y()*v().z();
+ const scalar twx = 2*w()*v().x();
return tensor
(
@@ -255,32 +337,181 @@ inline Foam::tensor Foam::quaternion::R() const
}
-inline Foam::vector Foam::quaternion::eulerAngles(const quaternion& q) const
+inline Foam::vector Foam::quaternion::twoAxes
+(
+ const scalar t11,
+ const scalar t12,
+ const scalar c2,
+ const scalar t31,
+ const scalar t32
+)
{
- vector angles(vector::zero);
+ return vector(atan2(t11, t12), acos(c2), atan2(t31, t32));
+}
- const scalar& w = q.w();
- const vector& v = q.v();
- angles[0] = Foam::atan2
- (
- 2*(w*v.x() + v.y()*v.z()),
- 1 - 2*(sqr(v.x()) + sqr(v.y()))
- );
- angles[1] = Foam::asin(2*(w*v.y() - v.z()*v.x()));
- angles[2] = Foam::atan2
- (
- 2*(w*v.z() + v.x()*v.y()),
- 1 - 2*(sqr(v.y()) + sqr(v.z()))
- );
+inline Foam::vector Foam::quaternion::threeAxes
+(
+ const scalar t11,
+ const scalar t12,
+ const scalar s2,
+ const scalar t31,
+ const scalar t32
+)
+{
+ return vector(atan2(t11, t12), asin(s2), atan2(t31, t32));
+}
- // Convert to degrees
-// forAll(angles, aI)
-// {
-// angles[aI] = Foam::radToDeg(angles[aI]);
-// }
- return angles;
+inline Foam::vector Foam::quaternion::eulerAngles
+(
+ const rotationSequence rs
+) const
+{
+ const scalar w2 = sqr(w());
+ const scalar x2 = sqr(v().x());
+ const scalar y2 = sqr(v().y());
+ const scalar z2 = sqr(v().z());
+
+ switch(rs)
+ {
+ case ZYX:
+ return threeAxes
+ (
+ 2*(v().x()*v().y() + w()*v().z()),
+ w2 + x2 - y2 - z2,
+ 2*(w()*v().y() - v().x()*v().z()),
+ 2*(v().y()*v().z() + w()*v().x()),
+ w2 - x2 - y2 + z2
+ );
+ break;
+
+ case ZYZ:
+ return twoAxes
+ (
+ 2*(v().y()*v().z() - w()*v().x()),
+ 2*(v().x()*v().z() + w()*v().y()),
+ w2 - x2 - y2 + z2,
+ 2*(v().y()*v().z() + w()*v().x()),
+ 2*(w()*v().y() - v().x()*v().z())
+ );
+ break;
+
+ case ZXY:
+ return threeAxes
+ (
+ 2*(w()*v().z() - v().x()*v().y()),
+ w2 - x2 + y2 - z2,
+ 2*(v().y()*v().z() + w()*v().x()),
+ 2*(w()*v().y() - v().x()*v().z()),
+ w2 - x2 - y2 + z2
+ );
+ break;
+
+ case ZXZ:
+ return twoAxes
+ (
+ 2*(v().x()*v().z() + w()*v().y()),
+ 2*(w()*v().x() - v().y()*v().z()),
+ w2 - x2 - y2 + z2,
+ 2*(v().x()*v().z() - w()*v().y()),
+ 2*(v().y()*v().z() + w()*v().x())
+ );
+ break;
+
+ case YXZ:
+ return threeAxes
+ (
+ 2*(v().x()*v().z() + w()*v().y()),
+ w2 - x2 - y2 + z2,
+ 2*(w()*v().x() - v().y()*v().z()),
+ 2*(v().x()*v().y() + w()*v().z()),
+ w2 - x2 + y2 - z2
+ );
+ break;
+
+ case YXY:
+ return twoAxes
+ (
+ 2*(v().x()*v().y() - w()*v().z()),
+ 2*(v().y()*v().z() + w()*v().x()),
+ w2 - x2 + y2 - z2,
+ 2*(v().x()*v().y() + w()*v().z()),
+ 2*(w()*v().x() - v().y()*v().z())
+ );
+ break;
+
+ case YZX:
+ return threeAxes
+ (
+ 2*(w()*v().y() - v().x()*v().z()),
+ w2 + x2 - y2 - z2,
+ 2*(v().x()*v().y() + w()*v().z()),
+ 2*(w()*v().x() - v().y()*v().z()),
+ w2 - x2 + y2 - z2
+ );
+ break;
+
+ case YZY:
+ return twoAxes
+ (
+ 2*(v().y()*v().z() + w()*v().x()),
+ 2*(w()*v().z() - v().x()*v().y()),
+ w2 - x2 + y2 - z2,
+ 2*(v().y()*v().z() - w()*v().x()),
+ 2*(v().x()*v().y() + w()*v().z())
+ );
+ break;
+
+ case XYZ:
+ return threeAxes
+ (
+ 2*(w()*v().x() - v().y()*v().z()),
+ w2 - x2 - y2 + z2,
+ 2*(v().x()*v().z() + w()*v().y()),
+ 2*(w()*v().z() - v().x()*v().y()),
+ w2 + x2 - y2 - z2
+ );
+ break;
+
+ case XYX:
+ return twoAxes
+ (
+ 2*(v().x()*v().y() + w()*v().z()),
+ 2*(w()*v().y() - v().x()*v().z()),
+ w2 + x2 - y2 - z2,
+ 2*(v().x()*v().y() - w()*v().z()),
+ 2*(v().x()*v().z() + w()*v().y())
+ );
+ break;
+
+ case XZY:
+ return threeAxes
+ (
+ 2*(v().y()*v().z() + w()*v().x()),
+ w2 - x2 + y2 - z2,
+ 2*(w()*v().z() - v().x()*v().y()),
+ 2*(v().x()*v().z() + w()*v().y()),
+ w2 + x2 - y2 - z2
+ );
+ break;
+
+ case XZX:
+ return twoAxes
+ (
+ 2*(v().x()*v().z() - w()*v().y()),
+ 2*(v().x()*v().y() + w()*v().z()),
+ w2 + x2 - y2 - z2,
+ 2*(v().x()*v().z() + w()*v().y()),
+ 2*(w()*v().z() - v().x()*v().y())
+ );
+ break;
+ default:
+ FatalErrorInFunction
+ << "Unknown rotation sequence " << rs << abort(FatalError);
+ return vector::zero;
+ break;
+ }
}
diff --git a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/SDA/SDA.C b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/SDA/SDA.C
index 269cc3217ae..cb8ac123850 100644
--- a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/SDA/SDA.C
+++ b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/SDA/SDA.C
@@ -90,7 +90,7 @@ Foam::septernion Foam::solidBodyMotionFunctions::SDA::transformation() const
swayA_*(sin(wr*time + phs) - sin(phs)),
heaveA_*(sin(wr*time + phh) - sin(phh))
);
- quaternion R(rollA*sin(wr*time + phr), 0, 0);
+ quaternion R(quaternion::XYZ, vector(rollA*sin(wr*time + phr), 0, 0));
septernion TR(septernion(CofG_ + T)*R*septernion(-CofG_));
InfoInFunction << "Time = " << time << " transformation: " << TR << endl;
diff --git a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/linearMotion/linearMotion.C b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/linearMotion/linearMotion.C
index 6a9f7bc5b4b..a3be9a8f836 100644
--- a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/linearMotion/linearMotion.C
+++ b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/linearMotion/linearMotion.C
@@ -73,7 +73,7 @@ Foam::solidBodyMotionFunctions::linearMotion::transformation() const
// Translation of centre of gravity with constant velocity
const vector displacement = velocity_*t;
- quaternion R(0, 0, 0);
+ quaternion R(1);
septernion TR(septernion(displacement)*R);
InfoInFunction << "Time = " << t << " transformation: " << TR << endl;
diff --git a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingLinearMotion/oscillatingLinearMotion.C b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingLinearMotion/oscillatingLinearMotion.C
index 5a090577ff7..9e03d3b93f2 100644
--- a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingLinearMotion/oscillatingLinearMotion.C
+++ b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingLinearMotion/oscillatingLinearMotion.C
@@ -73,7 +73,7 @@ Foam::solidBodyMotionFunctions::oscillatingLinearMotion::transformation() const
const vector displacement = amplitude_*sin(omega_*t);
- quaternion R(0, 0, 0);
+ quaternion R(1);
septernion TR(septernion(displacement)*R);
InfoInFunction << "Time = " << t << " transformation: " << TR << endl;
diff --git a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingRotatingMotion/oscillatingRotatingMotion.C b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingRotatingMotion/oscillatingRotatingMotion.C
index 64516acf992..fcceb2f0e4b 100644
--- a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingRotatingMotion/oscillatingRotatingMotion.C
+++ b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingRotatingMotion/oscillatingRotatingMotion.C
@@ -81,7 +81,7 @@ transformation() const
// Convert the rotational motion from deg to rad
eulerAngles *= pi/180.0;
- quaternion R(eulerAngles.x(), eulerAngles.y(), eulerAngles.z());
+ quaternion R(quaternion::XYZ, eulerAngles);
septernion TR(septernion(origin_)*R*septernion(-origin_));
InfoInFunction << "Time = " << t << " transformation: " << TR << endl;
diff --git a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/tabulated6DoFMotion/tabulated6DoFMotion.C b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/tabulated6DoFMotion/tabulated6DoFMotion.C
index b25838c75da..356d2a8266d 100644
--- a/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/tabulated6DoFMotion/tabulated6DoFMotion.C
+++ b/src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/tabulated6DoFMotion/tabulated6DoFMotion.C
@@ -104,7 +104,7 @@ Foam::solidBodyMotionFunctions::tabulated6DoFMotion::transformation() const
// Convert the rotational motion from deg to rad
TRV[1] *= pi/180.0;
- quaternion R(TRV[1].x(), TRV[1].y(), TRV[1].z());
+ quaternion R(quaternion::XYZ, TRV[1]);
septernion TR(septernion(CofG_ + TRV[0])*R*septernion(-CofG_));
InfoInFunction << "Time = " << t << " transformation: " << TR << endl;
--
GitLab