int VoronoiVerticesColors(VD vd, int ** voronoiPoints, double ** voronoiColors){
// Fill in points for voronoi edges
VD::Face_iterator ft = vd.faces_begin();
// DT dt = vd.dual();
// DT::Vertex_iterator dv_iter = dt.vertices_begin();
int i = 0, ict=0, x, y, idx = 0;
double currHeight;
// specify voronoi surfaces
for(;ft != vd.faces_end();ft++){
// for(;ict<5;ft++, ict++){
if(!(*ft).is_unbounded()){
VD::Delaunay_vertex_handle vh = (*ft).dual();
Point_2 center = vh->point();
double currX = center.x();
double currY = center.y();
Ccb_halfedge_circulator et_start = (*ft).ccb();
Ccb_halfedge_circulator et = et_start;
do {
if(et->has_source() && et->has_target()){
// middle
// delaunayPoints[didx++] = currX;
// delaunayPoints[didx++] = currY;
// delaunayPoints[didx++] = ZPOS - 0.4;
// delaunayColors[didx++] = 1.0f; //(double) ((rand() % (int) 255) + 1) / 255.0f;//
// delaunayColors[didx++] = 0.0f; //(double) ((rand() % (int) 255) + 1) / 255.0f;// 0.0f;
// delaunayColors[didx++] = 0.0f; //(double) ((rand() % (int) 255) + 1) / 255.0f;//0.0f;
if(
// (currX > -1 && currX < 1) && (currY > -1) && (currY < 1)
(currX >= 0 && currX <= IslandWidth) && (currY >= 0) && (currY <= IslandHeight)
// (et->source()->point().x() > -1) && (et->source()->point().x() < 1) &&
// (et->source()->point().y() > -1) && (et->source()->point().y() < 1)
){
double sx = et->source()->point().x();
double sy = et->source()->point().y();
double tx = et->target()->point().x();
double ty = et->target()->point().y();
if(!(sx < 0 || sx > IslandWidth || sy < 0 || sy > IslandHeight || tx < 0 || tx > IslandWidth || ty < 0 || ty > IslandHeight)){
// reallocate memory
(*voronoiPoints) = (int *) realloc((*voronoiPoints), (idx + 9) * sizeof(int));
(*voronoiPoints)[idx + 0] = currX;
(*voronoiPoints)[idx + 1] = currY;
(*voronoiPoints)[idx + 2] = ZPOS + 0.2;
// first vertex of triangle
(*voronoiPoints)[idx + 3] = et->source()->point().x();
(*voronoiPoints)[idx + 4] = et->source()->point().y();
(*voronoiPoints)[idx + 5] = ZPOS + 0.2;
// second vertex of triangle
(*voronoiPoints)[idx + 6] = et->target()->point().x();
(*voronoiPoints)[idx + 7] = et->target()->point().y();
(*voronoiPoints)[idx + 8] = ZPOS + 0.2;
idx += 9;
}
}
}
} while ( ++et != et_start );
}
}
return idx;
}
//! Initialize an arrangement
void init_arr(Arrangement& arr, int verbose_level)
{
// Initialize the data of the halfedges of the arrangement.
Halfedge_iterator heit;
for (heit = arr.halfedges_begin(); heit != arr.halfedges_end(); ++heit)
heit->set_data(heit->curve().data() *
((heit->direction() == CGAL::ARR_LEFT_TO_RIGHT) ? 1 : 2));
// Initialize the data of the faces of the arrangement.
Face_iterator fit;
for (fit = arr.faces_begin(); fit != arr.faces_end(); ++fit) {
unsigned int count = 0;
// Outer ccb
Outer_ccb_iterator ocit;
for (ocit = fit->outer_ccbs_begin(); ocit != fit->outer_ccbs_end(); ++ocit) {
Ccb_halfedge_circulator curr = *ocit;
do count += curr->data() * 2;
while (++curr != *ocit);
}
// Inner ccbs
Inner_ccb_iterator icit;
for (icit = fit->inner_ccbs_begin(); icit != fit->inner_ccbs_end(); ++icit) {
Ccb_halfedge_circulator curr = *icit;
do count += curr->data();
while (++curr != *icit);
}
fit->set_data(count);
}
// Initialize the data of the vertices of the arrangement.
Vertex_iterator vit;
for (vit = arr.vertices_begin(); vit != arr.vertices_end(); ++vit) {
unsigned int count = 0;
if (vit->is_isolated()) count = vit->face()->data();
else {
Halfedge_around_vertex_const_circulator curr = vit->incident_halfedges();
do count += curr->data();
while (++curr != vit->incident_halfedges());
}
vit->set_data(count);
}
if (verbose_level > 0) std::cout << "Arrangement Input: " << std::endl;
if (verbose_level > 1) {
std::cout << "Halfedge Data: " << std::endl;
Halfedge_iterator heit;
for (heit = arr.halfedges_begin(); heit != arr.halfedges_end(); ++heit)
std::cout << heit->source()->point() << " "
<< heit->target()->point() << " " << heit->data()
<< std::endl;
}
if (verbose_level > 0) {
std::cout << "Face Data: " << std::endl;
Face_iterator fit;
for (fit = arr.faces_begin(); fit != arr.faces_end(); ++fit)
std::cout << fit->data() << std::endl;
}
}
/** Compute graph.
* @param graph the resulting nodes and edges will be added to this graph.
* The graph will *not* be cleared automatically. The graph will be locked
* while adding nodes.
*/
void
NavGraphGeneratorVoronoi::compute(fawkes::LockPtr<fawkes::NavGraph> graph)
{
VD vd;
for (auto o : obstacles_) {
vd.insert(Site_2(o.first, o.second));
}
polygons_.clear();
Iso_rectangle rect(Point_2(bbox_p1_x_, bbox_p1_y_), Point_2(bbox_p2_x_, bbox_p2_y_));
std::map<std::string, Point_2> points;
std::map<std::string, std::string> props_gen;
props_gen["generated"] = "true";
unsigned int num_nodes = 0;
if (vd.is_valid()) {
VD::Edge_iterator e;
graph.lock();
for (e = vd.edges_begin(); e != vd.edges_end(); ++e) {
if (e->is_segment()) {
if (bbox_enabled_) {
CGAL::Bounded_side source_side, target_side;
source_side = rect.bounded_side(e->source()->point());
target_side = rect.bounded_side(e->target()->point());
if (source_side == CGAL::ON_UNBOUNDED_SIDE || target_side == CGAL::ON_UNBOUNDED_SIDE)
continue;
}
// check if we have a point in the vicinity
std::string source_name, target_name;
bool have_source = contains(points, e->source()->point(),
source_name, near_threshold_);
bool have_target = contains(points, e->target()->point(),
target_name, near_threshold_);
if (! have_source) {
source_name = genname(num_nodes);
//printf("Adding source %s\n", source_name.c_str());
graph->add_node(NavGraphNode(source_name,
e->source()->point().x(), e->source()->point().y(),
props_gen));
points[source_name] = e->source()->point();
}
if (! have_target) {
target_name = genname(num_nodes);
//printf("Adding target %s\n", target_name.c_str());
graph->add_node(NavGraphNode(target_name,
e->target()->point().x(), e->target()->point().y(),
props_gen));
points[target_name] = e->target()->point();
}
graph->add_edge(NavGraphEdge(source_name, target_name, props_gen));
} else {
//printf("Unbounded edge\n");
}
}
// Store Polygons
VD::Bounded_faces_iterator f;
for (f = vd.bounded_faces_begin(); f != vd.bounded_faces_end(); ++f) {
unsigned int num_v = 0;
Ccb_halfedge_circulator ec_start = f->outer_ccb();
Ccb_halfedge_circulator ec = ec_start;
do { ++num_v; } while ( ++ec != ec_start );
Polygon2D poly(num_v);
size_t poly_i = 0;
bool f_ok = true;
do {
const Point_2 &p = ec->source()->point();
if (bbox_enabled_) {
if (rect.has_on_unbounded_side(p)) {
f_ok = false;
break;
}
}
poly[poly_i][0] = p.x();
poly[poly_i][1] = p.y();
++poly_i;
} while ( ++ec != ec_start );
if (f_ok) polygons_.push_back(poly);
}
std::list<Eigen::Vector2f> node_coords;
std::vector<NavGraphNode>::const_iterator n;
for (n = graph->nodes().begin(); n != graph->nodes().end(); ++n) {
node_coords.push_back(Eigen::Vector2f(n->x(), n->y()));
}
polygons_.remove_if([&node_coords](const Polygon2D &poly) {
for (const auto nc : node_coords) {
if (polygon_contains(poly, nc)) return true;
}
return false;
}
);
polygons_.sort([](const Polygon2D &p1, const Polygon2D &p2)
{
return polygon_area(p2) < polygon_area(p1);
}
);
graph->calc_reachability();
graph.unlock();
}
}
