rodsolver.cc

#include <dune/common/bitfield.hh>

#include <dune/istl/io.hh>

#include <dune/disc/functions/p1function.hh>
#include <dune/disc/miscoperators/laplace.hh>
#include <dune/disc/operators/p1operator.hh>

#include "../common/trustregiongsstep.hh"
#include "../contact/src/contactmmgstep.hh"

#include "../solver/iterativesolver.hh"

#include "../common/energynorm.hh"
#include "../common/h1seminorm.hh"

#include "configuration.hh"
#include "quaternion.hh"

// for debugging
#include "../test/fdcheck.hh"

// Number of degrees of freedom: 
// 7 (x, y, z, q_1, q_2, q_3, q_4) for a spatial rod
//const int blocksize = 6;

template <class GridType>
void RodSolver<GridType>::
setTrustRegionObstacles(double trustRegionRadius,
                        std::vector<BoxConstraint<blocksize> >& trustRegionObstacles)
{
    for (int j=0; j<trustRegionObstacles.size(); j++) {

        for (int k=0; k<blocksize; k++) {

            trustRegionObstacles[j].val[2*k]   = -trustRegionRadius;
            trustRegionObstacles[j].val[2*k+1] =  trustRegionRadius;

        }
        
    }

    //std::cout << trustRegionObstacles << std::endl;
//     exit(0);
}


template <class GridType>
void RodSolver<GridType>::setup(const GridType& grid,
                                const Dune::RodAssembler<GridType>* rodAssembler,
                                const SolutionType& x,
                                int maxTrustRegionSteps,
                                double initialTrustRegionRadius,
                                int multigridIterations,
                                double mgTolerance,
                                int mu, 
                                int nu1,
                                int nu2,
                                int baseIterations,
                                double baseTolerance,
                                bool instrumented)
{
    using namespace Dune;

    grid_ = &grid;
    rodAssembler_             = rodAssembler;
    x_                        = x;
    maxTrustRegionSteps_      = maxTrustRegionSteps;
    initialTrustRegionRadius_ = initialTrustRegionRadius;
    multigridIterations_      = multigridIterations;
    qpTolerance_              = mgTolerance;
    mu_                       = mu;
    nu1_                      = nu1;
    nu2_                      = nu2;
    baseIt_                   = baseIterations;
    baseTolerance_            = baseTolerance;
    instrumented_             = instrumented;

    int numLevels = grid_->maxLevel()+1;

    // ////////////////////////////////////////////
    //   Construct array with the Dirichlet nodes
    // ////////////////////////////////////////////
    dirichletNodes_.resize(numLevels);
    for (int i=0; i<numLevels; i++) {
        
        dirichletNodes_[i].resize( blocksize * grid.size(i,1), false );
        
        for (int j=0; j<blocksize; j++) {
            dirichletNodes_[i][j] = true;
            dirichletNodes_[i][dirichletNodes_[i].size()-1-j] = true;
        }

    }

    // ////////////////////////////////
    //   Create a multigrid solver
    // ////////////////////////////////

    // First create a gauss-seidel base solver
    TrustRegionGSStep<MatrixType, CorrectionType>* baseSolverStep = new TrustRegionGSStep<MatrixType, CorrectionType>;

    EnergyNorm<MatrixType, CorrectionType>* baseEnergyNorm = new EnergyNorm<MatrixType, CorrectionType>(*baseSolverStep);

    IterativeSolver<MatrixType, CorrectionType>* baseSolver 
        = new IterativeSolver<MatrixType, CorrectionType>(baseSolverStep,
                                                          baseIt_,
                                                          baseTolerance_,
                                                          baseEnergyNorm,
                                                          Solver::QUIET);

    // Make pre and postsmoothers
    TrustRegionGSStep<MatrixType, CorrectionType>* presmoother  = new TrustRegionGSStep<MatrixType, CorrectionType>;
    TrustRegionGSStep<MatrixType, CorrectionType>* postsmoother = new TrustRegionGSStep<MatrixType, CorrectionType>;

    ContactMMGStep<MatrixType, CorrectionType>* mmgStep = new ContactMMGStep<MatrixType, CorrectionType>(numLevels);

    mmgStep->setMGType(mu_, nu1_, nu2_);
    mmgStep->dirichletNodes_    = &dirichletNodes_;
    mmgStep->basesolver_        = baseSolver;
    mmgStep->presmoother_       = presmoother;
    mmgStep->postsmoother_      = postsmoother;    
    mmgStep->hasObstacle_       = &hasObstacle_;
    mmgStep->obstacles_         = &trustRegionObstacles_;
    mmgStep->verbosity_         = Solver::FULL;

    // //////////////////////////////////////////////////////////////////////////////////////
    //   Assemble a Laplace matrix to create a norm that's equivalent to the H1-norm
    // //////////////////////////////////////////////////////////////////////////////////////
    LeafP1Function<GridType,double> u(grid),f(grid);
    LaplaceLocalStiffness<GridType,double> laplaceStiffness;
    LeafP1OperatorAssembler<GridType,double,1>* A = new LeafP1OperatorAssembler<GridType,double,1>(grid);
    A->assemble(laplaceStiffness,u,f);

    typedef typename LeafP1OperatorAssembler<GridType,double,1>::RepresentationType LaplaceMatrixType;

    if (h1SemiNorm_)
        delete h1SemiNorm_;

    h1SemiNorm_ = new H1SemiNorm<CorrectionType>(**A);

    mmgSolver_ = new IterativeSolver<MatrixType, CorrectionType>(mmgStep,
                                                                 multigridIterations_,
                                                                 qpTolerance_,
                                                                 h1SemiNorm_,
                                                                 Solver::FULL);

    // Write all intermediate solutions, if requested
    if (instrumented_)
        mmgSolver_->historyBuffer_ = "tmp/";

    // ////////////////////////////////////////////////////////////
    //    Create Hessian matrix and its occupation structure
    // ////////////////////////////////////////////////////////////
    
    if (hessianMatrix_)
        delete hessianMatrix_;

    hessianMatrix_ = new MatrixType;
    MatrixIndexSet indices(grid_->size(1), grid_->size(1));
    rodAssembler_->getNeighborsPerVertex(indices);
    indices.exportIdx(*hessianMatrix_);
    
    // //////////////////////////////////////////////////////////
    //   Create obstacles
    // //////////////////////////////////////////////////////////
    
    hasObstacle_.resize(numLevels);
    for (int i=0; i<hasObstacle_.size(); i++)
        hasObstacle_[i].resize(grid_->size(i, 1),true);
    
    trustRegionObstacles_.resize(numLevels);
    
    for (int i=0; i<numLevels; i++)
        trustRegionObstacles_[i].resize(grid_->size(i,1));
    
    // ////////////////////////////////////
    //   Create the transfer operators
    // ////////////////////////////////////
    for (int k=0; k<mmgStep->mgTransfer_.size(); k++)
        delete(mmgStep->mgTransfer_[k]);
    
    mmgStep->mgTransfer_.resize(numLevels-1);
    
    for (int i=0; i<mmgStep->mgTransfer_.size(); i++){
        TruncatedMGTransfer<CorrectionType>* newTransferOp = new TruncatedMGTransfer<CorrectionType>;
        newTransferOp->setup(*grid_,i,i+1);
        mmgStep->mgTransfer_[i] = newTransferOp;
    }
    
}

