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.

applications/test/quaternion/Test-quaternion.C

@@ -97,6 +97,18 @@ int main(int argc, char *argv[])
         }
     }
 
+    List<septernion> ss(3);
+    List<scalar> w(3);
+
+    ss[0] = septernion(vector(0, 0.1, 0), quaternion(0.7, vector(1, 2, 3)));
+    w[0] = 0.1;
+    ss[1] = septernion(vector(0, 0.2, 0), quaternion(-0.6, vector(-2, -1, -3)));
+    w[1] = 0.5;
+    ss[2] = septernion(vector(0, 0.3, 0), quaternion(0.3, vector(3, 2, 1)));
+    w[2] = 0.4;
+
+    Info<< "average(ss, w) " << average(ss, w) << endl;
+
     Info<< "End\n" << endl;
 
     return 0;

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