diff --git a/src/quaternion.hh b/src/quaternion.hh index 879fd520ccffad4893d34495f9df19e0e0f1dfec..45f9af6900a6204e6929e879d0a052516357ef92 100644 --- a/src/quaternion.hh +++ b/src/quaternion.hh @@ -9,9 +9,22 @@ class Quaternion : public Dune::FieldVector<T,4> { public: + /** \brief Default constructor */ Quaternion() {} + + /** \brief Copy constructor */ Quaternion(const Dune::FieldVector<T,4>& other) : Dune::FieldVector<T,4>(other) {} + /** \brief Constructor with rotation axis and angle */ + Quaternion(Dune::FieldVector<T,3> axis, T angle) { + axis /= axis.two_norm(); + axis *= std::sin(angle/2); + (*this)[0] = axis[0]; + (*this)[1] = axis[1]; + (*this)[2] = axis[2]; + (*this)[3] = std::cos(angle/2); + } + /** \brief Return the identity element */ static Quaternion<T> identity() { Quaternion<T> id; @@ -85,6 +98,32 @@ public: (*this) /= this->two_norm(); } + Dune::FieldVector<double,3> rotate(const Dune::FieldVector<double,3>& v) const { + + Dune::FieldVector<double,3> result; + Dune::FieldVector<double,3> d0 = director(0); + Dune::FieldVector<double,3> d1 = director(1); + Dune::FieldVector<double,3> d2 = director(2); + + for (int i=0; i<3; i++) + result[i] = v[0]*d0[i] + v[1]*d1[i] + v[2]*d2[i]; + + return result; + } + + /** \brief Interpolate between two rotations */ + static Quaternion<T> interpolate(const Quaternion<T>& a, const Quaternion<T>& b, double omega) { + + Quaternion<T> result; + + for (int i=0; i<4; i++) + result[i] = a[i]*(1-omega) + b[i]*omega; + + result.normalize(); + + return result; + } + }; #endif