From ada7618ddb1575a48a815e619efad0de6d747885 Mon Sep 17 00:00:00 2001 From: enricoturri1966 Date: Tue, 11 Oct 2022 10:47:20 +0200 Subject: [PATCH] Measuring: Gizmo measure shows dimensioning for distance circle-circle --- src/libslic3r/CMakeLists.txt | 1 + src/libslic3r/Measure.cpp | 241 +++++++++++++++++++- src/libslic3r/Measure.hpp | 4 + src/libslic3r/MeasureUtils.hpp | 390 +++++++++++++++++++++++++++++++++ 4 files changed, 632 insertions(+), 4 deletions(-) create mode 100644 src/libslic3r/MeasureUtils.hpp diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index a52848b229..79e0e6355b 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -182,6 +182,7 @@ set(SLIC3R_SOURCES MeshNormals.cpp Measure.hpp Measure.cpp + MeasureUtils.hpp CustomGCode.cpp CustomGCode.hpp Arrange.hpp diff --git a/src/libslic3r/Measure.cpp b/src/libslic3r/Measure.cpp index ea68503254..27a4f8a90d 100644 --- a/src/libslic3r/Measure.cpp +++ b/src/libslic3r/Measure.cpp @@ -1,10 +1,11 @@ #include "libslic3r/libslic3r.h" #include "Measure.hpp" +#include "MeasureUtils.hpp" #include "libslic3r/Geometry/Circle.hpp" #include "libslic3r/SurfaceMesh.hpp" - +#if ENABLE_MEASURE_GIZMO namespace Slic3r { namespace Measure { @@ -13,8 +14,6 @@ namespace Measure { constexpr double feature_hover_limit = 0.5; // how close to a feature the mouse must be to highlight it constexpr double edge_endpoint_limit = 0.5; // how close to an edge endpoint the mouse ... - - static std::pair get_center_and_radius(const std::vector& border, int start_idx, int end_idx, const Transform3d& trafo) { Vec2ds pts; @@ -754,7 +753,238 @@ MeasurementResult get_measurement(const SurfaceFeature& a, const SurfaceFeature& /////////////////////////////////////////////////////////////////////////// } else if (f1.get_type() == SurfaceFeatureType::Circle) { if (f2.get_type() == SurfaceFeatureType::Circle) { - result.distance_infinite = std::make_optional(DistAndPoints{0., Vec3d::Zero(), Vec3d::Zero()}); // TODO + const auto [c0, r0, n0] = f1.get_circle(); + const auto [c1, r1, n1] = f2.get_circle(); + + // The following code is an adaptation of the algorithm found in: + // https://github.com/davideberly/GeometricTools/blob/master/GTE/Mathematics/DistCircle3Circle3.h + // and described in: + // https://www.geometrictools.com/Documentation/DistanceToCircle3.pdf + + struct ClosestInfo + { + double sqrDistance{ 0.0 }; + Vec3d circle0Closest{ Vec3d::Zero() }; + Vec3d circle1Closest{ Vec3d::Zero() }; + + inline bool operator < (const ClosestInfo& other) const { return sqrDistance < other.sqrDistance; } + }; + std::array candidates{}; + + const double zero = 0.0; + + const Vec3d D = c1 - c0; + + if (!are_parallel(n0, n1)) { + auto orthonormal_basis = [](const Vec3d& v) { + std::array ret; + ret[2] = v.normalized(); + int index; + ret[2].maxCoeff(&index); + switch (index) + { + case 0: { ret[0] = Vec3d(ret[2].y(), -ret[2].x(), 0.0).normalized(); break; } + case 1: { ret[0] = Vec3d(0.0, ret[2].z(), -ret[2].y()).normalized(); break; } + case 2: { ret[0] = Vec3d(-ret[2].z(), 0.0, ret[2].x()).normalized(); break; } + } + ret[1] = ret[2].cross(ret[0]).normalized(); + return ret; + }; + + // Get parameters for constructing the degree-8 polynomial phi. + const double one = 1.0; + const double two = 2.0; + const double r0sqr = sqr(r0); + const double r1sqr = sqr(r1); + + // Compute U1 and V1 for the plane of circle1. + const std::array basis = orthonormal_basis(n1); + const Vec3d U1 = basis[0]; + const Vec3d V1 = basis[1]; + + // Construct the polynomial phi(cos(theta)). + const Vec3d N0xD = n0.cross(D); + const Vec3d N0xU1 = n0.cross(U1); + const Vec3d N0xV1 = n0.cross(V1); + const double a0 = r1 * D.dot(U1); + const double a1 = r1 * D.dot(V1); + const double a2 = N0xD.dot(N0xD); + const double a3 = r1 * N0xD.dot(N0xU1); + const double a4 = r1 * N0xD.dot(N0xV1); + const double a5 = r1sqr * N0xU1.dot(N0xU1); + const double a6 = r1sqr * N0xU1.dot(N0xV1); + const double a7 = r1sqr * N0xV1.dot(N0xV1); + Polynomial1 p0{ a2 + a7, two * a3, a5 - a7 }; + Polynomial1 p1{ two * a4, two * a6 }; + Polynomial1 p2{ zero, a1 }; + Polynomial1 p3{ -a0 }; + Polynomial1 p4{ -a6, a4, two * a6 }; + Polynomial1 p5{ -a3, a7 - a5 }; + Polynomial1 tmp0{ one, zero, -one }; + Polynomial1 tmp1 = p2 * p2 + tmp0 * p3 * p3; + Polynomial1 tmp2 = two * p2 * p3; + Polynomial1 tmp3 = p4 * p4 + tmp0 * p5 * p5; + Polynomial1 tmp4 = two * p4 * p5; + Polynomial1 p6 = p0 * tmp1 + tmp0 * p1 * tmp2 - r0sqr * tmp3; + Polynomial1 p7 = p0 * tmp2 + p1 * tmp1 - r0sqr * tmp4; + + // Parameters for polynomial root finding. The roots[] array + // stores the roots. We need only the unique ones, which is + // the responsibility of the set uniqueRoots. The pairs[] + // array stores the (cosine,sine) information mentioned in the + // PDF. TODO: Choose the maximum number of iterations for root + // finding based on specific polynomial data? + const uint32_t maxIterations = 128; + int32_t degree = 0; + size_t numRoots = 0; + std::array roots{}; + std::set uniqueRoots{}; + size_t numPairs = 0; + std::array, 16> pairs{}; + double temp = zero; + double sn = zero; + + if (p7.GetDegree() > 0 || p7[0] != zero) { + // H(cs,sn) = p6(cs) + sn * p7(cs) + Polynomial1 phi = p6 * p6 - tmp0 * p7 * p7; + degree = static_cast(phi.GetDegree()); + assert(degree > 0); + numRoots = RootsPolynomial::Find(degree, &phi[0], maxIterations, roots.data()); + for (size_t i = 0; i < numRoots; ++i) { + uniqueRoots.insert(roots[i]); + } + + for (auto const& cs : uniqueRoots) { + if (std::fabs(cs) <= one) { + temp = p7(cs); + if (temp != zero) { + sn = -p6(cs) / temp; + pairs[numPairs++] = std::make_pair(cs, sn); + } + else { + temp = std::max(one - sqr(cs), zero); + sn = std::sqrt(temp); + pairs[numPairs++] = std::make_pair(cs, sn); + if (sn != zero) + pairs[numPairs++] = std::make_pair(cs, -sn); + } + } + } + } + else { + // H(cs,sn) = p6(cs) + degree = static_cast(p6.GetDegree()); + assert(degree > 0); + numRoots = RootsPolynomial::Find(degree, &p6[0], maxIterations, roots.data()); + for (size_t i = 0; i < numRoots; ++i) { + uniqueRoots.insert(roots[i]); + } + + for (auto const& cs : uniqueRoots) { + if (std::fabs(cs) <= one) { + temp = std::max(one - sqr(cs), zero); + sn = std::sqrt(temp); + pairs[numPairs++] = std::make_pair(cs, sn); + if (sn != zero) + pairs[numPairs++] = std::make_pair(cs, -sn); + } + } + } + + for (size_t i = 0; i < numPairs; ++i) { + ClosestInfo& info = candidates[i]; + Vec3d delta = D + r1 * (pairs[i].first * U1 + pairs[i].second * V1); + info.circle1Closest = c0 + delta; + const double N0dDelta = n0.dot(delta); + const double lenN0xDelta = n0.cross(delta).norm(); + if (lenN0xDelta > 0.0) { + const double diff = lenN0xDelta - r0; + info.sqrDistance = sqr(N0dDelta) + sqr(diff); + delta -= N0dDelta * n0; + delta.normalize(); + info.circle0Closest = c0 + r0 * delta; + } + else { + const Vec3d r0U0 = r0 * get_orthogonal(n0, true); + const Vec3d diff = delta - r0U0; + info.sqrDistance = diff.dot(diff); + info.circle0Closest = c0 + r0U0; + } + } + + std::sort(candidates.begin(), candidates.begin() + numPairs); + } + else { + ClosestInfo& info = candidates[0]; + + const double N0dD = n0.dot(D); + const Vec3d normProj = N0dD * n0; + const Vec3d compProj = D - normProj; + Vec3d U = compProj; + const double d = U.norm(); + U.normalize(); + + // The configuration is determined by the relative location of the + // intervals of projection of the circles on to the D-line. + // Circle0 projects to [-r0,r0] and circle1 projects to + // [d-r1,d+r1]. + const double dmr1 = d - r1; + double distance; + if (dmr1 >= r0) { + // d >= r0 + r1 + // The circles are separated (d > r0 + r1) or tangent with one + // outside the other (d = r0 + r1). + distance = dmr1 - r0; + info.circle0Closest = c0 + r0 * U; + info.circle1Closest = c1 - r1 * U; + } + else { + // d < r0 + r1 + // The cases implicitly use the knowledge that d >= 0. + const double dpr1 = d + r1; + if (dpr1 <= r0) { + // Circle1 is inside circle0. + distance = r0 - dpr1; + if (d > 0.0) { + info.circle0Closest = c0 + r0 * U; + info.circle1Closest = c1 + r1 * U; + } + else { + // The circles are concentric, so U = (0,0,0). + // Construct a vector perpendicular to N0 to use for + // closest points. + U = get_orthogonal(n0, true); + info.circle0Closest = c0 + r0 * U; + info.circle1Closest = c1 + r1 * U; + } + } + else if (dmr1 <= -r0) { + // Circle0 is inside circle1. + distance = -r0 - dmr1; + if (d > 0.0) { + info.circle0Closest = c0 - r0 * U; + info.circle1Closest = c1 - r1 * U; + } + else { + // The circles are concentric, so U = (0,0,0). + // Construct a vector perpendicular to N0 to use for + // closest points. + U = get_orthogonal(n0, true); + info.circle0Closest = c0 + r0 * U; + info.circle1Closest = c1 + r1 * U; + } + } + else { + distance = (c1 - c0).norm(); + info.circle0Closest = c0; + info.circle1Closest = c1; + } + } + + info.sqrDistance = distance * distance + N0dD * N0dD; + } + + result.distance_infinite = std::make_optional(DistAndPoints{ std::sqrt(candidates[0].sqrDistance), candidates[0].circle0Closest, candidates[0].circle1Closest }); // TODO /////////////////////////////////////////////////////////////////////////// } else if (f2.get_type() == SurfaceFeatureType::Plane) { assert(measuring != nullptr); @@ -823,3 +1053,6 @@ MeasurementResult get_measurement(const SurfaceFeature& a, const SurfaceFeature& } // namespace Measure } // namespace Slic3r + + +#endif // ENABLE_MEASURE_GIZMO diff --git a/src/libslic3r/Measure.hpp b/src/libslic3r/Measure.hpp index 2e17eea5c5..f310d8f9fa 100644 --- a/src/libslic3r/Measure.hpp +++ b/src/libslic3r/Measure.hpp @@ -1,6 +1,8 @@ #ifndef Slic3r_Measure_hpp_ #define Slic3r_Measure_hpp_ +#if ENABLE_MEASURE_GIZMO + #include #include @@ -191,3 +193,5 @@ inline bool are_perpendicular(const SurfaceFeature& f1, const SurfaceFeature& f2 } // namespace Slic3r #endif // Slic3r_Measure_hpp_ + +#endif // ENABLE_MEASURE_GIZMO diff --git a/src/libslic3r/MeasureUtils.hpp b/src/libslic3r/MeasureUtils.hpp new file mode 100644 index 0000000000..4f84624eac --- /dev/null +++ b/src/libslic3r/MeasureUtils.hpp @@ -0,0 +1,390 @@ +#ifndef Slic3r_MeasureUtils_hpp_ +#define Slic3r_MeasureUtils_hpp_ + +#if ENABLE_MEASURE_GIZMO + +#include + +namespace Slic3r { +namespace Measure { + +// Utility class used to calculate distance circle-circle +// Adaptation of code found in: +// https://github.com/davideberly/GeometricTools/blob/master/GTE/Mathematics/Polynomial1.h + +class Polynomial1 +{ +public: + Polynomial1(std::initializer_list values) + { + // C++ 11 will call the default constructor for + // Polynomial1 p{}, so it is guaranteed that + // values.size() > 0. + m_coefficient.resize(values.size()); + std::copy(values.begin(), values.end(), m_coefficient.begin()); + EliminateLeadingZeros(); + } + + // Construction and destruction. The first constructor creates a + // polynomial of the specified degree but sets all coefficients to + // zero (to ensure initialization). You are responsible for setting + // the coefficients, presumably with the degree-term set to a nonzero + // number. In the second constructor, the degree is the number of + // initializers plus 1, but then adjusted so that coefficient[degree] + // is not zero (unless all initializer values are zero). + explicit Polynomial1(uint32_t degree) + : m_coefficient(static_cast(degree) + 1, 0.0) + {} + + // Eliminate any leading zeros in the polynomial, except in the case + // the degree is 0 and the coefficient is 0. The elimination is + // necessary when arithmetic operations cause a decrease in the degree + // of the result. For example, (1 + x + x^2) + (1 + 2*x - x^2) = + // (2 + 3*x). The inputs both have degree 2, so the result is created + // with degree 2. After the addition we find that the degree is in + // fact 1 and resize the array of coefficients. This function is + // called internally by the arithmetic operators, but it is exposed in + // the public interface in case you need it for your own purposes. + void EliminateLeadingZeros() + { + const size_t size = m_coefficient.size(); + if (size > 1) { + const double zero = 0.0; + int32_t leading; + for (leading = static_cast(size) - 1; leading > 0; --leading) { + if (m_coefficient[leading] != zero) + break; + } + + m_coefficient.resize(++leading); + } + } + + // Set all coefficients to the specified value. + void SetCoefficients(double value) + { + std::fill(m_coefficient.begin(), m_coefficient.end(), value); + } + + inline uint32_t GetDegree() const + { + // By design, m_coefficient.size() > 0. + return static_cast(m_coefficient.size() - 1); + } + + inline const double& operator[](uint32_t i) const { return m_coefficient[i]; } + inline double& operator[](uint32_t i) { return m_coefficient[i]; } + + // Evaluate the polynomial. If the polynomial is invalid, the + // function returns zero. + double operator()(double t) const + { + int32_t i = static_cast(m_coefficient.size()); + double result = m_coefficient[--i]; + for (--i; i >= 0; --i) { + result *= t; + result += m_coefficient[i]; + } + return result; + } + +protected: + // The class is designed so that m_coefficient.size() >= 1. + std::vector m_coefficient; +}; + +Polynomial1 operator * (const Polynomial1& p0, const Polynomial1& p1) +{ + const uint32_t p0Degree = p0.GetDegree(); + const uint32_t p1Degree = p1.GetDegree(); + Polynomial1 result(p0Degree + p1Degree); + result.SetCoefficients(0.0); + for (uint32_t i0 = 0; i0 <= p0Degree; ++i0) { + for (uint32_t i1 = 0; i1 <= p1Degree; ++i1) { + result[i0 + i1] += p0[i0] * p1[i1]; + } + } + return result; +} + +Polynomial1 operator + (const Polynomial1& p0, const Polynomial1& p1) +{ + const uint32_t p0Degree = p0.GetDegree(); + const uint32_t p1Degree = p1.GetDegree(); + uint32_t i; + if (p0Degree >= p1Degree) { + Polynomial1 result(p0Degree); + for (i = 0; i <= p1Degree; ++i) { + result[i] = p0[i] + p1[i]; + } + for (/**/; i <= p0Degree; ++i) { + result[i] = p0[i]; + } + result.EliminateLeadingZeros(); + return result; + } + else { + Polynomial1 result(p1Degree); + for (i = 0; i <= p0Degree; ++i) { + result[i] = p0[i] + p1[i]; + } + for (/**/; i <= p1Degree; ++i) { + result[i] = p1[i]; + } + result.EliminateLeadingZeros(); + return result; + } +} + +Polynomial1 operator - (const Polynomial1& p0, const Polynomial1& p1) +{ + const uint32_t p0Degree = p0.GetDegree(); + const uint32_t p1Degree = p1.GetDegree(); + uint32_t i; + if (p0Degree >= p1Degree) { + Polynomial1 result(p0Degree); + for (i = 0; i <= p1Degree; ++i) { + result[i] = p0[i] - p1[i]; + } + for (/**/; i <= p0Degree; ++i) { + result[i] = p0[i]; + } + result.EliminateLeadingZeros(); + return result; + } + else { + Polynomial1 result(p1Degree); + for (i = 0; i <= p0Degree; ++i) { + result[i] = p0[i] - p1[i]; + } + for (/**/; i <= p1Degree; ++i) { + result[i] = -p1[i]; + } + result.EliminateLeadingZeros(); + return result; + } +} + +Polynomial1 operator * (double scalar, const Polynomial1& p) +{ + const uint32_t degree = p.GetDegree(); + Polynomial1 result(degree); + for (uint32_t i = 0; i <= degree; ++i) { + result[i] = scalar * p[i]; + } + return result; +} + +// Utility class used to calculate distance circle-circle +// Adaptation of code found in: +// https://github.com/davideberly/GeometricTools/blob/master/GTE/Mathematics/RootsPolynomial.h + +class RootsPolynomial +{ +public: + // General equations: sum_{i=0}^{d} c(i)*t^i = 0. The input array 'c' + // must have at least d+1 elements and the output array 'root' must + // have at least d elements. + + // Find the roots on (-infinity,+infinity). + static int32_t Find(int32_t degree, const double* c, uint32_t maxIterations, double* roots) + { + if (degree >= 0 && c != nullptr) { + const double zero = 0.0; + while (degree >= 0 && c[degree] == zero) { + --degree; + } + + if (degree > 0) { + // Compute the Cauchy bound. + const double one = 1.0; + const double invLeading = one / c[degree]; + double maxValue = zero; + for (int32_t i = 0; i < degree; ++i) { + const double value = std::fabs(c[i] * invLeading); + if (value > maxValue) + maxValue = value; + } + const double bound = one + maxValue; + + return FindRecursive(degree, c, -bound, bound, maxIterations, roots); + } + else if (degree == 0) + // The polynomial is a nonzero constant. + return 0; + else { + // The polynomial is identically zero. + roots[0] = zero; + return 1; + } + } + else + // Invalid degree or c. + return 0; + } + + // If you know that p(tmin) * p(tmax) <= 0, then there must be at + // least one root in [tmin, tmax]. Compute it using bisection. + static bool Find(int32_t degree, const double* c, double tmin, double tmax, uint32_t maxIterations, double& root) + { + const double zero = 0.0; + double pmin = Evaluate(degree, c, tmin); + if (pmin == zero) { + root = tmin; + return true; + } + double pmax = Evaluate(degree, c, tmax); + if (pmax == zero) { + root = tmax; + return true; + } + + if (pmin * pmax > zero) + // It is not known whether the interval bounds a root. + return false; + + if (tmin >= tmax) + // Invalid ordering of interval endpoitns. + return false; + + for (uint32_t i = 1; i <= maxIterations; ++i) { + root = 0.5 * (tmin + tmax); + + // This test is designed for 'float' or 'double' when tmin + // and tmax are consecutive floating-point numbers. + if (root == tmin || root == tmax) + break; + + const double p = Evaluate(degree, c, root); + const double product = p * pmin; + if (product < zero) { + tmax = root; + pmax = p; + } + else if (product > zero) { + tmin = root; + pmin = p; + } + else + break; + } + + return true; + } + + // Support for the Find functions. + static int32_t FindRecursive(int32_t degree, double const* c, double tmin, double tmax, uint32_t maxIterations, double* roots) + { + // The base of the recursion. + const double zero = 0.0; + double root = zero; + if (degree == 1) { + int32_t numRoots; + if (c[1] != zero) { + root = -c[0] / c[1]; + numRoots = 1; + } + else if (c[0] == zero) { + root = zero; + numRoots = 1; + } + else + numRoots = 0; + + if (numRoots > 0 && tmin <= root && root <= tmax) { + roots[0] = root; + return 1; + } + return 0; + } + + // Find the roots of the derivative polynomial scaled by 1/degree. + // The scaling avoids the factorial growth in the coefficients; + // for example, without the scaling, the high-order term x^d + // becomes (d!)*x through multiple differentiations. With the + // scaling we instead get x. This leads to better numerical + // behavior of the root finder. + const int32_t derivDegree = degree - 1; + std::vector derivCoeff(static_cast(derivDegree) + 1); + std::vector derivRoots(derivDegree); + for (int32_t i = 0, ip1 = 1; i <= derivDegree; ++i, ++ip1) { + derivCoeff[i] = c[ip1] * (double)(ip1) / (double)degree; + } + const int32_t numDerivRoots = FindRecursive(degree - 1, &derivCoeff[0], tmin, tmax, maxIterations, &derivRoots[0]); + + int32_t numRoots = 0; + if (numDerivRoots > 0) { + // Find root on [tmin,derivRoots[0]]. + if (Find(degree, c, tmin, derivRoots[0], maxIterations, root)) + roots[numRoots++] = root; + + // Find root on [derivRoots[i],derivRoots[i+1]]. + for (int32_t i = 0, ip1 = 1; i <= numDerivRoots - 2; ++i, ++ip1) { + if (Find(degree, c, derivRoots[i], derivRoots[ip1], maxIterations, root)) + roots[numRoots++] = root; + } + + // Find root on [derivRoots[numDerivRoots-1],tmax]. + if (Find(degree, c, derivRoots[static_cast(numDerivRoots) - 1], tmax, maxIterations, root)) + roots[numRoots++] = root; + } + else { + // The polynomial is monotone on [tmin,tmax], so has at most one root. + if (Find(degree, c, tmin, tmax, maxIterations, root)) + roots[numRoots++] = root; + } + return numRoots; + } + + static double Evaluate(int32_t degree, const double* c, double t) + { + int32_t i = degree; + double result = c[i]; + while (--i >= 0) { + result = t * result + c[i]; + } + return result; + } +}; + +// Adaptation of code found in: +// https://github.com/davideberly/GeometricTools/blob/master/GTE/Mathematics/Vector.h + +// Construct a single vector orthogonal to the nonzero input vector. If +// the maximum absolute component occurs at index i, then the orthogonal +// vector U has u[i] = v[i+1], u[i+1] = -v[i], and all other components +// zero. The index addition i+1 is computed modulo N. +Vec3d get_orthogonal(const Vec3d& v, bool unitLength) +{ + double cmax = std::fabs(v[0]); + int32_t imax = 0; + for (int32_t i = 1; i < 3; ++i) { + double c = std::fabs(v[i]); + if (c > cmax) { + cmax = c; + imax = i; + } + } + + Vec3d result = Vec3d::Zero(); + int32_t inext = imax + 1; + if (inext == 3) + inext = 0; + + result[imax] = v[inext]; + result[inext] = -v[imax]; + if (unitLength) { + const double sqrDistance = result[imax] * result[imax] + result[inext] * result[inext]; + const double invLength = 1.0 / std::sqrt(sqrDistance); + result[imax] *= invLength; + result[inext] *= invLength; + } + return result; +} + +} // namespace Slic3r +} // namespace Measure + +#endif // Slic3r_MeasureUtils_hpp_ + +#endif // ENABLE_MEASURE_GIZMO