diff --git a/src/Mod/Mesh/App/CMakeLists.txt b/src/Mod/Mesh/App/CMakeLists.txt
index ca6c698a70..7ef7aa1354 100644
--- a/src/Mod/Mesh/App/CMakeLists.txt
+++ b/src/Mod/Mesh/App/CMakeLists.txt
@@ -94,6 +94,10 @@ SET(Core_SRCS
Core/Utilities.h
Core/Visitor.cpp
Core/Visitor.h
+ Core/CylinderFit.cpp
+ Core/CylinderFit.h
+ Core/SphereFit.cpp
+ Core/SphereFit.h
)
SOURCE_GROUP("Core" FILES ${Core_SRCS})
diff --git a/src/Mod/Mesh/App/Core/Approximation.cpp b/src/Mod/Mesh/App/Core/Approximation.cpp
index 97658d35fa..c04343db0d 100644
--- a/src/Mod/Mesh/App/Core/Approximation.cpp
+++ b/src/Mod/Mesh/App/Core/Approximation.cpp
@@ -32,6 +32,8 @@
#include "Approximation.h"
#include "Elements.h"
#include "Utilities.h"
+#include "CylinderFit.h"
+#include "SphereFit.h"
#include
#include
@@ -1051,6 +1053,29 @@ float CylinderFit::Fit()
_fRadius = float(radius);
_fLastResult = double(fit);
+
+#if defined(_DEBUG)
+ Base::Console().Message(" WildMagic Cylinder Fit: Base: (%0.4f, %0.4f, %0.4f), Axis: (%0.6f, %0.6f, %0.6f), Radius: %0.4f, Std Dev: %0.4f\n",
+ _vBase.x, _vBase.y, _vBase.z, _vAxis.x, _vAxis.y, _vAxis.z, _fRadius, GetStdDeviation());
+#endif
+
+ MeshCoreFit::CylinderFit cylFit;
+ cylFit.AddPoints(_vPoints);
+ //cylFit.SetApproximations(_fRadius, Base::Vector3d(_vBase.x, _vBase.y, _vBase.z), Base::Vector3d(_vAxis.x, _vAxis.y, _vAxis.z));
+
+ float result = cylFit.Fit();
+ if (result < FLOAT_MAX) {
+ Base::Vector3d base = cylFit.GetBase();
+ Base::Vector3d dir = cylFit.GetAxis();
+#if defined(_DEBUG)
+ Base::Console().Message("MeshCoreFit::Cylinder Fit: Base: (%0.4f, %0.4f, %0.4f), Axis: (%0.6f, %0.6f, %0.6f), Radius: %0.4f, Std Dev: %0.4f, Iterations: %d\n",
+ base.x, base.y, base.z, dir.x, dir.y, dir.z, cylFit.GetRadius(), cylFit.GetStdDeviation(), cylFit.GetNumIterations());
+#endif
+ _vBase = Base::convertTo(base);
+ _vAxis = Base::convertTo(dir);
+ _fRadius = (float)cylFit.GetRadius();
+ _fLastResult = result;
+ }
#else
int m = static_cast(_vPoints.size());
int n = 7;
@@ -1238,24 +1263,88 @@ float SphereFit::Fit()
_fRadius = float(sphere.Radius);
// TODO
-
_fLastResult = 0;
+
+#if defined(_DEBUG)
+ Base::Console().Message(" WildMagic Sphere Fit: Center: (%0.4f, %0.4f, %0.4f), Radius: %0.4f, Std Dev: %0.4f\n",
+ _vCenter.x, _vCenter.y, _vCenter.z, _fRadius, GetStdDeviation());
+#endif
+
+ MeshCoreFit::SphereFit sphereFit;
+ sphereFit.AddPoints(_vPoints);
+ sphereFit.ComputeApproximations();
+ float result = sphereFit.Fit();
+ if (result < FLOAT_MAX) {
+ Base::Vector3d center = sphereFit.GetCenter();
+#if defined(_DEBUG)
+ Base::Console().Message("MeshCoreFit::Sphere Fit: Center: (%0.4f, %0.4f, %0.4f), Radius: %0.4f, Std Dev: %0.4f, Iterations: %d\n",
+ center.x, center.y, center.z, sphereFit.GetRadius(), sphereFit.GetStdDeviation(), sphereFit.GetNumIterations());
+#endif
+ _vCenter = Base::convertTo(center);
+ _fRadius = (float)sphereFit.GetRadius();
+ _fLastResult = result;
+ }
+
return _fLastResult;
}
-float SphereFit::GetDistanceToSphere(const Base::Vector3f &) const
+float SphereFit::GetDistanceToSphere(const Base::Vector3f& rcPoint) const
{
- return FLOAT_MAX;
+ float fResult = FLOAT_MAX;
+ if (_bIsFitted) {
+ fResult = Base::Vector3f(rcPoint - _vCenter).Length() - _fRadius;
+ }
+ return fResult;
}
float SphereFit::GetStdDeviation() const
{
- return FLOAT_MAX;
+ // Mean: M=(1/N)*SUM Xi
+ // Variance: VAR=(N/N-3)*[(1/N)*SUM(Xi^2)-M^2]
+ // Standard deviation: SD=SQRT(VAR)
+ // Standard error of the mean: SE=SD/SQRT(N)
+ if (!_bIsFitted)
+ return FLOAT_MAX;
+
+ float fSumXi = 0.0f, fSumXi2 = 0.0f,
+ fMean = 0.0f, fDist = 0.0f;
+
+ float ulPtCt = float(CountPoints());
+ std::list< Base::Vector3f >::const_iterator cIt;
+
+ for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
+ fDist = GetDistanceToSphere( *cIt );
+ fSumXi += fDist;
+ fSumXi2 += ( fDist * fDist );
+ }
+
+ fMean = (1.0f / ulPtCt) * fSumXi;
+ return sqrt((ulPtCt / (ulPtCt - 3.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean));
}
void SphereFit::ProjectToSphere()
{
+ for (std::list< Base::Vector3f >::iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) {
+ Base::Vector3f& cPnt = *it;
+ // Compute unit vector from sphere centre to point.
+ // Because this vector is orthogonal to the sphere's surface at the
+ // intersection point we can easily compute the projection point on the
+ // closest surface point using the radius of the sphere
+ Base::Vector3f diff = cPnt - _vCenter;
+ double length = diff.Length();
+ if (length == 0.0)
+ {
+ // Point is exactly at the sphere center, so it can be projected in any direction onto the sphere!
+ // So here just project in +Z direction
+ cPnt.z += _fRadius;
+ }
+ else
+ {
+ diff /= length; // normalizing the vector
+ cPnt = _vCenter + diff * _fRadius;
+ }
+ }
}
// -------------------------------------------------------------------------------
diff --git a/src/Mod/Mesh/App/Core/CylinderFit.cpp b/src/Mod/Mesh/App/Core/CylinderFit.cpp
new file mode 100644
index 0000000000..3cd2fa770a
--- /dev/null
+++ b/src/Mod/Mesh/App/Core/CylinderFit.cpp
@@ -0,0 +1,654 @@
+/***************************************************************************
+ * Copyright (c) 2020 Graeme van der Vlugt *
+ * *
+ * This file is part of the FreeCAD CAx development system. *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of the GNU Library General Public *
+ * License as published by the Free Software Foundation; either *
+ * version 2 of the License, or (at your option) any later version. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU Library General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU Library General Public *
+ * License along with this library; see the file COPYING.LIB. If not, *
+ * write to the Free Software Foundation, Inc., 59 Temple Place, *
+ * Suite 330, Boston, MA 02111-1307, USA *
+ * *
+ ***************************************************************************/
+
+// Definitions:
+// Cylinder axis goes through a point (Xc,Yc,Zc) and has direction (L,M,N)
+// Cylinder radius is R
+// A point on the axis (X0i,Y0i,Z0i) can be described by:
+// (X0i,Y0i,Z0i) = (Xc,Yc,Zc) + s(L,M,N)
+// where s is the distance from (Xc,Yc,Zc) to (X0i,Y0i,Z0i) when (L,M,N) is
+// of unit length (normalized).
+// The distance between a cylinder surface point (Xi,Yi,Zi) and its
+// projection onto the axis (X0i,Y0i,Z0i) is the radius:
+// (Xi - X0i)^2 + (Yi - Y0i)^2 + (Zi - Z0i)^2 = R^2
+// Also the vector to a cylinder surface point (Xi,Yi,Zi) from its
+// projection onto the axis (X0i,Y0i,Z0i) is orthogonal to the axis so we can
+// write:
+// (Xi - X0i, Yi - Y0i, Zi - Z0i).(L,M,N) = 0 or
+// L(Xi - X0i) + M(Yi - Y0i) + N(Zi - Z0i) = 0
+// If we substitute these various equations into each other and further add
+// the constraint that L^2 + M^2 + N^2 = 1 then we can arrive at a single
+// equation for the cylinder surface points:
+// (Xi - Xc + L*L*(Xc - Xi) + L*M*(Yc - Yi) + L*N*(Zc - Zi))^2 +
+// (Yi - Yc + M*L*(Xc - Xi) + M*M*(Yc - Yi) + M*N*(Zc - Zi))^2 +
+// (Zi - Zc + N*L*(Xc - Xi) + N*M*(Yc - Yi) + N*N*(Zc - Zi))^2 - R^2 = 0
+// This equation is what is used in the least squares solution below. Because
+// we are constraining the direction vector to a unit length and also because
+// we need to stop the axis point from moving along the axis we need to fix one
+// of the ordinates in the solution. So from our initial approximations for the
+// axis direction (L0,M0,N0):
+// if (L0 > M0) && (L0 > N0) then fix Xc = 0 and use L = sqrt(1 - M^2 - N^2)
+// else if (M0 > L0) && (M0 > N0) then fix Yc = 0 and use M = sqrt(1 - L^2 - N^2)
+// else if (N0 > L0) && (N0 > M0) then fix Zc = 0 and use N = sqrt(1 - L^2 - M^2)
+// We thus solve for 5 unknown parameters.
+// Thus for the solution to succeed the initial axis direction should be reasonable.
+
+#include "PreCompiled.h"
+
+#ifndef _PreComp_
+# include
+# include
+# include
+#endif
+
+#include "CylinderFit.h"
+#include
+#include
+
+using namespace MeshCoreFit;
+
+CylinderFit::CylinderFit()
+ : _vBase(0,0,0)
+ , _vAxis(0,0,1)
+ , _dRadius(0)
+ , _numIter(0)
+ , _posConvLimit(0.0001)
+ , _dirConvLimit(0.000001)
+ , _vConvLimit(0.001)
+ , _maxIter(50)
+{
+}
+
+CylinderFit::~CylinderFit()
+{
+}
+
+// Set approximations before calling the fitting
+void CylinderFit::SetApproximations(double radius, const Base::Vector3d &base, const Base::Vector3d &axis)
+{
+ _bIsFitted = false;
+ _fLastResult = FLOAT_MAX;
+ _numIter = 0;
+ _dRadius = radius;
+ _vBase = base;
+ _vAxis = axis;
+ _vAxis.Normalize();
+}
+
+// Set iteration convergence criteria for the fit if special values are needed.
+// The default values set in the constructor are suitable for most uses
+void CylinderFit::SetConvergenceCriteria(double posConvLimit, double dirConvLimit, double vConvLimit, int maxIter)
+{
+ if (posConvLimit > 0.0)
+ _posConvLimit = posConvLimit;
+ if (dirConvLimit > 0.0)
+ _dirConvLimit = dirConvLimit;
+ if (vConvLimit > 0.0)
+ _vConvLimit = vConvLimit;
+ if (maxIter > 0)
+ _maxIter = maxIter;
+}
+
+
+double CylinderFit::GetRadius() const
+{
+ if (_bIsFitted)
+ return _dRadius;
+ else
+ return 0.0;
+}
+
+Base::Vector3d CylinderFit::GetBase() const
+{
+ if (_bIsFitted)
+ return _vBase;
+ else
+ return Base::Vector3d();
+}
+
+Base::Vector3d CylinderFit::GetAxis() const
+{
+ if (_bIsFitted)
+ return _vAxis;
+ else
+ return Base::Vector3d();
+}
+
+int CylinderFit::GetNumIterations() const
+{
+ if (_bIsFitted)
+ return _numIter;
+ else
+ return 0;
+}
+
+float CylinderFit::GetDistanceToCylinder(const Base::Vector3f &rcPoint) const
+{
+ float fResult = FLOAT_MAX;
+ if (_bIsFitted)
+ {
+ fResult = Base::Vector3d(rcPoint.x, rcPoint.y, rcPoint.z).DistanceToLine(_vBase, _vAxis) - _dRadius;
+ }
+ return fResult;
+}
+
+float CylinderFit::GetStdDeviation() const
+{
+ // Mean: M=(1/N)*SUM Xi
+ // Variance: VAR=(N/N-3)*[(1/N)*SUM(Xi^2)-M^2]
+ // Standard deviation: SD=SQRT(VAR)
+ // Standard error of the mean: SE=SD/SQRT(N)
+ if (!_bIsFitted)
+ return FLOAT_MAX;
+
+ float fSumXi = 0.0f, fSumXi2 = 0.0f,
+ fMean = 0.0f, fDist = 0.0f;
+
+ float ulPtCt = float(CountPoints());
+ std::list< Base::Vector3f >::const_iterator cIt;
+
+ for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
+ fDist = GetDistanceToCylinder( *cIt );
+ fSumXi += fDist;
+ fSumXi2 += ( fDist * fDist );
+ }
+
+ fMean = (1.0f / ulPtCt) * fSumXi;
+ return sqrt((ulPtCt / (ulPtCt - 3.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean));
+}
+
+void CylinderFit::ProjectToCylinder()
+{
+ Base::Vector3f cBase(_vBase.x, _vBase.y, _vBase.z);
+ Base::Vector3f cAxis(_vAxis.x, _vAxis.y, _vAxis.z);
+
+ for (std::list< Base::Vector3f >::iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) {
+ Base::Vector3f& cPnt = *it;
+ if (cPnt.DistanceToLine(cBase, cAxis) > 0) {
+ Base::Vector3f proj;
+ cBase.ProjectToPlane(cPnt, cAxis, proj);
+ Base::Vector3f diff = cPnt - proj;
+ diff.Normalize();
+ cPnt = proj + diff * _dRadius;
+ }
+ else {
+ // Point is on the cylinder axis, so it can be moved in
+ // any direction perpendicular to the cylinder axis
+ Base::Vector3f cMov(cPnt);
+ do {
+ float x = (float(rand()) / float(RAND_MAX));
+ float y = (float(rand()) / float(RAND_MAX));
+ float z = (float(rand()) / float(RAND_MAX));
+ cMov.Move(x,y,z);
+ }
+ while (cMov.DistanceToLine(cBase, cAxis) == 0);
+
+ Base::Vector3f proj;
+ cMov.ProjectToPlane(cPnt, cAxis, proj);
+ Base::Vector3f diff = cPnt - proj;
+ diff.Normalize();
+ cPnt = proj + diff * _dRadius;
+ }
+ }
+}
+
+// Compute approximations for the parameters using all points by computing a
+// line through the points. This doesn't work well if the points are only from
+// one small surface area.
+// In that case rather use SetApproximations() with a better estimate.
+void CylinderFit::ComputeApproximationsLine()
+{
+ _bIsFitted = false;
+ _fLastResult = FLOAT_MAX;
+ _numIter = 0;
+ _vBase.Set(0.0, 0.0, 0.0);
+ _vAxis.Set(0.0, 0.0, 0.0);
+ _dRadius = 0.0;
+ if (_vPoints.size() > 0)
+ {
+ std::vector input;
+ std::transform(_vPoints.begin(), _vPoints.end(), std::back_inserter(input),
+ [](const Base::Vector3f& v) { return Wm4::Vector3d(v.x, v.y, v.z); });
+ Wm4::Line3 kLine = Wm4::OrthogonalLineFit3(input.size(), input.data());
+ _vBase.Set(kLine.Origin.X(), kLine.Origin.Y(), kLine.Origin.Z());
+ _vAxis.Set(kLine.Direction.X(), kLine.Direction.Y(), kLine.Direction.Z());
+
+ for (std::list< Base::Vector3f >::const_iterator cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt)
+ _dRadius += Base::Vector3d(cIt->x, cIt->y, cIt->z).DistanceToLine(_vBase, _vAxis);
+ _dRadius /= (double)_vPoints.size();
+ }
+}
+
+float CylinderFit::Fit()
+{
+ _bIsFitted = false;
+ _fLastResult = FLOAT_MAX;
+ _numIter = 0;
+
+ // A minimum of 5 surface points is needed to define a cylinder
+ if (CountPoints() < 5)
+ return FLOAT_MAX;
+
+ // If approximations have not been set/computed then compute some now using the line fit method
+ if (_dRadius == 0.0)
+ ComputeApproximationsLine();
+
+ // Check parameters to define the best solution direction
+ // There are 7 parameters but 2 are actually dependent on the others
+ // so we are actually solving for 5 parameters.
+ // order of parameters depending on the solution direction:
+ // solL: Yc, Zc, M, N, R
+ // solM: Xc, Zc, L, N, R
+ // solN: Xc, Yc, L, M, R
+ SolutionD solDir;
+ findBestSolDirection(solDir);
+
+ // Initialise some matrices and vectors
+ std::vector< Base::Vector3d > residuals(CountPoints(), Base::Vector3d(0.0, 0.0, 0.0));
+ Matrix5x5 atpa;
+ Eigen::VectorXd atpl(5);
+
+ // Iteration loop...
+ double sigma0;
+ bool cont = true;
+ while (cont && (_numIter < _maxIter))
+ {
+ ++_numIter;
+
+ // Set up the quasi parameteric normal equations
+ setupNormalEquationMatrices(solDir, residuals, atpa, atpl);
+
+ // Solve the equations for the unknown corrections
+ Eigen::LLT< Matrix5x5 > llt(atpa);
+ if (llt.info() != Eigen::Success)
+ return FLOAT_MAX;
+ Eigen::VectorXd x = llt.solve(atpl);
+
+ // Check parameter convergence
+ cont = false;
+ if ((fabs(x(0)) > _posConvLimit) || (fabs(x(1)) > _posConvLimit) || // the two position parameter corrections
+ (fabs(x(2)) > _dirConvLimit) || (fabs(x(3)) > _dirConvLimit) || // the two direction parameter corrections
+ (fabs(x(4)) > _posConvLimit)) // the radius correction
+ cont = true;
+
+ // Before updating the unknowns, compute the residuals and sigma0 and check the residual convergence
+ bool vConverged;
+ if (!computeResiduals(solDir, x, residuals, sigma0, _vConvLimit, vConverged))
+ return FLOAT_MAX;
+ if (!vConverged)
+ cont = true;
+
+ // Update the parameters
+ if (!updateParameters(solDir, x))
+ return FLOAT_MAX;
+ }
+
+ // Check for convergence
+ if (cont)
+ return FLOAT_MAX;
+
+ _bIsFitted = true;
+ _fLastResult = sigma0;
+
+ return _fLastResult;
+}
+
+// Checks initial parameter values and defines the best solution direction to use
+// Axis direction = (L,M,N)
+// solution L: L is biggest axis component and L = f(M,N) and X = 0 (we move the base point along axis so that x = 0)
+// solution M: M is biggest axis component and M = f(L,N) and Y = 0 (we move the base point along axis so that y = 0)
+// solution N: N is biggest axis component and N = f(L,M) and Z = 0 (we move the base point along axis so that z = 0)
+// IMPLEMENT: use fix X,Y,or Z to value of associated centre of gravity coordinate
+// (because 0 could be along way away from cylinder points)
+void CylinderFit::findBestSolDirection(SolutionD &solDir)
+{
+ // Choose the best of the three solution 'directions' to use
+ // This is to avoid a square root of a negative number when computing the dependent parameters
+ Base::Vector3d dir = _vAxis;
+ Base::Vector3d pos = _vBase;
+ dir.Normalize();
+ double biggest = dir.x;
+ solDir = solL;
+ if (fabs (dir.y) > fabs (biggest))
+ {
+ biggest = dir.y;
+ solDir = solM;
+ }
+ if (fabs (dir.z) > fabs (biggest))
+ {
+ biggest = dir.z;
+ solDir = solN;
+ }
+ if (biggest < 0.0)
+ dir.Set(-dir.x, -dir.y, -dir.z); // multiplies by -1
+
+ double lambda;
+ switch (solDir)
+ {
+ case solL:
+ lambda = -pos.x / dir.x;
+ pos.x = 0.0;
+ pos.y = pos.y + lambda * dir.y;
+ pos.z = pos.z + lambda * dir.z;
+ break;
+ case solM:
+ lambda = -pos.y / dir.y;
+ pos.x = pos.x + lambda * dir.x;
+ pos.y = 0.0;
+ pos.z = pos.z + lambda * dir.z;
+ break;
+ case solN:
+ lambda = -pos.z / dir.z;
+ pos.x = pos.x + lambda * dir.x;
+ pos.y = pos.y + lambda * dir.y;
+ pos.z = 0.0;
+ break;
+ }
+ _vAxis = dir;
+ _vBase = pos;
+}
+
+// Set up the normal equation matrices
+// atpa ... 5x5 normal matrix
+// atpl ... 5x1 matrix (right-hand side of equation)
+void CylinderFit::setupNormalEquationMatrices(SolutionD solDir, const std::vector< Base::Vector3d > &residuals, Matrix5x5 &atpa, Eigen::VectorXd &atpl) const
+{
+ // Zero matrices
+ atpa.setZero();
+ atpl.setZero();
+
+ // For each point, setup the observation equation coefficients and add their
+ // contribution into the the normal equation matrices
+ double a[5], b[3];
+ double f0, qw;
+ std::vector< Base::Vector3d >::const_iterator vIt = residuals.begin();
+ std::list< Base::Vector3f >::const_iterator cIt;
+ for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt)
+ {
+ // if (using this point) { // currently all given points are used (could modify this if eliminating outliers, etc....
+ setupObservation(solDir, *cIt, *vIt, a, f0, qw, b);
+ addObservationU(a, f0, qw, atpa, atpl);
+ // }
+ }
+ setLowerPart(atpa);
+}
+
+// Sets up contributions of given observation to the quasi parameteric
+// normal equation matrices. Assumes uncorrelated coordinates.
+// point ... point
+// residual ... residual for this point computed from previous iteration (zero for first iteration)
+// a[5] ... parameter partials
+// f0 ... reference to f0 term
+// qw ... reference to quasi weight (here we are assuming equal unit weights for each observed point coordinate)
+// b[3] ... observation partials
+void CylinderFit::setupObservation(SolutionD solDir, const Base::Vector3f &point, const Base::Vector3d &residual, double a[5], double &f0, double &qw, double b[3]) const
+{
+ // This adjustment requires an update of the observation approximations
+ // because the residuals do not have a linear relationship.
+ // New estimates for the observations:
+ double xEstimate = (double)point.x + residual.x;
+ double yEstimate = (double)point.y + residual.y;
+ double zEstimate = (double)point.z + residual.z;
+
+ // intermediate parameters
+ double lambda = _vAxis.x * (xEstimate - _vBase.x) + _vAxis.y * (yEstimate - _vBase.y) + _vAxis.z * (zEstimate - _vBase.z);
+ double x0 = _vBase.x + lambda * _vAxis.x;
+ double y0 = _vBase.y + lambda * _vAxis.y;
+ double z0 = _vBase.z + lambda * _vAxis.z;
+ double dx = xEstimate - x0;
+ double dy = yEstimate - y0;
+ double dz = zEstimate - z0;
+ double dx00 = _vBase.x - xEstimate;
+ double dy00 = _vBase.y - yEstimate;
+ double dz00 = _vBase.z - zEstimate;
+
+ // partials of the observations
+ b[0] = 2.0 * (dx - _vAxis.x * _vAxis.x * dx - _vAxis.x * _vAxis.y * dy - _vAxis.x * _vAxis.z * dz);
+ b[1] = 2.0 * (dy - _vAxis.x * _vAxis.y * dx - _vAxis.y * _vAxis.y * dy - _vAxis.y * _vAxis.z * dz);
+ b[2] = 2.0 * (dz - _vAxis.x * _vAxis.z * dx - _vAxis.y * _vAxis.z * dy - _vAxis.z * _vAxis.z * dz);
+
+ // partials of the parameters
+ switch (solDir)
+ {
+ double ddxdl, ddydl, ddzdl;
+ double ddxdm, ddydm, ddzdm;
+ double ddxdn, ddydn, ddzdn;
+ case solL:
+ // order of parameters: Yc, Zc, M, N, R
+ ddxdm = -2.0 * _vAxis.y * dx00 + (_vAxis.x - _vAxis.y * _vAxis.y / _vAxis.x) * dy00 - (_vAxis.y * _vAxis.z / _vAxis.x) * dz00;
+ ddydm = (_vAxis.x - _vAxis.y * _vAxis.y / _vAxis.x) * dx00 + 2.0 * _vAxis.y * dy00 + _vAxis.z * dz00;
+ ddzdm = -(_vAxis.y * _vAxis.z / _vAxis.x) * dx00 + _vAxis.z * dy00;
+ ddxdn = -2.0 * _vAxis.z * dx00 - (_vAxis.y * _vAxis.z / _vAxis.x) * dy00 + (_vAxis.x - _vAxis.z * _vAxis.z / _vAxis.x) * dz00;
+ ddydn = -(_vAxis.y * _vAxis.z / _vAxis.x) * dx00 + _vAxis.y * dz00;
+ ddzdn = (_vAxis.x - _vAxis.z * _vAxis.z / _vAxis.x) * dx00 + _vAxis.y * dy00 + 2.0 * _vAxis.z * dz00;
+ a[0] = -b[1];
+ a[1] = -b[2];
+ a[2] = 2.0 * (dx * ddxdm + dy * ddydm + dz * ddzdm);
+ a[3] = 2.0 * (dx * ddxdn + dy * ddydn + dz * ddzdn);
+ a[4] = -2.0 * _dRadius;
+ break;
+ case solM:
+ // order of parameters: Xc, Zc, L, N, R
+ ddxdl = 2.0 * _vAxis.x * dx00 + (_vAxis.y - _vAxis.x * _vAxis.x / _vAxis.y) * dy00 + _vAxis.z * dz00;
+ ddydl = (_vAxis.y - _vAxis.x * _vAxis.x / _vAxis.y) * dx00 - 2.0 * _vAxis.x * dy00 - (_vAxis.x * _vAxis.z / _vAxis.y) * dz00;
+ ddzdl = _vAxis.z * dx00 - (_vAxis.x * _vAxis.z / _vAxis.y) * dy00;
+ ddxdn = -(_vAxis.x * _vAxis.z / _vAxis.y) * dy00 + _vAxis.x * dz00;
+ ddydn = -(_vAxis.x * _vAxis.z / _vAxis.y) * dx00 - 2.0 * _vAxis.z * dy00 + (_vAxis.y - _vAxis.z * _vAxis.z / _vAxis.y) * dz00;
+ ddzdn = _vAxis.x * dx00 + (_vAxis.y - _vAxis.z * _vAxis.z / _vAxis.y) * dy00 + 2.0 * _vAxis.z * dz00;
+ a[0] = -b[0];
+ a[1] = -b[2];
+ a[2] = 2.0 * (dx * ddxdl + dy * ddydl + dz * ddzdl);
+ a[3] = 2.0 * (dx * ddxdn + dy * ddydn + dz * ddzdn);
+ a[4] = -2.0 * _dRadius;
+ break;
+ case solN:
+ // order of parameters: Xc, Yc, L, M, R
+ ddxdl = 2.0 * _vAxis.x * dx00 + _vAxis.y * dy00 + (_vAxis.z - _vAxis.x * _vAxis.x / _vAxis.z) * dz00;
+ ddydl = _vAxis.y * dx00 - (_vAxis.x * _vAxis.y / _vAxis.z) * dz00;
+ ddzdl = (_vAxis.z - _vAxis.x * _vAxis.x / _vAxis.z) * dx00 - (_vAxis.x * _vAxis.y / _vAxis.z) * dy00 - 2.0 * _vAxis.x * dz00;
+ ddxdm = _vAxis.x * dy00 - (_vAxis.x * _vAxis.y / _vAxis.z) * dz00;
+ ddydm = _vAxis.x * dx00 + 2.0 * _vAxis.y * dy00 + (_vAxis.z - _vAxis.y * _vAxis.y / _vAxis.z) * dz00;
+ ddzdm = - (_vAxis.x * _vAxis.y / _vAxis.z) * dx00 + (_vAxis.z - _vAxis.y * _vAxis.y / _vAxis.z) * dy00 - 2.0 * _vAxis.y * dz00;
+ a[0] = -b[0];
+ a[1] = -b[1];
+ a[2] = 2.0 * (dx * ddxdl + dy * ddydl + dz * ddzdl);
+ a[3] = 2.0 * (dx * ddxdm + dy * ddydm + dz * ddzdm);
+ a[4] = -2.0 * _dRadius;
+ break;
+ }
+
+ // free term
+ f0 = _dRadius * _dRadius - dx * dx - dy * dy - dz * dz + b[0] * residual.x + b[1] * residual.y + b[2] * residual.z;
+
+ // quasi weight (using equal weights for cylinder point coordinate observations)
+ //w[0] = 1.0;
+ //w[1] = 1.0;
+ //w[2] = 1.0;
+ //qw = 1.0 / (b[0] * b[0] / w[0] + b[1] * b[1] / w[1] + b[2] * b[2] / w[2]);
+ qw = 1.0 / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
+}
+
+// Computes contribution of the given observation equation on the normal equation matrices
+// Call this for each observation (point)
+// Here we only add the contribution to the upper part of the normal matrix
+// and then after all observations have been added we need to set the lower part
+// (which is symmetrical to the upper part)
+// a[5] ... parameter partials
+// li ... free term (f0)
+// pi ... weight of observation (= quasi weight qw for this solution)
+// atpa ... 5x5 normal equation matrix
+// atpl ... 5x1 matrix/vector (right-hand side of equations)
+void CylinderFit::addObservationU(double a[5], double li, double pi, Matrix5x5 &atpa, Eigen::VectorXd &atpl) const
+{
+ for (int i = 0; i < 5; ++i)
+ {
+ double aipi = a[i] * pi;
+ for (int j = i; j < 5; ++j)
+ {
+ atpa(i, j) += aipi * a[j];
+ //atpa(j, i) = atpa(i, j); // it's a symmetrical matrix, we'll set this later after all observations processed
+ }
+ atpl(i) += aipi * li;
+ }
+}
+
+// Set the lower part of the normal matrix equal to the upper part
+// This is done after all the observations have been added
+void CylinderFit::setLowerPart(Matrix5x5 &atpa) const
+{
+ for (int i = 0; i < 5; ++i)
+ for (int j = i+1; j < 5; ++j) // skip the diagonal elements
+ atpa(j, i) = atpa(i, j);
+}
+
+// Compute the residuals and sigma0 and check the residual convergence
+bool CylinderFit::computeResiduals(SolutionD solDir, const Eigen::VectorXd &x, std::vector< Base::Vector3d > &residuals, double &sigma0, double vConvLimit, bool &vConverged) const
+{
+ vConverged = true;
+ int nPtsUsed = 0;
+ sigma0 = 0.0;
+ double a[5], b[3];
+ double f0, qw;
+ //double maxdVx = 0.0;
+ //double maxdVy = 0.0;
+ //double maxdVz = 0.0;
+ //double rmsVv = 0.0;
+ std::vector< Base::Vector3d >::iterator vIt = residuals.begin();
+ std::list< Base::Vector3f >::const_iterator cIt;
+ for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt)
+ {
+ // if (using this point) { // currently all given points are used (could modify this if eliminating outliers, etc....
+ ++nPtsUsed;
+ Base::Vector3d &v = *vIt;
+ setupObservation(solDir, *cIt, v, a, f0, qw, b);
+ double qv = -f0;
+ for (int i = 0; i < 5; ++i)
+ qv += a[i] * x(i);
+
+ // We are using equal weights for cylinder point coordinate observations (see setupObservation)
+ // i.e. w[0] = w[1] = w[2] = 1.0;
+ //double vx = -qw * qv * b[0] / w[0];
+ //double vy = -qw * qv * b[1] / w[1];
+ //double vz = -qw * qv * b[2] / w[2];
+ double vx = -qw * qv * b[0];
+ double vy = -qw * qv * b[1];
+ double vz = -qw * qv * b[2];
+ double dVx = fabs(vx - v.x);
+ double dVy = fabs(vy - v.y);
+ double dVz = fabs(vz - v.z);
+ v.x = vx;
+ v.y = vy;
+ v.z = vz;
+
+ //double vv = v.x * v.x + v.y * v.y + v.z * v.z;
+ //rmsVv += vv * vv;
+
+ //sigma0 += v.x * w[0] * v.x + v.y * w[1] * v.y + v.z * w[2] * v.z;
+ sigma0 += v.x * v.x + v.y * v.y + v.z * v.z;
+
+ if ((dVx > vConvLimit) || (dVy > vConvLimit) || (dVz > vConvLimit))
+ vConverged = false;
+
+ //if (dVx > maxdVx)
+ // maxdVx = dVx;
+ //if (dVy > maxdVy)
+ // maxdVy = dVy;
+ //if (dVz > maxdVz)
+ // maxdVz = dVz;
+ }
+
+ // Compute degrees of freedom and sigma0
+ if (nPtsUsed < 5) // A minimum of 5 surface points is needed to define a cylinder
+ {
+ sigma0 = 0.0;
+ return false;
+ }
+ int df = nPtsUsed - 5;
+ if (df == 0)
+ sigma0 = 0.0;
+ else
+ sigma0 = sqrt (sigma0 / (double)df);
+
+ //rmsVv = sqrt(rmsVv / (double)nPtsUsed);
+ //Base::Console().Message("X: %0.3e %0.3e %0.3e %0.3e %0.3e , Max dV: %0.4f %0.4f %0.4f , RMS Vv: %0.4f\n", x(0), x(1), x(2), x(3), x(4), maxdVx, maxdVy, maxdVz, rmsVv);
+
+ return true;
+}
+
+// Update the parameters after solving the normal equations
+bool CylinderFit::updateParameters(SolutionD solDir, const Eigen::VectorXd &x)
+{
+ // Update the parameters used as unknowns in the solution
+ switch (solDir)
+ {
+ case solL: // order of parameters: Yc, Zc, M, N, R
+ _vBase.y += x(0);
+ _vBase.z += x(1);
+ _vAxis.y += x(2);
+ _vAxis.z += x(3);
+ _dRadius += x(4);
+ break;
+ case solM: // order of parameters: Xc, Zc, L, N, R
+ _vBase.x += x(0);
+ _vBase.z += x(1);
+ _vAxis.x += x(2);
+ _vAxis.z += x(3);
+ _dRadius += x(4);
+ break;
+ case solN: // order of parameters: Xc, Yc, L, M, R
+ _vBase.x += x(0);
+ _vBase.y += x(1);
+ _vAxis.x += x(2);
+ _vAxis.y += x(3);
+ _dRadius += x(4);
+ break;
+ }
+
+ // Update the dependent axis direction parameter
+ double l2, m2, n2;
+ switch (solDir)
+ {
+ case solL:
+ l2 = 1.0 - _vAxis.y * _vAxis.y - _vAxis.z * _vAxis.z;
+ if (l2 <= 0.0)
+ return false; // L*L <= 0 !
+ _vAxis.x = sqrt(l2);
+ //_vBase.x = 0.0; // should already be 0
+ break;
+ case solM:
+ m2 = 1.0 - _vAxis.x * _vAxis.x - _vAxis.z * _vAxis.z;
+ if (m2 <= 0.0)
+ return false; // M*M <= 0 !
+ _vAxis.y = sqrt(m2);
+ //_vBase.y = 0.0; // should already be 0
+ break;
+ case solN:
+ n2 = 1.0 - _vAxis.x * _vAxis.x - _vAxis.y * _vAxis.y;
+ if (n2 <= 0.0)
+ return false; // N*N <= 0 !
+ _vAxis.z = sqrt(n2);
+ //_vBase.z = 0.0; // should already be 0
+ break;
+ }
+
+ return true;
+}
diff --git a/src/Mod/Mesh/App/Core/CylinderFit.h b/src/Mod/Mesh/App/Core/CylinderFit.h
new file mode 100644
index 0000000000..520fad2c9f
--- /dev/null
+++ b/src/Mod/Mesh/App/Core/CylinderFit.h
@@ -0,0 +1,151 @@
+/***************************************************************************
+ * Copyright (c) 2020 Graeme van der Vlugt *
+ * *
+ * This file is part of the FreeCAD CAx development system. *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of the GNU Library General Public *
+ * License as published by the Free Software Foundation; either *
+ * version 2 of the License, or (at your option) any later version. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU Library General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU Library General Public *
+ * License along with this library; see the file COPYING.LIB. If not, *
+ * write to the Free Software Foundation, Inc., 59 Temple Place, *
+ * Suite 330, Boston, MA 02111-1307, USA *
+ * *
+ ***************************************************************************/
+
+
+#ifndef MESH_CYLINDER_FIT_H
+#define MESH_CYLINDER_FIT_H
+
+#include "Approximation.h"
+#include
+
+// -------------------------------------------------------------------------------
+namespace MeshCoreFit {
+
+typedef Eigen::Matrix Matrix5x5;
+
+/**
+ * Best-fit cylinder for a given set of points.
+ * Doesn't expect points on any top or bottom end-planes, only points on the side surface
+ */
+class MeshExport CylinderFit : public MeshCore::Approximation
+{
+protected:
+ // Solution 'direction' enumeration
+ enum SolutionD {solL = 0, // solution L: L is biggest axis component and L = f(M,N)
+ solM = 1, // solution M: M is biggest axis component and M = f(L,N)
+ solN = 2 // solution N: N is biggest axis component and N = f(L,M)
+ };
+public:
+ /**
+ * Construction
+ */
+ CylinderFit();
+ /**
+ * Destruction
+ */
+ virtual ~CylinderFit();
+
+ /**
+ * Set approximations before calling Fit()
+ */
+ void SetApproximations(double radius, const Base::Vector3d &base, const Base::Vector3d &axis);
+ /**
+ * Set iteration convergence criteria for the fit if special values are needed.
+ * The default values set in the constructor are suitable for most uses
+ */
+ void SetConvergenceCriteria(double posConvLimit, double dirConvLimit, double vConvLimit, int maxIter);
+ /**
+ * Returns the radius of the fitted cylinder. If Fit() has not been called then zero is returned.
+ */
+ double GetRadius() const;
+ /**
+ * Returns the base of the fitted cylinder. If Fit() has not been called the null vector is returned.
+ */
+ Base::Vector3d GetBase() const;
+ /**
+ * Returns the axis of the fitted cylinder. If Fit() has not been called the null vector is returned.
+ */
+ Base::Vector3d GetAxis() const;
+ /**
+ * Returns the number of iterations that Fit() needed to converge. If Fit() has not been called then zero is returned.
+ */
+ int GetNumIterations() const;
+ /**
+ * Fit a cylinder into the given points. If the fit fails FLOAT_MAX is returned.
+ */
+ float Fit();
+ /**
+ * Returns the distance from the point \a rcPoint to the fitted cylinder. If Fit() has not been
+ * called FLOAT_MAX is returned.
+ */
+ float GetDistanceToCylinder(const Base::Vector3f &rcPoint) const;
+ /**
+ * Returns the standard deviation from the points to the fitted cylinder. If Fit() has not been
+ * called FLOAT_MAX is returned.
+ */
+ float GetStdDeviation() const;
+ /**
+ * Projects the points onto the fitted cylinder.
+ */
+ void ProjectToCylinder();
+
+protected:
+ /**
+ * Compute approximations for the parameters using all points using the line fit method
+ */
+ void ComputeApproximationsLine();
+ /**
+ * Checks initial parameter values and defines the best solution direction to use
+ */
+ void findBestSolDirection(SolutionD &solDir);
+ /**
+ * Set up the normal equations
+ */
+ void setupNormalEquationMatrices(SolutionD solDir, const std::vector< Base::Vector3d > &residuals, Matrix5x5 &atpa, Eigen::VectorXd &atpl) const;
+ /**
+ * Sets up contributions of given observation to the normal equation matrices.
+ */
+ void setupObservation(SolutionD solDir, const Base::Vector3f &point, const Base::Vector3d &residual, double a[5], double &f0, double &qw, double b[3]) const;
+ /**
+ * Computes contribution of the given observation equation on the normal equation matrices
+ */
+ void addObservationU(double a[5], double li, double pi, Matrix5x5 &atpa, Eigen::VectorXd &atpl) const;
+ /**
+ * Set the lower part of the normal matrix equal to the upper part
+ */
+ void setLowerPart(Matrix5x5 &atpa) const;
+
+ /**
+ * Compute the residuals and sigma0 and check the residual convergence
+ */
+ bool computeResiduals(SolutionD solDir, const Eigen::VectorXd &x, std::vector< Base::Vector3d > &residuals, double &sigma0, double vConvLimit, bool &vConverged) const;
+ /**
+ * Update the parameters after solving the normal equations
+ */
+ bool updateParameters(SolutionD solDir, const Eigen::VectorXd &x);
+
+protected:
+ Base::Vector3d _vBase; /**< Base vector of the cylinder (point on axis). */
+ Base::Vector3d _vAxis; /**< Axis of the cylinder. */
+ double _dRadius; /**< Radius of the cylinder. */
+ int _numIter; /**< Number of iterations for solution to converge. */
+ double _posConvLimit; /**< Position and radius parameter convergence threshold. */
+ double _dirConvLimit; /**< Direction parameter convergence threshold. */
+ double _vConvLimit; /**< Residual convergence threshold. */
+ int _maxIter; /**< Maximum number of iterations. */
+
+};
+
+
+} // namespace MeshCore
+
+#endif // MESH_CYLINDER_FIT_H
diff --git a/src/Mod/Mesh/App/Core/SphereFit.cpp b/src/Mod/Mesh/App/Core/SphereFit.cpp
new file mode 100644
index 0000000000..5285c69d1f
--- /dev/null
+++ b/src/Mod/Mesh/App/Core/SphereFit.cpp
@@ -0,0 +1,427 @@
+/***************************************************************************
+ * Copyright (c) 2020 Graeme van der Vlugt *
+ * *
+ * This file is part of the FreeCAD CAx development system. *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of the GNU Library General Public *
+ * License as published by the Free Software Foundation; either *
+ * version 2 of the License, or (at your option) any later version. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU Library General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU Library General Public *
+ * License along with this library; see the file COPYING.LIB. If not, *
+ * write to the Free Software Foundation, Inc., 59 Temple Place, *
+ * Suite 330, Boston, MA 02111-1307, USA *
+ * *
+ ***************************************************************************/
+
+#include "PreCompiled.h"
+
+#ifndef _PreComp_
+# include
+# include
+# include
+#endif
+
+#include "SphereFit.h"
+#include
+
+using namespace MeshCoreFit;
+
+SphereFit::SphereFit()
+ : _vCenter(0,0,0)
+ , _dRadius(0)
+ , _numIter(0)
+ , _posConvLimit(0.0001)
+ , _vConvLimit(0.001)
+ , _maxIter(50)
+{
+}
+
+SphereFit::~SphereFit()
+{
+}
+
+// Set approximations before calling the fitting
+void SphereFit::SetApproximations(double radius, const Base::Vector3d ¢er)
+{
+ _bIsFitted = false;
+ _fLastResult = FLOAT_MAX;
+ _numIter = 0;
+ _dRadius = radius;
+ _vCenter = center;
+}
+
+// Set iteration convergence criteria for the fit if special values are needed.
+// The default values set in the constructor are suitable for most uses
+void SphereFit::SetConvergenceCriteria(double posConvLimit, double vConvLimit, int maxIter)
+{
+ if (posConvLimit > 0.0)
+ _posConvLimit = posConvLimit;
+ if (vConvLimit > 0.0)
+ _vConvLimit = vConvLimit;
+ if (maxIter > 0)
+ _maxIter = maxIter;
+}
+
+
+double SphereFit::GetRadius() const
+{
+ if (_bIsFitted)
+ return _dRadius;
+ else
+ return 0.0;
+}
+
+Base::Vector3d SphereFit::GetCenter() const
+{
+ if (_bIsFitted)
+ return _vCenter;
+ else
+ return Base::Vector3d();
+}
+
+int SphereFit::GetNumIterations() const
+{
+ if (_bIsFitted)
+ return _numIter;
+ else
+ return 0;
+}
+
+float SphereFit::GetDistanceToSphere(const Base::Vector3f &rcPoint) const
+{
+ float fResult = FLOAT_MAX;
+ if (_bIsFitted)
+ {
+ fResult = Base::Vector3d((double)rcPoint.x - _vCenter.x, (double)rcPoint.y - _vCenter.y, (double)rcPoint.z - _vCenter.z).Length() - _dRadius;
+ }
+ return fResult;
+}
+
+float SphereFit::GetStdDeviation() const
+{
+ // Mean: M=(1/N)*SUM Xi
+ // Variance: VAR=(N/N-3)*[(1/N)*SUM(Xi^2)-M^2]
+ // Standard deviation: SD=SQRT(VAR)
+ // Standard error of the mean: SE=SD/SQRT(N)
+ if (!_bIsFitted)
+ return FLOAT_MAX;
+
+ float fSumXi = 0.0f, fSumXi2 = 0.0f,
+ fMean = 0.0f, fDist = 0.0f;
+
+ float ulPtCt = float(CountPoints());
+ std::list< Base::Vector3f >::const_iterator cIt;
+
+ for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
+ fDist = GetDistanceToSphere( *cIt );
+ fSumXi += fDist;
+ fSumXi2 += ( fDist * fDist );
+ }
+
+ fMean = (1.0f / ulPtCt) * fSumXi;
+ return sqrt((ulPtCt / (ulPtCt - 3.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean));
+}
+
+void SphereFit::ProjectToSphere()
+{
+ for (std::list< Base::Vector3f >::iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) {
+ Base::Vector3f& cPnt = *it;
+
+ // Compute unit vector from sphere centre to point.
+ // Because this vector is orthogonal to the sphere's surface at the
+ // intersection point we can easily compute the projection point on the
+ // closest surface point using the radius of the sphere
+ Base::Vector3d diff((double)cPnt.x - _vCenter.x, (double)cPnt.y - _vCenter.y, (double)cPnt.z - _vCenter.z);
+ double length = diff.Length();
+ if (length == 0.0)
+ {
+ // Point is exactly at the sphere center, so it can be projected in any direction onto the sphere!
+ // So here just project in +Z direction
+ cPnt.z += (float)_dRadius;
+ }
+ else
+ {
+ diff /= length; // normalizing the vector
+ Base::Vector3d proj = _vCenter + diff * _dRadius;
+ cPnt.x = (float)proj.x;
+ cPnt.y = (float)proj.y;
+ cPnt.z = (float)proj.z;
+ }
+ }
+}
+
+// Compute approximations for the parameters using all points:
+// Set centre to centre of gravity of points and radius to the average
+// distance from the centre of gravity to the points.
+void SphereFit::ComputeApproximations()
+{
+ _bIsFitted = false;
+ _fLastResult = FLOAT_MAX;
+ _numIter = 0;
+ _vCenter.Set(0.0, 0.0, 0.0);
+ _dRadius = 0.0;
+ if (_vPoints.size() > 0)
+ {
+ std::list< Base::Vector3f >::const_iterator cIt;
+ for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt)
+ {
+ _vCenter.x += cIt->x;
+ _vCenter.y += cIt->y;
+ _vCenter.z += cIt->z;
+ }
+ _vCenter /= (double)_vPoints.size();
+
+ for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt)
+ {
+ Base::Vector3d diff((double)cIt->x - _vCenter.x, (double)cIt->y - _vCenter.y, (double)cIt->z - _vCenter.z);
+ _dRadius += diff.Length();
+ }
+ _dRadius /= (double)_vPoints.size();
+ }
+}
+
+float SphereFit::Fit()
+{
+ _bIsFitted = false;
+ _fLastResult = FLOAT_MAX;
+ _numIter = 0;
+
+ // A minimum of 4 surface points is needed to define a sphere
+ if (CountPoints() < 4)
+ return FLOAT_MAX;
+
+ // If approximations have not been set/computed then compute some now
+ if (_dRadius == 0.0)
+ ComputeApproximations();
+
+ // Initialise some matrices and vectors
+ std::vector< Base::Vector3d > residuals(CountPoints(), Base::Vector3d(0.0, 0.0, 0.0));
+ Matrix4x4 atpa;
+ Eigen::VectorXd atpl(4);
+
+ // Iteration loop...
+ double sigma0;
+ bool cont = true;
+ while (cont && (_numIter < _maxIter))
+ {
+ ++_numIter;
+
+ // Set up the quasi parameteric normal equations
+ setupNormalEquationMatrices(residuals, atpa, atpl);
+
+ // Solve the equations for the unknown corrections
+ Eigen::LLT< Matrix4x4 > llt(atpa);
+ if (llt.info() != Eigen::Success)
+ return FLOAT_MAX;
+ Eigen::VectorXd x = llt.solve(atpl);
+
+ // Check parameter convergence (order of parameters: X,Y,Z,R)
+ cont = false;
+ if ((fabs(x(0)) > _posConvLimit) || (fabs(x(1)) > _posConvLimit) ||
+ (fabs(x(2)) > _posConvLimit) || (fabs(x(3)) > _posConvLimit))
+ cont = true;
+
+ // Before updating the unknowns, compute the residuals and sigma0 and check the residual convergence
+ bool vConverged;
+ if (!computeResiduals(x, residuals, sigma0, _vConvLimit, vConverged))
+ return FLOAT_MAX;
+ if (!vConverged)
+ cont = true;
+
+ // Update the parameters (order of parameters: X,Y,Z,R)
+ _vCenter.x += x(0);
+ _vCenter.y += x(1);
+ _vCenter.z += x(2);
+ _dRadius += x(3);
+ }
+
+ // Check for convergence
+ if (cont)
+ return FLOAT_MAX;
+
+ _bIsFitted = true;
+ _fLastResult = sigma0;
+
+ return _fLastResult;
+}
+
+// Set up the normal equation matrices
+// atpa ... 4x4 normal matrix
+// atpl ... 4x1 matrix (right-hand side of equation)
+void SphereFit::setupNormalEquationMatrices(const std::vector< Base::Vector3d > &residuals, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const
+{
+ // Zero matrices
+ atpa.setZero();
+ atpl.setZero();
+
+ // For each point, setup the observation equation coefficients and add their
+ // contribution into the the normal equation matrices
+ double a[4], b[3];
+ double f0, qw;
+ std::vector< Base::Vector3d >::const_iterator vIt = residuals.begin();
+ std::list< Base::Vector3f >::const_iterator cIt;
+ for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt)
+ {
+ // if (using this point) { // currently all given points are used (could modify this if eliminating outliers, etc....
+ setupObservation(*cIt, *vIt, a, f0, qw, b);
+ addObservationU(a, f0, qw, atpa, atpl);
+ // }
+ }
+ setLowerPart(atpa);
+}
+
+// Sets up contributions of given observation to the quasi parameteric
+// normal equation matrices. Assumes uncorrelated coordinates.
+// point ... point
+// residual ... residual for this point computed from previous iteration (zero for first iteration)
+// a[4] ... parameter partials (order of parameters: X,Y,Z,R)
+// f0 ... reference to f0 term
+// qw ... reference to quasi weight (here we are assuming equal unit weights for each observed point coordinate)
+// b[3] ... observation partials
+void SphereFit::setupObservation(const Base::Vector3f &point, const Base::Vector3d &residual, double a[4], double &f0, double &qw, double b[3]) const
+{
+ // This adjustment requires an update of the observation approximations
+ // because the residuals do not have a linear relationship.
+ // New estimates for the observations:
+ double xEstimate = (double)point.x + residual.x;
+ double yEstimate = (double)point.y + residual.y;
+ double zEstimate = (double)point.z + residual.z;
+
+ // partials of the observations
+ double dx = xEstimate - _vCenter.x;
+ double dy = yEstimate - _vCenter.y;
+ double dz = zEstimate - _vCenter.z;
+ b[0] = 2.0 * dx;
+ b[1] = 2.0 * dy;
+ b[2] = 2.0 * dz;
+
+ // partials of the parameters
+ a[0] = -b[0];
+ a[1] = -b[1];
+ a[2] = -b[2];
+ a[3] = -2.0 * _dRadius;
+
+ // free term
+ f0 = _dRadius * _dRadius - dx * dx - dy * dy - dz * dz + b[0] * residual.x + b[1] * residual.y + b[2] * residual.z;
+
+ // quasi weight (using equal weights for sphere point coordinate observations)
+ //w[0] = 1.0;
+ //w[1] = 1.0;
+ //w[2] = 1.0;
+ //qw = 1.0 / (b[0] * b[0] / w[0] + b[1] * b[1] / w[1] + b[2] * b[2] / w[2]);
+ qw = 1.0 / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
+}
+
+// Computes contribution of the given observation equation on the normal equation matrices
+// Call this for each observation (point)
+// Here we only add the contribution to the upper part of the normal matrix
+// and then after all observations have been added we need to set the lower part
+// (which is symmetrical to the upper part)
+// a[4] ... parameter partials
+// li ... free term (f0)
+// pi ... weight of observation (= quasi weight qw for this solution)
+// atpa ... 4x4 normal equation matrix
+// atpl ... 4x1 matrix/vector (right-hand side of equations)
+void SphereFit::addObservationU(double a[4], double li, double pi, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const
+{
+ for (int i = 0; i < 4; ++i)
+ {
+ double aipi = a[i] * pi;
+ for (int j = i; j < 4; ++j)
+ {
+ atpa(i, j) += aipi * a[j];
+ //atpa(j, i) = atpa(i, j); // it's a symmetrical matrix, we'll set this later after all observations processed
+ }
+ atpl(i) += aipi * li;
+ }
+}
+
+// Set the lower part of the normal matrix equal to the upper part
+// This is done after all the observations have been added
+void SphereFit::setLowerPart(Matrix4x4 &atpa) const
+{
+ for (int i = 0; i < 4; ++i)
+ for (int j = i+1; j < 4; ++j) // skip the diagonal elements
+ atpa(j, i) = atpa(i, j);
+}
+
+// Compute the residuals and sigma0 and check the residual convergence
+bool SphereFit::computeResiduals(const Eigen::VectorXd &x, std::vector< Base::Vector3d > &residuals, double &sigma0, double vConvLimit, bool &vConverged) const
+{
+ vConverged = true;
+ int nPtsUsed = 0;
+ sigma0 = 0.0;
+ double a[4], b[3];
+ double f0, qw;
+ //double maxdVx = 0.0;
+ //double maxdVy = 0.0;
+ //double maxdVz = 0.0;
+ //double rmsVv = 0.0;
+ std::vector< Base::Vector3d >::iterator vIt = residuals.begin();
+ std::list< Base::Vector3f >::const_iterator cIt;
+ for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt)
+ {
+ // if (using this point) { // currently all given points are used (could modify this if eliminating outliers, etc....
+ ++nPtsUsed;
+ Base::Vector3d &v = *vIt;
+ setupObservation(*cIt, v, a, f0, qw, b);
+ double qv = -f0;
+ for (int i = 0; i < 4; ++i)
+ qv += a[i] * x(i);
+
+ // We are using equal weights for sphere point coordinate observations (see setupObservation)
+ // i.e. w[0] = w[1] = w[2] = 1.0;
+ //double vx = -qw * qv * b[0] / w[0];
+ //double vy = -qw * qv * b[1] / w[1];
+ //double vz = -qw * qv * b[2] / w[2];
+ double vx = -qw * qv * b[0];
+ double vy = -qw * qv * b[1];
+ double vz = -qw * qv * b[2];
+ double dVx = fabs(vx - v.x);
+ double dVy = fabs(vy - v.y);
+ double dVz = fabs(vz - v.z);
+ v.x = vx;
+ v.y = vy;
+ v.z = vz;
+
+ //double vv = v.x * v.x + v.y * v.y + v.z * v.z;
+ //rmsVv += vv * vv;
+
+ //sigma0 += v.x * w[0] * v.x + v.y * w[1] * v.y + v.z * w[2] * v.z;
+ sigma0 += v.x * v.x + v.y * v.y + v.z * v.z;
+
+ if ((dVx > vConvLimit) || (dVy > vConvLimit) || (dVz > vConvLimit))
+ vConverged = false;
+
+ //if (dVx > maxdVx)
+ // maxdVx = dVx;
+ //if (dVy > maxdVy)
+ // maxdVy = dVy;
+ //if (dVz > maxdVz)
+ // maxdVz = dVz;
+ }
+
+ // Compute degrees of freedom and sigma0
+ if (nPtsUsed < 4) // A minimum of 4 surface points is needed to define a sphere
+ {
+ sigma0 = 0.0;
+ return false;
+ }
+ int df = nPtsUsed - 4;
+ if (df == 0)
+ sigma0 = 0.0;
+ else
+ sigma0 = sqrt (sigma0 / (double)df);
+
+ //rmsVv = sqrt(rmsVv / (double)nPtsUsed);
+ //Base::Console().Message("X: %0.3e %0.3e %0.3e %0.3e , Max dV: %0.4f %0.4f %0.4f , RMS Vv: %0.4f\n", x(0), x(1), x(2), x(3), maxdVx, maxdVy, maxdVz, rmsVv);
+
+ return true;
+}
diff --git a/src/Mod/Mesh/App/Core/SphereFit.h b/src/Mod/Mesh/App/Core/SphereFit.h
new file mode 100644
index 0000000000..1b475f36a0
--- /dev/null
+++ b/src/Mod/Mesh/App/Core/SphereFit.h
@@ -0,0 +1,130 @@
+/***************************************************************************
+ * Copyright (c) 2020 Graeme van der Vlugt *
+ * *
+ * This file is part of the FreeCAD CAx development system. *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of the GNU Library General Public *
+ * License as published by the Free Software Foundation; either *
+ * version 2 of the License, or (at your option) any later version. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU Library General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU Library General Public *
+ * License along with this library; see the file COPYING.LIB. If not, *
+ * write to the Free Software Foundation, Inc., 59 Temple Place, *
+ * Suite 330, Boston, MA 02111-1307, USA *
+ * *
+ ***************************************************************************/
+
+
+#ifndef MESH_SPHERE_FIT_H
+#define MESH_SPHERE_FIT_H
+
+#include "Approximation.h"
+#include
+
+// -------------------------------------------------------------------------------
+namespace MeshCoreFit {
+
+typedef Eigen::Matrix Matrix4x4;
+
+/**
+ * Best-fit sphere for a given set of points.
+ */
+class MeshExport SphereFit : public MeshCore::Approximation
+{
+public:
+ /**
+ * Construction
+ */
+ SphereFit();
+ /**
+ * Destruction
+ */
+ virtual ~SphereFit();
+
+ /**
+ * Set approximations before calling Fit()
+ */
+ void SetApproximations(double radius, const Base::Vector3d ¢er);
+ /**
+ * Set iteration convergence criteria for the fit if special values are needed.
+ * The default values set in the constructor are suitable for most uses
+ */
+ void SetConvergenceCriteria(double posConvLimit, double vConvLimit, int maxIter);
+ /**
+ * Returns the radius of the fitted sphere. If Fit() has not been called then zero is returned.
+ */
+ double GetRadius() const;
+ /**
+ * Returns the center of the fitted sphere. If Fit() has not been called the null vector is returned.
+ */
+ Base::Vector3d GetCenter() const;
+ /**
+ * Returns the number of iterations that Fit() needed to converge. If Fit() has not been called then zero is returned.
+ */
+ int GetNumIterations() const;
+ /**
+ * Compute approximations for the parameters using all points
+ */
+ void ComputeApproximations();
+ /**
+ * Fit a sphere onto the given points. If the fit fails FLOAT_MAX is returned.
+ */
+ float Fit();
+ /**
+ * Returns the distance from the point \a rcPoint to the fitted sphere. If Fit() has not been
+ * called FLOAT_MAX is returned.
+ */
+ float GetDistanceToSphere(const Base::Vector3f &rcPoint) const;
+ /**
+ * Returns the standard deviation from the points to the fitted sphere. If Fit() has not been
+ * called FLOAT_MAX is returned.
+ */
+ float GetStdDeviation() const;
+ /**
+ * Projects the points onto the fitted sphere.
+ */
+ void ProjectToSphere();
+
+protected:
+ /**
+ * Set up the normal equations
+ */
+ void setupNormalEquationMatrices(const std::vector< Base::Vector3d > &residuals, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const;
+ /**
+ * Sets up contributions of given observation to the normal equation matrices.
+ */
+ void setupObservation(const Base::Vector3f &point, const Base::Vector3d &residual, double a[4], double &f0, double &qw, double b[3]) const;
+ /**
+ * Computes contribution of the given observation equation on the normal equation matrices
+ */
+ void addObservationU(double a[4], double li, double pi, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const;
+ /**
+ * Set the lower part of the normal matrix equal to the upper part
+ */
+ void setLowerPart(Matrix4x4 &atpa) const;
+
+ /**
+ * Compute the residuals and sigma0 and check the residual convergence
+ */
+ bool computeResiduals(const Eigen::VectorXd &x, std::vector< Base::Vector3d > &residuals, double &sigma0, double vConvLimit, bool &vConverged) const;
+
+protected:
+ Base::Vector3d _vCenter;/**< Center of sphere. */
+ double _dRadius; /**< Radius of the sphere. */
+ int _numIter; /**< Number of iterations for solution to converge. */
+ double _posConvLimit; /**< Position and radius parameter convergence threshold. */
+ double _vConvLimit; /**< Residual convergence threshold. */
+ int _maxIter; /**< Maximum number of iterations. */
+
+};
+
+
+} // namespace MeshCore
+
+#endif // MESH_SPHERE_FIT_H