Make rodobstacle use the RigidBodyMotion class

[[Imported from SVN: r5493]]

rodobstacle.cc

@@ -20,7 +20,6 @@
 #include "src/planarrodassembler.hh"
 
 
-// Number of degrees of freedom: 
 // 3 (x, y, theta) for a planar rod
 const int blocksize = 3;
 
@@ -70,7 +69,8 @@ int main (int argc, char *argv[]) try
 {
     // Some types that I need
     typedef BCRSMatrix<FieldMatrix<double, blocksize, blocksize> > MatrixType;
-    typedef BlockVector<FieldVector<double, blocksize> >     VectorType;
+    typedef BlockVector<FieldVector<double, blocksize> >           CorrectionType;
+    typedef std::vector<RigidBodyMotion<2> >                       SolutionType;
 
     // parse data file
     ConfigParser parameterSet;
@@ -106,21 +106,21 @@ int main (int argc, char *argv[]) try
     // ////////////////////////////////
 
     // First create a gauss-seidel base solver
-    ProjectedBlockGSStep<MatrixType, VectorType> baseSolverStep;
+    ProjectedBlockGSStep<MatrixType, CorrectionType> baseSolverStep;
 
-    EnergyNorm<MatrixType, VectorType> baseEnergyNorm(baseSolverStep);
+    EnergyNorm<MatrixType, CorrectionType> baseEnergyNorm(baseSolverStep);
 
-    LoopSolver<VectorType> baseSolver(&baseSolverStep,
+    LoopSolver<CorrectionType> baseSolver(&baseSolverStep,
                                                        baseIt,
                                                        baseTolerance,
                                                        &baseEnergyNorm,
                                                        Solver::QUIET);
 
     // Make pre and postsmoothers
-    ProjectedBlockGSStep<MatrixType, VectorType> presmoother;
-    ProjectedBlockGSStep<MatrixType, VectorType> postsmoother;
+    ProjectedBlockGSStep<MatrixType, CorrectionType> presmoother;
+    ProjectedBlockGSStep<MatrixType, CorrectionType> postsmoother;
 
-    MonotoneMGStep<MatrixType, VectorType> multigridStep(1);
+    MonotoneMGStep<MatrixType, CorrectionType> multigridStep(1);
 
     multigridStep.setMGType(mu, nu1, nu2);
     multigridStep.ignoreNodes_       = &dirichletNodes[0];
@@ -129,12 +129,12 @@ int main (int argc, char *argv[]) try
     multigridStep.hasObstacle_       = &hasObstacle;
     multigridStep.obstacles_         = &trustRegionObstacles;
     multigridStep.verbosity_         = Solver::QUIET;
-    multigridStep.obstacleRestrictor_ = new MandelObstacleRestrictor<VectorType>;
+    multigridStep.obstacleRestrictor_ = new MandelObstacleRestrictor<CorrectionType>;
 
 
-    EnergyNorm<MatrixType, VectorType> energyNorm(multigridStep);
+    EnergyNorm<MatrixType, CorrectionType> energyNorm(multigridStep);
 
-    LoopSolver<VectorType> solver(&multigridStep,
+    LoopSolver<CorrectionType> solver(&multigridStep,
                                                    numIt,
                                                    tolerance,
                                                    &energyNorm,
@@ -142,18 +142,18 @@ int main (int argc, char *argv[]) try
 
     double trustRegionRadius = 0.1;
 
-    VectorType rhs;
-    VectorType x(grid.size(0,1));
-    VectorType corr;
+    CorrectionType rhs;
+    SolutionType x(grid.size(0,1));
+    CorrectionType corr;
 
     // //////////////////////////
     //   Initial solution
     // //////////////////////////
 
     for (int i=0; i<x.size(); i++) {
-        x[i][0] = 1.0/(x.size()-1);
-        x[i][1] = 0;
-        x[i][2] = 0;
+        x[i].r[0] = double(i)/(x.size()-1);
+        x[i].r[1] = 0;
+        x[i].q    = Rotation<2,double>::identity();
     }
 
 
@@ -216,7 +216,7 @@ int main (int argc, char *argv[]) try
         for (int i=0; i<trueObstacles[toplevel].size(); i++) {
             trueObstacles[toplevel][i].clear();
             //trueObstacles[toplevel][i].val[0] =     - x[i][0];
-            trueObstacles[toplevel][i].upper(0) = 0.1 - x[i][0];
+            trueObstacles[toplevel][i].upper(0) = 0.1 - x[i].r[0];
         }
         
 
@@ -233,7 +233,7 @@ int main (int argc, char *argv[]) try
         multigridStep.mgTransfer_.resize(toplevel);
 
         for (int i=0; i<multigridStep.mgTransfer_.size(); i++){
-            TruncatedCompressedMGTransfer<VectorType>* newTransferOp = new TruncatedCompressedMGTransfer<VectorType>;
+            TruncatedCompressedMGTransfer<CorrectionType>* newTransferOp = new TruncatedCompressedMGTransfer<CorrectionType>;
             newTransferOp->setup(grid,i,i+1);
             multigridStep.mgTransfer_[i] = newTransferOp;
         }
@@ -262,7 +262,7 @@ int main (int argc, char *argv[]) try
                                     trueObstacles[toplevel],
                                     dirichletNodes[toplevel]);
 
-            dynamic_cast<MultigridStep<MatrixType,VectorType>*>(solver.iterationStep_)->setProblem(hessianMatrix, corr, rhs, toplevel+1);
+            dynamic_cast<MultigridStep<MatrixType,CorrectionType>*>(solver.iterationStep_)->setProblem(hessianMatrix, corr, rhs, toplevel+1);
 
             solver.preprocess();
 
@@ -285,8 +285,10 @@ int main (int argc, char *argv[]) try
              // ////////////////////////////////////////////////////
              //   Check whether trust-region step can be accepted
              // ////////////////////////////////////////////////////
-             /** \todo Faster with expression templates */
-             VectorType newIterate = x;  newIterate += corr;
+
+             SolutionType newIterate = x;
+             for (int j=0; j<newIterate.size(); j++) 
+                 newIterate[j] = RigidBodyMotion<2>::exp(newIterate[j], corr[j]);
 
              /** \todo Don't always recompute oldEnergy */
              double oldEnergy = rodAssembler.computeEnergy(x); 
@@ -296,7 +298,8 @@ int main (int argc, char *argv[]) try
                  DUNE_THROW(SolverError, "Richtung ist keine Abstiegsrichtung!");
                  
              //  Add correction to the current solution
-             x += corr;
+             for (int j=0; j<x.size(); j++) 
+                 x[j] = RigidBodyMotion<2>::exp(x[j], corr[j]);
 
              // Subtract correction from the current obstacle
              for (int k=0; k<corr.size(); k++)
@@ -314,7 +317,7 @@ int main (int argc, char *argv[]) try
         std::string lagrangeFilename = "pressure/lagrange_" + levelAsAscii.str();
         std::ofstream lagrangeFile(lagrangeFilename.c_str());
         
-        VectorType lagrangeMultipliers;
+        CorrectionType lagrangeMultipliers;
         rodAssembler.assembleGradient(x, lagrangeMultipliers);
         lagrangeFile << lagrangeMultipliers << std::endl;
         

src/planarrodassembler.cc

@@ -43,7 +43,7 @@ getNeighborsPerVertex(MatrixIndexSet& nb) const
 
 template <class GridType, int polOrd>
 void Dune::PlanarRodAssembler<GridType, polOrd>::
-assembleMatrix(const BlockVector<FieldVector<double, blocksize> >& sol,
+assembleMatrix(const std::vector<RigidBodyMotion<2> >& sol,
                BCRSMatrix<MatrixBlock>& matrix)
 {
     const typename GridType::Traits::LevelIndexSet& indexSet = grid_->levelIndexSet(grid_->maxLevel());
@@ -68,7 +68,7 @@ assembleMatrix(const BlockVector<FieldVector<double, blocksize> >& sol,
         mat.setSize(numOfBaseFct, numOfBaseFct);
 
         // Extract local solution
-        BlockVector<FieldVector<double, blocksize> > localSolution(numOfBaseFct);
+        std::vector<RigidBodyMotion<2> > localSolution(numOfBaseFct);
         
         for (int i=0; i<numOfBaseFct; i++)
             localSolution[i] = sol[indexSet.subIndex(*it,i,gridDim)];
@@ -102,7 +102,7 @@ template <class GridType, int polOrd>
 template <class MatrixType>
 void Dune::PlanarRodAssembler<GridType, polOrd>::
 getLocalMatrix( EntityType &entity, 
-                const BlockVector<FieldVector<double, blocksize> >& localSolution,
+                const std::vector<RigidBodyMotion<2> >& localSolution,
                 const int matSize, MatrixType& localMat) const
 {
     const typename GridType::Traits::LevelIndexSet& indexSet = grid_->levelIndexSet(grid_->maxLevel());
@@ -162,10 +162,10 @@ getLocalMatrix( EntityType &entity,
         //   Interpolate
         // //////////////////////////////////
         
-        double x_s     = localSolution[0][0]*shapeGrad[0][0] + localSolution[1][0]*shapeGrad[1][0];
-        double y_s     = localSolution[0][1]*shapeGrad[0][0] + localSolution[1][1]*shapeGrad[1][0];
+        double x_s     = localSolution[0].r[0]*shapeGrad[0][0] + localSolution[1].r[0]*shapeGrad[1][0];
+        double y_s     = localSolution[0].r[1]*shapeGrad[0][0] + localSolution[1].r[1]*shapeGrad[1][0];
 
-        double theta   = localSolution[0][2]*shapeFunction[0] + localSolution[1][2]*shapeFunction[1];
+        double theta   = localSolution[0].q.angle_*shapeFunction[0] + localSolution[1].q.angle_*shapeFunction[1];
 
         for (int i=0; i<matSize; i++) {
 
@@ -262,7 +262,7 @@ getLocalMatrix( EntityType &entity,
 
 template <class GridType, int polOrd>
 void Dune::PlanarRodAssembler<GridType, polOrd>::
-assembleGradient(const BlockVector<FieldVector<double, blocksize> >& sol,
+assembleGradient(const std::vector<RigidBodyMotion<2> >& sol,
                  BlockVector<FieldVector<double, blocksize> >& grad) const
 {
     const typename GridType::Traits::LevelIndexSet& indexSet = grid_->levelIndexSet(grid_->maxLevel());
@@ -285,7 +285,7 @@ assembleGradient(const BlockVector<FieldVector<double, blocksize> >& sol,
             = Dune::LagrangeShapeFunctions<double, double, gridDim>::general(it->geometry().type(), elementOrder);
         const int numOfBaseFct = baseSet.size();  
         
-        FieldVector<double, blocksize> localSolution[numOfBaseFct];
+        RigidBodyMotion<2> localSolution[numOfBaseFct];
         
         for (int i=0; i<numOfBaseFct; i++)
             localSolution[i] = sol[indexSet.subIndex(*it,i,gridDim)];
@@ -329,11 +329,11 @@ assembleGradient(const BlockVector<FieldVector<double, blocksize> >& sol,
             //   Interpolate
             // //////////////////////////////////
 
-            double x_s     = localSolution[0][0]*shapeGrad[0][0] + localSolution[1][0]*shapeGrad[1][0];
-            double y_s     = localSolution[0][1]*shapeGrad[0][0] + localSolution[1][1]*shapeGrad[1][0];
-            double theta_s = localSolution[0][2]*shapeGrad[0][0] + localSolution[1][2]*shapeGrad[1][0];
+            double x_s     = localSolution[0].r[0]*shapeGrad[0][0] + localSolution[1].r[0]*shapeGrad[1][0];
+            double y_s     = localSolution[0].r[1]*shapeGrad[0][0] + localSolution[1].r[1]*shapeGrad[1][0];
+            double theta_s = localSolution[0].q.angle_*shapeGrad[0][0] + localSolution[1].q.angle_*shapeGrad[1][0];
 
-            double theta   = localSolution[0][2]*shapeFunction[0] + localSolution[1][2]*shapeFunction[1];
+            double theta   = localSolution[0].q.angle_*shapeFunction[0] + localSolution[1].q.angle_*shapeFunction[1];
 
             // /////////////////////////////////////////////
             //   Sum it all up
@@ -371,7 +371,7 @@ assembleGradient(const BlockVector<FieldVector<double, blocksize> >& sol,
 
 template <class GridType, int polOrd>
 double Dune::PlanarRodAssembler<GridType, polOrd>::
-computeEnergy(const BlockVector<FieldVector<double, blocksize> >& sol) const
+computeEnergy(const std::vector<RigidBodyMotion<2> >& sol) const
 {
     const int maxlevel = grid_->maxLevel();
     double energy = 0;
@@ -392,7 +392,7 @@ computeEnergy(const BlockVector<FieldVector<double, blocksize> >& sol) const
             = Dune::LagrangeShapeFunctions<double, double, gridDim>::general(it->geometry().type(), elementOrder);
         int numOfBaseFct = baseSet.size();
 
-        FieldVector<double, blocksize> localSolution[numOfBaseFct];
+        RigidBodyMotion<2> localSolution[numOfBaseFct];
         
         for (int i=0; i<numOfBaseFct; i++)
             localSolution[i] = sol[indexSet.subIndex(*it,i,gridDim)];
@@ -440,11 +440,11 @@ computeEnergy(const BlockVector<FieldVector<double, blocksize> >& sol) const
             //   Interpolate
             // //////////////////////////////////
 
-            double x_s     = localSolution[0][0]*shapeGrad[0][0] + localSolution[1][0]*shapeGrad[1][0];
-            double y_s     = localSolution[0][1]*shapeGrad[0][0] + localSolution[1][1]*shapeGrad[1][0];
-            double theta_s = localSolution[0][2]*shapeGrad[0][0] + localSolution[1][2]*shapeGrad[1][0];
+            double x_s     = localSolution[0].r[0]*shapeGrad[0][0] + localSolution[1].r[0]*shapeGrad[1][0];
+            double y_s     = localSolution[0].r[1]*shapeGrad[0][0] + localSolution[1].r[1]*shapeGrad[1][0];
+            double theta_s = localSolution[0].q.angle_*shapeGrad[0][0] + localSolution[1].q.angle_*shapeGrad[1][0];
 
-            double theta   = localSolution[0][2]*shapeFunction[0] + localSolution[1][2]*shapeFunction[1];
+            double theta   = localSolution[0].q.angle_*shapeFunction[0] + localSolution[1].q.angle_*shapeFunction[1];
 
             // /////////////////////////////////////////////
             //   Sum it all up

src/planarrodassembler.hh

@@ -57,14 +57,14 @@ namespace Dune
 
         /** \brief Assemble the tangent stiffness matrix and the right hand side
          */
-        void assembleMatrix(const BlockVector<FieldVector<double, blocksize> >& sol,
+        void assembleMatrix(const std::vector<RigidBodyMotion<2> >& sol,
                             BCRSMatrix<MatrixBlock>& matrix);
         
-        void assembleGradient(const BlockVector<FieldVector<double, blocksize> >& sol,
+        void assembleGradient(const std::vector<RigidBodyMotion<2> >& sol,
                               BlockVector<FieldVector<double, blocksize> >& grad) const;
 
         /** \brief Compute the energy of a deformation state */
-        double computeEnergy(const BlockVector<FieldVector<double, blocksize> >& sol) const;
+        double computeEnergy(const std::vector<RigidBodyMotion<2> >& sol) const;
 
         void getNeighborsPerVertex(MatrixIndexSet& nb) const;
         
@@ -73,7 +73,7 @@ namespace Dune
         /** \brief Compute the element tangent stiffness matrix  */
         template <class MatrixType>
         void getLocalMatrix( EntityType &entity, 
-                             const BlockVector<FieldVector<double, blocksize> >& localSolution, 
+                             const std::vector<RigidBodyMotion<2> >& localSolution, 
                              const int matSize, MatrixType& mat) const;
 
         

src/rodwriter.hh

@@ -10,7 +10,7 @@
 
 /** \brief Write a planar rod
  */
-void writeRod(const Dune::BlockVector<Dune::FieldVector<double,3> >& rod, 
+void writeRod(const std::vector<RigidBodyMotion<2> >& rod, 
               const std::string& filename)
 {
     int nLines = rod.size() + 1 + 3*rod.size();
@@ -69,18 +69,18 @@ void writeRod(const Dune::BlockVector<Dune::FieldVector<double,3> >& rod,
 
     // The center axis
     for (int i=0; i<rod.size(); i++)
-        outfile << rod[i][0] << "  " << rod[i][1] << "  0" << std::endl;
+        outfile << rod[i].r[0] << "  " << rod[i].r[1] << "  0" << std::endl;
 
     // The directors
     for (int i=0; i<rod.size(); i++) {
 
         Dune::FieldVector<double, 2> director;
-        director[0] = -cos(rod[i][2]);
-        director[1] = sin(rod[i][2]);
+        director[0] = -cos(rod[i].q.angle_);
+        director[1] = sin(rod[i].q.angle_);
         director *= directorLength;
 
-        outfile << rod[i][0]+director[0] << "  " << rod[i][1]+director[1] << "  0 " << std::endl;
-        outfile << rod[i][0]-director[0] << "  " << rod[i][1]-director[1] << "  0 " << std::endl;
+        outfile << rod[i].r[0]+director[0] << "  " << rod[i].r[1]+director[1] << "  0 " << std::endl;
+        outfile << rod[i].r[0]-director[0] << "  " << rod[i].r[1]-director[1] << "  0 " << std::endl;
 
     }