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;

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;
 }
+
+
+// ************************************************************************* //

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 rotationSequence rs,
-            const scalar angleY,
+            const vector& angles
-            const scalar angleZ
         );
 
         //- 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
+               //- Return a vector of euler angles corresponding to the
-               //  the x, y and z axes.
+               //  specified rotation sequence
-               inline vector eulerAngles(const quaternion& q) const;
+               inline vector eulerAngles(const rotationSequence rs) const;
 
                inline quaternion normalized() const;
 

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 rotationSequence rs,
-    const scalar angleY,
+    const vector& angles
-    const scalar angleZ
 )
 {
-    operator=(quaternion(vector(1, 0, 0), angleX));
+    switch(rs)
-    operator*=(quaternion(vector(0, 1, 0), angleY));
+    {
-    operator*=(quaternion(vector(0, 0, 1), angleZ));
+        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());
+    const scalar w2 = sqr(w());
-    scalar x2 = sqr(v().x());
+    const scalar x2 = sqr(v().x());
-    scalar y2 = sqr(v().y());
+    const scalar y2 = sqr(v().y());
-    scalar z2 = sqr(v().z());
+    const scalar z2 = sqr(v().z());
 
-    scalar txy = 2*v().x()*v().y();
+    const scalar txy = 2*v().x()*v().y();
-    scalar twz = 2*w()*v().z();
+    const scalar twz = 2*w()*v().z();
-    scalar txz = 2*v().x()*v().z();
+    const scalar txz = 2*v().x()*v().z();
-    scalar twy = 2*w()*v().y();
+    const scalar twy = 2*w()*v().y();
-    scalar tyz = 2*v().y()*v().z();
+    const scalar tyz = 2*v().y()*v().z();
-    scalar twx = 2*w()*v().x();
+    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
+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));
+}
+
+
+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*(w*v.x() + v.y()*v.z()),
+                2*(v().x()*v().y() + w()*v().z()),
-        1 - 2*(sqr(v.x()) + sqr(v.y()))
+                w2 + x2 - y2 - z2,
+                2*(w()*v().y() - v().x()*v().z()),
+                2*(v().y()*v().z() + w()*v().x()),
+                w2 - x2 - y2 + z2
             );
-    angles[1] = Foam::asin(2*(w*v.y() - v.z()*v.x()));
+            break;
-    angles[2] = Foam::atan2
+
+        case ZYZ:
+            return twoAxes
             (
-        2*(w*v.z() + v.x()*v.y()),
+                2*(v().y()*v().z() - w()*v().x()),
-        1 - 2*(sqr(v.y()) + sqr(v.z()))
+                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;
 
-    // Convert to degrees
+        case ZXY:
-//    forAll(angles, aI)
+            return threeAxes
-//    {
+            (
-//        angles[aI] = Foam::radToDeg(angles[aI]);
+                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;
 
-    return angles;
+        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;
+    }
 }
 
 

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;

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;

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;

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;

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;