23 #include <CGAL/Arr_segment_traits_2.h>
24 #include <CGAL/Cartesian.h>
25 #include <CGAL/MP_Float.h>
26 #include <CGAL/Quotient.h>
27 #include <CGAL/Simple_cartesian.h>
28 #include <CGAL/Sweep_line_2_algorithms.h>
29 #include <CGAL/algorithm.h>
30 #include <CGAL/point_generators_2.h>
31 #include <core/exception.h>
32 #include <core/threading/mutex_locker.h>
33 #include <navgraph/generators/grid.h>
35 typedef CGAL::Quotient<CGAL::MP_Float> NT;
36 typedef CGAL::Cartesian<NT> Kernel;
38 typedef Kernel::Point_2 Point;
39 typedef Kernel::Segment_2 Segment;
40 typedef Kernel::Vector_2 Vector_2;
41 typedef CGAL::Points_on_segment_2<Point> PG;
42 typedef CGAL::Creator_uniform_2<Point, Segment> Creator;
43 typedef CGAL::Join_input_iterator_2<PG, PG, Creator> Segm_iterator;
44 typedef CGAL::Counting_iterator<Segm_iterator, Segment> Count_iterator;
45 typedef std::vector<Segment> Vector;
47 typedef CGAL::Arr_segment_traits_2<Kernel> Traits_2;
48 typedef Traits_2::Curve_2 Curve_2;
63 if (params.find(
"spacing") == params.end()) {
64 throw Exception(
"Grid algorithm requires 'spacing' parameter");
66 if (params.find(
"margin") == params.end()) {
67 throw Exception(
"Grid algorithm requires 'margin' parameter");
69 add_diagonals_ =
false;
70 if (params.find(
"add-diagonals") != params.end()) {
71 add_diagonals_ = (params.at(
"add-diagonals") ==
"true");
75 spacing_ = std::stof(params.at(
"spacing"), &pos);
76 if (pos < params.at(
"spacing").size()) {
77 throw Exception(
"Cannot convert spacing '%s' to float", params.at(
"spacing").c_str());
79 }
catch (std::invalid_argument &e) {
81 }
catch (std::out_of_range &e) {
87 margin_ = std::stof(params.at(
"margin"), &pos);
88 if (pos < params.at(
"margin").size()) {
89 throw Exception(
"Cannot convert margin '%s' to float", params.at(
"margin").c_str());
91 }
catch (std::invalid_argument &e) {
93 }
catch (std::out_of_range &e) {
107 throw Exception(
"Grid algorithm requires bounding box");
113 if (CGAL::compare_xy<Kernel>(bb_p1, bb_p2) != CGAL::SMALLER) {
114 throw Exception(
"Bounding box P1 must be smaller than P2 (x1 < x2 or x1 == x2 and y1 < y2)");
121 const Vector_2 diff(bb_p2 - bb_p1);
122 if (diff.x() < spacing_ || diff.y() < spacing_) {
123 throw Exception(
"Bounding box too small for given spacing");
125 size_t nx = CGAL::to_double(diff.x()) / spacing_;
126 size_t ny = CGAL::to_double(diff.y()) / spacing_;
130 const float spacing_2 = spacing_ / 2;
133 PG p1(Point(bb_p1.x(), bb_p1.y() + spacing_2), Point(bb_p1.x(), bb_p2.y() - spacing_2), ny);
134 PG p2(Point(bb_p2.x(), bb_p1.y() + spacing_2), Point(bb_p2.x(), bb_p2.y() - spacing_2), ny);
135 Segm_iterator t1(p1, p2);
136 Count_iterator t1_begin(t1);
137 Count_iterator t1_end(t1, ny);
147 PG p3(Point(bb_p1.x() + spacing_2, bb_p1.y()), Point(bb_p2.x() - spacing_2, bb_p1.y()), nx);
148 PG p4(Point(bb_p1.x() + spacing_2, bb_p2.y()), Point(bb_p2.x() - spacing_2, bb_p2.y()), nx);
149 Segm_iterator t2(p3, p4);
150 Count_iterator t2_begin(t2);
151 Count_iterator t2_end(t2, nx);
161 std::vector<Curve_2> segs;
162 segs.reserve(nx + ny);
163 std::transform(t1_begin, t1_end, std::back_inserter(segs), [](
const Segment &s) -> Curve_2 {
164 return Curve_2(s.source(), s.target());
166 std::transform(t2_begin, t2_end, std::back_inserter(segs), [](
const Segment &s) -> Curve_2 {
167 return Curve_2(s.source(), s.target());
171 std::vector<Point> ipts;
172 ipts.reserve(nx * ny);
173 CGAL::compute_intersection_points(segs.begin(), segs.end(), std::back_inserter(ipts));
177 std::sort(ipts.begin(), ipts.end(), [](
const Point &p1,
const Point &p2) {
178 return CGAL::compare_yx<Kernel>(p1, p2) == CGAL::SMALLER;
188 std::map<std::string, std::string> props_gen = {{
"generated",
"true"}};
191 std::vector<std::pair<std::string, Point>> points;
192 points.reserve(ipts.size());
193 for (
size_t i = 0; i < ipts.size(); ++i) {
194 std::string name =
"G-" + std::to_string((i % nx) + 1) +
"-" + std::to_string((i / nx) + 1);
195 points.push_back(std::make_pair(name, ipts[i]));
197 NavGraphNode(name, CGAL::to_double(ipts[i].x()), CGAL::to_double(ipts[i].y()), props_gen));
207 for (
size_t i = 0; i < points.size(); ++i) {
215 const size_t prev_i = (((i / nx) - 1) * nx) + (i % nx);
218 if (add_diagonals_) {
219 if (prev_i % nx > 0) {
223 if (prev_i % nx < (nx - 1)) {
232 const std::vector<NavGraphEdge> &edges = graph->
edges();
235 std::list<NavGraphEdge> to_remove;
238 if (e.distance(o.first, o.second) <= margin_) {
239 to_remove.push_back(e);
244 std::for_each(to_remove.begin(), to_remove.end(), [&graph](
const NavGraphEdge &e) {
245 graph->remove_edge(e);