diff --git a/applications/test/quaternion/Test-quaternion.C b/applications/test/quaternion/Test-quaternion.C index 4d657fb81..c3acadf7c 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 ss(3); + List 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 e2bb676d8..fa509113e 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& qs, + const UList w +) +{ + quaternion qa(w[0]*qs[0]); + + for (label i=1; 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 ab1c5379b..a36b0eed3 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& qs, + const UList 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 0dd3718a4..34562bba0 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& ss, + const UList w +) +{ + septernion sa(w[0]*ss[0]); + + for (label i=1; i 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 c6bccca2e..eb34cb5a6 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& ss, + const UList w +); + //- Data associated with septernion type are contiguous template<> inline bool contiguous() {return true;}