quaternion/septernion: Added multi- quaternion/septernion averaging

Using method based on
http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf
but simplified for the case where the quaternions are similar.

src/OpenFOAM/primitives/quaternion/quaternion.C

@@ -69,6 +69,31 @@ Foam::quaternion Foam::slerp
 }
 
 
+Foam::quaternion Foam::average
+(
+    const UList<quaternion>& qs,
+    const UList<scalar> w
+)
+{
+    quaternion qa(w[0]*qs[0]);
+
+    for (label i=1; i<qs.size(); i++)
+    {
+        // Invert quaternion if it has the opposite sign to the average
+        if ((qa & qs[i]) > 0)
+        {
+            qa += w[i]*qs[i];
+        }
+        else
+        {
+            qa -= w[i]*qs[i];
+        }
+    }
+
+    return qa;
+}
+
+
 Foam::quaternion Foam::exp(const quaternion& q)
 {
     const scalar magV = mag(q.v());

src/OpenFOAM/primitives/quaternion/quaternion.H

@@ -259,6 +259,13 @@ quaternion slerp
     const scalar t
 );
 
+//- Simple weighted average with sign change
+quaternion average
+(
+    const UList<quaternion>& qs,
+    const UList<scalar> w
+);
+
 //- Exponent of a quaternion
 quaternion exp(const quaternion& q);
 

src/OpenFOAM/primitives/septernion/septernion.C

@@ -61,12 +61,41 @@ Foam::word Foam::name(const septernion& s)
 
 Foam::septernion Foam::slerp
 (
-    const septernion& qa,
-    const septernion& qb,
+    const septernion& sa,
+    const septernion& sb,
     const scalar t
 )
 {
-    return septernion((1 - t)*qa.t() + t*qb.t(), slerp(qa.r(), qb.r(), t));
+    return septernion((1 - t)*sa.t() + t*sb.t(), slerp(sa.r(), sb.r(), t));
+}
+
+
+Foam::septernion Foam::average
+(
+    const UList<septernion>& ss,
+    const UList<scalar> w
+)
+{
+    septernion sa(w[0]*ss[0]);
+
+    for (label i=1; i<ss.size(); i++)
+    {
+        sa.t() += w[i]*ss[i].t();
+
+        // Invert quaternion if it has the opposite sign to the average
+        if ((sa.r() & ss[i].r()) > 0)
+        {
+            sa.r() += w[i]*ss[i].r();
+        }
+        else
+        {
+            sa.r() -= w[i]*ss[i].r();
+        }
+    }
+
+    normalize(sa.r());
+
+    return sa;
 }
 
 

src/OpenFOAM/primitives/septernion/septernion.H

@@ -168,6 +168,13 @@ septernion slerp
     const scalar t
 );
 
+//- Simple weighted average
+septernion average
+(
+    const UList<septernion>& ss,
+    const UList<scalar> w
+);
+
 //- Data associated with septernion type are contiguous
 template<>
 inline bool contiguous<septernion>() {return true;}