diff --git a/src/ODE/sixDOF/finiteRotation/finiteRotation.C b/src/ODE/sixDOF/finiteRotation/finiteRotation.C index aa407d91f..16ab4d9a4 100644 --- a/src/ODE/sixDOF/finiteRotation/finiteRotation.C +++ b/src/ODE/sixDOF/finiteRotation/finiteRotation.C @@ -77,21 +77,15 @@ Foam::vector Foam::finiteRotation::eulerAngles(const tensor& rotT) scalar& pitchAngle = eulerAngles.y(); scalar& yawAngle = eulerAngles.z(); - // Calculate roll angle - rollAngle = atan2(rotT.yz(), rotT.zz()); - - // Use mag to avoid negative value due to round-off, HJ, 24/Feb/2016 - // Bugfix: missing sqr. SS, 18/Apr/2016 - const scalar c2 = sqrt(sqr(rotT.xx()) + sqr(rotT.xy())); - // Calculate pitch angle - pitchAngle = atan2(-rotT.xz(), c2); + pitchAngle = asin(rotT.xz()); - const scalar s1 = sin(rollAngle); - const scalar c1 = cos(rollAngle); + // Calculate roll angle + const scalar cosPitch = cos(pitchAngle); + rollAngle = asin(-rotT.yz()/cosPitch); // Calculate yaw angle - yawAngle = atan2(s1*rotT.zx() - c1*rotT.yx(), c1*rotT.yy() - s1*rotT.zy()); + yawAngle = asin(-rotT.xy()/cosPitch); return eulerAngles; } diff --git a/src/ODE/sixDOF/finiteRotation/finiteRotation.H b/src/ODE/sixDOF/finiteRotation/finiteRotation.H index 87ac3abde..623605576 100644 --- a/src/ODE/sixDOF/finiteRotation/finiteRotation.H +++ b/src/ODE/sixDOF/finiteRotation/finiteRotation.H @@ -70,9 +70,9 @@ class finiteRotation //- Calculate rotation angle from given rotation tensor static scalar rotAngle(const tensor& rotT); - //- Calculate Euler angles (x-y-z (roll-pitch-yaw) convention) from - // given rotation tensor. Reference: Mike Day, Insomniac Games, - // Extracting Euler Angles from a Rotation Matrix. + //- Calculate Euler angles (x-y-z (roll-pitch-yaw) convention by Bryan) + // given the rotation tensor (global-to-local transformation). + // Reference: Nikravesh: Computer-Aided Analysis of Mechanical Systems static vector eulerAngles(const tensor& rotT);