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 <cmath>
26
#include <iostream>
27
#endif
28

29
#include "MeshFlatteningNurbs.h"
30

31

32
// clang-format off
33
namespace nurbs{
34

35
double divide(double a, double b)
36
{
37
    if (fabs(b) < 10e-14)
38
        return 0;
39
    else
40
        return a / b;
41
}
42

43
Eigen::VectorXd NurbsBase1D::getKnotSequence(double u_min, double u_max, int u_deg, int num_u_poles)
44
{
45
    // boarder poles are on the surface
46
    std::vector<double> u_knots;
47
    for (int i=0; i < u_deg; i++)
48
        u_knots.push_back(u_min);
49
    for (int i=0; i < num_u_poles; i++)
50
        u_knots.push_back(u_min + (u_max - u_min) * i / (num_u_poles - 1));
51
    for (int i=0; i < u_deg; i++)
52
        u_knots.push_back(u_max);
53
    return Eigen::Map<Eigen::VectorXd>(u_knots.data(), u_knots.size());
54
}
55

56
Eigen::VectorXd NurbsBase1D::getWeightList(Eigen::VectorXd knots, int u_deg)
57
{
58
    Eigen::VectorXd weights;
59
    weights.resize(knots.rows() - u_deg - 1);
60
    weights.setOnes();
61
    return weights;
62
}
63

64

65
// DE BOOR ALGORITHM FROM OPENGLIDER
66
std::function<double(double)> get_basis(int degree, int i, Eigen::VectorXd knots)
67
    // Return a basis_function for the given degree """
68
{
69
    if (degree == 0)
70
    {
71
        return [degree, i, knots](double t)
72
        {
73
            // The basis function for degree = 0 as per eq. 7
74
            (void)degree;
75
            double t_this = knots[i];
76
            double t_next = knots[i+1];
77
            if (t == knots[0])
78
                return (double)(t_next >= t && t >= t_this);
79
            return (double)(t_next >= t && t > t_this);
80
        };
81
    }
82
    else
83
    {
84
        return [degree, i, knots](double t)
85
        {
86
            // The basis function for degree > 0 as per eq. 8
87
            if (t < knots[0])
88
                return get_basis(degree, i, knots)(knots[0]);
89
            else if (t > knots[knots.rows() - 1])
90
                return get_basis(degree, i, knots)(knots[knots.rows() - 1]);
91
            double out = 0.;
92
            double t_this = knots[i];
93
            double t_next = knots[i + 1];
94
            double t_precog  = knots[i + degree];
95
            double t_horizon = knots[i + degree + 1];
96
            if (t_this == t_horizon)
97
                return 0.;
98

99
            double bottom = (t_precog - t_this);
100
            out = divide(t - t_this, bottom) * get_basis(degree - 1, i, knots)(t);
101

102
            bottom = (t_horizon - t_next);
103
            out += divide(t_horizon - t, bottom) * get_basis(degree - 1, i + 1, knots)(t);
104
            return out;
105
        };
106
    }
107
}
108

109

110
std::function<double(double)> get_basis_derivative(int order, int degree, int i, Eigen::VectorXd knots)
111
    // Return the derivation of the basis function
112
    // order of basis function
113
    // degree of basis function
114
    //
115
    // knots sequence
116
{
117
    if (order == 1)
118
    {
119
        return [degree, i, knots, order](double t)
120
        {
121
            (void)order;
122
            double out = 0;
123
            if (!(knots[i + degree] - knots[i] == 0))
124
            {
125
                out +=  get_basis(degree - 1, i, knots)(t) *
126
                        degree / (knots[i + degree] - knots[i]);
127
            }
128
            if (!(knots[i + degree + 1] - knots[i + 1] == 0))
129
            {
130
                out -=  get_basis(degree - 1, i + 1, knots)(t) *
131
                        degree / (knots[i + degree + 1] - knots[i + 1]);
132
            }
133
            return out;
134
        };
135
    }
136
    else
137
    {
138
        return [degree, i, knots, order](double t)
139
        {
140
            double out = 0;
141
            if (!(knots[i + degree] - knots[i] == 0))
142
            {
143
                out +=  get_basis_derivative(order - 1, degree - 1, i, knots)(t) *
144
                        degree / (knots[i + degree] - knots[i]);
145
            }
146
            if (!(knots[i + degree + 1] - knots[i + 1] == 0))
147
            {
148
                out -=  get_basis_derivative(order - 1, degree - 1, i + 1, knots)(t) *
149
                        degree / (knots[i + degree + 1] - knots[i + 1]);
150
            }
151
            return out;
152
        };
153
    }
154
}
155

156

157
NurbsBase2D::NurbsBase2D(Eigen::VectorXd u_knots, Eigen::VectorXd v_knots,
158
                     Eigen::VectorXd weights,
159
                     int degree_u, int degree_v)
160
{
161
    // assert(weights.size() == u_knots.size() * v_knots.size());
162
    this->u_knots = u_knots;
163
    this->v_knots = v_knots;
164
    this->weights = weights;
165
    this->degree_u = degree_u;
166
    this->degree_v = degree_v;
167
    for (int u_i = 0; u_i < u_knots.size() - degree_u - 1; u_i ++)
168
        this->u_functions.push_back(get_basis(degree_u, u_i, u_knots));
169
    for (int v_i = 0; v_i < v_knots.size() - degree_v - 1; v_i ++)
170
        this->v_functions.push_back(get_basis(degree_v, v_i, v_knots));
171
}
172

173
Eigen::VectorXd NurbsBase2D::getInfluenceVector(Eigen::Vector2d u)
174
{
175
    Eigen::VectorXd n_u, n_v;
176
    double sum_weights = 0;
177
    Eigen::VectorXd infl(this->u_functions.size() * this->v_functions.size());
178
    int i = 0;
179

180
    n_u.resize(this->u_functions.size());
181
    n_v.resize(this->v_functions.size());
182

183
    for (unsigned int i = 0; i < this->u_functions.size(); i ++)
184
        n_u[i] = this->u_functions[i](u.x());
185
    for (unsigned int i = 0; i < this->v_functions.size(); i ++)
186
        n_v[i] = this->v_functions[i](u.y());
187

188
    for (unsigned int u_i = 0; u_i < this->u_functions.size(); u_i++)
189
    {
190
        for (unsigned int v_i = 0; v_i < this->v_functions.size(); v_i++)
191
        {
192
            sum_weights += weights[i] * n_u[u_i] * n_v[v_i];
193
            infl[i] = weights[i] * n_u[u_i] * n_v[v_i];
194
            i ++;
195
        }
196
    }
197
    return infl / sum_weights;
198
}
199

200
void add_triplets(Eigen::VectorXd values, double row, std::vector<trip> &triplets)
201
{
202
    for (unsigned int i=0; i < values.size(); i++)
203
    {
204
        if (values(i) != 0.) {
205
            triplets.emplace_back(trip(row, i, values(i)));
206
        }
207
    }
208
}
209

210
spMat NurbsBase2D::getInfluenceMatrix(Eigen::Matrix<double, Eigen::Dynamic, 2> U)
211
{
212
    std::vector<trip> triplets;
213
    for (unsigned int row_index = 0; row_index < U.rows(); row_index++)
214
        add_triplets(this->getInfluenceVector(U.row(row_index)), row_index, triplets);
215
    spMat mat(U.rows(), this->u_functions.size() * this->v_functions.size());
216
    mat.setFromTriplets(triplets.begin(), triplets.end());
217
    return mat;
218
}
219

220
void NurbsBase2D::computeFirstDerivatives()
221
{
222
    for (unsigned int u_i = 0; u_i < u_functions.size(); u_i ++)
223
        this->Du_functions.push_back(get_basis_derivative(1, this->degree_u, u_i, this->u_knots));
224
    for (unsigned int v_i = 0; v_i < v_functions.size(); v_i ++)
225
        this->Dv_functions.push_back(get_basis_derivative(1, this->degree_v, v_i, this->v_knots));
226
}
227

228
void NurbsBase2D::computeSecondDerivatives()
229
{
230
    for (unsigned int u_i = 0; u_i < u_functions.size(); u_i ++)
231
        this->DDu_functions.push_back(get_basis_derivative(2, this->degree_u, u_i, this->u_knots));
232
    for (unsigned int v_i = 0; v_i < v_functions.size(); v_i ++)
233
        this->DDv_functions.push_back(get_basis_derivative(2, this->degree_v, v_i, this->v_knots));
234
}
235

236
Eigen::VectorXd NurbsBase2D::getDuVector(Eigen::Vector2d u)
237
{
238
    Eigen::VectorXd A1, A2;
239
    double C1, C2;
240
    A1.resize(this->u_functions.size() * this->v_functions.size());
241
    A2.resize(this->u_functions.size() * this->v_functions.size());
242
    double A3 = 0;
243
    double A5 = 0;
244
    int i = 0;
245
    Eigen::VectorXd n_u, n_v, Dn_u;
246
    n_u.resize(this->u_functions.size());
247
    Dn_u.resize(this->u_functions.size());
248
    n_v.resize(this->v_functions.size());
249
    for (unsigned int u_i=0; u_i < this->u_functions.size(); u_i++)
250
    {
251
        // std::cout << "u_i: " << u_i << " , n_u: " << n_u.size()
252
        //           << " , Dn_u: " << Dn_u.size() << std::endl;
253
        n_u[u_i] = this->u_functions[u_i](u.x());
254
        Dn_u[u_i] = this->Du_functions[u_i](u.x());
255
    }
256
    for (unsigned int v_i=0; v_i < this->v_functions.size(); v_i++)
257
    {
258
        n_v[v_i] = this->v_functions[v_i](u.y());
259
        // std::cout << v_i << std::endl;
260
    }
261

262
    for (unsigned int u_i=0; u_i < this->u_functions.size(); u_i++)
263
    {
264
        for (unsigned int v_i=0; v_i < this->v_functions.size(); v_i++)
265
        {
266
            C1 = weights[i] * Dn_u[u_i] * n_v[v_i];
267
            C2 = weights[i] * n_u[u_i] * n_v[v_i];
268
            A1[i] = C1;
269
            A2[i] = C2;
270
            A3 += C2;
271
            A5 += C1;
272
            i ++;
273
        }
274
    }
275
    return (A1 * A3 - A2 * A5) / A3 / A3;
276
}
277

278
Eigen::VectorXd NurbsBase2D::getDvVector(Eigen::Vector2d u)
279
{
280
    Eigen::VectorXd A1, A2;
281
    double C1, C2;
282
    A1.resize(this->u_functions.size() * this->v_functions.size());
283
    A2.resize(this->u_functions.size() * this->v_functions.size());
284
    double A3 = 0;
285
    double A5 = 0;
286
    int i = 0;
287
    Eigen::VectorXd n_u, n_v, Dn_v;
288
    n_u.resize(this->u_functions.size());
289
    Dn_v.resize(this->v_functions.size());
290
    n_v.resize(this->v_functions.size());
291
    for (unsigned int u_i=0; u_i < this->u_functions.size(); u_i++)
292
    {
293
        n_u[u_i] = this->u_functions[u_i](u.x());
294
    }
295
    for (unsigned int v_i=0; v_i < this->v_functions.size(); v_i++)
296
    {
297
        n_v[v_i] = this->v_functions[v_i](u.y());
298
        Dn_v[v_i] = this->Dv_functions[v_i](u.y());
299
    }
300

301
    for (unsigned int u_i=0; u_i < this->u_functions.size(); u_i++)
302
    {
303
        for (unsigned int v_i=0; v_i < this->v_functions.size(); v_i++)
304
        {
305
            C1 = weights[i] * Dn_v[v_i] * n_u[u_i];
306
            C2 = weights[i] * n_v[v_i] * n_u[u_i];
307
            A1[i] = C1;
308
            A2[i] = C2;
309
            A3 += C2;
310
            A5 += C1;
311
            i ++;
312
        }
313
    }
314
    return (A1 * A3 - A2 * A5) / A3 / A3;
315
}
316

317

318
spMat NurbsBase2D::getDuMatrix(Eigen::Matrix<double, Eigen::Dynamic, 2> U)
319
{
320
    std::vector<trip> triplets;
321
    for (unsigned int row_index = 0; row_index < U.rows(); row_index++)
322
        add_triplets(this->getDuVector(U.row(row_index)), row_index, triplets);
323
    spMat mat(U.rows(), this->u_functions.size() * this->v_functions.size());
324
    mat.setFromTriplets(triplets.begin(), triplets.end());
325
    return mat;
326
}
327

328
spMat NurbsBase2D::getDvMatrix(Eigen::Matrix<double, Eigen::Dynamic, 2> U)
329
{
330
    std::vector<trip> triplets;
331
    for (unsigned int row_index = 0; row_index < U.rows(); row_index++)
332
        add_triplets(this->getDvVector(U.row(row_index)), row_index, triplets);
333
    spMat mat(U.rows(), this->u_functions.size() * this->v_functions.size());
334
    mat.setFromTriplets(triplets.begin(), triplets.end());
335
    return mat;
336
}
337

338

339
std::tuple<NurbsBase2D, Eigen::MatrixXd> NurbsBase2D::interpolateUBS(
340
    Eigen::Matrix<double, Eigen::Dynamic, 3> poles,
341
    int degree_u,
342
    int degree_v,
343
    int num_u_poles,
344
    int num_v_poles,
345
    int num_u_points,
346
    int num_v_points)
347
{
348
    double u_min = this->u_knots(0);
349
    double u_max = this->u_knots(this->u_knots.size() - 1);
350
    double v_min = this->v_knots(0);
351
    double v_max = this->v_knots(this->v_knots.size() - 1);
352
    Eigen::VectorXd weights, u_knots, v_knots;
353
    u_knots = NurbsBase1D::getKnotSequence(u_min, u_max, degree_u, num_u_poles);
354
    v_knots = NurbsBase1D::getKnotSequence(v_min, v_max, degree_v, num_v_poles);
355

356
    weights.resize((u_knots.rows() - degree_u - 1) * (v_knots.rows() - degree_v - 1));
357
    weights.setOnes();
358
    NurbsBase2D new_base(u_knots, v_knots, weights, degree_u, degree_v);
359
    Eigen::Matrix<double, Eigen::Dynamic, 2> uv_points = this->getUVMesh(num_u_points, num_v_points);
360
    Eigen::Matrix<double, Eigen::Dynamic, 3> xyz_points = this->getInfluenceMatrix(uv_points) * poles;
361
    spMat A = new_base.getInfluenceMatrix(uv_points);
362
    Eigen::LeastSquaresConjugateGradient<spMat > solver;
363
    solver.compute(A);
364
    Eigen::Matrix<double, Eigen::Dynamic, 3> new_poles = solver.solve(xyz_points);
365
    return std::tuple<NurbsBase2D, Eigen::MatrixXd >(new_base, new_poles);
366
}
367

368
Eigen::Matrix<double, Eigen::Dynamic, 2> NurbsBase2D::getUVMesh(int num_u_points, int num_v_points)
369
{
370
    double u_min = this->u_knots(0);
371
    double u_max = this->u_knots(this->u_knots.size() - 1);
372
    double v_min = this->v_knots(0);
373
    double v_max = this->v_knots(this->v_knots.size() - 1);
374
    Eigen::Matrix<double, Eigen::Dynamic, 2> uv_points;
375
    uv_points.resize(num_u_points * num_v_points, 2);
376
    int i = 0;
377
    for (int u = 0; u < num_u_points; u++)
378
    {
379
        for (int v = 0; v < num_v_points; v++)
380
        {
381
            uv_points(i, 0) = u_min + (u_max - u_min) * u / (num_u_points - 1);
382
            uv_points(i, 1) = v_min + (v_max - v_min) * v / (num_v_points - 1);
383
            i++;
384
        }
385

386
    }
387
    return uv_points;
388
}
389

390

391
NurbsBase1D::NurbsBase1D(Eigen::VectorXd u_knots, Eigen::VectorXd weights, int degree_u)
392
{
393
    this->u_knots = u_knots;
394
    this->weights = weights;
395
    this->degree_u = degree_u;
396
    for (int u_i = 0; u_i < u_knots.size() - degree_u - 1; u_i ++)
397
        this->u_functions.push_back(get_basis(degree_u, u_i, u_knots));
398
}
399

400
Eigen::VectorXd NurbsBase1D::getInfluenceVector(double u)
401
{
402
    Eigen::VectorXd n_u;
403
    double sum_weights = 0;
404
    Eigen::VectorXd infl(this->u_functions.size());
405

406
    n_u.resize(this->u_functions.size());
407

408
    for (unsigned int i = 0; i < this->u_functions.size(); i ++)
409
        n_u[i] = this->u_functions[i](u);
410

411
    for (unsigned int u_i = 0; u_i < this->u_functions.size(); u_i++)
412
    {
413
        sum_weights += weights[u_i] * n_u[u_i];
414
        infl[u_i] = weights[u_i] * n_u[u_i];
415
    }
416
    return infl / sum_weights;
417
}
418

419
spMat NurbsBase1D::getInfluenceMatrix(Eigen::VectorXd u)
420
{
421
    std::vector<trip> triplets;
422
    for (unsigned int row_index = 0; row_index < u.size(); row_index++)
423
        add_triplets(this->getInfluenceVector(u[row_index]), row_index, triplets);
424
    spMat mat(u.size(), this->u_functions.size());
425
    mat.setFromTriplets(triplets.begin(), triplets.end());
426
    return mat;
427
}
428

429
void NurbsBase1D::computeFirstDerivatives()
430
{
431
    for (unsigned int u_i = 0; u_i < u_functions.size(); u_i ++)
432
        this->Du_functions.push_back(get_basis_derivative(1, this->degree_u, u_i, this->u_knots));
433
}
434

435
void NurbsBase1D::computeSecondDerivatives()
436
{
437
    for (unsigned int u_i = 0; u_i < u_functions.size(); u_i ++)
438
        this->DDu_functions.push_back(get_basis_derivative(2, this->degree_u, u_i, this->u_knots));
439
}
440

441

442
Eigen::VectorXd NurbsBase1D::getDuVector(double u)
443
{
444
    Eigen::VectorXd A1, A2;
445
    double C1, C2;
446
    double C3 = 0;
447
    double C4 = 0;
448
    int i = 0;
449
    A1.resize(this->u_functions.size());
450
    A2.resize(this->u_functions.size());
451
    Eigen::VectorXd n_u, Dn_u;
452
    n_u.resize(this->u_functions.size());
453
    Dn_u.resize(this->u_functions.size());
454

455
    for (unsigned u_i=0; u_i < this->u_functions.size(); u_i++)
456
    {
457
        n_u[u_i] = this->u_functions[u_i](u);
458
        Dn_u[u_i] = this->Du_functions[u_i](u);
459
    }
460

461
    for (unsigned int u_i=0; u_i < this->Du_functions.size(); u_i++)
462
    {
463
        C1 = weights[i] * Dn_u[u_i];
464
        C2 = weights[i] * n_u[u_i];
465
        C3 += C1;
466
        C4 += C2;
467

468
        A1[i] = C1;
469
        A2[i] = C2;
470
        i ++;
471
    }
472
    return (A1 * C4 - A2 * C3) / C4 / C4 ;
473
}
474

475

476
spMat NurbsBase1D::getDuMatrix(Eigen::VectorXd U)
477
{
478
    std::vector<trip> triplets;
479
    for (unsigned int row_index = 0; row_index < U.size(); row_index++)
480
        add_triplets(this->getDuVector(U[row_index]), row_index, triplets);
481
    spMat mat(U.size(), this->u_functions.size());
482
    mat.setFromTriplets(triplets.begin(), triplets.end());
483
    return mat;
484
}
485

486
std::tuple<NurbsBase1D, Eigen::Matrix<double, Eigen::Dynamic, 3>> NurbsBase1D::interpolateUBS(
487
    Eigen::Matrix<double,
488
    Eigen::Dynamic, 3> poles,
489
    int degree,
490
    int num_poles,
491
    int num_points)
492
{
493
    double u_min = this->u_knots(0);
494
    double u_max = this->u_knots(this->u_knots.size() - 1);
495
    Eigen::VectorXd u_knots, weights;
496
    u_knots = NurbsBase1D::getKnotSequence(u_min, u_max, degree, num_poles);
497
    weights = NurbsBase1D::getWeightList(u_knots, degree);
498
    NurbsBase1D new_base(u_knots, weights, degree);
499
    Eigen::Matrix<double, Eigen::Dynamic, 1> u_points = this->getUMesh(num_points);
500
    Eigen::Matrix<double, Eigen::Dynamic, 3> xyz_points;
501
    xyz_points = this->getInfluenceMatrix(u_points) * poles;
502
    spMat A = new_base.getInfluenceMatrix(u_points);
503
    Eigen::LeastSquaresConjugateGradient<spMat > solver;
504
    solver.compute(A);
505
    Eigen::Matrix<double, Eigen::Dynamic, 3> new_poles = solver.solve(xyz_points);
506
    return std::tuple<NurbsBase1D, Eigen::Matrix<double, Eigen::Dynamic, 3> >(new_base, new_poles);
507
}
508

509
Eigen::VectorXd NurbsBase1D::getUMesh(int num_u_points)
510
{
511
    double u_min = this->u_knots(0);
512
    double u_max = this->u_knots(this->u_knots.size() - 1);
513
    Eigen::Matrix<double, Eigen::Dynamic, 1> u_points;
514
    u_points.setLinSpaced(num_u_points, u_min, u_max);
515
    return u_points;
516
}
517

518

519
}
520
// clang-format on
521