FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2005 Imetric 3D GmbH                                    *
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

25
#ifndef _PreComp_
26
#include <algorithm>
27
#include <cstdlib>
28
#include <iterator>
29
#endif
30

31
#include <Base/BoundBox.h>
32
#include <Base/Console.h>
33
#include <Mod/Mesh/App/WildMagic4/Wm4ApprPolyFit3.h>
34
#include <Mod/Mesh/App/WildMagic4/Wm4ApprQuadraticFit3.h>
35
#include <Mod/Mesh/App/WildMagic4/Wm4ApprSphereFit3.h>
36
#include <boost/math/special_functions/fpclassify.hpp>
37

38
// #define FC_USE_EIGEN
39
#include <Eigen/Eigen>
40
#include <Eigen/QR>
41
#ifdef FC_USE_EIGEN
42
#include <Eigen/Eigenvalues>
43
#endif
44

45
#include "Approximation.h"
46
#include "CylinderFit.h"
47
#include "Elements.h"
48
#include "SphereFit.h"
49
#include "Utilities.h"
50

51

52
using namespace MeshCore;
53

54
Approximation::Approximation() = default;
55

56
Approximation::~Approximation()
57
{
58
    Clear();
59
}
60

61
void Approximation::GetMgcVectorArray(std::vector<Wm4::Vector3<double>>& rcPts) const
62
{
63
    std::list<Base::Vector3f>::const_iterator It;
64
    rcPts.reserve(_vPoints.size());
65
    for (It = _vPoints.begin(); It != _vPoints.end(); ++It) {
66
        rcPts.push_back(Base::convertTo<Wm4::Vector3d>(*It));
67
    }
68
}
69

70
void Approximation::AddPoint(const Base::Vector3f& rcVector)
71
{
72
    _vPoints.push_back(rcVector);
73
    _bIsFitted = false;
74
}
75

76
void Approximation::AddPoints(const std::vector<Base::Vector3f>& points)
77
{
78
    std::copy(points.begin(), points.end(), std::back_inserter(_vPoints));
79
    _bIsFitted = false;
80
}
81

82
void Approximation::AddPoints(const std::set<Base::Vector3f>& points)
83
{
84
    std::copy(points.begin(), points.end(), std::back_inserter(_vPoints));
85
    _bIsFitted = false;
86
}
87

88
void Approximation::AddPoints(const std::list<Base::Vector3f>& points)
89
{
90
    std::copy(points.begin(), points.end(), std::back_inserter(_vPoints));
91
    _bIsFitted = false;
92
}
93

94
void Approximation::AddPoints(const MeshPointArray& points)
95
{
96
    std::copy(points.begin(), points.end(), std::back_inserter(_vPoints));
97
    _bIsFitted = false;
98
}
99

100
Base::Vector3f Approximation::GetGravity() const
101
{
102
    Base::Vector3f clGravity;
103
    if (!_vPoints.empty()) {
104
        for (const auto& vPoint : _vPoints) {
105
            clGravity += vPoint;
106
        }
107
        clGravity *= 1.0f / float(_vPoints.size());
108
    }
109
    return clGravity;
110
}
111

112
std::size_t Approximation::CountPoints() const
113
{
114
    return _vPoints.size();
115
}
116

117
void Approximation::Clear()
118
{
119
    _vPoints.clear();
120
    _bIsFitted = false;
121
}
122

123
float Approximation::GetLastResult() const
124
{
125
    return _fLastResult;
126
}
127

128
bool Approximation::Done() const
129
{
130
    return _bIsFitted;
131
}
132

133
// -------------------------------------------------------------------------------
134

135
PlaneFit::PlaneFit()
136
    : _vBase(0, 0, 0)
137
    , _vDirU(1, 0, 0)
138
    , _vDirV(0, 1, 0)
139
    , _vDirW(0, 0, 1)
140
{}
141

142
float PlaneFit::Fit()
143
{
144
    _bIsFitted = true;
145
    if (CountPoints() < 3) {
146
        return FLOAT_MAX;
147
    }
148

149
    double sxx {0.0};
150
    double sxy {0.0};
151
    double sxz {0.0};
152
    double syy {0.0};
153
    double syz {0.0};
154
    double szz {0.0};
155
    double mx {0.0};
156
    double my {0.0};
157
    double mz {0.0};
158

159
    for (const auto& vPoint : _vPoints) {
160
        sxx += double(vPoint.x * vPoint.x);
161
        sxy += double(vPoint.x * vPoint.y);
162
        sxz += double(vPoint.x * vPoint.z);
163
        syy += double(vPoint.y * vPoint.y);
164
        syz += double(vPoint.y * vPoint.z);
165
        szz += double(vPoint.z * vPoint.z);
166
        mx += double(vPoint.x);
167
        my += double(vPoint.y);
168
        mz += double(vPoint.z);
169
    }
170

171
    size_t nSize = _vPoints.size();
172
    sxx = sxx - mx * mx / (double(nSize));
173
    sxy = sxy - mx * my / (double(nSize));
174
    sxz = sxz - mx * mz / (double(nSize));
175
    syy = syy - my * my / (double(nSize));
176
    syz = syz - my * mz / (double(nSize));
177
    szz = szz - mz * mz / (double(nSize));
178

179
#if defined(FC_USE_EIGEN)
180
    Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
181
    covMat(0, 0) = sxx;
182
    covMat(1, 1) = syy;
183
    covMat(2, 2) = szz;
184
    covMat(0, 1) = sxy;
185
    covMat(1, 0) = sxy;
186
    covMat(0, 2) = sxz;
187
    covMat(2, 0) = sxz;
188
    covMat(1, 2) = syz;
189
    covMat(2, 1) = syz;
190
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(covMat);
191

192
    Eigen::Vector3d u = eig.eigenvectors().col(1);
193
    Eigen::Vector3d v = eig.eigenvectors().col(2);
194
    Eigen::Vector3d w = eig.eigenvectors().col(0);
195

196
    _vDirU.Set(u.x(), u.y(), u.z());
197
    _vDirV.Set(v.x(), v.y(), v.z());
198
    _vDirW.Set(w.x(), w.y(), w.z());
199
    _vBase.Set(mx / (float)nSize, my / (float)nSize, mz / (float)nSize);
200

201
    float sigma = w.dot(covMat * w);
202
#else
203
    // Covariance matrix
204
    Wm4::Matrix3<double> akMat(sxx, sxy, sxz, sxy, syy, syz, sxz, syz, szz);
205
    Wm4::Matrix3<double> rkRot, rkDiag;
206
    try {
207
        akMat.EigenDecomposition(rkRot, rkDiag);
208
    }
209
    catch (const std::exception&) {
210
        return FLOAT_MAX;
211
    }
212

213
    // We know the Eigenvalues are ordered
214
    // rkDiag(0,0) <= rkDiag(1,1) <= rkDiag(2,2)
215
    //
216
    // points describe a line or even are identical
217
    if (rkDiag(1, 1) <= 0) {
218
        return FLOAT_MAX;
219
    }
220

221
    Wm4::Vector3<double> U = rkRot.GetColumn(1);
222
    Wm4::Vector3<double> V = rkRot.GetColumn(2);
223
    Wm4::Vector3<double> W = rkRot.GetColumn(0);
224

225
    // It may happen that the result have nan values
226
    for (int i = 0; i < 3; i++) {
227
        if (boost::math::isnan(W[i])) {
228
            return FLOAT_MAX;
229
        }
230
    }
231

232
    // In some cases when the points exactly lie on a plane it can happen that
233
    // U or V have nan values but W is valid.
234
    // In this case create an orthonormal basis
235
    bool validUV = true;
236
    for (int i = 0; i < 3; i++) {
237
        if (boost::math::isnan(U[i]) || boost::math::isnan(V[i])) {
238
            validUV = false;
239
            break;
240
        }
241
    }
242

243
    if (!validUV) {
244
        Wm4::Vector3<double>::GenerateOrthonormalBasis(U, V, W);
245
    }
246

247
    _vDirU.Set(float(U.X()), float(U.Y()), float(U.Z()));
248
    _vDirV.Set(float(V.X()), float(V.Y()), float(V.Z()));
249
    _vDirW.Set(float(W.X()), float(W.Y()), float(W.Z()));
250
    _vBase.Set(float(mx / nSize), float(my / nSize), float(mz / nSize));
251
    float sigma = float(W.Dot(akMat * W));
252
#endif
253

254
    // In case sigma is nan
255
    if (boost::math::isnan(sigma)) {
256
        return FLOAT_MAX;
257
    }
258

259
    // This must be caused by some round-off errors. Theoretically it's impossible
260
    // that 'sigma' becomes negative because the covariance matrix is positive semi-definite.
261
    if (sigma < 0) {
262
        sigma = 0;
263
    }
264

265
    // make a right-handed system
266
    if ((_vDirU % _vDirV) * _vDirW < 0.0f) {
267
        Base::Vector3f tmp = _vDirU;
268
        _vDirU = _vDirV;
269
        _vDirV = tmp;
270
    }
271

272
    if (nSize > 3) {
273
        sigma = sqrt(sigma / (nSize - 3));
274
    }
275
    else {
276
        sigma = 0;
277
    }
278

279
    _fLastResult = sigma;
280
    return _fLastResult;
281
}
282

