Symmetric_Dirichlet_ADOLC.hh 9.1 KB
Newer Older
Pit Nestle's avatar
Pit Nestle committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46

#ifndef SYMMETRIC_DIRICHLET_ADOLC
#define SYMMETRIC_DIRICHLET_ADOLC

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#include <iostream>
#include <cmath>

#if (COMISO_ADOLC_AVAILABLE && COMISO_EIGEN3_AVAILABLE)

#include <CoMISo/Utils/StopWatch.hh>
#include <vector>
#include <CoMISo/NSolver/LinearConstraint.hh>
#include <CoMISo/NSolver/NPDerivativeChecker.hh>
#include <CoMISo/NSolver/NProblemInterfaceADOLC.hh>
#include <CoMISo/NSolver/IPOPTSolver.hh>
#include <CoMISo/NSolver/NewtonSolver.hh>


class Symmetric_Dirichlet_ADOLC : public COMISO::NProblemInterfaceADOLC
{
public:
    typedef Eigen::Matrix<adouble, Eigen::Dynamic, Eigen::Dynamic> MatrixXad;

  Symmetric_Dirichlet_ADOLC() : COMISO::NProblemInterfaceADOLC(),
      mesh_vertex_count(0),
      param_vertex_count(0),
      iteration(0)
  {}

  virtual int n_unknowns()
  {
      return n_vertices()*2;
  }

  virtual void initial_x(double* _x)
  {
    int v = n_vertices();
    for (int i = 0; i < v; i++)
    {
        _x[i+0] = this->par(i, 0).value();
        _x[i+v] = this->par(i, 1).value();
    }
  }

Pit Nestle's avatar
Pit Nestle committed
47 48
  inline

Pit Nestle's avatar
Pit Nestle committed
49 50 51 52
  virtual adouble eval_f_adouble(const adouble* _x)
  {
      int v = n_vertices();
      int f = n_faces();
Pit Nestle's avatar
Pit Nestle committed
53
      adouble res = 0.0;
Pit Nestle's avatar
Pit Nestle committed
54
      MatrixXad par;
Pit Nestle's avatar
Pit Nestle committed
55

Pit Nestle's avatar
Pit Nestle committed
56 57 58 59 60 61 62 63 64 65
      par.resize(v, 2);

      for (int i = 0; i < v; i++)
      {
          par(i, 0) = _x[i+0];
          par(i, 1) = _x[i+v];
      }

      MatrixXad jacobian;
      jacobian.resize(f, 4);
Pit Nestle's avatar
Pit Nestle committed
66

Pit Nestle's avatar
Pit Nestle committed
67
      jacobian.col(0) = this->dx * par.col(0);
Pit Nestle's avatar
Pit Nestle committed
68
      jacobian.col(1) = this->dy * par.col(0);
Pit Nestle's avatar
Pit Nestle committed
69
      jacobian.col(2) = this->dx * par.col(1);
Pit Nestle's avatar
Pit Nestle committed
70
      jacobian.col(3) = this->dy * par.col(1);
Pit Nestle's avatar
Pit Nestle committed
71 72 73 74


      for(int i = 0; i < f; i++)
      {
Pit Nestle's avatar
Pit Nestle committed
75 76 77 78 79 80 81 82 83 84 85 86 87 88
          adouble j00 = jacobian(i, 0);
          adouble j01 = jacobian(i, 1);
          adouble j10 = jacobian(i, 2);
          adouble j11 = jacobian(i, 3);



          //std::cout << "\n ########## Jacobian ##########" << std::endl;


          adouble j_n = j00*j00
                       +j01*j01
                       +j10*j10
                       +j11*j11;
Pit Nestle's avatar
Pit Nestle committed
89

Pit Nestle's avatar
Pit Nestle committed
90 91 92
          adouble det = j00*j11 - j01*j10; // a*d - b*c

          if (det.value() <= 0)
Pit Nestle's avatar
Pit Nestle committed
93 94 95 96
          {
              return std::numeric_limits<adouble>::infinity();
          }

Pit Nestle's avatar
Pit Nestle committed
97 98 99 100 101 102 103 104 105
          j00 /= det;
          j01 /= det;
          j10 /= det;
          j11 /= det;

          adouble ji_n = j00*j00
                           +j01*j01
                           +j10*j10
                           +j11*j11;
Pit Nestle's avatar
Pit Nestle committed
106

Pit Nestle's avatar
Pit Nestle committed
107 108
          res += area(i) * ((j_n+ji_n) - 4);
          //std::printf("i=%d, j_norm=%f, j⁻1_norm=%f, res=%f\n", i, j_n.value(), ji_n.value(), res.value());
Pit Nestle's avatar
Pit Nestle committed
109 110
      }

Pit Nestle's avatar
Pit Nestle committed
111 112 113 114 115 116 117 118 119 120 121 122
      double lambda = 1e-4;
      for (int i = 0; i < v; i++)
      {
          res += ((_x[i]-this->par(i, 0))*(_x[i]-this->par(i, 0)))*lambda;
          res += ((_x[i+v]-this->par(i, 1))*(_x[i+v]-this->par(i, 1)))*lambda;
      }
      for (int i = 0; i < v; i++)
      {
          this->par(i, 0) = _x[i+0];
          this->par(i, 1) = _x[i+v];
      }
      return res;
Pit Nestle's avatar
Pit Nestle committed
123 124 125 126 127 128 129 130 131
  }



