From 46fe369dd6589c787c762467776ea4f299cac017 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Tue, 3 Sep 2013 16:30:49 +0000
Subject: [PATCH] Implement projectOntoNormalSpace and the Weingarten map

[[Imported from SVN: r9449]]
---
 dune/gfe/rigidbodymotion.hh | 57 ++++++++++++++++++++++++++++++++++++-
 dune/gfe/unitvector.hh      | 30 +++++++++++++++++++
 2 files changed, 86 insertions(+), 1 deletion(-)

diff --git a/dune/gfe/rigidbodymotion.hh b/dune/gfe/rigidbodymotion.hh
index 052bfed2..9a5820f8 100644
--- a/dune/gfe/rigidbodymotion.hh
+++ b/dune/gfe/rigidbodymotion.hh
@@ -245,9 +245,64 @@ public:
 
     /** \brief Project tangent vector of R^n onto the tangent space */
     EmbeddedTangentVector projectOntoTangentSpace(const EmbeddedTangentVector& v) const {
-        DUNE_THROW(Dune::NotImplemented, "!");
+        EmbeddedTangentVector result;
+
+        // translation part
+        for (int i=0; i<N; i++)
+          result[i] = v[i];
+
+        // rotation part
+        typename Rotation<T,N>::EmbeddedTangentVector rotV;
+        for (int i=0; i<Rotation<T,N>::embeddedDim; i++)
+            rotV[i] = v[i+N];
+
+        rotV = q.projectOntoTangentSpace(rotV);
+
+        for (int i=0; i<Rotation<T,N>::embeddedDim; i++)
+          result[i+N] = rotV[i];
+
+        return result;
     }
 
+    /** \brief Project tangent vector of R^n onto the normal space space */
+    EmbeddedTangentVector projectOntoNormalSpace(const EmbeddedTangentVector& v) const {
+
+        EmbeddedTangentVector result;
+
+        // translation part
+        for (int i=0; i<N; i++)
+          result[i] = v[i];
+
+        // rotation part
+        T sp = 0;
+        for (int i=0; i<Rotation<T,N>::embeddedDim; i++)
+          sp += v[i+N] * q[i];
+
+        for (int i=0; i<Rotation<T,N>::embeddedDim; i++)
+          result[i+N] = sp * q[i];
+
+        return result;
+    }
+
+    /** \brief The Weingarten map */
+    EmbeddedTangentVector weingarten(const EmbeddedTangentVector& z, const EmbeddedTangentVector& v) const {
+
+        EmbeddedTangentVector result;
+
+        // translation part: nothing, the space is flat
+        for (int i=0; i<N; i++)
+          result[i] = 0;
+
+        // rotation part
+        T sp = 0;
+        for (int i=0; i<Rotation<T,N>::embeddedDim; i++)
+          sp += v[i+N] * q[i];
+
+        for (int i=0; i<Rotation<T,N>::embeddedDim; i++)
+          result[i+N] = -sp * z[i+N];
+
+        return result;
+    }
 
     /** \brief Compute an orthonormal basis of the tangent space of SE(3).
 
diff --git a/dune/gfe/unitvector.hh b/dune/gfe/unitvector.hh
index e20e343c..a64902d1 100644
--- a/dune/gfe/unitvector.hh
+++ b/dune/gfe/unitvector.hh
@@ -335,6 +335,36 @@ public:
         return result;
     }
 
+    /** \brief Project tangent vector of R^n onto the normal space space */
+    EmbeddedTangentVector projectOntoNormalSpace(const EmbeddedTangentVector& v) const {
+
+        EmbeddedTangentVector result;
+
+        T sp = 0;
+        for (int i=0; i<N; i++)
+          sp += v[i] * data_[i];
+
+        for (int i=0; i<N; i++)
+          result[i] = sp * data_[i];
+
+        return result;
+    }
+
+    /** \brief The Weingarten map */
+    EmbeddedTangentVector weingarten(const EmbeddedTangentVector& z, const EmbeddedTangentVector& v) const {
+
+        EmbeddedTangentVector result;
+
+        T sp = 0;
+        for (int i=0; i<N; i++)
+          sp += v[i] * data_[i];
+
+        for (int i=0; i<N; i++)
+          result[i] = -sp * z[i];
+
+        return result;
+    }
+
     /** \brief The global coordinates, if you really want them */
     const CoordinateType& globalCoordinates() const {
         return data_;
-- 
GitLab