283
Base::Vector3f PlaneFit::GetBase() const
284
{
285
    if (_bIsFitted) {
286
        return _vBase;
287
    }
288
    else {
289
        return Base::Vector3f();
290
    }
291
}
292

293
Base::Vector3f PlaneFit::GetDirU() const
294
{
295
    if (_bIsFitted) {
296
        return _vDirU;
297
    }
298
    else {
299
        return Base::Vector3f();
300
    }
301
}
302

303
Base::Vector3f PlaneFit::GetDirV() const
304
{
305
    if (_bIsFitted) {
306
        return _vDirV;
307
    }
308
    else {
309
        return Base::Vector3f();
310
    }
311
}
312

313
Base::Vector3f PlaneFit::GetNormal() const
314
{
315
    if (_bIsFitted) {
316
        return _vDirW;
317
    }
318
    else {
319
        return Base::Vector3f();
320
    }
321
}
322

323
float PlaneFit::GetDistanceToPlane(const Base::Vector3f& rcPoint) const
324
{
325
    float fResult = FLOAT_MAX;
326
    if (_bIsFitted) {
327
        fResult = (rcPoint - _vBase) * _vDirW;
328
    }
329
    return fResult;
330
}
331

332
float PlaneFit::GetStdDeviation() const
333
{
334
    // Mean: M=(1/N)*SUM Xi
335
    // Variance: VAR=(N/N-1)*[(1/N)*SUM(Xi^2)-M^2]
336
    // Standard deviation: SD=SQRT(VAR)
337
    // Standard error of the mean: SE=SD/SQRT(N)
338
    if (!_bIsFitted) {
339
        return FLOAT_MAX;
340
    }
341

342
    float fSumXi = 0.0f, fSumXi2 = 0.0f, fMean = 0.0f, fDist = 0.0f;
343

344
    float ulPtCt = float(CountPoints());
345
    std::list<Base::Vector3f>::const_iterator cIt;
346

347
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
348
        fDist = GetDistanceToPlane(*cIt);
349
        fSumXi += fDist;
350
        fSumXi2 += (fDist * fDist);
351
    }
352

353
    fMean = (1.0f / ulPtCt) * fSumXi;
354
    return sqrt((ulPtCt / (ulPtCt - 1.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean));
355
}
356

357
float PlaneFit::GetSignedStdDeviation() const
358
{
359
    // if the nearest point to the gravity is at the side
360
    // of normal direction the value will be
361
    // positive otherwise negative
362
    if (!_bIsFitted) {
363
        return FLOAT_MAX;
364
    }
365

366
    float fSumXi = 0.0F, fSumXi2 = 0.0F, fMean = 0.0F, fDist = 0.0F;
367
    float fMinDist = FLOAT_MAX;
368
    float fFactor = 0.0F;
369

370
    float ulPtCt = float(CountPoints());
371
    Base::Vector3f clGravity, clPt;
372
    std::list<Base::Vector3f>::const_iterator cIt;
373
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
374
        clGravity += *cIt;
375
    }
376
    clGravity *= (1.0f / ulPtCt);
377

378
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
379
        if ((clGravity - *cIt).Length() < fMinDist) {
380
            fMinDist = (clGravity - *cIt).Length();
381
            clPt = *cIt;
382
        }
383
        fDist = GetDistanceToPlane(*cIt);
384
        fSumXi += fDist;
385
        fSumXi2 += (fDist * fDist);
386
    }
387

388
    // which side
389
    if ((clPt - clGravity) * GetNormal() > 0) {
390
        fFactor = 1.0f;
391
    }
392
    else {
393
        fFactor = -1.0f;
394
    }
395

396
    fMean = 1.0f / ulPtCt * fSumXi;
397

398
    return fFactor * sqrt((ulPtCt / (ulPtCt - 3.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean));
399
}
400

401
void PlaneFit::ProjectToPlane()
402
{
403
    Base::Vector3f cGravity(GetGravity());
404
    Base::Vector3f cNormal(GetNormal());
405

406
    for (auto& cPnt : _vPoints) {
407
        float fD = (cPnt - cGravity) * cNormal;
408
        cPnt = cPnt - fD * cNormal;
409
    }
410
}
411

412
void PlaneFit::Dimension(float& length, float& width) const
413
{
414
    const Base::Vector3f& bs = _vBase;
415
    const Base::Vector3f& ex = _vDirU;
416
    const Base::Vector3f& ey = _vDirV;
417

418
    Base::BoundBox3f bbox;
419
    std::list<Base::Vector3f>::const_iterator cIt;
420
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
421
        Base::Vector3f pnt = *cIt;
422
        pnt.TransformToCoordinateSystem(bs, ex, ey);
423
        bbox.Add(pnt);
424
    }
425

426
    length = bbox.MaxX - bbox.MinX;
427
    width = bbox.MaxY - bbox.MinY;
428
}
429

430
std::vector<Base::Vector3f> PlaneFit::GetLocalPoints() const
431
{
432
    std::vector<Base::Vector3f> localPoints;
433
    if (_bIsFitted && _fLastResult < FLOAT_MAX) {
434
        Base::Vector3d bs = Base::convertTo<Base::Vector3d>(this->_vBase);
435
        Base::Vector3d ex = Base::convertTo<Base::Vector3d>(this->_vDirU);
436
        Base::Vector3d ey = Base::convertTo<Base::Vector3d>(this->_vDirV);
437
        // Base::Vector3d ez = Base::convertTo<Base::Vector3d>(this->_vDirW);
438

439
        localPoints.insert(localPoints.begin(), _vPoints.begin(), _vPoints.end());
440
        for (auto& localPoint : localPoints) {
441
            Base::Vector3d clPoint = Base::convertTo<Base::Vector3d>(localPoint);
442
            clPoint.TransformToCoordinateSystem(bs, ex, ey);
443
            localPoint.Set(static_cast<float>(clPoint.x),
444
                           static_cast<float>(clPoint.y),
445
                           static_cast<float>(clPoint.z));
446
        }
447
    }
448

449
    return localPoints;
450
}
451

