From c030c5e40a8548292bc12f63e32973dfd3c82ba5 Mon Sep 17 00:00:00 2001
From: laurence <laurence>
Date: Mon, 9 Jul 2012 12:31:30 +0100
Subject: [PATCH] ENH: Add rotation tensor constructor to quaternion class

---
 .../primitives/quaternion/quaternion.H        |  3 +
 .../primitives/quaternion/quaternionI.H       | 75 +++++++++++++++++++
 2 files changed, 78 insertions(+)

diff --git a/src/OpenFOAM/primitives/quaternion/quaternion.H b/src/OpenFOAM/primitives/quaternion/quaternion.H
index 9a7c297189..1c2bfb1d2e 100644
--- a/src/OpenFOAM/primitives/quaternion/quaternion.H
+++ b/src/OpenFOAM/primitives/quaternion/quaternion.H
@@ -109,6 +109,9 @@ public:
             const scalar angleZ
         );
 
+        //- Construct a quaternion from a rotation tensor
+        inline explicit quaternion(const tensor& rotationTensor);
+
         //- Construct from Istream
         quaternion(Istream&);
 
diff --git a/src/OpenFOAM/primitives/quaternion/quaternionI.H b/src/OpenFOAM/primitives/quaternion/quaternionI.H
index 19b5c79130..01c5b74bc2 100644
--- a/src/OpenFOAM/primitives/quaternion/quaternionI.H
+++ b/src/OpenFOAM/primitives/quaternion/quaternionI.H
@@ -66,6 +66,81 @@ inline Foam::quaternion::quaternion
     operator*=(quaternion(vector(0, 0, 1), angleZ));
 }
 
+inline Foam::quaternion::quaternion
+(
+    const tensor& rotationTensor
+)
+{
+    scalar trace = rotationTensor.xx()
+                 + rotationTensor.yy()
+                 + rotationTensor.zz();
+
+    if (trace > 0)
+    {
+        scalar s = 0.5/Foam::sqrt(trace + 1.0);
+
+        w_ = 0.25/s;
+        v_[0] = (rotationTensor.zy() - rotationTensor.yz())*s;
+        v_[1] = (rotationTensor.xz() - rotationTensor.zx())*s;
+        v_[2] = (rotationTensor.yx() - rotationTensor.xy())*s;
+    }
+    else
+    {
+        if
+        (
+            rotationTensor.xx() > rotationTensor.yy()
+         && rotationTensor.xx() > rotationTensor.zz()
+        )
+        {
+            scalar s = 2.0*Foam::sqrt
+            (
+                1.0
+              + rotationTensor.xx()
+              - rotationTensor.yy()
+              - rotationTensor.zz()
+            );
+
+            w_ = (rotationTensor.zy() - rotationTensor.yz())/s;
+            v_[0] = 0.25*s;
+            v_[1] = (rotationTensor.xy() + rotationTensor.yx())/s;
+            v_[2] = (rotationTensor.xz() + rotationTensor.zx())/s;
+        }
+        else if
+        (
+            rotationTensor.yy() > rotationTensor.zz()
+        )
+        {
+            scalar s = 2.0*Foam::sqrt
+            (
+                1.0
+              + rotationTensor.yy()
+              - rotationTensor.xx()
+              - rotationTensor.zz()
+            );
+
+            w_ = (rotationTensor.xz() - rotationTensor.xz())/s;
+            v_[0] = (rotationTensor.xy() + rotationTensor.yx())/s;
+            v_[1] = 0.25*s;
+            v_[2] = (rotationTensor.yz() + rotationTensor.zy())/s;
+        }
+        else
+        {
+            scalar s = 2.0*Foam::sqrt
+            (
+                1.0
+              + rotationTensor.zz()
+              - rotationTensor.xx()
+              - rotationTensor.yy()
+            );
+
+            w_ = (rotationTensor.yx() - rotationTensor.xy())/s;
+            v_[0] = (rotationTensor.xz() + rotationTensor.zx())/s;
+            v_[1] = (rotationTensor.yz() + rotationTensor.zy())/s;
+            v_[2] = 0.25*s;
+        }
+    }
+}
+
 
 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //