WIP: sidehead routedown when pillar is too long

This commit is contained in:
tamasmeszaros 2019-03-05 18:21:20 +01:00
parent 7552556998
commit 34e0b69179
2 changed files with 100 additions and 18 deletions

View file

@ -415,8 +415,8 @@ struct Pillar {
return {endpoint(X), endpoint(Y), endpoint(Z) + height};
}
void add_base(double height = 3, double radius = 2) {
if(height <= 0) return;
Pillar& add_base(double height = 3, double radius = 2) {
if(height <= 0) return *this;
assert(steps >= 0);
auto last = int(steps - 1);
@ -459,7 +459,7 @@ struct Pillar {
indices.emplace_back(last, offs + last, offs);
indices.emplace_back(hcenter, last, 0);
indices.emplace_back(offs, offs + last, lcenter);
return *this;
}
bool has_base() const { return !base.points.empty(); }
@ -1472,17 +1472,56 @@ class SLASupportTree::Algorithm {
if(ncount == neighbors) break;
}
unsigned needpillars = 0;
if(ncount < 1 && pillar.height > H1) {
// No neighbors could be found and the pillar is too long.
// No neighbors could not be found and the pillar is too long.
BOOST_LOG_TRIVIAL(warning) << "Pillar is too long and has no "
"neighbors. Head ID: "
<< pillar.start_junction_id;
// double D = 2*m_cfg.base_radius_mm;
// Vec3d jp = pillar.startpoint();
// double h = D / std::cos(m_cfg.bridge_slope);
// bool found = false;
// double phi = 0;
// // Search for a suitable angle for the two pillars
// while(!found && phi < 2*PI) {
// }
needpillars = 1;
} else if(ncount < 2 && pillar.height > H2) {
// Not enough neighbors to support this pillar
BOOST_LOG_TRIVIAL(warning) << "Pillar is too long and has too "
"few neighbors. Head ID: "
<< pillar.start_junction_id;
needpillars = 2 - ncount;
}
// WIP:
// note: sideheads ARE tested to reach the ground!
// if(needpillars > 0) {
// if(pillar.starts_from_head) {
// // search for a sidehead for this head. We will route that
// // to the ground.
// const Head& head = m_result.head(unsigned(pillar.start_junction_id));
// for(auto cl : m_pillar_clusters) {
// auto it = std::find(cl.begin(), cl.end(), head.id);
// if(it != cl.end()) {
// cl.erase(it);
// for(size_t j = 0; j < cl.size() && j < needpillars; j++) {
// unsigned hid = cl[j];
// m_result.add_pillar(hid, endpoint, )
// .add_base(m_cfg.base_height_mm, m_cfg.base_radius_mm);
// }
// }
// }
// }
// }
});
}

View file

@ -354,26 +354,18 @@ PointSet normals(const PointSet& points,
namespace bgi = boost::geometry::index;
using Index3D = bgi::rtree< SpatElement, bgi::rstar<16, 4> /* ? */ >;
ClusteredPoints cluster(Index3D& sindex, double dist, unsigned max_points)
ClusteredPoints cluster(Index3D& sindex, unsigned max_points,
std::function<std::vector<SpatElement>(const Index3D&, const SpatElement&)> qfn)
{
using Elems = std::vector<SpatElement>;
// Recursive function for visiting all the points in a given distance to
// each other
std::function<void(Elems&, Elems&)> group =
[&sindex, &group, max_points, dist](Elems& pts, Elems& cluster)
[&sindex, &group, max_points, qfn](Elems& pts, Elems& cluster)
{
for(auto& p : pts) {
std::vector<SpatElement> tmp;
sindex.query(
bgi::nearest(p.first, max_points),
std::back_inserter(tmp)
);
for(auto it = tmp.begin(); it < tmp.end(); ++it)
if(distance(p.first, it->first) > dist) it = tmp.erase(it);
std::vector<SpatElement> tmp = qfn(sindex, p);
auto cmp = [](const SpatElement& e1, const SpatElement& e2){
return e1.second < e2.second;
};
@ -417,6 +409,25 @@ ClusteredPoints cluster(Index3D& sindex, double dist, unsigned max_points)
return result;
}
namespace {
std::vector<SpatElement> distance_queryfn(const Index3D& sindex,
const SpatElement& p,
double dist,
unsigned max_points)
{
std::vector<SpatElement> tmp; tmp.reserve(max_points);
sindex.query(
bgi::nearest(p.first, max_points),
std::back_inserter(tmp)
);
for(auto it = tmp.begin(); it < tmp.end(); ++it)
if(distance(p.first, it->first) > dist) it = tmp.erase(it);
return tmp;
}
}
// Clustering a set of points by the given criteria
ClusteredPoints cluster(
const std::vector<unsigned>& indices,
@ -430,7 +441,35 @@ ClusteredPoints cluster(
// Build the index
for(auto idx : indices) sindex.insert( std::make_pair(pointfn(idx), idx));
return cluster(sindex, dist, max_points);
return cluster(sindex, max_points,
[dist, max_points](const Index3D& sidx, const SpatElement& p)
{
return distance_queryfn(sidx, p, dist, max_points);
});
}
// Clustering a set of points by the given criteria
ClusteredPoints cluster(
const std::vector<unsigned>& indices,
std::function<Vec3d(unsigned)> pointfn,
std::function<bool(const SpatElement&, const SpatElement&)> predicate,
unsigned max_points)
{
// A spatial index for querying the nearest points
Index3D sindex;
// Build the index
for(auto idx : indices) sindex.insert( std::make_pair(pointfn(idx), idx));
return cluster(sindex, max_points,
[max_points, predicate](const Index3D& sidx, const SpatElement& p)
{
std::vector<SpatElement> tmp; tmp.reserve(max_points);
sidx.query(bgi::satisfies([p, predicate](const SpatElement& e){
return predicate(p, e);
}), std::back_inserter(tmp));
return tmp;
});
}
ClusteredPoints cluster(const PointSet& pts, double dist, unsigned max_points)
@ -442,7 +481,11 @@ ClusteredPoints cluster(const PointSet& pts, double dist, unsigned max_points)
for(Eigen::Index i = 0; i < pts.rows(); i++)
sindex.insert(std::make_pair(Vec3d(pts.row(i)), unsigned(i)));
return cluster(sindex, dist, max_points);
return cluster(sindex, max_points,
[dist, max_points](const Index3D& sidx, const SpatElement& p)
{
return distance_queryfn(sidx, p, dist, max_points);
});
}
}