FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2017 Lorenz Lechner                                     *
3
 *                                                                         *
4
 *   This file is part of the FreeCAD CAx development system.              *
5
 *                                                                         *
6
 *   This library is free software; you can redistribute it and/or         *
7
 *   modify it under the terms of the GNU Library General Public           *
8
 *   License as published by the Free Software Foundation; either          *
9
 *   version 2 of the License, or (at your option) any later version.      *
10
 *                                                                         *
11
 *   This library  is distributed in the hope that it will be useful,      *
12
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
13
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
14
 *   GNU Library General Public License for more details.                  *
15
 *                                                                         *
16
 *   You should have received a copy of the GNU Library General Public     *
17
 *   License along with this library; see the file COPYING.LIB. If not,    *
18
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
19
 *   Suite 330, Boston, MA  02111-1307, USA                                *
20
 *                                                                         *
21
 ***************************************************************************/
22

23
#include "PreCompiled.h"
24
#ifndef _PreComp_
25
#include <array>
26
#include <cmath>
27
#include <iostream>
28
#include <map>
29
#include <set>
30
#include <vector>
31
#endif
32

33
#ifndef M_PI
34
#define M_PI 3.14159265358979323846f
35
#endif
36

37
#include <Eigen/SparseCholesky>
38

39
#include "MeshFlatteningLscmRelax.h"
40

41

42
// TODO:
43
// area constrained (scale the final unwrapped mesh to the original area)
44
// FEM approach
45