452
Base::BoundBox3f PlaneFit::GetBoundings() const
453
{
454
    Base::BoundBox3f bbox;
455
    std::vector<Base::Vector3f> pts = GetLocalPoints();
456
    for (const auto& it : pts) {
457
        bbox.Add(it);
458
    }
459
    return bbox;
460
}
461

462
// -------------------------------------------------------------------------------
463

464
bool QuadraticFit::GetCurvatureInfo(double x,
465
                                    double y,
466
                                    double z,
467
                                    double& rfCurv0,
468
                                    double& rfCurv1,
469
                                    Base::Vector3f& rkDir0,
470
                                    Base::Vector3f& rkDir1,
471
                                    double& dDistance)
472
{
473
    assert(_bIsFitted);
474
    bool bResult = false;
475

476
    if (_bIsFitted) {
477
        Wm4::Vector3<double> Dir0, Dir1;
478
        FunctionContainer clFuncCont(_fCoeff);
479
        bResult = clFuncCont.CurvatureInfo(x, y, z, rfCurv0, rfCurv1, Dir0, Dir1, dDistance);
480

481
        dDistance = double(clFuncCont.GetGradient(x, y, z).Length());
482
        rkDir0 = Base::convertTo<Base::Vector3f>(Dir0);
483
        rkDir1 = Base::convertTo<Base::Vector3f>(Dir1);
484
    }
485

486
    return bResult;
487
}
488

489
bool QuadraticFit::GetCurvatureInfo(double x, double y, double z, double& rfCurv0, double& rfCurv1)
490
{
491
    bool bResult = false;
492

493
    if (_bIsFitted) {
494
        FunctionContainer clFuncCont(_fCoeff);
495
        bResult = clFuncCont.CurvatureInfo(x, y, z, rfCurv0, rfCurv1);
496
    }
497

498
    return bResult;
499
}
500

501
const double& QuadraticFit::GetCoeffArray() const
502
{
503
    return _fCoeff[0];
504
}
505

506
double QuadraticFit::GetCoeff(std::size_t ulIndex) const
507
{
508
    assert(ulIndex < 10);
509

510
    if (_bIsFitted) {
511
        return _fCoeff[ulIndex];
512
    }
513
    else {
514
        return double(FLOAT_MAX);
515
    }
516
}
517

518
float QuadraticFit::Fit()
519
{
520
    float fResult = FLOAT_MAX;
521

522
    if (CountPoints() > 0) {
523
        std::vector<Wm4::Vector3<double>> cPts;
524
        GetMgcVectorArray(cPts);
525
        fResult = (float)Wm4::QuadraticFit3<double>(CountPoints(), &(cPts[0]), _fCoeff);
526
        _fLastResult = fResult;
527

528
        _bIsFitted = true;
529
    }
530

531
    return fResult;
532
}
533

534
void QuadraticFit::CalcEigenValues(double& dLambda1,
535
                                   double& dLambda2,
536
                                   double& dLambda3,
537
                                   Base::Vector3f& clEV1,
538
                                   Base::Vector3f& clEV2,
539
                                   Base::Vector3f& clEV3) const
540
{
541
    assert(_bIsFitted);
542

543
    /*
544
     * F(x,y,z) = a11*x*x + a22*y*y + a33*z*z +2*a12*x*y + 2*a13*x*z + 2*a23*y*z + 2*a10*x + 2*a20*y
545
     * + 2*a30*z * a00 = 0
546
     *
547
     * Form matrix:
548
     *
549
     *      ( a11   a12   a13 )
550
     * A =  ( a21   a22   a23 )       where a[i,j] = a[j,i]
551
     *      ( a31   a32   a33 )
552
     *
553
     * Coefficients of the Quadrik-Fit based on the notation used here:
554
     *
555
     *   0 = C[0] + C[1]*X + C[2]*Y + C[3]*Z + C[4]*X^2 + C[5]*Y^2
556
     *     + C[6]*Z^2 + C[7]*X*Y + C[8]*X*Z + C[9]*Y*Z
557
     *
558
     * Square:        a11 := c[4],    a22 := c[5],    a33 := c[6]
559
     * Mixed:         a12 := c[7]/2,  a13 := c[8]/2,  a23 := c[9]/2
560
     * Linear:        a10 := c[1]/2,  a20 := c[2]/2,  a30 := c[3]/2
561
     * Constant:      a00 := c[0]
562
     *
563
     */
564

565
    Wm4::Matrix3<double> akMat(_fCoeff[4],
566
                               _fCoeff[7] / 2.0,
567
                               _fCoeff[8] / 2.0,
568
                               _fCoeff[7] / 2.0,
569
                               _fCoeff[5],
570
                               _fCoeff[9] / 2.0,
571
                               _fCoeff[8] / 2.0,
572
                               _fCoeff[9] / 2.0,
573
                               _fCoeff[6]);
574

575
    Wm4::Matrix3<double> rkRot, rkDiag;
576
    akMat.EigenDecomposition(rkRot, rkDiag);
577

578
    Wm4::Vector3<double> vEigenU = rkRot.GetColumn(0);
579
    Wm4::Vector3<double> vEigenV = rkRot.GetColumn(1);
580
    Wm4::Vector3<double> vEigenW = rkRot.GetColumn(2);
581

582
    clEV1 = Base::convertTo<Base::Vector3f>(vEigenU);
583
    clEV2 = Base::convertTo<Base::Vector3f>(vEigenV);
584
    clEV3 = Base::convertTo<Base::Vector3f>(vEigenW);
585

586
    dLambda1 = rkDiag[0][0];
587
    dLambda2 = rkDiag[1][1];
588
    dLambda3 = rkDiag[2][2];
589
}
590

591
void QuadraticFit::CalcZValues(double x, double y, double& dZ1, double& dZ2) const
592
{
593
    assert(_bIsFitted);
594

595
    double dDisk = _fCoeff[3] * _fCoeff[3] + 2 * _fCoeff[3] * _fCoeff[8] * x
596
        + 2 * _fCoeff[3] * _fCoeff[9] * y + _fCoeff[8] * _fCoeff[8] * x * x
597
        + 2 * _fCoeff[8] * x * _fCoeff[9] * y + _fCoeff[9] * _fCoeff[9] * y * y
598
        - 4 * _fCoeff[6] * _fCoeff[0] - 4 * _fCoeff[6] * _fCoeff[1] * x
599
        - 4 * _fCoeff[6] * _fCoeff[2] * y - 4 * _fCoeff[6] * _fCoeff[7] * x * y
600
        - 4 * _fCoeff[6] * _fCoeff[4] * x * x - 4 * _fCoeff[6] * _fCoeff[5] * y * y;
601

602
    if (fabs(_fCoeff[6]) < 0.000005) {
603
        dZ1 = double(FLOAT_MAX);
604
        dZ2 = double(FLOAT_MAX);
605
        return;
606
    }
607

608
    if (dDisk < 0.0) {
609
        dZ1 = double(FLOAT_MAX);
610
        dZ2 = double(FLOAT_MAX);
611
        return;
612
    }
613
    else {
614
        dDisk = sqrt(dDisk);
615
    }
616

617
    dZ1 = 0.5 * ((-_fCoeff[3] - _fCoeff[8] * x - _fCoeff[9] * y + dDisk) / _fCoeff[6]);
618
    dZ2 = 0.5 * ((-_fCoeff[3] - _fCoeff[8] * x - _fCoeff[9] * y - dDisk) / _fCoeff[6]);
619
}
620

621
// -------------------------------------------------------------------------------
622

623
SurfaceFit::SurfaceFit()
624
    : _fCoeff {}
625
{}
626

627
float SurfaceFit::Fit()
628
{
629
    float fResult = FLOAT_MAX;
630

631
    if (CountPoints() > 0) {
632
        fResult = float(PolynomFit());
633
        _fLastResult = fResult;
634

635
        _bIsFitted = true;
636
    }
637

638
    return fResult;
639
}
640

641
bool SurfaceFit::GetCurvatureInfo(double x,
642
                                  double y,
643
                                  double z,
644
                                  double& rfCurv0,
645
                                  double& rfCurv1,
646
                                  Base::Vector3f& rkDir0,
647
                                  Base::Vector3f& rkDir1,
648
                                  double& dDistance)
649
{
650
    bool bResult = false;
651

652
    if (_bIsFitted) {
653
        Wm4::Vector3<double> Dir0, Dir1;
654
        FunctionContainer clFuncCont(_fCoeff);
655
        bResult = clFuncCont.CurvatureInfo(x, y, z, rfCurv0, rfCurv1, Dir0, Dir1, dDistance);
656

657
        dDistance = double(clFuncCont.GetGradient(x, y, z).Length());
658
        rkDir0 = Base::convertTo<Base::Vector3f>(Dir0);
659
        rkDir1 = Base::convertTo<Base::Vector3f>(Dir1);
660
    }
661

662
    return bResult;
663
}
664

665
bool SurfaceFit::GetCurvatureInfo(double x, double y, double z, double& rfCurv0, double& rfCurv1)
666
{
667
    assert(_bIsFitted);
668
    bool bResult = false;
669

670
    if (_bIsFitted) {
671
        FunctionContainer clFuncCont(_fCoeff);
672
        bResult = clFuncCont.CurvatureInfo(x, y, z, rfCurv0, rfCurv1);
673
    }
674

675
    return bResult;
676
}
677

678
double SurfaceFit::PolynomFit()
679
{
680
    if (PlaneFit::Fit() >= FLOAT_MAX) {
681
        return double(FLOAT_MAX);
682
    }
683

684
    Base::Vector3d bs = Base::convertTo<Base::Vector3d>(this->_vBase);
685
    Base::Vector3d ex = Base::convertTo<Base::Vector3d>(this->_vDirU);
686
    Base::Vector3d ey = Base::convertTo<Base::Vector3d>(this->_vDirV);
687
    // Base::Vector3d ez = Base::convertTo<Base::Vector3d>(this->_vDirW);
688

689
    // A*x = b
690
    // See also www.cs.jhu.edu/~misha/Fall05/10.23.05.pdf
691
    // z = f(x,y) = a*x^2 + b*y^2 + c*x*y + d*x + e*y + f
692
    // z = P * Vi with Vi=(xi^2,yi^2,xiyi,xi,yi,1) and P=(a,b,c,d,e,f)
693
    // To get the best-fit values the sum needs to be minimized:
694
    // S = sum[(z-zi)^2} -> min with zi=z coordinates of the given points
695
    // <=> S = sum[z^2 - 2*z*zi + zi^2] -> min
696
    // <=> S(P) = sum[(P*Vi)^2 - 2*(P*Vi)*zi + zi^2] -> min
697
    // To get the optimum the gradient of the expression must be the null vector
698
    // Note: grad F(P) = (P*Vi)^2 = 2*(P*Vi)*Vi
699
    //       grad G(P) = -2*(P*Vi)*zi = -2*Vi*zi
700
    //       grad H(P) = zi^2 = 0
701
    //  => grad S(P) = sum[2*(P*Vi)*Vi - 2*Vi*zi] = 0
702
    // <=> sum[(P*Vi)*Vi] = sum[Vi*zi]
703
    // <=> sum[(Vi*Vi^t)*P] = sum[Vi*zi] where (Vi*Vi^t) is a symmetric matrix
704
    // <=> sum[(Vi*Vi^t)]*P = sum[Vi*zi]
705
    Eigen::Matrix<double, 6, 6> A = Eigen::Matrix<double, 6, 6>::Zero();
706
    Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();
707
    Eigen::Matrix<double, 6, 1> x = Eigen::Matrix<double, 6, 1>::Zero();
708

709
    std::vector<Base::Vector3d> transform;
710
    transform.reserve(_vPoints.size());
711

712
    double dW2 = 0;
713
    for (std::list<Base::Vector3f>::const_iterator it = _vPoints.begin(); it != _vPoints.end();
714
         ++it) {
715
        Base::Vector3d clPoint = Base::convertTo<Base::Vector3d>(*it);
716
        clPoint.TransformToCoordinateSystem(bs, ex, ey);
717
        transform.push_back(clPoint);
718
        double dU = clPoint.x;
719
        double dV = clPoint.y;
720
        double dW = clPoint.z;
721

722
        double dU2 = dU * dU;
723
        double dV2 = dV * dV;
724
        double dUV = dU * dV;
725

726
        dW2 += dW * dW;
727

728
        A(0, 0) = A(0, 0) + dU2 * dU2;
729
        A(0, 1) = A(0, 1) + dU2 * dV2;
730
        A(0, 2) = A(0, 2) + dU2 * dUV;
731
        A(0, 3) = A(0, 3) + dU2 * dU;
732
        A(0, 4) = A(0, 4) + dU2 * dV;
733
        A(0, 5) = A(0, 5) + dU2;
734
        b(0) = b(0) + dU2 * dW;
735

736
        A(1, 1) = A(1, 1) + dV2 * dV2;
737
        A(1, 2) = A(1, 2) + dV2 * dUV;
738
        A(1, 3) = A(1, 3) + dV2 * dU;
739
        A(1, 4) = A(1, 4) + dV2 * dV;
740
        A(1, 5) = A(1, 5) + dV2;
741
        b(1) = b(1) + dV2 * dW;
742

743
        A(2, 2) = A(2, 2) + dUV * dUV;
744
        A(2, 3) = A(2, 3) + dUV * dU;
745
        A(2, 4) = A(2, 4) + dUV * dV;
746
        A(2, 5) = A(2, 5) + dUV;
747
        b(3) = b(3) + dUV * dW;
748

749
        A(3, 3) = A(3, 3) + dU * dU;
750
        A(3, 4) = A(3, 4) + dU * dV;
751
        A(3, 5) = A(3, 5) + dU;
752
        b(3) = b(3) + dU * dW;
753

754
        A(4, 4) = A(4, 4) + dV * dV;
755
        A(4, 5) = A(4, 5) + dV;
756
        b(5) = b(5) + dV * dW;
757

758
        A(5, 5) = A(5, 5) + 1;
759
        b(5) = b(5) + 1 * dW;
760
    }
761

762
    // Mat is symmetric
763
    //
764
    A(1, 0) = A(0, 1);
765
    A(2, 0) = A(0, 2);
766
    A(3, 0) = A(0, 3);
767
    A(4, 0) = A(0, 4);
768
    A(5, 0) = A(0, 5);
769

770
    A(2, 1) = A(1, 2);
771
    A(3, 1) = A(1, 3);
772
    A(4, 1) = A(1, 4);
773
    A(5, 1) = A(1, 5);
774

775
    A(3, 2) = A(2, 3);
776
    A(4, 2) = A(2, 4);
777
    A(5, 2) = A(2, 5);
778

779
    A(4, 3) = A(3, 4);
780
    A(5, 3) = A(3, 5);
781

782
    A(5, 4) = A(4, 5);
783

784
    Eigen::HouseholderQR<Eigen::Matrix<double, 6, 6>> qr(A);
785
    x = qr.solve(b);
786

787
    // FunctionContainer gets an implicit function F(x,y,z) = 0 of this form
788
    // _fCoeff[0] +
789
    // _fCoeff[1]*x   + _fCoeff[2]*y   + _fCoeff[3]*z   +
790
    // _fCoeff[4]*x^2 + _fCoeff[5]*y^2 + _fCoeff[6]*z^2 +
791
    // _fCoeff[7]*x*y + _fCoeff[8]*x*z + _fCoeff[9]*y*z
792
    //
793
    // The bivariate polynomial surface we get here is of the form
794
    // z = f(x,y) = a*x^2 + b*y^2 + c*x*y + d*x + e*y + f
795
    // Writing it as implicit surface F(x,y,z) = 0 gives this form
796
    // F(x,y,z) = f(x,y) - z = a*x^2 + b*y^2 + c*x*y + d*x + e*y - z + f
797
    // Thus:
798
    // _fCoeff[0] = f
799
    // _fCoeff[1] = d
800
    // _fCoeff[2] = e
801
    // _fCoeff[3] = -1
802
    // _fCoeff[4] = a
803
    // _fCoeff[5] = b
804
    // _fCoeff[6] = 0
805
    // _fCoeff[7] = c
806
    // _fCoeff[8] = 0
807
    // _fCoeff[9] = 0
808

809
    _fCoeff[0] = x(5);
810
    _fCoeff[1] = x(3);
811
    _fCoeff[2] = x(4);
812
    _fCoeff[3] = -1.0;
813
    _fCoeff[4] = x(0);
814
    _fCoeff[5] = x(1);
815
    _fCoeff[6] = 0.0;
816
    _fCoeff[7] = x(2);
817
    _fCoeff[8] = 0.0;
818
    _fCoeff[9] = 0.0;
819

820
    // Get S(P) = sum[(P*Vi)^2 - 2*(P*Vi)*zi + zi^2]
821
    double sigma = 0;
822
    FunctionContainer clFuncCont(_fCoeff);
823
    for (const auto& it : transform) {
824
        double u = it.x;
825
        double v = it.y;
826
        double z = clFuncCont.F(u, v, 0.0);
827
        sigma += z * z;
828
    }
829

830
    sigma += dW2 - 2 * x.dot(b);
831
    // This must be caused by some round-off errors. Theoretically it's impossible
832
    // that 'sigma' becomes negative.
833
    if (sigma < 0) {
834
        sigma = 0;
835
    }
836
    if (!_vPoints.empty()) {
837
        sigma = sqrt(sigma / _vPoints.size());
838
    }
839

840
    _fLastResult = static_cast<float>(sigma);
841
    return double(_fLastResult);
842
}
843

