public_mirrors/OrcaSlicer-bambulab
1
0
Fork 0
mirror of https://github.com/FULU-Foundation/OrcaSlicer-bambulab.git synced 2026-07-11 08:04:25 +00:00
Code Issues Activity
Files
8ca7e56d0f641c89ca82306f6616d6f9d926aa12
OrcaSlicer-bambulab/src/libslic3r/SLA/SLASupportTreeBuildsteps.cpp
T
tamasmeszaros 8ca7e56d0f Fix tests on all platforms 
Try to link tests on Mac.


Fix inaccurate pad brim size


fix build on mac (attempt 2)


Fixes for support tree faults and race conditions in release mode.


Fix crashing test executable on gcc 4.9


fix warning on msvc
2019-10-02 14:55:16 +02:00

1394 lines
51 KiB
C++
Raw Blame History

#include "SLASupportTreeBuildsteps.hpp"
#include <libnest2d/optimizers/nlopt/genetic.hpp>
#include <libnest2d/optimizers/nlopt/subplex.hpp>
#include <boost/log/trivial.hpp>
namespace Slic3r {
namespace sla {
SupportTreeBuildsteps::SupportTreeBuildsteps(SupportTreeBuilder & builder,
const SupportableMesh &sm)
: m_cfg(sm.cfg)
, m_mesh(sm.emesh)
, m_support_pts(sm.pts)
, m_support_nmls(sm.pts.size(), 3)
, m_builder(builder)
, m_points(sm.pts.size(), 3)
, m_thr(builder.ctl().cancelfn)
{
// Prepare the support points in Eigen/IGL format as well, we will use
// it mostly in this form.
long i = 0;
for (const SupportPoint &sp : m_support_pts) {
m_points.row(i)(X) = double(sp.pos(X));
m_points.row(i)(Y) = double(sp.pos(Y));
m_points.row(i)(Z) = double(sp.pos(Z));
++i;
}
}
bool SupportTreeBuildsteps::execute(SupportTreeBuilder & builder,
const SupportableMesh &sm)
{
if(sm.pts.empty()) return false;
SupportTreeBuildsteps alg(builder, sm);
// Let's define the individual steps of the processing. We can experiment
// later with the ordering and the dependencies between them.
enum Steps {
BEGIN,
FILTER,
PINHEADS,
CLASSIFY,
ROUTING_GROUND,
ROUTING_NONGROUND,
CASCADE_PILLARS,
HEADLESS,
MERGE_RESULT,
DONE,
ABORT,
NUM_STEPS
//...
};
// Collect the algorithm steps into a nice sequence
std::array<std::function<void()>, NUM_STEPS> program = {
[] () {
// Begin...
// Potentially clear up the shared data (not needed for now)
},
std::bind(&SupportTreeBuildsteps::filter, &alg),
std::bind(&SupportTreeBuildsteps::add_pinheads, &alg),
std::bind(&SupportTreeBuildsteps::classify, &alg),
std::bind(&SupportTreeBuildsteps::routing_to_ground, &alg),
std::bind(&SupportTreeBuildsteps::routing_to_model, &alg),
std::bind(&SupportTreeBuildsteps::interconnect_pillars, &alg),
std::bind(&SupportTreeBuildsteps::routing_headless, &alg),
std::bind(&SupportTreeBuildsteps::merge_result, &alg),
[] () {
// Done
},
[] () {
// Abort
}
};
Steps pc = BEGIN;
if(sm.cfg.ground_facing_only) {
program[ROUTING_NONGROUND] = []() {
BOOST_LOG_TRIVIAL(info)
<< "Skipping model-facing supports as requested.";
};
program[HEADLESS] = []() {
BOOST_LOG_TRIVIAL(info) << "Skipping headless stick generation as"
" requested.";
};
}
// Let's define a simple automaton that will run our program.
auto progress = [&builder, &pc] () {
static const std::array<std::string, NUM_STEPS> stepstr {
"Starting",
"Filtering",
"Generate pinheads",
"Classification",
"Routing to ground",
"Routing supports to model surface",
"Interconnecting pillars",
"Processing small holes",
"Merging support mesh",
"Done",
"Abort"
};
static const std::array<unsigned, NUM_STEPS> stepstate {
0,
10,
30,
50,
60,
70,
80,
85,
99,
100,
0
};
if(builder.ctl().stopcondition()) pc = ABORT;
switch(pc) {
case BEGIN: pc = FILTER; break;
case FILTER: pc = PINHEADS; break;
case PINHEADS: pc = CLASSIFY; break;
case CLASSIFY: pc = ROUTING_GROUND; break;
case ROUTING_GROUND: pc = ROUTING_NONGROUND; break;
case ROUTING_NONGROUND: pc = CASCADE_PILLARS; break;
case CASCADE_PILLARS: pc = HEADLESS; break;
case HEADLESS: pc = MERGE_RESULT; break;
case MERGE_RESULT: pc = DONE; break;
case DONE:
case ABORT: break;
default: ;
}
builder.ctl().statuscb(stepstate[pc], stepstr[pc]);
};
// Just here we run the computation...
while(pc < DONE) {
progress();
program[pc]();
}
return pc == ABORT;
}
EigenMesh3D::hit_result SupportTreeBuildsteps::pinhead_mesh_intersect(
const Vec3d &s, const Vec3d &dir, double r_pin, double r_back, double width)
{
static const size_t SAMPLES = 8;
// method based on:
// https://math.stackexchange.com/questions/73237/parametric-equation-of-a-circle-in-3d-space
// We will shoot multiple rays from the head pinpoint in the direction
// of the pinhead robe (side) surface. The result will be the smallest
// hit distance.
// Move away slightly from the touching point to avoid raycasting on the
// inner surface of the mesh.
Vec3d v = dir; // Our direction (axis)
Vec3d c = s + width * dir;
const double& sd = m_cfg.safety_distance_mm;
// Two vectors that will be perpendicular to each other and to the
// axis. Values for a(X) and a(Y) are now arbitrary, a(Z) is just a
// placeholder.
Vec3d a(0, 1, 0), b;
// The portions of the circle (the head-back circle) for which we will
// shoot rays.
std::array<double, SAMPLES> phis;
for(size_t i = 0; i < phis.size(); ++i) phis[i] = i*2*PI/phis.size();
auto& m = m_mesh;
using HitResult = EigenMesh3D::hit_result;
// Hit results
std::array<HitResult, SAMPLES> hits;
// We have to address the case when the direction vector v (same as
// dir) is coincident with one of the world axes. In this case two of
// its components will be completely zero and one is 1.0. Our method
// becomes dangerous here due to division with zero. Instead, vector
// 'a' can be an element-wise rotated version of 'v'
auto chk1 = [] (double val) {
return std::abs(std::abs(val) - 1) < 1e-20;
};
if(chk1(v(X)) || chk1(v(Y)) || chk1(v(Z))) {
a = {v(Z), v(X), v(Y)};
b = {v(Y), v(Z), v(X)};
}
else {
a(Z) = -(v(Y)*a(Y)) / v(Z); a.normalize();
b = a.cross(v);
}
// Now a and b vectors are perpendicular to v and to each other.
// Together they define the plane where we have to iterate with the
// given angles in the 'phis' vector
ccr_par::enumerate(
phis.begin(), phis.end(),
[&hits, &m, sd, r_pin, r_back, s, a, b, c](double phi, size_t i) {
double sinphi = std::sin(phi);
double cosphi = std::cos(phi);
// Let's have a safety coefficient for the radiuses.
double rpscos = (sd + r_pin) * cosphi;
double rpssin = (sd + r_pin) * sinphi;
double rpbcos = (sd + r_back) * cosphi;
double rpbsin = (sd + r_back) * sinphi;
// Point on the circle on the pin sphere
Vec3d ps(s(X) + rpscos * a(X) + rpssin * b(X),
s(Y) + rpscos * a(Y) + rpssin * b(Y),
s(Z) + rpscos * a(Z) + rpssin * b(Z));
// Point ps is not on mesh but can be inside or
// outside as well. This would cause many problems
// with ray-casting. To detect the position we will
// use the ray-casting result (which has an is_inside
// predicate).
// This is the point on the circle on the back sphere
Vec3d p(c(X) + rpbcos * a(X) + rpbsin * b(X),
c(Y) + rpbcos * a(Y) + rpbsin * b(Y),
c(Z) + rpbcos * a(Z) + rpbsin * b(Z));
Vec3d n = (p - ps).normalized();
auto q = m.query_ray_hit(ps + sd * n, n);
if (q.is_inside()) { // the hit is inside the model
if (q.distance() > r_pin + sd) {
// If we are inside the model and the hit
// distance is bigger than our pin circle
// diameter, it probably indicates that the
// support point was already inside the
// model, or there is really no space
// around the point. We will assign a zero
// hit distance to these cases which will
// enforce the function return value to be
// an invalid ray with zero hit distance.
// (see min_element at the end)
hits[i] = HitResult(0.0);
} else {
// re-cast the ray from the outside of the
// object. The starting point has an offset
// of 2*safety_distance because the
// original ray has also had an offset
auto q2 = m.query_ray_hit(
ps + (q.distance() + 2 * sd) * n, n);
hits[i] = q2;
}
} else
hits[i] = q;
});
auto mit = std::min_element(hits.begin(), hits.end());
return *mit;
}
EigenMesh3D::hit_result SupportTreeBuildsteps::bridge_mesh_intersect(
const Vec3d &s, const Vec3d &dir, double r, bool ins_check)
{
static const size_t SAMPLES = 8;
// helper vector calculations
Vec3d a(0, 1, 0), b;
const double& sd = m_cfg.safety_distance_mm;
// INFO: for explanation of the method used here, see the previous
// method's comments.
auto chk1 = [] (double val) {
return std::abs(std::abs(val) - 1) < 1e-20;
};
if(chk1(dir(X)) || chk1(dir(Y)) || chk1(dir(Z))) {
a = {dir(Z), dir(X), dir(Y)};
b = {dir(Y), dir(Z), dir(X)};
}
else {
a(Z) = -(dir(Y)*a(Y)) / dir(Z); a.normalize();
b = a.cross(dir);
}
// circle portions
std::array<double, SAMPLES> phis;
for(size_t i = 0; i < phis.size(); ++i) phis[i] = i*2*PI/phis.size();
auto& m = m_mesh;
using HitResult = EigenMesh3D::hit_result;
// Hit results
std::array<HitResult, SAMPLES> hits;
ccr_par::enumerate(
phis.begin(), phis.end(),
[&m, a, b, sd, dir, r, s, ins_check, &hits] (double phi, size_t i) {
double sinphi = std::sin(phi);
double cosphi = std::cos(phi);
// Let's have a safety coefficient for the radiuses.
double rcos = (sd + r) * cosphi;
double rsin = (sd + r) * sinphi;
// Point on the circle on the pin sphere
Vec3d p (s(X) + rcos * a(X) + rsin * b(X),
s(Y) + rcos * a(Y) + rsin * b(Y),
s(Z) + rcos * a(Z) + rsin * b(Z));
auto hr = m.query_ray_hit(p + sd*dir, dir);
if(ins_check && hr.is_inside()) {
if(hr.distance() > 2 * r + sd) hits[i] = HitResult(0.0);
else {
// re-cast the ray from the outside of the object
auto hr2 =
m.query_ray_hit(p + (hr.distance() + 2*sd)*dir, dir);
hits[i] = hr2;
}
} else hits[i] = hr;
});
auto mit = std::min_element(hits.begin(), hits.end());
return *mit;
}
bool SupportTreeBuildsteps::interconnect(const Pillar &pillar,
const Pillar &nextpillar)
{
// We need to get the starting point of the zig-zag pattern. We have to
// be aware that the two head junctions are at different heights. We
// may start from the lowest junction and call it a day but this
// strategy would leave unconnected a lot of pillar duos where the
// shorter pillar is too short to start a new bridge but the taller
// pillar could still be bridged with the shorter one.
bool was_connected = false;
Vec3d supper = pillar.startpoint();
Vec3d slower = nextpillar.startpoint();
Vec3d eupper = pillar.endpoint();
Vec3d elower = nextpillar.endpoint();
double zmin = m_builder.ground_level + m_cfg.base_height_mm;
eupper(Z) = std::max(eupper(Z), zmin);
elower(Z) = std::max(elower(Z), zmin);
// The usable length of both pillars should be positive
if(slower(Z) - elower(Z) < 0) return false;
if(supper(Z) - eupper(Z) < 0) return false;
double pillar_dist = distance(Vec2d{slower(X), slower(Y)},
Vec2d{supper(X), supper(Y)});
double bridge_distance = pillar_dist / std::cos(-m_cfg.bridge_slope);
double zstep = pillar_dist * std::tan(-m_cfg.bridge_slope);
if(pillar_dist < 2 * m_cfg.head_back_radius_mm ||
pillar_dist > m_cfg.max_pillar_link_distance_mm) return false;
if(supper(Z) < slower(Z)) supper.swap(slower);
if(eupper(Z) < elower(Z)) eupper.swap(elower);
double startz = 0, endz = 0;
startz = slower(Z) - zstep < supper(Z) ? slower(Z) - zstep : slower(Z);
endz = eupper(Z) + zstep > elower(Z) ? eupper(Z) + zstep : eupper(Z);
if(slower(Z) - eupper(Z) < std::abs(zstep)) {
// no space for even one cross
// Get max available space
startz = std::min(supper(Z), slower(Z) - zstep);
endz = std::max(eupper(Z) + zstep, elower(Z));
// Align to center
double available_dist = (startz - endz);
double rounds = std::floor(available_dist / std::abs(zstep));
startz -= 0.5 * (available_dist - rounds * std::abs(zstep));
}
auto pcm = m_cfg.pillar_connection_mode;
bool docrosses =
pcm == PillarConnectionMode::cross ||
(pcm == PillarConnectionMode::dynamic &&
pillar_dist > 2*m_cfg.base_radius_mm);
// 'sj' means starting junction, 'ej' is the end junction of a bridge.
// They will be swapped in every iteration thus the zig-zag pattern.
// According to a config parameter, a second bridge may be added which
// results in a cross connection between the pillars.
Vec3d sj = supper, ej = slower; sj(Z) = startz; ej(Z) = sj(Z) + zstep;
// TODO: This is a workaround to not have a faulty last bridge
while(ej(Z) >= eupper(Z) /*endz*/) {
if(bridge_mesh_intersect(sj, dirv(sj, ej), pillar.r) >= bridge_distance)
{
m_builder.add_crossbridge(sj, ej, pillar.r);
was_connected = true;
}
// double bridging: (crosses)
if(docrosses) {
Vec3d sjback(ej(X), ej(Y), sj(Z));
Vec3d ejback(sj(X), sj(Y), ej(Z));
if (sjback(Z) <= slower(Z) && ejback(Z) >= eupper(Z) &&
bridge_mesh_intersect(sjback, dirv(sjback, ejback),
pillar.r) >= bridge_distance) {
// need to check collision for the cross stick
m_builder.add_crossbridge(sjback, ejback, pillar.r);
was_connected = true;
}
}
sj.swap(ej);
ej(Z) = sj(Z) + zstep;
}
return was_connected;
}
bool SupportTreeBuildsteps::connect_to_nearpillar(const Head &head,
long nearpillar_id)
{
auto nearpillar = [this, nearpillar_id]() {
return m_builder.pillar(nearpillar_id);
};
if (m_builder.bridgecount(nearpillar()) > m_cfg.max_bridges_on_pillar)
return false;
Vec3d headjp = head.junction_point();
Vec3d nearjp_u = nearpillar().startpoint();
Vec3d nearjp_l = nearpillar().endpoint();
double r = head.r_back_mm;
double d2d = distance(to_2d(headjp), to_2d(nearjp_u));
double d3d = distance(headjp, nearjp_u);
double hdiff = nearjp_u(Z) - headjp(Z);
double slope = std::atan2(hdiff, d2d);
Vec3d bridgestart = headjp;
Vec3d bridgeend = nearjp_u;
double max_len = m_cfg.max_bridge_length_mm;
double max_slope = m_cfg.bridge_slope;
double zdiff = 0.0;
// check the default situation if feasible for a bridge
if(d3d > max_len || slope > -max_slope) {
// not feasible to connect the two head junctions. We have to search
// for a suitable touch point.
double Zdown = headjp(Z) + d2d * std::tan(-max_slope);
Vec3d touchjp = bridgeend; touchjp(Z) = Zdown;
double D = distance(headjp, touchjp);
zdiff = Zdown - nearjp_u(Z);
if(zdiff > 0) {
Zdown -= zdiff;
bridgestart(Z) -= zdiff;
touchjp(Z) = Zdown;
double t = bridge_mesh_intersect(headjp, {0,0,-1}, r);
// We can't insert a pillar under the source head to connect
// with the nearby pillar's starting junction
if(t < zdiff) return false;
}
if(Zdown <= nearjp_u(Z) && Zdown >= nearjp_l(Z) && D < max_len)
bridgeend(Z) = Zdown;
else
return false;
}
// There will be a minimum distance from the ground where the
// bridge is allowed to connect. This is an empiric value.
double minz = m_builder.ground_level + 2 * m_cfg.head_width_mm;
if(bridgeend(Z) < minz) return false;
double t = bridge_mesh_intersect(bridgestart,
dirv(bridgestart, bridgeend), r);
// Cannot insert the bridge. (further search might not worth the hassle)
if(t < distance(bridgestart, bridgeend)) return false;
std::lock_guard<ccr::BlockingMutex> lk(m_bridge_mutex);
if (m_builder.bridgecount(nearpillar()) < m_cfg.max_bridges_on_pillar) {
// A partial pillar is needed under the starting head.
if(zdiff > 0) {
m_builder.add_pillar(unsigned(head.id), bridgestart, r);
m_builder.add_junction(bridgestart, r);
}
auto &br = m_builder.add_bridge(bridgestart, bridgeend, r);
m_builder.increment_bridges(nearpillar());
if (head.pillar_id == ID_UNSET)
m_builder.head(unsigned(head.id)).bridge_id = br.id;
} else return false;
return true;
}
bool SupportTreeBuildsteps::search_pillar_and_connect(const Head &head)
{
PointIndex spindex = m_pillar_index.guarded_clone();
long nearest_id = ID_UNSET;
Vec3d querypoint = head.junction_point();
while(nearest_id < 0 && !spindex.empty()) { m_thr();
// loop until a suitable head is not found
// if there is a pillar closer than the cluster center
// (this may happen as the clustering is not perfect)
// than we will bridge to this closer pillar
Vec3d qp(querypoint(X), querypoint(Y), m_builder.ground_level);
auto qres = spindex.nearest(qp, 1);
if(qres.empty()) break;
auto ne = qres.front();
nearest_id = ne.second;
if(nearest_id >= 0) {
auto nearpillarID = unsigned(nearest_id);
if(nearpillarID < m_builder.pillarcount()) {
if(!connect_to_nearpillar(head, nearpillarID)) {
nearest_id = ID_UNSET; // continue searching
spindex.remove(ne); // without the current pillar
}
}
}
}
return nearest_id >= 0;
}
void SupportTreeBuildsteps::create_ground_pillar(const Vec3d &jp,
const Vec3d &sourcedir,
double radius,
long head_id)
{
// People were killed for this number (seriously)
static const double SQR2 = std::sqrt(2.0);
static const Vec3d DOWN = {0.0, 0.0, -1.0};
double gndlvl = m_builder.ground_level;
Vec3d endp = {jp(X), jp(Y), gndlvl};
double sd = m_cfg.pillar_base_safety_distance_mm;
long pillar_id = ID_UNSET;
double min_dist = sd + m_cfg.base_radius_mm + EPSILON;
double dist = 0;
bool can_add_base = true;
bool normal_mode = true;
if (m_cfg.object_elevation_mm < EPSILON
&& (dist = std::sqrt(m_mesh.squared_distance(endp))) < min_dist) {
// Get the distance from the mesh. This can be later optimized
// to get the distance in 2D plane because we are dealing with
// the ground level only.
normal_mode = false;
double mind = min_dist - dist;
double azimuth = std::atan2(sourcedir(Y), sourcedir(X));
double sinpolar = std::sin(PI - m_cfg.bridge_slope);
double cospolar = std::cos(PI - m_cfg.bridge_slope);
double cosazm = std::cos(azimuth);
double sinazm = std::sin(azimuth);
auto dir = Vec3d(cosazm * sinpolar, sinazm * sinpolar, cospolar)
.normalized();
using namespace libnest2d::opt;
StopCriteria scr;
scr.stop_score = min_dist;
SubplexOptimizer solver(scr);
auto result = solver.optimize_max(
[this, dir, jp, gndlvl](double mv) {
Vec3d endpt = jp + SQR2 * mv * dir;
endpt(Z) = gndlvl;
return std::sqrt(m_mesh.squared_distance(endpt));
},
initvals(mind), bound(0.0, 2 * min_dist));
mind = std::get<0>(result.optimum);
endp = jp + SQR2 * mind * dir;
Vec3d pgnd = {endp(X), endp(Y), gndlvl};
can_add_base = result.score > min_dist;
double gnd_offs = m_mesh.ground_level_offset();
auto abort_in_shame =
[gnd_offs, &normal_mode, &can_add_base, &endp, jp, gndlvl]()
{
normal_mode = true;
can_add_base = false; // Nothing left to do, hope for the best
endp = {jp(X), jp(Y), gndlvl - gnd_offs };
};
// We have to check if the bridge is feasible.
if (bridge_mesh_intersect(jp, dir, radius) < (endp - jp).norm())
abort_in_shame();
else {
// If the new endpoint is below ground, do not make a pillar
if (endp(Z) < gndlvl)
endp = endp - SQR2 * (gndlvl - endp(Z)) * dir; // back off
else {
auto hit = bridge_mesh_intersect(endp, DOWN, radius);
if (!std::isinf(hit.distance())) abort_in_shame();
Pillar &plr = m_builder.add_pillar(endp, pgnd, radius);
if (can_add_base)
plr.add_base(m_cfg.base_height_mm,
m_cfg.base_radius_mm);
pillar_id = plr.id;
}
m_builder.add_bridge(jp, endp, radius);
m_builder.add_junction(endp, radius);
// Add a degenerated pillar and the bridge.
// The degenerate pillar will have zero length and it will
// prevent from queries of head_pillar() to have non-existing
// pillar when the head should have one.
if (head_id >= 0)
m_builder.add_pillar(unsigned(head_id), jp, radius);
}
}
if (normal_mode) {
Pillar &plr = head_id >= 0
? m_builder.add_pillar(unsigned(head_id), endp, radius)
: m_builder.add_pillar(jp, endp, radius);
if (can_add_base)
plr.add_base(m_cfg.base_height_mm, m_cfg.base_radius_mm);
pillar_id = plr.id;
}
if(pillar_id >= 0) // Save the pillar endpoint in the spatial index
m_pillar_index.guarded_insert(endp, unsigned(pillar_id));
}
void SupportTreeBuildsteps::filter()
{
// Get the points that are too close to each other and keep only the
// first one
auto aliases = cluster(m_points, D_SP, 2);
PtIndices filtered_indices;
filtered_indices.reserve(aliases.size());
m_iheads.reserve(aliases.size());
m_iheadless.reserve(aliases.size());
for(auto& a : aliases) {
// Here we keep only the front point of the cluster.
filtered_indices.emplace_back(a.front());
}
// calculate the normals to the triangles for filtered points
auto nmls = sla::normals(m_points, m_mesh, m_cfg.head_front_radius_mm,
m_thr, filtered_indices);
// Not all of the support points have to be a valid position for
// support creation. The angle may be inappropriate or there may
// not be enough space for the pinhead. Filtering is applied for
// these reasons.
using libnest2d::opt::bound;
using libnest2d::opt::initvals;
using libnest2d::opt::GeneticOptimizer;
using libnest2d::opt::StopCriteria;
ccr::SpinningMutex mutex;
auto addfn = [&mutex](PtIndices &container, unsigned val) {
std::lock_guard<ccr::SpinningMutex> lk(mutex);
container.emplace_back(val);
};
auto filterfn = [this, &nmls, addfn](unsigned fidx, size_t i) {
m_thr();
auto n = nmls.row(Eigen::Index(i));
// for all normals we generate the spherical coordinates and
// saturate the polar angle to 45 degrees from the bottom then
// convert back to standard coordinates to get the new normal.
// Then we just create a quaternion from the two normals
// (Quaternion::FromTwoVectors) and apply the rotation to the
// arrow head.
double z = n(2);
double r = 1.0; // for normalized vector
double polar = std::acos(z / r);
double azimuth = std::atan2(n(1), n(0));
// skip if the tilt is not sane
if(polar >= PI - m_cfg.normal_cutoff_angle) {
// We saturate the polar angle to 3pi/4
polar = std::max(polar, 3*PI / 4);
// save the head (pinpoint) position
Vec3d hp = m_points.row(fidx);
double w = m_cfg.head_width_mm +
m_cfg.head_back_radius_mm +
2*m_cfg.head_front_radius_mm;
double pin_r = double(m_support_pts[fidx].head_front_radius);
// Reassemble the now corrected normal
auto nn = Vec3d(std::cos(azimuth) * std::sin(polar),
std::sin(azimuth) * std::sin(polar),
std::cos(polar)).normalized();
// check available distance
EigenMesh3D::hit_result t
= pinhead_mesh_intersect(hp, // touching point
nn, // normal
pin_r,
m_cfg.head_back_radius_mm,
w);
if(t.distance() <= w) {
// Let's try to optimize this angle, there might be a
// viable normal that doesn't collide with the model
// geometry and its very close to the default.
StopCriteria stc;
stc.max_iterations = m_cfg.optimizer_max_iterations;
stc.relative_score_difference = m_cfg.optimizer_rel_score_diff;
stc.stop_score = w; // space greater than w is enough
GeneticOptimizer solver(stc);
solver.seed(0); // we want deterministic behavior
auto oresult = solver.optimize_max(
[this, pin_r, w, hp](double plr, double azm)
{
auto dir = Vec3d(std::cos(azm) * std::sin(plr),
std::sin(azm) * std::sin(plr),
std::cos(plr)).normalized();
double score = pinhead_mesh_intersect(
hp, dir, pin_r, m_cfg.head_back_radius_mm, w);
return score;
},
initvals(polar, azimuth), // start with what we have
bound(3 * PI / 4,
PI), // Must not exceed the tilt limit
bound(-PI, PI) // azimuth can be a full search
);
if(oresult.score > w) {
polar = std::get<0>(oresult.optimum);
azimuth = std::get<1>(oresult.optimum);
nn = Vec3d(std::cos(azimuth) * std::sin(polar),
std::sin(azimuth) * std::sin(polar),
std::cos(polar)).normalized();
t = oresult.score;
}
}
// save the verified and corrected normal
m_support_nmls.row(fidx) = nn;
if (t.distance() > w) {
// Check distance from ground, we might have zero elevation.
if (hp(Z) + w * nn(Z) < m_builder.ground_level) {
addfn(m_iheadless, fidx);
} else {
// mark the point for needing a head.
addfn(m_iheads, fidx);
}
} else if (polar >= 3 * PI / 4) {
// Headless supports do not tilt like the headed ones
// so the normal should point almost to the ground.
addfn(m_iheadless, fidx);
}
}
};
ccr::enumerate(filtered_indices.begin(), filtered_indices.end(), filterfn);
m_thr();
}
void SupportTreeBuildsteps::add_pinheads()
{
for (unsigned i : m_iheads) {
m_thr();
m_builder.add_head(
i,
m_cfg.head_back_radius_mm,
m_support_pts[i].head_front_radius,
m_cfg.head_width_mm,
m_cfg.head_penetration_mm,
m_support_nmls.row(i), // dir
m_support_pts[i].pos.cast<double>() // displacement
);
}
}
void SupportTreeBuildsteps::classify()
{
// We should first get the heads that reach the ground directly
PtIndices ground_head_indices;
ground_head_indices.reserve(m_iheads.size());
m_iheads_onmodel.reserve(m_iheads.size());
// First we decide which heads reach the ground and can be full
// pillars and which shall be connected to the model surface (or
// search a suitable path around the surface that leads to the
// ground -- TODO)
for(unsigned i : m_iheads) {
m_thr();
auto& head = m_builder.head(i);
Vec3d n(0, 0, -1);
double r = head.r_back_mm;
Vec3d headjp = head.junction_point();
// collision check
auto hit = bridge_mesh_intersect(headjp, n, r);
if(std::isinf(hit.distance())) ground_head_indices.emplace_back(i);
else if(m_cfg.ground_facing_only) head.invalidate();
else m_iheads_onmodel.emplace_back(std::make_pair(i, hit));
}
// We want to search for clusters of points that are far enough
// from each other in the XY plane to not cross their pillar bases
// These clusters of support points will join in one pillar,
// possibly in their centroid support point.
auto pointfn = [this](unsigned i) {
return m_builder.head(i).junction_point();
};
auto predicate = [this](const PointIndexEl &e1,
const PointIndexEl &e2) {
double d2d = distance(to_2d(e1.first), to_2d(e2.first));
double d3d = distance(e1.first, e2.first);
return d2d < 2 * m_cfg.base_radius_mm
&& d3d < m_cfg.max_bridge_length_mm;
};
m_pillar_clusters = cluster(ground_head_indices,
pointfn,
predicate,
m_cfg.max_bridges_on_pillar);
}
void SupportTreeBuildsteps::routing_to_ground()
{
const double pradius = m_cfg.head_back_radius_mm;
ClusterEl cl_centroids;
cl_centroids.reserve(m_pillar_clusters.size());
for (auto &cl : m_pillar_clusters) {
m_thr();
// place all the centroid head positions into the index. We
// will query for alternative pillar positions. If a sidehead
// cannot connect to the cluster centroid, we have to search
// for another head with a full pillar. Also when there are two
// elements in the cluster, the centroid is arbitrary and the
// sidehead is allowed to connect to a nearby pillar to
// increase structural stability.
if (cl.empty()) continue;
// get the current cluster centroid
auto & thr = m_thr;
const auto &points = m_points;
long lcid = cluster_centroid(
cl, [&points](size_t idx) { return points.row(long(idx)); },
[thr](const Vec3d &p1, const Vec3d &p2) {
thr();
return distance(Vec2d(p1(X), p1(Y)), Vec2d(p2(X), p2(Y)));
});
assert(lcid >= 0);
unsigned hid = cl[size_t(lcid)]; // Head ID
cl_centroids.emplace_back(hid);
Head &h = m_builder.head(hid);
h.transform();
create_ground_pillar(h.junction_point(), h.dir, h.r_back_mm, h.id);
}
// now we will go through the clusters ones again and connect the
// sidepoints with the cluster centroid (which is a ground pillar)
// or a nearby pillar if the centroid is unreachable.
size_t ci = 0;
for (auto cl : m_pillar_clusters) {
m_thr();
auto cidx = cl_centroids[ci++];
// TODO: don't consider the cluster centroid but calculate a
// central position where the pillar can be placed. this way
// the weight is distributed more effectively on the pillar.
auto centerpillarID = m_builder.head_pillar(cidx).id;
for (auto c : cl) {
m_thr();
if (c == cidx) continue;
auto &sidehead = m_builder.head(c);
sidehead.transform();
if (!connect_to_nearpillar(sidehead, centerpillarID) &&
!search_pillar_and_connect(sidehead)) {
Vec3d pstart = sidehead.junction_point();
// Vec3d pend = Vec3d{pstart(X), pstart(Y), gndlvl};
// Could not find a pillar, create one
create_ground_pillar(pstart, sidehead.dir, pradius, sidehead.id);
}
}
}
}
void SupportTreeBuildsteps::routing_to_model()
{
// We need to check if there is an easy way out to the bed surface.
// If it can be routed there with a bridge shorter than
// min_bridge_distance.
// First we want to index the available pillars. The best is to connect
// these points to the available pillars
auto routedown = [this](Head& head, const Vec3d& dir, double dist)
{
head.transform();
Vec3d hjp = head.junction_point();
Vec3d endp = hjp + dist * dir;
m_builder.add_bridge(hjp, endp, head.r_back_mm);
m_builder.add_junction(endp, head.r_back_mm);
this->create_ground_pillar(endp, dir, head.r_back_mm, head.id);
};
std::vector<unsigned> modelpillars;
ccr::SpinningMutex mutex;
auto onmodelfn =
[this, routedown, &modelpillars, &mutex]
(const std::pair<unsigned, EigenMesh3D::hit_result> &el, size_t)
{
m_thr();
unsigned idx = el.first;
EigenMesh3D::hit_result hit = el.second;
auto& head = m_builder.head(idx);
Vec3d hjp = head.junction_point();
// /////////////////////////////////////////////////////////////////
// Search nearby pillar
// /////////////////////////////////////////////////////////////////
if(search_pillar_and_connect(head)) { head.transform(); return; }
// /////////////////////////////////////////////////////////////////
// Try straight path
// /////////////////////////////////////////////////////////////////
// Cannot connect to nearby pillar. We will try to search for
// a route to the ground.
double t = bridge_mesh_intersect(hjp, head.dir, head.r_back_mm);
double d = 0, tdown = 0;
Vec3d dirdown(0.0, 0.0, -1.0);
t = std::min(t, m_cfg.max_bridge_length_mm);
while(d < t && !std::isinf(tdown = bridge_mesh_intersect(
hjp + d*head.dir,
dirdown, head.r_back_mm))) {
d += head.r_back_mm;
}
if(std::isinf(tdown)) { // we heave found a route to the ground
routedown(head, head.dir, d); return;
}
// /////////////////////////////////////////////////////////////////
// Optimize bridge direction
// /////////////////////////////////////////////////////////////////
// Straight path failed so we will try to search for a suitable
// direction out of the cavity.
// Get the spherical representation of the normal. its easier to
// work with.
double z = head.dir(Z);
double r = 1.0; // for normalized vector
double polar = std::acos(z / r);
double azimuth = std::atan2(head.dir(Y), head.dir(X));
using libnest2d::opt::bound;
using libnest2d::opt::initvals;
using libnest2d::opt::GeneticOptimizer;
using libnest2d::opt::StopCriteria;
StopCriteria stc;
stc.max_iterations = m_cfg.optimizer_max_iterations;
stc.relative_score_difference = m_cfg.optimizer_rel_score_diff;
stc.stop_score = 1e6;
GeneticOptimizer solver(stc);
solver.seed(0); // we want deterministic behavior
double r_back = head.r_back_mm;
auto oresult = solver.optimize_max(
[this, hjp, r_back](double plr, double azm)
{
Vec3d n = Vec3d(std::cos(azm) * std::sin(plr),
std::sin(azm) * std::sin(plr),
std::cos(plr)).normalized();
return bridge_mesh_intersect(hjp, n, r_back);
},
initvals(polar, azimuth), // let's start with what we have
bound(3*PI/4, PI), // Must not exceed the slope limit
bound(-PI, PI) // azimuth can be a full range search
);
d = 0; t = oresult.score;
polar = std::get<0>(oresult.optimum);
azimuth = std::get<1>(oresult.optimum);
Vec3d bridgedir = Vec3d(std::cos(azimuth) * std::sin(polar),
std::sin(azimuth) * std::sin(polar),
std::cos(polar)).normalized();
t = std::min(t, m_cfg.max_bridge_length_mm);
while(d < t && !std::isinf(tdown = bridge_mesh_intersect(
hjp + d*bridgedir,
dirdown,
head.r_back_mm))) {
d += head.r_back_mm;
}
if(std::isinf(tdown)) { // we heave found a route to the ground
routedown(head, bridgedir, d); return;
}
// /////////////////////////////////////////////////////////////////
// Route to model body
// /////////////////////////////////////////////////////////////////
double zangle = std::asin(hit.direction()(Z));
zangle = std::max(zangle, PI/4);
double h = std::sin(zangle) * head.fullwidth();
// The width of the tail head that we would like to have...
h = std::min(hit.distance() - head.r_back_mm, h);
if(h > 0) {
Vec3d endp{hjp(X), hjp(Y), hjp(Z) - hit.distance() + h};
auto center_hit = m_mesh.query_ray_hit(hjp, dirdown);
double hitdiff = center_hit.distance() - hit.distance();
Vec3d hitp = std::abs(hitdiff) < 2*head.r_back_mm?
center_hit.position() : hit.position();
head.transform();
Pillar& pill = m_builder.add_pillar(unsigned(head.id),
endp,
head.r_back_mm);
Vec3d taildir = endp - hitp;
double dist = distance(endp, hitp) + m_cfg.head_penetration_mm;
double w = dist - 2 * head.r_pin_mm - head.r_back_mm;
Head tailhead(head.r_back_mm,
head.r_pin_mm,
w,
m_cfg.head_penetration_mm,
taildir,
hitp);
tailhead.transform();
pill.base = tailhead.mesh;
// Experimental: add the pillar to the index for cascading
std::lock_guard<ccr::SpinningMutex> lk(mutex);
modelpillars.emplace_back(unsigned(pill.id));
return;
}
// We have failed to route this head.
BOOST_LOG_TRIVIAL(warning)
<< "Failed to route model facing support point."
<< " ID: " << idx;
head.invalidate();
};
ccr::enumerate(m_iheads_onmodel.begin(), m_iheads_onmodel.end(), onmodelfn);
for(auto pillid : modelpillars) {
auto& pillar = m_builder.pillar(pillid);
m_pillar_index.insert(pillar.endpoint(), pillid);
}
}
void SupportTreeBuildsteps::interconnect_pillars()
{
// Now comes the algorithm that connects pillars with each other.
// Ideally every pillar should be connected with at least one of its
// neighbors if that neighbor is within max_pillar_link_distance
// Pillars with height exceeding H1 will require at least one neighbor
// to connect with. Height exceeding H2 require two neighbors.
double H1 = m_cfg.max_solo_pillar_height_mm;
double H2 = m_cfg.max_dual_pillar_height_mm;
double d = m_cfg.max_pillar_link_distance_mm;
//A connection between two pillars only counts if the height ratio is
// bigger than 50%
double min_height_ratio = 0.5;
std::set<unsigned long> pairs;
// A function to connect one pillar with its neighbors. THe number of
// neighbors is given in the configuration. This function if called
// for every pillar in the pillar index. A pair of pillar will not
// be connected multiple times this is ensured by the 'pairs' set which
// remembers the processed pillar pairs
auto cascadefn =
[this, d, &pairs, min_height_ratio, H1] (const PointIndexEl& el)
{
Vec3d qp = el.first; // endpoint of the pillar
const Pillar& pillar = m_builder.pillar(el.second); // actual pillar
// Get the max number of neighbors a pillar should connect to
unsigned neighbors = m_cfg.pillar_cascade_neighbors;
// connections are already enough for the pillar
if(pillar.links >= neighbors) return;
// Query all remaining points within reach
auto qres = m_pillar_index.query([qp, d](const PointIndexEl& e){
return distance(e.first, qp) < d;
});
// sort the result by distance (have to check if this is needed)
std::sort(qres.begin(), qres.end(),
[qp](const PointIndexEl& e1, const PointIndexEl& e2){
return distance(e1.first, qp) < distance(e2.first, qp);
});
for(auto& re : qres) { // process the queried neighbors
if(re.second == el.second) continue; // Skip self
auto a = el.second, b = re.second;
// Get unique hash for the given pair (order doesn't matter)
auto hashval = pairhash(a, b);
// Search for the pair amongst the remembered pairs
if(pairs.find(hashval) != pairs.end()) continue;
const Pillar& neighborpillar = m_builder.pillar(re.second);
// this neighbor is occupied, skip
if(neighborpillar.links >= neighbors) continue;
if(interconnect(pillar, neighborpillar)) {
pairs.insert(hashval);
// If the interconnection length between the two pillars is
// less than 50% of the longer pillar's height, don't count
if(pillar.height < H1 ||
neighborpillar.height / pillar.height > min_height_ratio)
m_builder.increment_links(pillar);
if(neighborpillar.height < H1 ||
pillar.height / neighborpillar.height > min_height_ratio)
m_builder.increment_links(neighborpillar);
}
// connections are enough for one pillar
if(pillar.links >= neighbors) break;
}
};
// Run the cascade for the pillars in the index
m_pillar_index.foreach(cascadefn);
// We would be done here if we could allow some pillars to not be
// connected with any neighbors. But this might leave the support tree
// unprintable.
//
// The current solution is to insert additional pillars next to these
// lonely pillars. One or even two additional pillar might get inserted
// depending on the length of the lonely pillar.
size_t pillarcount = m_builder.pillarcount();
// Again, go through all pillars, this time in the whole support tree
// not just the index.
for(size_t pid = 0; pid < pillarcount; pid++) {
auto pillar = [this, pid]() { return m_builder.pillar(pid); };
// Decide how many additional pillars will be needed:
unsigned needpillars = 0;
if (pillar().bridges > m_cfg.max_bridges_on_pillar)
needpillars = 3;
else if (pillar().links < 2 && pillar().height > H2) {
// Not enough neighbors to support this pillar
needpillars = 2;
} else if (pillar().links < 1 && pillar().height > H1) {
// No neighbors could be found and the pillar is too long.
needpillars = 1;
}
needpillars = std::max(pillar().links, needpillars) - pillar().links;
if (needpillars == 0) continue;
// Search for new pillar locations:
bool found = false;
double alpha = 0; // goes to 2Pi
double r = 2 * m_cfg.base_radius_mm;
Vec3d pillarsp = pillar().startpoint();
// temp value for starting point detection
Vec3d sp(pillarsp(X), pillarsp(Y), pillarsp(Z) - r);
// A vector of bool for placement feasbility
std::vector<bool> canplace(needpillars, false);
std::vector<Vec3d> spts(needpillars); // vector of starting points
double gnd = m_builder.ground_level;
double min_dist = m_cfg.pillar_base_safety_distance_mm +
m_cfg.base_radius_mm + EPSILON;
while(!found && alpha < 2*PI) {
for (unsigned n = 0;
n < needpillars && (!n || canplace[n - 1]);
n++)
{
double a = alpha + n * PI / 3;
Vec3d s = sp;
s(X) += std::cos(a) * r;
s(Y) += std::sin(a) * r;
spts[n] = s;
// Check the path vertically down
auto hr = bridge_mesh_intersect(s, {0, 0, -1}, pillar().r);
Vec3d gndsp{s(X), s(Y), gnd};
// If the path is clear, check for pillar base collisions
canplace[n] = std::isinf(hr.distance()) &&
std::sqrt(m_mesh.squared_distance(gndsp)) >
min_dist;
}
found = std::all_of(canplace.begin(), canplace.end(),
[](bool v) { return v; });
// 20 angles will be tried...
alpha += 0.1 * PI;
}
std::vector<long> newpills;
newpills.reserve(needpillars);
if (found)
for (unsigned n = 0; n < needpillars; n++) {
Vec3d s = spts[n];
Pillar p(s, Vec3d(s(X), s(Y), gnd), pillar().r);
p.add_base(m_cfg.base_height_mm, m_cfg.base_radius_mm);
if (interconnect(pillar(), p)) {
Pillar &pp = m_builder.add_pillar(p);
m_pillar_index.insert(pp.endpoint(), unsigned(pp.id));
m_builder.add_junction(s, pillar().r);
double t = bridge_mesh_intersect(pillarsp,
dirv(pillarsp, s),
pillar().r);
if (distance(pillarsp, s) < t)
m_builder.add_bridge(pillarsp, s, pillar().r);
if (pillar().endpoint()(Z) > m_builder.ground_level)
m_builder.add_junction(pillar().endpoint(),
pillar().r);
newpills.emplace_back(pp.id);
m_builder.increment_links(pillar());
m_builder.increment_links(pp);
}
}
if(!newpills.empty()) {
for(auto it = newpills.begin(), nx = std::next(it);
nx != newpills.end(); ++it, ++nx) {
const Pillar& itpll = m_builder.pillar(*it);
const Pillar& nxpll = m_builder.pillar(*nx);
if(interconnect(itpll, nxpll)) {
m_builder.increment_links(itpll);
m_builder.increment_links(nxpll);
}
}
m_pillar_index.foreach(cascadefn);
}
}
}
void SupportTreeBuildsteps::routing_headless()
{
// For now we will just generate smaller headless sticks with a sharp
// ending point that connects to the mesh surface.
// We will sink the pins into the model surface for a distance of 1/3 of
// the pin radius
for(unsigned i : m_iheadless) {
m_thr();
const auto R = double(m_support_pts[i].head_front_radius);
const double HWIDTH_MM = R/3;
// Exact support position
Vec3d sph = m_support_pts[i].pos.cast<double>();
Vec3d n = m_support_nmls.row(i); // mesh outward normal
Vec3d sp = sph - n * HWIDTH_MM; // stick head start point
Vec3d dir = {0, 0, -1};
Vec3d sj = sp + R * n; // stick start point
// This is only for checking
double idist = bridge_mesh_intersect(sph, dir, R, true);
double dist = ray_mesh_intersect(sj, dir);
if (std::isinf(dist))
dist = sph(Z) - m_mesh.ground_level()
+ m_mesh.ground_level_offset();
if(std::isnan(idist) || idist < 2*R ||
std::isnan(dist) || dist < 2*R)
{
BOOST_LOG_TRIVIAL(warning) << "Can not find route for headless"
<< " support stick at: "
<< sj.transpose();
continue;
}
Vec3d ej = sj + (dist + HWIDTH_MM)* dir;
m_builder.add_compact_bridge(sp, ej, n, R, !std::isinf(dist));
}
}
}
}
View Git Blame Copy Permalink