From 1e2caab6d31dd917cb5eb9e851c7d89de64a25df Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Sat, 18 Apr 2009 18:29:54 +0000
Subject: [PATCH] the exponential map from a given point is moved from the
solver into the manifold classes
[[Imported from SVN: r4049]]
---
src/riemanniantrsolver.cc | 13 ++-----------
src/rigidbodymotion.hh | 20 +++++++++++++++++++-
src/rotation.hh | 8 +++++++-
3 files changed, 28 insertions(+), 13 deletions(-)
diff --git a/src/riemanniantrsolver.cc b/src/riemanniantrsolver.cc
index 31b3fefd..7b09fe79 100644
--- a/src/riemanniantrsolver.cc
+++ b/src/riemanniantrsolver.cc
@@ -304,17 +304,8 @@ void RiemannianTrustRegionSolver<GridType,TargetSpace>::solve()
// ////////////////////////////////////////////////////
SolutionType newIterate = x_;
- for (int j=0; j<newIterate.size(); j++) {
-
- // Add translational correction
- for (int k=0; k<3; k++)
- newIterate[j].r[k] += corr[j][k];
-
- // Add rotational correction
- Rotation<3,double> qCorr = Rotation<3,double>::exp(corr[j][3], corr[j][4], corr[j][5]);
- newIterate[j].q = newIterate[j].q.mult(qCorr);
-
- }
+ for (int j=0; j<newIterate.size(); j++)
+ newIterate[j] = TargetSpace::exp(newIterate[j], corr[j]);
/** \todo Don't always recompute oldEnergy */
double oldEnergy = assembler_->computeEnergy(x_);
diff --git a/src/rigidbodymotion.hh b/src/rigidbodymotion.hh
index 40e8ff6e..58e0adc8 100644
--- a/src/rigidbodymotion.hh
+++ b/src/rigidbodymotion.hh
@@ -9,7 +9,25 @@ template <int dim, class ctype=double>
struct RigidBodyMotion
{
/** \brief Type of an infinitesimal rigid body motion */
- typedef Dune::FieldVector<ctype, (dim==3) ? 6 : 3> TangentVector;
+ typedef Dune::FieldVector<ctype, dim + Rotation<dim,ctype>::TangentVector::size> TangentVector;
+
+ /** \brief The exponential map from a given point $p \in SE(d)$. */
+ static RigidBodyMotion<dim,ctype> exp(const RigidBodyMotion<dim,ctype>& p, const TangentVector& v) {
+
+ RigidBodyMotion<dim,ctype> result;
+
+ // Add translational correction
+ for (int i=0; i<dim; i++)
+ result.r[i] = p.r[i] + v[i];
+
+ // Add rotational correction
+ typename Rotation<dim,ctype>::TangentVector qCorr;
+ for (int i=0; i<Rotation<dim,ctype>::TangentVector::size; i++)
+ qCorr[i] = v[dim+i];
+
+ result.q = Rotation<dim,ctype>::exp(p.q, qCorr);
+ return result;
+ }
/** \brief Compute difference vector from a to b on the tangent space of a */
static TangentVector difference(const RigidBodyMotion<dim,ctype>& a,
diff --git a/src/rotation.hh b/src/rotation.hh
index c20900fa..98061554 100644
--- a/src/rotation.hh
+++ b/src/rotation.hh
@@ -89,7 +89,7 @@ public:
/** \brief The exponential map from \f$ \mathfrak{so}(3) \f$ to \f$ SO(3) \f$
*/
- static Quaternion<T> exp(const Dune::FieldVector<T,3>& v) {
+ static Rotation<3,T> exp(const Dune::FieldVector<T,3>& v) {
return exp(v[0], v[1], v[2]);
}
@@ -114,6 +114,12 @@ public:
return q;
}
+ /** \brief The exponential map from a given point $p \in SO(3)$. */
+ static Rotation<3,T> exp(const Rotation<3,T>& p, const TangentVector& v) {
+ Rotation<3,T> corr = exp(v);
+ return p.mult(corr);
+ }
+
static Dune::FieldMatrix<T,4,3> Dexp(const Dune::FieldVector<T,3>& v) {
Dune::FieldMatrix<T,4,3> result(0);
--
GitLab