  virtual void store_result(const double* _x)
  {
      int v = n_vertices();
      res.resize(v, 2);

Pit Nestle's avatar
Pit Nestle committed
132
//      std::cout << "\n ########## RES ##########\n" << std::endl;
Pit Nestle's avatar
Pit Nestle committed
133 134
      for (int i = 0; i < v; i++)
      {
Pit Nestle's avatar
Pit Nestle committed
135
//          printf(" %f \t %f \n", _x[i+0], _x[i+v]);
Pit Nestle's avatar
Pit Nestle committed
136 137 138 139 140 141 142 143
          res(i, 0) = _x[i+0];
          res(i, 1) = _x[i+v];
      }
  }

  virtual bool constant_gradient() const { return false; }
  virtual bool constant_hessian()  const { return false; }
  virtual bool sparse_gradient() { return false;}
Pit Nestle's avatar
Pit Nestle committed
144
  virtual bool sparse_hessian () { return true;}
Pit Nestle's avatar
Pit Nestle committed
145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167

  void init()
  {
      if (iteration == 0) // otherwise everthing should be set already
      {
          size_t f = n_faces();
          size_t v = n_vertices();

          this->ref.resize(v, 3);
          this->par.resize(v, 2);
          this->fac.resize(f, 3);

          this->w.resize(f, 4);

          this->dx.resize(f, v);
          this->dy.resize(f, v);

          this->sv.resize(f, 2);

          this->lambda.resize(f, 4);

          this->area.resize(f);

Pit Nestle's avatar
Pit Nestle committed
168 169 170
//          std::cout << "V : " << v << " | F : " << f << std::endl;
//          std::cout << "\n ########## Initial Param ##########" << std::endl;

Pit Nestle's avatar
Pit Nestle committed
171 172
          for (size_t i = 0; i < v; i++)
          {
Pit Nestle's avatar
Pit Nestle committed
173 174 175 176 177
              double x, y, z;
              x = this->i_reference_mesh_vertices[i][0];
              y = this->i_reference_mesh_vertices[i][1];
              z = this->i_reference_mesh_vertices[i][2];
              this->ref.row(i) << x, y, z;
Pit Nestle's avatar
Pit Nestle committed
178 179 180 181 182

              adouble u, v;
              u = this->i_param_mesh_vertices[i][0];
              v = this->i_param_mesh_vertices[i][1];
              this->par.row(i) << u, v;
Pit Nestle's avatar
Pit Nestle committed
183 184

//              std::cout << par(i, 0).value() << " | " << par(i, 1).value() << std::endl;
Pit Nestle's avatar
Pit Nestle committed
185 186 187 188 189
          }

          //Local Transformation and FE Gradient Matrices
          std::vector<Eigen::Triplet<adouble>> _dx;
          std::vector<Eigen::Triplet<adouble>> _dy;
Pit Nestle's avatar
Pit Nestle committed
190 191 192 193


//          std::cout << "\n ########## Local Calculation ##########" << std::endl;

Pit Nestle's avatar
Pit Nestle committed
194 195 196 197 198 199 200 201 202 203 204
          for (size_t i = 0; i < f; i++)
          {
              int a, b, c;
              a = this->i_faces[i][0];
              b = this->i_faces[i][1];
              c = this->i_faces[i][2];

              this->fac.row(i) << a, b, c;

              //FE Gradient Matrices

Pit Nestle's avatar
Pit Nestle committed
205

Pit Nestle's avatar
Pit Nestle committed
206
              //Triangle Edge Vectors
Pit Nestle's avatar
Pit Nestle committed
207 208 209
              Eigen::Vector3d ab = (this->ref.row(b) - this->ref.row(a)).transpose();
              Eigen::Vector3d ac = (this->ref.row(c) - this->ref.row(a)).transpose();

Pit Nestle's avatar
Pit Nestle committed
210 211

              //Length of edges
Pit Nestle's avatar
Pit Nestle committed
212 213
              double ab_n = ab.norm();
              double ac_n = ac.norm();
Pit Nestle's avatar
Pit Nestle committed
214 215

              //Angle between edges
Pit Nestle's avatar
Pit Nestle committed
216
              double angle = std::acos(((ab.dot(ac)) / (ab_n * ac_n)));
Pit Nestle's avatar
Pit Nestle committed
217 218

              //local vector ab
Pit Nestle's avatar
Pit Nestle committed
219 220
              double ab_x = ab_n;
              double ab_y = 0;
Pit Nestle's avatar
Pit Nestle committed
221 222

              //local vector ac
Pit Nestle's avatar
Pit Nestle committed
223 224
              double ac_x = std::cos(angle) * ac_n;
              double ac_y = std::sin(angle) * ac_n;
Pit Nestle's avatar
Pit Nestle committed
225

Pit Nestle's avatar
Pit Nestle committed
226
              double tmp_area = 0.5 * ab_x * ac_y;
Pit Nestle's avatar
Pit Nestle committed
227 228 229 230 231 232 233 234

//              assert(tmp_area > 0.0);

              if (tmp_area <= 0.0)
                  std::cerr << "Error: area is not positive: " << tmp_area << std::endl;

              area(i) = tmp_area; //

Pit Nestle's avatar
Pit Nestle committed
235 236 237 238 239 240
              double p_0x = 0; //p_0
              double p_0y = 0;
              double p_1x = ab_x; //p_1
              double p_1y = ab_y;
              double p_2x = ac_x; //p_2
              double p_2y = ac_y;
Pit Nestle's avatar
Pit Nestle committed
241 242 243 244 245 246 247 248 249


              _dx.push_back(Eigen::Triplet<adouble>(i, a, (p_1y - p_2y)*(1/(2*area(i)))));
              _dx.push_back(Eigen::Triplet<adouble>(i, b, (p_2y - p_0y)*(1/(2*area(i)))));
              _dx.push_back(Eigen::Triplet<adouble>(i, c, (p_0y - p_1y)*(1/(2*area(i)))));

              _dy.push_back(Eigen::Triplet<adouble>(i, a, (p_2x - p_1x)*(1/(2*area(i)))));
              _dy.push_back(Eigen::Triplet<adouble>(i, b, (p_0x - p_2x)*(1/(2*area(i)))));
              _dy.push_back(Eigen::Triplet<adouble>(i, c, (p_1x - p_0x)*(1/(2*area(i)))));
Pit Nestle's avatar
Pit Nestle committed
250 251 252 253 254 255 256 257


//              std::cout << "\n Triangle Edge Vectors" << std::endl;
//              std::cout << "ab : ( " << ab(0) << " | " << ab(1) << " ), ac : ( " << ac(0) << " | " << ac(1) << " )" << std::endl;


//              std::cout << "\n Triangle Edge Length" << std::endl;
//              std::cout << "ab : " << ab_n << ", ac : " << ac_n << std::endl;
Pit Nestle's avatar
Pit Nestle committed
258 259
          }

Pit Nestle's avatar
Pit Nestle committed
260 261


Pit Nestle's avatar
Pit Nestle committed
262 263
          this->dx.setFromTriplets(_dx.begin(), _dx.end());
          this->dy.setFromTriplets(_dy.begin(), _dy.end());
Pit Nestle's avatar
Pit Nestle committed
264 265
      }

Pit Nestle's avatar
Pit Nestle committed
266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341
      iteration++;
  }

