quaternion: added constructor from angle cosine and slerp

src/OpenFOAM/primitives/quaternion/quaternion.C

@@ -51,6 +51,31 @@ Foam::word Foam::name(const quaternion& q)
 }
 
 
+Foam::quaternion Foam::slerp
+(
+    const quaternion& qa,
+    const quaternion& qb,
+    const scalar t
+)
+{
+    // Calculate angle between the quaternions
+    scalar cosHalfTheta = qa & qb;
+
+    if (mag(cosHalfTheta) >= 1)
+    {
+        return qa;
+    }
+
+    scalar halfTheta = acos(cosHalfTheta);
+    scalar sinHalfTheta = sqrt(1.0 - sqr(cosHalfTheta));
+
+    scalar wa = sin((1 - t)*halfTheta)/sinHalfTheta;
+    scalar wb = sin(t*halfTheta)/sinHalfTheta;
+
+    return wa*qa + wb*qb;
+}
+
+
 // * * * * * * * * * * * * * * * IOstream Operators  * * * * * * * * * * * * //
 
 Foam::Istream& Foam::operator>>(Istream& is, quaternion& q)

src/OpenFOAM/primitives/quaternion/quaternion.H

@@ -106,6 +106,15 @@ public:
         //  and angle theta
         inline quaternion(const vector& d, const scalar theta);
 
+        //- Construct a rotation quaternion given the direction d
+        //  and cosine angle cosTheta and a if d is normalized
+        inline quaternion
+        (
+            const vector& d,
+            const scalar cosTheta,
+            const bool normalized
+        );
+
         //- Construct given scalar part, the vector part = vector::zero
         inline explicit quaternion(const scalar w);
 
@@ -209,6 +218,14 @@ inline quaternion inv(const quaternion& q);
 //- Return a string representation of a quaternion
 word name(const quaternion&);
 
+//- Spherical linear interpolation of quaternions
+quaternion slerp
+(
+    const quaternion& qa,
+    const quaternion& qb,
+    const scalar t
+);
+
 //- Data associated with quaternion type are contiguous
 template<>
 inline bool contiguous<quaternion>() {return true;}

src/OpenFOAM/primitives/quaternion/quaternionI.H

@@ -40,6 +40,26 @@ inline Foam::quaternion::quaternion(const vector& d, const scalar theta)
     v_((sin(0.5*theta)/mag(d))*d)
 {}
 
+inline Foam::quaternion::quaternion
+(
+    const vector& d,
+    const scalar cosTheta,
+    const bool normalized
+)
+{
+    scalar cosHalfTheta2 = 0.5*(cosTheta + 1);
+    w_ = sqrt(cosHalfTheta2);
+
+    if (normalized)
+    {
+        v_ = sqrt(1 - cosHalfTheta2)*d;
+    }
+    else
+    {
+        v_ = (sqrt(1 - cosHalfTheta2)/mag(d))*d;
+    }
+}
+
 inline Foam::quaternion::quaternion(const scalar w)
 :
     w_(w),
@@ -69,7 +89,8 @@ inline Foam::quaternion::quaternion
     const tensor& rotationTensor
 )
 {
-    scalar trace = rotationTensor.xx()
+    scalar trace =
+        rotationTensor.xx()
       + rotationTensor.yy()
       + rotationTensor.zz();