From ce2129ac7e28729edddd19560cc091819bd2f078 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Wed, 10 Feb 2010 21:55:00 +0000
Subject: [PATCH] implement the exponential map
[[Imported from SVN: r5533]]
---
src/unitvector.hh | 9 +++++++++
1 file changed, 9 insertions(+)
diff --git a/src/unitvector.hh b/src/unitvector.hh
index 200dfd48..9826c247 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_);
--
GitLab