From 0a13a399e7c7b3f974dfb28739deba6a1eff97f9 Mon Sep 17 00:00:00 2001
From: Vuko Vukcevic <vuko.vukcevic@fsb.hr>
Date: Mon, 13 Mar 2017 12:30:51 +0100
Subject: [PATCH] Update to eulerAngles static function in finiteRotation

---
 src/ODE/sixDOF/finiteRotation/finiteRotation.C | 16 +++++-----------
 src/ODE/sixDOF/finiteRotation/finiteRotation.H |  6 +++---
 2 files changed, 8 insertions(+), 14 deletions(-)

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