Add the missing features and syntactic sugar that were

needed to make the targetspacetest.cc test case pass.

[[Imported from SVN: r8041]]

dune/gfe/rigidbodymotion.hh

@@ -6,39 +6,52 @@
 #include <dune/gfe/realtuple.hh>
 #include "rotation.hh"
 
-/** \brief A rigid-body motion in R^d, i.e., a member of SE(d) */
-template <int dim, class T=double>
+/** \brief A rigid-body motion in R^N, i.e., a member of SE(N) */
+template <int N, class T=double>
 struct RigidBodyMotion
 {
-private:
+public:
     
     /** \brief Dimension of manifold */
-    static const int dimension = dim + Rotation<dim,T>::TangentVector::dimension;
+    static const int dim = N + Rotation<N,T>::dim;
     
     /** \brief Dimension of the embedding space */
-    static const int embeddedDimension = dim + Rotation<dim,T>::EmbeddedTangentVector::dimension;
-    
-public:
+    static const int embeddedDim = N + Rotation<N,T>::embeddedDim;
     
     /** \brief Type of an infinitesimal rigid body motion */
-    typedef Dune::FieldVector<T, dimension> TangentVector;
+    typedef Dune::FieldVector<T, dim> TangentVector;
 
     /** \brief Type of an infinitesimal rigid body motion */
-    typedef Dune::FieldVector<T, embeddedDimension> EmbeddedTangentVector;
+    typedef Dune::FieldVector<T, embeddedDim> EmbeddedTangentVector;
 
     /** \brief The type used for coordinates */
     typedef T ctype;
     
+    /** \brief The type used for global coordinates */
+    typedef Dune::FieldVector<T,embeddedDim> CoordinateType;
+
     /** \brief Default constructor */
     RigidBodyMotion()
     {}
     
     /** \brief Constructor from a translation and a rotation */
-    RigidBodyMotion(const Dune::FieldVector<ctype, dim>& translation,
-                    const Rotation<dim,ctype>& rotation)
+    RigidBodyMotion(const Dune::FieldVector<ctype, N>& translation,
+                    const Rotation<N,ctype>& rotation)
     : r(translation), q(rotation)
     {}
 
+    RigidBodyMotion(const CoordinateType& globalCoordinates)
+    {
+        for (int i=0; i<N; i++)
+            r[i] = globalCoordinates[i];
+        
+        for (int i=N; i<embeddedDim; i++)
+            q[i-N] = globalCoordinates[i];
+        
+        // Turn this into a unit quaternion if it isn't already
+        q.normalize();
+    }
+    
     /** \brief The exponential map from a given point $p \in SE(d)$. 
      
      Why the template parameter?  Well, it should work with both TangentVector and EmbeddedTangentVector.
@@ -46,87 +59,88 @@ public:
      and then the compiler complains about having two methods with the same signature.
      */
     template <class TVector>
-    static RigidBodyMotion<dim,ctype> exp(const RigidBodyMotion<dim,ctype>& p, const TVector& v) {
+    static RigidBodyMotion<N,ctype> exp(const RigidBodyMotion<N,ctype>& p, const TVector& v) {
 
-        RigidBodyMotion<dim,ctype> result;
+        RigidBodyMotion<N,ctype> result;
 
         // Add translational correction
-        for (int i=0; i<dim; i++)
+        for (int i=0; i<N; i++)
             result.r[i] = p.r[i] + v[i];
 
         // Add rotational correction
         typedef typename Dune::SelectType<Dune::is_same<TVector,TangentVector>::value,
-                                          typename Rotation<dim,ctype>::TangentVector,
-                                          typename Rotation<dim,ctype>::EmbeddedTangentVector>::Type RotationTangentVector;
+                                          typename Rotation<N,ctype>::TangentVector,
+                                          typename Rotation<N,ctype>::EmbeddedTangentVector>::Type RotationTangentVector;
         RotationTangentVector qCorr;
         for (int i=0; i<RotationTangentVector::dimension; i++)
-            qCorr[i] = v[dim+i];
+            qCorr[i] = v[N+i];
 
-        result.q = Rotation<dim,ctype>::exp(p.q, qCorr);
+        result.q = Rotation<N,ctype>::exp(p.q, qCorr);
         return result;
     }
 
     /** \brief Compute geodesic distance from a to b */
-    static T distance(const RigidBodyMotion<dim,ctype>& a, const RigidBodyMotion<dim,ctype>& b) {
+    static T distance(const RigidBodyMotion<N,ctype>& a, const RigidBodyMotion<N,ctype>& b) {
         
         T euclideanDistanceSquared = (a.r - b.r).two_norm2();
         
-        T rotationDistance = Rotation<dim,ctype>::distance(a.q, b.q);
+        T rotationDistance = Rotation<N,ctype>::distance(a.q, b.q);
         
         return std::sqrt(euclideanDistanceSquared + rotationDistance*rotationDistance);
     }
     
     /** \brief Compute difference vector from a to b on the tangent space of a */
-    static TangentVector difference(const RigidBodyMotion<dim,ctype>& a,
-                                    const RigidBodyMotion<dim,ctype>& b) {
+    static TangentVector difference(const RigidBodyMotion<N,ctype>& a,
+                                    const RigidBodyMotion<N,ctype>& b) {
 
         TangentVector result;
 
         // Usual linear difference
-        for (int i=0; i<dim; i++)
+        for (int i=0; i<N; i++)
             result[i] = a.r[i] - b.r[i];
 
         // Subtract orientations on the tangent space of 'a'
-        typename Rotation<dim,ctype>::TangentVector v = Rotation<dim,ctype>::difference(a.q, b.q).axial();
+        typename Rotation<N,ctype>::TangentVector v = Rotation<N,ctype>::difference(a.q, b.q).axial();
 
         // Compute difference on T_a SO(3)
-        for (int i=0; i<Rotation<dim,ctype>::TangentVector::dimension; i++)
-            result[i+dim] = v[i];
+        for (int i=0; i<Rotation<N,ctype>::TangentVector::dimension; i++)
+            result[i+N] = v[i];
 
         return result;
     }
     
-    static EmbeddedTangentVector derivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype>& a,
-                                                                              const RigidBodyMotion<dim,ctype>& b) {
+    static EmbeddedTangentVector derivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<N,ctype>& a,
+                                                                              const RigidBodyMotion<N,ctype>& b) {
 
         // linear part
-        Dune::FieldVector<ctype,dim> linearDerivative = a.r;
+        Dune::FieldVector<ctype,N> linearDerivative = a.r;
         linearDerivative -= b.r;
         linearDerivative *= -2;
 
         // rotation part
-        typename Rotation<dim,ctype>::EmbeddedTangentVector rotationDerivative 
-                = Rotation<dim,ctype>::derivativeOfDistanceSquaredWRTSecondArgument(a.q, b.q);
+        typename Rotation<N,ctype>::EmbeddedTangentVector rotationDerivative 
+                = Rotation<N,ctype>::derivativeOfDistanceSquaredWRTSecondArgument(a.q, b.q);
         
         return concat(linearDerivative, rotationDerivative);
     }
     
     /** \brief Compute the Hessian of the squared distance function keeping the first argument fixed */
-    static Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> secondDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+    static Dune::FieldMatrix<double,embeddedDim,embeddedDim> secondDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<N,ctype> & p, const RigidBodyMotion<N,ctype> & q)
     {
-        Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> result(0);
+        Dune::FieldMatrix<double,embeddedDim,embeddedDim> result(0);
         
         // The linear part
-        Dune::FieldMatrix<double,dim,dim> linearPart = RealTuple<dim>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
-        for (int i=0; i<dim; i++)
-            for (int j=0; j<dim; j++)
+        Dune::FieldMatrix<double,N,N> linearPart = RealTuple<N>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
+        for (int i=0; i<N; i++)
+            for (int j=0; j<N; j++)
                 result[i][j] = linearPart[i][j];
 
         // The rotation part
-        Dune::FieldMatrix<double,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
-        for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::dimension; i++)
-            for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::dimension; j++)
-                result[dim+i][dim+j] = rotationPart[i][j];
+        Dune::FieldMatrix<double,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim> rotationPart 
+                = Rotation<N,ctype>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
+        for (int i=0; i<Rotation<N,T>::embeddedDim; i++)
+            for (int j=0; j<Rotation<N,T>::embeddedDim; j++)
+                result[N+i][N+j] = rotationPart[i][j];
 
         return result;
     }
