From fc32d538282ef637ac19c40dcb6013ee454abeba Mon Sep 17 00:00:00 2001
From: Henry Weller <http://cfd.direct>
Date: Sat, 16 Apr 2016 15:59:05 +0100
Subject: [PATCH] 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.
---
.../test/quaternion/Test-quaternion.C | 12 +++++++
.../primitives/quaternion/quaternion.C | 25 +++++++++++++
.../primitives/quaternion/quaternion.H | 7 ++++
.../primitives/septernion/septernion.C | 35 +++++++++++++++++--
.../primitives/septernion/septernion.H | 7 ++++
5 files changed, 83 insertions(+), 3 deletions(-)
diff --git a/applications/test/quaternion/Test-quaternion.C b/applications/test/quaternion/Test-quaternion.C
index 4d657fb818..c3acadf7cf 100644
--- a/applications/test/quaternion/Test-quaternion.C
+++ b/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;
diff --git a/src/OpenFOAM/primitives/quaternion/quaternion.C b/src/OpenFOAM/primitives/quaternion/quaternion.C
index e2bb676d8f..fa509113e0 100644
--- a/src/OpenFOAM/primitives/quaternion/quaternion.C
+++ b/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());
diff --git a/src/OpenFOAM/primitives/quaternion/quaternion.H b/src/OpenFOAM/primitives/quaternion/quaternion.H
index ab1c5379bb..a36b0eed35 100644
--- a/src/OpenFOAM/primitives/quaternion/quaternion.H
+++ b/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);
diff --git a/src/OpenFOAM/primitives/septernion/septernion.C b/src/OpenFOAM/primitives/septernion/septernion.C
index 0dd3718a45..34562bba06 100644
--- a/src/OpenFOAM/primitives/septernion/septernion.C
+++ b/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;
}
diff --git a/src/OpenFOAM/primitives/septernion/septernion.H b/src/OpenFOAM/primitives/septernion/septernion.H
index c6bccca2eb..eb34cb5a6e 100644
--- a/src/OpenFOAM/primitives/septernion/septernion.H
+++ b/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;}
--
GitLab