diff --git a/src/Mod/Mesh/App/Core/Approximation.cpp b/src/Mod/Mesh/App/Core/Approximation.cpp index c04343db0d..0411d1e0fd 100644 --- a/src/Mod/Mesh/App/Core/Approximation.cpp +++ b/src/Mod/Mesh/App/Core/Approximation.cpp @@ -1027,6 +1027,7 @@ CylinderFit::CylinderFit() : _vBase(0,0,0) , _vAxis(0,0,1) , _fRadius(0) + , _initialGuess(false) { } @@ -1034,6 +1035,21 @@ CylinderFit::~CylinderFit() { } +Base::Vector3f CylinderFit::GetInitialAxisFromNormals(const std::vector& n) const +{ + PlaneFit planeFit; + planeFit.AddPoints(n); + planeFit.Fit(); + return planeFit.GetNormal(); +} + +void CylinderFit::SetInitialValues(const Base::Vector3f& b, const Base::Vector3f& n) +{ + _vBase = b; + _vAxis = n; + _initialGuess = true; +} + float CylinderFit::Fit() { if (CountPoints() < 7) @@ -1046,15 +1062,22 @@ float CylinderFit::Fit() [](const Base::Vector3f& v) { return Wm4::Vector3d(v.x, v.y, v.z); }); Wm4::Vector3d cnt, axis; + if (_initialGuess) { + cnt = Base::convertTo(_vBase); + axis = Base::convertTo(_vAxis); + } + double radius, height; - Wm4::CylinderFit3 fit(input.size(), input.data(), cnt, axis, radius, height, false); + Wm4::CylinderFit3 fit(input.size(), input.data(), cnt, axis, radius, height, _initialGuess); + _initialGuess = false; + _vBase = Base::convertTo(cnt); _vAxis = Base::convertTo(axis); _fRadius = float(radius); _fLastResult = double(fit); -#if defined(_DEBUG) +#if defined(FC_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 @@ -1067,7 +1090,7 @@ float CylinderFit::Fit() if (result < FLOAT_MAX) { Base::Vector3d base = cylFit.GetBase(); Base::Vector3d dir = cylFit.GetAxis(); -#if defined(_DEBUG) +#if defined(FC_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 @@ -1183,6 +1206,29 @@ float CylinderFit::GetStdDeviation() const return sqrt((ulPtCt / (ulPtCt - 3.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean)); } +void CylinderFit::GetBounding(Base::Vector3f& bottom, Base::Vector3f& top) const +{ + float distMin = FLT_MAX; + float distMax = FLT_MIN; + + std::list::const_iterator cIt; + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) { + float dist = cIt->DistanceToPlane(_vBase, _vAxis); + if (dist < distMin) { + distMin = dist; + bottom = *cIt; + } + if (dist > distMax) { + distMax = dist; + top = *cIt; + } + } + + // Project the points onto the cylinder axis + bottom = bottom.Perpendicular(_vBase, _vAxis); + top = top.Perpendicular(_vBase, _vAxis); +} + void CylinderFit::ProjectToCylinder() { Base::Vector3f cBase(GetBase()); diff --git a/src/Mod/Mesh/App/Core/Approximation.h b/src/Mod/Mesh/App/Core/Approximation.h index a74a9b2c8e..db61724d3c 100644 --- a/src/Mod/Mesh/App/Core/Approximation.h +++ b/src/Mod/Mesh/App/Core/Approximation.h @@ -393,11 +393,16 @@ public: virtual ~CylinderFit(); float GetRadius() const; Base::Vector3f GetBase() const; + void SetInitialValues(const Base::Vector3f&, const Base::Vector3f&); /** * Returns the axis of the fitted cylinder. If Fit() has not been called the null vector is * returned. */ Base::Vector3f GetAxis() const; + /** + * Returns an initial axis based on point normals. + */ + Base::Vector3f GetInitialAxisFromNormals(const std::vector& n) const; /** * Fit a cylinder into the given points. If the fit fails FLOAT_MAX is returned. */ @@ -416,11 +421,17 @@ public: * Projects the points onto the fitted cylinder. */ void ProjectToCylinder(); + /** + * Get the bottom and top points of the cylinder. The distance of these + * points gives the height of the cylinder. + */ + void GetBounding(Base::Vector3f& bottom, Base::Vector3f& top) const; protected: Base::Vector3f _vBase; /**< Base vector of the cylinder. */ Base::Vector3f _vAxis; /**< Axis of the cylinder. */ float _fRadius; /**< Radius of the cylinder. */ + bool _initialGuess; }; // ------------------------------------------------------------------------------- diff --git a/src/Mod/Mesh/App/Core/MeshKernel.cpp b/src/Mod/Mesh/App/Core/MeshKernel.cpp index 69831877e6..c73e2314b8 100644 --- a/src/Mod/Mesh/App/Core/MeshKernel.cpp +++ b/src/Mod/Mesh/App/Core/MeshKernel.cpp @@ -1079,6 +1079,26 @@ std::vector MeshKernel::CalcVertexNormals() const return normals; } +std::vector MeshKernel::GetFacetNormals(const std::vector& facets) const +{ + std::vector normals; + normals.reserve(facets.size()); + + for (std::vector::const_iterator it = facets.begin(); it != facets.end(); ++it) { + const MeshFacet& face = _aclFacetArray[*it]; + + const Base::Vector3f& p1 = _aclPointArray[face._aulPoints[0]]; + const Base::Vector3f& p2 = _aclPointArray[face._aulPoints[1]]; + const Base::Vector3f& p3 = _aclPointArray[face._aulPoints[2]]; + + Base::Vector3f n = (p2 - p1) % (p3 - p1); + n.Normalize(); + normals.emplace_back(n); + } + + return normals; +} + // Evaluation float MeshKernel::GetSurface() const { diff --git a/src/Mod/Mesh/App/Core/MeshKernel.h b/src/Mod/Mesh/App/Core/MeshKernel.h index 993af4fe3a..d2a4ec91b7 100644 --- a/src/Mod/Mesh/App/Core/MeshKernel.h +++ b/src/Mod/Mesh/App/Core/MeshKernel.h @@ -112,6 +112,7 @@ public: * by summarizing the normals of the associated facets. */ std::vector CalcVertexNormals() const; + std::vector GetFacetNormals(const std::vector&) const; /** Returns the facet at the given index. This method is rather slow and should be * called occasionally only. For fast access the MeshFacetIterator interface should diff --git a/src/Mod/Mesh/Gui/SegmentationBestFit.cpp b/src/Mod/Mesh/Gui/SegmentationBestFit.cpp index f8419d160b..65c25b437d 100644 --- a/src/Mod/Mesh/Gui/SegmentationBestFit.cpp +++ b/src/Mod/Mesh/Gui/SegmentationBestFit.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -54,7 +55,7 @@ public: virtual std::vector getParameter(FitParameter::Points pts) const { std::vector values; MeshCore::PlaneFit fit; - fit.AddPoints(pts); + fit.AddPoints(pts.points); if (fit.Fit() < FLOAT_MAX) { Base::Vector3f base = fit.GetBase(); Base::Vector3f axis = fit.GetNormal(); @@ -77,9 +78,20 @@ public: virtual std::vector getParameter(FitParameter::Points pts) const { std::vector values; MeshCore::CylinderFit fit; - fit.AddPoints(pts); + fit.AddPoints(pts.points); + if (!pts.normals.empty()) { + Base::Vector3f base = fit.GetGravity(); + Base::Vector3f axis = fit.GetInitialAxisFromNormals(pts.normals); + fit.SetInitialValues(base, axis); + +#if defined(FC_DEBUG) + Base::Console().Message("Initial axis: (%f, %f, %f)\n", axis.x, axis.y, axis.z); +#endif + } + if (fit.Fit() < FLOAT_MAX) { - Base::Vector3f base = fit.GetBase(); + Base::Vector3f base, top; + fit.GetBounding(base, top); Base::Vector3f axis = fit.GetAxis(); float radius = fit.GetRadius(); values.push_back(base.x); @@ -89,6 +101,25 @@ public: values.push_back(axis.y); values.push_back(axis.z); values.push_back(radius); + +#if defined(FC_DEBUG) + // Only for testing purposes + try { + float height = Base::Distance(base, top); + Gui::Command::doCommand(Gui::Command::App, + "cyl = App.ActiveDocument.addObject('Part::Cylinder', 'Cylinder')\n" + "cyl.Radius = %f\n" + "cyl.Height = %f\n" + "cyl.Placement = App.Placement(App.Vector(%f,%f,%f), App.Rotation(App.Vector(0,0,1), App.Vector(%f,%f,%f)))\n", + radius, height, base.x, base.y, base.z, axis.x, axis.y, axis.z); + + Gui::Command::doCommand(Gui::Command::App, + "axis = cyl.Placement.Rotation.multVec(App.Vector(0,0,1))\n" + "print('Final axis: ({}, {}, {})'.format(axis.x, axis.y, axis.z))\n"); + } + catch (...) { + } +#endif } return values; } @@ -102,7 +133,7 @@ public: virtual std::vector getParameter(FitParameter::Points pts) const { std::vector values; MeshCore::SphereFit fit; - fit.AddPoints(pts); + fit.AddPoints(pts.points); if (fit.Fit() < FLOAT_MAX) { Base::Vector3f base = fit.GetCenter(); float radius = fit.GetRadius(); @@ -230,14 +261,15 @@ void ParametersDialog::on_compute_clicked() { const Mesh::MeshObject& kernel = myMesh->Mesh.getValue(); if (kernel.hasSelectedFacets()) { + FitParameter::Points fitpts; std::vector facets, points; kernel.getFacetsFromSelection(facets); points = kernel.getPointsFromFacets(facets); MeshCore::MeshPointArray coords = kernel.getKernel().GetPoints(points); + fitpts.normals = kernel.getKernel().GetFacetNormals(facets); // Copy points into right format - FitParameter::Points fitpts; - fitpts.insert(fitpts.end(), coords.begin(), coords.end()); + fitpts.points.insert(fitpts.points.end(), coords.begin(), coords.end()); coords.clear(); values = fitParameter->getParameter(fitpts); diff --git a/src/Mod/Mesh/Gui/SegmentationBestFit.h b/src/Mod/Mesh/Gui/SegmentationBestFit.h index 2bff2026b2..b1da47ce35 100644 --- a/src/Mod/Mesh/Gui/SegmentationBestFit.h +++ b/src/Mod/Mesh/Gui/SegmentationBestFit.h @@ -41,7 +41,10 @@ class Ui_SegmentationBestFit; class FitParameter { public: - typedef std::vector Points; + struct Points { + std::vector points; + std::vector normals; + }; virtual ~FitParameter() {} virtual std::vector getParameter(Points) const = 0; };