844
double SurfaceFit::Value(double x, double y) const
845
{
846
    double z = 0.0;
847
    if (_bIsFitted) {
848
        FunctionContainer clFuncCont(_fCoeff);
849
        z = clFuncCont.F(x, y, 0.0);
850
    }
851

852
    return z;
853
}
854

855
void SurfaceFit::GetCoefficients(double& a, double& b, double& c, double& d, double& e, double& f)
856
    const
857
{
858
    a = _fCoeff[4];
859
    b = _fCoeff[5];
860
    c = _fCoeff[7];
861
    d = _fCoeff[1];
862
    e = _fCoeff[2];
863
    f = _fCoeff[0];
864
}
865

866
void SurfaceFit::Transform(std::vector<Base::Vector3f>& pts) const
867
{
868
    Base::Vector3d bs = Base::convertTo<Base::Vector3d>(this->_vBase);
869
    Base::Vector3d ex = Base::convertTo<Base::Vector3d>(this->_vDirU);
870
    Base::Vector3d ey = Base::convertTo<Base::Vector3d>(this->_vDirV);
871
    Base::Vector3d ez = Base::convertTo<Base::Vector3d>(this->_vDirW);
872

873
    Base::Matrix4D mat;
874
    mat[0][0] = ex.x;
875
    mat[0][1] = ey.x;
876
    mat[0][2] = ez.x;
877
    mat[0][3] = bs.x;
878

879
    mat[1][0] = ex.y;
880
    mat[1][1] = ey.y;
881
    mat[1][2] = ez.y;
882
    mat[1][3] = bs.y;
883

884
    mat[2][0] = ex.z;
885
    mat[2][1] = ey.z;
886
    mat[2][2] = ez.z;
887
    mat[2][3] = bs.z;
888

889
    std::transform(pts.begin(), pts.end(), pts.begin(), [&mat](const Base::Vector3f& pt) {
890
        Base::Vector3f v(pt);
891
        mat.multVec(v, v);
892
        return v;
893
    });
894
}
895

896
void SurfaceFit::Transform(std::vector<Base::Vector3d>& pts) const
897
{
898
    Base::Vector3d bs = Base::convertTo<Base::Vector3d>(this->_vBase);
899
    Base::Vector3d ex = Base::convertTo<Base::Vector3d>(this->_vDirU);
900
    Base::Vector3d ey = Base::convertTo<Base::Vector3d>(this->_vDirV);
901
    Base::Vector3d ez = Base::convertTo<Base::Vector3d>(this->_vDirW);
902

903
    Base::Matrix4D mat;
904
    mat[0][0] = ex.x;
905
    mat[0][1] = ey.x;
906
    mat[0][2] = ez.x;
907
    mat[0][3] = bs.x;
908

909
    mat[1][0] = ex.y;
910
    mat[1][1] = ey.y;
911
    mat[1][2] = ez.y;
912
    mat[1][3] = bs.y;
913

914
    mat[2][0] = ex.z;
915
    mat[2][1] = ey.z;
916
    mat[2][2] = ez.z;
917
    mat[2][3] = bs.z;
918

919
    std::transform(pts.begin(), pts.end(), pts.begin(), [&mat](const Base::Vector3d& pt) {
920
        Base::Vector3d v(pt);
921
        mat.multVec(v, v);
922
        return v;
923
    });
924
}
925

926
/*!
927
 * \brief SurfaceFit::toBezier
928
 * This function computes the Bezier representation of the polynomial surface of the form
929
 * f(x,y) = a*x*x + b*y*y + c*x*y + d*y + e*f + f
930
 * by getting the 3x3 control points.
931
 */
932
std::vector<Base::Vector3d>
933
SurfaceFit::toBezier(double umin, double umax, double vmin, double vmax) const
934
{
935
    std::vector<Base::Vector3d> pts;
936
    pts.reserve(9);
937

938
    // the Bezier surface is defined by the 3x3 control points
939
    // P11 P21 P31
940
    // P12 P22 P32
941
    // P13 P23 P33
942
    //
943
    // The surface goes through the points P11, P31, P31 and P33
944
    // To get the four control points P21, P12, P32 and P23 we inverse
945
    // the de-Casteljau algorithm used for Bezier curves of degree 2
946
    // as we already know the points for the parameters
947
    // (0, 0.5), (0.5, 0), (0.5, 1.0) and (1.0, 0.5)
948
    // To get the control point P22 we inverse the de-Casteljau algorithm
949
    // for the surface point on (0.5, 0.5)
950
    double umid = 0.5 * (umin + umax);
951
    double vmid = 0.5 * (vmin + vmax);
952

953
    // first row
954
    double z11 = Value(umin, vmin);
955
    double v21 = Value(umid, vmin);
956
    double z31 = Value(umax, vmin);
957
    double z21 = 2.0 * v21 - 0.5 * (z11 + z31);
958

959
    // third row
960
    double z13 = Value(umin, vmax);
961
    double v23 = Value(umid, vmax);
962
    double z33 = Value(umax, vmax);
963
    double z23 = 2.0 * v23 - 0.5 * (z13 + z33);
964

965
    // second row
966
    double v12 = Value(umin, vmid);
967
    double z12 = 2.0 * v12 - 0.5 * (z11 + z13);
968
    double v32 = Value(umax, vmid);
969
    double z32 = 2.0 * v32 - 0.5 * (z31 + z33);
970
    double v22 = Value(umid, vmid);
971
    double z22 = 4.0 * v22 - 0.25 * (z11 + z31 + z13 + z33 + 2.0 * (z12 + z21 + z32 + z23));
972

973
    // first row
974
    pts.emplace_back(umin, vmin, z11);
975
    pts.emplace_back(umid, vmin, z21);
976
    pts.emplace_back(umax, vmin, z31);
977

978
    // second row
979
    pts.emplace_back(umin, vmid, z12);
980
    pts.emplace_back(umid, vmid, z22);
981
    pts.emplace_back(umax, vmid, z32);
982

983
    // third row
984
    pts.emplace_back(umin, vmax, z13);
985
    pts.emplace_back(umid, vmax, z23);
986
    pts.emplace_back(umax, vmax, z33);
987
    return pts;
988
}
989

