diff --git a/src/unitvector.hh b/src/unitvector.hh index 200dfd48950a05d72b21b2b85d1881d294d9db3a..9826c247e8c4bd8efd7cde571f467ce163a94bc5 100644 --- a/src/unitvector.hh +++ b/src/unitvector.hh @@ -18,6 +18,15 @@ public: return *this; } + /** \brief The exponention map */ + static UnitVector exp(const UnitVector& p, const TangentVector& v) { + const double norm = v.two_norm(); + UnitVector result = p; + result.data_ *= std::cos(norm); + result.data_.axpy(std::sin(norm)/norm, v); + return result; + } + /** \brief Length of the great arc connecting the two points */ static double distance(const UnitVector& a, const UnitVector& b) { return std::acos(a.data_ * b.data_);