173 lines
7 KiB
C++
173 lines
7 KiB
C++
#ifndef slic3r_Geometry_Circle_hpp_
|
|
#define slic3r_Geometry_Circle_hpp_
|
|
|
|
namespace Slic3r { namespace Geometry {
|
|
|
|
/// Find the center of the circle corresponding to the vector of Points as an arc.
|
|
Point circle_center_taubin_newton(const Points::const_iterator& input_start, const Points::const_iterator& input_end, size_t cycles = 20);
|
|
inline Point circle_center_taubin_newton(const Points& input, size_t cycles = 20) { return circle_center_taubin_newton(input.cbegin(), input.cend(), cycles); }
|
|
|
|
/// Find the center of the circle corresponding to the vector of Pointfs as an arc.
|
|
Vec2d circle_center_taubin_newton(const Vec2ds::const_iterator& input_start, const Vec2ds::const_iterator& input_end, size_t cycles = 20);
|
|
inline Vec2d circle_center_taubin_newton(const Vec2ds& input, size_t cycles = 20) { return circle_center_taubin_newton(input.cbegin(), input.cend(), cycles); }
|
|
|
|
// https://en.wikipedia.org/wiki/Circumscribed_circle
|
|
// Circumcenter coordinates, Cartesian coordinates
|
|
template<typename Vector>
|
|
bool circle_center(const Vector &a, Vector b, Vector c, Vector ¢er)
|
|
{
|
|
using Scalar = typename Vector::Scalar;
|
|
b -= a;
|
|
c -= a;
|
|
if (Scalar d = b.x() * c.y() - b.y() * c.x(); d == 0)
|
|
return false;
|
|
else {
|
|
Vector v = c.squaredNorm() * b - b.squaredNorm() * c;
|
|
center = a + Vector(- v.y(), v.x()) / (2 * d);
|
|
return true;
|
|
}
|
|
}
|
|
|
|
/*
|
|
// Likely a bit more accurate accurate version of circle_center() by centering.
|
|
template<typename Vector>
|
|
bool circle_center_centered(Vector a, Vector b, Vector c, Vector ¢er)
|
|
{
|
|
auto bbox_center = 0.5 * (a.cwiseMin(b).cwiseMin(c) + a.cwiseMax(b).cwiseMax(c));
|
|
a -= bbox_center;
|
|
b -= bbox_center;
|
|
c -= bbox_center;
|
|
auto bc = b - c;
|
|
auto ca = c - a;
|
|
auto ab = a - b;
|
|
if (d = ao.x() * bc.y() + bo.x() * ca.y() + co.x() * ab.y(); d == 0)
|
|
return false;
|
|
else {
|
|
center = bbox_center - perp(ao.squaredNorm() * bc + bo.squaredNorm() * ca + co.squaredNorm() * ab) / (2 * d);
|
|
return true;
|
|
}
|
|
}
|
|
*/
|
|
|
|
// 2D circle defined by its center and squared radius
|
|
template<typename Vector>
|
|
struct CircleSq {
|
|
using Scalar = typename Vector::Scalar;
|
|
|
|
Vector center;
|
|
Scalar radius2;
|
|
|
|
CircleSq(const Vector &a, const Vector &b) : center(Scalar(0.5) * (a + b)) { radius2 = (a - center).squaredNorm(); }
|
|
CircleSq(const Vector &a, const Vector &b, const Vector &c) {
|
|
if (circle_center(a, b, c, this->center))
|
|
this->radius = (a - this->center).squaredNorm();
|
|
else
|
|
*this = make_invalid();
|
|
}
|
|
|
|
bool invalid() const { return this->radius2 < 0; }
|
|
bool valid() const { return ! this->invalid(); }
|
|
bool contains(const Vector &p) const { return (p - this->center).squaredNorm() < this->radius2; }
|
|
bool contains_with_eps(const Vector &p, const Scalar relative_epsilon2 = Scalar((1 + 1e-14) * (1 + 1e-14))) const
|
|
{ Scalar r2 = this->radius2 * relative_epsilon2; return (p - this->center).squaredNorm() < r2; }
|
|
|
|
static CircleSq make_invalid() { return Circle { { 0, 0 }, -1 }; }
|
|
};
|
|
|
|
// 2D circle defined by its center and radius
|
|
template<typename Vector>
|
|
struct Circle {
|
|
using Scalar = typename Vector::Scalar;
|
|
|
|
Vector center;
|
|
Scalar radius;
|
|
|
|
Circle(const Vector &a, const Vector &b) : center(Scalar(0.5) * (a + b)) { radius = (a - center).norm(); }
|
|
Circle(const Vector &a, const Vector &b, const Vector &c) {
|
|
this->center = circle_center(a, b, c);
|
|
this->radius = (a - this->center).norm();
|
|
}
|
|
template<typename Vector2>
|
|
explicit Circle(const CircleSq<Vector2> &c) : center(c.center), radius(sqrt(c.radius2)) {}
|
|
|
|
bool invalid() const { return this->radius < 0; }
|
|
bool valid() const { return ! this->invalid(); }
|
|
bool contains(const Vector &p) const { return (p - this->center).squaredNorm() < this->radius * this->radius; }
|
|
bool contains_with_eps(const Vector &p, const Scalar relative_epsilon = 1 + 1e-14) const { Scalar r = this->radius * relative_epsilon; return (p - this->center).squaredNorm() < r * r; }
|
|
|
|
static Circle make_invalid() { return Circle { { 0, 0 }, -1 }; }
|
|
};
|
|
|
|
// Randomized algorithm by Emo Welzl
|
|
template<typename Vector, typename Points>
|
|
CircleSq<Vector> smallest_enclosing_circle2_welzl(const Points &points)
|
|
{
|
|
using Scalar = typename Vector::Scalar;
|
|
|
|
const auto &p0 = points[0].cast<Scalar>();
|
|
auto circle = CircleSq<Vector>(p0, points[1].cast<Scalar>());
|
|
for (size_t i = 2; i < points.size(); ++ i)
|
|
if (const Vector &p = points[i].cast<Scalar>(); ! circle.contains_with_eps(p)) {
|
|
// p is the first point on the smallest circle enclosing points[0..i]
|
|
auto c = CircleSq<Vector>(p0, p);
|
|
for (size_t j = 1; j < i; ++ j)
|
|
if (const Vector &q = points[j].cast<Scalar>(); ! c.contains_with_eps(q)) {
|
|
// q is the second point on the smallest circle enclosing points[0..i]
|
|
auto c2 = CircleSq<Vector>(p, q);
|
|
for (int k = 0; k < j; ++ k)
|
|
if (const Vector &r = points[k].cast<Scalar>(); ! c2.contains_with_eps(r)) {
|
|
if (Vector center; circle_center<Vector>(p, q, r, center)) {
|
|
c2.center = center;
|
|
c2.radius2 = (center - p).squaredNorm();
|
|
}
|
|
}
|
|
if (c2.radius2 > 0)
|
|
c = c2;
|
|
}
|
|
if (c.radius2 > 0)
|
|
circle = c;
|
|
}
|
|
return circle;
|
|
}
|
|
|
|
template<typename Vector, typename Points>
|
|
Circle<Vector> smallest_enclosing_circle_welzl(const Points &points)
|
|
{
|
|
return Circle<Vector>(smallest_enclosing_circle2_welzl<Vector, Points>(points));
|
|
}
|
|
|
|
// Ugly named variant, that accepts the squared line
|
|
// Don't call me with a nearly zero length vector!
|
|
// sympy:
|
|
// factor(solve([a * x + b * y + c, x**2 + y**2 - r**2], [x, y])[0])
|
|
// factor(solve([a * x + b * y + c, x**2 + y**2 - r**2], [x, y])[1])
|
|
template<typename T>
|
|
int ray_circle_intersections_r2_lv2_c(T r2, T a, T b, T lv2, T c, std::pair<Eigen::Matrix<T, 2, 1, Eigen::DontAlign>, Eigen::Matrix<T, 2, 1, Eigen::DontAlign>> &out)
|
|
{
|
|
T x0 = - a * c;
|
|
T y0 = - b * c;
|
|
T d2 = r2 * lv2 - c * c;
|
|
if (d2 < T(0))
|
|
return 0;
|
|
T d = sqrt(d2);
|
|
out.first.x() = (x0 + b * d) / lv2;
|
|
out.first.y() = (y0 - a * d) / lv2;
|
|
out.second.x() = (x0 - b * d) / lv2;
|
|
out.second.y() = (y0 + a * d) / lv2;
|
|
return d == T(0) ? 1 : 2;
|
|
}
|
|
template<typename T>
|
|
int ray_circle_intersections(T r, T a, T b, T c, std::pair<Eigen::Matrix<T, 2, 1, Eigen::DontAlign>, Eigen::Matrix<T, 2, 1, Eigen::DontAlign>> &out)
|
|
{
|
|
T lv2 = a * a + b * b;
|
|
if (lv2 < T(SCALED_EPSILON * SCALED_EPSILON)) {
|
|
//FIXME what is the correct epsilon?
|
|
// What if the line touches the circle?
|
|
return false;
|
|
}
|
|
return ray_circle_intersections_r2_lv2_c2(r * r, a, b, a * a + b * b, c, out);
|
|
}
|
|
|
|
} } // namespace Slic3r::Geometry
|
|
|
|
#endif slic3r_Geometry_Circle_hpp_
|