WIP: sidehead routedown when pillar is too long
This commit is contained in:
parent
7552556998
commit
34e0b69179
2 changed files with 100 additions and 18 deletions
|
@ -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);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
});
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
});
|
||||
}
|
||||
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue