From 867bb84c31494b25bebed6c10c28a3a9946a8cca Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Thu, 11 Aug 2011 12:48:20 +0000
Subject: [PATCH] fix (hopefully) all places where dim==3 was hardwired

[[Imported from SVN: r7573]]
---
 dune/gfe/rigidbodymotion.hh | 108 ++++++++++++++++++++----------------
 1 file changed, 59 insertions(+), 49 deletions(-)

diff --git a/dune/gfe/rigidbodymotion.hh b/dune/gfe/rigidbodymotion.hh
index a5e0655d..396db128 100644
--- a/dune/gfe/rigidbodymotion.hh
+++ b/dune/gfe/rigidbodymotion.hh
@@ -10,11 +10,21 @@
 template <int dim, class T=double>
 struct RigidBodyMotion
 {
+private:
+    
+    /** \brief Dimension of manifold */
+    static const int dimension = dim + Rotation<dim,T>::TangentVector::size;
+    
+    /** \brief Dimension of the embedding space */
+    static const int embeddedDimension = dim + Rotation<dim,T>::EmbeddedTangentVector::size;
+    
+public:
+    
     /** \brief Type of an infinitesimal rigid body motion */
-    typedef Dune::FieldVector<T, dim + Rotation<dim,T>::TangentVector::size> TangentVector;
+    typedef Dune::FieldVector<T, dimension> TangentVector;
 
     /** \brief Type of an infinitesimal rigid body motion */
-    typedef Dune::FieldVector<T, dim + Rotation<dim,T>::EmbeddedTangentVector::size> EmbeddedTangentVector;
+    typedef Dune::FieldVector<T, embeddedDimension> EmbeddedTangentVector;
 
     /** \brief The type used for coordinates */
     typedef T ctype;
@@ -102,21 +112,21 @@ struct RigidBodyMotion
     }
     
     /** \brief Compute the Hessian of the squared distance function keeping the first argument fixed */
-    static Dune::FieldMatrix<double,7,7> secondDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+    static Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> secondDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
     {
-        Dune::FieldMatrix<double,7,7> result(0);
+        Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> result(0);
         
         // The linear part
-        Dune::FieldMatrix<double,3,3> linearPart = RealTuple<3>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
-        for (int i=0; i<3; i++)
-            for (int j=0; j<3; j++)
+        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++)
                 result[i][j] = linearPart[i][j];
 
         // The rotation part
-        Dune::FieldMatrix<double,4,4> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
-        for (int i=0; i<4; i++)
-            for (int j=0; j<4; j++)
-                result[3+i][3+j] = rotationPart[i][j];
+        Dune::FieldMatrix<double,Rotation<dim,T>::EmbeddedTangentVector::size,Rotation<dim,T>::EmbeddedTangentVector::size> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
+        for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::size; i++)
+            for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::size; j++)
+                result[dim+i][dim+j] = rotationPart[i][j];
 
         return result;
     }
@@ -125,21 +135,21 @@ struct RigidBodyMotion
 
     Unlike the distance itself the squared distance is differentiable at zero
      */
-    static Dune::FieldMatrix<double,7,7> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+    static Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
     {
-        Dune::FieldMatrix<double,7,7> result(0);
+        Dune::FieldMatrix<double,embeddedDimension,embeddedDimension> result(0);
         
         // The linear part
-        Dune::FieldMatrix<double,3,3> linearPart = RealTuple<3>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.r,q.r);
-        for (int i=0; i<3; i++)
-            for (int j=0; j<3; j++)
+        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++)
                 result[i][j] = linearPart[i][j];
 
         // The rotation part
-        Dune::FieldMatrix<double,4,4> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.q,q.q);
-        for (int i=0; i<4; i++)
-            for (int j=0; j<4; j++)
-                result[3+i][3+j] = rotationPart[i][j];
+        Dune::FieldMatrix<double,Rotation<dim,T>::EmbeddedTangentVector::size,Rotation<dim,T>::EmbeddedTangentVector::size> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.q,q.q);
+        for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::size; i++)
+            for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::size; j++)
+                result[dim+i][dim+j] = rotationPart[i][j];
 
         return result;
     }
@@ -148,23 +158,23 @@ struct RigidBodyMotion
 
     Unlike the distance itself the squared distance is differentiable at zero
      */
