Commit fc32d538 authored by Henry Weller's avatar Henry Weller
Browse files

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.
parent d02e0aa0
......@@ -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;
......
......@@ -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());
......
......@@ -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);
......
......@@ -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;
}
......
......@@ -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;}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment