From 031a01977e3edf0a4349838ef38a51f0a3fa3434 Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Fri, 29 Aug 2014 10:48:11 +0000
Subject: [PATCH] Test computing the Hessian of a fixed iterate

This code compares scalar and vector AD, and a finite difference
approximation.

[[Imported from SVN: r9869]]
---
 test/adolctest.cc | 668 +++++++++++++++++++++++++++++++++++-----------
 1 file changed, 511 insertions(+), 157 deletions(-)

diff --git a/test/adolctest.cc b/test/adolctest.cc
index aca5c619..2e7ed44e 100644
--- a/test/adolctest.cc
+++ b/test/adolctest.cc
@@ -1,230 +1,584 @@
-#include "config.h"
+#include <config.h>
 
-#include <adolc/adouble.h>            // use of active doubles
-#include <adolc/drivers/drivers.h>    // use of "Easy to Use" drivers
-// gradient(.) and hessian(.)
-#include <adolc/taping.h>             // use of taping
-
-#undef overwrite  // stupid: ADOL-C sets this to 1, so the name cannot be used
+#define SECOND_ORDER
 
-#include <iostream>
-#include <vector>
+#include <fenv.h>
 
-#include <cstdlib>
-#include <math.h>
+// Includes for the ADOL-C automatic differentiation library
+// Need to come before (almost) all others.
+#include <adolc/adouble.h>
+#include <adolc/drivers/drivers.h>    // use of "Easy to Use" drivers
+#include <adolc/taping.h>
 
 #include <dune/gfe/adolcnamespaceinjections.hh>
+#include <dune/common/fmatrix.hh>
 
-#include <dune/common/parametertree.hh>
-#include <dune/common/parametertreeparser.hh>
-
-#include <dune/istl/io.hh>
+#include <dune/geometry/quadraturerules.hh>
 
 #include <dune/grid/yaspgrid.hh>
-#include <dune/geometry/quadraturerules.hh>
+#include <dune/grid/utility/structuredgridfactory.hh>
+
+#include <dune/istl/io.hh>
 
-#include <dune/localfunctions/lagrange/q1.hh>
+#include <dune/fufem/functionspacebases/p2nodalbasis.hh>
 
-#include <dune/fufem/functionspacebases/q1nodalbasis.hh>
 
-#include <dune/gfe/realtuple.hh>
+#include <dune/gfe/rotation.hh>
+#include <dune/gfe/localgeodesicfestiffness.hh>
 #include <dune/gfe/localgeodesicfefunction.hh>
-#include <dune/gfe/harmonicenergystiffness.hh>
-#include <dune/gfe/cosseratenergystiffness.hh>
-#include <dune/gfe/localgeodesicfeadolcstiffness.hh>
+#include <dune/gfe/rotation.hh>
+
+// grid dimension
+const int dim = 2;
 
-#include "valuefactory.hh"
-#include "multiindex.hh"
+// Image space of the geodesic fe functions
+typedef Rotation<double,3> TargetSpace;
 
 using namespace Dune;
 
-/** \brief Computes the diameter of a set */
-template <class TargetSpace>
-double diameter(const std::vector<TargetSpace>& v)
+
+
+template<class GridView, class LocalFiniteElement, int dim, class field_type=double>
+class CosseratEnergyLocalStiffness
+    : public LocalGeodesicFEStiffness<GridView,LocalFiniteElement,Rotation<field_type,dim> >
 {
-    double d = 0;
-    for (size_t i=0; i<v.size(); i++)
-        for (size_t j=0; j<v.size(); j++)
-            d = std::max(d, TargetSpace::distance(v[i],v[j]));
-    return d;
-}
+    // grid types
+    typedef typename GridView::Grid::ctype DT;
+    typedef Rotation<field_type,dim> TargetSpace;
+    typedef typename TargetSpace::ctype RT;
+    typedef typename GridView::template Codim<0>::Entity Entity;
 
+    // some other sizes
+    enum {gridDim=GridView::dimension};
 
+public:
 
-template <int blocksize, class TargetSpace>
-void compareMatrices(const Matrix<FieldMatrix<double,blocksize,blocksize> >& fdMatrix,
-                     const Matrix<FieldMatrix<double,blocksize,blocksize> >& adolcMatrix,
-                     const std::vector<TargetSpace>& configuration)
-{
-  assert(fdMatrix.N() == adolcMatrix.N());
-  assert(fdMatrix.M() == adolcMatrix.M());
+    /** \brief Assemble the energy for a single element */
+    RT energy (const Entity& element,
+               const LocalFiniteElement& localFiniteElement,
+               const std::vector<TargetSpace>& localSolution) const
+    {
+      assert(element.type() == localFiniteElement.type());
+      typedef typename GridView::template Codim<0>::Entity::Geometry Geometry;
 
-  Matrix<FieldMatrix<double,blocksize,blocksize> > diff = fdMatrix;
-  diff -= adolcMatrix;
+      RT energy = 0;
 
-  if ( (diff.frobenius_norm() / fdMatrix.frobenius_norm()) > 1e-4) {
+      typedef LocalGeodesicFEFunction<gridDim, DT, LocalFiniteElement, TargetSpace> LocalGFEFunctionType;
+      LocalGFEFunctionType localGeodesicFEFunction(localFiniteElement,localSolution);
 
-    std::cout << "Matrices differ for" << std::endl;
-    for (size_t j=0; j<configuration.size(); j++)
-      std::cout << configuration[j] << std::endl;
+      int quadOrder = (element.type().isSimplex()) ? localFiniteElement.localBasis().order()
+                                                   : localFiniteElement.localBasis().order() * gridDim;
 
-    std::cout << "finite differences:" << std::endl;
-    printmatrix(std::cout, fdMatrix, "finite differences", "--");
-    std::cout << "ADOL-C:" << std::endl;
-    printmatrix(std::cout, adolcMatrix, "ADOL-C", "--");
-    assert(false);
-  }
-}
+      const Dune::QuadratureRule<DT, gridDim>& quad
+          = Dune::QuadratureRules<DT, gridDim>::rule(element.type(), quadOrder);
 
-template <class TargetSpace, int gridDim>
-int testHarmonicEnergy() {
+      for (size_t pt=0; pt<quad.size(); pt++) {
 
-  std::cout << " --- Testing " << className<TargetSpace>() << ", domain dimension: " << gridDim << " ---" << std::endl;
+        // Local position of the quadrature point
+        const Dune::FieldVector<DT,gridDim>& quadPos = quad[pt].position();
 
-  // set up a test grid
-  typedef YaspGrid<gridDim> GridType;
-  FieldVector<double,gridDim> l(1);
-  std::array<int,gridDim> elements;
-  std::fill(elements.begin(), elements.end(), 1);
-  GridType grid(l,elements);
+        const DT integrationElement = element.geometry().integrationElement(quadPos);
 
-  typedef Q1NodalBasis<typename GridType::LeafGridView,double> Q1Basis;
-  Q1Basis q1Basis(grid.leafGridView());
+        const typename Geometry::JacobianInverseTransposed& jacobianInverseTransposed = element.geometry().jacobianInverseTransposed(quadPos);
 
-  typedef Q1LocalFiniteElement<double,double,gridDim> LocalFE;
-  LocalFE localFiniteElement;
+        DT weight = quad[pt].weight() * integrationElement;
 
-  // Assembler using finite differences
-  HarmonicEnergyLocalStiffness<typename GridType::LeafGridView,
-                               typename Q1Basis::LocalFiniteElement,
-                               TargetSpace> harmonicEnergyLocalStiffness;
+        // The value of the local function
+        Rotation<field_type,dim> value = localGeodesicFEFunction.evaluate(quadPos);
 
-  // Assembler using ADOL-C
-  typedef typename TargetSpace::template rebind<adouble>::other ATargetSpace;
-  HarmonicEnergyLocalStiffness<typename GridType::LeafGridView,
-                               typename Q1Basis::LocalFiniteElement,
-                               ATargetSpace> harmonicEnergyADOLCLocalStiffness;
-  LocalGeodesicFEADOLCStiffness<typename GridType::LeafGridView,
-                                typename Q1Basis::LocalFiniteElement,
-                                TargetSpace> localGFEADOLCStiffness(&harmonicEnergyADOLCLocalStiffness);
+        // The derivative of the local function defined on the reference element
+        typename LocalGFEFunctionType::DerivativeType referenceDerivative = localGeodesicFEFunction.evaluateDerivative(quadPos,value);
 
-  size_t nDofs = localFiniteElement.localBasis().size();
+        // The derivative of the function defined on the actual element
+        typename LocalGFEFunctionType::DerivativeType derivative(0);
 
-  std::vector<TargetSpace> localSolution(nDofs);
+        for (size_t comp=0; comp<referenceDerivative.N(); comp++)
+            jacobianInverseTransposed.umv(referenceDerivative[comp], derivative[comp]);
 
-  std::vector<TargetSpace> testPoints;
-  ValueFactory<TargetSpace>::get(testPoints);
+        //////////////////////////////////////////////////////////
+        //  Compute the derivative of the rotation
+        //  Note: we need it in matrix coordinates
+        //////////////////////////////////////////////////////////
 
-  int nTestPoints = testPoints.size();
+        Dune::FieldMatrix<field_type,dim,dim> R;
+        value.matrix(R);
 
-  MultiIndex index(nDofs, nTestPoints);
-  int numIndices = index.cycle();
+        // Add the local energy density
+        energy += 2.5e3*weight *derivative.frobenius_norm2();
 
-  for (int i=0; i<numIndices; i++, ++index) {
+      }
 
-    for (size_t j=0; j<nDofs; j++)
-      localSolution[j] = testPoints[index[j]];
+      return energy;
+    }
 
-    if (diameter(localSolution) > 0.5*M_PI)
-        continue;
+};
 
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief Assembles energy gradient and Hessian with ADOL-C
+ */
+template<class GridView, class LocalFiniteElement>
+class LocalGeodesicFEADOLCStiffness
+{
+    // grid types
+    typedef typename GridView::Grid::ctype DT;
+    typedef typename TargetSpace::ctype RT;
+    typedef typename GridView::template Codim<0>::Entity Entity;
 
-    std::vector<typename TargetSpace::TangentVector> localGradient;
-    harmonicEnergyLocalStiffness.assembleGradientAndHessian(*grid.template leafbegin<0>(),localFiniteElement, localSolution,
-                                                            localGradient);
+    typedef typename TargetSpace::template rebind<adouble>::other ATargetSpace;
 
-    localGFEADOLCStiffness.assembleGradientAndHessian(*grid.template leafbegin<0>(),localFiniteElement, localSolution,
-                                                      localGradient);
+    // some other sizes
+    enum {gridDim=GridView::dimension};
 
-    compareMatrices(harmonicEnergyLocalStiffness.A_, localGFEADOLCStiffness.A_, localSolution);
+public:
 
-  }
+    //! Dimension of the embedding space
+    enum { embeddedBlocksize = TargetSpace::EmbeddedTangentVector::dimension };
 
-  return 0;
-}
+    LocalGeodesicFEADOLCStiffness(const LocalGeodesicFEStiffness<GridView, LocalFiniteElement, ATargetSpace>* energy)
+    : localEnergy_(energy)
+    {}
+
+    /** \brief Compute the energy at the current configuration */
+    virtual RT energy (const Entity& e,
+               const LocalFiniteElement& localFiniteElement,
+               const std::vector<TargetSpace>& localSolution) const;
 
-int testCosseratEnergy() {
+    /** \brief Assemble the local stiffness matrix at the current position
 
-  typedef RigidBodyMotion<double,3> TargetSpace;
-  const int gridDim = 2;
+    This uses the automatic differentiation toolbox ADOL_C.
+    */
+    virtual void assembleGradientAndHessian(const Entity& e,
+                         const LocalFiniteElement& localFiniteElement,
+                         const std::vector<TargetSpace>& localSolution,
+                         std::vector<Dune::FieldVector<double, 4> >& localGradient,
+                         Dune::Matrix<Dune::FieldMatrix<RT,embeddedBlocksize,embeddedBlocksize> >& localHessian,
+                         bool vectorMode);
 
-  std::cout << " --- Testing " << className<TargetSpace>() << ", domain dimension: " << gridDim << " ---" << std::endl;
+    const LocalGeodesicFEStiffness<GridView, LocalFiniteElement, ATargetSpace>* localEnergy_;
 
-  ParameterTree parameterSet;
-  ParameterTreeParser::readINITree("../cosserat-continuum.parset", parameterSet);
+};
 
-  const ParameterTree& materialParameters = parameterSet.sub("materialParameters");
-  std::cout << "Material parameters:" << std::endl;
-  materialParameters.report();
 
-  // set up a test grid
-  typedef YaspGrid<gridDim> GridType;
-  FieldVector<double,gridDim> l(1);
-  std::array<int,gridDim> elements;
-  std::fill(elements.begin(), elements.end(), 1);
-  GridType grid(l,elements);
+template <class GridView, class LocalFiniteElement>
+typename LocalGeodesicFEADOLCStiffness<GridView, LocalFiniteElement>::RT
+LocalGeodesicFEADOLCStiffness<GridView, LocalFiniteElement>::
+energy(const Entity& element,
+       const LocalFiniteElement& localFiniteElement,
+       const std::vector<TargetSpace>& localSolution) const
+{
+    double pureEnergy;
 
-  typedef Q1NodalBasis<typename GridType::LeafGridView,double> Q1Basis;
-  Q1Basis q1Basis(grid.leafGridView());
+    std::vector<ATargetSpace> localASolution(localSolution.size());
 
-  typedef Q1LocalFiniteElement<double,double,gridDim> LocalFE;
-  LocalFE localFiniteElement;
+    trace_on(1);
 
-  // Assembler using finite differences
-  CosseratEnergyLocalStiffness<GridType::LeafGridView,
-                               Q1Basis::LocalFiniteElement,
-                               3> cosseratEnergyLocalStiffness(materialParameters,NULL,NULL);
+    adouble energy = 0;
 
-  // Assembler using ADOL-C
-  CosseratEnergyLocalStiffness<GridType::LeafGridView,
-                               Q1Basis::LocalFiniteElement,
-                               3,adouble> cosseratEnergyADOLCLocalStiffness(materialParameters, NULL, NULL);
-  LocalGeodesicFEADOLCStiffness<GridType::LeafGridView,
-                                Q1Basis::LocalFiniteElement,
-                                TargetSpace> localGFEADOLCStiffness(&cosseratEnergyADOLCLocalStiffness);
+    // The following loop is not quite intuitive: we copy the localSolution into an
+    // array of FieldVector<double>, go from there to FieldVector<adouble> and
+    // only then to ATargetSpace.
+    // Rationale: The constructor/assignment-from-vector of TargetSpace frequently
+    // contains a projection onto the manifold from the surrounding Euclidean space.
+    // ADOL-C needs a function on the whole Euclidean space, hence that projection
+    // is part of the function and needs to be taped.
 
-  size_t nDofs = localFiniteElement.localBasis().size();
+    // The following variable cannot be declared inside of the loop, or ADOL-C will report wrong results
+    // (Presumably because several independent variables use the same memory location.)
+    std::vector<typename ATargetSpace::CoordinateType> aRaw(localSolution.size());
+    for (size_t i=0; i<localSolution.size(); i++) {
+      typename TargetSpace::CoordinateType raw = localSolution[i].globalCoordinates();
+      for (size_t j=0; j<raw.size(); j++)
+        aRaw[i][j] <<= raw[j];
+      localASolution[i] = aRaw[i];  // may contain a projection onto M -- needs to be done in adouble
+    }
 
-  std::vector<TargetSpace> localSolution(nDofs);
-  std::vector<TargetSpace> testPoints;
-  ValueFactory<TargetSpace>::get(testPoints);
+    energy = localEnergy_->energy(element,localFiniteElement,localASolution);
 
-  int nTestPoints = testPoints.size();
+    energy >>= pureEnergy;
 
-  MultiIndex index(nDofs, nTestPoints);
-  int numIndices = index.cycle();
+    trace_off();
+    return pureEnergy;
+}
 
-  for (int i=0; i<numIndices; i++, ++index) {
 
-    for (size_t j=0; j<nDofs; j++)
-      localSolution[j] = testPoints[index[j]];
 
-    if (diameter(localSolution) > TargetSpace::convexityRadius)
-        continue;
+// ///////////////////////////////////////////////////////////
+//   Compute gradient and Hessian together
+//   To compute the Hessian we need to compute the gradient anyway, so we may
+//   as well return it.  This saves assembly time.
+// ///////////////////////////////////////////////////////////
+template <class GridType, class LocalFiniteElement>
+void LocalGeodesicFEADOLCStiffness<GridType, LocalFiniteElement>::
+assembleGradientAndHessian(const Entity& element,
+                const LocalFiniteElement& localFiniteElement,
+                const std::vector<TargetSpace>& localSolution,
+                std::vector<Dune::FieldVector<double,4> >& localGradient,
+                Dune::Matrix<Dune::FieldMatrix<RT,embeddedBlocksize,embeddedBlocksize> >& localHessian,
+                bool vectorMode)
+{
+    // Tape energy computation.  We may not have to do this every time, but it's comparatively cheap.
+    energy(element, localFiniteElement, localSolution);
+
+    /////////////////////////////////////////////////////////////////
+    // Compute the gradient.
+    /////////////////////////////////////////////////////////////////
+
+    // Copy data from Dune data structures to plain-C ones
+    size_t nDofs = localSolution.size();
+    size_t nDoubles = nDofs*embeddedBlocksize;
+    std::vector<double> xp(nDoubles);
+    int idx=0;
+    for (size_t i=0; i<nDofs; i++)
+        for (size_t j=0; j<embeddedBlocksize; j++)
+            xp[idx++] = localSolution[i].globalCoordinates()[j];
+
+  // Compute gradient
+    std::vector<double> g(nDoubles);
+    gradient(1,nDoubles,xp.data(),g.data());                  // gradient evaluation
+
+    // Copy into Dune type
+    std::vector<typename TargetSpace::EmbeddedTangentVector> localEmbeddedGradient(localSolution.size());
+
+    idx=0;
+    for (size_t i=0; i<nDofs; i++)
+        for (size_t j=0; j<embeddedBlocksize; j++)
+            localGradient[i][j] = g[idx++];
+
+    /////////////////////////////////////////////////////////////////
+    // Compute Hessian
+    /////////////////////////////////////////////////////////////////
+
+    localHessian.setSize(nDofs,nDofs);
+
+    double* rawHessian[nDoubles];
+    for(size_t i=0; i<nDoubles; i++)
+        rawHessian[i] = (double*)malloc((i+1)*sizeof(double));
+
+    if (vectorMode)
+      hessian2(1,nDoubles,xp.data(),rawHessian);
+    else
+      hessian(1,nDoubles,xp.data(),rawHessian);
+
+    // Copy Hessian into Dune data type
+    for(size_t i=0; i<nDoubles; i++)
+      for (size_t j=0; j<nDoubles; j++)
+      {
+        double value = (i>=j) ? rawHessian[i][j] : rawHessian[j][i];
+        localHessian[j/embeddedBlocksize][i/embeddedBlocksize][j%embeddedBlocksize][i%embeddedBlocksize] = value;
+      }
+
+    for(size_t i=0; i<nDoubles; i++)
+        free(rawHessian[i]);
 
-    std::vector<typename TargetSpace::TangentVector> localGradient;
-    cosseratEnergyLocalStiffness.assembleGradientAndHessian(*grid.leafbegin<0>(),localFiniteElement, localSolution,
-                                                            localGradient);
+}
 
-    localGFEADOLCStiffness.assembleGradientAndHessian(*grid.leafbegin<0>(),localFiniteElement, localSolution,
-                                                      localGradient);
+/** \brief Assembles energy gradient and Hessian with ADOL-C
+ */
+template<class GridView, class LocalFiniteElement>
+class LocalGeodesicFEFDStiffness
+{
+    // grid types
+    typedef typename GridView::Grid::ctype DT;
+    typedef typename TargetSpace::ctype RT;
+    typedef typename GridView::template Codim<0>::Entity Entity;
+
+public:
+
+    //! Dimension of a tangent space
+    enum { blocksize = TargetSpace::TangentVector::dimension };
+
+    //! Dimension of the embedding space
+    enum { embeddedBlocksize = TargetSpace::EmbeddedTangentVector::dimension };
+
+    LocalGeodesicFEFDStiffness(const LocalGeodesicFEStiffness<GridView, LocalFiniteElement, TargetSpace>* energy)
+    : localEnergy_(energy)
+    {}
+
+    /** \brief Compute the energy at the current configuration */
+    virtual RT energy (const Entity& element,
+               const LocalFiniteElement& localFiniteElement,
+               const std::vector<TargetSpace>& localSolution) const
+    {
+      return localEnergy_->energy(element,localFiniteElement,localSolution);
+    }
+
+    virtual void assembleGradientAndHessian(const Entity& e,
+                                 const LocalFiniteElement& localFiniteElement,
+                                 const std::vector<TargetSpace>& localSolution,
+                                 std::vector<Dune::FieldVector<double,4> >& localGradient,
+                                 Dune::Matrix<Dune::FieldMatrix<RT,embeddedBlocksize,embeddedBlocksize> >& localHessian);
+
+    const LocalGeodesicFEStiffness<GridView, LocalFiniteElement, TargetSpace>* localEnergy_;
+};
+
+// ///////////////////////////////////////////////////////////
+//   Compute gradient by finite-difference approximation
+// ///////////////////////////////////////////////////////////
+template <class GridType, class LocalFiniteElement>
+void LocalGeodesicFEFDStiffness<GridType, LocalFiniteElement>::
+assembleGradientAndHessian(const Entity& element,
+                const LocalFiniteElement& localFiniteElement,
+                const std::vector<TargetSpace>& localSolution,
+                std::vector<Dune::FieldVector<double, 4> >& localGradient,
+                Dune::Matrix<Dune::FieldMatrix<RT,embeddedBlocksize,embeddedBlocksize> >& localHessian)
+{
+    // Number of degrees of freedom for this element
+    size_t nDofs = localSolution.size();
+
+    // Clear assemble data
+    localHessian.setSize(nDofs, nDofs);
+    localHessian = 0;
+
+    const double eps = 1e-4;
+
+    std::vector<Dune::FieldMatrix<double,embeddedBlocksize,embeddedBlocksize> > B(localSolution.size());
+    for (size_t i=0; i<B.size(); i++)
+    {
+        B[i] = 0;
+        for (int j=0; j<embeddedBlocksize; j++)
+          B[i][j][j] = 1.0;
+    }
+
+    // Precompute negative energy at the current configuration
+    // (negative because that is how we need it as part of the 2nd-order fd formula)
+    RT centerValue   = -energy(element, localFiniteElement, localSolution);
+
+    // Precompute energy infinitesimal corrections in the directions of the local basis vectors
+    std::vector<Dune::array<RT,embeddedBlocksize> > forwardEnergy(nDofs);
+    std::vector<Dune::array<RT,embeddedBlocksize> > backwardEnergy(nDofs);
+
+    for (size_t i=0; i<localSolution.size(); i++) {
+        for (size_t i2=0; i2<embeddedBlocksize; i2++) {
+            typename TargetSpace::EmbeddedTangentVector epsXi = B[i][i2];
+            epsXi *= eps;
+            typename TargetSpace::EmbeddedTangentVector minusEpsXi = epsXi;
+            minusEpsXi  *= -1;
+
+            std::vector<TargetSpace> forwardSolution  = localSolution;
+            std::vector<TargetSpace> backwardSolution = localSolution;
+
+            forwardSolution[i]  = TargetSpace(localSolution[i].globalCoordinates() + epsXi);
+            backwardSolution[i] = TargetSpace(localSolution[i].globalCoordinates() + minusEpsXi);
+
+            forwardEnergy[i][i2]  = energy(element, localFiniteElement, forwardSolution);
+            backwardEnergy[i][i2] = energy(element, localFiniteElement, backwardSolution);
+
+        }
+
+    }
+
+    //////////////////////////////////////////////////////////////
+    //   Compute gradient by finite-difference approximation
+    //////////////////////////////////////////////////////////////
+
+    localGradient.resize(localSolution.size());
+
+    for (size_t i=0; i<localSolution.size(); i++)
+        for (int j=0; j<embeddedBlocksize; j++)
+            localGradient[i][j] = (forwardEnergy[i][j] - backwardEnergy[i][j]) / (2*eps);
+
+    ///////////////////////////////////////////////////////////////////////////
+    //   Compute Riemannian Hesse matrix by finite-difference approximation.
+    //   We loop over the lower left triangular half of the matrix.
+    //   The other half follows from symmetry.
+    ///////////////////////////////////////////////////////////////////////////
+    //#pragma omp parallel for schedule (dynamic)
+    for (size_t i=0; i<localSolution.size(); i++) {
+        for (size_t i2=0; i2<embeddedBlocksize; i2++) {
+            for (size_t j=0; j<=i; j++) {
+                for (size_t j2=0; j2<((i==j) ? i2+1 : embeddedBlocksize); j2++) {
+
+                    std::vector<TargetSpace> forwardSolutionXiEta  = localSolution;
+                    std::vector<TargetSpace> backwardSolutionXiEta  = localSolution;
+
+                    typename TargetSpace::EmbeddedTangentVector epsXi  = B[i][i2];    epsXi *= eps;
+                    typename TargetSpace::EmbeddedTangentVector epsEta = B[j][j2];   epsEta *= eps;
+
+                    typename TargetSpace::EmbeddedTangentVector minusEpsXi  = epsXi;   minusEpsXi  *= -1;
+                    typename TargetSpace::EmbeddedTangentVector minusEpsEta = epsEta;  minusEpsEta *= -1;
+
+                    if (i==j)
+                        forwardSolutionXiEta[i] = TargetSpace(localSolution[i].globalCoordinates() + epsXi+epsEta);
+                    else {
+                        forwardSolutionXiEta[i] = TargetSpace(localSolution[i].globalCoordinates() + epsXi);
+                        forwardSolutionXiEta[j] = TargetSpace(localSolution[j].globalCoordinates() + epsEta);
+                    }
+
+                    if (i==j)
+                        backwardSolutionXiEta[i] = TargetSpace(localSolution[i].globalCoordinates() + minusEpsXi+minusEpsEta);
+                    else {
+                        backwardSolutionXiEta[i] = TargetSpace(localSolution[i].globalCoordinates() + minusEpsXi);
+                        backwardSolutionXiEta[j] = TargetSpace(localSolution[j].globalCoordinates() + minusEpsEta);
+                    }
+
+                    RT forwardValue  = energy(element, localFiniteElement, forwardSolutionXiEta) - forwardEnergy[i][i2] - forwardEnergy[j][j2];
+                    RT backwardValue = energy(element, localFiniteElement, backwardSolutionXiEta) - backwardEnergy[i][i2] - backwardEnergy[j][j2];
+
+                    localHessian[i][j][i2][j2] = localHessian[j][i][j2][i2] = 0.5 * (forwardValue - 2*centerValue + backwardValue) / (eps*eps);
+
+                }
+            }
+        }
+    }
+}
 
-    compareMatrices(cosseratEnergyLocalStiffness.A_, localGFEADOLCStiffness.A_, localSolution);
 
+// Compare two matrices
+void compareMatrices(const Matrix<FieldMatrix<double,4,4> >& matrixA, std::string nameA,
+                     const Matrix<FieldMatrix<double,4,4> >& matrixB, std::string nameB)
+{
+  double maxAbsDifference = -1;
+  double maxRelDifference = -1;
+
+  for(int i=0; i<matrixA.N(); i++) {
+
+    for (int j=0; j<matrixA.M(); j++ ) {
+
+      for (int ii=0; ii<4; ii++)
+        for (int jj=0; jj<4; jj++)
+        {
+          double valueA = matrixA[i][j][ii][jj];
+          double valueB = matrixB[i][j][ii][jj];
+
+          double absDifference = valueA - valueB;
+          double relDifference = std::abs(absDifference) / std::abs(valueA);
+          maxAbsDifference = std::max(maxAbsDifference, std::abs(absDifference));
+          if (not isinf(relDifference))
+            maxRelDifference = std::max(maxRelDifference, relDifference);
+
+          if (relDifference > 1)
+            std::cout << i << ", " << j << "   " << ii << ", " << jj
+            << ",       " << nameA << ": " << valueA << ",           " << nameB << ": " << valueB << std::endl;
+        }
+    }
   }
 
-  return 0;
+  std::cout << nameA << " vs. " << nameB << " -- max absolute / relative difference is " << maxAbsDifference << " / " << maxRelDifference << std::endl;
 }
 
 
-int main()
+int main (int argc, char *argv[]) try
 {
-   testHarmonicEnergy<UnitVector<double,2>, 1>();
-   testHarmonicEnergy<UnitVector<double,3>, 1>();
-   testHarmonicEnergy<UnitVector<double,2>, 2>();
-   testHarmonicEnergy<UnitVector<double,3>, 2>();
+    typedef std::vector<TargetSpace> SolutionType;
 
-   testCosseratEnergy();
-}
+    // ///////////////////////////////////////
+    //    Create the grid
+    // ///////////////////////////////////////
+    typedef YaspGrid<dim> GridType;
+
+    shared_ptr<GridType> grid;
+
+    FieldVector<double,dim> lower = {{0, 0}};
+    FieldVector<double,dim> upper = {{0.38, 0.128}};
+
+    array<unsigned int,dim> elements = {{15, 5}};
+    grid = StructuredGridFactory<GridType>::createCubeGrid(lower, upper, elements);
+
+    typedef GridType::LeafGridView GridView;
+    GridView gridView = grid->leafGridView();
+
+    typedef P2NodalBasis<GridView,double> FEBasis;
+    FEBasis feBasis(gridView);
+
+    // /////////////////////////////////////////
+    //   Read Dirichlet values
+    // /////////////////////////////////////////
+
+    // //////////////////////////
+    //   Initial iterate
+    // //////////////////////////
+
+    SolutionType x(feBasis.size());
+
+    //////////////////////////////////////////7
+    //  Read initial iterate from file
+    //////////////////////////////////////////7
+    Dune::BlockVector<FieldVector<double,7> > xEmbedded(x.size());
+
+    std::ifstream file("dangerous_iterate", std::ios::in|std::ios::binary);
+    if (not(file))
+      DUNE_THROW(SolverError, "Couldn't open file 'dangerous_iterate' for reading");
+
+    GenericVector::readBinary(file, xEmbedded);
+
+    file.close();
+
+    for (int ii=0; ii<x.size(); ii++)
+    {
+      // The first 3 of the 7 entries are irrelevant
+      FieldVector<double, 4> rotationEmbedded;
+      for (int jj=0; jj<4; jj++)
+        rotationEmbedded[jj] = xEmbedded[ii][jj+3];
+
+      x[ii] = TargetSpace(rotationEmbedded);
+    }
+
+    // ////////////////////////////////////////////////////////////
+    //   Create an assembler for the energy functional
+    // ////////////////////////////////////////////////////////////
+
+    // Assembler using ADOL-C
+    CosseratEnergyLocalStiffness<GridView,
+                                 FEBasis::LocalFiniteElement,
+                                 3,adouble> cosseratEnergyADOLCLocalStiffness;
+
+    LocalGeodesicFEADOLCStiffness<GridView,
+                                  FEBasis::LocalFiniteElement> localGFEADOLCStiffness(&cosseratEnergyADOLCLocalStiffness);
+
+    CosseratEnergyLocalStiffness<GridView,
+                                 FEBasis::LocalFiniteElement,
+                                 3,double> cosseratEnergyFDLocalStiffness;
+
+    LocalGeodesicFEFDStiffness<GridView,
+                             FEBasis::LocalFiniteElement> localGFEFDStiffness(&cosseratEnergyFDLocalStiffness);
+
+    // Compute and compare matrices
+    auto it    = gridView.template begin<0>();
+    auto endit = gridView.template end<0>  ();
+
+    for( ; it != endit; ++it ) {
+
+        std::cout << "  ++++  element " << gridView.indexSet().index(*it) << " ++++" << std::endl;
+
+        const int numOfBaseFct = feBasis.getLocalFiniteElement(*it).localBasis().size();
+
+        // Extract local solution
+        std::vector<TargetSpace> localSolution(numOfBaseFct);
+
+        for (int i=0; i<numOfBaseFct; i++)
+            localSolution[i] = x[feBasis.index(*it,i)];
+
+        std::vector<Dune::FieldVector<double,4> > localADGradient(numOfBaseFct);
+        std::vector<Dune::FieldVector<double,4> > localADVMGradient(numOfBaseFct);  // VM: vector-mode
+        std::vector<Dune::FieldVector<double,4> > localFDGradient(numOfBaseFct);
+
+        Matrix<FieldMatrix<double,4,4> > localADHessian;
+        Matrix<FieldMatrix<double,4,4> > localADVMHessian;   // VM: vector-mode
+        Matrix<FieldMatrix<double,4,4> > localFDHessian;
+
+        // setup local matrix and gradient
+        localGFEADOLCStiffness.assembleGradientAndHessian(*it,
+                                                          feBasis.getLocalFiniteElement(*it),
+                                                          localSolution,
+                                                          localADGradient,
+                                                          localADHessian,
+                                                          false);   // 'true' means 'vector mode'
+
+        localGFEADOLCStiffness.assembleGradientAndHessian(*it,
+                                                          feBasis.getLocalFiniteElement(*it),
+                                                          localSolution,
+                                                          localADGradient,
+                                                          localADVMHessian,
+                                                          true);   // 'true' means 'vector mode'
+
+        localGFEFDStiffness.assembleGradientAndHessian(*it, feBasis.getLocalFiniteElement(*it), localSolution, localFDGradient, localFDHessian);
+
+        compareMatrices(localADHessian, "AD", localFDHessian, "FD");
+        compareMatrices(localADHessian, "AD scalar", localADVMHessian, "AD vector");
+    }
+
+    // //////////////////////////////
+ } catch (Exception e) {
+
+    std::cout << e << std::endl;
+
+ }
-- 
GitLab