-    static Tensor3<double,7,7,7> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+    static Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
     {
-        Tensor3<double,7,7,7> result(0);
+        Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> result(0);
         
         // The linear part
-        Tensor3<double,3,3,3> linearPart = RealTuple<3>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
-        for (int i=0; i<3; i++)
-            for (int j=0; j<3; j++)
-                for (int k=0; k<3; k++)
+        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++)
                     result[i][j][k] = linearPart[i][j][k];
 
         // The rotation part
-        Tensor3<double,4,4,4> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
-        for (int i=0; i<4; i++)
-            for (int j=0; j<4; j++)
-                for (int k=0; k<4; k++)
-                    result[3+i][3+j][3+j] = rotationPart[i][j][k];
+        Tensor3<double,Rotation<dim,T>::EmbeddedTangentVector::size,Rotation<dim,T>::EmbeddedTangentVector::size,Rotation<dim,T>::EmbeddedTangentVector::size> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
+        for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::size; i++)
+            for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::size; j++)
+                for (int k=0; k<Rotation<dim,T>::EmbeddedTangentVector::size; k++)
+                    result[dim+i][dim+j][dim+j] = rotationPart[i][j][k];
 
         return result;
     }
@@ -173,23 +183,23 @@ struct RigidBodyMotion
 
     Unlike the distance itself the squared distance is differentiable at zero
      */
-    static Tensor3<double,7,7,7> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+    static Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
     {
-        Tensor3<double,7,7,7> result(0);
+        Tensor3<double,embeddedDimension,embeddedDimension,embeddedDimension> result(0);
         
         // The linear part
-        Tensor3<double,3,3,3> linearPart = RealTuple<3>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.r,q.r);
-        for (int i=0; i<3; i++)
-            for (int j=0; j<3; j++)
-                for (int k=0; k<3; k++)
+        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++)
                     result[i][j][k] = linearPart[i][j][k];
 
         // The rotation part
-        Tensor3<double,4,4,4> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.q,q.q);
-        for (int i=0; i<4; i++)
-            for (int j=0; j<4; j++)
-                for (int k=0; k<4; k++)
-                    result[3+i][3+j][3+j] = rotationPart[i][j][k];
+        Tensor3<double,Rotation<dim,T>::EmbeddedTangentVector::size,Rotation<dim,T>::EmbeddedTangentVector::size,Rotation<dim,T>::EmbeddedTangentVector::size> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.q,q.q);
+        for (int i=0; i<Rotation<dim,T>::EmbeddedTangentVector::size; i++)
+            for (int j=0; j<Rotation<dim,T>::EmbeddedTangentVector::size; j++)
+                for (int k=0; k<Rotation<dim,T>::EmbeddedTangentVector::size; k++)
+                    result[dim+i][dim+j][dim+j] = rotationPart[i][j][k];
 
         return result;
     }
@@ -204,20 +214,20 @@ struct RigidBodyMotion
     
     /** \brief Compute an orthonormal basis of the tangent space of SE(3).
 
-    This basis is of course not globally continuous.
+    This basis may not be globally continuous.
     */
-    Dune::FieldMatrix<double,6,7> orthonormalFrame() const {
-        Dune::FieldMatrix<double,6,7> result(0);
+    Dune::FieldMatrix<double,dimension,embeddedDimension> orthonormalFrame() const {
+        Dune::FieldMatrix<double,dimension,embeddedDimension> result(0);
 
         // Get the R^d part
         for (int i=0; i<dim; i++)
             result[i][i] = 1;
         
-        Dune::FieldMatrix<double,3,4> SO3Part = q.orthonormalFrame();
+        Dune::FieldMatrix<double,Rotation<dim>::TangentVector::size,Rotation<dim>::EmbeddedTangentVector::size> SO3Part = q.orthonormalFrame();
 
-        for (int i=0; i<dim; i++)
-            for (int j=0; j<4; j++)
-                result[3+i][3+j] = SO3Part[i][j];
+        for (int i=0; i<Rotation<dim>::TangentVector::size; i++)
+            for (int j=0; j<Rotation<dim>::EmbeddedTangentVector::size; j++)
+                result[dim+i][dim+j] = SO3Part[i][j];
 
         return result;
     }
-- 
GitLab