@@ -135,21 +149,22 @@ public:
 
     Unlike the distance itself the squared distance is differentiable at zero
      */
-    static Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+    static Dune::FieldMatrix<double,embeddedDim,embeddedDim> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RigidBodyMotion<N,ctype> & p, const RigidBodyMotion<N,ctype> & q)
     {
-        Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> result(0);
+        Dune::FieldMatrix<double,embeddedDim,embeddedDim> result(0);
         
         // The linear part
-        Dune::FieldMatrix<double,dim,dim> linearPart = RealTuple<dim>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.r,q.r);
-        for (int i=0; i<dim; i++)
-            for (int j=0; j<dim; j++)
+        Dune::FieldMatrix<double,N,N> linearPart = RealTuple<N>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.r,q.r);
+        for (int i=0; i<N; i++)
+            for (int j=0; j<N; j++)
                 result[i][j] = linearPart[i][j];
 
         // The rotation part
-        Dune::FieldMatrix<double,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.q,q.q);
-        for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::dimension; i++)
-            for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::dimension; j++)
-                result[dim+i][dim+j] = rotationPart[i][j];
+        Dune::FieldMatrix<double,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim> rotationPart 
+                = Rotation<N,ctype>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.q,q.q);
+        for (int i=0; i<Rotation<N,T>::embeddedDim; i++)
+            for (int j=0; j<Rotation<N,T>::embeddedDim; j++)
+                result[N+i][N+j] = rotationPart[i][j];
 
         return result;
     }