990
// -------------------------------------------------------------------------------
991

992
namespace MeshCore
993
{
994

995
struct LMCylinderFunctor
996
{
997
    Eigen::MatrixXd measuredValues;
998

999
    // Compute 'm' errors, one for each data point, for the given parameter values in 'x'
1000
    int operator()(const Eigen::VectorXd& xvec, Eigen::VectorXd& fvec) const
1001
    {
1002
        // 'xvec' has dimensions n x 1
1003
        // It contains the current estimates for the parameters.
1004

1005
        // 'fvec' has dimensions m x 1
1006
        // It will contain the error for each data point.
1007
        double aParam = xvec(0);  // dir_x
1008
        double bParam = xvec(1);  // dir_y
1009
        double cParam = xvec(2);  // dir_z
1010
        double dParam = xvec(3);  // cnt_x
1011
        double eParam = xvec(4);  // cnt_y
1012
        double fParam = xvec(5);  // cnt_z
1013
        double gParam = xvec(6);  // radius
1014

1015
        // use distance functions (fvec(i)) for cylinders as defined in the paper:
1016
        // Least-Squares Fitting Algorithms of the NIST Algorithm Testing System
1017
        for (int i = 0; i < values(); i++) {
1018
            double xValue = measuredValues(i, 0);
1019
            double yValue = measuredValues(i, 1);
1020
            double zValue = measuredValues(i, 2);
1021

1022
            double a = aParam / (sqrt(aParam * aParam + bParam * bParam + cParam * cParam));
1023
            double b = bParam / (sqrt(aParam * aParam + bParam * bParam + cParam * cParam));
1024
            double c = cParam / (sqrt(aParam * aParam + bParam * bParam + cParam * cParam));
1025
            double x = dParam;
1026
            double y = eParam;
1027
            double z = fParam;
1028
            double u = c * (yValue - y) - b * (zValue - z);
1029
            double v = a * (zValue - z) - c * (xValue - x);
1030
            double w = b * (xValue - x) - a * (yValue - y);
1031

1032
            fvec(i) = sqrt(u * u + v * v + w * w) - gParam;
1033
        }
1034
        return 0;
1035
    }
1036

1037
    // Compute the jacobian of the errors
1038
    int df(const Eigen::VectorXd& x, Eigen::MatrixXd& fjac) const
1039
    {
1040
        // 'x' has dimensions n x 1
1041
        // It contains the current estimates for the parameters.
1042

1043
        // 'fjac' has dimensions m x n
1044
        // It will contain the jacobian of the errors, calculated numerically in this case.
1045

1046
        const double epsilon = 1e-5;
1047

1048
        for (int i = 0; i < x.size(); i++) {
1049
            Eigen::VectorXd xPlus(x);
1050
            xPlus(i) += epsilon;
1051
            Eigen::VectorXd xMinus(x);
1052
            xMinus(i) -= epsilon;
1053

1054
            Eigen::VectorXd fvecPlus(values());
1055
            operator()(xPlus, fvecPlus);
1056

1057
            Eigen::VectorXd fvecMinus(values());
1058
            operator()(xMinus, fvecMinus);
1059

1060
            Eigen::VectorXd fvecDiff(values());
1061
            fvecDiff = (fvecPlus - fvecMinus) / (2.0f * epsilon);
1062

1063
            fjac.block(0, i, values(), 1) = fvecDiff;
1064
        }
1065

1066
        return 0;
1067
    }
1068

1069
    // Number of data points, i.e. values.
1070
    int m;
1071

1072
    // Returns 'm', the number of values.
1073
    int values() const
1074
    {
1075
        return m;
1076
    }
1077

1078
    // The number of parameters, i.e. inputs.
1079
    int n;
1080

1081
    // Returns 'n', the number of inputs.
1082
    int inputs() const
1083
    {
1084
        return n;
1085
    }
1086
};
1087

1088
}  // namespace MeshCore
1089

1090
CylinderFit::CylinderFit()
1091
    : _vBase(0, 0, 0)
1092
    , _vAxis(0, 0, 1)
1093
{}
1094

1095
Base::Vector3f CylinderFit::GetInitialAxisFromNormals(const std::vector<Base::Vector3f>& n) const
1096
{
1097
#if 0
1098
    int nc = 0;
1099
    double x = 0.0;
1100
    double y = 0.0;
1101
    double z = 0.0;
1102
    for (int i = 0; i < (int)n.size()-1; ++i) {
1103
        for (int j = i+1; j < (int)n.size(); ++j) {
1104
            Base::Vector3f cross = n[i] % n[j];
1105
            if (cross.Sqr() > 1.0e-6) {
1106
                cross.Normalize();
1107
                x += cross.x;
1108
                y += cross.y;
1109
                z += cross.z;
1110
                ++nc;
1111
            }
1112
        }
1113
    }
1114

1115
    if (nc > 0) {
1116
        x /= (double)nc;
1117
        y /= (double)nc;
1118
        z /= (double)nc;
1119
        Base::Vector3f axis(x,y,z);
1120
        axis.Normalize();
1121
        return axis;
1122
    }
1123

1124
    PlaneFit planeFit;
1125
    planeFit.AddPoints(n);
1126
    planeFit.Fit();
1127
    return planeFit.GetNormal();
1128
#endif
1129

1130
    // Like a plane fit where the base is at (0,0,0)
1131
    double sxx {0.0};
1132
    double sxy {0.0};
1133
    double sxz {0.0};
1134
    double syy {0.0};
1135
    double syz {0.0};
1136
    double szz {0.0};
1137

1138
    for (auto it : n) {
1139
        sxx += double(it.x * it.x);
1140
        sxy += double(it.x * it.y);
1141
        sxz += double(it.x * it.z);
1142
        syy += double(it.y * it.y);
1143
        syz += double(it.y * it.z);
1144
        szz += double(it.z * it.z);
1145
    }
1146

1147
    Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
1148
    covMat(0, 0) = sxx;
1149
    covMat(1, 1) = syy;
1150
    covMat(2, 2) = szz;
1151
    covMat(0, 1) = sxy;
1152
    covMat(1, 0) = sxy;
1153
    covMat(0, 2) = sxz;
1154
    covMat(2, 0) = sxz;
1155
    covMat(1, 2) = syz;
1156
    covMat(2, 1) = syz;
1157
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(covMat);
1158
    Eigen::Vector3d w = eig.eigenvectors().col(0);
1159

1160
    Base::Vector3f normal;
1161
    normal.Set(w.x(), w.y(), w.z());
1162
    return normal;
1163
}
1164

1165
void CylinderFit::SetInitialValues(const Base::Vector3f& b, const Base::Vector3f& n)
1166
{
1167
    _vBase = b;
1168
    _vAxis = n;
1169
    _initialGuess = true;
1170
}
1171

