songtianlun/openfoam
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

quaternion: Added generalized construction from and conversion to Euler-angles

Browse Source 
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;
This commit is contained in:
Henry Weller 2016-03-14 08:07:42 +00:00
parent 6e2e761d37
commit b4ebcd770f
8 changed files with 366 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
applications/test/quaternion/Test-quaternion.C
View File

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

37
src/OpenFOAM/primitives/quaternion/quaternion.H
View File

@ -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;

307
src/OpenFOAM/primitives/quaternion/quaternionI.H
View File

@ -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;
}
}

2
src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/SDA/SDA.C
View File

@ -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;

2
src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/linearMotion/linearMotion.C
View File

@ -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;

2
src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingLinearMotion/oscillatingLinearMotion.C
View File

@ -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;

2
src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/oscillatingRotatingMotion/oscillatingRotatingMotion.C
View File

@ -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;

2
src/dynamicFvMesh/solidBodyMotionFvMesh/solidBodyMotionFunctions/tabulated6DoFMotion/tabulated6DoFMotion.C
View File

@ -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;