46
// clang-format off
47
namespace lscmrelax
48
{
49

50
using trip = Eigen::Triplet<double>;
51
using spMat = Eigen::SparseMatrix<double>;
52

53

54

55
ColMat<double, 2> map_to_2D(ColMat<double, 3> points)
56
{
57
    ColMat<double, 4> mat(points.size(), 4);
58
    mat << points, ColMat<double, 1>::Ones(points.size());
59
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(mat, Eigen::ComputeThinU | Eigen::ComputeThinV);
60
    Vector3 n = svd.matrixV().row(2);
61

62
    n.normalize();
63
    Vector3 y(0, 1, 0);
64
    if ((n - y).norm() < 0.0001)
65
        y = Vector3(1, 0, 0);
66
    Vector3 x = n.cross(y);
67
    x.normalize();
68
    y = n.cross(x);
69
    Eigen::Matrix<double, 3, 2> transform;
70
    transform.col(0) = x;
71
    transform.col(1) = y;
72
    return points * transform;
73
}
74

75

76
unsigned int get_max_distance(Vector3 point, RowMat<double, 3> vertices, double & max_dist)
77
{
78
    max_dist = 0;
79
    double dist;
80
    unsigned long max_dist_index = 0;
81
    long j = 0;
82
    for (j=0; j < vertices.cols(); j++)
83
    {
84
        // debugging
85
        dist = (point - vertices.col(j)).norm();
86
        if (dist > max_dist)
87
        {
88
            max_dist = dist;
89
            max_dist_index = j;
90
        }
91
    }
92
    return max_dist_index;
93
}
94

95

96
LscmRelax::LscmRelax(
97
        RowMat<double, 3> vertices,
98
        RowMat<long, 3> triangles,
99
        std::vector<long> fixed_pins)
100
{
101
    this->vertices = vertices;
102
    this->triangles = triangles;
103
    this->flat_vertices.resize(2, this->vertices.cols());
104
    this->fixed_pins = fixed_pins;
105

106
    // set the fixed pins of the flat-mesh:
107
    this->set_fixed_pins();
108

109

110
    unsigned int fixed_count = 0;
111
    for (long i=0; i < this->vertices.cols(); i++)
112
    {
113
        if (fixed_count < this->fixed_pins.size())
114
        {
115
            if (i == this->fixed_pins[fixed_count])
116
                fixed_count ++;
117
            else
118
                this->old_order.push_back(i);
119
        }
120
        else
121
            this->old_order.push_back(i);
122
    }
123

124
    for (auto fixed_index: this->fixed_pins)
125
        this->old_order.push_back(fixed_index);
126

127
    // get the reversed map:
128
    this->new_order.resize(this->old_order.size());
129
    long j = 0;
130
    for (auto index: this->old_order)
131
    {
132
        this->new_order[index] = j;
133
        j++;
134
    }
135

136
    // set the C-mat
137
    this->C << 1,   nue, 0,
138
                nue, 1,  0,
139
                0,   0, (1 - nue) / 2;
140
    this->C *= elasticity / (1 - nue * nue);
141
}
142

143

144

145
//////////////////////////////////////////////////////////////////////////
146
/////////////////                 F.E.M                      /////////////
147
//////////////////////////////////////////////////////////////////////////
148
void LscmRelax::relax(double weight)
149
{
150
    ColMat<double, 3> d_q_l_g = this->q_l_m - this->q_l_g;
151
    // for every triangle
152
    Eigen::Matrix<double, 3, 6> B;
153
    Eigen::Matrix<double, 2, 2> T;
154
    Eigen::Matrix<double, 6, 6> K_m;
155
    Eigen::Matrix<double, 6, 1> u_m, rhs_m;
156
    Eigen::VectorXd rhs(this->vertices.cols() * 2 + 3);
157
    if (this->sol.size() == 0)
158
        this->sol.Zero(this->vertices.cols() * 2 + 3);
159
    spMat K_g(this->vertices.cols() * 2 + 3, this->vertices.cols() * 2 + 3);
160
    std::vector<trip> K_g_triplets;
161
    Vector2 v1, v2, v3, v12, v23, v31;
162
    long row_pos, col_pos;
163
    double A;
164

165
    rhs.setZero();
166

167
    for (long i=0; i<this->triangles.cols(); i++)
168
    {
169
        // 1: construct B-mat in m-system
170
        v1 = this->flat_vertices.col(this->triangles(0, i));
171
        v2 = this->flat_vertices.col(this->triangles(1, i));
172
        v3 = this->flat_vertices.col(this->triangles(2, i));
173
        v12 = v2 - v1;
174
        v23 = v3 - v2;
175
        v31 = v1 - v3;
176
        B << -v23.y(),   0,        -v31.y(),   0,        -v12.y(),   0,
177
              0,         v23.x(),   0,         v31.x(),   0,         v12.x(),
178
             -v23.x(),   v23.y(),  -v31.x(),   v31.y(),  -v12.x(),   v12.y();
179
        T << v12.x(), -v12.y(),
180
             v12.y(), v12.x();
181
        T /= v12.norm();
182
        A = std::abs(this->q_l_m(i, 0) * this->q_l_m(i, 2) / 2);
183
        B /= A * 2; // (2*area)
184

185
        // 2: sigma due dqlg in m-system
186
        u_m << Vector2(0, 0), T * Vector2(d_q_l_g(i, 0), 0), T * Vector2(d_q_l_g(i, 1), d_q_l_g(i, 2));
187

188
        // 3: rhs_m = B.T * C * B * dqlg_m
189
        //    K_m = B.T * C * B
190
        rhs_m = B.transpose() * this->C * B * u_m * A;
191
        K_m = B.transpose() * this->C * B * A;
192

193
        // 5: add to rhs_g, K_g
194
        for (int j=0; j < 3; j++)
195
        {
196
            row_pos = this->triangles(j, i);
197
            rhs[row_pos * 2]     += rhs_m[j * 2];
198
            rhs[row_pos * 2 + 1] += rhs_m[j * 2 +1];
199
            for (int k=0; k < 3; k++)
200
            {
201
                col_pos = this->triangles(k, i);
202
                K_g_triplets.emplace_back(trip(row_pos * 2,     col_pos * 2,        K_m(j * 2,      k * 2)));
203
                K_g_triplets.emplace_back(trip(row_pos * 2 + 1, col_pos * 2,        K_m(j * 2 + 1,  k * 2)));
204
                K_g_triplets.emplace_back(trip(row_pos * 2 + 1, col_pos * 2 + 1,    K_m(j * 2 + 1,  k * 2 + 1)));
205
                K_g_triplets.emplace_back(trip(row_pos * 2,     col_pos * 2 + 1,    K_m(j * 2,      k * 2 + 1)));
206
                // we don't have to fill all because the matrix is symmetric.
207
            }
208
        }
209
    }
210
    // FIXING SOME PINS:
211
    // - if there are no pins (or only one pin) selected solve the system without the nullspace solution.
212
    // - if there are some pins selected, delete all columns, rows that refer to this pins
213
    //          set the diagonal element of these pins to 1 + the rhs to zero
214
    //          (? is it possible to fix in the inner of the face? for sure for fem, but lscm could have some problems)
215
    //          (we also need some extra variables to see if the pins come from user)
216

217
    // fixing some points
218
    // although only internal forces are applied there has to be locked
219
    // at least 3 degrees of freedom to stop the mesh from pure rotation and pure translation
220
    // std::vector<long> fixed_dof;
221
    // fixed_dof.push_back(this->triangles(0, 0) * 2); //x0
222
    // fixed_dof.push_back(this->triangles(0, 0) * 2 + 1); //y0
223
    // fixed_dof.push_back(this->triangles(1, 0) * 2 + 1); // y1
224

225
    // align flat mesh to fixed edge
226
    // Vector2 edge = this->flat_vertices.col(this->triangles(1, 0)) -
227
    //                this->flat_vertices.col(this->triangles(0, 0));
228
    // edge.normalize();
229
    // Eigen::Matrix<double, 2, 2> rot;
230
    // rot << edge.x(), edge.y(), -edge.y(), edge.x();
231
    // this->flat_vertices = rot * this->flat_vertices;
232

233
    // // return true if triplet row / col is in fixed_dof
234
    // auto is_in_fixed_dof = [fixed_dof](const trip & element) -> bool {
235
    //     return (
236
    //         (std::find(fixed_dof.begin(), fixed_dof.end(), element.row()) != fixed_dof.end()) or
237
    //         (std::find(fixed_dof.begin(), fixed_dof.end(), element.col()) != fixed_dof.end()));
238
    // };
239
    // std::cout << "size of triplets: " << K_g_triplets.size() << std::endl;
240
    // K_g_triplets.erase(
241
    //     std::remove_if(K_g_triplets.begin(), K_g_triplets.end(), is_in_fixed_dof),
242
    //     K_g_triplets.end());
243
    // std::cout << "size of triplets: " << K_g_triplets.size() << std::endl;
244
    // for (long fixed: fixed_dof)
245
    // {
246
    //     K_g_triplets.push_back(trip(fixed, fixed, 1.));
247
    //     rhs[fixed] = 0;
248
    // }
249

250
    // for (long i=0; i< this->vertices.cols() * 2; i++)
251
    //     K_g_triplets.push_back(trip(i, i, 0.01));
252

253
    // lagrange multiplier
254
    for (long i=0; i < this->flat_vertices.cols() ; i++)
255
    {
256
        // fixing total ux
257
        K_g_triplets.emplace_back(trip(i * 2, this->flat_vertices.cols() * 2, 1));
258
        K_g_triplets.emplace_back(trip(this->flat_vertices.cols() * 2, i * 2, 1));
259
        // fixing total uy
260
        K_g_triplets.emplace_back(trip(i * 2 + 1, this->flat_vertices.cols() * 2 + 1, 1));
261
        K_g_triplets.emplace_back(trip(this->flat_vertices.cols() * 2 + 1, i * 2 + 1, 1));
262
        // fixing ux*y-uy*x
263
        K_g_triplets.emplace_back(trip(i * 2, this->flat_vertices.cols() * 2 + 2, - this->flat_vertices(1, i)));
264
        K_g_triplets.emplace_back(trip(this->flat_vertices.cols() * 2 + 2, i * 2, - this->flat_vertices(1, i)));
265
        K_g_triplets.emplace_back(trip(i * 2 + 1, this->flat_vertices.cols() * 2 + 2, this->flat_vertices(0, i)));
266
        K_g_triplets.emplace_back(trip(this->flat_vertices.cols() * 2 + 2, i * 2 + 1, this->flat_vertices(0, i)));
267
    }
268

269
    // project out the nullspace solution:
270

271
    // Eigen::VectorXd nullspace1(this->flat_vertices.cols() * 2);
272
    // Eigen::VectorXd nullspace2(this->flat_vertices.cols() * 2);
273
    // nullspace1.setZero();
274
    // nullspace2.setOnes();
275
    // for (long i= 0; i < this->flat_vertices.cols(); i++)
276
    // {
277
    //     nullspace1(i) = 1;
278
    //     nullspace2(i) = 0;
279
    // }
280
    // nullspace1.normalize();
281
    // nullspace2.normalize();
282
    // rhs -= nullspace1.dot(rhs) * nullspace1;
283
    // rhs -= nullspace2.dot(rhs) * nullspace2;
284

285
    K_g.setFromTriplets(K_g_triplets.begin(), K_g_triplets.end());
286
    // rhs +=  K_g * Eigen::VectorXd::Ones(K_g.rows());
287

288
    // solve linear system (privately store the value for guess in next step)
289
    Eigen::SimplicialLDLT<spMat, Eigen::Lower> solver;
290
    solver.compute(K_g);
291
    this->sol = solver.solve(-rhs);
292
    this->set_shift(this->sol.head(this->vertices.cols() * 2) * weight);
293
    this->set_q_l_m();
294
}
295

296

297
void LscmRelax::area_relax(double weight)
298
{
299
//     TODO: doesn't work so far
300
    if (this->sol.size() == 0)
301
        this->sol.Zero(this->vertices.cols());
302
    std::vector<trip> K_g_triplets;
303
    spMat K_g(this->vertices.cols() * 2, this->vertices.cols() * 2);
304
    spMat K_g_lsq(this->triangles.cols(), this->vertices.cols() * 2);
305
    Eigen::VectorXd rhs_lsq(this->triangles.cols());
306
    Eigen::VectorXd rhs(this->vertices.cols() * 2);
307
    rhs.setZero();
308

309
    Eigen::Matrix<double, 1, 6> B;
310
    double delta_a;
311
    Vector2 v1, v2, v3, v12, v23, v31;
312

313

314
    for (long i=0; i<this->triangles.cols(); i++)
315
    {
316
//         1: construct B-mat in m-system
317
        v1 = this->flat_vertices.col(this->triangles(0, i));
318
        v2 = this->flat_vertices.col(this->triangles(1, i));
319
        v3 = this->flat_vertices.col(this->triangles(2, i));
320
        v12 = v2 - v1;
321
        v23 = v3 - v2;
322
        v31 = v1 - v3;
323
        B << -v23.y(), v23.x(), -v31.y(), v31.x(), -v12.y(), v12.x();
324
	delta_a = fabs(this->q_l_g(i, 0) * this->q_l_g(i, 2)) -
325
		  fabs(this->q_l_m(i, 0) * this->q_l_m(i, 2));
326
	rhs_lsq[i] = delta_a * 0.1;
327

328
    std::array<int, 6> range_6 {{0, 1, 2, 3, 4, 5}};
329
	std::array<long, 6> indices;
330
	for (int index=0; index<3; index++)
331
	{
332
	    indices[index * 2] = this->triangles(index, i) * 2;
333
	    indices[index * 2 + 1] = this->triangles(index, i) * 2 + 1;
334
	}
335

336
	for(auto col: range_6)
337
	{
338
        K_g_triplets.emplace_back(trip(i, indices[col], (double) B[col]));
339
	}
340
    }
341
    K_g_lsq.setFromTriplets(K_g_triplets.begin(), K_g_triplets.end());
342
    K_g_triplets.clear();
343

344
    K_g.setFromTriplets(K_g_triplets.begin(), K_g_triplets.end());
345
    Eigen::ConjugateGradient<spMat> solver;
346
    solver.compute(K_g);
347
    this->sol = solver.solve(-rhs);
348
    this->set_shift(this->sol * weight);
349

350
}
351

352
void LscmRelax::edge_relax(double weight)
353
{
354
//  1. get all edges
355
    std::set<std::array<long, 2>> edges;
356
    std::array<long, 2> edge;
357
    for(long i=0; i<this->triangles.cols(); i++)
358
    {
359
        for(int j=0; j<3; j++)
360
        {
361
            long k = j+1;
362
            if (k==3)
363
                k = 0;
364
            if (this->triangles(j, i) < this->triangles(k, i))
365
                edge = std::array<long, 2>{{this->triangles(j, i), this->triangles(k, i)}};
366
            else
367
                edge = std::array<long, 2>{{this->triangles(k, i), this->triangles(j, i)}};
368
            edges.insert(edge);
369
        }
370
    }
371
//  2. create system
372

373
    if (this->sol.size() == 0)
374
        this->sol.Zero(this->vertices.cols());
375

376
    std::vector<trip> K_g_triplets;
377
    spMat K_g(this->vertices.cols() * 2, this->vertices.cols() * 2);
378
    Eigen::VectorXd rhs(this->vertices.cols() * 2);
379
    rhs.setZero();
380
    for(auto edge : edges)
381
    {
382
// 	this goes to the right side
383
	Vector3 v1_g = this->vertices.col(edge[0]);
384
	Vector3 v2_g = this->vertices.col(edge[1]);
385
	Vector2 v1_l = this->flat_vertices.col(edge[0]);
386
	Vector2 v2_l = this->flat_vertices.col(edge[1]);
387
	std::vector<int> range_4 {0, 1, 2, 3};
388
	std::vector<long> indices {edge[0] * 2, edge[0] * 2 + 1, edge[1] * 2, edge[1] * 2 + 1};
389
	double l_g = (v2_g - v1_g).norm();
390
	double l_l = (v2_l - v1_l).norm();
391
	Vector2 t = (v2_l - v1_l);  // direction
392
	t.normalize();
393

394
	Eigen::Matrix<double, 1, 4> B;
395
	Eigen::Matrix<double, 4, 4> K;
396
	Eigen::Matrix<double, 4, 1> rhs_m;
397

398
	B << -t.x(), -t.y(), t.x(), t.y();
399
	K = 1. / l_g * B.transpose() * B;
400
	rhs_m = - B.transpose() * (l_g - l_l);
401

402

403
	for(auto row: range_4)
404
	{
405
	    for (auto col: range_4)
406
	    {
407
        K_g_triplets.emplace_back(trip(indices[row], indices[col], (double) K(row, col)));
408
	    }
409
	    rhs(indices[row]) += rhs_m[row];
410
	}
411
    }
412

413
    K_g.setFromTriplets(K_g_triplets.begin(), K_g_triplets.end());
414
    Eigen::ConjugateGradient<spMat,Eigen::Lower, NullSpaceProjector> solver;
415
    solver.preconditioner().setNullSpace(this->get_nullspace());
416
    solver.compute(K_g);
417
    this->sol = solver.solve(-rhs);
418
    this->set_shift(this->sol * weight);
419
}
420

421

422
//////////////////////////////////////////////////////////////////////////
423
/////////////////                 L.S.C.M                    /////////////
424
//////////////////////////////////////////////////////////////////////////
425
void LscmRelax::lscm()
426
{
427
    this->set_q_l_g();
428
    std::vector<trip> triple_list;
429
    long i;
430
    double x21, x31, y31, x32;
431

432
    // 1. create the triplet list (t * 2, v * 2)
433
    for(i=0; i<this->triangles.cols(); i++)
434
    {
435
        x21 = this->q_l_g(i, 0);
436
        x31 = this->q_l_g(i, 1);
437
        y31 = this->q_l_g(i, 2);
438
        x32 = x31 - x21;
439

440
        triple_list.emplace_back(trip(2 * i, this->new_order[this->triangles(0, i)] * 2, x32));
441
        triple_list.emplace_back(trip(2 * i, this->new_order[this->triangles(0, i)] * 2 + 1, -y31));
442
        triple_list.emplace_back(trip(2 * i, this->new_order[this->triangles(1, i)] * 2, -x31));
443
        triple_list.emplace_back(trip(2 * i, this->new_order[this->triangles(1, i)] * 2 + 1, y31));
444
        triple_list.emplace_back(trip(2 * i, this->new_order[this->triangles(2, i)] * 2, x21));
445

446
        triple_list.emplace_back(trip(2 * i + 1, this->new_order[this->triangles(0, i)] * 2, y31));
447
        triple_list.emplace_back(trip(2 * i + 1, this->new_order[this->triangles(0, i)] * 2 + 1, x32));
448
        triple_list.emplace_back(trip(2 * i + 1, this->new_order[this->triangles(1, i)] * 2, -y31));
449
        triple_list.emplace_back(trip(2 * i + 1, this->new_order[this->triangles(1, i)] * 2 + 1, -x31));
450
        triple_list.emplace_back(trip(2 * i + 1, this->new_order[this->triangles(2, i)] * 2 + 1, x21));
451

452
    }
453
    // 2. divide the triplets in matrix(unknown part) and rhs(known part) and reset the position
454
    std::vector<trip> rhs_triplets;
455
    std::vector<trip> mat_triplets;
456
    for (auto triplet: triple_list)
457
    {
458
        if (triplet.col() > static_cast<int>((this->vertices.cols() - this->fixed_pins.size()) * 2 - 1))
459
            rhs_triplets.push_back(triplet);
460
        else
461
            mat_triplets.push_back(triplet);
462
    }
463

464
    // 3. create a rhs_pos vector
465
    Eigen::VectorXd rhs_pos(this->vertices.cols() * 2);
466
    rhs_pos.setZero();
467
    for (auto index: this->fixed_pins)
468
    {
469
        rhs_pos[this->new_order[index] * 2] = this->flat_vertices(0, index);      //TODO: not yet set
470
        rhs_pos[this->new_order[index] * 2 + 1] = this->flat_vertices(1, index);
471
    }
472

473
    // 4. fill a sparse matrix and calculdate the rhs
474
    Eigen::VectorXd rhs(this->triangles.cols() * 2); // maybe use a sparse vector
475
    spMat B(this->triangles.cols() * 2, this->vertices.cols() * 2);
476
    B.setFromTriplets(rhs_triplets.begin(), rhs_triplets.end());
477
    rhs = B * rhs_pos;
478

479
    // 5. create the lhs matrix
480
    spMat A(this->triangles.cols() * 2, (this->vertices.cols() - this->fixed_pins.size()) * 2);
481
    A.setFromTriplets(mat_triplets.begin(), mat_triplets.end());
482

483
    // 6. solve the system and set the flatted coordinates
484
    // Eigen::SparseQR<spMat, Eigen::COLAMDOrdering<int> > solver;
485
    Eigen::LeastSquaresConjugateGradient<spMat > solver;
486
    Eigen::VectorXd sol(this->vertices.size() * 2);
487
    solver.compute(A);
488
    sol = solver.solve(-rhs);
489

490
    // TODO: create function, is needed also in the fem step
491
    this->set_position(sol);
492
    this->set_q_l_m();
493
    this->transform(true);
494
    // this->rotate_by_min_bound_area();
495
    this->set_q_l_m();
496

497
}
498

499
void LscmRelax::set_q_l_g()
500
{
501
    // get the coordinates of a triangle in local coordinates from the 3d mesh
502
    // x1, y1, y2 = 0
503
    // -> vector<x2, x3, y3>
504
    this->q_l_g.resize(this->triangles.cols(), 3);
505
    for (long i = 0; i < this->triangles.cols(); i++)
506
    {
507
        Vector3 r1 = this->vertices.col(this->triangles(0, i));
508
        Vector3 r2 = this->vertices.col(this->triangles(1, i));
509
        Vector3 r3 = this->vertices.col(this->triangles(2, i));
510
        Vector3 r21 = r2 - r1;
511
        Vector3 r31 = r3 - r1;
512
        double r21_norm = r21.norm();
513
        r21.normalize();
514
        // if triangle is fliped this gives wrong results?
515
        this->q_l_g.row(i) << r21_norm, r31.dot(r21), r31.cross(r21).norm();
516
    }
517
}
518

519
void LscmRelax::set_q_l_m()
520
{
521
    // get the coordinates of a triangle in local coordinates from the 2d map
522
    // x1, y1, y2 = 0
523
    // -> vector<x2, x3, y3>
524
    this->q_l_m.resize(this->triangles.cols(), 3);
525
    for (long i = 0; i < this->triangles.cols(); i++)
526
    {
527
        Vector2 r1 = this->flat_vertices.col(this->triangles(0, i));
528
        Vector2 r2 = this->flat_vertices.col(this->triangles(1, i));
529
        Vector2 r3 = this->flat_vertices.col(this->triangles(2, i));
530
        Vector2 r21 = r2 - r1;
531
        Vector2 r31 = r3 - r1;
532
        double r21_norm = r21.norm();
533
        r21.normalize();
534
        // if triangle is fliped this gives wrong results!
535
        this->q_l_m.row(i) << r21_norm, r31.dot(r21), -(r31.x() * r21.y() - r31.y() * r21.x());
536
    }
537
}
538

539
void LscmRelax::set_fixed_pins()
540
{
541
    // if less then one fixed pin is set find two by an automated algorithm and align them to y = 0
542
    // if more then two pins are chosen find a leastsquare-plane and project the points on it
543
    // insert the points in the flat-vertices vector
544
    if (this->fixed_pins.size() == 0)
545
        this->fixed_pins.push_back(0);
546
    if (this->fixed_pins.size() == 1)
547
    {
548

549
        double dist;
550
        this->fixed_pins.push_back(get_max_distance(this->vertices.col(this->fixed_pins[0]), this->vertices, dist));
551
        this->flat_vertices.col(this->fixed_pins[0]) = Vector2(0, 0);
552
        this->flat_vertices.col(this->fixed_pins[1]) = Vector2(dist, 0);
553
    }
554
    std::sort(this->fixed_pins.begin(), this->fixed_pins.end());
555
    // not yet working
556
    // if (this->fixed_pins.size() > 2)
557
    // {
558
    //     std::vector<Vector3> fixed_3d_points;
559
    //     for (unsigned int index: this->fixed_pins)
560
    //         fixed_3d_points.push_back(this->vertices[index]);
561
    //     std::vector<Vector2> maped_points = map_to_2D(fixed_3d_points);
562
    //     unsigned int i = 0;
563
    //     for (unsigned int index: this->fixed_pins)
564
    //     {
565
    //         this->flat_vertices[i] = maped_points[i];
566
    //         i++;
567
    //     }
568
    // }
569
}
570

571

572
ColMat<double, 3> LscmRelax::get_flat_vertices_3D()
573
{
574
    ColMat<double, 2> mat = this->flat_vertices.transpose();
575
    ColMat<double, 3> mat3d(mat.rows(), 3);
576
    mat3d << mat, ColMat<double, 1>::Zero(mat.rows());
577
    return mat3d;
578
}
579

580
void LscmRelax::set_position(Eigen::VectorXd sol)
581
{
582
    for (long i=0; i < this->vertices.size(); i++)
583
    {
584
        if (sol.size() > i * 2 + 1)
585
            this->flat_vertices.col(this->old_order[i]) << sol[i * 2], sol[i * 2 + 1];
586
    }
587
}
588

589
void LscmRelax::set_shift(Eigen::VectorXd sol)
590
{
591
    for (long i=0; i < this->vertices.size(); i++)
592
    {
593
        if (sol.size() > i * 2 + 1)
594
            this->flat_vertices.col(i) += Vector2(sol[i * 2], sol[i * 2 + 1]);
595
    }
596
}
597

598
double LscmRelax::get_area()
599
{
600
    double area = 0;
601
    for(long i = 0; i < this->triangles.cols(); i++)
602
    {
603
        area += this->q_l_g(i, 0) * this->q_l_g(i, 2);
604
    }
605
    return area / 2;
606
}
607

608
double LscmRelax::get_flat_area()
609
{
610
    double area = 0;
611
    for(long i = 0; i < this->triangles.cols(); i++)
612
    {
613
        area += this->q_l_m(i, 0) * this->q_l_m(i, 2);
614
    }
615
    return area / 2;
616
}
617

618
void LscmRelax::transform(bool scale)
619
{
620
    // assuming q_l_m and flat_vertices are set
621
    Vector2 weighted_center, center;
622
    weighted_center.setZero();
623
    double flat_area = 0;
624
    double global_area = 0;
625
    double element_area;
626
    for(long i = 0; i < this->triangles.cols(); i++)
627
    {
628
        global_area += this->q_l_g(i, 0) * this->q_l_g(i, 2) / 2;
629
        element_area = this->q_l_m(i, 0) * this->q_l_m(i, 2) / 2;
630
        for (int j=0; j < 3; j++)
631
            weighted_center += this->flat_vertices.col(this->triangles(j, i)) * element_area / 3;
632
        flat_area += element_area;
633
    }
634
    center = weighted_center / flat_area;
635
    for (long i = 0; i < this->flat_vertices.cols(); i++)
636
        this->flat_vertices.col(i) -= center;
637
    if (scale)
638
        this->flat_vertices *= std::pow(global_area / flat_area, 0.5);
639
    this->set_q_l_m();
640
}
641

642
void LscmRelax::rotate_by_min_bound_area()
643
{
644
    int n = 100;
645
    double phi;
646
    double min_phi = 0;
647
    double  min_area = 0;
648
    bool x_dominant = false;
649
    // rotate vector by 90 degree and find min area
650
    for (int i = 0; i < n + 1; i++ )
651
    {
652
        phi = i * M_PI / n;
653
        Eigen::VectorXd x_proj = this->flat_vertices.transpose() * Vector2(std::cos(phi), std::sin(phi));
654
        Eigen::VectorXd y_proj = this->flat_vertices.transpose() * Vector2(-std::sin(phi), std::cos(phi));
655
        double x_distance = x_proj.maxCoeff() - x_proj.minCoeff();
656
        double y_distance = y_proj.maxCoeff() - y_proj.minCoeff();
657
        double area = x_distance * y_distance;
658
        if (min_area == 0 || area < min_area)
659
        {
660
            min_area = area;
661
            min_phi = phi;
662
            x_dominant = x_distance > y_distance;
663
        }
664
    }
665
    Eigen::Matrix<double, 2, 2> rot;
666
    min_phi += x_dominant * M_PI / 2;
667
    rot << std::cos(min_phi), std::sin(min_phi), -std::sin(min_phi), std::cos(min_phi);
668
    this->flat_vertices = rot * this->flat_vertices;
669
}
670

671
std::vector<long> LscmRelax::get_fem_fixed_pins()
672
{
673
    long min_x_index = 0;
674
    double min_x = this->vertices(0, 0);
675
    for (long i=0; i<this->flat_vertices.cols(); i++)
676
    {
677
        // get index with min x
678
        if (this->flat_vertices(0, i) < min_x)
679
        {
680
            min_x = this->flat_vertices(0, i);
681
            min_x_index = i;
682
        }
683
    }
684
    double min_y = this->flat_vertices(1, min_x_index);
685
    long max_x_index = 0;
686
    double max_x = 0;
687
    for (long i=0; i<this->flat_vertices.cols(); i++)
688
    {
689
        // get index with min x
690
        double d_x = (this->flat_vertices(0, i) - min_x);
691
        double d_y = (this->flat_vertices(1, i) - min_y);
692
        if (d_x * d_x - d_y * d_y > max_x)
693
        {
694
            max_x = d_x * d_x - d_y * d_y;
695
            max_x_index = i;
696
        }
697
    }
698
    return std::vector<long>{min_x_index * 2, min_x_index * 2 + 1, max_x_index * 2 + 1};
699
}
700

701
Eigen::MatrixXd LscmRelax::get_nullspace()
702
{
703
    Eigen::MatrixXd null_space;
704
    null_space.setZero(this->flat_vertices.size() * 2, 3);
705

706
    for (int i=0; i<this->flat_vertices.cols(); i++)
707
    {
708
        null_space(i * 2, 0) =  1;  // x-translation
709
        null_space(i * 2 + 1, 1) =  1;  // y-translation
710
        null_space(i * 2, 2) =  -  this->flat_vertices(1, i);  // z-rotation
711
        null_space(i * 2 + 1, 2) = this->flat_vertices(0, i);  // z-rotation
712

713
    }
714
    return null_space;
715
}
716

717

718
}
719
// clang-format on
720