mirror of
https://github.com/OrcaSlicer/OrcaSlicer.git
synced 2026-10-06 15:21:09 +00:00
HanifKoh
8a6377f087
* Add Missing Includes Across src/libslic3r Every libslic3r source and header now directly includes the headers declaring what it uses, rather than relying on the precompiled header or transitive includes. Generated with clang-tidy misc-include-cleaner, with libslic3r headers spelled libslic3r/... so they resolve outside the library's private include paths. MultiMaterialSegmentation.hpp, Support/SupportParameters.hpp and Format/STEP.hpp are made self-contained by hand. * Make the libslic3r Headers Compile on Their Own Each now includes, or forward-declares, what it uses instead of relying on what its includers happened to include first. Left out: I18N.hpp, which errors on purpose when included from GUI code, and VoxelizeCSGMesh.hpp and SLA/bicubic.h, which nothing includes and which no longer compile at all. * Add the Includes Missing From the Hand-Fixed libslic3r Headers clang-tidy would not edit these headers while they failed to compile on their own, so the first pass skipped them. With the headers now self-contained, a second pass adds the rest. * Keep Windows Setup Ahead of the Added libslic3r Includes Print.cpp and Thread.cpp open with a _WIN32 block that has to come first; without the precompiled header, Print.cpp otherwise reaches windows.h through OCCT with NONLS defined and boost/regex fails. OpenVDBUtils.cpp and SLA/SupportTreeBuilder.cpp had includes inside #ifndef NOMINMAX, which libslic3r defines on Windows, so those were skipped there. .clang-tidy also ignores the MSVC STL and UCRT internals, Boost.Multiprecision's fwd.hpp and CPython's Windows include directory. * Re-Add libslic3r Includes After the Clipper2 2.0.1 Migration Rebasing onto main took main's version of the files the Clipper2 migration rewrote, so their added includes are restored here, along with includes for main's new code. Clipper2's individual headers are now ignored by clang-tidy: they only build the Z variant through clipper2_z.hpp, which defines USINGZ first, so including clipper.core.h and the like directly broke ClipperZUtils.cpp.
1453 lines
64 KiB
C++
1453 lines
64 KiB
C++
#include "Point.hpp"
|
|
#include "TriangleMesh.hpp"
|
|
#include "libslic3r/libslic3r.h"
|
|
#include "Measure.hpp"
|
|
#include "MeasureUtils.hpp"
|
|
|
|
#include "libslic3r/Geometry/Circle.hpp"
|
|
#include "libslic3r/SurfaceMesh.hpp"
|
|
|
|
|
|
#include <cstdlib>
|
|
#include <limits>
|
|
#include <array>
|
|
#include <cstddef>
|
|
#include <algorithm>
|
|
#include <cassert>
|
|
#include <boost/container/small_vector.hpp>
|
|
#include <Eigen/Geometry>
|
|
#include <cmath>
|
|
#include <math.h>
|
|
#include <iterator>
|
|
#include <memory>
|
|
#include <iostream>
|
|
#include <Eigen/Core>
|
|
#include <cstdint>
|
|
#include <numeric>
|
|
#include <optional>
|
|
#include <set>
|
|
#include <tbb/parallel_for.h>
|
|
#include <tuple>
|
|
#include <vector>
|
|
#include <utility>
|
|
|
|
#define DEBUG_EXTRACT_ALL_FEATURES_AT_ONCE 0
|
|
|
|
namespace Slic3r {
|
|
namespace Measure {
|
|
bool get_point_projection_to_plane(const Vec3d &pt, const Vec3d &plane_origin, const Vec3d &plane_normal, Vec3d &intersection_pt)
|
|
{
|
|
Vec3d normal = plane_normal.normalized();
|
|
Vec3d BA = plane_origin - pt;
|
|
auto length = BA.dot(normal);
|
|
intersection_pt = pt + length * normal;
|
|
return true;
|
|
}
|
|
|
|
Vec3d get_one_point_in_plane(const Vec3d &plane_origin, const Vec3d &plane_normal)
|
|
{
|
|
Vec3d dir(1, 0, 0);
|
|
float eps = 1e-3;
|
|
if (abs(plane_normal.dot(dir)) > 1 - eps) {
|
|
dir = Vec3d(0, 1, 0);
|
|
}
|
|
Vec3d new_pt = plane_origin + dir;
|
|
Vec3d retult;
|
|
get_point_projection_to_plane(new_pt, plane_origin, plane_normal, retult);
|
|
return retult;
|
|
}
|
|
|
|
constexpr double feature_hover_limit = 0.5; // how close to a feature the mouse must be to highlight it
|
|
|
|
static std::tuple<Vec3d, double, double> get_center_and_radius(const std::vector<Vec3d>& points, const Transform3d& trafo, const Transform3d& trafo_inv)
|
|
{
|
|
Vec2ds out;
|
|
double z = 0.;
|
|
for (const Vec3d& pt : points) {
|
|
Vec3d pt_transformed = trafo * pt;
|
|
z = pt_transformed.z();
|
|
out.emplace_back(pt_transformed.x(), pt_transformed.y());
|
|
}
|
|
|
|
const int iter = points.size() < 10 ? 2 :
|
|
points.size() < 100 ? 4 :
|
|
6;
|
|
|
|
double error = std::numeric_limits<double>::max();
|
|
auto circle = Geometry::circle_ransac(out, iter, &error);
|
|
|
|
return std::make_tuple(trafo.inverse() * Vec3d(circle.center.x(), circle.center.y(), z), circle.radius, error);
|
|
}
|
|
|
|
|
|
|
|
static std::array<Vec3d, 3> orthonormal_basis(const Vec3d& v)
|
|
{
|
|
std::array<Vec3d, 3> ret;
|
|
ret[2] = v.normalized();
|
|
int index;
|
|
ret[2].cwiseAbs().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;
|
|
}
|
|
|
|
|
|
|
|
class MeasuringImpl {
|
|
public:
|
|
explicit MeasuringImpl(const indexed_triangle_set& its);
|
|
struct PlaneData {
|
|
std::vector<int> facets;
|
|
std::vector<std::vector<Vec3d>> borders; // FIXME: should be in fact local in update_planes()
|
|
std::vector<SurfaceFeature> surface_features;
|
|
Vec3d normal;
|
|
float area;
|
|
bool features_extracted = false;
|
|
};
|
|
|
|
std::optional<SurfaceFeature> get_feature(size_t face_idx, const Vec3d &point, const Transform3d &world_tran,bool only_select_plane);
|
|
int get_num_of_planes() const;
|
|
const std::vector<int>& get_plane_triangle_indices(int idx) const;
|
|
std::vector<int>* get_plane_tri_indices(int idx);
|
|
const std::vector<SurfaceFeature>& get_plane_features(unsigned int plane_id);
|
|
std::vector<SurfaceFeature>* get_plane_features_pointer(unsigned int plane_id);
|
|
const indexed_triangle_set& get_its() const;
|
|
|
|
private:
|
|
void update_planes();
|
|
void extract_features(int plane_idx);
|
|
|
|
std::vector<PlaneData> m_planes;
|
|
std::vector<size_t> m_face_to_plane;
|
|
indexed_triangle_set m_its;
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
MeasuringImpl::MeasuringImpl(const indexed_triangle_set& its)
|
|
: m_its(its)
|
|
{
|
|
update_planes();
|
|
|
|
// Extracting features will be done as needed.
|
|
// To extract all planes at once, run the following:
|
|
#if DEBUG_EXTRACT_ALL_FEATURES_AT_ONCE
|
|
for (int i=0; i<int(m_planes.size()); ++i)
|
|
extract_features(i);
|
|
#endif
|
|
}
|
|
|
|
|
|
void MeasuringImpl::update_planes()
|
|
{
|
|
// Now we'll go through all the facets and append Points of facets sharing the same normal.
|
|
// This part is still performed in mesh coordinate system.
|
|
const size_t num_of_facets = m_its.indices.size();
|
|
m_face_to_plane.resize(num_of_facets, size_t(-1));
|
|
const std::vector<Vec3f> face_normals = its_face_normals(m_its);
|
|
const std::vector<Vec3i32> face_neighbors = its_face_neighbors(m_its);
|
|
std::vector<int> facet_queue(num_of_facets, 0);
|
|
int facet_queue_cnt = 0;
|
|
const stl_normal* normal_ptr = nullptr;
|
|
size_t seed_facet_idx = 0;
|
|
|
|
auto is_same_normal = [](const stl_normal& a, const stl_normal& b) -> bool {
|
|
return (std::abs(a(0) - b(0)) < 0.001 && std::abs(a(1) - b(1)) < 0.001 && std::abs(a(2) - b(2)) < 0.001);
|
|
};
|
|
|
|
m_planes.clear();
|
|
m_planes.reserve(num_of_facets / 5); // empty plane data object is quite lightweight, let's save the initial reallocations
|
|
|
|
|
|
// First go through all the triangles and fill in m_planes vector. For each "plane"
|
|
// detected on the model, it will contain list of facets that are part of it.
|
|
// We will also fill in m_face_to_plane, which contains index into m_planes
|
|
// for each of the source facets.
|
|
while (1) {
|
|
// Find next unvisited triangle:
|
|
for (; seed_facet_idx < num_of_facets; ++ seed_facet_idx)
|
|
if (m_face_to_plane[seed_facet_idx] == size_t(-1)) {
|
|
facet_queue[facet_queue_cnt ++] = seed_facet_idx;
|
|
normal_ptr = &face_normals[seed_facet_idx];
|
|
m_face_to_plane[seed_facet_idx] = m_planes.size();
|
|
m_planes.emplace_back();
|
|
break;
|
|
}
|
|
if (seed_facet_idx == num_of_facets)
|
|
break; // Everything was visited already
|
|
|
|
while (facet_queue_cnt > 0) {
|
|
int facet_idx = facet_queue[-- facet_queue_cnt];
|
|
const stl_normal& this_normal = face_normals[facet_idx];
|
|
if (is_same_normal(this_normal, *normal_ptr)) {
|
|
// const Vec3i32& face = m_its.indices[facet_idx];
|
|
|
|
m_face_to_plane[facet_idx] = m_planes.size() - 1;
|
|
m_planes.back().facets.emplace_back(facet_idx);
|
|
for (int j = 0; j < 3; ++ j)
|
|
if (int neighbor_idx = face_neighbors[facet_idx][j]; neighbor_idx >= 0 && m_face_to_plane[neighbor_idx] == size_t(-1))
|
|
facet_queue[facet_queue_cnt ++] = neighbor_idx;
|
|
}
|
|
}
|
|
|
|
m_planes.back().normal = normal_ptr->cast<double>();
|
|
std::sort(m_planes.back().facets.begin(), m_planes.back().facets.end());
|
|
}
|
|
|
|
// Check that each facet is part of one of the planes.
|
|
assert(std::none_of(m_face_to_plane.begin(), m_face_to_plane.end(), [](size_t val) { return val == size_t(-1); }));
|
|
|
|
// Now we will walk around each of the planes and save vertices which form the border.
|
|
const SurfaceMesh sm(m_its);
|
|
|
|
const auto& face_to_plane = m_face_to_plane;
|
|
auto& planes = m_planes;
|
|
|
|
tbb::parallel_for(tbb::blocked_range<size_t>(0, m_planes.size()),
|
|
[&planes, &face_to_plane, &face_neighbors, &sm](const tbb::blocked_range<size_t>& range) {
|
|
for (size_t plane_id = range.begin(); plane_id != range.end(); ++plane_id) {
|
|
|
|
const auto& facets = planes[plane_id].facets;
|
|
planes[plane_id].borders.clear();
|
|
std::vector<std::array<bool, 3>> visited(facets.size(), {false, false, false});
|
|
|
|
for (int face_id=0; face_id<int(facets.size()); ++face_id) {
|
|
assert(face_to_plane[facets[face_id]] == plane_id);
|
|
|
|
for (int edge_id=0; edge_id<3; ++edge_id) {
|
|
// Every facet's edge which has a neighbor from a different plane is
|
|
// part of an edge that we want to walk around. Skip the others.
|
|
int neighbor_idx = face_neighbors[facets[face_id]][edge_id];
|
|
if (neighbor_idx == -1)
|
|
goto PLANE_FAILURE;
|
|
if (visited[face_id][edge_id] || face_to_plane[neighbor_idx] == plane_id) {
|
|
visited[face_id][edge_id] = true;
|
|
continue;
|
|
}
|
|
|
|
Halfedge_index he = sm.halfedge(Face_index(facets[face_id]));
|
|
while (he.side() != edge_id)
|
|
he = sm.next(he);
|
|
|
|
// he is the first halfedge on the border. Now walk around and append the points.
|
|
//const Halfedge_index he_orig = he;
|
|
planes[plane_id].borders.emplace_back();
|
|
std::vector<Vec3d>& last_border = planes[plane_id].borders.back();
|
|
last_border.reserve(4);
|
|
last_border.emplace_back(sm.point(sm.source(he)).cast<double>());
|
|
//Vertex_index target = sm.target(he);
|
|
const Halfedge_index he_start = he;
|
|
|
|
Face_index fi = he.face();
|
|
auto face_it = std::lower_bound(facets.begin(), facets.end(), int(fi));
|
|
assert(face_it != facets.end());
|
|
assert(*face_it == int(fi));
|
|
visited[face_it - facets.begin()][he.side()] = true;
|
|
|
|
do {
|
|
const Halfedge_index he_orig = he;
|
|
he = sm.next_around_target(he);
|
|
if (he.is_invalid())
|
|
goto PLANE_FAILURE;
|
|
|
|
// For broken meshes, the iteration might never get back to he_orig.
|
|
// Remember all halfedges we saw to break out of such infinite loops.
|
|
boost::container::small_vector<Halfedge_index, 10> he_seen;
|
|
|
|
while ( face_to_plane[sm.face(he)] == plane_id && he != he_orig) {
|
|
he_seen.emplace_back(he);
|
|
he = sm.next_around_target(he);
|
|
if (he.is_invalid() || std::find(he_seen.begin(), he_seen.end(), he) != he_seen.end())
|
|
goto PLANE_FAILURE;
|
|
}
|
|
he = sm.opposite(he);
|
|
if (he.is_invalid())
|
|
goto PLANE_FAILURE;
|
|
|
|
Face_index fi = he.face();
|
|
auto face_it = std::lower_bound(facets.begin(), facets.end(), int(fi));
|
|
if (face_it == facets.end() || *face_it != int(fi)) // This indicates a broken mesh.
|
|
goto PLANE_FAILURE;
|
|
|
|
if (visited[face_it - facets.begin()][he.side()] && he != he_start) {
|
|
last_border.resize(1);
|
|
break;
|
|
}
|
|
visited[face_it - facets.begin()][he.side()] = true;
|
|
|
|
last_border.emplace_back(sm.point(sm.source(he)).cast<double>());
|
|
|
|
// In case of broken meshes, this loop might be infinite. Break
|
|
// out in case it is clearly going bad.
|
|
if (last_border.size() > 3*facets.size()+1)
|
|
goto PLANE_FAILURE;
|
|
|
|
} while (he != he_start);
|
|
|
|
if (last_border.size() == 1)
|
|
planes[plane_id].borders.pop_back();
|
|
else {
|
|
assert(last_border.front() == last_border.back());
|
|
last_border.pop_back();
|
|
}
|
|
}
|
|
}
|
|
continue; // There was no failure.
|
|
|
|
PLANE_FAILURE:
|
|
planes[plane_id].borders.clear();
|
|
}});
|
|
m_planes.shrink_to_fit();
|
|
}
|
|
|
|
void MeasuringImpl::extract_features(int plane_idx)
|
|
{
|
|
assert(! m_planes[plane_idx].features_extracted);
|
|
|
|
PlaneData& plane = m_planes[plane_idx];
|
|
plane.surface_features.clear();
|
|
const Vec3d& normal = plane.normal;
|
|
|
|
Eigen::Quaterniond q;
|
|
q.setFromTwoVectors(plane.normal, Vec3d::UnitZ());
|
|
Transform3d trafo = Transform3d::Identity();
|
|
trafo.rotate(q);
|
|
const Transform3d trafo_inv = trafo.inverse();
|
|
|
|
std::vector<double> angles; // placed in outer scope to prevent reallocations
|
|
std::vector<double> lengths;
|
|
|
|
for (const std::vector<Vec3d>& border : plane.borders) {
|
|
if (border.size() <= 1)
|
|
continue;
|
|
|
|
bool done = false;
|
|
|
|
if (border.size() > 4) {
|
|
const auto& [center, radius, err] = get_center_and_radius(border, trafo, trafo_inv);
|
|
|
|
if (err < 0.05) {
|
|
// The whole border is one circle. Just add it into the list of features
|
|
// and we are done.
|
|
|
|
bool is_polygon = border.size()>4 && border.size()<=8;
|
|
bool lengths_match = std::all_of(border.begin()+2, border.end(), [is_polygon](const Vec3d& pt) {
|
|
return Slic3r::is_approx((pt - *((&pt)-1)).squaredNorm(), (*((&pt)-1) - *((&pt)-2)).squaredNorm(), is_polygon ? 0.01 : 0.01);
|
|
});
|
|
|
|
if (lengths_match && (is_polygon || border.size() > 8)) {
|
|
if (is_polygon) {
|
|
// This is a polygon, add the separate edges with the center.
|
|
for (int j=0; j<int(border.size()); ++j)
|
|
plane.surface_features.emplace_back(SurfaceFeature(SurfaceFeatureType::Edge,
|
|
border[j==0 ? border.size()-1 : j-1], border[j],
|
|
std::make_optional(center)));
|
|
} else {
|
|
// The fit went well and it has more than 8 points - let's consider this a circle.
|
|
plane.surface_features.emplace_back(SurfaceFeature(SurfaceFeatureType::Circle, center, plane.normal, std::nullopt, radius));
|
|
}
|
|
done = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (! done) {
|
|
// In this case, the border is not a circle and may contain circular
|
|
// segments. Try to find them and then add all remaining edges as edges.
|
|
|
|
auto are_angles_same = [](double a, double b) { return Slic3r::is_approx(a,b,0.01); };
|
|
auto are_lengths_same = [](double a, double b) { return Slic3r::is_approx(a,b,0.01); };
|
|
|
|
|
|
// Given an idx into border, return the index that is idx+offset position,
|
|
// while taking into account the need for wrap-around and the fact that
|
|
// the first and last point are the same.
|
|
auto offset_to_index = [border_size = int(border.size())](int idx, int offset) -> int {
|
|
assert(std::abs(offset) < border_size);
|
|
int out = idx+offset;
|
|
if (out >= border_size)
|
|
out = out - border_size;
|
|
else if (out < 0)
|
|
out = border_size + out;
|
|
|
|
return out;
|
|
};
|
|
|
|
// First calculate angles at all the vertices.
|
|
angles.clear();
|
|
lengths.clear();
|
|
int first_different_angle_idx = 0;
|
|
for (int i=0; i<int(border.size()); ++i) {
|
|
const Vec3d& v2 = border[i] - (i == 0 ? border[border.size()-1] : border[i-1]);
|
|
const Vec3d& v1 = (i == int(border.size()-1) ? border[0] : border[i+1]) - border[i];
|
|
double angle = atan2(-normal.dot(v1.cross(v2)), -v1.dot(v2)) + M_PI;
|
|
if (angle > M_PI)
|
|
angle = 2*M_PI - angle;
|
|
|
|
angles.push_back(angle);
|
|
lengths.push_back(v2.norm());
|
|
if (first_different_angle_idx == 0 && angles.size() > 1) {
|
|
if (! are_angles_same(angles.back(), angles[angles.size()-2]))
|
|
first_different_angle_idx = angles.size()-1;
|
|
}
|
|
}
|
|
assert(border.size() == angles.size());
|
|
assert(border.size() == lengths.size());
|
|
|
|
// First go around the border and pick what might be circular segments.
|
|
// Save pair of indices to where such potential segments start and end.
|
|
// Also remember the length of these segments.
|
|
int start_idx = -1;
|
|
bool circle = false;
|
|
bool first_iter = true;
|
|
std::vector<SurfaceFeature> circles;
|
|
std::vector<SurfaceFeature> edges;
|
|
std::vector<std::pair<int, int>> circles_idxs;
|
|
//std::vector<double> circles_lengths;
|
|
std::vector<Vec3d> single_circle; // could be in loop-scope, but reallocations
|
|
double single_circle_length = 0.;
|
|
int first_pt_idx = offset_to_index(first_different_angle_idx, 1);
|
|
int i = first_pt_idx;
|
|
while (i != first_pt_idx || first_iter) {
|
|
if (are_angles_same(angles[i], angles[offset_to_index(i,-1)])
|
|
&& i != offset_to_index(first_pt_idx, -1) // not the last point
|
|
&& i != start_idx ) {
|
|
// circle
|
|
if (! circle) {
|
|
circle = true;
|
|
single_circle.clear();
|
|
single_circle_length = 0.;
|
|
start_idx = offset_to_index(i, -2);
|
|
single_circle = { border[start_idx], border[offset_to_index(start_idx,1)] };
|
|
single_circle_length += lengths[offset_to_index(i, -1)];
|
|
}
|
|
single_circle.emplace_back(border[i]);
|
|
single_circle_length += lengths[i];
|
|
} else {
|
|
if (circle && single_circle.size() >= 5) { // Less than 5 vertices? Not a circle.
|
|
single_circle.emplace_back(border[i]);
|
|
single_circle_length += lengths[i];
|
|
|
|
bool accept_circle = true;
|
|
{
|
|
// Check that lengths of internal (!!!) edges match.
|
|
int j = offset_to_index(start_idx, 3);
|
|
while (j != i) {
|
|
if (! are_lengths_same(lengths[offset_to_index(j,-1)], lengths[j])) {
|
|
accept_circle = false;
|
|
break;
|
|
}
|
|
j = offset_to_index(j, 1);
|
|
}
|
|
}
|
|
|
|
if (accept_circle) {
|
|
const auto& [center, radius, err] = get_center_and_radius(single_circle, trafo, trafo_inv);
|
|
|
|
// Check that the fit went well. The tolerance is high, only to
|
|
// reject complete failures.
|
|
accept_circle &= err < 0.05;
|
|
|
|
// If the segment subtends less than 90 degrees, throw it away.
|
|
accept_circle &= single_circle_length / radius > 0.9*M_PI/2.;
|
|
|
|
if (accept_circle) {
|
|
// Add the circle and remember indices into borders.
|
|
circles_idxs.emplace_back(start_idx, i);
|
|
circles.emplace_back(SurfaceFeature(SurfaceFeatureType::Circle, center, plane.normal, std::nullopt, radius));
|
|
}
|
|
}
|
|
}
|
|
circle = false;
|
|
}
|
|
// Take care of the wrap around.
|
|
first_iter = false;
|
|
i = offset_to_index(i, 1);
|
|
}
|
|
|
|
// We have the circles. Now go around again and pick edges, while jumping over circles.
|
|
if (circles_idxs.empty()) {
|
|
// Just add all edges.
|
|
for (int i=1; i<int(border.size()); ++i)
|
|
edges.emplace_back(SurfaceFeature(SurfaceFeatureType::Edge, border[i-1], border[i]));
|
|
edges.emplace_back(SurfaceFeature(SurfaceFeatureType::Edge, border[0], border[border.size()-1]));
|
|
} else if (circles_idxs.size() > 1 || circles_idxs.front().first != circles_idxs.front().second) {
|
|
// There is at least one circular segment. Start at its end and add edges until the start of the next one.
|
|
int i = circles_idxs.front().second;
|
|
int circle_idx = 1;
|
|
while (true) {
|
|
i = offset_to_index(i, 1);
|
|
edges.emplace_back(SurfaceFeature(SurfaceFeatureType::Edge, border[offset_to_index(i,-1)], border[i]));
|
|
if (circle_idx < int(circles_idxs.size()) && i == circles_idxs[circle_idx].first) {
|
|
i = circles_idxs[circle_idx].second;
|
|
++circle_idx;
|
|
}
|
|
if (i == circles_idxs.front().first)
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Merge adjacent edges where needed.
|
|
assert(std::all_of(edges.begin(), edges.end(),
|
|
[](const SurfaceFeature& f) { return f.get_type() == SurfaceFeatureType::Edge; }));
|
|
for (int i=edges.size()-1; i>=0; --i) {
|
|
const auto& [first_start, first_end] = edges[i==0 ? edges.size()-1 : i-1].get_edge();
|
|
const auto& [second_start, second_end] = edges[i].get_edge();
|
|
|
|
if (Slic3r::is_approx(first_end, second_start)
|
|
&& Slic3r::is_approx((first_end-first_start).normalized().dot((second_end-second_start).normalized()), 1.)) {
|
|
// The edges have the same direction and share a point. Merge them.
|
|
edges[i==0 ? edges.size()-1 : i-1] = SurfaceFeature(SurfaceFeatureType::Edge, first_start, second_end);
|
|
edges.erase(edges.begin() + i);
|
|
}
|
|
}
|
|
|
|
// Now move the circles and edges into the feature list for the plane.
|
|
assert(std::all_of(circles.begin(), circles.end(), [](const SurfaceFeature& f) {
|
|
return f.get_type() == SurfaceFeatureType::Circle;
|
|
}));
|
|
assert(std::all_of(edges.begin(), edges.end(), [](const SurfaceFeature& f) {
|
|
return f.get_type() == SurfaceFeatureType::Edge;
|
|
}));
|
|
plane.surface_features.insert(plane.surface_features.end(), std::make_move_iterator(circles.begin()),
|
|
std::make_move_iterator(circles.end()));
|
|
plane.surface_features.insert(plane.surface_features.end(), std::make_move_iterator(edges.begin()),
|
|
std::make_move_iterator(edges.end()));
|
|
}
|
|
}
|
|
|
|
// The last surface feature is the plane itself.
|
|
Vec3d cog = Vec3d::Zero();
|
|
size_t counter = 0;
|
|
for (const std::vector<Vec3d>& b : plane.borders) {
|
|
for (size_t i = 0; i < b.size(); ++i) {
|
|
cog += b[i];
|
|
++counter;
|
|
}
|
|
}
|
|
cog /= double(counter);
|
|
plane.surface_features.emplace_back(SurfaceFeature(SurfaceFeatureType::Plane,
|
|
plane.normal, cog, std::optional<Vec3d>(), plane_idx + 0.0001));
|
|
|
|
plane.borders.clear();
|
|
plane.borders.shrink_to_fit();
|
|
|
|
plane.features_extracted = true;
|
|
}
|
|
|
|
std::optional<SurfaceFeature> MeasuringImpl::get_feature(size_t face_idx, const Vec3d &point, const Transform3d &world_tran,bool only_select_plane)
|
|
{
|
|
if (face_idx >= m_face_to_plane.size())
|
|
return std::optional<SurfaceFeature>();
|
|
|
|
const PlaneData& plane = m_planes[m_face_to_plane[face_idx]];
|
|
|
|
if (! plane.features_extracted)
|
|
extract_features(m_face_to_plane[face_idx]);
|
|
|
|
size_t closest_feature_idx = size_t(-1);
|
|
double min_dist = std::numeric_limits<double>::max();
|
|
|
|
MeasurementResult res;
|
|
SurfaceFeature point_sf(point);
|
|
|
|
assert(plane.surface_features.empty() || plane.surface_features.back().get_type() == SurfaceFeatureType::Plane);
|
|
|
|
if (!only_select_plane) {
|
|
for (size_t i = 0; i < plane.surface_features.size() - 1; ++i) {
|
|
// The -1 is there to prevent measuring distance to the plane itself,
|
|
// which is needless and relatively expensive.
|
|
res = get_measurement(plane.surface_features[i], point_sf);
|
|
if (res.distance_strict) { // TODO: this should become an assert after all combinations are implemented.
|
|
double dist = res.distance_strict->dist;
|
|
if (dist < feature_hover_limit && dist < min_dist) {
|
|
min_dist = std::min(dist, min_dist);
|
|
closest_feature_idx = i;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (closest_feature_idx != size_t(-1)) {
|
|
const SurfaceFeature &f = plane.surface_features[closest_feature_idx];
|
|
if (f.get_type() == SurfaceFeatureType::Edge) {
|
|
// If this is an edge, check if we are not close to the endpoint. If so,
|
|
// we will include the endpoint as well. Close = 10% of the lenghth of
|
|
// the edge, clamped between 0.025 and 0.5 mm.
|
|
const auto &[sp, ep] = f.get_edge();
|
|
double len_sq = (ep - sp).squaredNorm();
|
|
double limit_sq = std::max(0.025 * 0.025, std::min(0.5 * 0.5, 0.1 * 0.1 * len_sq));
|
|
if ((point - sp).squaredNorm() < limit_sq) {
|
|
SurfaceFeature local_f(sp);
|
|
local_f.origin_surface_feature = std::make_shared<SurfaceFeature>(local_f);
|
|
local_f.translate(world_tran);
|
|
return std::make_optional(local_f);
|
|
}
|
|
|
|
if ((point - ep).squaredNorm() < limit_sq) {
|
|
SurfaceFeature local_f(ep);
|
|
local_f.origin_surface_feature = std::make_shared<SurfaceFeature>(local_f);
|
|
local_f.translate(world_tran);
|
|
return std::make_optional(local_f);
|
|
}
|
|
}
|
|
SurfaceFeature f_tran(f);
|
|
f_tran.origin_surface_feature = std::make_shared<SurfaceFeature>(f);
|
|
f_tran.translate(world_tran);
|
|
return std::make_optional(f_tran);
|
|
}
|
|
}
|
|
|
|
// Nothing detected, return the plane as a whole.
|
|
assert(plane.surface_features.back().get_type() == SurfaceFeatureType::Plane);
|
|
auto cur_plane = const_cast<PlaneData*>(&plane);
|
|
SurfaceFeature f_tran(cur_plane->surface_features.back());
|
|
f_tran.origin_surface_feature = std::make_shared<SurfaceFeature>(cur_plane->surface_features.back());
|
|
f_tran.translate(world_tran);
|
|
return std::make_optional(f_tran);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int MeasuringImpl::get_num_of_planes() const
|
|
{
|
|
return (m_planes.size());
|
|
}
|
|
|
|
|
|
|
|
const std::vector<int>& MeasuringImpl::get_plane_triangle_indices(int idx) const
|
|
{
|
|
assert(idx >= 0 && idx < int(m_planes.size()));
|
|
return m_planes[idx].facets;
|
|
}
|
|
|
|
std::vector<int>* MeasuringImpl::get_plane_tri_indices(int idx)
|
|
{
|
|
assert(idx >= 0 && idx < int(m_planes.size()));
|
|
return &m_planes[idx].facets;
|
|
}
|
|
|
|
const std::vector<SurfaceFeature>& MeasuringImpl::get_plane_features(unsigned int plane_id)
|
|
{
|
|
assert(plane_id < m_planes.size());
|
|
if (! m_planes[plane_id].features_extracted)
|
|
extract_features(plane_id);
|
|
return m_planes[plane_id].surface_features;
|
|
}
|
|
|
|
std::vector<SurfaceFeature>* MeasuringImpl::get_plane_features_pointer(unsigned int plane_id) {
|
|
assert(plane_id < m_planes.size());
|
|
if (!m_planes[plane_id].features_extracted)
|
|
extract_features(plane_id);
|
|
return &m_planes[plane_id].surface_features;
|
|
}
|
|
|
|
const indexed_triangle_set& MeasuringImpl::get_its() const
|
|
{
|
|
return this->m_its;
|
|
}
|
|
|
|
Measuring::Measuring(const indexed_triangle_set& its)
|
|
: priv{std::make_unique<MeasuringImpl>(its)}
|
|
{}
|
|
|
|
Measuring::~Measuring() {}
|
|
|
|
|
|
|
|
std::optional<SurfaceFeature> Measuring::get_feature(size_t face_idx, const Vec3d &point, const Transform3d &world_tran, bool only_select_plane) const
|
|
{
|
|
if (face_idx == 7516 || face_idx == 7517) {
|
|
std::cout << "";
|
|
}
|
|
return priv->get_feature(face_idx, point, world_tran, only_select_plane);
|
|
}
|
|
|
|
|
|
int Measuring::get_num_of_planes() const
|
|
{
|
|
return priv->get_num_of_planes();
|
|
}
|
|
|
|
|
|
const std::vector<int>& Measuring::get_plane_triangle_indices(int idx) const
|
|
{
|
|
return priv->get_plane_triangle_indices(idx);
|
|
}
|
|
|
|
const std::vector<SurfaceFeature>& Measuring::get_plane_features(unsigned int plane_id) const
|
|
{
|
|
return priv->get_plane_features(plane_id);
|
|
}
|
|
|
|
const indexed_triangle_set& Measuring::get_its() const
|
|
{
|
|
return priv->get_its();
|
|
}
|
|
|
|
const AngleAndEdges AngleAndEdges::Dummy = { 0.0, Vec3d::Zero(), { Vec3d::Zero(), Vec3d::Zero() }, { Vec3d::Zero(), Vec3d::Zero() }, 0.0, true };
|
|
|
|
static AngleAndEdges angle_edge_edge(const std::pair<Vec3d, Vec3d>& e1, const std::pair<Vec3d, Vec3d>& e2)
|
|
{
|
|
if (are_parallel(e1, e2))
|
|
return AngleAndEdges::Dummy;
|
|
|
|
Vec3d e1_unit = edge_direction(e1.first, e1.second);
|
|
Vec3d e2_unit = edge_direction(e2.first, e2.second);
|
|
|
|
// project edges on the plane defined by them
|
|
Vec3d normal = e1_unit.cross(e2_unit).normalized();
|
|
const Eigen::Hyperplane<double, 3> plane(normal, e1.first);
|
|
Vec3d e11_proj = plane.projection(e1.first);
|
|
Vec3d e12_proj = plane.projection(e1.second);
|
|
Vec3d e21_proj = plane.projection(e2.first);
|
|
Vec3d e22_proj = plane.projection(e2.second);
|
|
|
|
const bool coplanar = (e2.first - e21_proj).norm() < EPSILON && (e2.second - e22_proj).norm() < EPSILON;
|
|
|
|
// rotate the plane to become the XY plane
|
|
auto qp = Eigen::Quaternion<double>::FromTwoVectors(normal, Vec3d::UnitZ());
|
|
auto qp_inverse = qp.inverse();
|
|
const Vec3d e11_rot = qp * e11_proj;
|
|
const Vec3d e12_rot = qp * e12_proj;
|
|
const Vec3d e21_rot = qp * e21_proj;
|
|
const Vec3d e22_rot = qp * e22_proj;
|
|
|
|
// discard Z
|
|
const Vec2d e11_rot_2d = Vec2d(e11_rot.x(), e11_rot.y());
|
|
const Vec2d e12_rot_2d = Vec2d(e12_rot.x(), e12_rot.y());
|
|
const Vec2d e21_rot_2d = Vec2d(e21_rot.x(), e21_rot.y());
|
|
const Vec2d e22_rot_2d = Vec2d(e22_rot.x(), e22_rot.y());
|
|
|
|
// find intersection (arc center) of edges in XY plane
|
|
const Eigen::Hyperplane<double, 2> e1_rot_2d_line = Eigen::Hyperplane<double, 2>::Through(e11_rot_2d, e12_rot_2d);
|
|
const Eigen::Hyperplane<double, 2> e2_rot_2d_line = Eigen::Hyperplane<double, 2>::Through(e21_rot_2d, e22_rot_2d);
|
|
const Vec2d center_rot_2d = e1_rot_2d_line.intersection(e2_rot_2d_line);
|
|
|
|
// arc center in original coordinate
|
|
const Vec3d center = qp_inverse * Vec3d(center_rot_2d.x(), center_rot_2d.y(), e11_rot.z());
|
|
|
|
// ensure the edges are pointing away from the center
|
|
std::pair<Vec3d, Vec3d> out_e1 = e1;
|
|
std::pair<Vec3d, Vec3d> out_e2 = e2;
|
|
if ((center_rot_2d - e11_rot_2d).squaredNorm() > (center_rot_2d - e12_rot_2d).squaredNorm()) {
|
|
std::swap(e11_proj, e12_proj);
|
|
std::swap(out_e1.first, out_e1.second);
|
|
e1_unit = -e1_unit;
|
|
}
|
|
if ((center_rot_2d - e21_rot_2d).squaredNorm() > (center_rot_2d - e22_rot_2d).squaredNorm()) {
|
|
std::swap(e21_proj, e22_proj);
|
|
std::swap(out_e2.first, out_e2.second);
|
|
e2_unit = -e2_unit;
|
|
}
|
|
|
|
// arc angle
|
|
const double angle = std::acos(std::clamp(e1_unit.dot(e2_unit), -1.0, 1.0));
|
|
// arc radius
|
|
const Vec3d e1_proj_mid = 0.5 * (e11_proj + e12_proj);
|
|
const Vec3d e2_proj_mid = 0.5 * (e21_proj + e22_proj);
|
|
const double radius = std::min((center - e1_proj_mid).norm(), (center - e2_proj_mid).norm());
|
|
|
|
return { angle, center, out_e1, out_e2, radius, coplanar };
|
|
}
|
|
|
|
static AngleAndEdges angle_edge_plane(const std::pair<Vec3d, Vec3d>& e, const std::tuple<int, Vec3d, Vec3d>& p)
|
|
{
|
|
const auto& [idx, normal, origin] = p;
|
|
Vec3d e1e2_unit = edge_direction(e);
|
|
if (are_perpendicular(e1e2_unit, normal))
|
|
return AngleAndEdges::Dummy;
|
|
|
|
// ensure the edge is pointing away from the intersection
|
|
// 1st calculate instersection between edge and plane
|
|
const Eigen::Hyperplane<double, 3> plane(normal, origin);
|
|
const Eigen::ParametrizedLine<double, 3> line = Eigen::ParametrizedLine<double, 3>::Through(e.first, e.second);
|
|
const Vec3d inters = line.intersectionPoint(plane);
|
|
|
|
// then verify edge direction and revert it, if needed
|
|
Vec3d e1 = e.first;
|
|
Vec3d e2 = e.second;
|
|
if ((e1 - inters).squaredNorm() > (e2 - inters).squaredNorm()) {
|
|
std::swap(e1, e2);
|
|
e1e2_unit = -e1e2_unit;
|
|
}
|
|
|
|
if (are_parallel(e1e2_unit, normal)) {
|
|
const std::array<Vec3d, 3> basis = orthonormal_basis(e1e2_unit);
|
|
const double radius = (0.5 * (e1 + e2) - inters).norm();
|
|
const Vec3d edge_on_plane_dir = (basis[1].dot(origin - inters) >= 0.0) ? basis[1] : -basis[1];
|
|
std::pair<Vec3d, Vec3d> edge_on_plane = std::make_pair(inters, inters + radius * edge_on_plane_dir);
|
|
if (!inters.isApprox(e1)) {
|
|
edge_on_plane.first += radius * edge_on_plane_dir;
|
|
edge_on_plane.second += radius * edge_on_plane_dir;
|
|
}
|
|
return AngleAndEdges(0.5 * double(PI), inters, std::make_pair(e1, e2), edge_on_plane, radius, inters.isApprox(e1));
|
|
}
|
|
|
|
const Vec3d e1e2 = e2 - e1;
|
|
const double e1e2_len = e1e2.norm();
|
|
|
|
// calculate 2nd edge (on the plane)
|
|
const Vec3d temp = normal.cross(e1e2);
|
|
const Vec3d edge_on_plane_unit = normal.cross(temp).normalized();
|
|
std::pair<Vec3d, Vec3d> edge_on_plane = { origin, origin + e1e2_len * edge_on_plane_unit };
|
|
|
|
// ensure the 2nd edge is pointing in the correct direction
|
|
const Vec3d test_edge = (edge_on_plane.second - edge_on_plane.first).cross(e1e2);
|
|
if (test_edge.dot(temp) < 0.0)
|
|
edge_on_plane = { origin, origin - e1e2_len * edge_on_plane_unit };
|
|
|
|
AngleAndEdges ret = angle_edge_edge({ e1, e2 }, edge_on_plane);
|
|
ret.radius = (inters - 0.5 * (e1 + e2)).norm();
|
|
return ret;
|
|
}
|
|
|
|
static AngleAndEdges angle_plane_plane(const std::tuple<int, Vec3d, Vec3d>& p1, const std::tuple<int, Vec3d, Vec3d>& p2)
|
|
{
|
|
const auto& [idx1, normal1, origin1] = p1;
|
|
const auto& [idx2, normal2, origin2] = p2;
|
|
|
|
// are planes parallel ?
|
|
if (are_parallel(normal1, normal2))
|
|
return AngleAndEdges::Dummy;
|
|
|
|
auto intersection_plane_plane = [](const Vec3d& n1, const Vec3d& o1, const Vec3d& n2, const Vec3d& o2) {
|
|
Eigen::MatrixXd m(2, 3);
|
|
m << n1.x(), n1.y(), n1.z(), n2.x(), n2.y(), n2.z();
|
|
Eigen::VectorXd b(2);
|
|
b << o1.dot(n1), o2.dot(n2);
|
|
Eigen::VectorXd x = m.colPivHouseholderQr().solve(b);
|
|
return std::make_pair(n1.cross(n2).normalized(), Vec3d(x(0), x(1), x(2)));
|
|
};
|
|
|
|
// Calculate intersection line between planes
|
|
const auto [intersection_line_direction, intersection_line_origin] = intersection_plane_plane(normal1, origin1, normal2, origin2);
|
|
|
|
// Project planes' origin on intersection line
|
|
const Eigen::ParametrizedLine<double, 3> intersection_line = Eigen::ParametrizedLine<double, 3>(intersection_line_origin, intersection_line_direction);
|
|
const Vec3d origin1_proj = intersection_line.projection(origin1);
|
|
const Vec3d origin2_proj = intersection_line.projection(origin2);
|
|
|
|
// Calculate edges on planes
|
|
const Vec3d edge_on_plane1_unit = (origin1 - origin1_proj).normalized();
|
|
const Vec3d edge_on_plane2_unit = (origin2 - origin2_proj).normalized();
|
|
const double radius = std::max(10.0, std::max((origin1 - origin1_proj).norm(), (origin2 - origin2_proj).norm()));
|
|
const std::pair<Vec3d, Vec3d> edge_on_plane1 = { origin1_proj + radius * edge_on_plane1_unit, origin1_proj + 2.0 * radius * edge_on_plane1_unit };
|
|
const std::pair<Vec3d, Vec3d> edge_on_plane2 = { origin2_proj + radius * edge_on_plane2_unit, origin2_proj + 2.0 * radius * edge_on_plane2_unit };
|
|
|
|
AngleAndEdges ret = angle_edge_edge(edge_on_plane1, edge_on_plane2);
|
|
ret.radius = radius;
|
|
return ret;
|
|
}
|
|
|
|
MeasurementResult get_measurement(const SurfaceFeature &a, const SurfaceFeature &b, bool deal_circle_result)
|
|
{
|
|
assert(a.get_type() != SurfaceFeatureType::Undef && b.get_type() != SurfaceFeatureType::Undef);
|
|
|
|
const bool swap = int(a.get_type()) > int(b.get_type());
|
|
const SurfaceFeature& f1 = swap ? b : a;
|
|
const SurfaceFeature& f2 = swap ? a : b;
|
|
|
|
MeasurementResult result;
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////
|
|
if (f1.get_type() == SurfaceFeatureType::Point) {
|
|
if (f2.get_type() == SurfaceFeatureType::Point) {
|
|
Vec3d diff = (f2.get_point() - f1.get_point());
|
|
result.distance_strict = std::make_optional(DistAndPoints{diff.norm(), f1.get_point(), f2.get_point()});
|
|
result.distance_xyz = diff;
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
} else if (f2.get_type() == SurfaceFeatureType::Edge) {
|
|
const auto [s,e] = f2.get_edge();
|
|
const Eigen::ParametrizedLine<double, 3> line(s, (e-s).normalized());
|
|
const double dist_inf = line.distance(f1.get_point());
|
|
const Vec3d proj = line.projection(f1.get_point());
|
|
const double len_sq = (e-s).squaredNorm();
|
|
const double dist_start_sq = (proj-s).squaredNorm();
|
|
const double dist_end_sq = (proj-e).squaredNorm();
|
|
if (dist_start_sq < len_sq && dist_end_sq < len_sq) {
|
|
// projection falls on the line - the strict distance is the same as infinite
|
|
result.distance_strict = std::make_optional(DistAndPoints{dist_inf, f1.get_point(), proj});
|
|
} else { // the result is the closer of the endpoints
|
|
const bool s_is_closer = dist_start_sq < dist_end_sq;
|
|
result.distance_strict = std::make_optional(DistAndPoints{std::sqrt(std::min(dist_start_sq, dist_end_sq) + sqr(dist_inf)), f1.get_point(), s_is_closer ? s : e});
|
|
}
|
|
result.distance_infinite = std::make_optional(DistAndPoints{dist_inf, f1.get_point(), proj});
|
|
///////////////////////////////////////////////////////////////////////////
|
|
} else if (f2.get_type() == SurfaceFeatureType::Circle) {
|
|
// Find a plane containing normal, center and the point.
|
|
const auto [c, radius, n] = f2.get_circle();
|
|
const Eigen::Hyperplane<double, 3> circle_plane(n, c);
|
|
const Vec3d proj = circle_plane.projection(f1.get_point());
|
|
if (proj.isApprox(c)) {
|
|
const Vec3d p_on_circle = c + radius * get_orthogonal(n, true);
|
|
result.distance_strict = std::make_optional(DistAndPoints{ radius, c, p_on_circle });
|
|
}
|
|
else {
|
|
if (deal_circle_result == false) {
|
|
const Eigen::Hyperplane<double, 3> circle_plane(n, c);
|
|
const Vec3d proj = circle_plane.projection(f1.get_point());
|
|
const double dist = std::sqrt(std::pow((proj - c).norm() - radius, 2.) + (f1.get_point() - proj).squaredNorm());
|
|
|
|
const Vec3d p_on_circle = c + radius * (proj - c).normalized();
|
|
result.distance_strict = std::make_optional(DistAndPoints{dist, f1.get_point(), p_on_circle});
|
|
}
|
|
else {
|
|
const double dist = (f1.get_point() - c).norm();
|
|
result.distance_strict = std::make_optional(DistAndPoints{dist, f1.get_point(), c});
|
|
}
|
|
}
|
|
///////////////////////////////////////////////////////////////////////////
|
|
} else if (f2.get_type() == SurfaceFeatureType::Plane) {
|
|
const auto [idx, normal, pt] = f2.get_plane();
|
|
Eigen::Hyperplane<double, 3> plane(normal, pt);
|
|
result.distance_infinite = std::make_optional(DistAndPoints{plane.absDistance(f1.get_point()), f1.get_point(), plane.projection(f1.get_point())}); // TODO
|
|
// TODO: result.distance_strict =
|
|
}
|
|
///////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////
|
|
}
|
|
else if (f1.get_type() == SurfaceFeatureType::Edge) {
|
|
if (f2.get_type() == SurfaceFeatureType::Edge) {
|
|
std::vector<DistAndPoints> distances;
|
|
|
|
auto add_point_edge_distance = [&distances](const Vec3d& v, const std::pair<Vec3d, Vec3d>& e) {
|
|
const MeasurementResult res = get_measurement(SurfaceFeature(v), SurfaceFeature(SurfaceFeatureType::Edge, e.first, e.second));
|
|
double distance = res.distance_strict->dist;
|
|
Vec3d v2 = res.distance_strict->to;
|
|
|
|
const Vec3d e1e2 = e.second - e.first;
|
|
const Vec3d e1v2 = v2 - e.first;
|
|
if (e1v2.dot(e1e2) >= 0.0 && e1v2.norm() < e1e2.norm())
|
|
distances.emplace_back(distance, v, v2);
|
|
};
|
|
|
|
std::pair<Vec3d, Vec3d> e1 = f1.get_edge();
|
|
std::pair<Vec3d, Vec3d> e2 = f2.get_edge();
|
|
|
|
distances.emplace_back((e2.first - e1.first).norm(), e1.first, e2.first);
|
|
distances.emplace_back((e2.second - e1.first).norm(), e1.first, e2.second);
|
|
distances.emplace_back((e2.first - e1.second).norm(), e1.second, e2.first);
|
|
distances.emplace_back((e2.second - e1.second).norm(), e1.second, e2.second);
|
|
add_point_edge_distance(e1.first, e2);
|
|
add_point_edge_distance(e1.second, e2);
|
|
add_point_edge_distance(e2.first, e1);
|
|
add_point_edge_distance(e2.second, e1);
|
|
auto it = std::min_element(distances.begin(), distances.end(),
|
|
[](const DistAndPoints& item1, const DistAndPoints& item2) {
|
|
return item1.dist < item2.dist;
|
|
});
|
|
result.distance_infinite = std::make_optional(*it);
|
|
|
|
result.angle = angle_edge_edge(f1.get_edge(), f2.get_edge());
|
|
///////////////////////////////////////////////////////////////////////////
|
|
} else if (f2.get_type() == SurfaceFeatureType::Circle) {
|
|
const std::pair<Vec3d, Vec3d> e = f1.get_edge();
|
|
const auto &[center, radius, normal] = f2.get_circle();
|
|
const Vec3d e1e2 = (e.second - e.first);
|
|
const Vec3d e1e2_unit = e1e2.normalized();
|
|
|
|
std::vector<DistAndPoints> distances;
|
|
distances.emplace_back(*get_measurement(SurfaceFeature(e.first), f2).distance_strict);
|
|
distances.emplace_back(*get_measurement(SurfaceFeature(e.second), f2).distance_strict);
|
|
|
|
const Eigen::Hyperplane<double, 3> plane(e1e2_unit, center);
|
|
const Eigen::ParametrizedLine<double, 3> line = Eigen::ParametrizedLine<double, 3>::Through(e.first, e.second);
|
|
const Vec3d inter = line.intersectionPoint(plane);
|
|
const Vec3d e1inter = inter - e.first;
|
|
if (e1inter.dot(e1e2) >= 0.0 && e1inter.norm() < e1e2.norm()) distances.emplace_back(*get_measurement(SurfaceFeature(inter), f2).distance_strict);
|
|
|
|
auto it = std::min_element(distances.begin(), distances.end(), [](const DistAndPoints &item1, const DistAndPoints &item2) { return item1.dist < item2.dist; });
|
|
if (deal_circle_result == false) {
|
|
result.distance_infinite = std::make_optional(DistAndPoints{it->dist, it->from, it->to});
|
|
}
|
|
else{
|
|
const double dist = (it->from - center).norm();
|
|
result.distance_infinite = std::make_optional(DistAndPoints{dist, it->from, center});
|
|
}
|
|
///////////////////////////////////////////////////////////////////////////
|
|
} else if (f2.get_type() == SurfaceFeatureType::Plane) {
|
|
const auto [from, to] = f1.get_edge();
|
|
const auto [idx, normal, origin] = f2.get_plane();
|
|
|
|
const Vec3d edge_unit = (to - from).normalized();
|
|
if (are_perpendicular(edge_unit, normal)) {
|
|
std::vector<DistAndPoints> distances;
|
|
const Eigen::Hyperplane<double, 3> plane(normal, origin);
|
|
distances.push_back(DistAndPoints{ plane.absDistance(from), from, plane.projection(from) });
|
|
distances.push_back(DistAndPoints{ plane.absDistance(to), to, plane.projection(to) });
|
|
auto it = std::min_element(distances.begin(), distances.end(),
|
|
[](const DistAndPoints& item1, const DistAndPoints& item2) {
|
|
return item1.dist < item2.dist;
|
|
});
|
|
result.distance_infinite = std::make_optional(DistAndPoints{ it->dist, it->from, it->to });
|
|
}
|
|
else {
|
|
auto plane_features = f2.world_plane_features;
|
|
std::vector<DistAndPoints> distances;
|
|
for (const SurfaceFeature& sf : *plane_features) {
|
|
if (sf.get_type() == SurfaceFeatureType::Edge) {
|
|
const auto m = get_measurement(sf, f1);
|
|
if (!m.distance_infinite.has_value()) {
|
|
distances.clear();
|
|
break;
|
|
}
|
|
else
|
|
distances.push_back(*m.distance_infinite);
|
|
}
|
|
}
|
|
if (!distances.empty()) {
|
|
auto it = std::min_element(distances.begin(), distances.end(),
|
|
[](const DistAndPoints& item1, const DistAndPoints& item2) {
|
|
return item1.dist < item2.dist;
|
|
});
|
|
result.distance_infinite = std::make_optional(DistAndPoints{ it->dist, it->from, it->to });
|
|
}
|
|
}
|
|
result.angle = angle_edge_plane(f1.get_edge(), f2.get_plane());
|
|
}
|
|
///////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////
|
|
} else if (f1.get_type() == SurfaceFeatureType::Circle) {
|
|
if (f2.get_type() == SurfaceFeatureType::Circle) {
|
|
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)) {
|
|
// 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;
|
|
}
|
|
if (deal_circle_result == false) {
|
|
result.distance_infinite = std::make_optional(
|
|
DistAndPoints{std::sqrt(candidates[0].sqrDistance), candidates[0].circle0Closest, candidates[0].circle1Closest}); // TODO
|
|
} else {
|
|
const double dist = (c0 - c1).norm();
|
|
result.distance_strict = std::make_optional(DistAndPoints{dist, c0, c1});
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
} else if (f2.get_type() == SurfaceFeatureType::Plane) {
|
|
const auto [center, radius, normal1] = f1.get_circle();
|
|
const auto [idx2, normal2, origin2] = f2.get_plane();
|
|
|
|
const bool coplanar = are_parallel(normal1, normal2) && Eigen::Hyperplane<double, 3>(normal1, center).absDistance(origin2) < EPSILON;
|
|
if (!coplanar) {
|
|
auto plane_features = f2.world_plane_features;
|
|
std::vector<DistAndPoints> distances;
|
|
for (const SurfaceFeature& sf : *plane_features) {
|
|
if (sf.get_type() == SurfaceFeatureType::Edge) {
|
|
const auto m = get_measurement(sf, f1);
|
|
if (!m.distance_infinite.has_value()) {
|
|
distances.clear();
|
|
break;
|
|
}
|
|
else
|
|
distances.push_back(*m.distance_infinite);
|
|
}
|
|
}
|
|
if (!distances.empty()) {
|
|
auto it = std::min_element(distances.begin(), distances.end(),
|
|
[](const DistAndPoints& item1, const DistAndPoints& item2) {
|
|
return item1.dist < item2.dist;
|
|
});
|
|
result.distance_infinite = std::make_optional(DistAndPoints{ it->dist, it->from, it->to });
|
|
}
|
|
else {
|
|
const Eigen::Hyperplane<double, 3> plane(normal2, origin2);
|
|
result.distance_infinite = std::make_optional(DistAndPoints{plane.absDistance(center), center, plane.projection(center)});
|
|
}
|
|
}
|
|
else {
|
|
result.distance_strict = std::make_optional(DistAndPoints{0, center, origin2});
|
|
}
|
|
}
|
|
///////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////
|
|
} else if (f1.get_type() == SurfaceFeatureType::Plane) {
|
|
const auto [idx1, normal1, pt1] = f1.get_plane();
|
|
const auto [idx2, normal2, pt2] = f2.get_plane();
|
|
|
|
if (are_parallel(normal1, normal2)) {
|
|
// The planes are parallel, calculate distance.
|
|
const Eigen::Hyperplane<double, 3> plane(normal2, pt2);
|
|
result.distance_infinite = std::make_optional(DistAndPoints{ plane.absDistance(pt1), pt1, plane.projection(pt1) });
|
|
}
|
|
else
|
|
result.angle = angle_plane_plane(f1.get_plane(), f2.get_plane());
|
|
}
|
|
|
|
if (swap) {
|
|
auto swap_dist_and_points = [](DistAndPoints& dp) {
|
|
auto back = dp.to;
|
|
dp.to = dp.from;
|
|
dp.from = back;
|
|
};
|
|
if (result.distance_infinite.has_value()) {
|
|
swap_dist_and_points(*result.distance_infinite);
|
|
}
|
|
if (result.distance_strict.has_value()) {
|
|
swap_dist_and_points(*result.distance_strict);
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
bool can_set_xyz_distance(const SurfaceFeature &a, const SurfaceFeature &b) {
|
|
const bool swap = int(a.get_type()) > int(b.get_type());
|
|
const SurfaceFeature &f1 = swap ? b : a;
|
|
const SurfaceFeature &f2 = swap ? a : b;
|
|
if (f1.get_type() == SurfaceFeatureType::Point){
|
|
if (f2.get_type() == SurfaceFeatureType::Point) {
|
|
return true;
|
|
}
|
|
}
|
|
else if (f1.get_type() == SurfaceFeatureType::Circle) {
|
|
if (f2.get_type() == SurfaceFeatureType::Circle) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
AssemblyAction get_assembly_action(const SurfaceFeature& a, const SurfaceFeature& b)
|
|
{
|
|
AssemblyAction action;
|
|
const SurfaceFeature &f1 = a;
|
|
const SurfaceFeature &f2 = b;
|
|
if (f1.get_type() == SurfaceFeatureType::Plane) {
|
|
action.can_set_feature_1_reverse_rotation = true;
|
|
if (f2.get_type() == SurfaceFeatureType::Plane) {
|
|
const auto [idx1, normal1, pt1] = f1.get_plane();
|
|
const auto [idx2, normal2, pt2] = f2.get_plane();
|
|
action.can_set_to_center_coincidence = true;
|
|
action.can_set_feature_2_reverse_rotation = true;
|
|
if (are_parallel(normal1, normal2)) {
|
|
action.can_set_to_parallel = false;
|
|
action.has_parallel_distance = true;
|
|
action.can_around_center_of_faces = true;
|
|
Vec3d proj_pt2;
|
|
Measure::get_point_projection_to_plane(pt2, pt1, normal1, proj_pt2);
|
|
action.parallel_distance = (pt2 - proj_pt2).norm();
|
|
if ((pt2 - proj_pt2).dot(normal1) < 0) {
|
|
action.parallel_distance = -action.parallel_distance;
|
|
}
|
|
action.angle_radian = 0;
|
|
|
|
} else {
|
|
action.can_set_to_parallel = true;
|
|
action.has_parallel_distance = false;
|
|
action.can_around_center_of_faces = false;
|
|
action.parallel_distance = 0;
|
|
action.angle_radian = std::acos(std::clamp(normal2.dot(-normal1), -1.0, 1.0));
|
|
}
|
|
}
|
|
}
|
|
return action;
|
|
}
|
|
|
|
void SurfaceFeature::translate(const Vec3d& displacement) {
|
|
switch (get_type()) {
|
|
case Measure::SurfaceFeatureType::Point: {
|
|
m_pt1 = m_pt1 + displacement;
|
|
break;
|
|
}
|
|
case Measure::SurfaceFeatureType::Edge: {
|
|
m_pt1 = m_pt1 + displacement;
|
|
m_pt2 = m_pt2 + displacement;
|
|
if (m_pt3.has_value()) { //extra_point()
|
|
m_pt3 = *m_pt3 + displacement;
|
|
}
|
|
break;
|
|
}
|
|
case Measure::SurfaceFeatureType::Plane: {
|
|
//m_pt1 is normal;
|
|
m_pt2 = m_pt2 + displacement;
|
|
break;
|
|
}
|
|
case Measure::SurfaceFeatureType::Circle: {
|
|
m_pt1 = m_pt1 + displacement;
|
|
// m_pt2 is normal;
|
|
break;
|
|
}
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
void SurfaceFeature::translate(const Transform3d &tran)
|
|
{
|
|
switch (get_type()) {
|
|
case Measure::SurfaceFeatureType::Point: {
|
|
m_pt1 = tran * m_pt1;
|
|
break;
|
|
}
|
|
case Measure::SurfaceFeatureType::Edge: {
|
|
m_pt1 = tran * m_pt1;
|
|
m_pt2 = tran * m_pt2;
|
|
if (m_pt3.has_value()) { // extra_point()
|
|
m_pt3 = tran * *m_pt3;
|
|
}
|
|
break;
|
|
}
|
|
case Measure::SurfaceFeatureType::Plane: {
|
|
// m_pt1 is normal;
|
|
Vec3d temp_pt1 = m_pt2 + m_pt1;
|
|
temp_pt1 = tran * temp_pt1;
|
|
m_pt2 = tran * m_pt2;
|
|
m_pt1 = (temp_pt1 - m_pt2).normalized();
|
|
break;
|
|
}
|
|
case Measure::SurfaceFeatureType::Circle: {
|
|
// m_pt1 is center;
|
|
// m_pt2 is normal;
|
|
auto local_normal = m_pt2;
|
|
auto local_center = m_pt1;
|
|
Vec3d temp_pt2 = local_normal + local_center;
|
|
temp_pt2 = tran * temp_pt2;
|
|
m_pt1 = tran * m_pt1;
|
|
auto world_center = m_pt1;
|
|
m_pt2 = (temp_pt2 - m_pt1).normalized();
|
|
|
|
auto calc_world_radius = [&local_center, &local_normal, &tran, &world_center](const Vec3d &pt, double &value) {
|
|
Vec3d intersection_pt;
|
|
get_point_projection_to_plane(pt, local_center, local_normal, intersection_pt);
|
|
Vec3d local_radius_pt = (intersection_pt - local_center).normalized() * value + local_center;
|
|
Vec3d radius_pt = tran * local_radius_pt;
|
|
value = (radius_pt - world_center).norm();
|
|
};
|
|
//m_value is radius
|
|
auto new_pt = get_one_point_in_plane(local_center, local_normal);
|
|
calc_world_radius(new_pt, m_value);
|
|
break;
|
|
}
|
|
default: break;
|
|
}
|
|
}
|
|
}//namespace Measure
|
|
} // namespace Slic3r
|
|
|