41void VoronoiCell::ComputeFinalCentroid() {
44 for (
auto const &p : cell) {
45 polygon.push_back(CGAL_Point2{p[0], p[1]});
47 auto cgal_centroid = CGAL::centroid(polygon.begin(), polygon.end());
48 centroid_ = CGALtoCC(cgal_centroid);
49 centroid_ += origin_shift;
51 centroid_ = centroid_ / mass_;
55void Voronoi::CellNavigator(
56 VoronoiCell
const &vcell,
57 std::function<
void(
double, Point2)> evaluator_func) {
58 auto const &cell = vcell.cell;
62 for (
int i = 1; i < n; ++i) {
63 if (cell[i].x() < cell[left_id].x()) {
66 if (cell[i].x() > cell[right_id].x()) {
71 int min_i = std::round(cell[left_id].x() / resolution_);
72 min_i = min_i < 0 ? 0 : min_i;
74 int max_i = std::round(cell[right_id].x() / resolution_);
75 max_i = max_i > map_size_.x() ? map_size_.x() : max_i;
77 auto next_id = [=](
int const id) {
return (
id + 1) % n; };
78 auto prev_id = [=](
int const id) {
return id == 0 ? (n - 1) : (id - 1); };
80 auto cc_pt_id = next_id(left_id);
81 auto c_pt_id = prev_id(left_id);
83 for (
int i = min_i; i < max_i; ++i) {
84 double x = i * resolution_ + resolution_ / 2.;
86 if (cc_pt_id == left_id) {
89 if (cell[cc_pt_id].x() < x) {
90 cc_pt_id = next_id(cc_pt_id);
96 if (c_pt_id == left_id) {
99 if (cell[c_pt_id].x() < x) {
100 c_pt_id = prev_id(c_pt_id);
106 if (cell[cc_pt_id].x() < x) {
109 if (cell[c_pt_id].x() < x) {
113 auto prev_pt = cell[prev_id(cc_pt_id)];
114 auto cc_pt = cell[cc_pt_id];
115 auto x1 = prev_pt.x();
116 auto y1 = prev_pt.y();
120 if ((x2 - x1) <
kEps) {
121 throw std::runtime_error{
"Unexpected error!"};
123 auto y_lower = y1 + (x - x1) * (y2 - y1) / (x2 - x1);
125 auto next_pt = cell[next_id(c_pt_id)];
126 auto c_pt = cell[c_pt_id];
132 if ((x2 - x1) <
kEps) {
133 throw std::runtime_error{
"Unexpected error!"};
135 auto y_upper = y1 + (x - x1) * (y2 - y1) / (x2 - x1);
137 int min_j = std::round(y_lower / resolution_);
138 min_j = min_j < 0 ? 0 : min_j;
140 int max_j = std::round(y_upper / resolution_);
141 max_j = max_j > map_size_.y() ? map_size_.y() : max_j;
143 for (
int j = min_j; j < max_j; ++j) {
144 double y = j * resolution_ + resolution_ / 2.;
145 Point2 curr_pt(x, y);
146 auto map_val = map_->operator()(i, j);
147 evaluator_func(map_val, curr_pt);
152void Voronoi::ComputeMassCentroid(VoronoiCell &vcell) {
154 if (compute_single_ ==
true) {
155 vcell.origin_shift = origin_shift_;
158 auto fp = std::bind(&VoronoiCell::MassCentroidFunctional, &vcell,
159 std::placeholders::_1, std::placeholders::_2);
160 CellNavigator(vcell, fp);
161 vcell.ComputeFinalCentroid();
167void Voronoi::ComputeVoronoiCells() {
168 if (num_sites_ == 1) {
170 vcell.site = sites_[0];
171 vcell.cell = PointVector{Point2{0, 0}, Point2{map_size_.x(), 0},
172 Point2{map_size_.x(), map_size_.y()},
173 Point2{0, map_size_.y()}};
174 ComputeMassCentroid(vcell);
175 if (compute_single_ ==
true) {
176 voronoi_cell_ = vcell;
178 voronoi_cells_[0] = vcell;
184 Delaunay_triangulation_2 dt2;
185 std::vector<CGAL_Point2> CGAL_sites;
186 CGAL_sites.reserve(num_sites_);
188 for (
auto const &pt : sites_) {
189 CGAL_sites.push_back(CGAL_Point2(pt.x(), pt.y()));
191 dt2.insert(CGAL_sites.begin(), CGAL_sites.end());
193 CGAL_DelaunayHelper vor;
199 vor.segments_.push_back(
200 Segment_2(CGAL_Point2(0, 0), CGAL_Point2(map_size_.x(), 0)));
201 vor.segments_.push_back(Segment_2(CGAL_Point2(map_size_.x(), 0),
202 CGAL_Point2(map_size_.x(), map_size_.y())));
203 vor.segments_.push_back(Segment_2(CGAL_Point2(map_size_.x(), map_size_.y()),
204 CGAL_Point2(0, map_size_.y())));
205 vor.segments_.push_back(
206 Segment_2(CGAL_Point2(0, map_size_.y()), CGAL_Point2(0, 0)));
209 for (
auto const &seg : vor.segments_) {
211 if (seg.is_degenerate()) {
214 CGAL::insert(arr, seg);
218 CGAL::insert(arr, vor.rays_.begin(), vor.rays_.end());
226 CGAL::insert(arr, vor.lines_.begin(), vor.lines_.end());
236 CGAL_pl cgal_pl(arr);
239 if (compute_single_ ==
true) {
240 auto pt = CGAL_sites[robot_id_];
242 auto pt_obj = cgal_pl.locate(pt);
243 auto f = boost::get<Arrangement_2::Face_const_handle>(&pt_obj);
244 CGAL_CCBTraversal<Arrangement_2>((*f)->outer_ccb(), polygon);
245 if (not polygon.is_counterclockwise_oriented()) {
246 polygon.reverse_orientation();
248 PointVector poly_points;
249 for (
auto const &p : (polygon)) {
250 poly_points.push_back(CGALtoCC(p));
252 voronoi_cell_.site = CGALtoCC(pt);
253 voronoi_cell_.cell = poly_points;
255 ComputeMassCentroid(voronoi_cell_);
265#pragma omp parallel for num_threads(num_sites_)
266 for (
int iSite = 0; iSite < num_sites_; ++iSite) {
267 auto pt = CGAL_sites[iSite];
268 auto pt_obj = cgal_pl.locate(pt);
269 auto *f = boost::get<Arrangement_2::Face_const_handle>(&pt_obj);
271 std::cout << pt << std::endl;
272 throw std::runtime_error{
"Could not find a face for the robot"};
276 typename Arrangement_2::Ccb_halfedge_const_circulator circ =
278 typename Arrangement_2::Ccb_halfedge_const_circulator curr = circ;
279 auto curr_pt = curr->source()->point();
282 curr_pt = he->target()->point();
283 polygon.push_back(curr_pt);
285 }
while (curr != circ);
287 if (polygon.size() == 0) {
288 throw std::runtime_error{
"Zero size polygon"};
294 vcell.site = CGALtoCC(pt);
295 vcell.cell.reserve(polygon.size());
296 for (
auto const &p : polygon) {
297 vcell.cell.push_back(CGALtoCC(p));
299 ComputeMassCentroid(vcell);
300 voronoi_cells_[iSite] = vcell;
Contains the configuration for the CGAL library.
Namespace for the CoverageControl library.
Contains utility functions for CGAL.
Class for computing Voronoi cells.