template <class GridType>
void RodSolver<GridType>::solve()
{
    using namespace Dune;

    double trustRegionRadius = initialTrustRegionRadius_;

    // /////////////////////////////////////////////////////
    //   Set up the log file, if requested
    // /////////////////////////////////////////////////////
    FILE* fp;
    if (instrumented_) {

        fp = fopen("statistics", "w");
        if (!fp)
            DUNE_THROW(IOError, "Couldn't open statistics file for writing!");
    
    }

    // /////////////////////////////////////////////////////
    //   Trust-Region Solver
    // /////////////////////////////////////////////////////
    for (int i=0; i<maxTrustRegionSteps_; i++) {
        
        std::cout << "----------------------------------------------------" << std::endl;
        std::cout << "      Trust-Region Step Number: " << i << std::endl;
        std::cout << "----------------------------------------------------" << std::endl;
        
        std::cout << "### Trust-Region Radius: " << trustRegionRadius << " ###" << std::endl;
        
        CorrectionType rhs;
        CorrectionType corr(x_.size());
        corr = 0;

        //rodAssembler.setParameters(0,0,0,1,1,1);
        std::cout << "Rod energy: " <<rodAssembler_->computeEnergy(x_) << std::endl;
        rodAssembler_->assembleGradient(x_, rhs);
        rodAssembler_->assembleMatrix(x_, *hessianMatrix_);

        gradientFDCheck(x_, rhs, *rodAssembler_);
        hessianFDCheck(x_, *hessianMatrix_, *rodAssembler_);

#if 0
        for (int j=0; j<hessianMatrix_->N(); j++) {

            for (int k=0; k<hessianMatrix_->M(); k++) {

                if (hessianMatrix_->exists(j,k)) {

                    for (int l=0; l<6; l++)
                        for (int m=0; m<6; m++)
                            assert( std::abs((*hessianMatrix_)[j][k][l][m] - (*hessianMatrix_)[k][j][m][l]) < 1e-6);
#if 0
                    (*hessianMatrix_)[j][k] = 0;

                    if (j==k)
                        for (int l=0; l<6; l++)
                            (*hessianMatrix_)[j][k][l][l] = 1;
#endif
                }
            }
        }
#endif   
        rhs *= -1;

        std::cout << "Gradient:\n" << rhs << std::endl;

        // Create trust-region obstacle on maxlevel
        setTrustRegionObstacles(trustRegionRadius,
                                trustRegionObstacles_[grid_->maxLevel()]);
        
        dynamic_cast<MultigridStep<MatrixType,CorrectionType>*>(mmgSolver_->iterationStep_)->setProblem(*hessianMatrix_, corr, rhs, grid_->maxLevel()+1);
        
        mmgSolver_->preprocess();
        
        dynamic_cast<MultigridStep<MatrixType,CorrectionType>*>(mmgSolver_->iterationStep_)->preprocess();
        
        
        // /////////////////////////////
        //    Solve !
        // /////////////////////////////
        mmgSolver_->solve();
        
        corr = dynamic_cast<MultigridStep<MatrixType,CorrectionType>*>(mmgSolver_->iterationStep_)->getSol();
        
        //std::cout << "Correction: " << std::endl << corr << std::endl;
        
        if (instrumented_) {

            fprintf(fp, "Trust-region step: %d, trust-region radius: %g\n",
                    i, trustRegionRadius);
                    
            // ///////////////////////////////////////////////////////////////
            //   Compute and measure progress against the exact solution
            //   for each trust region step
            // ///////////////////////////////////////////////////////////////
    
            CorrectionType exactSolution = corr;

            // Start from 0
            double oldError = 0;
            double totalConvRate = 1;
            double convRate = 1;
    
            // Write statistics of the initial solution
            // Compute the energy norm
            oldError = h1SemiNorm_->compute(exactSolution);
    
            for (int j=0; j<multigridIterations_; j++) {
        
                // read iteration from file
                CorrectionType intermediateSol(grid_->size(1));
                intermediateSol = 0;
                char iSolFilename[100];
                sprintf(iSolFilename, "tmp/intermediatesolution_%04d", j);
                
                FILE* fpInt = fopen(iSolFilename, "rb");
                if (!fpInt)
                    DUNE_THROW(IOError, "Couldn't open intermediate solution");
                for (int k=0; k<intermediateSol.size(); k++)
                    for (int l=0; l<blocksize; l++)
                        fread(&intermediateSol[k][l], sizeof(double), 1, fpInt);
                
                fclose(fpInt);
                //std::cout << "intermediateSol\n" << intermediateSol << std::endl;

                // Compute errors
                intermediateSol -= exactSolution;
      
                //std::cout << "error\n" << intermediateSol << std::endl;

                // Compute the H1 norm
                double error = h1SemiNorm_->compute(intermediateSol);

                convRate = error / oldError;
                totalConvRate *= convRate;
                
                if (error < 1e-12)
                    break;
                
                printf("Iteration: %d  ", j);
                printf("Errors:  error %g,  convergence Rate: %g,  total conv rate %g\n", 
                       error, convRate, pow(totalConvRate, 1/((double)j+1)), 0);
                fprintf(fp, "%d %g %g %g\n", j+1, error, convRate, pow(totalConvRate, 1/((double)j+1)));
                
                
                oldError = error;
                
            }
            
            
        }


        printf("infinity norm of the correction: %g\n", corr.infinity_norm());
        if (corr.infinity_norm() < 1e-5) {
            std::cout << "CORRECTION IS SMALL ENOUGH" << std::endl;
            break;
        }
        
        // ////////////////////////////////////////////////////
        //   Check whether trust-region step can be accepted
        // ////////////////////////////////////////////////////
        
        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
            Quaternion<double> qCorr = Quaternion<double>::exp(corr[j][3], corr[j][4], corr[j][5]);
            newIterate[j].q = newIterate[j].q.mult(qCorr);
            
        }
        
#if 0
        std::cout << "newIterate: \n";
        for (int j=0; j<newIterate.size(); j++)
            std::cout << newIterate[j] << std::endl;
#endif     
        
        /** \todo Don't always recompute oldEnergy */
        double oldEnergy = rodAssembler_->computeEnergy(x_);
        double energy    = rodAssembler_->computeEnergy(newIterate); 
        
        // compute the model decrease
        // It is $ m(x) - m(x+s) = -<g,s> - 0.5 <s, Hs>
        // Note that rhs = -g
        CorrectionType tmp(corr.size());
        tmp = 0;
        hessianMatrix_->umv(corr, tmp);
        double modelDecrease = (rhs*corr) - 0.5 * (corr*tmp);
        
        std::cout << "Model decrease: " << modelDecrease 
                  << ",  functional decrease: " << oldEnergy - energy << std::endl;
        
        assert(modelDecrease >= 0);
        
        if (energy >= oldEnergy) {
            printf("Richtung ist keine Abstiegsrichtung!\n");
//             std::cout << "old energy: " << oldEnergy << "   new energy: " << energy << std::endl;
//             exit(0);
        }

        if (std::abs(oldEnergy-energy) < 1e-12)
            std::cout << "Suspecting rounding problems" << std::endl;

        // //////////////////////////////////////////////
        //   Check for acceptance of the step
        // //////////////////////////////////////////////
        if ( (oldEnergy-energy) / modelDecrease > 0.9) {
            // very successful iteration
            
            x_ = newIterate;
            trustRegionRadius *= 2;
            
        } else if ( (oldEnergy-energy) / modelDecrease > 0.01
                    || std::abs(oldEnergy-energy) < 1e-12) {
            // successful iteration
            x_ = newIterate;
            
        } else {
            // unsuccessful iteration
            trustRegionRadius /= 2;
            std::cout << "Unsuccessful iteration!" << std::endl;
        }
        
        //  Write current energy
        std::cout << "--- Current energy: " << energy << " ---" << std::endl;
    }

    // //////////////////////////////////////////////
    //   Close logfile
    // //////////////////////////////////////////////
    if (instrumented_)
        fclose(fp);
}