@@ -158,23 +173,25 @@ public:
 
     Unlike the distance itself the squared distance is differentiable at zero
      */
-    static Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+    static Tensor3<double,embeddedDim,embeddedDim,embeddedDim> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<N,ctype> & p, const RigidBodyMotion<N,ctype> & q)
     {
-        Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> result(0);
+        Tensor3<double,embeddedDim,embeddedDim,embeddedDim> result(0);
         
         // The linear part
-        Tensor3<double,dim,dim,dim> linearPart = RealTuple<dim>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
-        for (int i=0; i<dim; i++)
-            for (int j=0; j<dim; j++)
-                for (int k=0; k<dim; k++)
+        Tensor3<double,N,N,N> linearPart = RealTuple<N>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
+        for (int i=0; i<N; i++)
+            for (int j=0; j<N; j++)
+                for (int k=0; k<N; k++)
                     result[i][j][k] = linearPart[i][j][k];
 
         // The rotation part
-        Tensor3<double,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
-        for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::dimension; i++)
-            for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::dimension; j++)
-                for (int k=0; k<Rotation<dim,T>::EmbeddedTangentVector::dimension; k++)
-                    result[dim+i][dim+j][dim+k] = rotationPart[i][j][k];
+        Tensor3<double,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim> rotationPart 
+                = Rotation<N,ctype>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
+                
+        for (int i=0; i<Rotation<N,T>::embeddedDim; i++)
+            for (int j=0; j<Rotation<N,T>::embeddedDim; j++)
+                for (int k=0; k<Rotation<N,T>::embeddedDim; k++)
+                    result[N+i][N+j][N+k] = rotationPart[i][j][k];
 
         return result;
     }
@@ -183,23 +200,23 @@ public:
 
     Unlike the distance itself the squared distance is differentiable at zero
      */
-    static Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+    static Tensor3<double,embeddedDim,embeddedDim,embeddedDim> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RigidBodyMotion<N,ctype> & p, const RigidBodyMotion<N,ctype> & q)
     {
-        Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> result(0);
+        Tensor3<double,embeddedDim,embeddedDim,embeddedDim> result(0);
         
         // The linear part
-        Tensor3<double,dim,dim,dim> linearPart = RealTuple<dim>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.r,q.r);
-        for (int i=0; i<dim; i++)
-            for (int j=0; j<dim; j++)
-                for (int k=0; k<dim; k++)
+        Tensor3<double,N,N,N> linearPart = RealTuple<N>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.r,q.r);
+        for (int i=0; i<N; i++)
+            for (int j=0; j<N; j++)
+                for (int k=0; k<N; k++)
                     result[i][j][k] = linearPart[i][j][k];
 
         // The rotation part