1172
float CylinderFit::Fit()
1173
{
1174
    if (CountPoints() < 7) {
1175
        return FLOAT_MAX;
1176
    }
1177
    _bIsFitted = true;
1178

1179
#if 1
1180
    // Do the cylinder fit
1181
    MeshCoreFit::CylinderFit cylFit;
1182
    cylFit.AddPoints(_vPoints);
1183
    if (_initialGuess) {
1184
        cylFit.SetApproximations(_fRadius,
1185
                                 Base::Vector3d(_vBase.x, _vBase.y, _vBase.z),
1186
                                 Base::Vector3d(_vAxis.x, _vAxis.y, _vAxis.z));
1187
    }
1188

1189
    float result = cylFit.Fit();
1190
    if (result < FLOAT_MAX) {
1191
        Base::Vector3d base = cylFit.GetBase();
1192
        Base::Vector3d dir = cylFit.GetAxis();
1193

1194
#if defined(FC_DEBUG)
1195
        Base::Console().Log(
1196
            "MeshCoreFit::Cylinder Fit:  Base: (%0.4f, %0.4f, %0.4f),  Axis: (%0.6f, %0.6f, "
1197
            "%0.6f),  Radius: %0.4f,  Std Dev: %0.4f,  Iterations: %d\n",
1198
            base.x,
1199
            base.y,
1200
            base.z,
1201
            dir.x,
1202
            dir.y,
1203
            dir.z,
1204
            cylFit.GetRadius(),
1205
            cylFit.GetStdDeviation(),
1206
            cylFit.GetNumIterations());
1207
#endif
1208
        _vBase = Base::convertTo<Base::Vector3f>(base);
1209
        _vAxis = Base::convertTo<Base::Vector3f>(dir);
1210
        _fRadius = (float)cylFit.GetRadius();
1211
        _fLastResult = result;
1212
    }
1213
#else
1214
    int m = static_cast<int>(_vPoints.size());
1215
    int n = 7;
1216

1217
    Eigen::MatrixXd measuredValues(m, 3);
1218
    int index = 0;
1219
    for (const auto& it : _vPoints) {
1220
        measuredValues(index, 0) = it.x;
1221
        measuredValues(index, 1) = it.y;
1222
        measuredValues(index, 2) = it.z;
1223
        index++;
1224
    }
1225

1226
    Eigen::VectorXd x(n);
1227
    x(0) = 1.0;  // initial value for dir_x
1228
    x(1) = 1.0;  // initial value for dir_y
1229
    x(2) = 1.0;  // initial value for dir_z
1230
    x(3) = 0.0;  // initial value for cnt_x
1231
    x(4) = 0.0;  // initial value for cnt_y
1232
    x(5) = 0.0;  // initial value for cnt_z
1233
    x(6) = 0.0;  // initial value for radius
1234

1235
    //
1236
    // Run the LM optimization
1237
    // Create a LevenbergMarquardt object and pass it the functor.
1238
    //
1239

1240
    LMCylinderFunctor functor;
1241
    functor.measuredValues = measuredValues;
1242
    functor.m = m;
1243
    functor.n = n;
1244

1245
    Eigen::LevenbergMarquardt<LMCylinderFunctor, double> lm(functor);
1246
    int status = lm.minimize(x);
1247
    Base::Console().Log("Cylinder fit: %d, iterations: %d, gradient norm: %f\n",
1248
                        status,
1249
                        lm.iter,
1250
                        lm.gnorm);
1251

1252
    _vAxis.x = x(0);
1253
    _vAxis.y = x(1);
1254
    _vAxis.z = x(2);
1255
    _vAxis.Normalize();
1256

1257
    _vBase.x = x(3);
1258
    _vBase.y = x(4);
1259
    _vBase.z = x(5);
1260

1261
    _fRadius = x(6);
1262

1263
    _fLastResult = lm.gnorm;
1264
#endif
1265

1266
    return _fLastResult;
1267
}
1268

1269
float CylinderFit::GetRadius() const
1270
{
1271
    return _fRadius;
1272
}
1273

1274
Base::Vector3f CylinderFit::GetBase() const
1275
{
1276
    if (_bIsFitted) {
1277
        return _vBase;
1278
    }
1279
    else {
1280
        return Base::Vector3f();
1281
    }
1282
}
1283

1284
Base::Vector3f CylinderFit::GetAxis() const
1285
{
1286
    if (_bIsFitted) {
1287
        return _vAxis;
1288
    }
1289
    else {
1290
        return Base::Vector3f();
1291
    }
1292
}
1293

1294
float CylinderFit::GetDistanceToCylinder(const Base::Vector3f& rcPoint) const
1295
{
1296
    float fResult = FLOAT_MAX;
1297
    if (_bIsFitted) {
1298
        fResult = rcPoint.DistanceToLine(_vBase, _vAxis) - _fRadius;
1299
    }
1300
    return fResult;
1301
}
1302

1303
float CylinderFit::GetStdDeviation() const
1304
{
1305
    // Mean: M=(1/N)*SUM Xi
1306
    // Variance: VAR=(N/N-1)*[(1/N)*SUM(Xi^2)-M^2]
1307
    // Standard deviation: SD=SQRT(VAR)
1308
    // Standard error of the mean: SE=SD/SQRT(N)
1309
    if (!_bIsFitted) {
1310
        return FLOAT_MAX;
1311
    }
1312

1313
    float fSumXi = 0.0f, fSumXi2 = 0.0f, fMean = 0.0f, fDist = 0.0f;
1314

1315
    float ulPtCt = float(CountPoints());
1316
    std::list<Base::Vector3f>::const_iterator cIt;
1317

1318
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
1319
        fDist = GetDistanceToCylinder(*cIt);
1320
        fSumXi += fDist;
1321
        fSumXi2 += (fDist * fDist);
1322
    }
1323

1324
    fMean = (1.0f / ulPtCt) * fSumXi;
1325
    return sqrt((ulPtCt / (ulPtCt - 1.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean));
1326
}
1327

1328
void CylinderFit::GetBounding(Base::Vector3f& bottom, Base::Vector3f& top) const
1329
{
1330
    float distMin = FLT_MAX;
1331
    float distMax = FLT_MIN;
1332

1333
    std::list<Base::Vector3f>::const_iterator cIt;
1334
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
1335
        float dist = cIt->DistanceToPlane(_vBase, _vAxis);
1336
        if (dist < distMin) {
1337
            distMin = dist;
1338
            bottom = *cIt;
1339
        }
1340
        if (dist > distMax) {
1341
            distMax = dist;
1342
            top = *cIt;
1343
        }
1344
    }
1345

1346
    // Project the points onto the cylinder axis
1347
    bottom = bottom.Perpendicular(_vBase, _vAxis);
1348
    top = top.Perpendicular(_vBase, _vAxis);
1349
}
1350

1351
void CylinderFit::ProjectToCylinder()
1352
{
1353
    Base::Vector3f cBase(GetBase());
1354
    Base::Vector3f cAxis(GetAxis());
1355

1356
    for (auto& cPnt : _vPoints) {
1357
        if (cPnt.DistanceToLine(cBase, cAxis) > 0) {
1358
            Base::Vector3f proj;
1359
            cBase.ProjectToPlane(cPnt, cAxis, proj);
1360
            Base::Vector3f diff = cPnt - proj;
1361
            diff.Normalize();
1362
            cPnt = proj + diff * _fRadius;
1363
        }
1364
        else {
1365
            // Point is on the cylinder axis, so it can be moved in
1366
            // any direction perpendicular to the cylinder axis
1367
            Base::Vector3f cMov(cPnt);
1368
            do {
1369
                float x = (float(rand()) / float(RAND_MAX));
1370
                float y = (float(rand()) / float(RAND_MAX));
1371
                float z = (float(rand()) / float(RAND_MAX));
1372
                cMov.Move(x, y, z);
1373
            } while (cMov.DistanceToLine(cBase, cAxis) == 0);
1374

1375
            Base::Vector3f proj;
1376
            cMov.ProjectToPlane(cPnt, cAxis, proj);
1377
            Base::Vector3f diff = cPnt - proj;
1378
            diff.Normalize();
1379
            cPnt = proj + diff * _fRadius;
1380
        }
1381
    }
1382
}
1383

