diff --git a/src/quaternion.hh b/src/quaternion.hh index c2e233c338ff918037cc996743c7132dfe30d7e0..a09525b711409b88e849d1beccbdac2594832034 100644 --- a/src/quaternion.hh +++ b/src/quaternion.hh @@ -338,6 +338,8 @@ public: /** \brief Interpolate between two rotations \param omega must be between 0 and 1 + \todo I'd say this method is incorrect and is other one is correct. + The solver works much better with this one, though. I don't get it. */ static Quaternion<T> interpolateDerivative(const Quaternion<T>& a, const Quaternion<T>& b, double omega) { @@ -361,6 +363,33 @@ public: return a.mult(result); } + /** \brief Interpolate between two rotations + \param omega must be between 0 and 1 + */ + static Quaternion<T> interpolateDerivative(const Quaternion<T>& a, const Quaternion<T>& b, + double omega, double intervalLength) { + Quaternion<T> result(0); + + // Compute difference on T_a SO(3) + Dune::FieldVector<double,3> xi = difference(a,b); + + xi /= intervalLength; + + Dune::FieldVector<double,3> v = xi; + v *= omega; + + // ////////////////////////////////////////////////////////////// + // v now contains the derivative at 'a'. The derivative at + // the requested site is v pushed forward by Dexp. + // ///////////////////////////////////////////////////////////// + + Dune::FieldMatrix<double,4,3> diffExp = Quaternion<double>::Dexp(v); + + diffExp.umv(xi,result); + + return a.mult(result); + } + /** \brief Return the corresponding orthogonal matrix */ void matrix(Dune::FieldMatrix<T,3,3>& m) const {