  int n_faces()
  {
      return this->i_faces.size();
  }

  int n_vertices()
  {
      return this->i_reference_mesh_vertices.size();
  }

  void addReferenceVertex(double _x, double _y, double _z)
  {
      this->i_reference_mesh_vertices.push_back({_x, _y, _z});
  }

  void addReferenceVertex(double _x, double _y)
  {
      this->i_reference_mesh_vertices.push_back({_x, _y, 0});
  }


  void addParamVertex(double _x, double _y)
  {
      this->i_param_mesh_vertices.push_back({_x, _y});
      this->param_vertex_count++;
  }

  void addFace(int _a, int _b, int _c)
  {
      this->i_faces.push_back({_a, _b, _c});
  }

private:
  //== INPUT ====================================================================
    //Base Mesh
    std::vector<Eigen::Vector3d> i_reference_mesh_vertices;
    int mesh_vertex_count;

    //Parametrization Mesh
    std::vector<Eigen::Vector2d> i_param_mesh_vertices;
    int param_vertex_count;

    //Triangles
    std::vector<Eigen::Vector3i> i_faces;

    //Result
    bool resultAvailable;

    int iteration;




  public:
  //== COMPUTE ===================================================================
    Eigen::MatrixXd ref; //Reference Mesh,    #V x 2
    MatrixXad       par; //Parametrization,   #V x 2
    Eigen::MatrixXi fac;    //Faces,             #F x 3
    Eigen::MatrixXd res;

    Eigen::MatrixXd w; //Weight Matrix, colums represent diagonal of W_ii, (w11 | w12 | w21 | w22) ,#F x 4

    Eigen::SparseMatrix<adouble> dx; //FE Gradient Matrices, #F x #V
    Eigen::SparseMatrix<adouble> dy;

    Eigen::MatrixXd lambda; //Rotation depending on energy, #F x 4

    Eigen::MatrixXd sv; //Singular Values of the jacobians, #F x 2

    Eigen::Matrix<adouble, Eigen::Dynamic, 1> area;
};
#endif
#endif