@@ -233,14 +233,23 @@ void Voronoi::ComputeVoronoiCells() {
233233 /* std::cout << "lines inserted" << std::endl; */
234234 /* CGAL::insert(arr, vor.segments_.begin(), vor.segments_.end()); */
235235 /* std::cout << "arr end" << std::endl; */
236- CGAL_pl cgal_pl (arr);
236+ // CGAL_pl cgal_pl(arr);
237237 /* std::cout << "cgal_pl end" << std::endl; */
238238
239+ std::vector<CGAL_Query_result> query_results_vor, query_results_vor_sorted;
239240 if (compute_single_ == true ) {
240241 auto pt = CGAL_sites[robot_id_];
242+ std::list<CGAL_Point2> temp_site; temp_site.push_back (pt);
243+ CGAL::locate (arr, temp_site.begin (), temp_site.end (), std::back_inserter (query_results_vor));
241244 Polygon_2 polygon;
242- auto pt_obj = cgal_pl.locate (pt);
243- auto f = boost::get<Arrangement_2::Face_const_handle>(&pt_obj);
245+ const Arrangement_2::Face_const_handle* f;
246+ if ((f = std::get_if<Arrangement_2::Face_const_handle>(&query_results_vor[0 ].second ))) {
247+ if ((*f)->is_unbounded ()) {
248+ throw std::runtime_error{" inside the unbounded face." };
249+ }
250+ } else {
251+ throw std::runtime_error{" Invalid object." };
252+ }
244253 CGAL_CCBTraversal<Arrangement_2>((*f)->outer_ccb (), polygon);
245254 if (not polygon.is_counterclockwise_oriented ()) {
246255 polygon.reverse_orientation ();
@@ -262,14 +271,30 @@ void Voronoi::ComputeVoronoiCells() {
262271 /* PrunePolygons(polygon_list, map_size_); */
263272 // Create voronoi_cells_ such that the correct cell is assigned to the robot
264273 /* std::cout << "Before parallel for" << std::endl; */
274+ CGAL::locate (arr, CGAL_sites.begin (), CGAL_sites.end (), std::back_inserter (query_results_vor));
275+ /* Results (point, object) are in xy-lexicographic order */
276+ /* Need to sort the results to match the order of the sites */
277+ for (int i = 0 ; i < num_sites_; ++i) {
278+
279+ auto it = std::find_if (query_results_vor.begin (), query_results_vor.end (),
280+ [i, CGAL_sites](CGAL_Query_result const &qr) {
281+ return qr.first == CGAL_sites[i];
282+ });
283+ if (it == query_results_vor.end ()) {
284+ throw std::runtime_error{" Could not find a query result" };
285+ }
286+ query_results_vor_sorted.push_back (*it);
287+ }
288+
265289#pragma omp parallel for num_threads(num_sites_)
266290 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);
270- if (not f) {
271- std::cout << pt << std::endl;
272- throw std::runtime_error{" Could not find a face for the robot" };
291+ const Arrangement_2::Face_const_handle* f;
292+ if ((f = std::get_if<Arrangement_2::Face_const_handle>(&query_results_vor_sorted[iSite].second ))) {
293+ if ((*f)->is_unbounded ()) {
294+ throw std::runtime_error{" inside the unbounded face." };
295+ }
296+ } else {
297+ throw std::runtime_error{" Invalid object." };
273298 }
274299 /* CGAL_CCBTraversal<Arrangement_2> ((*f)->outer_ccb(), polygon); */
275300 Polygon_2 polygon;
@@ -291,7 +316,7 @@ void Voronoi::ComputeVoronoiCells() {
291316 /* polygon.reverse_orientation(); */
292317 /* } */
293318 VoronoiCell vcell;
294- vcell.site = CGALtoCC (pt );
319+ vcell.site = CGALtoCC (CGAL_sites[iSite] );
295320 vcell.cell .reserve (polygon.size ());
296321 for (auto const &p : polygon) {
297322 vcell.cell .push_back (CGALtoCC (p));
0 commit comments