-        Tensor3<double,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension,Rotation<dim,T>::EmbeddedTangentVector::dimension> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.q,q.q);
-        for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::dimension; i++)
-            for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::dimension; j++)
-                for (int k=0; k<Rotation<dim,T>::EmbeddedTangentVector::dimension; k++)
-                    result[dim+i][dim+j][dim+k] = rotationPart[i][j][k];
+        Tensor3<double,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim,Rotation<N,T>::embeddedDim> rotationPart = Rotation<N,ctype>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.q,q.q);
+        for (int i=0; i<Rotation<N,T>::embeddedDim; i++)
+            for (int j=0; j<Rotation<N,T>::embeddedDim; j++)
+                for (int k=0; k<Rotation<N,T>::embeddedDim; k++)
+                    result[N+i][N+j][N+k] = rotationPart[i][j][k];
 
         return result;
     }
@@ -216,50 +233,55 @@ public:
 
     This basis may not be globally continuous.
     */
-    Dune::FieldMatrix<double,dimension,embeddedDimension> orthonormalFrame() const {
-        Dune::FieldMatrix<double,dimension,embeddedDimension> result(0);
+    Dune::FieldMatrix<double,dim,embeddedDim> orthonormalFrame() const {
+        Dune::FieldMatrix<double,dim,embeddedDim> result(0);
 
         // Get the R^d part
-        for (int i=0; i<dim; i++)
+        for (int i=0; i<N; i++)
             result[i][i] = 1;
         
-        Dune::FieldMatrix<double,Rotation<dim>::TangentVector::dimension,Rotation<dim>::EmbeddedTangentVector::dimension> SO3Part = q.orthonormalFrame();
+        Dune::FieldMatrix<double,Rotation<N>::dim,Rotation<N>::embeddedDim> SO3Part = q.orthonormalFrame();
 
-        for (int i=0; i<Rotation<dim>::TangentVector::dimension; i++)
-            for (int j=0; j<Rotation<dim>::EmbeddedTangentVector::dimension; j++)
-                result[dim+i][dim+j] = SO3Part[i][j];
+        for (int i=0; i<Rotation<N>::dim; i++)
+            for (int j=0; j<Rotation<N>::embeddedDim; j++)
+                result[N+i][N+j] = SO3Part[i][j];
 
         return result;
     }
     
+    /** \brief The global coordinates, if you really want them */
+    CoordinateType globalCoordinates() const {
+        return concat(r, q.globalCoordinates());
+    }
+
 
 
     // Translational part
-    Dune::FieldVector<ctype, dim> r;
+    Dune::FieldVector<ctype, N> r;
 
     // Rotational part
-    Rotation<dim,ctype> q;
+    Rotation<N,ctype> q;
     
 private:
     
     /** \brief Concatenate two FieldVectors */
-    template <int N, int M>
-    static Dune::FieldVector<ctype,N+M> concat(const Dune::FieldVector<ctype,N>& a,
+    template <int NN, int M>
+    static Dune::FieldVector<ctype,NN+M> concat(const Dune::FieldVector<ctype,NN>& a,
                                                const Dune::FieldVector<ctype,M>& b)
     {
-        Dune::FieldVector<ctype,N+M> result;
-        for (int i=0; i<N; i++)
+        Dune::FieldVector<ctype,NN+M> result;
+        for (int i=0; i<NN; i++)
             result[i] = a[i];
         for (int i=0; i<M; i++)
-            result[i+N] = b[i];
+            result[i+NN] = b[i];
         return result;
     }
 
 };
 
 //! Send configuration to output stream
-template <int dim, class ctype>
-std::ostream& operator<< (std::ostream& s, const RigidBodyMotion<dim,ctype>& c)
+template <int N, class ctype>
+std::ostream& operator<< (std::ostream& s, const RigidBodyMotion<N,ctype>& c)
   {
       s << "(" << c.r << ")  (" << c.q << ")";
       return s;