1384
// -----------------------------------------------------------------------------
1385

1386
SphereFit::SphereFit()
1387
    : _vCenter(0, 0, 0)
1388
{}
1389

1390
float SphereFit::GetRadius() const
1391
{
1392
    if (_bIsFitted) {
1393
        return _fRadius;
1394
    }
1395
    else {
1396
        return FLOAT_MAX;
1397
    }
1398
}
1399

1400
Base::Vector3f SphereFit::GetCenter() const
1401
{
1402
    if (_bIsFitted) {
1403
        return _vCenter;
1404
    }
1405
    else {
1406
        return Base::Vector3f();
1407
    }
1408
}
1409

1410
float SphereFit::Fit()
1411
{
1412
    _bIsFitted = true;
1413
    if (CountPoints() < 4) {
1414
        return FLOAT_MAX;
1415
    }
1416

1417
    std::vector<Wm4::Vector3d> input;
1418
    std::transform(_vPoints.begin(),
1419
                   _vPoints.end(),
1420
                   std::back_inserter(input),
1421
                   [](const Base::Vector3f& v) {
1422
                       return Wm4::Vector3d(v.x, v.y, v.z);
1423
                   });
1424

1425
    Wm4::Sphere3d sphere;
1426
    Wm4::SphereFit3<double>(input.size(), input.data(), 10, sphere, false);
1427
    _vCenter = Base::convertTo<Base::Vector3f>(sphere.Center);
1428
    _fRadius = float(sphere.Radius);
1429

1430
    // TODO
1431
    _fLastResult = 0;
1432

1433
#if defined(_DEBUG)
1434
    Base::Console().Message("   WildMagic Sphere Fit:  Center: (%0.4f, %0.4f, %0.4f),  Radius: "
1435
                            "%0.4f,  Std Dev: %0.4f\n",
1436
                            _vCenter.x,
1437
                            _vCenter.y,
1438
                            _vCenter.z,
1439
                            _fRadius,
1440
                            GetStdDeviation());
1441
#endif
1442

1443
    MeshCoreFit::SphereFit sphereFit;
1444
    sphereFit.AddPoints(_vPoints);
1445
    sphereFit.ComputeApproximations();
1446
    float result = sphereFit.Fit();
1447
    if (result < FLOAT_MAX) {
1448
        Base::Vector3d center = sphereFit.GetCenter();
1449
#if defined(_DEBUG)
1450
        Base::Console().Message("MeshCoreFit::Sphere Fit:  Center: (%0.4f, %0.4f, %0.4f),  Radius: "
1451
                                "%0.4f,  Std Dev: %0.4f,  Iterations: %d\n",
1452
                                center.x,
1453
                                center.y,
1454
                                center.z,
1455
                                sphereFit.GetRadius(),
1456
                                sphereFit.GetStdDeviation(),
1457
                                sphereFit.GetNumIterations());
1458
#endif
1459
        _vCenter = Base::convertTo<Base::Vector3f>(center);
1460
        _fRadius = (float)sphereFit.GetRadius();
1461
        _fLastResult = result;
1462
    }
1463

1464
    return _fLastResult;
1465
}
1466

1467
float SphereFit::GetDistanceToSphere(const Base::Vector3f& rcPoint) const
1468
{
1469
    float fResult = FLOAT_MAX;
1470
    if (_bIsFitted) {
1471
        fResult = Base::Vector3f(rcPoint - _vCenter).Length() - _fRadius;
1472
    }
1473
    return fResult;
1474
}
1475

1476
float SphereFit::GetStdDeviation() const
1477
{
1478
    // Mean: M=(1/N)*SUM Xi
1479
    // Variance: VAR=(N/N-1)*[(1/N)*SUM(Xi^2)-M^2]
1480
    // Standard deviation: SD=SQRT(VAR)
1481
    // Standard error of the mean: SE=SD/SQRT(N)
1482
    if (!_bIsFitted) {
1483
        return FLOAT_MAX;
1484
    }
1485

1486
    float fSumXi = 0.0f, fSumXi2 = 0.0f, fMean = 0.0f, fDist = 0.0f;
1487

1488
    float ulPtCt = float(CountPoints());
1489
    std::list<Base::Vector3f>::const_iterator cIt;
1490

1491
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
1492
        fDist = GetDistanceToSphere(*cIt);
1493
        fSumXi += fDist;
1494
        fSumXi2 += (fDist * fDist);
1495
    }
1496

1497
    fMean = (1.0f / ulPtCt) * fSumXi;
1498
    return sqrt((ulPtCt / (ulPtCt - 1.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean));
1499
}
1500

1501
void SphereFit::ProjectToSphere()
1502
{
1503
    for (auto& cPnt : _vPoints) {
1504
        // Compute unit vector from sphere centre to point.
1505
        // Because this vector is orthogonal to the sphere's surface at the
1506
        // intersection point we can easily compute the projection point on the
1507
        // closest surface point using the radius of the sphere
1508
        Base::Vector3f diff = cPnt - _vCenter;
1509
        double length = diff.Length();
1510
        if (length == 0.0) {
1511
            // Point is exactly at the sphere center, so it can be projected in any direction onto
1512
            // the sphere! So here just project in +Z direction
1513
            cPnt.z += _fRadius;
1514
        }
1515
        else {
1516
            diff /= length;  // normalizing the vector
1517
            cPnt = _vCenter + diff * _fRadius;
1518
        }
1519
    }
1520
}
1521

1522
// -------------------------------------------------------------------------------
1523

1524
PolynomialFit::PolynomialFit()
1525
    : _fCoeff {}
1526
{}
1527

1528
float PolynomialFit::Fit()
1529
{
1530
    std::vector<float> x, y, z;
1531
    x.reserve(_vPoints.size());
1532
    y.reserve(_vPoints.size());
1533
    z.reserve(_vPoints.size());
1534
    for (std::list<Base::Vector3f>::const_iterator it = _vPoints.begin(); it != _vPoints.end();
1535
         ++it) {
1536
        x.push_back(it->x);
1537
        y.push_back(it->y);
1538
        z.push_back(it->z);
1539
    }
1540

1541
    try {
1542
        float* coeff = Wm4::PolyFit3<float>(_vPoints.size(), &(x[0]), &(y[0]), &(z[0]), 2, 2);
1543
        for (int i = 0; i < 9; i++) {
1544
            _fCoeff[i] = coeff[i];
1545
        }
1546
    }
1547
    catch (const std::exception&) {
1548
        return FLOAT_MAX;
1549
    }
1550

1551
    return 0.0f;
1552
}
1553

1554
float PolynomialFit::Value(float x, float y) const
1555
{
1556
    float fValue = _fCoeff[0] + _fCoeff[1] * x + _fCoeff[2] * x * x + _fCoeff[3] * y
1557
        + _fCoeff[4] * x * y + _fCoeff[5] * x * x * y + _fCoeff[6] * y * y + _fCoeff[7] * x * y * y
1558
        + _fCoeff[8] * x * x * y * y;
1559
    return fValue;
1560
}
1561