mirror of
https://git.mirrors.martin98.com/https://github.com/prusa3d/PrusaSlicer.git
synced 2025-07-11 05:41:49 +08:00
Measuring: Gizmo measure shows dimensioning for distance circle-circle
This commit is contained in:
enricoturri1966
parent
bc1e5a0272
commit
ada7618ddb
@ -182,6 +182,7 @@ set(SLIC3R_SOURCES
|
||||
MeshNormals.cpp
|
||||
Measure.hpp
|
||||
Measure.cpp
|
||||
MeasureUtils.hpp
|
||||
CustomGCode.cpp
|
||||
CustomGCode.hpp
|
||||
Arrange.hpp
|
||||
|
||||
@ -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<Vec3d, double> get_center_and_radius(const std::vector<Vec3d>& 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<ClosestInfo, 16> candidates{};
|
||||
|
||||
const double zero = 0.0;
|
||||
|
||||
const Vec3d D = c1 - c0;
|
||||
|
||||
if (!are_parallel(n0, n1)) {
|
||||
auto orthonormal_basis = [](const Vec3d& v) {
|
||||
std::array<Vec3d, 3> 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<Vec3d, 3> 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<double, 8> roots{};
|
||||
std::set<double> uniqueRoots{};
|
||||
size_t numPairs = 0;
|
||||
std::array<std::pair<double, double>, 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<int32_t>(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<int32_t>(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
|
||||
|
||||
@ -1,6 +1,8 @@
|
||||
#ifndef Slic3r_Measure_hpp_
|
||||
#define Slic3r_Measure_hpp_
|
||||
|
||||
#if ENABLE_MEASURE_GIZMO
|
||||
|
||||
#include <optional>
|
||||
#include <memory>
|
||||
|
||||
@ -191,3 +193,5 @@ inline bool are_perpendicular(const SurfaceFeature& f1, const SurfaceFeature& f2
|
||||
} // namespace Slic3r
|
||||
|
||||
#endif // Slic3r_Measure_hpp_
|
||||
|
||||
#endif // ENABLE_MEASURE_GIZMO
|
||||
|
||||
390
src/libslic3r/MeasureUtils.hpp
Normal file
390
src/libslic3r/MeasureUtils.hpp
Normal file
@ -0,0 +1,390 @@
|
||||
#ifndef Slic3r_MeasureUtils_hpp_
|
||||
#define Slic3r_MeasureUtils_hpp_
|
||||
|
||||
#if ENABLE_MEASURE_GIZMO
|
||||
|
||||
#include <initializer_list>
|
||||
|
||||
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<double> values)
|
||||
{
|
||||
// C++ 11 will call the default constructor for
|
||||
// Polynomial1<Real> 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<size_t>(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<int32_t>(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<uint32_t>(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<int32_t>(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<double> 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<double> derivCoeff(static_cast<size_t>(derivDegree) + 1);
|
||||
std::vector<double> 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<size_t>(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
|
||||
Loading…
x
Reference in New Issue
Block a user