diff --git a/applications/test/quaternion/Test-quaternion.C b/applications/test/quaternion/Test-quaternion.C index 4d657fb818c03129d1c55fb72eed05a38dc34671..c3acadf7cf4100d1922fd5356565a934e061ac24 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 e2bb676d8f2f7444ae2e5b4f7052b1e4239e4751..fa509113e0ae7dbc0d1fc2b9ebf6dbfbe6057975 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 ab1c5379bb02dbe23677823f49f78ae4c06ada55..a36b0eed350f0a1ca03344adf8391508c7111d81 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 0dd3718a45bec352064783de11b106fbef74526e..34562bba06b7b516a15e6e3ac43eec25bc10d30a 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 c6bccca2eb55d76ea13bb19b03e2b9cb6cf5cfda..eb34cb5a6e766fa15a0c5d095c353041082e1fcc 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;}